机器人定位中的航迹推算

1、航迹推算是一种使用最广泛的定位手段，特别适于短时短距离定位，精度很高。对于长时间 运动的，可以应用其他的传感器配合相关的定位算法进行校正。利用陀螺仪和加速度计分别测量出旋转率和加速度，再进行积分，从而可求出走过的距离和 航向的变化，进而分析出机器人的位置和姿态。超声波传感器可用于测距，从而探测路标（设置为室内墙壁或天花板），计算位置，来纠正 陀螺仪和编码器的定位误差。(X(0)>Y(0)图1 航迹推算算法（DR）的原理其推导的一般方程为:2、（/）=2、（万一1 > +5 人(1 1 ) + 1)2s,一y = A3 sin(0 + A6 )COS(9( & - 1 )Z

2、J y ,7,5及(4一 1 )+ S/.(一)sin(9a 1)z > A. SR(k > SL(k1 )(pg=4(k ) HT)其中柒），到上）为车辆在k时刻的位置以及方向，s“g1）,5：晨一1）分别为车辆右轮和左轮在旌1时刻到k时刻时间间隔所走过的距离“为车辆 的轮距。因为我们 用了陀螺仪可以测出转过的角度，所以没有必要用上面的公式，但上式可用于修正陀螺仪测 出的角度值。A0注：必然要求陀螺安装在机器人不活动的部件上，并且陀螺的安装只能与车体固 连在一起。微机械陀螺作为重要的传感器，它的输出信号是一个与转动角速率基本成线 性关系的模拟电压值，通过采集其输出的模拟电压值，经

3、过AD转换为数字信号, 对转换完的信号进行标度变换得到其转动的角速率，再积分即可得到角度值。根据以上假设，车体被简化成了一个具有两个平移自由度（纵向和侧向）和一 个转动自由度（横摆）的单质量刚体。机器人在全局坐标系中的姿态如图所示。其中，坐标系。TY为全局坐标系， 夕点为机器人上的一个参考点，坐标系O'XP为以P点为原点的车体固连坐标系, X，轴与1轴的夹角为8。机器人的姿态（Posture）可以用点在全局坐标系中 的坐标（x, y）和6表示，即可用三维矢量4 = （x,y,6）7'表示。同时，还可以得 到由全局坐标系到车体固连坐标系的坐标旋转矩阵如下：'cos

4、4; 一 sin 6 O'R(6)= sing cos 6 0< 001，本系统在设计时主要参考DR航位推算m，DR的图解如图。其原理是以地球表面某点作为当地坐标系的原点，利用里程计输出的距离信 息和特定传感器输出的角度信息，计算确定自主车当前的位置。其推导的一般方程为：X （） = X （0） + 1。 cos（9（i）k-ly a）=y（o）+gjo（i）sin（*）"1伏攵） = 6（0）+.（，）其中：x（o）,y（o）为初始时刻自主车所在位置，。与。为从i-i时 刻到i时刻自主车行驶距离和方位角。现在航位推算方法很多，但是其本质都是根据传感器得到的数据，或者直

5、接给出 方位角的信息，或者推算出方位角的值，然后在代入、式，求出其相对位置。码盘的距离计算：将两个光电编码器分别安装在车的左右主动轮上，光电编码器同步转动。设 车轮直径D,光电编码器线数（即每转一圈输出的脉冲数）是P, 时间内光电 编码器输出脉冲数是、，车轮运行距离是A5 （左轮运行距离$一右轮运行距离AS&）,车轮运行速度是口（左轮运行速度匕，右轮运行速度A。），且两轮中心距离为D=2L。那么，Ai3 = AS Z /Ar = （N/ / P）mD / Ar< A喙=ASk / Ar = （Nk / P）乃.D /Av = $ / Ar = （> + A。）/ 2 = （

6、N/ + NQ / 2 / P）wD / Z通过码盘得到的只是机器小车左右轮的转速，还必须知道两个轮子的周长 （或直径），才能得到机器小车的行进速度。实际操作中发现，直接测量轮子的 半径或直径，误差会很大。有两种方法可以测量轮子的周长：（1）直接法，可以 让机器人小车在地上行走，车轮转动一周（或更多），再测量机器小车行走的距 离；（2）间接法，通过DSP工作在仿真模式，让机器小车在地上行走一段距离， 测量这段距离，同时在CCS中读取码盘的反馈值，（或将程序烧录到DSP后通过 LED显示得到码盘的反馈值）。以上两种方法均可以相对减小测量误差，但推荐 采用第二种方法，因为第二种方法可以同时减小或消

7、除码盘测量的误差。不管采 用哪一种方法，都应该加大测量距离，同时通过多次测量取平均，才能达到较高 的精度。运用上述方法可以得到机器小车左轮前进1m的码盘反馈值和右轮前进1m的码盘反馈值%刖（或者左轮的周长Q和右轮的周长CQ。在机器小车行走的过程中，实时采集码盘信息，可以分别得到左右轮的行进 速度。而实际系统设计中需要车体的速度，可以将左右轮的速度取平均，得到两 轮连线中心的速度即车体的速度V,这样可以提高精度，更准确的得到车体行进 的速度。则（）式变为点= NJNl。=Nr/Nr0（）n =（4 +。）/2 = （N, / Nlo + NJ NQI2到此，已经实现距离信息的获取。陀螺的角度计算

8、：陀螺的输出信号不是直接与当前的角速率对应，而是输出电压的变化AV与 角速率成比例关系。因此需要一个静态基准作为参比，实际操作中发现用实验测 定的静态基准作为参比电压，短期内精度还可以满足要求。但过一段时间，陀螺 的静态基点会漂移，这样会造成很大的误差。所以设计时，实时采集陀螺工作的 静态基点，作为参比。由于比赛前机器小车摆放到位到比赛开始还有1分钟的时 间，可以利用这段时间采集陀螺的静态信号，取平均后作为陀螺当时的静态基点。 并且20分钟内陀螺的静态基点基本无漂移，所以赛前一分钟内采集的数据可以 作为整场比赛的静态基点匕。机器小车行进过程中不断采集陀螺的输出信号V,则对应当前 的角速率值/（

9、x） = k*AV,其中k是陀螺的比例因子。对角速率的积分就可以得到角度值，常用积分公式为梯形公式。设已知。时刻的初始角度为q, x时刻对应的速率值为/（X）,则可求出/，时刻的角度名(bfa)。构造以。，为结点的线性插值多项式于(x) =f(a) +f(b)a-bb-a则从。时刻到匕时刻角度的变化量为：rb戈一Z?X - CI8= "皿=-/(«) + -f(b) dxJab-a+ (x-a)dxa-bJab-aJa= /m)xl(-b)2 + f(b) a-b 2b-a 2= L(b-a)f(a) + f(b)可得：& = q + 夕=a+J(-，)/()+/例

10、 乙()()()这样，在初始角度q的基础上，每个采样周期都计算一次更新角度为,这 个名相当于下一个周期的初始坐标a，如此循环，就可以知道任意时刻机器人 的角度变化及姿态。但是机器人在运动的过程中，角速率不断积分，得到的角度会大于360°或者 小于0° ,这样会给后续的计算和路径规划带来很大麻烦。因此，在实际操作中, 将得到的角度规范到0, 360°）的区间内，即6日0,360）。要知道机器人的位置坐标，首先需要建立x-y二维坐标系。由于根据陀螺和 码盘计算的位置坐标和姿态都是相对于车体的起始位置和姿态，即是在车体坐标 系（以车体的参考点为坐标原点，x轴方向与全场坐

11、标系的x轴方向大概一致， 误差是由摆放的角度误差决定的）中，这样如果比赛策略需求车体的起始摆放位 置变化，那么整个坐标系都会有变化，场上关键点的坐标也会变化。这样通用性 很差，而且不利于操作。所以建立一个全场坐标系很有必要，它不依赖车体的起 始位置和姿态，而是通过改变车体的起始坐标和姿态角来实现不同的摆车需求。图4. 8机器人运动原理图建立图所示的直角坐标系。机器人从4匕），,8）点出发，经时间/沿圆弧走 到即4（x + Ax,y + Ay,e + A8）,其中x,y力分别表示机器人在起始 位置的横、纵坐标值以及前进方向与横坐标的夹角，逆时针方向为正的方向角， ，乂小。表示在7时间内机器人的横

12、、纵坐标和角度的增加量，AS表示A点 到/V点的瓠长。于是以,），可由如下的公式计算得到:Ax = AA e cos(6 +a n=2R sin() cos(6 + ) 22e=A5 sin()/ cos(6 + ) 222Ay = AA '>sin( +=2R sin(-) sin( 0 +e22AC r ./£、 /.，C=AS sin()/ sin( 0 + 22其中，AA1表示A点到/T点的距离。当机器人走直线时si吟吟=。，则式，可写成:eAx = A5 cos(6 + ；-)Ay = $ sin(6 + 7-)由于时间间隔&很短，线段A4'的长度近似等于圆弧长度，设，时间内编码盘走过的距离为/,贝设q为上次采样的角度，4为当前的角度，则 3=小8。通过计算加时间内机器人的位置变化量人、,），进行累加就可以求 出机器人在整个场地上的位置。0x' = x +Ar = x + 44'cos(6 + )z/。+ (。+ 八。)、()()=X + / c