机器人定位中的航迹推算

航迹推算是一种使用最广泛的定位手段，特别适于短时短距离定位， 精度很高。对于长时间运动的，可以应用其他的传感器配合相关的定位算法进行校正。利用陀螺仪和加速度计分别测量出旋转率和加速度，再进行积分，从而可求出走过的距离和航向的变化，进而分析出机器人的位置和姿态。超声波传感器可用于测距，从而探测路标（设置为室内墙壁或天花板），计算位置，来纠正陀螺仪和编码器的定位误差。22图丨航迹推算算法仃用的原理其推导的一般方程为:巴1）加4-Jsin (驴 )其中:WyUg、为车辆在k时刻的位董以及方向比4I ）分别为车辆右轮和左轮在k 时刻到於时刻时间间隔所走过的距离/为车辆的轮距。因为我们用了陀螺仪可以测出转过的角度，所以没有必要用上面的公式，但上式可用于修正陀螺仪测出的角度值。A.t = 鼻 cc 塔+22A v = Ar sin (0 4- AG )AO = qbAa&厂十2注：必然要求陀螺安装在机器人不活动的部件上，并且陀螺的安装只能与车体固 连在一起。微机械陀螺作为重要的传感器，它的输出信号是一个与转动角速率基本成线 性关系的模拟电压值，通过采集其输出的模拟电压值，经过AD转换为数字信号, 对转换完的信号进行标度变换得到其转动的角速率，再积分即可得到角度值根据以上假设，车体被简化成了一个具有两个平移自由度（纵向和侧向）和一个转动自由度（横摆）的单质量刚体。机器人在全局坐标系中的姿态如图 2.3所示。其中，坐标系OXY为全局坐标 系，P点为机器人上的一个参考点，坐标系 OXY，为以P点为原点的车体固连坐 标系，X轴与X轴的夹角为机器人的姿态（Posture）可以用P点在全局坐 标系中的坐标（x, y）和二表示，即可用三维矢量 =（x,y门）T表示。同时，还 可以得到由全局坐标系到车体固连坐标系的坐标旋转矩阵如下：RL）二cos -sin -sin -cos-图2. 1轮式移动机器人的姿态示意图其原理是以地球表面某点作为当地坐标系的原点， 利用里程计输出的距离信 息和特定传感器输出的角度信息，计算确定自主车当前的位置。其推导的一般方程为：kJX(k) =X(0) D(i)cosC )i T(2.8)k -1(2.9)Y(k)二Y(0) D(i)sin()ik =(2.10)珂k） （0）、宀（i）i其中：X（0）, Y（0）为初始时刻自主车所在位置，D（i）与，（i）为从i-1时刻到i时刻自主车行驶距离和方位角。现在航位推算方法很多，但是其本质都是根据传感器得到的数据， 或者直接给出 方位角的信息，或者推算出方位角的值，然后在代入（2.8）、（2.9）式，求出其相 对位置。码盘的距离计算：将两个光电编码器分别安装在车的左右主动轮上，光电编码器同步转动。设车轮直径D，光电编码器线数（即每转一圈输出的脉冲数）是 P，二t时间内光电编码器输出脉冲数是 N，车轮运行距离是S（左轮运行距离：SL，右轮运行距 离厶Sr）,车轮运行速度是 W （左轮运行速度:Vl，右轮运行速度 Vr），且两轮 中心距离为D=2L。那么，J? vL = ：S L / ：t = (NL / P)二-D / X.：Vr 二.：Sr / -：t = (Nr / P)二 D / ：t：v = . S /. ：t = ( :vL ： - vR) / 2 = (N LN R) / 2 / P)二 _D /. ：t通过码盘得到的只是机器小车左右轮的转速，还必须知道两个轮子的周长(或直径)，才能得到机器小车的行进速度。实际操作中发现，直接测量轮子的半径或直径，误差会很大。有两种方法可以测量轮子的周长：(1)直接法，可以让机器人小车在地上行走，车轮转动一周(或更多)，再测量机器小车行走的距 离；(2)间接法，通过DSP工作在仿真模式，让机器小车在地上行走一段距离， 测量这段距离，同时在 CCS中读取码盘的反馈值，(或将程序烧录到DSP后通 过LED显示得到码盘的反馈值)。以上两种方法均可以相对减小测量误差，但推 荐采用第二种方法，因为第二种方法可以同时减小或消除码盘测量的误差。不管采用哪一种方法，都应该加大测量距离，同时通过多次测量取平均，才能达到较 高的精度。运用上述方法可以得到机器小车左轮前进1m的码盘反馈值Nl。和右轮前进 1m的码盘反馈值Nr。(或者左轮的周长Cl和右轮的周长Cr)。在机器小车行走的过程中，实时采集码盘信息，可以分别得到左右轮的行进 速度。而实际系统设计中需要车体的速度，可以将左右轮的速度取平均，得到两 轮连线中心的速度即车体的速度 v，这样可以提高精度，更准确的得到车体行进 的速度。贝9( 3.1)式变为Vl = Nl / NloVR N R / N R0( 4.7)v=(Vl Vr)/2 =(Nl/Nl。Nr/Nro)/2到此，已经实现距离信息的获取。陀螺的角度计算：陀螺的输出信号不是直接与当前的角速率对应，而是输出电压的变化V与角速率成比例关系。因此需要一个静态基准作为参比，实际操作中发现用实验测 定的静态基准作为参比电压，短期内精度还可以满足要求。但过一段时间，陀螺 的静态基点会漂移，这样会造成很大的误差。所以设计时，实时采集陀螺工作的 静态基点，作为参比。由于比赛前机器小车摆放到位到比赛开始还有 1分钟的时 间，可以利用这段时间采集陀螺的静态信号，取平均后作为陀螺当时的静态基点。 并且20分钟内陀螺的静态基点基本无漂移，所以赛前一分钟内采集的数据可以 作为整场比赛的静态基点V。机器小车行进过程中不断采集陀螺的输出信号 V,则* =V -Vo，对应当前的角速率值f(x)二kr：V，其中k是陀螺的比例因子。对角速率的积分就可以得 到角度值，常用积分公式为梯形公式。设已知a时刻的初始角度为 片，x时刻对应的速率值为f (x)，则可求出b时刻的角度一 (b a)(4.4)构造以a, b为结点的线性插值多项式 f (x) 口 f(a) 口 f(b) a bb a则从a时刻到b时刻角度的变化量为：b二 f (x)dx = af(a)诜 f(b) dxf (a)a -balpdX 出ba (x - a)dxf (a)a b1(-b)2(b - a)2(4.5)1-(b-a)f(a) f (b)l2可得：2 _ 3(4.6)1-片 (b - a) I f (ap f (b) I2这样，在初始角度齐的基础上，每个采样周期都计算一次更新角度七，这个二2相当于下一个周期的初始坐标片，如此循环，就可以知道任意时刻机器人 的角度变化及姿态。但是机器人在运动的过程中，角速率不断积分，得到的角度会大于360或者 小于0，这样会给后续的计算和路径规划带来很大麻烦。 因此，在实际操作中， 将得到的角度规范到0, 360 )的区间内，即0,360) o要知道机器人的位置坐标，首先需要建立 x-y二维坐标系。由于根据陀螺和 码盘计算的位置坐标和姿态都是相对于车体的起始位置和姿态，即是在车体坐标系(以车体的参考点为坐标原点，x轴方向与全场坐标系的x轴方向大概一致， 误差是由摆放的角度误差决定的)中，这样如果比赛策略需求车体的起始摆放位 置变化，那么整个坐标系都会有变化，场上关键点的坐标也会变化。 这样通用性 很差，而且不利于操作。所以建立一个全场坐标系很有必要， 它不依赖车体的起 始位置和姿态，而是通过改变车体的起始坐标和姿态角来实现不同的摆车需求。图4. 1 机器人运动原理图建立图4.8所示的直角坐标系。机器人从A(x,y,R点出发，经时间厶t沿圆弧走到A(x,y)，即A (x . x,y y ： i，其中x, y分别表示机器人在起始位置的横、纵坐标值以及前进方向与横坐标的夹角，逆时针方向为正的方向角， ixiy,T表示在氏时间内机器人的横、纵坐标和角度的增加量，丄S表示A点到A点的弧长。于是 n 可由如下的公式计算得到:A0：x = AAcos( )2A0a A6=2R *sin() *cos()2 2S *sin()/ *cos( )2 2 y = AAsi n()2=2R *sin() *sin( )2 2二：Ssin() / *sin()2 2 2其中，AA表示A点到A点的距离。当机器人走直线时sin()10，则式(4.8),(4.9)可写成:2 2f八A6lx _ ：S *cos( ) 2y = S *sin()L2(4.8)(4.9)(4.10)由于时间间隔-t很短，线段AA的长度近似等于圆弧长度，设讥时间内编 码盘走过的距离为I，则I 於。设可为上次采样的角度，七为当前的角度，则6 -。通过计算注时间内机器人的位置变化量：x ：y，进行累加就可以求出机器人在整个场地上的位置。A9x = x x = x AAcos( )(丁)(4.11)2日1 +日2=x I * cos(二2)2= y AAsin(+ T)=y I *sin()(4.=x I *cos(2+92=y 丨 *sin(_)2这样，在初始坐标(x,y)的基础上，每个采样周期都计算一次更新坐标(x, y)，这个(x,y)相当于下一个周期的初始坐标(x,y)，如此循环，就可以知道任意时刻机器人所位置在的坐标