专利名称：一种机器人位置和姿态的推算定位方法及装置的制作方法
技术领域：
本发明属于移动机器人领域中的自定位，具体地说涉及一种机器人位置和姿态的推算定位方法以及用于实现上述方法的装置。
背景技术：
传统上移动机器人的运动模型采用速度和转向角的形式描述(参见“董胜龙，陈卫东，席裕庚，多移动机器人编队的分布式控制系统，机器人，第22卷第6期，第434页，2000年”)。移动机器人的定位，就是确定其当前的位置和方向，其中当前的方向又被称之为姿态。利用里程计推算机器人位置和姿态的定位方法称为推算定位法。传统的移动机器人推算定位法的工作原理为根据上述运动模型，利用两个轮子加装的里程计测出的微小位移，计算出移动机器人的位置和姿态的变化量，通过累加实现移动机器人的自定位(参见“刘传才，杨静宇，江志明，四轮移动机器人的前后轮里程仪系统的误差分析，机器人，第20卷第2期，第143～147页，1998年”)。
目前，移动机器人的推算定位都采用这种利用两个轮子的里程计的推算定位法。这种方法存在的不足之处是任何一个里程计产生的偏差都会严重影响定位精度。例如，在一个轮子打滑或者遇到突起物时，该轮的里程计测出的位移与实际位移有较大偏差，导致推算出的位置和姿态具有较大误差。
发明内容
本发明的目的在于提供一种移动机器人位置和姿态的推算定位方法。
本发明的又一目的在于提供一种用于实现上述方法的装置。
为实现上述目的，本发明的技术方案是利用多个轮子的里程信息获得多组转弯半径和转向角，再通过信息融合技术得到运动轨迹的转弯半径和转向角，利用基于轨迹的运动学模型获得机器人的位置和方向。在本发明中，利用多个轮子的里程计信息进行推算定位，即使一个轮子的里程计测出的位移与实际位移有较大偏差时，通过信息融合也可以得到准确的运动轨迹信息，从而得到准确的机器人位置和姿态。
本发明提供的用于实现上述方法的装置是在机器人底盘上的驱动轮侧增加了一个测量轮，该驱动轮与该测量轮之间没有机械联系。电动机驱动该驱动轮使机器人运动，控制机器人的运动速度；导向轮控制机器人的运动方向。在测量轮与平衡轮上分别各安装一里程计，记录其运动距离。在导向轮上的二个自由度安装二个里程计，其中一个里程计记录导向轮的运动距离，另一个里程计记录导向轮的偏转角。
由上述的四个里程计采集的数据中的任何二个数据计算出采样期间的转弯半径和转向角，因此可有6组数据。但由于地面不平、测量噪声等随机因素的影响，用这6组数据计算出的转弯半径和转向角的值是不同的。为消除随机因素的影响，获得较准确的转弯半径和转向角，对各组不同的转弯半径和转向角用公知的模糊融合算法形成估计值。
将记录导向轮偏转角的里程计在第i次和第i+1次采样时测量到的导向轮的偏转角记为αi和αi+1，逆时针方向为正。由其余三个里程计在第i次和第i+1次采样期间的运动距离记为S1ii+1、S2ii+1、S3ii+1。
移动机器人的运动基本为圆弧运动和直线运动，当移动机器人为圆弧运动时，由下面的位置和姿态递推公式求取机器人O1点的位置和姿态TO1i+1=TO1icθii+1-sθii+1(rii+1+l/2)cθii+1-dsθii+1-(rii+1+l/2)sθii+1cθii+1(rii+1+l/2)sθii+1+dcθii+1-d001]]>当移动机器人为直线运动时，机器人沿 方向运动，S1ii+1、S2ii+1、S3ii+1非常接近，而且导向轮的转向角αi+1≈0。对S1ii+1、S2ii+1、S3ii+1和αi+1融合形成估计值Sii+1。O1点在第i+1次采样时的位置和姿态Ti+1O1可由下式获得TO1i+1=TO1i10001Sii+1001]]>
式中TiO1为移动机器人在第i次采样时刻的位置和姿态；Ti+1O1为移动机器人在i+1次采样时刻的位置和姿态；rii+1和θii+1分别为利用模糊融合算法形成的移动机器人在任一次采样期间的转弯半径和转向角的估计值。
本发明采用的位置和姿态的递推公式只需当前的转弯半径、转向角、上一次采样时刻的位置和姿态等三个参数，适合于在线实时定位。
图1为里程计安装位置示意图；图2为圆弧运动轨迹时运动模型推导原理示意图。
具体实施例方式
请参阅图1。在机器人底盘B上的驱动轮W4侧增加了一个测量轮W1，该驱动轮W4与该测量轮W1之间没有机械联系。电动机M1驱动该驱动轮W4使机器人运动，控制机器人的运动速度；导向轮W3控制机器人的运动方向。在测量轮W1与平衡轮W2上分别各安装一里程计C7和C2，记录其运动距离。在导向轮W3上安装二个里程计C3和C4，其中里程计C3记录导向轮W3的运动距离，另一个里程计C4记录导向轮W3的偏转角。
本例中，里程计C1～C3采用增量型编码器E6D，C4采用绝对型编码器E6CP，计算机采用工业控制计算机ADVANTECH-610。里程计信号输入工业控制计算机，采样周期0.1秒。
请参阅图2。将里程计C4在第i次和第i+1次采样时测量到的导向轮W3的偏转角记为αi和αi+1，逆时针方向为正。由里程计C1～C3在第i次和第i+1次采样期间的运动距离记为S1ii+1、S2ii+1、S3ii+1。由S1ii+1、S2ii+1、S3ii+1和αi+1中的任何两个数据均可计算出第i次和第i+1次采样期间的转弯半径rii+1和转向角θii+1的值，共6组。例如，由S1ii+1、S2ii+1计算出rii+1和θii+1的值为rii+1=S1ii+1S2ii+1-S1ii+1l,]]>θii+1=S2ii+1-S1ii+1l,]]>
其中l为W1与W2之间的距离。
其它5组rii+1和θii+1的值的求取与之类似。但由于地面不平、测量噪声等随机因素的影响，利用不同的两个数据计算出的rii+1、θii+1的值是不同的。为消除随机因素的影响，获得较准确的rii+1与θii+1，对各组不同的rii+1与θii+1用公知的模糊融合算法形成估计值。
移动机器人作圆弧运动时，圆弧中心O2可以看作一个旋转关节，取 与 方向相同，见图2。O2点的位置在W1与W2之间的直线上，W1在 轴的坐标为rii+1。于是，O2点在第i次采样时的位置和姿态TiO1可由第i次采样时的位置和姿态TiO1经平移变换获得，O2点在第i+1次采样时的位置和姿态Ti+1O2可由TiO2经旋转变换获得，O1点在第i+1次采样时的位置和姿态Ti+1O1可由Ti+1O2经平移变换获得。圆弧运动时，利用融合形成估计值rii+1与θii+1，由下面的位置和姿态递推公式求取机器人O1点的位置和姿态TO1i+1=TO1icθii+1-sθii+1(rii+1+l/2)cθii+1-dsθii+1-(rii+1+l/2)sθii+1cθii+1(rii+1+l/2)sθii+1+dcθii+1-d001]]>直线运动时，机器人沿 方向运动，S1ii+1、S2ii+1、D3ii+1非常接近，而且W3的转向角αi+1≈0。对S1ii+1、S2ii+1、S3ii+1和αi+1融合形成估计值Sii+1。O1点在第i+1次采样时的位置和姿态 可由下式获得TO1i+1=TO1i10001Sii+1001]]>为进行比较，传统的推算定位利用C1和C2的信息定位，本发明利用C1～C4的信息定位。在平坦地面上，机器人沿半径2米的圆运动一圈。利用传统的推算定位的位置误差为150mm，相对误差为1.2％。利用本发明的推算定位的位置误差为43mm，相对误差为0.3％。在机器人的W2的运动路径放置一个突起物，机器人沿半径2米的圆运动一圈。利用传统的推算定位的位置误差为756mm，相对误差为6％。利用本发明的推算定位的位置误差为91mm，相对误差为0.7％。
由上述可见，本发明不仅提高了定位精度，而且提高了抗干扰能力，可用于不平整地面。
权利要求
1.一种机器人位置和姿态的推算定位方法，利用多个里程信息获得多组转弯半径和转向角；当移动机器人作圆弧运动时，由下面的位置和姿态递推公式求取机器人的位置和姿态TO1i+1=TO1icθii+1-sθii+1(rii+1+l/2)cθii+1-dsθii+1-(rii+1+l/2)sθii+1cθii+1(rii+1+l/2)sθii+1+dcθii+1-d001]]>当移动机器人为直线运动时，机器人的位置和姿态由下式获得TO1i+1=TO1i10001Sii+1001]]>式中 为移动机器人在第i次采样时刻的位置和姿态； 为移动机器人在i+1次采样时刻的位置和姿态；rii+1和θii+1分别为移动机器人在第i和第i+1次采样期间的转弯半径和转向角。
2.如权利要求1所述的方法，其特征在于，rii+1和θii+1分别为经过模糊融合算法得到的移动机器人在第i和第i+1次采样期间的转弯半径和转向角的估计值。
3.一种用于实现上述方法的装置，在底座上设置有一测量轮，分离的设置在驱动轮的一侧；测量轮与平衡轮上分别安装一个里程计，测量驱动轮和测量轮的运动距离；导向轮上的二个自由度安装二个里程计，其中一个里程计测量导向轮的运动距离，另一个里程计测量导向轮的偏转角；一电动机，带动驱动轮，使移动机器人运动，并控制移动机器人的运动速度；上述各里程计测量的信号输入计算机。
4.如权利要求3所述的装置，其特征在于，所述四个里程计中，测量导向轮偏转角的里程计为绝对型编码器，其余三个测量距离的里程计为相对型编码器。
全文摘要
一种机器人位置和姿态的推算定位方法及装置，在驱动轮侧加装测量轮，在机器人的每个轮子的各个自由度安装里程计。测量距离的里程计采用相对码盘，测量角度的里程计采用绝对码盘。通过不同的里程计信息的组合，获得多组转弯半径和转向角，再通过信息融合技术得到运动轨迹的转弯半径和转向角，然后利用基于轨迹的运动学新模型获得机器人的位置和方向。基于轨迹的运动学模型是一组位置和姿态的递推公式，它只用到当前的转弯半径、转向角、上一次采样时刻的位置和姿态。与传统的利用两个轮子的里程信息的推算定位法相比，本发明不仅提高了定位精度，提高了抗干扰能力，而且可用于不平整地面，并适合于在线实时定位。
文档编号G01C22/00GK1499324SQ02150239
公开日2004年5月26日 申请日期2002年11月6日 优先权日2002年11月6日
发明者徐德, 潭民, 徐 德 申请人:中国科学院自动化研究所