南阳理工学院本科生毕业设计(论文)
学院（系）：电子与电气工程系
专业：自动化
学生：
指导教师：
完成日期 2011 年 5 月
南阳理工学院本科生毕业设计（论文）
基于DSP的全向运动控制系统软件设计
Software Design of Omni-directional Motion Control
System Based on DSP
总计：42 页
表格： 5 个
插图： 31 幅
南阳理工学院本科毕业设计（论文）
基于DSP的全向运动控制系统软件设计
Software Design of Omni-directional Motion Control
System Based on DSP
学院（系）：电子与电气工程系
专业：自动化
学生姓名：
学号：
指导教师（职称）：
评阅教师：
完成日期：
南阳理工学院
Nanyang Institute of Technology
基于DSP的全向运动控制系统软件设计
自动化专业
[摘要] 本文基于DSP C2000系列TMS320LF2407A核心控制芯片，以CCStudio V3.3软件为开发平台，在了解RoboCup中型组足球机器人和其他全向机器人的基础上，主要完成了全向运动控制系统的软件设计。通过对全向运动控制系统的研究，建立了三轴全向运动学数学模型，并对整体平移运动、原地旋转运动、边平移边旋转三种运动方式进行分析和数学建模；利用上位机，经无线模块发送运动方式和各种运行参数，控制运动系统实现各种运动模式；经全向运动控制系统软件的编写和与相关硬件联机调试，系统实现了预订的全向运动形式。通过试验验证和结果分析，各种运动状态的准确性已达到性能的基本要求。
[关键词] DSP；全向运动控制；数学建模；MATLAB仿真；串行通信;软件设计
Software Design of Omni-directional Motion Control
System Based on DSP
Automation Specialty LI Hai－qing
Abstract:This article is based on The TMS320LF2407A in the series of DSP C2000 as the control core chip and the software of CCStudio V3.3 as the development platform. It is basis on the understanding of medium-sized group about soccer robot in Robocup and other omnidirectional robot, mainly accomplish the software design of a Omni-directional Motion Control System. Through the research to the motion control system, motion control system was established to all three axis kinematics mathematic model, build the mathematical modeling and analysis three kind of movement ,which include the whole translation movement, spin around movement and edge translation and https://www.doczj.com/doc/c814816699.html,ing the PC, The wireless module to send the movement and a variety of operating parameters, control motion system to achieve a variety of motion modes; By the whole motion control system software to write and online calibration with relevant hardware, the system realized the omnidirectional movement we expected. Through the results analysis and experimental verification, the accuracy have already meet requirements of various sports state.
Key Words: DSP; omnidirectional motion control; mathematical modeling; MATLAB simulation; serial communication；software design
目录
1 引言 (1)
1.1 全向运动控制系统发展现状 (1)
1.2 本课题的研究意义及前景 (2)
1.3 论文组织结构 (3)
2 全向运动控制系统分析 (3)
2.1 全向运动控制系统运动学模型建立 (3)
2.2不同运动方式的运动特性 (5)
2.2.1 平移运动 (5)
2.2.2 原地旋转运动 (7)
2.3.3边平移边旋转运动 (8)
3 基于DSP的硬件系统简介 (8)
3.1 控制芯片选择 (8)
3.2 硬件系统结构图 (9)
3.3 硬件系统基本模块 (10)
4系统运动控制的MATLAB仿真 (13)
4.1 电机PID控制 (13)
4.2 转速检测 (14)
4.3 MATLAB仿真 (15)
5系统软件设计 (16)
5.1 软件开发平台及仿真器 (16)
5.2 运动控制软件设计 (18)
5.2.1 主程序 (18)
5.2.2 三种基本运动状态子程序 (19)
5.2.3 电机控制子程序 (20)
5.2.4 无线发送子程序 (20)
5.3 上位机软件及通讯协议 (21)
6 实验验证及结果分析 (22)
6.1试验场地 (23)
6.2 各项性能测试 (24)
6.2.1 速度PID测试 (24)
6.2.2 平移运动测试 (24)
6.2.3 原地旋转测试 (25)
6.2.4 平移+旋转运动测试 (26)
6.3 影响因素分析 (26)
结束语 (28)
参考文献 (29)
附录 (30)
致谢 (42)
1 引言
随着机器人技术的日新月异，机器人应用领域也已从工业走向普通生活。机器人技术已经在人们的生活中越来越发挥着重要的作用。作为机器人中的全向运动机器人，有着其特殊的运动形式，即可以不改变姿势向任何方向运动，找到最佳位置。同时也可以边往某个方向运动，边调整自身的姿势达到最佳运动效果，可以在平面内完美的往任何方向做三自由度运动。全向运动机器人其高灵活度，势必会在机器人领域发挥不可代替的作用。
在全向运动机器人中现阶段比较常见的是三轮结构和四轮结构（如图1所示）。通过控制各个轮子间相互协调运动达到全方位的运动效果，包括全方位平移、全方位边平移边旋转和原地旋转运动形式。
图1 三轮和四轮的结构模型
1.1 全向运动控制系统发展现状
在机器人高速发展的今天，机器人的使用越来越广泛，如用于军事、勘探、工业和家居等等。中国要想变成世界强国，也必须重视起机器人技术。同时机器人技术不能只靠少量研究人员，必须从学生时代培养机器人技术。只有这样中国的机器人事业才会涌现大批的人才，能够加速机器人技术的发展。同时机器人技术本身是对电子技术的一个集合运用，从学生时代从事机器人技术的学习研究的过程，不但对机器人技术加速发展，还带动中国其它电子技术的发展，带动科技强国，科技富国。
全向运动机器人是移动机器人中的一部分，其高灵活性和准确性越来越突出其优越性。现在在全向运动机器人方面使用较多的是RoboCup中型组足球机器人（如图2）。1997年成立于日本，足球机器人作为人工智能的一个重要组成部分，已是当前机器人研究领域最为活跃的领域之一[1]。RoboCup以机器人足球作为中心研究课题，通过举办机器人足球比赛，旨在促进人工智能、机器人技术及其相关学科的发展。RoboCup的最终目标是在2050年成立一支完全自主的拟人机器人足球队，能够与人类进行一场真正意义上的足球赛。RoboCup的最终目标是：到21世纪中叶，一支完全自治的人形机器人
足球队应该能在遵循国际足联正式规则的比赛中，战胜最近的人类世界杯冠军队。要想实现足球机器人灵活自如的“踢足球”，传统的三轮和四轮结构移动不够灵活已经被淘汰，现在使用的是三轮和四轮结构配备全向轮的全向运动机器人。可以不做任何准备的情况下朝球移动，并且可以边移动边调整踢球姿态已达到最近效果。对然全向运动机器人有了很多，但是很多都在实验室，还不能在实际的环境使用，全向运动机器人还需要很长的时间发展。
图2 RoboCup中型组机器人
1.2 本课题的研究意义及前景
随着机器人技术的发展，机器人从工业走进生活中。国外很多国家已经研制出很多全方位机器人，应用领域不断扩大，如导游机器人、导购机器人、电动轮椅、平稳的测量装置、医院巡视病房机器人和仓库作业机器人等。这些机器人很大程度上代替了人大劳动，提高了人的生活质量。如图3是三种全向机器人。
全向运动机器人在社会服务、教育、娱乐、军事和环境探测领域都将发挥着不可代替的作用。其未来的应用前景将十分广阔[2]。
图3 三种全向机器人
1.3 论文组织结构
本设计内容安排分为6章，其各章节的内如安排如下：
第1章：引言。介绍此课题的现状和未来发展前景。
第2章：全向运动控制系统分析。建立全向运动数学模型，对模型进行力的分解合成，计算各种运动状态下各电机运动公式。分别分析了全向运动三种基本运动形式：平移运动、原地旋转运动、平移+旋转运动。
第3章：DSP及相关控制系统。选择TMS320LF2407A为核心控制芯片，选择电机、驱动模块、显示模块、无线通讯模块、电源模块等等，组建硬件系统
第4章：系统运动控制部分的设计和MATLAB仿真。根据系统运动形式计算好各个电机矢量速度，如何能让电机稳准快的执行设定的矢量速度是运送形式是否实现的关键。本设计采用传统的PID增量式控制方式及MALTAB仿真来实现。
第5章：系统软件设计。根据建立数学模型及外设资源设计系统结构和系统流程图，通过CCS3.3编写系统程序，利用上位机和无线传输模块对机器人运动形式进行控制，其中也包括建立简单的无线通讯协议。
第6章：试验验证及结果分析。通过前几章设计出了全向机器人，本章主要是通过试验验证各种运动形式是否达到要求，并对结果进行分析。
2 全向运动控制系统分析
2.1 全向运动控制系统运动学模型建立
在引言中介绍了全向运动控制的几种结构，本设计选用三轮结构构建全向运动系统[3][4]。其结构图和实物付如图4所示。
(a) 结构图(b) 实物图
图4 全向结构图和实物图
其中三个驱动轮的旋转轴互相成120 °夹角，且驱动轮中心到整体系统中心的距离L1=L2=L3。为实现全向运动控制，首先对系统进行运动方式建模[5]，，建立运动学模型。对全向机器人的运动学模型（图5所示）进行分析并对控制算法进行研究。要由图5中(a)图所示，小车运行在二维平面内，世界坐标系X-Y 和机器人坐标系a a y x -。θ为X 与a x 的夹角，α为驱动轮与a y 的夹角，驱动轮之间的夹角为120o 角。i L (i=1,2,3)为机器人中心到驱动轮中心的距离,由于本设计使用的瑞典轮在旋转的时候对地面的接触位置一直在变化，会造成驱动轮与机器人中心的距离也一直在变化，但变化值比较小。为方便建立运动学模型及计算取L=L1=L2=L3。i V (i=1,2,3)为驱动轮i 提供沿驱动方向的速度，规定方向逆时针为正。
由图5中(b)所示，V 为机器人系统整体速度，世界坐标X-Y 坐标系中X 轴与整体速度V 的逆时针夹角为β，在世界坐标系中将整体速度V 分解成沿X 轴和Y 轴的速度分别为X V 和y V 。
(a) 运动学建模 (b) 系统整体速度矢量分解
图5 运动学模型 根据运动学模型分析出各个驱动轮的速度为下式(1)：
1sin()cos()2cos sin 3sin()cos()X y X y X y V V V L V V V L V V V L θθθ
θθθθθθ????=-α--α-+???=++???=-α++α++??
(1) 式(1)中：由机械结构可知α=30 o ，θ取逆时针为正，θ?
为角速度且逆时针为正。将这些已知的值代入式(1)中得到式(2)。
1sin()cos()662cos sin 3sin()cos()66X y X y X y V V V L V V V L V V V L ππθθθ
θθθππθθθ????=----+???=++???=-++++??
(2) 由式(2)写出矩阵形式，即式(3)。
s i n ()c o s ()1662cos sin 3sin()cos()66x y L V V V L V V L ππθθθθππθθθ?????---- ??? ? ? ? ?= ? ? ? ? ? ???--+ ????
? (3) 所以在任意时刻，小车的运动形态就是 T x y V V V θ???=????。在实际中，路径规划、位
姿控制，通过矢量分解、坐标变化，即可转换为对机器人每个轴驱动电机的运动控制。 为使模型成立，机器人应该符合以下几点要求：
(1) 驱动轮与地面有足够的摩擦力，不存在打滑现象。
(2) 小车的构造及安装位置应尽量精确。
(3) 各个驱动轮速度应在电机提供的最高速之内[-Vmax Vmax]。
(4) 忽略驱动轮转动过程中L 的变化，取L=L1=L2=L3。
(5) 车的重量均匀分布三个驱动轮上。
2.2不同运动方式的运动特性
全向运动的运动形式大体可分为平移运动、原地旋转运动、边平移边旋转运动三种运动形式。三种运动形式有着不同的运动特性，以下将分三小节对三种运动形式进行单独分析。
2.2.1 平移运动
平移是基本的也是比较常用的运动形式。由于是平移不考虑旋转运动，我们将模型进行简化，其简化后运动学模型图如图6所示。
(a) 平移模型 (b) 系统整体平移模型
图6 简化后运动运动学模型 单独考虑平移运动故将式(3)进行简化为式(4)：
11222103012x y L V V V L V V L ??-- ????? ? ? ?= ? ? ? ? ? ?????
?- ??? (4) 假设小车整体平移矢量速度为V ，与X 轴成γ角。所以我们可以分解为x V 和y V ，其分解公式为式(5)。
cos()sin()
x y V V V V γγ=???=?? (5) 由式(4)和式(5)我们可以得到三个驱动轮的速度，见式(6)。
1122331sin()cos()sin()622cos()
cos()1sin()cos()sin()62V V V V V V V V V V V V πγγγγγπγγγ??=-+=--?????=?=??????=--=-??? (6) 驱动轮速度1V 、2V 、3V 的速度范围都是[]max max V V -。max V 的值则由电机性能决定。通过式(6)我们可以反向推导出平移速度V 的速度范围。如图7，其中圆是以驱动轮最大速度max V 大小为半径的圆，红色速度V 与X 轴的角度为γ，随着γ由
0°~360°变化V
的轨迹为红色正六边形。其中最大速度为max 。
图7 平移最大速度模型
2.2.2 原地旋转运动
全向机器人不但要平移，而且很多时候都需要转向，转向运动分为原地旋转运动和边平移边旋转运动。原地旋转是旋转的最基本运动形式，边平移边旋转运动将在下节中阐述，本节主要分析原地旋转运动形式。
原地旋转运动时V=0，即x V 和y V 都为0，所以由式(2)可的下式(7)。
123V L V L V L θ
θθ????=??=???=?
(7)
其中θ?
为角速度，逆时针为正。所以可以得知三个驱动电机的矢量速度是相同的。为了表述直观，建立原地旋转运动模型，其模型图如图8所示。
图8 原地旋转运动模型
2.3.3边平移边旋转运动
边平移边旋转运动方式是全向运动中较为复杂的运动方式，即不做任何准备动作的前提下往任何方向运动并且可以边运动边调整自身姿态。
这种运动形式是平移与旋转的合成。在前文2.1中已经推导出公式(1)。此处不再过多叙述。
3 基于DSP的硬件系统简介
3.1 控制芯片选择
作为运动控制系统的核心控制芯片，根据以往在实验室的经验，其核心控制芯片当属DSP C2000系类。C2000系类对于运动控制有着诸多的优点，本设计采用C2000系类TMS320LF2407A的控制板。TMS320LF2407A主要特点如下：
?高性能、高速度：集成了高性能的DSP内核和微处理器的片内外设；每秒4000
万条指令(40MIPS)的处理速度。
?高可靠性、可编程性：TMS320LF2407A的16位定点DSP内核为模拟系统的设
计者提供了一个不牺牲系统精度和性能的数字解决方案。
?两个事件管理器模块EV A和EVB，可以提供完整的、高效的电机控制方案，提
供所有的PWM(8个16位脉冲调制通道)和IO，可以控制所有类型的电机。
?采用高性能静态CMOS技术，使得供电电压降为3.3V，减少了控制器的损耗。
?片内有高达32K×16位的Flash程序存储器：高达2.5K字×16位的数据/程序RAM；
544字节的双端口RAM(DARAM)；2K字的单口RAM(SARAM)。
?可扩展的外部存储器总共具有192K×16位的空间，分别为64K字的程序存储器
空间、64K字的数据存储空间和64K字的I/O空间。
?10位ADC转换器，其特性为：最小转换时间为500ns、8个或16个多路复用的
输入通道，采集时间和转换时间分开，提高了采样率和输入阻抗，并且支持自动
顺序采样，不需CPU干预。
?CAN总线控制器，可以为控制器、传感器、激励源以及其它节点提供良好的通
讯，特别适用于工业现场和汽车等强噪声和恶劣的环境中。
?5个外部中断(两个驱动保护、复位和两个可屏蔽中断) [6][7]。
为了满足机器人大量扩展传感器的需要和减少处理器外围数字逻辑器件的使用数量，该运动控制卡采用Altera公司的复杂可编程逻辑器件（CPLD）来扩展DSP的I/O 端口和实现外围数字逻辑电路设计[8]。
3.2 硬件系统结构图
通过第2章关于运动学模型的建立，为使得系统能够按照建立的数学模型进行运动，除了3.1节的DSP主控制芯片外还需要其它的硬件的支持，本课题设计的硬件系统结构图如图9所示。其主要的工作方式为：PC上位机选择相应的运动方式，经过RS232协议连接无线模块，由无线模块发送数据。DSP下位机的无线模块接收上位机发送的数据，经RS232通信协议发送给DSP。DSP接收到数据进行相应的处理，分析出要进行的运动形式及各种参数，同时利用I2C协议采集指南针的数据，将这些信息通过第2章的全向运动学模型分析的各种公式转换成各个电机的速度值。为使得电机能够稳、准、快的执行，本设计采用经典PID控制，以增量式编码器采集的值为反馈。在进行的过程中液晶模块实时显示运动方式和参数及指南针的值，同时将编码器的值通过无线模块传送给PC上位机。DSP下位机将按照程序设定执行上位机设定的运动方式，只有上位机重新发送新的运动方式才会改变。
图9 硬件系统结构图
根据系统要求分配系统配置（见表1）：
表1 DSP2407A系统资源分配
3.3 硬件系统基本模块
根据图9硬件系统结构图所示，本节将分析几个重要的模块。分为电源模块、无线通信模块、指南针模块、液晶显示模块，DSP下位机模块、电机及驱动模块和全向轮。其中DSP下位机3.1节已经介绍，在此不再叙述。
?电源模块
在控制系统中，主要用到系统板电源模块、DSP、外围器件和12V电机供电。TMS320LF2407A不采用5V供电而是采用低电压3.3V供电方式。电池选用15V的锂电池，需要将15V转换为12V、5V、3.3V的电压。电机需要大电流在此选用两块2576开关电源进行处理。一块转换为12V单独供电机，另一块转换5V供外围5V器件和DSP 开发板。在开发板上选用LM1117作为系统板电源模块的主芯片，把输入的+5V变为+3.3V。
?无线通信模块
DSP2407A处理器内嵌的异步串行接口SCI是一个标准的通用异步接收/发送通讯接口，接收和发送都是双缓冲的，有自己的是能和中断位，支持半双工和全双工工作。通讯接口有SCITXD和SCIRXD的TTL电平信号。本设计是使用AS-WLCOM无线模块。本设计用的是RS-232电平接口，通讯波特率设置9600bps，无奇偶校验位，8位数据位和一个停止位。AS-WLCOM的链接方法如图10所示。
图10无线发送接收模块连接图
指南针模块
采用CY—26平面数字指南针模块图具有磁偏角补偿功能，通过硬磁补偿和设置地磁夹角能够表示成全向机器人的夹角。指南针模块使用232（9600bps）协议及IIC协议。本设计中采用IIC通信协议。测量范围：0°~ 360°、分辨率：0.1 °、测量精度：1 °、重复精度：1 °、响应频率：30 HZ。
DSP 2407A中SCI已经使用了无线模块，所以在本很设计中使用IIC通讯协议进行通信。IIC通信模块地址及数据含义见表2。
表2 内部数据
16位数据，数据范围0-3599（因为分辨率为0.1°）
实际当前磁偏角值为“磁偏角高8 位”与“磁偏角低8 位”合成的16位数据，数据范围0-3599（因为分辨率为0.1°）
通过IIC控制指南针命令，磁偏角的值，范围0-3599（因为分辨率为0.1°），是由2个8位的数据组成，当修改模块磁偏角时，分为高8位值，低8位值，写入模块。其命令见表3。
表3 模块命令表
?液晶显示模块
使用1.7英寸TFT全彩屏，显示运动过程中各种状态和参数如图11，通过显示模块能够很好的显示运动状态，同时系统出现问题也能通过显示反应出来。
图11 液晶显示
?电机及驱动模块
电机使用Faulhaber带双路编码器减速电机2230 12V220，减速后速：220RPM（转每分钟），每圈脉冲：512CPR（脉冲每圈），对于编码器将在下章介绍。
对于电机驱动本设计选用L298N，它是一款具有15管脚Multiwatt封装的大功率集成驱动芯片。它是一个能够承受高电压，大电流的双H桥驱动。L298N可驱动2个电机，OUT1，OUT2和OUT3，OUT4之间可分别接电机。L298N功能模块如表4所示。
表4 L298N功能模块
L298N驱动电路如图12所示。单个电机的控制方法如下：通过IO口控制IN1和IN2，使能ENA为PWM，则通过控制IN1和IN2来实现正反转，通过ENA的PWM占空比调节速度快慢。
图12 L298N电机驱动电路原理图
全向轮
作为全向运动的驱动部分，驱动轮与普通驱动轮有着重要的区别。在驱动轮的边缘处有侧滑的小轮，他们能够使得轮子向前后提供驱动力，同时能够使的侧面与地面很小的摩擦力，实现侧面移动。
4系统运动控制的MATLAB仿真
本设计的全向运动控制系统使用三轴全向控制，即控制3个电机相互协调运动来实现全向轮动。电机的控制对整个系统是至关重要的。本章主要分析系统的PID控制和MATLAB 仿真。
4.1 电机PID控制
电机是运动系统的执行机构，不管DSP计算的多么正确，但是电机执行达不到要求，同样系统是无法实现全向运动的。为使得电机能够稳、准、快的执行，而PID是根据偏差的比例、积分、微分的线性组合进行反馈控制，是迄今为止工业中应用最为广泛的一种控制方法。所以本设计采用PID分别对三个电机进行闭环控制。
数字PID可分为位置式PID和增量式PID控制。由于位置式PID是全量输出，所以每次输出均与过去的状态有关，计算时要对e（k）进行累加，DSP运算量大。而增量式PID控制的优点是只输出增量，计算误差动作时造成的影响小，同时对控制算法不需要
图13
电机闭环控制系统
(9) 前一时刻减去后一时刻的值为式(10)：
(10)
数字PID 最终输出量为式(11)：
(11)
4.2 转速检测 本设计选用增量式PID 即4.1节中所阐述的PID 。对于PID 控制反馈必不可少，在本设计使用的是Faulhaber 带双路编码器减速电机，即增量式光电编码器来做为反馈环节，其工作原理如图14。光源经过码盘和检测光栅照射的信号通过光电检测器件检测，检测出的正弦值经转换电路转换成矩形波，最后出来A 相B 相Z 相。A 相B 相相差90度，通过软件能够很好的得到此时电机的速度和方向。对AB 相的采集是电机闭环控制的反馈量，关乎电机PID 控制是否能达到稳准快的要求。本设计使用输入捕捉CAP 功能对脉冲进行采集，将采集量进行相应处理即得到PID 控制的反馈。
[]000(1)(1)()(1)(2)d p i i u k K e k e i e k e k T T =-=-++---??
??
∑[]()[](1)()(1)
()(1)()2(1)(2)p i d k u k u k k e k e k k e k k e k e k e k ?-=--=--++--+-()(1)()
u k u k u k =-+?