肖立武，曾连荪，朱锦顺

（上海海事大学 信息工程学院，上海 201306）

在移动机器人的导航领域，轨迹研究探索不少但领域内与轨迹推演相关的文献较少。本文探讨不依靠其它惯导器件（如电磁罗盘）辅助，仅使用机器人驱动电机配套的编码器解析机器人的相对位移和航向角数据，将解算得到数据发给上位机，上位机对该数据进行解析并依据这些解析得到的数据绘制实时轨迹，最终实现轮式机器人的轨迹推演。该方案增强了系统的紧凑度和可靠性，同时也节省了系统的构建成本。本文所提算法框架在本研究组所开发的机器人平台Carmela[3]上验证。

1 基于Arduino的运动状态分析

在分析Arduino的运动状态[4]的时候，Arduino只有两种运动状态（如图 1）。一种是类似如处在世界坐标系下的a点（此时的a点是静止时，也即Arduino的最开始状态）坐标（通过最初的测量可知），另一种则是Arduino运动一会到达b点的状态，此时我们只知道b点位于以a点为原点的相对坐标系下的坐标（通过传感器测量值解算可知）。当我们解算出b点处于世界坐标系下的坐标时，b点此时的状态就转换成了a点时的状态，也即第一种，然而当Arduino从b点运动到另外一个点，不妨取做c点，此时c点又是相对于b的相对坐标，b和c又转换成了a和b时的状态形式，Arduino的运动状态不断循环往复。

图1 Arduino的运动状态Fig.1 Arduino motion state

2 数据解析原理

根据Arduino的运动模型的分析，不难分析出求出世界坐标系下b点的坐标和航向角是数据解析的关键。将b点在相对坐标系中进行相应的分解如图 2所示，计算出b点在世界坐标系下x轴的坐标Xwb如式(1)所示；将b点在相对坐标系中进行相应的分解如图 2，计算出b点在世界坐标系下y轴的坐标Ywb如式(2)所示；将b点在相对坐标系中进行相应的分解如图 2，计算出世界坐标系下航向角θwb如式(3)所示。

图2 计算航向参数的分解图Fig.2 The vehicle diagram of calculating Xwb

最终推到出数据解析的数学原理合成公式[2]如式(4)所示。

3 下位机数据的采集

Ardunio[1]作为下位机平台它的重要职责在于：1)执行上位机传达的命令以及反馈命令的执行情况；2)采集数据给上位机；当ardunio采集数据给传递个上位机时，ardunio解算到b点相对a点的坐标，是上位机轨迹推演时进行数据解析的前提所在。

3.1 短时运动模型

微控制器中断编程系数、编码器的分辨率n、外部中断辨识出的编码器的脉冲数与轮子实际的转数之间的关系如

机器人的运动非匀速，特别是加入PID控制时，线速度和角速度都会频繁变化，但在极短的时间内（例如本文所设定的时间间隔为0.2 s）或极小的位移内，机器人的短时运动可假设为恒速，机器人运动所驶过的圆弧可近似为直线。轮式机器人的转弯运动是其它多种运动状态的典型表现，例如直行是转弯运动的特例，由转弯运动推导出的相对运动模型具有普适性，可描述所有运动状态。记机器人左右轮距为B，同侧的前后轮距为L，轮子的周长为C。本文将机器人的质心等效为左前轮和右前轮的轴中心点，即三轮机器人模型[5]，在采样周期足够短，机器人运行的位移足够小的前提下完成左转动作，其详细的几何关系示于图3。设在此采样周期内机器人左转Φ，左、右轮距转弯圆心的半径分别为Rleft和Rright；左、右轮的位移分别为Sleft和Sright；可求得机器人左前轮和右前轮处编码器的计数分别为Nfrontleft和Nfrontright，它们的符号表征了正转和反转，相应地，左前轮和右前轮的实际转数为Mfrontleft和 Mfrontright。 则：

将式(5)代入，可得：

图3 几何关系图Fig.3 Geometric diagram

如图3所示，并由圆心角的定义可知：

则根据三角形的相似性可得：

化简可得：

将式(12)代入式(10)得:

将式(8)和式(9)代入式(13)得:

参见图 3，又由三角形相似可得：

将式(12)代入式(15)，并化简得：

将式(8)和式(9)代入式(16)可得：

则式(14)和式(17)构成了从编码器脉冲计数解析两个采样时刻间相对运动的短时运动模型，当将此左转模型推广到一般运动时，转角Φ的符号表征了机器人的运动状态：当Φ大于零时为左转；当Φ小于零时为右转；当Φ等于零时直行；当S大于零时为前进；当S小于零时为后退；当S等于零时机器人停止。

3.2 b相对a点的转角和位移的合成

机器人在一个较长的时间内的动作可分解成若干短时运动周期，机器人作业时的任意动作都可基于短时运动模型合成，一个长时运动的转角可表示为：

类似地，位移可相应地表示成：

4 上位机轨迹推演原理

上位机的轨迹推演原理由两部分构成：数据解析原理和视觉暂留现象。根据上述数据解析原理设计出了核心的C++数据解析代码compound函数。视觉暂留现象（Visual staying phenomenon，duration of vision）又称“余晖效应”，1824年由英国伦敦大学教授皮特‘马克’罗葛特在他的研究报告《移动物体的视觉暂留现象》中最先提出。人眼在观察景物时，光信号传入大脑神经，需经过一段短暂的时间，光的作用结束后，视觉形象并不立即消失，这种残留的视觉称“后像”，视觉的这一现象则被称为“视觉暂留”。轨迹的绘制部分就必须借助视觉暂留现象：由于电脑的主频很高，人会产生眼视觉暂留，根本无法分辨一次次重新绘制的静态图，而在人眼中呈现出来的就是一幅动态的图像。使用Qt的开源版本[6]中自带的QPainterPath类，使用解析出来的数据进行路径绘制，那么下一次的绘制则是在上一次的路径的基础之上lineTo到刚刚获得的点上。从而我们就可以实现推演从轮式机器人的运动轨迹。

5 结论

使用课题组搭建的低成本四驱轮式机器人平台Carmela[3]验证本文提及的算法。该平台的3个关键参数m,n和ω的取值分别为43.7，16和2。实验前，通过角度测量在地面上标记了不同角度（45度、90度和180度）所对应的线。实验中，手工遥控机器人转动并逼近不同角度所对应的基准线，通过使用基于第3节介绍的算法，机器人驱动电机配套的编码器解算机器人的相对位移和航向角数据，并通过RS232发送到上位机，上位机依据第2节介绍的算法实时解析这些上传的数据，并将解析得到的数据绘制实时轨迹，实地实验验证了轮式机器人相对位移解算以及对上报数据进行解析算法的性能。本算法为轮式机器人的轨迹估算推演提供了新思路，实验结果证明了该算法的正确性与实用性，可以很好的解决轮式机器人导航中轨迹推演的问题。

[1]Arduino.An open-source electronics prototyping platform[EB/OL].[2012-12-18]http://www.arduino.cc/.

[2]Smith R,Self M,Cheeseman P.Estimating uncertain spatial relationships in robotics[J].Autonomous Robot Vehicles,1990:167-193.

[3]孙作雷,张波,曾连荪.基于Arduino和Qt的开源实验机器人平台设计[J].上海海事大学学报,2013,34(2)：80-83.SUN Zuo-lei,ZHANG Bo,ZENG Lian-sun.Low-cost opensource experimental robot platform design based on Arduino and QT[J].Journal of Shanghai Maritme Univ,2013,34(2)：80-83.

[4]王仲民,岳宏,李充宁,等.轮式移动机器人轨迹跟踪的最优控制[J].机械科学与技术,2006,25(1):21-23.WANG Zhong-min,YUE Hong,LI Chong-ning,et al.Optional Control of Wheeled Mobile Robot Trajectory Tracking[J].Mechanical Science and Technology,2006,25(1):21-23.

[5]刘磊,向平,王永骥,等.非完整约束下的轮式移动机器人轨迹跟踪[J].清华大学学报:自然科学版,2007,42(S2):1884-1889.LIU Lei,XIANG Ping,WANG Yong-ji,et al.Trajectory tracking of a nonholonomic wheeled mobile robot[J].J Tsinghua Uni:Sci&Tech,2007,42(S2):1884-1889.

[6]Digia Plc.Qt open-source project[EB/OL].[2013-01-18]http://qt-project.org/.