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

1、基于DSP的全向运动控制系统软件设计 基于DSP的全向运动控制系统软件设计 摘 要 本文基于DSP C2000系列TMS320LF2407A核心控制芯片，以CCStudio V3.3软件为开发平台，在了解RoboCup中型组足球机器人和其他全向机器人的基础上，主要完成了全向运动控制系统的软件设计。通过对全向运动控制系统的研究，建立了三轴全向运动学数学模型，并对整体平移运动、原地旋转运动、边平移边旋转三种运动方式进行分析和数学建模；利用上位机，经无线模块发送运动方式和各种运行参数，控制运动系统实现各种运动模式；经全向运动控制系统软件的编写和与相关硬件联机调试，系统实现了预订的全向运动形式。通过试

2、验验证和结果分析，各种运动状态的准确性已达到性能的基本要求。关键词 DSP；全向运动控制； 数学建模 ；MATLAB仿真 ；串行通信;软件设计Software Design of Omni-directional Motion Control System Based on DSPAbstract: 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 p

3、latform. 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 t

4、o 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 rotating.Using the PC, The wireless module to send the movement and a variety of operating pa

5、rameters, 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 accu

6、racy have already meet requirements of various sports state.Key Words: DSP; omnidirectional motion control; mathematical modeling; MATLAB simulation; serial communication；software design目 录1 引言 11.1 全向运动控制系统发展现状 11.2 本课题的研究意义及前景 21.3 论文组织结构 32 全向运动控制系统分析 32.1 全向运动控制系统运动学模型建立 32.2不同运动方式的运动特性 52.2.1 平

7、移运动 52.2.2 原地旋转运动 72.3.3边平移边旋转运动 83 基于DSP的硬件系统简介 83.1 控制芯片选择 83.2 硬件系统结构图 93.3 硬件系统基本模块 104 系统运动控制的MATLAB仿真 134.1 电机PID控制 134.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.

8、2 各项性能测试 246.2.1 速度PID测试 246.2.2 平移运动测试 246.2.3 原地旋转测试 256.2.4 平移+旋转运动测试 266.3 影响因素分析 26结束语 28参考文献 29附录 30致谢 421 引言随着机器人技术的日新月异，机器人应用领域也已从工业走向普通生活。机器人技术已经在人们的生活中越来越发挥着重要的作用。作为机器人中的全向运动机器人，有着其特殊的运动形式，即可以不改变姿势向任何方向运动，找到最佳位置。同时也可以边往某个方向运动，边调整自身的姿势达到最佳运动效果，可以在平面内完美的往任何方向做三自由度运动。全向运动机器人其高灵活度，势必会在机器人领域发挥不

9、可代替的作用。在全向运动机器人中现阶段比较常见的是三轮结构和四轮结构（如图1所示）。通过控制各个轮子间相互协调运动达到全方位的运动效果，包括全方位平移、全方位边平移边旋转和原地旋转运动形式。要完整文章毕业论文的+扣扣：（149-908-83-24）去掉中间的横线图1 三轮和四轮的结构模型2 全向运动控制系统分析2.1 全向运动控制系统运动学模型建立在引言中介绍了全向运动控制的几种结构，本设计选用三轮结构构建全向运动系统34。其结构图和实物付如图4所示 。 (a) 结构图 (b) 实物图 图4 全向结构图和实物图由图5中(b)所示，V为机器人系统整体速度，世界坐标X-Y坐标系中X轴与整体速度V的

10、逆时针夹角为，在世界坐标系中将整体速度V分解成沿X轴和Y轴的速度分别为和。2.2不同运动方式的运动特性 全向运动的运动形式大体可分为平移运动、原地旋转运动、边平移边旋转运动三种运动形式。三种运动形式有着不同的运动特性，以下将分三小节对三种运动形式进行单独分析。2.2.1 平移运动平移是基本的也是比较常用的运动形式。由于是平移不考虑旋转运动，我们将模型进行简化，其简化后运动学模型图如图6所示。 (a) 平移模型 (b) 系统整体平移模型图6 简化后运动运动学模型单独考虑平移运动故将式(3)进行简化为式(4)： (4)假设小车整体平移矢量速度为V，与X轴成角。所以我们可以分解为和，其分解公式为式(

11、5)。 (5)由式(4)和式(5)我们可以得到三个驱动轮的速度，见式(6)。 (6)驱动轮速度、的速度范围都是。的值则由电机性能决定。通过式(6)我们可以反向推导出平移速度V的速度范围。如图7，其中圆是以驱动轮最大速度大小为半径的圆，红色速度V与X轴的角度为，随着由0360变化V的轨迹为红色正六边形。其中最大速度为。图7 平移最大速度模型3 基于DSP的硬件系统简介3.1 控制芯片选择作为运动控制系统的核心控制芯片，根据以往在实验室的经验，其核心控制芯片当属DSP C2000系类。C2000系类对于运动控制有着诸多的优点，本设计采用C2000系类TMS320LF2407A的控制板。TMS320

12、LF2407A主要特点如下： 3.2 硬件系统结构图 通过第2章关于运动学模型的建立，为使得系统能够按照建立的数学模型进行运动，除了3.1节的DSP主控制芯片外还需要其它的硬件的支持，本课题设计的硬件系统结构图如图9所示。其主要的工作方式为： PC上位机选择相应的运动方式，经过RS232协议连接无线模块，由无线模块发送数据。DSP下位机的无线模块接收上位机发送的数据，经RS232通信协议发送给DSP。DSP接收到数据进行相应的处理，分析出要进行的运动形式及各种参数，同时利用I2C协议采集指南针的数据，将这些信息通过第2章的全向运动学模型分析的各种公式转换成各个电机的速度值。为使得电机能够稳、准

13、、快的执行，本设计采用经典PID控制，以增量式编码器采集的值为反馈。在进行的过程中液晶模块实时显示运动方式和参数及指南针的值，同时将编码器的值通过无线模块传送给PC上位机。DSP下位机将按照程序设定执行上位机设定的运动方式，只有上位机重新发送新的运动方式才会改变。3.3 硬件系统基本模块根据图9硬件系统结构图所示，本节将分析几个重要的模块。分为电源模块、无线通信模块、指南针模块、液晶显示模块，DSP下位机模块、电机及驱动模块和全向轮。其中DSP下位机3.1节已经介绍，在此不再叙述。要完整文章毕业论文的+扣扣：（149-908-83-24）去掉中间的横线4 系统运动控制的MATLAB仿真本设计的

14、全向运动控制系统使用三轴全向控制，即控制3个电机相互协调运动来实现全向轮动。电机的控制对整个系统是至关重要的。本章主要分析系统的PID控制和MATLAB仿真。4.1 电机PID控制电机是运动系统的执行机构，不管DSP计算的多么正确，但是电机执行达不到要求，同样系统是无法实现全向运动的。为使得电机能够稳、准、快的执行，而PID是根据偏差的比例、积分、微分的线性组合进行反馈控制，是迄今为止工业中应用最为广泛的一种控制方法。所以本设计采用PID分别对三个电机进行闭环控制。4.2 转速检测 本设计选用增量式PID即4.1节中所阐述的PID。对于PID控制反馈必不可少，在本设计使用的是Faulhaber

15、带双路编码器减速电机，即增量式光电编码器来做为反馈环节，其工作原理如图14。光源经过码盘和检测光栅照射的信号通过光电检测器件检测，检测出的正弦值经转换电路转换成矩形波，最后出来A相B相Z相。A相B相相差90度，通过软件能够很好的得到此时电机的速度和方向。对AB相的采集是电机闭环控制的反馈量，关乎电机PID控制是否能达到稳准快的要求。本设计使用输入捕捉CAP功能对脉冲进行采集，将采集量进行相应处理即得到PID控制的反馈。4.3 MATLAB仿真在三轴控制系统中，PID是应用最广泛的控制算法。针对电机模型，这里设定电机的模型为，针对此电机模型进行MATLAB仿真，观察PID控制算法的控制效果101

16、1。控制结果比较曲线如图15所示。绿色曲线为期望值，蓝色曲线为PID控制跟踪曲线，红色曲线为误差曲线，实际速度可以通过编码器反馈。可以看出PID控制方式的响应速度、跟踪精度可以达到系统要求。要完整文章毕业论文的+扣扣：（149-908-83-24）去掉中间的横线5 系统软件设计根据前几章模型建立分析和硬件系统构建，本章主要介绍全向运动控制系统程序编写。前两节介绍了下位机全向运动系统使用的程序编写平台软件和整个系统流程图及各部分子程序的流程图。最后一节介绍上位机部分，编写上位机软件部分和上位机下位机使用的通讯协议。5.1 软件开发平台及仿真器本设计的核心控制芯片为TMS320LF2407A，其软

17、件开发平台使用官方工具CCS v3.3，仿真器选用XDS510-USB2.0。安装CCS V3.3后生成两个部分，即CCStudio V3.3 和Setup CCStudio v3.3 。其中利用Setup CCStudio v3.3对开发工具参考硬件进行相应配置，利用CCStudio V3.3建立工程编写软件,其开发周期示意图为图18所示12。图19即为CCStudio V3.3开发环境。5.2 运动控制软件设计由于本课题是基于DSP全向运动控制系统软件设计，侧重控制全向运动，故本设计将检测部分做了精简，只需要采集指南针和编码脉冲。其全向运动方式经过上位机无线传输指令，然后DSP接收指令进行

18、处理最后控制电机协调运动，根据指南针和编码器进行闭环控制，以达到最佳效果。6 实验验证及结果分析 为验证本设计的各种全向运动是否准确，本章将对各种全向运动形式进行试验验证。设计试验场地，记录测试试验数据，对结果进行分析。6.1试验场地 由于本设计使用的驱动轮为尼龙材质。驱动轮与地面接触部分硬度较大，如果在普通地板上驱动轮将得不到足够大的摩擦力，有严重的“打滑”现象。为提供足够摩擦力结合实验室条件，选择飞思卡尔赛道使用的KT板，做成120CM*100CM的试验场地，并在场地上标上边距为20cm的方格，同时为验证转角正确性标上了相隔45的虚线，结构图见图26。6.2 各项性能测试 需要试验验证测试

19、的有：PID验证、平移验证、原地旋转验证、边平移边旋转运动。设计制作试验装置，分别对各种性能测试。6.3 影响因素分析 影响系统性能的因素大部分来源于外部，大致分为以下几种情况：（1） 电池电量不足，导致系统电机启动或大的变化时电流不足。导致系统不能很好的按照设定的形式运动。(2） 外部磁场干扰，数字指南针能够很灵敏的感应磁场，所以外部稍强点的磁场就能干扰系统，一方面是金属铁，其很容易磁化，导致变成弱磁铁。在室内钢筋结构的环境下各个位置受到的磁干扰不同，经测试最大可影响角度达30角，严重影响了系统性能。另一方面是电机，由于电机带有磁性较强磁铁，产生磁场的磁场也对指南针有一定干扰。去除干扰的方法

20、只能经过硬磁补偿来减少很少部分干扰，大多数干扰无法去除。(3） 驱动轮干扰，本设计使用的是瑞典全向轮，轮子外侧有两排相互交错的侧轮，使得驱动轮滚动过程中，造成上下震动。同时由于驱动轮滚动会造成电机力矩发生变化，L=12或14.5cm，及内外两个侧滑轮着地时会造成力矩2.5cm的变化，会使得系统转动发生误差。要完整文章毕业论文的+扣扣：（149-908-83-24）去掉中间的横线结束语全向运动控系统在全向机器人上的应用可实现机器人向任意方向做直线运动而不需要事先作旋转运动，并且在以直线运动到达目标点的过程中同时可以做自身旋转运动，以调整机器人的姿态，从而达到终态所需的姿态角。其特殊的移动方式使得

21、全向机器人越来越得到重视和应用，其未来的实用领域和前景将更加宽广。本设计完成的主要工作为以下几点：(1) 对目前的几种运动结构进行了研究分析，设计了互成120度的三轴运动结构。(2) 对三轴运动控制结构进行了数学建模，进行了运动学分析。对全向运动的运动的几种运动形式进行了详细的分析计算。(3) 运用TMS320LF2407A芯片为控制核心，使用了其模块UART（SCI）、IIC（模拟）、PWM、I/O、CAP、外部扩展RAM。同时围绕核心的外部器件进行程序的编写调试，已达到全向控制的目的。(4) 使用了VC6.0的MSComm控件编写上位机软件，设计上位机界面和功能，编写通信协议。使得系统能够

22、经过选择相关运动形式，修改上位机的相关参数来实现运动系统的全向运动。同时能够在上位机显示系统现在运动的状态和三个电机速度值。并实现了通过游戏手柄来遥控系统的各种运动状态。(5) 对多电机实现了数字增量式PID控制，三个电机各自PID控制且互不影响，并通过MATLAB进行了仿真。使得计算出的速度能够很好的执行，提高了系统的准确性。(6) 实现了全向运动控制系统的所有运动形式，且经过试验测试验证了其准确性。参考文献1 徐旭,李实,叶榛,孙增圻.A Survey: RoboCup and the Research Proceedings of the 3rd World Congress on In

23、telligent Control and AutomationM,20082 赵冬斌,易建强,邓旭玥.全方位移动机器人结构和运动分析J.机器人.2003(25) 3 海丹.全向移动平台的设计与控制D.长沙:国防科技大学. 20094 龚建伟.Visual C+/Turbo C串口通信编程实践. 北京.电子工业出版社，20045 RaulRojas:Omnidirectional Control,FreieUniversityBerlin,Technical Report B-10-03, FU.Bedin.June 20036 刘和平.DSP原理及电机控制应用基于TMS320LF240X系列

24、M.北京:北京航空航天大学出版社,2008.7 王茂飞,程昱.TMS320C2000DSP技术与应用开发M.北京:北京清华大学出版社,2007.8 Texas Instruments.TMS320LF/LC240xA DSP Controllers Reference Guide-System and Peripherals SPRU357BM.December,2001.9 陶永华，尹怡欣，葛芦生.新型PID控制及其应用M.北京.机械工业出版社.199810 薛定宇.控制系统计算机辅助设计-MATLAB语言与应用（第2版）M.北京:清华大学出版社,2006. 11 范海艇.基于Nios的中型

25、足球机器人底层控制系统的研究与实现D.上海,上海大学. 12 姜艳波.数字信号处理器-DSP应用100例M，北京：化学工业出版社，200713 海丹.全向移动平台的设计与控制D.长沙:国防科技大学，200614 张宏林.精通Visual C+串口通信技术与工程实践(第3版) .北京. 人民邮电出版社，200815 龚建伟.Visual C+/Turbo C串口通信编程实践（第二版）M,北京：电子工业出版社，2007.9附录 基于DSP的全新运动控制系统软件清单如下:其中自己建的有GB161.H（存储液晶字模数据），24C02.C（操作指南针），design.C（运动控制策略），ILI9163.

26、C（液晶显示），main.c（主程序）。由于程序比较大，在此只附录上main.c和design.c两个文件。Main.c ：/*/* File Name : main.c */* Autor : Li Hai-qing */* Date : 04/01/2011 */* Description : This file provides main function, provide initialization */* 串口发送与中断接收程序,波特率设置为9600 */* IO口占用：PE1-PE6（PWM） PA3-PA6和PB0-PB7和PE7液晶屏使用 */* PF0=CAP5,PF1=c

27、ap6,PA3=cap1三电机测速。PE2=PWM8 PE4=PWM10，PE6=PWM12三电机 */* PC0-1 PF2-3 PF4-5电机正反转 */*/#include global.c #include math.hvoid SystemInit();void Timer1Init();void ILI9163_init(); void KickDog(); void PWM_Init();void IOinit();void Timer4Init();void Timer2Init();/*/* 主程序 */*/main() SystemInit(); /系统初始化 IOinit

28、(); /IO口初始化 SCI_Init(); /SCI初始化 PWM_Init(); /PWM初始化 ILI9163_init(); /液晶初始化 Timer1Init(); /*定时器1初始化*/ Timer4Init(); /*定时器4初始化*/ Timer2Init(); /*定时器2初始化*/ asm( CLRC INTM ); display(0xff07,0xffe0); LCD_PutString(40,03,李海清,0x001f,0x07e0); / 液晶界面显示信息 LCD_PutString(16,23,运动控制系统,0x001f,0x07e0); / LCD_PutSt

29、ring(0,143,指南针:,0x001f,0x07e0); / while(1) SciRed(); /判断并读取SCI数据 speedPV(); /速度函数 /*/* 系统初始化 */*/void SystemInit() asm( SETC INTM ); /* 关闭总中断 */ asm( CLRC SXM ); /* 禁止符号位扩展 */ asm( CLRC CNF ); /* B0块映射为 on-chip DARAM*/ asm( CLRC OVM ); /* 累加器结果正常溢出*/ SCSR1=0x834E; /* 系统时钟CLKOUT=20*2=40M */ /* 打开ADC,EVA,EVB,CAN和SCI的时钟*/ WDCR=0x006F; /* 禁止看门狗,看门狗时钟64分频 */ KickDog(); /* 初始化看门狗 */ IFR=0xFFFF; /* 清除中断标志 */ IMR=0x000B; /* 打开中断1,2*/ /*/* IO口初始化 */*/void IOinit() MCRC=MCRC & 0x0354; /PE0-PE7，PF0-PF5有效 0000 MCRA=MCRA & 0x0008;