机器人定位中的航迹推算

航迹推算是一种使用最广泛的定位手段，特别适于短时短距离定位，精度很高。

对于长时间运动的，可以应用其他的传感器配合相关的定位算法进行校正。

利用陀螺仪和加速度计分别测量出旋转率和加速度，再进行积分，从而可求出走过的距离和航向的变化，进而分析出机器人的位置和姿态。

超声波传感器可用于测距，从而探测路标（设置为室内墙壁或天花板），计算位置，来纠正陀螺仪和编码器的定位误差。

的轮距。

因为我们用了陀螺仪可以测出转过的角度，所以没有必要用上面的公式，但上式可用于修正陀螺仪测出的角度值。

注：必然要求陀螺安装在机器人不活动的部件上，并且陀螺的安装只能与车体固连在一起。

微机械陀螺作为重要的传感器，它的输出信号是一个与转动角速率基本成线性关系的模拟电压值，通过采集其输出的模拟电压值，经过AD 转换为数字信号，对转换完的信号进行标度变换得到其转动的角速率，再积分即可得到角度值[16]。

根据以上假设，车体被简化成了一个具有两个平移自由度(纵向和侧向)和一个转动自由度(横摆)的单质量刚体。

；机器人在全局坐标系中的姿态如图所示。

其中，坐标系OXY 为全局坐标系，P 点为机器人上的一个参考点，坐标系Y X O '''为以P 点为原点的车体固连坐标系，X '轴与X 轴的夹角为θ。

机器人的姿态（Posture ）可以用P 点在全局坐标系中的坐标（x ，y ）和θ表示，即可用三维矢量T y x ),,(θξ=表示。

同时，还可以得到由全局坐标系到车体固连坐标系的坐标旋转矩阵如下：图2. 3 轮式移动机器人的姿态示意图XYOθOX 'xy￥PY 'cos sin 0()sin cos 0001R θθθθθ-⎛⎫ ⎪= ⎪ ⎪⎝⎭[13]图2. 5 航位推算算法(DR)的原理其原理是以地球表面某点作为当地坐标系的原点，利用里程计输出的距离信息和特定传感器输出的角度信息，计算确定自主车当前的位置。

其推导的一般方程为：10()(0)()cos(())k i X k X D i i ϕ-==+∑10()(0)()sin(())k i Y k Y D i i ϕ-==+∑10()(0)()k i k i θθθ-==+∆∑其中：(0)X ,(0)Y 为初始时刻自主车所在位置，()D i 与()i θ为从i-1时刻到i 时刻自主车行驶距离和方位角。

·现在航位推算方法很多，但是其本质都是根据传感器得到的数据，或者直接给出方位角的信息，或者推算出方位角的值，然后在代入、式，求出其相对位置。

码盘的距离计算：将两个光电编码器分别安装在车的左右主动轮上，光电编码器同步转动。

设车轮直径D ，光电编码器线数（即每转一圈输出的脉冲数）是P ，t ∆时间内光电编码器输出脉冲数是N ，车轮运行距离是S ∆ (左轮运行距离L S ∆，右轮运行距离R S ∆)，车轮运行速度是v ∆ (左轮运行速度L v ∆，右轮运行速度R v ∆)，且两轮中心距离为D=2L 。

那么，/(/)//(/)//()/2(()/2/)/L L L R R R L R L R v S t N P D t v S t N P D tv S t v v N N P D tπππ∆=∆∆=∆⎧⎪∆=∆∆=∆⎨⎪∆=∆∆=∆+∆=+∆⎩通过码盘得到的只是机器小车左右轮的转速，还必须知道两个轮子的周长（或直径），才能得到机器小车的行进速度。

实际操作中发现，直接测量轮子的半径或直径，误差会很大。

有两种方法可以测量轮子的周长：（1）直接法，可以让机器人小车在地上行走，车轮转动一周（或更多），再测量机器小车行走的距离；（2）间接法，通过DSP 工作在仿真模式，让机器小车在地上行走一段距离，测量这段距离，同时在CCS 中读取码盘的反馈值，（或将程序烧录到DSP 后通过LED 显示得到码盘的反馈值）。

以上两种方法均可以相对减小测量误差，但推荐采用第二种方法，因为第二种方法可以同时减小或消除码盘测量的误差。

不管采用哪一种方法，都应该加大测量距离，同时通过多次测量取平均，才能达到较高的精度。

运用上述方法可以得到机器小车左轮前进1m 的码盘反馈值0L N 和右轮前进1m 的码盘反馈值0R N （或者左轮的周长L C 和右轮的周长R C ）。

在机器小车行走的过程中，实时采集码盘信息，可以分别得到左右轮的行进速度。

而实际系统设计中需要车体的速度，可以将左右轮的速度取平均，得到两轮连线中心的速度即车体的速度v ，这样可以提高精度，更准确的得到车体行进的速度。

则（）式变为0000//()/2(//)/2L L L R R R L R L L R R v N N v N N v v v N N N N =⎧⎪=⎨⎪=+=+⎩（） 到此，已经实现距离信息的获取。

…陀螺的角度计算：陀螺的输出信号不是直接与当前的角速率对应，而是输出电压的变化V ∆与角速率成比例关系。

因此需要一个静态基准作为参比，实际操作中发现用实验测定的静态基准作为参比电压，短期内精度还可以满足要求。

但过一段时间，陀螺的静态基点会漂移，这样会造成很大的误差。

所以设计时，实时采集陀螺工作的静态基点，作为参比。

由于比赛前机器小车摆放到位到比赛开始还有1分钟的时间，可以利用这段时间采集陀螺的静态信号，取平均后作为陀螺当时的静态基点。

并且20分钟内陀螺的静态基点基本无漂移，所以赛前一分钟内采集的数据可以作为整场比赛的静态基点0V 。

机器小车行进过程中不断采集陀螺的输出信号V ，则0V V V ∆=-，对应当前的角速率值()f x k V =*∆，其中k 是陀螺的比例因子。

对角速率的积分就可以得到角度值，常用积分公式为梯形公式。

设已知a 时刻的初始角度为1θ，x 时刻对应的速率值为()f x ，则可求出b 时刻的角度2θ (b →a )。

构造以a ，b 为结点的线性插值多项式()()()x b x af x f a f b a b b a --=+-- （）则从a 时刻到b 时刻角度的变化量为：[]22()()()()()()()()1()1()()221()()()2bba ab b a a x b x a f x dx f a f b dxa b b a f a f b x b dx x a dx a b b af a f b a b b a a b b a b a f a f b θ--⎡⎤∆==+⎢⎥--⎣⎦=-+---=⨯-+⨯---=-+⎰⎰⎰⎰ （）可得：[]2111 ()()()2b a f a f b θθθθ=+∆=+-+（）这样，在初始角度1θ的基础上，每个采样周期都计算一次更新角度2θ，这个2θ相当于下一个周期的初始坐标1θ，如此循环，就可以知道任意时刻机器人的角度变化及姿态。

：但是机器人在运动的过程中，角速率不断积分，得到的角度会大于360︒或者小于0°，这样会给后续的计算和路径规划带来很大麻烦。

因此，在实际操作中，将得到的角度规范到[0, 360︒）的区间内，即[0,360)θ∈。

要知道机器人的位置坐标，首先需要建立x-y 二维坐标系。

由于根据陀螺和码盘计算的位置坐标和姿态都是相对于车体的起始位置和姿态，即是在车体坐标系（以车体的参考点为坐标原点，x 轴方向与全场坐标系的x 轴方向大概一致，误差是由摆放的角度误差决定的）中，这样如果比赛策略需求车体的起始摆放位置变化，那么整个坐标系都会有变化，场上关键点的坐标也会变化。

这样通用性很差，而且不利于操作。

所以建立一个全场坐标系很有必要，它不依赖车体的起始位置和姿态，而是通过改变车体的起始坐标和姿态角来实现不同的摆车需求。

图4. 8 机器人运动原理图建立图所示的直角坐标系。

机器人从(,,)A x y θ点出发，经时间t ∆沿圆弧走到'A (',',')x y θ，即'A (,,)x x y y θθ+∆+∆+∆，其中,,x y θ分别表示机器人在起始位置的横、纵坐标值以及前进方向与横坐标的夹角，逆时针方向为正的方向角，,,x y θ∆∆∆表示在t ∆时间内机器人的横、纵坐标和角度的增加量，S ∆表示A 点到'A 点的弧长。

于是,x y ∆∆可由如下的公式计算得到：'cos()22sin()cos()22 [sin()/]cos()222x AA R S θθθθθθθθθ∆∆=•+∆∆=••+∆∆∆=∆••+ 'sin()22sin()sin()22 [sin()/]sin()222y AA R S θθθθθθθθθ∆∆=•+∆∆=••+∆∆∆=∆••+ 其中，'AA 表示A 点到'A 点的距离。

当机器人走直线时sin()/022θθ∆∆=，则式,可写成： |cos()2sin()2x S y S θθθθ∆⎧∆=∆•+⎪⎪⎨∆⎪∆=∆•+⎪⎩由于时间间隔t ∆很短，线段'AA 的长度近似等于圆弧长度，设t ∆时间内编码盘走过的距离为l ，则l S ≈∆。

设1θ为上次采样的角度，2θ为当前的角度，则2θθθ=+∆。

通过计算t ∆时间内机器人的位置变化量,x y ∆∆，进行累加就可以求出机器人在整个场地上的位置。

12''cos()2()cos()2cos()2x x x x AA x l x l θθθθθθθ∆=+∆=++++∆=+•+=+• （） 12''sin()2()sin()2sin()2y y y y AA y l y l θθθθθθθ∆=+∆=++++∆=+•+=+• （） 这样，在初始坐标(x,y)的基础上，每个采样周期都计算一次更新坐标(',')x y ，这个(',')x y 相当于下一个周期的初始坐标(,)x y ，如此循环，就可以知道任意时刻机器人所位置在的坐标。