移動機器人里程計校準的方法

里程計系統誤差的三個主要來源爲“左右輪實際直徑與標稱直徑的偏差”，“左右輪實際間距與標稱間距的偏差”和“兩輪子直徑的實際平均值與標稱平均值不相等”。“左右輪實際直徑與標稱直徑的偏差”會導致直線運動的距離誤差。“左右輪實際間距與標稱間距的偏差”會導致旋轉運動的方向誤差。“兩輪子直徑的實際平均值與標稱平均值不相等”將同時影響直線運動和旋轉運動。

這裏僅記錄一下我瞭解到的里程計標定方法。

方法一：

ROS機器人Diego 1#製作（四）base controller—線速度的標定
ROS機器人Diego 1#製作（五）base controller—角速度的標定
上面兩篇博文描述了一種簡單的標定輪子直徑和輪間距的方法。這裏假設左右兩輪的直徑是一樣的。通過控制機器人直行一米，對比機器人實際走的距離與標稱一米的偏差。然後通過修改輪子直徑來儘量縮小偏差。控制機器人旋轉，不斷獲取里程計計算出的角度。當角度達到360度時就停止機器人。對比實際轉過的角度與標稱360度的偏差，然後修改輪子間距來儘量縮小偏差。

方法二：

基於線性最小二乘對里程計進行標定。該方法不直接標定輪子的間距和輪子直徑。因爲激光數據比碼盤計算的里程要準。該方法使用ros中的csm包處理激光數據得到機器人在$\triangle t$時間間隔內機器人的位姿變化$\left(\begin{array}{c}{u_{i x}^{*}} \\ {u_{i y}^{*}} \\ {u_{i \theta}^{*}}\end{array}\right)$。將該狀態量用列向量$u_{i}^{*}$表示，即$u_{i}^{*}=\left(\begin{array}{c}{u_{i x}^{*}} \\ {u_{i y}^{*}} \\ {u_{i \theta}^{*}}\end{array}\right)$。csm包可用以下命令獲取

sudo apt-get install ros-kinetic-csm 

與激光數據對齊時間戳的情況下，我們同樣能得到同一時間段內里程計計算出來的位姿變化。這裏用$u_{i}=\left(\begin{array}{c}{u_{i x}} \\ {u_{i y}} \\ {u_{i \theta}}\end{array}\right)$表示。我們可以得到線性轉換方程：$u_{i}^{*}=X * u_{i}$
令矩陣$X=\left[\begin{array}{lll}{x_{11}} & {x_{12}} & {x_{13}} \\ {x_{21}} & {x_{22}} & {x_{23}} \\ {x_{31}} & {x_{32}} & {x_{33}}\end{array}\right]$
則針對每一組數據可以得到下列方程組：
$u_{i x} * x_{11}+u_{i y} * x_{12}+u_{i \theta} * x_{13}=u_{i x}^{*}$
$u_{i x} * x_{21}+u_{i y} * x_{22}+u_{i \theta} * x_{23}=u_{i y}^{*}$
$u_{i x} * x_{31}+u_{i y} * x_{32}+u_{i \theta} * x_{33}=u_{i \theta}^{*}$

只要求出矩陣$X$，我們就可以將每次的里程計算出的位姿增量轉換程具有激光數據精度的里程計增量。將上面的方程組寫成如下的矩陣形式：
$\left[\begin{array}{ccccccccc}{u_{i x}} & {u_{i y}} & {u_{i \theta}} & {0} & {0} & {0} & {0} & {0} & {0} \\ {0} & {0} & {0} & {u_{i x}} & {u_{i y}} & {u_{i \theta}} & {0} & {0} & {0} \\ {0} & {0} & {0} & {0} & {0} & {0} & {u_{i x}} & {u_{i y}} & {u_{i \theta}}\end{array}\right]\left[\begin{array}{c}{x_{11}} \\ {\vdots} \\ {x_{33}}\end{array}\right]=\left[\begin{array}{c}{u_{i x}^{*}} \\ {u_{i y}^{*}} \\ {u_{i \theta}^{*}}\end{array}\right]$
每一幀數據形成這樣一個方程組，可以用$A_{i} \vec{X}=b_{i}$表示。校準時會接收n幀數據形成一個超定方程組，可以用$A \vec{X}=b$表示。其中$A=\begin{array}{c}{A_{1}} \\ {\vdots} \\ {A_{n}}\end{array}$，$b=\begin{array}{c}{b_{1}} \\ {\vdots} \\ {b_{n}}\end{array}$。由於超定方程組是沒有準確的解的，但可以用線性最小二乘得到一個近似解$\vec{X}$。$\vec{X}$是矩陣$X$在向量空間$A$上的投影。所以它們是最接近的。而且近似解$\vec{X}$有如下通解形式:
$\vec{X}=\left(A^{T} A\right)^{-1} A^{T} b$
使用該方法需要注意以下兩點：
1）里程計數據和激光數據需保持時間戳一致。
2）校準時需要在環境特徵多的地方。如果在長的走廊裏，激光數據計算得到的里程增量是不準的。

方法三：

輪式里程計與2D激光聯合標定及里程計內參校準
Simultaneous Calibration of Odometry and Sensor Parameters for Mobile Robots
論文的源碼實現