基于双DSPs架构的移动机器人运动控制系统的设计

李怀俊

(1.华南理工大学 机械与汽车工程学院，广州 510640；2.广东交通职业技术学院，广州 510650)

0 引言

近年来，随着技术的发展，移动机器人广泛地应用在工业现场、医院和家庭等环境。为了适应不同的环境，机器人的机动性变得越来越重要，而机器人的机动性取决于移动平台的设计。因各种不同环境的需求与限制，目前已研制出足型机器人、轮型移动平台和爬虫型移动平台等移动机构，其中轮型平台的移动效率最好[1,2]。设计运动快速灵活的移动机构必须考虑两个重要因素：运动机构的性能和运动机构的可控性。运动机构性能决定了机器人运动性能的潜能，运动机构的可控性决定了运动机构性能能否被有效地利用起来。由于采用全向轮运动机构的机器人向任意方向做直线运动的同时可做自身旋转运动调整机器人的姿态，这样，就使得移动机器人有运动快速灵活、易于控制等优点。因此，全向轮已逐渐取代传统的差动轮系，成为移动机器人设计发展的趋势。

1 运动控制系统

1.1 机器人运动学模型

全向轮及其运动机构如图1所示。

图1 全向轮及其运动机构示意图

全向轮运动机构包括全向轮、电机、驱动轴系等几部分。全向轮由全方向性的轮子组成，此种轮子在周围上装有与轮轴垂直的小轮，因此可自由地沿著两种方向移动。当驱动电机提供动力时，全向轮沿着轴向转动，此时全向轮上的小轮则无作用。反之，当全向轮往轮轴方向移动时，驱动电机则不输出扭矩，此时全向轮上的小轮变为辅助轮，减少移动时的摩擦力，帮助机器人移动。全向轮的优点如下：(a) 轮轴不需移动，即可改变机器人方向；(b) 原地旋转不需要回转半径；(c) 可同时且独立地控制机器人的移动与转动。

基于全向轮的机器人运动学模型如图2所示，VX为机器人体坐标系横坐标方向速度，Vy为机器人体坐标系纵坐标方向速度， w为机器人角速度，角速度正方向为机器人体坐标系逆时针方向。V1、V2、V3、V4分别为机器人1号、2号、3号、4号四个轮子的速度，正方向为机器人体坐标系逆时针方向。 R为驱动轮中心到车体中心的距离。

图2 机器人运动学模型

则四个轮子的速度分解公式如下：

1.2 双DSPs架构

本设计选择TI专门为工业控制和机器人控制设计的TMS320LF2407A[3,4]作为移动机器人的控制核心芯片，因为它具有资源丰富，运算速度快的优点，主要表现在：(a)有两个正交编码脉冲（QEP）电路；(b) 有16个16位的脉宽调制通道；(c)有串行通信接口SCI模块及串行外设接口SPI模块；(d) 40MIPS的执行速度为机器人的实时控制提供了保证。

但全向轮有四个电机，一个TMS320LF2407A提供两路QEP用来检测电机转速，如何处理另外两个电机的码盘信号？目前的方案有：DSPs＋CPLD、DSPs+集成芯片，但这将导致电路复杂、电路的可靠性降低，而且单个DSPs要处理四个电机的信号和其他外围事件，使得单个DSPs负担很重，实时性很难保证。因此，本设计采用双DSPs架构，一个DSPs为主处理器，另一个DSPs为从处理器。主处理器负责与上位机通信，接收上位机发出的四个电机速度值和执行一些控制命令，并且返回底层的信息（如电机码盘信号、指南针、电池电压等）给上位机，实时监控机器人底层硬件状态。从DSPs通过SPI通信协议与主DSPs进行通信，主DSPs将从上位机接收的3、4号电机的速度值发送给从DSPs，从DSPs将3、4号电机的码盘信号、电池电压和指南针等信息传送给主DSPs。由于控制系统采用双DSPs架构，提高了电路的可靠性和稳定性，同时双DSPs协同处理信息，处理速度快，实时性强。

1.3 系统硬件组成

图3 运动控制系统整体结构图

运动控制系统整体结构如图3所示。

整个电路[5]分为主从DSPs单元、电机驱动电路单元、电机码盘接口电路单元、电平转换电路单元、电池电压检测电路单元、其他辅助电路与功能扩展接口电路单元等几大块。作为控制核心的DSPs的SCI模块接收上位机发送过来的机器人速度控制命令，QEP模块获得电机的实际转速，SPI模块进行主从DSPs间的通信。DSPs经过控制算法，产生PWM，通过电机驱动电路来控制电机。

2 系统软件的设计

系统软件的设计[6]主要包括主DSPs、从DSPs、主从DSPs间的通信等部分，其中主DSPs程序流程如图4所示。从DSPs没有主DSPs的通信接收子程序，但多了与电子指南针的通信部分，整体来说从DSPs的程序流程与主DSPs基本相同。

图4 主DSPs程序流程图

2.1 通讯接收子程序

TMS320LF2407A的串行通信接口SCI模块支持CPU与其他使用标准格式的异步外设之间的数字通信。SCI接收器和发送器是双缓冲的，每一个都有自己单独的使能和中断标志位。两者都可以独立工作，或者在全双工的方式下同时工作。为了确保数据的完整性，SCI对接收到的数据进行间断检测、奇偶性校验、超时和帧出错的检验。本设计中上位机通过USB转串口232的方式与主DSPs进行通信。指令帧由5字节数据组成，数据帧第一字节为握手信号,第二字节到第五字节分别为四个电机的给定速度值。当DSPs接收一个完整的数据帧，通信接收程序将该指令帧保存起来，并将通信接收标志ReceValid置1。指令帧数据的格式如表1所示。此外，从DSPs也通过SCI模块与数字指南针进行通信。

表1 指令帧格式

2.2 正交编码脉冲程序

TMS320LF2407A带有两路正交编码脉冲（QEP）。该模块被使能后，可以在编码和计数引脚上输入正交编码脉冲。正交编码脉冲是两个频率变化且正交（即相位相差90度）的脉冲，当它由电机轴上的光电编码器产生时，电机的旋转方向可通过检测两个脉冲序列中的哪一列先到达来确定，转速可由脉冲数来确定。由于每个TMS320LF2407A带有两路QEP，所以可以很方便地测出四个轮子的实际转速而无需外加测试电路。

2.3 模糊自适应PID轮速控制

全向轮由四个电机驱动的轮子构成，任一时刻机器人的速度都由四个轮子的不同转速所合成。为了使四个电机同时获得较好的动态和静态特性，控制算法采用了模糊自适应PID控制[7.8]。自适应模糊PID控制以误差 和误差变化 作为输入，利用模糊控制规则在线对PID参数进行修改，以满足不同时刻 和 对PID参数自整定的要求。离散PID算法如下所示：

式中T为采样周期，K为采用序号。e(K)为第K个采样周期的偏差值，Kp为比例系数，Ki为积分作用系数，Kd为微分作用系数。电机控制框图如图5所示：

PID参数的整定必须考虑不同时刻KP、Ki、Kd三个参数的作用及相互之间的关系。经过反复实验，建立的模糊规则表如表2所示。

将系统误差e和误差变化ec定义为模糊集上的论域，如果e和ec超出了论域，则以论域中的最大或最小值代替，即：

其模糊子集为：

则第K个采样时间的整定为：

在线运行时，控制系统通过对模糊逻辑规则的结果处理、查表和运算，完成Kp、Ki、Kd参数的在线校正。

图5 电机控制框图

表2 KP、Ki 、Kd模糊规则表

2.4 PWM程序

PWM信号是一串宽度变化的脉冲序列。这些脉冲平均分布在一段定长的周期中，从而每个周期中有一个脉冲。该定长周期为PWM（载波）周期，其倒数为PWM（载波）频率。PWM脉冲的宽度由另一个具有所需值的调制信号决定。PWM频率过小，高低电平转换就过慢，不利于电动机的控制。PWM频率过大，计算的精度不是很好。这里设定其频率为200KHZ。要产生一个PWM信号，需要有一个合适的定时器来重复产生一个与PWM周期相同的计数周期和一个用来保持调制值的比较寄存器。比较寄存器的值不断与定时器的值相比较，当两个值匹配时，在相应的输出引脚就会产生一个电平转换(从低到高或从高到低)。当两个值之间的第二个匹配产生或一个定时器周期结束时，相应的输出引脚会产生又一转换 (从高到低或从低到高)。在每个定时器周期中，这个过程都会出现，但每次比较寄存器中的调制值可以是不同的。这样在相应的输出就会产生一串占空比不同的脉冲信号。

2.5 主从DSPs间的通信

主从DSPs间通过串行外设接口(SPI)模块进行数据的交互。SPI是一个高速、同步串行I/O口，它允许长度可编程的串行位流（1～16位）以可编程的位传输速度移入或移出。通常SPI用于DSPs和外部外设以及其它处理器之间的通信。主DSP与从DSP通信流程图如图6所示。

图6 主从DSP间通信流程图

3 系统异常预防与处理

系统异常主要有两方面：电路系统异常和执行机构异常。为了机器人运动控制系统能够稳定可靠工作，必须要有异常预防与处理的机制。本设计中使用的异常预防与处理方法主要如下：

隔离：DSPs的输入输出端口经过光耦隔离与电机驱动电路连接；

电源处理：机器人采用一组24伏电池供电，每路的电机驱动电路都有独立的保护电路和滤波电路，将各个电路引起的影响减到最小，DSPs主控板经过精密、可靠的滤波电路和稳压电路供电；

DSPs主控板有外围硬件微处理器监控电路，一旦出现异常，200毫秒内使DSPs重新复位，保证控制系统在短时间内恢复正常运行；

电机长时间堵转会损坏电机。因此，通过底层控制程序对堵转电机进行监测，一旦发生电机堵转，程序就进入异常处理模式。电机堵转检测方法有两种：1）电机输出电压高（高于18伏），但电机的转速低并且持续一段时间（大于250毫秒），则该电机处于堵转状态；2）通过计算电机电流来检测电机的工作状态是否正常。本设计采用第一种方法：电机处于堵转时，程序进入异常处理模式，使电机工作在锁住的状态，整个机器人表现为原地不动。程序进入异常处理模式后开始计时，经过2秒后底层控制程序切换到正常状态，检测电机工作是否正常，如此反复，直到电机停止堵转。

4 实验结果及分析

机器人以0.85m/s的速度直线运动时，测得轮子的速度响应曲线如图7所示。

图7 全向轮速度响应曲线图

图中，曲线1为常规PID控制策略下的速度响应曲线，KP、Ki、Kd及采样周期T分别为1、0.04、10、0.1s；曲线2、3为采用模糊PID控制策略下的速度响应曲线，PID参数均与曲线1相同，采样周期T分别为0.1s和0.05s。由曲线3可知，采用模糊PID算法且采样周期较小时，轮子的响应时间小于300ms、超调量σ=1.5%,响应时间短，移动机器人运动快速灵活。

5 结论

作为一种新型的运动机构，全向轮可以使移动机器人更加灵活。全向轮通过给定不同的速度，使得机器人能够自由、自主地360度全方位运动，使得机器人涉及的领域更加广泛。为了充分发挥全向轮结构的优势，必须选用转速快、输出扭矩大的电机组合，因此电机驱动电路的负担较重，同时主控板的硬件资源和软件处理能力也必须足以满足移动机器人实时控制的需要。因此，本文设计的运动控制系统采用了双DSPs架构，克服了DSPs+CPLD、DSPs+集成芯片造成的电路复杂，可靠性低、实时性不强等缺点。该系统具有电路简洁、集成度高、可靠性好等优点，实验证明移动机器人运动快速灵活、具有很好的可控性和稳定性。

[1] 谢云,杨宜民.全自主机器人足球系统的研究综述[J].机器人,2004,26(5):474-480.

[2] 李磊,叶涛,等.移动机器人技术研究现状与未来[J].机器人,2002,24(5):475-480.

[3] 刘和平,严利平,张学锋,桌清锋.TMS320LF240x DSP结构、原理及应用[M],北京:北京航空航天大学出社,2002.

[4] 章云,谢莉萍,熊红艳.DSP控制器及其应用[M].北京:机械工业出版社,2001.

[5] 李哲英.实用电子电路设计[M],电子工业出版社,1992.

[6] 张培仁,张志坚,等.基于16/32位DSP机器人控制系统设计与实现[M].北京:清华大学出版社,2006.

[7] 舒志兵,等.交流伺服运动控制系统[M].北京:清华大学出版社,2006.

[8] 陶永华.新型PID控制及其应用[M].北京:机械工业出版社, 2002.

[9] 刘金琨.智能控制[M].北京:电子工业出版社,2007.