移動機器人輪式里程計

移動機器人靈魂三問：我在哪？ 我要去哪裏？ 怎麼去？其中，第一問對應機器人定位問題。定位問題可闡述爲：移動機器人根據自身狀態、傳感器信息實時確定自己在世界（全局或局部）中的位置與姿態。 阿克曼轉向的無人駕駛汽車的定位方案主要有：輪式里程計、視覺里程計、激光里程計、慣性導航模塊（IMU+GPS）以及多傳感器融合。輪式里程計是一種最簡單，獲取成本最低的方法。與其它定位方案一樣，輪式里程計也需要傳感器感知外部信息，只不過，輪式里程計採用的電機轉速測量模塊是一種成本非常低廉的傳感器。本文對搭建智能小車系統過程中構建小車輪式里程計的實踐進行總結，主要涉及輪式里程計的構建原理、方法、結論與不足等方面。

一、輪式里程計原理

如果你擁有一輛馬車，你知道你馬車輪子的周長，並且你安裝了一種裝置可以統計車輪轉了多少圏數。你從A地一到達B地，便可以計算出A到B的路程，換句話說：你知道自己行走了多遠的路程。

具體地，我對於左右兩輪分別安裝一個轉速測量單元，我可以實時獲取馬車當前左右兩輪的轉速$a_l, a_r$，單位爲(圈/s)。我們可以測量輪子的周長爲$S$，進一步得左右輪行進的速度$v_l=a_l S, v_r=a_r S$。我們取左右兩輪軸中心那個點的速度作爲馬車本體的速度，可計算爲$v=\frac{v_l+v_r}{2}$。

以上，我們通過給馬車左右兩輪各安裝一個轉速測量裝置，便可實時獲取馬車當前的行進速度，從而計算馬車行進的路程。

對於移動機器人來說，“當前行駛了多少路程”的確是一個有用的信息。假定機器人的初始始位置已知，在一個行駛維度爲一維的環境（例如：單行線公路、單行線導軌），機器人可以根據輪子的圈數實時估計自己在這個一維行駛環境的何處。

但是，絕大部分移動機器人應用都不是一維這樣簡單的環境，而是二維的，甚至是三維的（加上高度）。先說明，單單輪式里程計信息最多能夠應對二維導航環境（x, y）。我們還拿上面那輛馬車來進一步說明，輪式里程計如何用於二維平面的定位。

想象一下：你駕着馬車在廣場上，想往左邊那個出去行進；於是，你向左邊扯了一下馬的繮繩，車子便向左邊轉去…

假定你在馬車左右兩個輪子上都安裝了測量轉圏數的傳感器，你會發現同一時間內，馬車左側的輪子轉的圈數少些（碾壓過的痕跡短一些），右側的輪子轉的圈數大些（碾壓過的痕跡長一些）。也即，左側輪子的轉速慢於右側輪子。反之，你可以根據左側輪子與右側輪子的轉速來獲取馬車的行進方向信息。

進一步想象一下：你的馬被一根繩子限制繞廣場中央的一根大柱子繞圏。先把馬車看作一個質點，則馬車行進的速度(v)、角度變化率（角速度$\omega$）以及馬車離中心柱子的距離($R$)的關係是有數學公式表示：

$v=\omega R$

我們將馬車繞柱走的圖局部放大得到如下圖，

假定馬車左右車輪從黑色處駛到藍色處，我們可以看到左輪與右輪分另壓出了兩道轍，分別稱之爲內轍與外轍。內轍的長度$s_l$比外轍$s_r$要短。我們可以測量馬車左右兩輪的距離，被稱爲後軸距，標記爲$T$。由幾何關係可知，馬車轉的角度$\theta$與所做圓周運動的角度相等。可得：

$\left\{ \begin{array}{cc} s_l=\theta R \\ s_r=\theta(R+T)\end{array}\right.$

等式兩邊除以時間，進一步得：

$\left\{ \begin{array}{cc} v_l=\omega R \\ v_r=\omega(R+T)\end{array}\right.$

以上式子可反推出角速度：

$\omega=\frac{v_r-v_l}{T}$

左右輪的轉速本身可以用來計算線速度，他兩者又可以計算角速度。在一個平面上行駛的地面移動機器人，知道線速度$v$與角速度$\omega$，便可以在線推演出它在二維平面上的位置($x, y$)與姿態（$\varphi$）。

至此，你發現了輪式里程計所有的奧祕。

二、輪式里程計獲取方法

2.1以阿克曼轉向汽車爲例

如果，機器人爲與Turtlebot類似的左右差速驅動，控制是將期望速度與角速度轉換爲左右輪的期望轉速，里程計則是通過實時測量的左右輪轉速信息推演機器人的輪式里程計信息。很直接，沒什麼好講的。與這種簡單的左右差速驅動的機器人不同，現實中的汽車都是阿克曼轉向幾何運動學模型，具體可看之前的博客無人車系統（一）：運動學模型及其線性化。

我們知道，汽車是通過方向盤控制車的轉向，通過剎車油門控制車的行駛速度（準確的說應該是直接控制加速度，間接控制了速度）。後面兩個輪爲主動輪，前面爲從動輪。後軸上的左右輪共用一個發動機驅動。爲了能讓汽車在轉彎，或曲線行駛時，左右輪的速度能相互協調，這時就需要加入差速器用以調整左右輪的轉速差。備註：差速器也用於四驅車中，用於調節前後四個輪子的轉速差。 下圖顯示的就是一個差速器。

因此，一輛在路面上行駛，輪胎沒有打滑的汽車，左右輪的轉速依然可以用來做輪式里程計。

我們利用轉速測量單元獲得轉化後的後軸左右輪行駛速度分別爲$v_l, v_r$，則可得汽車的瞬時速度：

$v=\frac{v_l+v_r}{2} \tag{1}$

同樣可得瞬時的角速度：

$\omega=\frac{v_r-v_l}{T}\tag{2}$

可以利用如下運動學模型實時在線推演汽車的位置與姿態。
$\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}$
其中，第$t$次觀測的速度爲$v_t$，角速度爲$\omega_{t}$。由此，我們構建了普通汽車的輪式里程計信息。

2.2以阿克曼轉向智能小車爲例

普通阿克曼智能小車考慮機構與控制成本問題，一般不會採用複雜的差速器機構。一般的阿克曼智能小車底盤標配是後輪兩個直流電機控制速度，前面一個舵機控制轉向。由於沒有差速器，爲了能夠很好的完成曲線行駛，後面左右兩的轉速一般不是相等的，而是根據轉向角度有一個主動差速。所以，左右兩個電機在負責小車速度的同時，同時還需要配合舵機控制小車的轉向。

由於，左右電機是主動採用PID控制轉速來達到期望速度與期望轉角的，小車實際的航向由舵機與兩個電機一起決定，相互影響。當舵機控制到的期望角度與電機要達到的期望角度不一致時，由於側滑的存在，我們得到的輪式里程計是不準確的。只有當舵機要達到的轉向角與電機達到的轉向角一致時，我們得到的輪式里程計纔是準確的。

2.2.1 正向控制

由於前輪轉角裝置的原因，前面左右兩輪的在同一方向盤控制下，往往形成的轉角不同（如上上圖的$\alpha,\beta$所示）。單獨分析前面左右兩輪的轉角對於分析汽車模型益處不大，於是阿克曼四輪小車的模型進一步簡化爲如下圖所示的兩輪單車模型（虛擬的前後輪都是汽車的中軸線上）。

普通汽車的幾何運動學模型如下（控制輸入爲前輪轉角$\delta$與線速度$v$）：

$\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}$

離散形式爲：

$\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}$

阿克曼小車可以通過PID控制器，允許接收期望的速度作爲控制輸入，但是在方向控制上，是轉向角控制，也即前輪胎的轉向角度作爲輸入的。

由於後軸兩個電機要控制速度，又要同時配合舵機控制航向，進一步可計算出左右輪的速度$v_r, v_l$。假定當前我們要期望的轉向角爲$\delta$，首先舵機控制前軸兩輪合併出的轉向角應該爲$\delta$，然後，後軸兩個電機控制左右兩輪的速度符合該轉向角下的條件：

$\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}$

2.2.2 反向推演

只要正向控制準確了，反射推演出來的里程計信息就比較準確了。推演的過程與普通汽車的推演公式一樣。

我自己搭了一臺阿克曼轉向智能小車，推演的輪式里程計與肉眼觀測值偏差很大。如果直行時，輪式里程計誤差還可以容忍，前進兩米，誤差爲5cm左右。但是一轉向，航向角度偏差非常大。角度一偏差，後面得到的位置根本就不能用。

這主要是因爲，舵機的角度與小車轉向角的關係沒有標定好。例如：我本來想讓小車轉20度，由於轉換關係不準，舵機控制得出的是1790，最終控制的轉角爲30度。這只是打個比方，總之類似。

於是我，重新採集數據，擬合出了舵機控制值與轉向角之間的函數關係。

1. 先採集不同舵機值下的小車轉彎半徑（轉彎半徑可計算得到轉向角度）
   注意：採集的時候，電機一定是釋放狀態，只控制舵機，然後用手推着小車運動。

舵機值	轉彎半徑	轉向角度
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

2. 利用多項式擬合

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\delta^3-0.0371598336\delta^2+14.4805392\delta+1428.73418$

經過標定後，舵機能夠準確的控制轉角，最終所得的輪式里程計信息也較準確，航向角在轉一圈後誤差爲5度左右。

結論

輪式里程計利用觀測的左右輪轉速值得到的速度與角速度，代入移動機器人運動學模型中，推演出機器人當前的位置與航向角信息。由於，測量誤差、執行機構誤差、模型誤差以及可能發生的側滑等原因，輪式里程計信息會有一定的誤差。由於輪式里程計是利用幾何模型不斷推演出來的，誤差會一直累積在結果裏，最終會不可用。具有相同問題的還有IMU，也即慣性測量單元，同樣的，它也是採用幾何模型推演得到移動機器人的里程計信息。單獨的輪式里程計，由於沒有定時校準，誤差一直累積，最終無法滿足應用，實際應用中，輪式里程計與IMU、GPS、視覺里程計或激光里程計等信息融合得到較準確的定位信息。