基于DSP的全向运动控制系统软件设计毕业设计

1、南阳理工学院本科生毕业设计（论文）学院（系）：电子与电气工程系专业：自动化学生：指导教师：完成日期2011年5月南阳理工学院本科生毕业设计（论文）基于DSP的全向运动控制系统软件设计SoftwareDesignofOmni-directionalMotionControlSystemBasedonDSP总计：42页表格：5个插图：31幅南阳理工学院本科毕业设计（论文）基于DSP的全向运动控制系统软件设计SoftwareDesignofOmni-directionalMotionControlSystemBasedonDSP学院（系）：电子与电气工程系专业：自动化学生姓名：学号：指导教师（职称）

2、：评阅教师：完成日期：南阳理工学院NanyangInstituteofTechnology基于DSP的全向运动控制系统软件设计基于DSP的全向运动控制系统软件设计自动化专业摘要本文基于DSPC2000系列TMS320LF2407A心控制芯片，以CCStudioV3.3软件为开发平台，在了解RoboCup中型组足球机器人和其他全向机器人的基础上，主要完成了全向运动控制系统的软件设计。通过对全向运动控制系统的研究，建立了三轴全向运动学数学模型，并对整体平移运动、原地旋转运动、边平移边旋转三种运动方式进行分析和数学建模；利用上位机，经无线模块发送运动方式和各种运行参数，控制运动系统实现各种运动模式；

3、经全向运动控制系统软件的编写和与相关硬件联机调试，系统实现了预订的全向运动形式。通过试验验证和结果分析，各种运动状态的准确性已达到性能的基本要求。关键词DSP全向运动控制；数学建模；MATLA防真；串行通信；软件设计SoftwareDesignofOmni-directionalMotionControlSystemBasedonDSPAutomationSpecialtyLIHaiqingAbstract:ThisarticleisbasedonTheTMS320LF2407AintheseriesofDSPC2000asthecontrolcorechipandthesoftwareofC

4、CStudioV3.3asthedevelopmentplatform.Itisbasisontheunderstandingofmedium-sizedgroupaboutsoccerrobotinRobocupandotheromnidirectionalrobot,mainlyaccomplishthesoftwaredesignofaOmni-directionalMotionControlSystem.Throughtheresearchtothemotioncontrolsystem,motioncontrolsystemwasestablishedtoallthreeaxiski

5、nematicsmathematicmodel,buildthemathematicalmodelingandanalysisthreekindofmovement,whichincludethewholetranslationmovement,spinaroundmovementandedgetranslationandrotating.UsingthePC,Thewirelessmoduletosendthemovementandavarietyofoperatingparameters,controlmotionsystemtoachieveavarietyofmotionmodes;B

6、ythewholemotioncontrolsystemsoftwaretowriteandonlinecalibrationwithrelevanthardware,thesystemrealizedtheomnidirectionalmovementweexpected.Throughtheresultsanalysisandexperimentalverification,theaccuracyhavealreadymeetrequirementsofvarioussportsstate.KeyWords:DSP;omnidirectionalmotioncontrol;mathemat

7、icalmodeling;MATLABsimulation;serialcommunication；softwaredesign基于DSP的全向运动控制系统软件设计目录1引言11.1 全向运动控制系统发展现状11.2 本课题的研究意义及前景21.3 论文组织结构32全向运动控制系统分析32.1 全向运动控制系统运动学模型建立32.2 不同运动方式的运动特性52.2.1 平移运动52.2.2 原地旋转运动72.3.3边平移边旋转运动83基于DSP的硬件系统简介83.1 控制芯片选择83.2 硬件系统结构图93.3 硬件系统基本模块104系统运动控制的MATLA防真134.1 电机PID控制134

8、.2 转速检测144.3 MATLAB仿真155系统软件设计165.1 软件开发平台及仿真器165.2 运动控制软件设计185.2.1 主程序185.2.2 三种基本运动状态子程序195.2.3 电机控制子程序205.2.4 无线发送子程序205.3 上位机软件及通讯协议216实验验证及结果分析226.1 试验场地236.2 各项性能测试246.2.1 速度PID测试24II基于DSP的全向运动控制系统软件设计6.2.2 平移运动测试246.2.3 原地旋转测试256.2.4 平移+旋转运动测试266.3 影响因素分析26结束语28参考文献29附录30致谢42iii基于DSP的全向运动控制系统

9、软件设计1引言随着机器人技术的日新月异，机器人应用领域也已从工业走向普通生活。机器人技术已经在人们的生活中越来越发挥着重要的作用。作为机器人中的全向运动机器人，有着其特殊的运动形式，即可以不改变姿势向任何方向运动，找到最佳位置。同时也可以边往某个方向运动，边调整自身的姿势达到最佳运动效果，可以在平面内完美的往任何方向做三自由度运动。全向运动机器人具高灵活度，势必会在机器人领域发挥不可代替的作用。在全向运动机器人中现阶段比较常见的是三轮结构和四轮结构（如图1所示）。通过控制各个轮子间相互协调运动达到全方位的运动效果，包括全方位平移、全方位边平移边旋转和原地旋转运动形式。图1三轮和四轮的结构模型1

10、.1 全向运动控制系统发展现状在机器人高速发展的今天，机器人的使用越来越广泛，如用于军事、勘探、工业和家居等等。中国要想变成世界强国，也必须重视起机器人技术。同时机器人技术不能只靠少量研究人员，必须从学生时代培养机器人技术。只有这样中国的机器人事业才会涌现大批的人才，能够加速机器人技术的发展。同时机器人技术本身是对电子技术的一个集合运用，从学生时代从事机器人技术的学习研究的过程，不但对机器人技术加速发展，还带动中国其它电子技术的发展，带动科技强国，科技富国。全向运动机器人是移动机器人中的一部分，其高灵活性和准确性越来越突出其优越性。现在在全向运动机器人方面使用较多的是RoboCup中型组足球机

11、器人（如图2）。1997年成立于日本，足球机器人作为人工智能的一个重要组成部分，已是当前机器人研究领域最为活跃的领域之一1。RoboCup以机器人足球作为中心研究课题，通过举办机器人足球比赛，旨在促进人工智能、机器人技术及其相关学科的发展。RoboCup的最终目标是在2050年成立一支完全自主的拟人机器人足球队，能够与人类进行一场真正意义上的足球赛。RoboCup的最终目标是：到21世纪中叶，一支完全自治的人形机器人基于DSP的全向运动控制系统软件设计足球队应该能在遵循国际足联正式规则的比赛中，战胜最近的人类世界杯冠军队。要想实现足球机器人灵活自如的“踢足球”，传统的三轮和四轮结构移动不够灵活

12、已经被淘汰，现在使用的是三轮和四轮结构配备全向轮的全向运动机器人。可以不做任何准备的情况下朝球移动，并且可以边移动边调整踢球姿态已达到最近效果。对然全向运动机器人有了很多，但是很多都在实验室，还不能在实际的环境使用，全向运动机器人还需要很长的时间发展。图2RoboCup中型组机器人1.2 本课题的研究意义及前景随着机器人技术的发展，机器人从工业走进生活中。国外很多国家已经研制出很多全方位机器人，应用领域不断扩大，如导游机器人、导购机器人、电动轮椅、平稳的测量装置、医院巡视病房机器人和仓库作业机器人等。这些机器人很大程度上代替了人大劳动，提高了人的生活质量。如图3是三种全向机器人。全向运动机器人

13、在社会服务、教育、娱乐、军事和环境探测领域都将发挥着不可代替的作用。其未来的应用前景将十分广阔2o图3三种全向机器人2基于DSP的全向运动控制系统软件设计1.3 论文组织结构本设计内容安排分为6章，其各章节的内如安排如下：第1章：引言。介绍此课题的现状和未来发展前景。第2章：全向运动控制系统分析。建立全向运动数学模型，对模型进行力的分解合成，计算各种运动状态下各电机运动公式。分别分析了全向运动三种基本运动形式：平移运动、原地旋转运动、平移+旋转运动。第3章：DSP及相关控制系统。选才？TMS320LF2407A为核心控制芯片，选择电机、驱动模块、显示模块、无线通讯模块、电源模块等等，组建硬件系

14、统第4章：系统运动控制部分的设计和MATLAB仿真。根据系统运动形式计算好各个电机矢量速度，如何能让电机稳准快的执行设定的矢量速度是运送形式是否实现的关键。本设计采用传统的PID增量式控制方式及MALTAB仿真来实现。第5章：系统软件设计。根据建立数学模型及外设资源设计系统结构和系统流程图，通过CCS3.3编写系统程序，利用上位机和无线传输模块对机器人运动形式进行控制，其中也包括建立简单的无线通讯协议。第6章：试验验证及结果分析。通过前几章设计出了全向机器人，本章主要是通过试验验证各种运动形式是否达到要求，并对结果进行分析。2全向运动控制系统分析2.1全向运动控制系统运动学模型建立在引言中介绍

15、了全向运动控制的几种结构，本设计选用三轮结构构建全向运动系统34。其结构图和实物付如图4所示。(a)结构图(b)实物图全向结构图和实物图基于DSP的全向运动控制系统软件设计其中三个驱动轮的旋转轴互相成120°夹角，且驱动轮中心到整体系统中心的距离L1=L2=L3o为实现全向运动控制，首先对系统进行运动方式建模5,建立运动学模型。对全向机器人的运动学模型(图5所示)进行分析并对控制算法进行研究。要由图5中(a)图所示，小车运行在二维平面内，世界坐标系X-Y和机器人坐标系xa-ya。8为X与xa的夹角，口为驱动轮与ya的夹角，驱动轮之间的夹角为120°角。Li(i=1,2,3)

16、为机器人中心到驱动轮中心的距离，由于本设计使用的瑞典轮在旋转的时候对地面的接触位置一直在变化，会造成驱动轮与机器人中心的距离也一直在变化，但变化值比较小。为方便建立运动学模型及计算取L=L1=L2=L3Vi(i=1,2,3)为驱动轮i提供沿驱动方向的速度，规定方向逆时针为正。由图5中(b)所示，V为机器人系统整体速度，世界坐标X-Y坐标系中X轴与整体速度V的逆时针夹角为B,在世界坐标系中将整体速度V分解成沿X轴和Y轴的速度分别为VX和Vy。(a)运动学建模(b)系统整体速度矢量分解图5运动学模型根据运动学模型分析出各个驱动轮的速度为下式(1):VI =-Vxsin(0-9)-Vycos(a-日

17、)+L日IVII 2=VXcos7ysinlV3一VXsin(:二)Vycos(-1)L式(1)中：由机械结构可知口=300,e取逆时针为正，,为角速度且逆时针为正。将这些已知的值代入式(1)中得到式(2)基于DSP的全向运动控制系统软件设计(2)VIII =VXsin(一二)-Vycos(i)L=6y6jiV2=Vxcos二Vysin1LiIX 3=Vxsin(一+日)+Vycos(一+日)+L日66由式(2)写出矩阵形式，即式(3)。V/1、V2N3cosr兀一sin(-)6eBcors(sin1cos(-1)6所以在任意时刻，小车的运动形态就是Vq。在实际中，路径规划、姿控制，通过矢量分

18、解、坐标变化，即可转换为对机器人每个轴驱动电机的运动控制。为使模型成立，机器人应该符合以下几点要求：(1)驱动轮与地面有足够的摩擦力，不存在打滑现象。(2)小车的构造及安装位置应尽量精确。(3)各个驱动轮速度应在电机提供的最高速之内卜VmaxVmax(4)忽略驱动轮转动过程中L的变化，取L=L1=L2=L3。(5)车的重量均匀分布三个驱动轮上。2.2不同运动方式的运动特性全向运动的运动形式大体可分为平移运动、原地旋转运动、边平移边旋转运动三种运动形式。三种运动形式有着不同的运动特性，以下将分三小节对三种运动形式进行单独分析。2.2.1 平移运动平移是基本的也是比较常用的运动形式。由于是平移不考

19、虑旋转运动，我们将模型进行简化，具简化后运动学模型图如图6所示。基于DSP的全向运动控制系统软件设计(a)平移模型(b)系统整体平移模型图6简化后运动运动学模型单独考虑平移运动故将式(3)进行简化为式(4):V1121220七./32V0(4)假设小车整体平移矢量速度为V,与X轴成丫角。所以我们可以分解为Vx和Vy,其分解公式为式(5)。Vx=Vcos()Vy二Vsin()由式(4)和式(5)我们可以得到三个驱动轮的速度，见式(6)。1M-Vcos()2M=Vcos1)1V3=-Vcos()-Vsin()Vsin()V=Msing+¥)=V2=Vcos()R乂=7叫-)(6)驱动轮速

20、度Vi、5、V3的速度范围都是-VmaxVmaxLVmax的值则由电机性能决定。通过式(6)我们可以反向推导出平移速度V的速度范围。如图7,其中圆是以驱动轮最大速度Vmax大小为半径的圆，红色速度V与X轴的角度为丫，随着丫由基于DSP的全向运动控制系统软件设计0°360°变化V的轨迹为红色正六边形。其中最大速度为2VTV3图7平移最大速度模型8所示。72.2.2 原地旋转运动全向机器人不但要平移，而且很多时候都需要转向，转向运动分为原地旋转运动和边平移边旋转运动。原地旋转是旋转的最基本运动形式，边平移边旋转运动将在下节中阐述，本节主要分析原地旋转运动形式。原地旋转运动时V=

21、0,即Vx和Vy者B为0,所以由式（2）可的下式（7）。V 1=LQV 2=LSV 3=le其中日为角速度，逆时针为正。所以可以得知三个驱动电机的矢量速度是相同的。为了表述直观，建立原地旋转运动模型，其模型图如图基于DSP的全向运动控制系统软件设计1.1 .3边平移边旋转运动边平移边旋转运动方式是全向运动中较为复杂的运动方式，即不做任何准备动作的前提下往任何方向运动并且可以边运动边调整自身姿态。这种运动形式是平移与旋转的合成。在前文2.1中已经推导出公式（1）。此处不再过多叙述。3基于DSP勺硬件系统简介1.2 控制芯片选择作为运动控制系统的核心控制芯片，根据以往在实验室的经验，其核心控制芯片

22、当属DSPC2000系类。C2000系类对于运动控制有着诸多的优点，本设计采用C2000系类TMS320LF2407A的控制板。TMS320LF2407A主要特点如下：高性能、高速度：集成了高性能的DSP内核和微处理器的片内外设；每秒4000万条指令（40MIPS）的处理速度。高可靠性、可编程性：TMS320LF2407A的16位定点DSP内核为模拟系统的设计者提供了一个不牺牲系统精度和性能的数字解决方案。两个事件管理器模块EVA和EVB,可以提供完整的、高效的电机控制方案，提供所有的PWM（8个16位脉冲调制通道）和IO,可以控制所有类型的电机。采用高性能静态CMOS技术，使得供电电压降为3

23、.3V,减少了控制器的损耗。片内有高达32KX16位的Flash程序存储器：高达2.5K字X6位的数据/程序RAM;544字节的双端口RAM（DARAM）;2K字的单口RAM（SARAM）。可扩展的外部存储器总共具有192KX16位的空间，分别为64K字的程序存储器空间、64K字的数据存储空间和64K字的I/O空间。10位ADC转换器，其特性为：最小转换时间为500ns、8个或16个多路复用的输入通道，采集时间和转换时间分开，提高了采样率和输入阻抗，并且支持自动顺序采样，不需CPU干预。CAN总线控制器，可以为控制器、传感器、激励源以及其它节点提供良好的通讯，特别适用于工业现场和汽车等强噪声和

24、恶劣的环境中。5个外部中断（两个驱动保护、复位和两个可屏蔽中断）67。为了满足机器人大量扩展传感器的需要和减少处理器外围数字逻辑器件的使用数量，该运动控制卡采用Altera公司的复杂可编程逻辑器件（CPLD）来扩展DSP的I/O端口和实现外围数字逻辑电路设计8基于DSP的全向运动控制系统软件设计1.3 硬件系统结构图通过第2章关于运动学模型的建立，为使得系统能够按照建立的数学模型进行运动，除了3.1节的DSP主控制芯片外还需要其它的硬件的支持，本课题设计的硬件系统结构图如图9所示。其主要的工作方式为：PC上位机选择相应的运动方式，经过RS232协议连接无线模块，由无线模块发送数据。DSP下位机

25、的无线模块接收上位机发送的数据，经RS232通信协议发送给DSP。DSP接收到数据进行相应的处理，分析出要进行的运动形式及各种参数，同时利用I2C协议采集指南针的数据，将这些信息通过第2章的全向运动学模型分析的各种公式转换成各个电机的速度值。为使得电机能够稳、准、快的执行，本设计采用经典PID控制，以增量式编码器采集的值为反馈。在进行的过程中液晶模块实时显示运动方式和参数及指南针的值，同时将编码器的值通过无线模块传送给PC上位机。DSP下位机将按照程序设定执行上位机设定的运动方式，只有上位机重新发送新的运动方式才会改变。根据系统要求分配系统配置（见表1）：基于DSP的全向运动控制系统软件设计表

26、1DSP2407A系统资源分配功能模块名称实现功能通讯模块SCI（PA0、PA1）通过无线模块与电脑上位机通讯（2个I/O）电机调速电机转向PWM:PE2、PE4、PE63个PWM控制控制调速，（3个I/O）正反转：PC0-1、PF2-3、PF4-5每个电机两个正反转控制信号控制正反转（6个I/O）编码值读取CAP:PF0、PF1、PA3利用CAP脉冲捕捉功能获取编码器脉冲，得到速度（3个I/O）液晶显示PF7、PA4-7液晶使能控制IO口（5个I/O）PB0-7液晶数据8位数据口（8个I/O）指南针模块IIC:PE1、PE3软件卞II拟IIC与指南针通化PE1是SCL,PE3是SDA（2个I

27、/O）1.4 硬件系统基本模块根据图9硬件系统结构图所示，本节将分析几个重要的模块。分为电源模块、无线通信模块、指南针模块、液晶显示模块，DSP下位机模块、电机及驱动模块和全向轮。其中DSP下位机3.1节已经介绍，在此不再叙述。电源模块在控制系统中，主要用到系统板电源模块、DSP、外围器件和12V电机供电。TMS320LF2407A不采用5V供电而是采用低电压3.3V供电方式。电池选用15V的锂电池，需要将15V转换为12V、5V、3.3V的电压。电机需要大电流在此选用两块2576开关电源进行处理。一块转换为12V单独供电机，另一块转换5V供外围5V器件和DSP开发板。在开发板上选用LM111

28、7作为系统板电源模块的主芯片，把输入的+5V变为+3.3V。无线通信模块DSP2407A处理器内嵌的异步串行接口SCI是一个标准的通用异步接收/发送通讯接口，接收和发送都是双缓冲的，有自己的是能和中断位，支持半双工和全双工工作。通讯接口有SCITXD和SCIRXD的TTL电平信号。本设计是使用AS-WLCOM无线模块。本设计用的是RS-232电平接口，通讯波特率设置9600bps,无奇偶校验位，8位数据位和一个停止位。AS-WLCOM的链接方法如图10所示。10基于DSP的全向运动控制系统软件设计DSP下位机图10无线发送接收模块连接图指南针模块采用C26平面数字指南针模块图具有磁偏角补偿功能

29、，通过硬磁补偿和设置地磁夹角能够表示成全向机器人的夹角。指南针模块使用232（9600bps）协议及IIC协议。本设计中采用IIC通信协议。测量范围：0°360°、分辨率：0.1、°测量精度：1°、重复精度：1°、响应频率：30H乙DSP2407A中SCI已经使用了无线模块，所以在本很设计中使用IIC通讯协议进行通信。IIC通信模块地址及数据含义见表2。表2内部数据模块内部地址地址的数据含义0x00未用到0x01角度值高8位0x02角度值低8位0x03磁偏角高8位0x04磁偏角低8位0x05未用到0x06未用到0x07校准等级值实际当前角度值为

30、上表“角度值高8位”与“角度值低8位”合成的16位数据,数据范围0-3599（因为分辨率为0.1°）实际当前磁偏角值为“磁偏角高8位”与“磁偏角低8位”合成的16位数据,数据范围0-3599（因为分辨率为0.1°）通过IIC控制指南针命令，磁偏角的值，范围0-3599（因为分辨率为0.1°）,是由2个8位的数据组成，当修改模块磁偏角时，分为高8位值，低8位值，写入模块其命令见表3。11基于DSP的全向运动控制系统软件设计表3模块命令表命令值作用0x00+0x31角度测量0x00+0xC0校准0X00+0XC1停止校准0x00+(0xA0+0xAA+0XA5+0XC

31、5)恢复出厂设置0x00+(0xA0+0xAA+0XA5+IIC_ADDR)IIC地址修改0x03+磁偏高8位值磁偏角修改0x03+磁偏高8位值磁偏角修改液晶显示模块使用1.7英寸TFT全彩屏，显示运动过程中各种状态和参数如图11,通过显示模块能够很好的显示运动状态，同时系统出现问题也能通过显示反应出来。I上瑞晶盅UN次+UZ巳口忆1阳出出SSS图11液晶显示电机及驱动模块电机使用Faulhaber带双路编码器减速电机223012V220,减速后速：220RPM（转每分钟），每圈脉冲：512CPR（脉冲每圈），对于编码器将在下章介绍。对于电机驱动本设计选用L298N,它是一款具有15管脚Mul

32、tiwatt封装的大功率集成驱动芯片。它是一个能够承受高电压，大电流的双H桥驱动。L298N可驱动2个电机，OUT1,OUT2和OUT3,OUT4之间可分别接电机。L298N功能模块如表4所示。表4L298N功能模块ENAIN1IN2运转状态0XX停止110正转101反转111刹停100停止12基于DSP的全向运动控制系统软件设计L298N驱动电路如图12所示。单个电机的控制方法如下：通过IO口控制IN1和IN2,使能ENA为PWM,则通过控制IN1和IN2来实现正反转，通过ENA的PWM占空比调节速度快慢。图12L298N电机驱动电路原理图全向轮作为全向运动的驱动部分，驱动轮与普通驱动轮有着

33、重要的区别。在驱动轮的边缘处有侧滑的小轮，他们能够使得轮子向前后提供驱动力，同时能够使的侧面与地面很小的摩擦力，实现侧面移动。4系统运动控制的MATLAB!本设计的全向运动控制系统使用三轴全向控制，即控制3个电机相互协调运动来实现全向轮动。电机的控制对整个系统是至关重要的。本章主要分析系统的PID控制和MATLAB仿真。4.1电机PID控制电机是运动系统的执行机构，不管DSP#算的多么正确，但是电机执行达不到要求，同样系统是无法实现全向运动的。为使得电机能够稳、准、快的执行，而PID是根据偏差的比例、积分、微分的线性组合进行反馈控制，是迄今为止工业中应用最为广泛的一种控制方法。所以本设计采用P

34、ID分别对三个电机进行闭环控制。数字PID可分为位置式PID和增量式PID控制。由于位置式PID是全量输出，所以每次输出均与过去的状态有关，计算时要对e(k)进行累加，DSR!算量大。而增量式PID控制的优点是只输出增量，计算误差动作时造成的影响小，同时对控制算法不需要13基于DSP的全向运动控制系统软件设计累加，增量只跟最近几次的偏差采样值有关，易获得较好的控制效果90本设计采用增量式PID,其闭环系统见图13所示。图13电机闭环控制系统已知模拟PID控制算法的传递函数为式（8）:USDSKPES1-1STdsTi数字式PID前一刻输出的值为式（9）:Tku（k-1）=KpJe（k-1）+3

35、e（i）TTdLe(k-1)-e(k-2)lTo(8)(9)前一时刻减去后一时刻的值为式（10）:：(k-1)=u(k)-u(k-1)=kpl.e(k)-e(k-1)lkekkdle(k)-2e(k-1)e(k-2)l(10)数字PID最终输出量为式（11）:u(k)=u(k-1)u(k)(11)4.2转速检测本设计选用增量式PID即4.1节中所阐述的PID。对于PID控制反馈必不可少，在本设计使用的是Faulhaber带双路编码器减速电机，即增量式光电编码器来做为反馈环节，其工作原理如图14。光源经过码盘和检测光栅照射的信号通过光电检测器件检测，检测出的正弦值经转换电路转换成矩形波，最后出来

36、A相B相Z相。A相B相相差90度，通过软件能够很好的得到此时电机的速度和方向。对AB相的采集是电机闭环控制的反馈量，关乎电机PID控制是否能达到稳准快的要求。本设计使用输入捕捉CAP功能对脉冲进行采集，将采集量进行相应处理即得到PID控制的反馈。14基于DSP的全向运动控制系统软件设计光源码盘检测光明”舞卧转毋电躇正核波形下波17所示。绿色曲线为期望曲线，蓝色曲线,jiJirLJi_rLrLrL”相_ji_ri_rLrLrLrLrL1r5B千目_nZ相：14增量式编码器输出信号波形4.3MATLAB仿真在三轴控制系统中，PID是应用最广泛的控制算法。针对电机模型，这里设定电机的模型为G（s）=

37、23，针对此电机模型进行MATLAB仿真，观察PID控制算法2s3s4的控制效果1011。控制结果比较曲线如图15所示。绿色曲线为期望值，蓝色曲线为PID控制跟踪曲线，红色曲线为误差曲线，实际速度可以通过编码器反馈。可以看出PID控制方式的响应速度、跟踪精度可以达到系统要求。针对三轴运动控制系统设计以下速度合成实验，假设机器人坐标系和场地坐标系具有同样的方位，要求机器人（作为整体）沿45。（中=45=）的方向从静止开始，匀加速运动2秒，达到1.45m/s,然后匀速运动2秒，再在1秒之内以匀减速将速度减为0,从启动到停止共运行5s。运行结果如图16所示，绿色曲线为期望曲线，蓝色虚线为PID控制跟

38、踪曲线，从图中可以看出，PID控制可以达到系统的控制要求。动轴单独控制，三个轮子相应跟踪曲线如图15基于DSP的全向运动控制系统软件设计为PID控制跟踪曲线，从曲线中可以看出,(b)2号电机图17三个电机相应跟踪曲线PID控制能够达到三轴运动从上面的速度以及位移跟踪曲线的仿真结果可以看出,控制系统的要求。5系统软件设计根据前几章模型建立分析和硬件系统构建，本章主要介绍全向运动控制系统程序编写。前两节介绍了下位机全向运动系统使用的程序编写平台软件和整个系统流程图及各部分子程序的流程图。最后一节介绍上位机部分，编写上位机软件部分和上位机下位机使用的通讯协议。5.1 软件开发平台及仿真器本设计的核心

39、控制芯片为TMS320LF2407右软件开发平台使用官方工具CCS/3.3,仿真器选用XDS510-USB2.0安装CCSV3.3后生成两个部分，即CCStudioV3.3和SetupCCStudiov3.3。其中利用SetupCCStudiov3.3对开发工具参考硬件进行相应配置，1618所示12。图19即为基于DSP的全向运动控制系统软件设计利用CCStudioV3.3建立工程编写软件，具开发周期示意图为图CCStudioV3.3开发环境。图18CCS开发周期示意图/F24O710451口KauiIatdz_/Cf0_1-TKS32UCZxk-CodeCDi|JU:reiStudiu-Hu

40、tCcnnrctcd-bbiel.c$EileliltMiwproject融by国曜用电c|lotil_3J.pjtDebucSILQplioTiZoalsYinimUtlp-3置,帝僧丐7通K居0厘L*寸Frflj«ctsB宙pjlQ_3口纳5日止PrajeIDoetMAntt_JD5F/EE05CouiuIG4iArAtliFi1A5I_|lududtl;±1口上皿.-5oarce刃*u"1drsiciLcJjfljQBALC!IM8163.C_周|%7国D5P.OT|加打占用：麻7一儆J再蛉FMQ-F.然和FR47登砌FW葆彘屏粳甩二式33.ET="

41、;狼，上寸卬邛二亳.寸朋雄.生ur.HF.心=/对：'二三型砒七网手7耳铲.？FR-魂柢史夏蓉#incliid&*globol.cHneInde"mathJ.voidSystiiiit();voidTiffierlTnitQ;voidlLI9163_init();widRickDogO:voidPWM.Init)vi&idI0init():voidTimer4Init();voidTimer?Initf):主程序jhj.1()Syste«Init();''系统优ICinitO；5比7题捺之SCI_Ioit)ILI9163_iDitOj

42、TuwrllnitO；定时篓初始住中Tinier4Init()iTunsr2lDit();b1sh.("CLRCINTM")：.displayfOnffOTOsffeO)iLCT_PDtStriii9(40.03L率海清",Qs001f.(h07e0)；LCD_PutStnns(l6,23,OsO(JltJi07bO)j"liilftU)Jtflb3T,Cd.6图19CCStudioV3.3开发环境仿真器XDS510-USB2.0的作用：通过CCS3.3编译的程序下载DSP上两种方式,一个是烧写FLASH,一个是仿真。烧写FLASH即将编译好的程序烧写进

43、DSP内部FLASH中，此种方法能够是DSP独立运行软件，具有掉电保持功能。仿真是将程序下载到外部数据存储器RAM中，其特点是下载速度快，但掉电数据丢失，适合在开发软件频繁修改阶段使用。而烧写FLASH则比较复杂烧写时间比较长，不利于开发软件阶段使用叽17基于DSP的全向运动控制系统软件设计通过仿真或者烧写FLASHY可以在线监控DSP各个寄存器和变量的值，有利于排除各种错误。5.2 运动控制软件设计由于本课题是基于DSP全向运动控制系统软件设计，侧重控制全向运动，故本设计将检测部分做了精简，只需要采集指南针和编码脉冲。其全向运动方式经过上位机无线传输指令，然后DSP接收指令进行处理最后控制电

44、机协调运动，根据指南针和编码器进行闭环控制，以达到最佳效果。5.2.1 主程序开始图20主程序流程图18基于DSP的全向运动控制系统软件设计根据前几章的设计和分析，设计出软件系统流程图，如上图20所示。系统上电后开始，系统初始化后开始检测是否有接收数据，接收到之后判断运动方式，如是停止信号则电机停止运动。如接收到的信号是平移运动则进入平移运动相应子程序，计算电机速度值并赋值给PWM寄存器，电机驱动控制驱动轮转速。同时电机PID会根据设定值和误差进行调节，使得系统运行更加稳定。通过软件的反复运动则完成了整个系统的运动过程。5.2.2 三种基本运动状态子程序当接收到的数据判断出要运行那种运动方式后

45、，将进入相应子程序中。子系统流程图如下图21所示。按照协议得到各种参数值利用指南针及相关公式得到三电机值I屏幕显相关参数及指南针角度+返回图21运动方式子程序流程图19基于DSP的全向运动控制系统软件设计本课题设计的软件有三种运动方式，即有三种运动状态控制子程序。由于三种运动状态子程序其流程大致相同，不同之处在于对接收到的数据的处理和相应的计算以及相关显示部分。故将三个子程序软件系统流程图用一个表示。其中“屏幕显示相应运动界面”显示的是运动状态的界面。只有三种运动状态发生改变时此界面才变化，这是为了节省系统执行时间，因彩屏执行完此界面操作需要占用部分时间。显示的其他参数则实时变化。这样能够通过

46、TFT液晶显示出运行方式和参数，有利于调试程序和查找错误。5.2.3电机控制子程序电机控制主要是将其他子程序计算好的速度值进行PID控制，这样能够使得三个电机安装设定的速度运行。电机的稳、准、快的运行将会使运动方式更接近理论目标。故将电机控制部分单独设为子程序进行叙述。其子程序系统流程图见下图22所示。图22电机控制子程序流程图PID执行周期为3m§利用定时器计时，当时间到之后PID标志位置1,进入PID子程序后清楚此标志位。5.2.4 无线发送子程序无线发送子程序流程图如图23所示。20基于DSP的全向运动控制系统软件设计将三组数值进行相应转换写入SCI发送寄存器图23无线发送子程

47、序为使得上位机更好的监控系统，同时有利于PID调试，故将得到的编码值经无线传输模块发送给上位机软件，并在相应位置显示出来。发送的数据不是一个数据，而DSP5次只能发送8位数据，故需要发送多次并检测是否发送完毕。如没有发送完毕则继续发送，如果发送完毕则返回主程序。5.3 上位机软件及通讯协议上位机软件是利用VC6.0的MSOCOMM控件编写。上位机软件界面如图24所示图24上位机界面21基于DSP的全向运动控制系统软件设计MSComm控件是MicrosoftCommunicationsControl的简称，是Microsoft公司提供的简化Windows下串口编程的ActiveX控件，它为应用程

48、序提供了通过用口接口收发数据的简便方法14o同时方便不同电脑的串口情况需求，设计上位机软件时设置了4个COM口（1-4）。为了更好的观测和研究几种运动状态，故上位机软件设计成各个运动状态分开的界面。最后加上一个“演示”模块，即设定几个特殊方向角利用图片直观表示出来，能够实现平移和边平移边旋转的运动，能够对整个设计系统有个更直观的操作。选才¥COM并打开，选中要运行的运动类型并在相关位置写上各种参数。点击开始，则上位机软件将写入的参数进行相关处理并按照一定的协议组织要发送的数据，通过RS232经无线模块发送出去。本设计系统通讯协议为简单通讯协议15o协议格式见表5所示。表5简单通信协议

49、命令分类协议格式备注速度分解*xaaaybbbzccc*“*”为开始和结束标志，“aaa”、“bbb”、“ccc”为电机速度，“xyz”为电机正反转标志1为正转，2为反转速度平移*#aaaxbbb0y00*“*”为开始结束标志，“#”代表速度合成平移标志，“aaa”整体速度，“x”为顺时针或逆时针平移（1为顺时针2为逆时针），“y”为机器人方向是否跟随移动方向（1为摆向移动的方向，0为头不随移动而改变）原地旋转*xaaabbb0000*“*”为开始结束标志，“”标志原地旋转标志，“x”为顺时针或逆时针旋转（1为顺时针2为逆时针），“aaa”旋转角度，“bbb”为速度。同速度合成平移同速度合成平

50、移在上位机选择不同的运动方式和相关参数，通过无线传输模块传送给DSP,在DSP上对接收到的数据根据协议进行处理，提取出运动方式和相关参数，然后进行控制小车运动。小车也将采集到的速度值经过无线模块传送给上位机。例如让系统往05045T运动，即系统往正Y轴以50的速度前进且旋转，那么发送的协议为：*#05010450100*。*112010582105*:即分别控制三电机，电机1速度正转120,电机2正转58,电机3反转105。6实验验证及结果分析为验证本设计的各种全向运动是否准确，本章将对各种全向运动形式进行试验验证。设计试验场地，记录测试试验数据，对结果进行分析。22基于DSP的全向运动控制系

51、统软件设计6.1 试验场地由于本设计使用的驱动轮为尼龙材质。驱动轮与地面接触部分硬度较大，如果在普通地板上驱动轮将得不到足够大的摩擦力，有严重的“打滑”现象。为提供足够摩擦力结合实验室条件，选择飞思卡尔赛道使用的KT板，做成120CM*100cM的试验场地,并在场地上标上边距为20cm的方格，同时为验证转角正确性标上了相隔45°的虚线,结构图见图26。(b)实际场地图26试验场地23基于DSP的全向运动控制系统软件设计6.2 各项性能测试需要试验验证测试的有：PID验证、平移验证、原地旋转验证、边平移边旋转运动。设计制作试验装置，分别对各种性能测试。6.2.1 速度PID测试对电机P

52、ID效果进行测试，测试方法为记录从启动到稳定后一段时间内相隔时间段增量编码器记录数。在系统中每隔3ms记录一次3ms内记录数。在这个试验中测试占空比为200（占空比2/5）的上升曲线。下面红色线为开环曲线，上面深蓝色曲线为PID的效果曲线。能够满足要求，效果如下图痴I丽丽曲线210.器码编.PWM200PIDPWM200_OOOOO432101111180706050403020020406080100120140160180200220240260280300时间ms图27电机实际PID效果曲线06.2.2 平移运动测试平移运动时时全向运动的最主要部分，其是否准确稳定关乎全向运动系统整体的成

53、败。在验证平移性能的时，采用在系统上安装自制铅笔留痕装置（如图28）,能够跟随系统运动时留下系统运动的轨迹，依此来采集系统平移是的各项性能。图28留痕装置24基于DSP的全向运动控制系统软件设计下图29为系统沿x轴负方向平移的曲线图，起始位置为(11020),结束位置为(1019.5),即走过1m的距离误差e=0.5mm最大1.1cm。图29系统平移曲线6.2.3 原地旋转测试系统安装有数字指南针，能够根据地球磁场感知方位。其快速性和精确性为系统准确的方位提供可能。验证指南针转角是否正确比较简单，直接观看TFT液晶显示的指南针角度就可以知道准确性。同时可以参考试验场地标出互成45°的

54、虚线看是否转到位，下图30为试验场地转角有0°到450时的情况。图300。到45。转角示意图25基于DSP的全向运动控制系统软件设计通过指南针显示能够很清晰的看到，最终转到的角度已经达到了设定角度，误差在±1°,而且此误差是为了避免转到角度出现来回校准而设置的死区，故原地旋转运动能够准确的执行。6.2.4 平移+旋转运动测试全向运动特与其他运动形式最大的区别在于它能二维平面内不但能够不做任何准备的情况下往任意方向平移，但最高级运动形式为在全向平移的基础上能够调整自身姿态也就是平移过程中自身能够旋转。下图31为系统由（2020）到（8080）边移动边旋转，黑色三角箭

55、头为系统方向。图31系统平移+旋转曲线经显示测试，其最终坐标在（8278）,误差约等于在3cni6.3 影响因素分析影响系统性能的因素大部分来源于外部，大致分为以下几种情况：（1）电池电量不足，导致系统电机启动或大的变化时电流不足。导致系统不能很好的按照设定的形式运动。（2）外部磁场干扰，数字指南针能够很灵敏的感应磁场，所以外部稍强点的磁场就能干扰系统，一方面是金属铁，其很容易磁化，导致变成弱磁铁。在室内钢筋结构的环境下各个位置受到的磁干扰不同，经测试最大可影响角度达30°角，严重影响了系统性能。另一方面是电机，由于电机带有磁性较强磁铁，产生磁场的磁场也对指南针有一定干扰。去除干扰的方法只能经过硬磁补偿来减少很少部分干扰，大多数干扰无法去除。26基于DSP的全向运动控制系统软件设计(3)驱动轮干扰，本设计使用的是瑞典全向轮，轮子外侧有两排相互交错的侧轮，使得驱动轮滚动过程中，造成上下震动。同时由于驱动轮滚动会造成电机力矩发生变化，L=12或14.5cm,及内外两个侧滑轮着地时会造成力矩2.5cm的变化，会使得系统转动发生误差。27基于DSP的全向运动控制系统软件设计结束语全向运动控系统在全向机器人上的应用可实现机