移动机器人的在线实时定位研究(翻译).docx

移动机器人的在线实时定位研究徐德 谭民（北京中科院自动化研究所复杂系统与智能科学实验室）（电子邮箱：）摘要对推算定位法进行了研究,提出了一种改进方案.通过对移动机器人运动轨迹与状态的分析,导出了一类移动机器人的基于轨迹的运动学新模型.利用移动机器人三个轮子的里程信息和导向轮的转角信息,通过信息模糊融合获得转弯半径和转角,再利用运动学模型获得机器人的位置和方向.将这种改进的推算定位法与主动灯塔法相结合,提出了一种用于室内移动机器人的定位方法.仿真结果表明,该方法具有实时性好、精度高、成本低、鲁棒性好等特点,并适用于不平整地面。关键词： 移动机器人，定位，运动学，模糊融合1 绪论定位是机器人的基础，因此，精准的实时定位是提高机器人性能的关键因素，也是一个机器人研究的热点。定位的方法可以分为两种：相对位置和绝对位置。相对位置定位法又叫推算定位，可以细分为测距法和惯性导航。绝对位置定位法可以使用全球定位系统、基站定位、罗盘导航、地标导航和地图匹配等方式实现。它们各有特色，但是只有推算定位，基站定位，地标导航和地图匹配可以适用于室内移动机器人。因为测距法在实时系统的实际应用中有很高的性价比所以成了推算定位最常用的方法。通常的做法是将其他的绝对定位方式得到的结果通过信息融合的方法得出更好的定位效果。这种方式能在小范围和大范围内都有很高的定位精度。在1996年，Lazea用运动和反向运动的模型研究机器人。1997年，Chong 和Kleeman 提出了一种利用统计误差来判断定位误差的推算定位机器人模型。同在1996年，Borenstein 和Feng 提出一种系统误差验证法。这些方法都能很好地用来实时定位，但是在不平整的场地它们的性能都会大打折扣。同一年，Burgard提出另外一种解决方法，通过将摄像头得到的图像跟全局地图进行比较将可以得到绝对坐标和方向估计。1999年，Roy和Thrun给出一种利用外部地图的统计学方法用以校正机器人的位置。El-Hakim 提出一个建立室内3D地图用来定位。这样使定位的精度提高了一个级别，但是3D模式需要大量的资源来解析，这就使得在线机器人的移动速度受到很大的限制。另外一个缺陷就是在特征的匹配上面很容易出错。推算定位法定位的精确性对系统性能的影响很大。如果绝对定位方式无法实施的话，系统的定位将完全依靠推算定位。另一方面，信息融合要用到的迭代算法可以通过改变迭代的次数来改变推算定位的精度。因此，正如前面指出的，提高推算定位的精度可以提高机器人的性能使机器人的导航变得更加容易。本文提出一种利用测量的冗余数据结合基站可以提高室内机器人实时定位的精度的方法。2 一种新的运动模型和有冗余测量信息的推算定位本文涉及到的机器人是一个三轮轮式机器人，如图1所示。前轮是一个导向轮，左边的后轮是一个驱动轮，右边的后轮是一个平衡轮。为了减少启动和刹车驱动轮对距离测量的影响，我们在驱动轮的外侧安装了一个靠地面摩擦力带动的测量轮，三个编码器分别装在测量、平衡、导向轮上面用来测量距离，如图1所示。用来测量偏航角的编码器4也装在导向轮上面。编码器4用的是绝对编码，其它3个都是用相对的。图 1 三轮移动机器人运动系统方案图为了方便起见，我们分别用W1，W2，W3来表示测量轮，平衡轮，导向轮。O1表示机器人的几何中心。分别设W1，W2 之间的距离为l, W3到W1，W2 连线的垂直距离为h, O1到W1，W2 连线的垂直距离为d 。假设O1，W3跟W1 W2连线的中点都在W3到W1，W2 连线的垂线上。XOY为基准坐标系，X1O1Y1为机器人的坐标系。Y1的方向跟O1到W3的方向相同。Sjii+1 表示W1在一个取样中以Y1 为正方向移动的距离。我们用一个33矩阵TO1i 来表示机器人的位置和姿态：TO1i =mxinxipximyinyipyi001 （1）mxii+myij 在XOY坐标系中表示向量X1 ，nxii+nyij 在XOY坐标系中表示向量Y1 ，pxi, pyi在XOY坐标系中表示机器人的姿态。2.1 基于运动轨迹的模型机器人的运动轨迹可以用直线和圆弧来表示。当它直线运动时，其位置和方向可以从移动的距离得出，当它的运动轨迹是弧线时，其位置和方向可以从弧线的半径和中心角得出。而圆弧的半径和中心角度可以利用信息融合从4个编码器得到。具体怎么得到这些实时的参数我们将在2.2讨论。2.1.1 机器人做直线运动当机器人向Y1的方向做直线运动时。有S1ii+1=S2ii+1=S3ii+1，ai=0，ai为W3 的偏航角。O1 在采样点i+1-th 时的位置和姿态TO1i+1可以通过以下的平移变换得到：TO1i+1=mxinxipxi+S1ii+1nximyinyipyi+S1ii+1nyi001 (2)2.1.2 机器人做圆弧运动当机器人运动路径为弧线时，将机器人视作一个质点O2 即机器人的中心点。X2和Y2的方向可以定义为X1和Y1的方向如图2所示。我们将TO1i平移变化可以得到TO2i，通过角度变化可以由TO2i得到 TO2i+1。TO2i和 TO2i+1分别为O1 在采样点 i-th 和 i+1-th的位置和姿态。图 2 轨迹曲线图TO1i+1 =TO1i+Trans-rii+1-l2,-dRotii+1Transrii+1+l2,d =mxinxipximyinyipyi00110-rii+1-l201-d001cii+1-sii+10sii+1cii+1000110rii+1-l201d001 =mxicii+1+nxisii+1-mxisii+1+nxicii+1mxicii+1+nxisii+1-mxirii+1+l2-mxisii+1+nxicii+1-nxid+pximyicii+1+nyisii+1-myisii+1+nyicii+1myicii+1+nyisii+1-myirii+1+l2-myisii+1+nyicii+1-nyid+pyi001 （3）其中rii+1 是一个采样时间间隔内W1运动的轨迹与X2的交点；ii+1是一个采样时间间隔内弧线的中心角度。2.2 ii+1和rii+1的测量方法我们在鸟瞰图上用ai表示W3在采样时间i-th时刻的偏航角，并定义该时刻的正方向为逆时针方向。ii+1和rii+1可以从S1ii+1, S2ii+1, S3ii+1 和 ai 中的任意两个参数求得。因地面的不平和测量的误差从不同的一组参数得到的ii+1和rii+1的值可能会不一样。很容易从图2的几何关系中得到6组ii+1和rii+1的值。为了减少随机因子的影响得到更加精确的ii+1和rii+1，我们用6组数据进行估值。在这6组数据里面有6个不同的ii+1和4个不同的rii+1。因此我们分别对ii+1和rii+1进行融合。通常，随机误差是正态分布的。所以，我们通常用一种跟正态分布很类似的指数函数来表示正态分布，如下所示：rrj=krje-rj-r2r2j=kje-j-22 (4)有,m=4和n=6。r和是模糊集。r和0是分布参数由公式5确定。krj和kj用来表示rj和j的集中度，由公式（6）（7）求得。r=14maxr1r4-minr1r4=14max16-min16 （5） （6）krjl= 1 , rj-rlr0 , others , kjl= 1 , j-l0 , others （7）rii+1和ii+1的估计值在公式（8）中用Takagi-Sugeno模糊算法求得。 （8）然后，我们分别将rii+1,ii+1和r和进行比较。如果rii+1-r0.1mm和ii+1-0.00001rad，我们就取rii+1和ii+1，否则就令r=rii+1,=ii+1,用公式（48）重新计算rii+1,ii+1。最多重新计算5次即可。3 一种结合基站和推算定位的新的定位方法1999年，Ghidary提出了一种基于基站的室内定位方法。他的具体做法如下：机器人身上的超声波和红外发生器同时发出信号。当室内的红外接收器接收到机器人发出的信号时，触发计时器开始计时，接收到超声波的时候定时器停止计时。这样我们可以得到超声波的传送时间。从而可以计算出机器人跟基站之间的距离。用三个这样的基站通过这样的方法就可以确定机器人的具体位置。用这种方法需要在机器人身上装8个超声波发射器，8个红外发射器。在室内装有3个超声波接收器，3个红外接收器。为了保证没有盲区，在一个64的房间内安装这些接收器必须保证同时至少能有3个能够接收到机器人发出的信号。这种方法成本很高。另外，机器人的运动靠一个从机控制，位置的确认由主机完成。主从机交换信息造成这个闭环系统内严重延时。这种方法有一个缺陷就是机器人的红外信号不能被遮挡，然而在室内调试的时候人经常站在机器人旁边，导致定位的失败。为了克服这种定位方法的缺点，减少电脑负荷，降低运行成本。我们提出了一种新的方案，如图3（a）所示。在机器人身上安装1个无线电发射器和2个超声波接收器，再在天花板上安装一个无线电接收器和一个超声波发射器。无线电发射器发出一个脉冲信号触发天花板上的超声波发生器并开启机器人身上的定时器。当机器人身上的两个超声波接收器接收到信号时分别停止计时。然后通过转换可以得到超声发射器跟接收器之间的距离。由于推算定位在瞬时定位上有着很高的精确度，我们得到一种新的结合基站的方法。a 基站定位方案 b 位置校正图 3 结合AB DR的定位方案为了简化计算，我们将一个超声波接收器安装在机器人的几何中心O1，另一个安装在W1和W2连线的中点M处。P0点为超声波发射器在地面投影的位置。在采样时刻i-th O1到P0的距离为RO1i， P0到M的距离为RMi，利用装在机器人上的超声波传感器可以很准确地得出RO1i和RMi。我们可以通过d, RO1i ,P0, RMi来修正PO1i。由图3可以看出由P0、PO1i、PMi为顶点构成的三角形，边RO1i和RMi之间的夹角可以由以下公式求得:-cos-1RO1i2+RMi2-d22RO1iRMi （9）PMi表示修正后的PMi。Mj表示从PMi到P0的方向角。PO1i表示修正后的PO1i。O1j表示从PO1i到P0的方向角。标记j表示Mj计算的次数。我们有以下公式: O1j=Mj （10） pMxi=p0x+RMicosMjpMyi=p0y+RMicosMj （11） pO1xi=p0x+RO1icosO1jpO1yi=p0y+RO1icosO1j （12）p0x和p0y为P0点的坐标。纬是一个无符号数，范围为0,。，由公式（10）可以知道O1j有两个可能解，但是它们中只有一个为真解。明显地，得到Mj后PO1i、PMi可以得到修正。目标函数如（13）所示由PMi到PMi和PO1i到PO1i的距离来配置可以得到Mj。在图3（b）所示不变的情况下，我们可以通过目标函数求得Rmi跟RO1i 也就是说可以求得PMi和PO1i。E=12pMxi-pMxi2+pMyi-pMyi2+pO1xi-pO1xi2+pO1yi-pO1yi2 （13）对E和Mj求微分可得：dE dMj=-RMipMxi-pMxisinMj+RMipMyi-pMyicosMj-RO1ipO1xi-pO1xisinO1j+ RO1ipO1yi-pO1yicosO1j （14）向梯度下降的方向搜索Mj。向梯度上升的方向搜索Mjj+1，具体如下：Mjj+1=-kRMiRO1idE dMj （15）这里k是一个步进因子，k=0.5。Mj+1=Mj+Mjj+1 （16）另外，M0，就是Mj的初值，定义为从PMi到P0 的方位角。M0=tan-12pMyi-p0y,pMxi-p0x （17）在计算的过程中，我们很难确定O1j的两个可能值哪个为真。所以我们将两个值M1j，M2j都代入公式（13）得出两个函数值E1，E2。用迭代算法比较E1，E2的大小。如果E1E2 就把M1j作为Mj，否则就把M2j作为Mj。然后将Mj代入公（11）、（12）可以求出PMi和PO1i的坐标，就可以确切地知道PMi和PO1i位置的改变。从PMi到PO1i的方位角可以用PMi和PO1i的坐标计算出来。因此机器人的位置也就明确了 。=tan-12pO1yi-pMyi,pO1xi-pMxi （18）TO1i=cos-sinpO1xisincospO1yi001 （19）4 仿真结果机器人的三个轮子的参数如下：l=600mm, d=400mm, h=800mm. O1点的参考运行轨迹为一个圆心在点（4000mm,4000mm）处半径为2000mm的圆。机器人的步进为每次千分之一圆的周长。W1W3的坐标和W3的偏航角可以由O1的参考轨迹和机器人的步进次数决定。它们再加上一个有界随机数就可以得到测量值了：Dm=Da1+0.1Dd （20）Dm是测量值，Da是求得的精确值，Dd是有界随机值。另外，当机器人的一个轮子从凹坑中驶过时我们利用公式（21）得到另外一个不同的测量值，用Dm+Dp表示。Dp=5sini50, i=0,1,50 （21）Dp是坑洞数据。在仿真中，当偏航角少于0.01弧度、S1ii+1和S2ii+1的平均值大于5mm时，或者当转弯半径大于100,000mm、S1ii+1和S2ii+1的平均值小于30mm时，我们认为机器人做的运动轨迹为直线。4.1 推算定位的改善仿真图4给出了一般的推算定位的仿真结果和改善后的结果。图4（a）（b）是在平坦的场地的仿真结果。（a）是用到第2节中提到的改进型的推算定位法的结果。距离参考结束点的距离是43mm，而通常的仿真结果是150mm。机器人在参考轨迹下结束点参考点、改进DR和一般DR的位置和姿态如下：105959.6013600.0001，105990.3013569.8001，106077.7013507.2001图4（c）和（d）是在不平整的场地W1的仿真结果。图上省略了W2和W3的仿真结果，因为它们的结果在（c）（d）上没有什么区别。在仿真中，我们是每200步加一个坑洞。在图4中我们可以看到，改进后的推算定位法比一般的抗随机干扰能力强得多，定位的精度也就高得多了。图 4 DR仿真结果4.2 改进的推算结合基站定位的仿真在推算结合基站定位的仿真中，我们用改进后的推算定位作为主要的定位方法，基站定位只是用来校正。机器人的位置和姿态每10步改变一些。W1坑洞校正数据在200步以后加上。图5（a）是改进的方法的仿真结果，终点距离参考点的距离为0.94mm。图5（b）中是一般方法的仿真结果，终点距参考点的距离为65.33mm。图 5 结合DR和AB的定位仿真在结束位置的参考点、改进方案和一般方案的位置和姿态值如下：105959.6013600.0001，105960.4013599.5001，105904.5013635.1001图5（c）、（d）分别是目标函数E1、E2的结果，图5（e）是机器人利用基站实时校正的次数。我们可以看到（e）中的校对次数很少，说明迭代算法收敛得很快，在实时系统中性能很高。5 结论本文通过分析机器人的状态和轨迹，提出一种新的基于轨迹的运动机器人模型。它可以提供运动路径的信息，即通过信息融合将3个轮子的里程信息以及导向轮的偏航角融合而得到。依靠运动模型提供机器人的位置和姿态。结合推算和基站定位，提出了一种新的移动机器人的在线实时定位方法。仿真结果证明这种方法行之有效。这种方法有如下特点：1) 它适用于不平整的地面。2) 具有很好的实时性。可以适应机器人的高速移动。3) 具有很高的定位精度。在仿真中，每个轮子都加了变化范围为0.2至20的零均值随机噪声。 W1测量数据从第200步开始依据公式（21）加上了坑洞数据。机器人在半径为2000mm的圆周上运动，位置误差仅为0.94mm。4) 具有很强的抗干扰能力。5) 只需要当前数据即可实现机器人位置和姿态的在线校正。6) 暂时性失去超声波信号对定位影响不大。7) 相对于方案12来说，成本优势突显。参考文献1 Borcnstcin J, Feng L, Wehe D. Mobile robot positioning: Sensors and techniques. Journal of Robotic systems, 1997, 14(4):2312492 Thrapp R, Westbrook C, Subramanian D. Robust localization algorithms for an autonomous campus tour guide. In: Proceeding of the IEEE International Conference on Robotics & Automation, Seoul: IEEE Press, 2001. 206520713 Bonnifait Ph, Garcia G. Design and experimental validation of an odometric and goniometric localization system for outdoor robot vehicles. EEE International Conference on Robotics and Automation, 1998,14(4): 5415484 Kleeman L. Optimal estimation of position and heading for mobile robots using ultrasonic beacons and dead-reckoning. In: EEE International Conference on Robotics and Automation, Nice: IEEE Press, 1992, 258225875 Chong K S, Kleeman L. Feature-based mapping in real, large scale environments using an ultrasonic array. International Journal Robotics Reasearch, 1999, 18(1): 3196 Lazea Gh, Lupu E, Patko M. Aspects on kinematic modelling and simulation of wheeled robots. International Symposium on systems Theory, Robotics, Computers and Process Informatics, SINTES 8, Craiova: University of Craiova Press, 1996, 1501567 Chong K.S, Kleeman L. Accurate odometry and error modeling robot. In: Proceedings of IEEE International conference on Robotics and Automation, New Mexico: IEEE Press, 1997, 278327888 Borenstein J, Feng L. Measurement and correction of systematic odonetry errors in mobi