全方位移动平台运动控制系统的实现

曾令栋，裴 东，2，王全州，高文辉

(1.西北师范大学 物理与电子工程学院，甘肃 兰州 730070； 2.甘肃省智能信息技术与应用工程研究中心,甘肃 兰州 730070)

0 引言

全方位移动平台由于其出色的灵活性，在服务机器人的设计中成为最佳的选择。而全方位移动平台运动控制的性能又是机器人稳定性的主要影响因素，因此机器人底层运动控制也成为机器人研究中的热点之一[1-10]。本文研究了一种基于FPGA的全方位移动平台运动控制的方法。根据实际机器人底层调试的需求增加了无线控制、速度显示等模块，并对移动平台进行避障的开发，减轻上位机的运算负载。为了方便后续工作，该移动平台建立了独立运行于个人PC的上位机，从而形成完整的运动控制系统。系统控制的优先级自下而上逐层递减，保证了系统能够稳定运行。

1 系统总体设计

全方位运动平台的总体结构如图1所示。首先是上位机通过无线蓝牙模块发送对核心控制模块的控制指令，再由匹配的另一个蓝牙模块进行接受，传送至核心控制模块处理，在核心控制模块将指令进行解析分配，然后以PWM形式送至驱动模块，驱动电机运转。在控制平台进行移动操作时，超声波测距模块将实时测距数据传送至核心控制模块，在核心控制模块对超声波测距信息进行分析，达到避障的目的。液晶显示模块进行间断刷新，用以显示测速模块实时采集的轮子速度和核心控制模块下发的理论速度。

图1 系统总体硬件模块

FPGA作为整个系统的核心控制模块，主要实现对上位机信号接收、超声波测距信号处理、测速信号的采集以及对驱动模块下发驱动指令、LCD液晶显示模块间断刷新指令。FPGA从上位机接受到指令后经过指令解析，将指令进行分配，即分析指令内容再由FPGA内部分配至各硬件电路[11]。而后将数据传输至各个模块。

在FPGA分配解析得到机器人坐标系下的速度后，将机器人坐标系下的速度值转化成机器人的3个全向轮的角速度，将得到的角速度值计算出相应的占空比，生成相应占空比的PWM波形，输出信号接到直流伺服电机驱动器[2]，然后通过测速模块，计算出轮子实际的速度值，在LCD液晶显示模块实时显示速度。

2 运动控制算法及实现

2.1 全方位移动平台的运动模型

运动平台在平面上的位置如图2所示，世界坐标系为xoy，平台坐标系为XOY，点O为在运动平台上的参考点，平台的位置和姿态用ξ=[x,y,θ]T表示，其中(x,y)为点O在平面直角坐标系中的位置，θ为世界坐标系下x轴到机器人坐标系X轴的角度，顺时针为反方向[3]。

图2 平台坐标系

假设在平台坐标中平台的移动速度为[VxVyφ]。其中Vx、Vy分别为平台移动速度V在平台坐标系下的X轴与Y轴的分量，φ为运动平台自转角速度。则存在：

(1)

平台移动可分解为轴向可控的主动轮移动和径向不可控的从动轮移动，运动平台通过调节主动轮速度来调节车体的速度。如图3所示，以XOY为平台坐标系，建立在车体上布置三个全向轮时主动轮和从动滚子的运动学模型[2]。

图3 运动学模型图

(2)

根据三轮速度之间的关系可得全方位移动平台的主动轮在平面世界坐标系中的速度要求：

(3)

根据这一速度要求进而可以对电机发出相应的控制信号。

在实际情况中，全方位移动平台有直线运动、原地自转、圆弧运动这几种情况，其中圆弧运动又包含平动圆弧和转动圆弧。对于不同的情况进行以下分析。

(1)全向方位移动平台沿任意方作直线移动时，φ=0。

(4)

当Vx=0、Vy≠0时，平台作前后直线移动；

当Vx≠0、Vy=0时，平台作左右直线移动；

当Vx≠0、Vy≠0时，平台作斜向直线移动。

(2)移动平台绕自身中心自转时，Vx=Vy=0，φ≠0，这时：

V1l=V2l=V3l=Lφ

(5)

(3)圆弧运动：

①平动圆弧：φ=0，运动平台凭借Vx、Vy改变其运动方向，从而实现圆弧运动。

(6)

图4 运动平台的圆弧运动轨迹

(7)

2.2 全方位移动平台运动算法实现

在得到全方位移动平台的算法后，系统要进行运动控制的矢量分解，其作用是将平台坐标系下的速度转化为三个全向轮的速度。根据系统平台实际的机械结构，运动平台矢量分解采用下式实现：

(8)

由于单个驱动器的最大速度归一化扩大百倍后为100整，所以平台坐标系中Vx和Vy的速度范围通过MATLAB分析得到如图5所示的平台坐标系速度范围图。

图5 平台坐标系Vx和Vy的速度范围图

从图5中可以看出X和Y的方向会限制在第四幅图中的不规则八边形内。八边形的边为X和Y综合速度极限。如果在限制范围外，运动平台会出现速度饱和的情况。

根据运动算法和MATLAB仿真分析的结果，在FPGA中产生相应的PWM波进行全方位移动平台的运动控制。其仿真结果如图6所示。

图6 速度控制仿真图

3 各硬件模块的实现

3.1 蓝牙通信模块

为了方便对全方位移动平台在行进中发送控制指令，平台采用蓝牙通信。由于对传输速率的要求较低，试验阶段传输距离在10 m以内即可，平台涉及的蓝牙模块型号为HC05。该模块有命令响应、工作连接两种工作模式。在对蓝牙模块设置时，将蓝牙模块置为命令响应模式，将两个蓝牙模块的串口波特率设置为9 600 b/s，然后将工作模式设置一个为主一个为从[5]。蓝牙开启后会自动配对，配对成功之后便可以进行数据的接收和发送。

3.2 超声波避障模块

超声波模块的原理是通过发送多组等时长的超声波，然后检测声波回响的时间长短，根据声波回响时间的长短测定声波源和障碍物之间的距离。超声波模块有两个端口，分别是触发端和回响端。FPGA通过向触发端发送一个时长为10 ms的高电平开启超声波模块的发送器，然后接收器会在回响端产生一个根据实际距离变化的高电平。FPGA通过测量回响端接收的高电平时长来计算超声波模块与被测物之间的距离[6-7]。

超声波模块在FPGA中控制的RTL级电路如图7所示。U1产生10 ms高电平的触发信号Trig_Sig，U2将接收到的回响信号Echo_Sig经过计时得出实际距离。通过并行线Dis_Num传送至其他模块。根据实际需求，只需测量距离在1 m以内，所以Dis_Num的位宽为7。

图7 超声波测距模块控制电路(RTL级)

3.3 LCD显示模块

液晶显示在全方位移动运动平台中的功能是为了实时显示超声波测距数值以及三个轮子的指定转速。该显示模块采用无字库的驱动芯片，所以作为显示只需要其绘图功能即可。

液晶屏的输入模式采用串行传输。通过SPI总线协议将数据发送至液晶屏显示。SPI是一种高速全双工同步的通信总线，共4线，分别是CS，A0，SCL和SDI。由于SPI总线有主机和从机之分，因此在平台中，液晶显示为从机而FPGA为主机。FPGA将需要显示的数据通过SPI总线传送至液晶显示，SI、SCL、CS都是主机输出、从机输入。关于SPI总线的液晶显示时序图如图8所示。

图8 SPI总线液晶时序图

3.4 驱动模块与测速模块

平台采用的驱动器是型号为MLDS3610的直流伺服驱动器。以PWM信号速度控制的方式进行驱动。在FPGA中设计PWM波形输出模块，模块采用500 Hz的频率。PWM的占空比范围为1%～99%。而在此范围内，线性度能够保证在0.1%以内。在驱动器中设定占空比=50%时，电机速度V=0；占空比<50%时，电机反转；占空比>50%时，电机正转。从而达到调节速度和方向的目的。

FPGA除了要对驱动器指定需要的速度外，还需要一个实时的速度反馈来达到闭环控制的效果。常见的测速算法有四种：M法测速，T法测速，M/T法测速以及变M/T法测速。通过对运动平台电机的实际测试，其速度变化为1～9 600 r/m；速度为1时实测编码器输出方波频率为8.5 Hz左右。速度为9 600 r/m时实测编码器输出测方波频率为82 000 Hz左右。本文中将速度归一化处理后扩大一百倍，速度就为0～100 r/m之间，而此时速度为1 r/m时，编码器输出大约816 Hz左右。所以对编码器测速范围应该在800～82 500 Hz之间。测速结果将结果统一在0～100 r/m之间，结果每增加1 r/m实际速度增加96 r/m。所以M法测速能够满足平台的测速需求[8]。

4 系统评述

与两轮差速驱动的底盘结构相比，全向轮结构的移动底盘和两轮差速驱动的底盘有很大的不同。全向轮结构具有很大的灵活性，在狭小的空间里能够实现零转弯半径的操作，防止后期的上位单目摄像头结构在底盘转弯时丢失跟踪的目标。

与文献[2]的系统相比，文献[2]中FPGA核心板仅进行了运动控制的处理，板子资源稍有浪费。本文中的FPGA不仅实现了运动控制算法，还增加了速度的实时显示、超声波避障、蓝牙通信等功能。在下位机部分就能够实现避障的功能，有利于降低上位机计算量的开销。同时，下位机使用蓝牙通信使得下位机自成一系统，即使在行进中也可以对底盘的一些运动参数进行修改。

5 结论

针对服务机器人底层运动控制系统，设计了一种基于FPGA的全方位移动平台。在实际测试中发现这种采用FPGA的方案具有很好的实时性，速度控制精确度高，而且由于FPGA优越的处理能力和多引脚的特性，平台的可扩展性较强，如后期配置Kinect传感器进行图像处理，则可实现机器人跟随的目的。同时该平台为多电机的机器人运动控制系统提供了具体的实现方案，具有一定的参考价值。

[1] 赵伟雄. 全方位移动机器人控制系统研究与实现[D].天津：河北工程大学,2016.

[2] 王全州,裴东,陶中幸,等. 中型组机器人运动控制系统的FPGA设计[J]. 现代电子技术,2010, 33(14):127-130.

[3] 陈汝佳. 轮式人形家庭服务机器人设计与实现[D].成都：电子科技大学,2014.

[4] 梁延德,于华玉,何福本. 全方位小型足球机器人运动特性分析[J]. 组合机床与自动化加工技术,2008(4):5-8.

[5] 郭栋.蓝牙HCI-UART主控制接口的FPGA设计与实现[J]. 微计算机信息,2010, 26(11):140-141，144.

[6] 吴超. 基于FPGA的超声波测距系统[D].武汉：武汉理工大学,2009.

[7] 刘明,肖文健,齐晓慧.超声避障技术在轮式机器人导航中的应用研究[J].科技信息,2012(22):129-130.

[8] CHARITKHUAN C, BHURIPANYO J, CHOOMUANG R. FPGA implementation of closed-loop control system for small-sized RoboCup[C]//2006 IEEE Conference on Robotics, Automation and Mechatronics, IEEE, 2006:1-6.

[9] 胡振旺,陈益民,李林. 基于STM32的家庭服务机器人系统设计[J]. 电子产品世界,2015, 22(9):55-57.

[10] 王少君,刘永强,杨绍普,等. 基于光电编码器的测速方法研究及实验验证[J]. 自动化与仪表,2015, 30(6):68-72.

[11] 王先胜.基于FPGA的UART电路的设计与应用[D].西安:西安电子科技大学,2014.