移动机器人轮式里程计
移动机器人灵魂三问：我在哪？ 我要去哪里？ 怎么去？其中，第一问对应机器人定位问题。定位问题可阐述为：移动机器人根据自身状态、传感器信息实时确定自己在世界（全局或局部）中的位置与姿态。 阿克曼转向的无人驾驶汽车的定位方案主要有：轮式里程计、视觉里程计、激光里程计、惯性导航模块（IMU+GPS）以及多传感器融合。轮式里程计是一种最简单，获取成本最低的方法。与其它定位方案一样，轮式里程计也需要传感器感知外部信息，只不过，轮式里程计采用的电机转速测量模块是一种成本非常低廉的传感器。本文对搭建智能小车系统过程中构建小车轮式里程计的实践进行总结，主要涉及轮式里程计的构建原理、方法、结论与不足等方面。
一、轮式里程计原理
如果你拥有一辆马车，你知道你马车轮子的周长，并且你安装了一种装置可以统计车轮转了多少圏数。你从A地一到达B地，便可以计算出A到B的路程，换句话说：你知道自己行走了多远的路程。
具体地，我对于左右两轮分别安装一个转速测量单元，我可以实时获取马车当前左右两轮的转速 a l , a r a_l, a_r al,ar，单位为(圈/s)。我们可以测量轮子的周长为 S S S，进一步得左右轮行进的速度 v l = a l S , v r = a r S v_l=a_l S, v_r=a_r S vl=alS,vr=arS。我们取左右两轮轴中心那个点的速度作为马车本体的速度，可计算为 v = v l + v r 2 v=\frac{v_l+v_r}{2} v=2vl+vr。
以上，我们通过给马车左右两轮各安装一个转速测量装置，便可实时获取马车当前的行进速度，从而计算马车行进的路程。
对于移动机器人来说，“当前行驶了多少路程”的确是一个有用的信息。假定机器人的初始始位置已知，在一个行驶维度为一维的环境（例如：单行线公路、单行线导轨），机器人可以根据轮子的圈数实时估计自己在这个一维行驶环境的何处。
但是，绝大部分移动机器人应用都不是一维这样简单的环境，而是二维的，甚至是三维的（加上高度）。先说明，单单轮式里程计信息最多能够应对二维导航环境（x, y）。我们还拿上面那辆马车来进一步说明，轮式里程计如何用于二维平面的定位。
想象一下：你驾着马车在广场上，想往左边那个出去行进；于是，你向左边扯了一下马的缰绳，车子便向左边转去…
假定你在马车左右两个轮子上都安装了测量转圏数的传感器，你会发现同一时间内，马车左侧的轮子转的圈数少些（碾压过的痕迹短一些），右侧的轮子转的圈数大些（碾压过的痕迹长一些）。也即，左侧轮子的转速慢于右侧轮子。反之，你可以根据左侧轮子与右侧轮子的转速来获取马车的行进方向信息。
进一步想象一下：你的马被一根绳子限制绕广场中央的一根大柱子绕圏。先把马车看作一个质点，则马车行进的速度(v)、角度变化率（角速度 ω \omega ω）以及马车离中心柱子的距离( R R R)的关系是有数学公式表示：
v = ω R v=\omega R v=ωR
我们将马车绕柱走的图局部放大得到如下图，
假定马车左右车轮从黑色处驶到蓝色处，我们可以看到左轮与右轮分另压出了两道辙，分别称之为内辙与外辙。内辙的长度 s l s_l sl比外辙 s r s_r sr要短。我们可以测量马车左右两轮的距离，被称为后轴距，标记为 T T T。由几何关系可知，马车转的角度 θ \theta θ与所做圆周运动的角度相等。可得：
{ s l = θ R s r = θ ( R + T ) \left\{ \begin{array}{cc} s_l=\theta R \\ s_r=\theta(R+T)\end{array}\right. {sl=θRsr=θ(R+T)
等式两边除以时间，进一步得：
{ v l = ω R v r = ω ( R + T ) \left\{ \begin{array}{cc} v_l=\omega R \\ v_r=\omega(R+T)\end{array}\right. {vl=ωRvr=ω(R+T)
以上式子可反推出角速度：
ω = v r − v l T \omega=\frac{v_r-v_l}{T} ω=Tvr−vl
左右轮的转速本身可以用来计算线速度，他两者又可以计算角速度。在一个平面上行驶的地面移动机器人，知道线速度 v v v与角速度 ω \omega ω，便可以在线推演出它在二维平面上的位置( x , y x, y x,y)与姿态（ φ \varphi φ）。
至此，你发现了轮式里程计所有的奥秘。
二、轮式里程计获取方法
2.1以阿克曼转向汽车为例
如果，机器人为与Turtlebot类似的左右差速驱动，控制是将期望速度与角速度转换为左右轮的期望转速，里程计则是通过实时测量的左右轮转速信息推演机器人的轮式里程计信息。很直接，没什么好讲的。与这种简单的左右差速驱动的机器人不同，现实中的汽车都是阿克曼转向几何运动学模型，具体可看之前的博客无人车系统（一）：运动学模型及其线性化。
我们知道，汽车是通过方向盘控制车的转向，通过刹车油门控制车的行驶速度（准确的说应该是直接控制加速度，间接控制了速度）。后面两个轮为主动轮，前面为从动轮。后轴上的左右轮共用一个发动机驱动。为了能让汽车在转弯，或曲线行驶时，左右轮的速度能相互协调，这时就需要加入差速器用以调整左右轮的转速差。备注：差速器也用于四驱车中，用于调节前后四个轮子的转速差。 下图显示的就是一个差速器。
因此，一辆在路面上行驶，轮胎没有打滑的汽车，左右轮的转速依然可以用来做轮式里程计。
我们利用转速测量单元获得转化后的后轴左右轮行驶速度分别为 v l , v r v_l, v_r vl,vr，则可得汽车的瞬时速度：
v = v l + v r 2 (1) v=\frac{v_l+v_r}{2} \tag{1} v=2vl+vr(1)
同样可得瞬时的角速度：
ω = v r − v l T (2) \omega=\frac{v_r-v_l}{T}\tag{2} ω=Tvr−vl(2)
可以利用如下运动学模型实时在线推演汽车的位置与姿态。
{ x t + 1 = x t + v t cos ( θ t ) d t y t + 1 = y t + v t sin ( θ t d t θ t + 1 = θ t + ω t d t (3) \left\{\begin{array}{l} x_{t+1}=x_{t}+v_t \cos (\theta_{t})d_t \\ y_{t+1}=y_{t}+v_t \sin (\theta_{t}d_t \\ \theta_{t+1}=\theta_{t}+\omega_t d_t \end{array}\right. \tag{3} ⎩⎨⎧xt+1=xt+vtcos(θt)dtyt+1=yt+vtsin(θtdtθt+1=θt+ωtdt(3)
其中，第 t t t次观测的速度为 v t v_t vt，角速度为 ω t \omega_{t} ωt。由此，我们构建了普通汽车的轮式里程计信息。
2.2以阿克曼转向智能小车为例
普通阿克曼智能小车考虑机构与控制成本问题，一般不会采用复杂的差速器机构。一般的阿克曼智能小车底盘标配是后轮两个直流电机控制速度，前面一个舵机控制转向。由于没有差速器，为了能够很好的完成曲线行驶，后面左右两的转速一般不是相等的，而是根据转向角度有一个主动差速。所以，左右两个电机在负责小车速度的同时，同时还需要配合舵机控制小车的转向。
由于，左右电机是主动采用PID控制转速来达到期望速度与期望转角的，小车实际的航向由舵机与两个电机一起决定，相互影响。当舵机控制到的期望角度与电机要达到的期望角度不一致时，由于侧滑的存在，我们得到的轮式里程计是不准确的。只有当舵机要达到的转向角与电机达到的转向角一致时，我们得到的轮式里程计才是准确的。
2.2.1 正向控制
由于前轮转角装置的原因，前面左右两轮的在同一方向盘控制下，往往形成的转角不同（如上上图的 α , β \alpha,\beta α,β所示）。单独分析前面左右两轮的转角对于分析汽车模型益处不大，于是阿克曼四轮小车的模型进一步简化为如下图所示的两轮单车模型（虚拟的前后轮都是汽车的中轴线上）。
普通汽车的几何运动学模型如下（控制输入为前轮转角 δ \delta δ与线速度 v v v）：
{ x ˙ = v cos ( θ ) y ˙ = v sin ( θ ) θ ˙ = v tan ( δ ) L (4) \left\{\begin{array}{l} \dot{x}=v \cos (\theta) \\ \dot{y}=v \sin (\theta) \\ \dot{\theta}=v \frac{\tan (\delta)}{L} \end{array}\right. \tag{4} ⎩⎨⎧x˙=vcos(θ)y˙=vsin(θ)θ˙=vLtan(δ)(4)
离散形式为：
{ x t + 1 = x t + v t cos ( θ t ) y t + 1 = y t + v sin ( θ t θ t + 1 = θ t + v tan ( δ t t ) L (5) \left\{\begin{array}{l} x_{t+1}=x_{t}+v_t \cos (\theta_{t}) \\ y_{t+1}=y_{t}+v \sin (\theta_{t} \\ \theta_{t+1}=\theta_{t}+v \frac{\tan (\delta_{t_t})}{L} \end{array}\right. \tag{5} ⎩⎨⎧xt+1=xt+vtcos(θt)yt+1=yt+vsin(θtθt+1=θt+vLtan(δtt)(5)
阿克曼小车可以通过PID控制器，允许接收期望的速度作为控制输入，但是在方向控制上，是转向角控制，也即前轮胎的转向角度作为输入的。
由于后轴两个电机要控制速度，又要同时配合舵机控制航向，进一步可计算出左右轮的速度 v r , v l v_r, v_l vr,vl。假定当前我们要期望的转向角为 δ \delta δ，首先舵机控制前轴两轮合并出的转向角应该为 δ \delta δ，然后，后轴两个电机控制左右两轮的速度符合该转向角下的条件：
{ v l = v ( 1 − T tan ( δ ) 2 L ) v r = v ( 1 + T tan ( δ ) 2 L ) (6) \left\{ \begin{array}{cc} v_l=v(1-\frac{T\tan(\delta)}{2L})\\ v_r=v(1+\frac{T\tan(\delta)}{2L})\end{array}\right.\tag{6} {vl=v(1−2LTtan(δ))vr=v(1+2LTtan(δ))(6)
2.2.2 反向推演
只要正向控制准确了，反射推演出来的里程计信息就比较准确了。推演的过程与普通汽车的推演公式一样。
我自己搭了一台阿克曼转向智能小车，推演的轮式里程计与肉眼观测值偏差很大。如果直行时，轮式里程计误差还可以容忍，前进两米，误差为5cm左右。但是一转向，航向角度偏差非常大。角度一偏差，后面得到的位置根本就不能用。
这主要是因为，舵机的角度与小车转向角的关系没有标定好。例如：我本来想让小车转20度，由于转换关系不准，舵机控制得出的是1790，最终控制的转角为30度。这只是打个比方，总之类似。
于是我，重新采集数据，拟合出了舵机控制值与转向角之间的函数关系。
- 先采集不同舵机值下的小车转弯半径（转弯半径可计算得到转向角度）
注意：采集的时候，电机一定是释放状态，只控制舵机，然后用手推着小车运动。
| 舵机值 | 转弯半径 | 转向角度 |
|---|---|---|
| 747 | -0.575/2 | -2.66845002e+01 |
| 832 | -0.618/2 | -2.50625556e+01 |
| 918 | -0.675/2 | -2.31780985e+01 |
| 1003 | -0.77/2 | -2.05723525e+01 |
| 1088 | -0.89/2 | -1.79896036e+01 |
| 1174 | -1.13/2 | -1.43460118e+01 |
| 1259 | -1.75/2 | -9.37735309e+00 |
| 1344 | -1.812 | -4.55946818e+00 |
| 1433 | 9999999.0 | 8.27924097e-07 |
| 1515 | 1.745 | 4.73374990e+00 |
| 1600 | 1.54/2 | 1.06286404e+01 |
| 1685 | 1.11/2 | 1.45935543e+01 |
| 1771 | 0.86/2 | 1.85747431e+01 |
| 1856 | 0.715/2 | 2.20083522e+01 |
| 1941 | 0.63/2 | 2.46423513e+01 |
| 2027 | 0.585/2 | 2.62901592e+01 |
| 2112 | 0.55/2 | 2.77198853e+01 |
- 利用多项式拟合
from math import *
import numpy as np
L = 0.1445
r = np.array([-0.575/2, -0.618/2, -0.675/2, -0.77/2, -0.89/2, -1.13/2, -1.75/2, -1.812, 9999999.0, 1.745, 1.54/2, 1.11/2, 0.86/2, 0.715/2, 0.63/2, 0.585/2, 0.55/2])
x = np.arctan(L/r)*180/np.pi
y = np.array([747, 832, 918, 1003, 1088, 1174, 1259, 1344, 1433, 1515, 1600, 1685, 1771, 1856, 1941, 2027, 2112])
p = np.polyfit(x, y, 3) # 得到多项式函数的参数P
import matplotlib.pyplot as plt
%matplotlib inline
x_test = []
y_test = []
for i in range(-30, 30):
x_test.append(i)
y_test.append(np.polyval(p, i))
plt.plot(x_test, y_test, 'r')
plt.plot(x, y, 'g')
plt.show()
拟合函数如下所示：
红线为拟合的结果，绿线为采集的数据。
拟合参数为：
P = [ 1.34487806e-02 -3.71598336e-02 1.44805392e+01 1.42873418e+03]
舵 机 值 = 0.0134487806 δ 3 − 0.0371598336 δ 2 + 14.4805392 δ + 1428.73418 舵机值=0.0134487806\delta^3-0.0371598336\delta^2+14.4805392\delta+1428.73418 舵机值=0.0134487806δ3−0.0371598336δ2+14.4805392δ+1428.73418
经过标定后，舵机能够准确的控制转角，最终所得的轮式里程计信息也较准确，航向角在转一圈后误差为5度左右。
结论
轮式里程计利用观测的左右轮转速值得到的速度与角速度，代入移动机器人运动学模型中，推演出机器人当前的位置与航向角信息。由于，测量误差、执行机构误差、模型误差以及可能发生的侧滑等原因，轮式里程计信息会有一定的误差。由于轮式里程计是利用几何模型不断推演出来的，误差会一直累积在结果里，最终会不可用。具有相同问题的还有IMU，也即惯性测量单元，同样的，它也是采用几何模型推演得到移动机器人的里程计信息。单独的轮式里程计，由于没有定时校准，误差一直累积，最终无法满足应用，实际应用中，轮式里程计与IMU、GPS、视觉里程计或激光里程计等信息融合得到较准确的定位信息。