机器人定位中的航迹推算

航迹推算是一种使用最广泛的定位手段，特别适于短时短距离定位，精度很高。对于长时间运动的，可以应用其他的传感器配合相关的定位算法进行校正。

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

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

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

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

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

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

机器人在全局坐标系中的姿态如图所示。其中，坐标系OXY 为全局坐标系，P 点为机器人上的一个参考点，坐标系Y X O '''为以P 点为原点的车体固连坐标系，X '轴与X 轴的夹角为θ。机器人的姿态（Posture ）可以用P 点在全局坐标系中的坐标（x ，y ）和θ表示，即可用三维矢量T y x ),,(θξ=表示。同时，还可以得到由全局坐标系到车体固连坐标系的坐标旋转矩阵如下：

图2. 3 轮式移动机器人的姿态示意图

X

Y

O

θ

O

X '

x

y

￥

P

Y '

cos sin 0()sin cos 000

1R θθθθ

θ-⎛⎫ ⎪= ⎪ ⎪⎝

⎭

[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 t

v 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 ，这样可以提高精度，更准确的得到车体行进的速度。则（）式变为

00

00//()/2(//)/2

L 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 是陀螺的比例因子。对角速率的积分就可以得