基于三点定位的机器人路径跟踪算法

苏有刚

(莱芜职业技术学院研发办公室，山东莱芜 271100)

1 前言

在机器人路径跟踪方面的研究很多，可视图法［1］、神经网络［2］、鼠群算法［3］等已取得很大进展，但大部分都局限于一般的研究，有很多不足，特别缺少对特定环境的机器人路径跟踪［4］的研究，象港口、机场、车站货物装卸、高炉上料、农业田间工作、卫生清洁、军事巡逻等.本文针对这些特定场所的特点，提出了三点定位的机器人路径跟踪的实现方法.提高机器人对环境的适应速度和精度，为区域相对［5］固定场所的机器人应用奠定基础.

2 机器人的三点定位原理

机器人三点定位原理如图1，即用2个激光测距传感器发射器A、B，放在机器人工作目标区的现场适当位置，图1中放在了左边A、B处，这样有利于坐标定位和计算.在工作过程中激光测距传感器的发射器位置A、B固定，距离已知，确定如图1一方位固定已知的参考坐标轴，为工作路径提供参照，机器人在整个移动过程中，以激光测距传感器发射器A、B为定位参考.

在机器人车前和车后分别装有激光测距超声波接收器C、D，实时接收激光测距传感器发射器A、B的信号，不断测距，并计算出机器人的相应距离和角度，实现实时定位，实现移动机器人自身运动方向，包括车前和车后的方向和速度的精确定位.再通过和预置工作目标区地图的比较拟合，最终实现移动机器人的完全自主定位.工作目标区地图即机器人将要处理的工作目标大小形状方向的详细轮廓图，是机器人具体的工作环境图，在工作前由操作者输入机器，在定位过程中作为内部参照，而传感器作为外部参照，在工作过程中实现内外定位结合.在此过程中，融合了传感器和工作目标区地图的信息，并将它用于环境信息预测跟踪，最后由匹配的环境信息反过来对机器人的位置进行修正，得到其位置的较好的定位.这样可解决其它定位方法带来的累积误差，而依靠每一个位置所观察到的不同的信息和已有地图匹配，可以不断地修正机器人自身位置的误差量，从而可实现长时间的自主移动.

图1 机器人定位原理

3 三点定位算法的实现

三点定位的推导原理如图2所示，定位问题用数学语言可简单描述如下:设传感器发射点A，B坐标为(XA，YA)，(XB，YB)，并使AB平行于Y轴，垂直于X轴，设置在目标工作区左边，作为定位参考，且固定设置AB距离为l0，机器人经过的每个路径S的坐标为(X，Y，θ)，θ是机器人当前所处位置对坐标轴的方向的方位角，与定位的机器人当前的位置为Si(Xi，Yi，θi)(i=1，2，…，n).任取路径S上一点C(Xci，Yci，θci)，C和图1提到的D(Xdi，Ydi，θdi)设置在机器人车身中心的对称位置，前后放置.由此根据三角原理对于点C(Xci，Yci，θci)，AC，BC距离可由测距仪测定分别设为laci和lbci，由三角余弦定理可得

则可得点C(Xci，Yci，θci)坐标为

图2 机器人三点定位示意图

机器人定位首先利用三点定位法计算完成机器人车身点定位，再利用双三点定位法计算完成位姿的定位，然后算出机器人位置.位姿的定位精确定位移动机器人自身运动方向，包括车前和车后的方向，速度，相对于地图的精确位置，这样可以使机器人自动矫正位姿.

所谓双三点定位法，即由三点定位分别测定机器人车身前后不同位置两个点，再通过两个点与参考坐标的两个点比较位置，确定偏差.参考坐标的两个点选在工作环境中，其距离及在地图中位置确定，分别利用三点定位法分别测定机器人车身前后不同位置两个点，两点确定一线，两线比较就可得到机器人的方位，确定位姿，机器人位置本文是取车身两点中点.这种定位由机器人自身自动完成，是自主的.利用绝对定位和相对定位相结合的方法，实现移动机器人定位，这种方法对车辆机器人定位非常适合.这样机器人得到的原始数据是对应距离，定位结果是对应距离即(X，Y，θ)的形式.

但利用三点定位只能定位路径S的坐标为(X，Y，θ)，不能确定机器人车身的位姿，因此采用四点定位或者说双三点定位，即同时测算点C(Xci，Yci，θci)，和点D(Xdi，Ydi，θdi)坐标，由此确定机器人位姿，并完成定位.图3点C(Xci，Yci，θci)坐标以求出，同理设AD，BD距离可由测距仪测定分别设为ladi和lbdi可得点D(Xdi，Ydi，θdi)坐标为

则可得点D(Xdi，Ydi，θdi)坐标为

由此可得CD的斜率

此即为取i点时刻机器人的位姿角，由于机器人可认为工作在二维平面上，由此即为机器人的方向角.实际应用中可取CD中点为此时刻的机器人位置即

然后再有Xi和Yi求出θi，这样就确定机器人位置S(Xi，Yi，θi)(i=1，2，…，n)，位姿角为βi.

图3 机器人双三点定位示意图

4 机器人路径跟踪的实现

由于机器人实时定位是不连续的，机器人并不能实时测定自身的位置，在一个周期中只能定位一系列连续的点S(Xi，Yi，θi)(i=1，2，…，n)，路径跟踪就是利用最近的两个测量点来实现.如图4上点Sn(Xn，Yn)和Sn+1(Xn+1，Yn+1).最佳轨迹如图4虚线X0.点S'表示调整后的位置.关系如下:

因为Xn+1＜Xn，所以θn=βn－90°.

因为此时机器人实际轨迹近似在点Sn和Sn+1的线上，已经偏离了最佳轨迹，路径跟踪的实现为调整机器人车位角度，使机器人位于点Sn+1和S'的线上，使相位角θn+1变为θn+1=θn/2(可根据实际数据调整)进行调控.这种路径跟踪可以使机器人无限接近最佳轨迹，实现机器人路径的最优跟踪，并且计算量较小，反应速度快，并且提高精度可通过提高采样频率实现.当然当移动速度快时会降低控制精度.同理，当机器人偏离最佳轨X0时，需进行调控，反之趋近时不用调整.具体位置需具体分析.

图4 机器人路径跟踪实现

5 路径跟踪的实验

实验采用自制的4轮机器人，由转速可精确调节的伺服电机驱动，测距传感器选用相位激光传感器，测量精度不小于2mm，用PC机完成数据处理和控制，采样周0.02ms，AB距离设为10m.

(1)选取任意固定适当坐标，以X－轴为机器人00方向.机器人随机放置目标区，AB距离设为10m，平行于Y轴进行实验.经测量结果如表1所示.结果说明精度较高.

表1 任意放置测量结果

(2)模拟直线路径20m，地面平坦，设置2处障碍物，设定速度为1m/s，模拟路径结果如图5所示，虚线代表设定路径，实线为实际路径，图中说明实际误差没超过10cm，调整精度较高，反应也较快.

从以上试验结果可以看到，路径跟踪精度很高，测量距离对测量精度影响较大，距离越远误差变小，这与激光测距绝对误差小有关.由于周期性定位，这种路径跟踪方法不产生累积误差.本试验精度也和控制系统的性能相关.

图5 试验模拟路径图

6 结论

本文给出的利用双三点定位旋转车位角度的机器人路径跟踪的方法，计算方法虽然简单，但该路径跟踪方法工作量小、精度高、反应快、使用便利，适于机器人工作区域相对确定范围有限的场合，仿真试验证明，该计算方法具有很高的实用性.

［1］陈少斌，蒋静坪.基于神经网络和粒子群优化算法的移动机器人动态避障路径规划［J］.系统仿真技术，2006，2(4):192－197.

［2］刘徐迅，曹阳，陈晓伟.基于移动机器人路径规划的鼠群算法［J］.控制与决策，2008，23(9):1060－1064.

［3］P.Núńez，R.Vázquez－Martín，J.C.del Toro，A.Bandera，F.Sandoval.Natural landmark extraction for mobile robot navigation based on an adaptive curvature estimation［J］.Robotics and Autonomous Systems，2008，56:247－264.

［4］Durrant－Whyte，H.F.RYE D，Nebot E.Localization of automous guided vehicles［A］.Proceedings of international Symp on Robotics Reseach［C］.New York:Springer－verlag，1995.

［5］Gordon Wells，Carme Torras.Assessing image features for vision－Based robot positioning［J］.Journal of Intelligent and Robotic Systems，2001，30:95－118.