[PR-3]ArUco EKF SLAM 擴充套件卡爾曼SLAM

阿新 • • 發佈：2018-12-13

原文有視訊

這篇文章實現了《概率機器人》第10章中提到的EKF-SLAM演算法，更確切的說是實現了已知一致性的EKF-SLAM演算法。

ArUco EKF SLAM

EKF-SLAM一般是基於路標的SLAM系統。本文使用了一種人工路標——ArUco碼。每個ArUco碼有一個獨立的ID，通過PnP方法還可以計算出碼和相機之間的相對位姿。OpenCV中集成了ArUco碼庫，提供了檢測和位姿估計的功能。大家可以參考：

Aruco Marker

首先在房間的地面上貼若干ArUco碼作為路標，然後遙控一個帶有攝像頭+編碼器的機器人在房間內運動。本文的目標就是通過EKF演算法同時估計出這些碼的位置和機器人的位姿。

實驗環境

要實現EKF-SLAM，最關鍵的就是建立運動模型和觀測模型，將這兩個模型直接帶進EKF演算法框架就是EKF-SLAM。EKF-SLAM演算法使用擴充套件的狀態空間： ${\bf{X}} = {\left[ {\matrix{ x & y & \theta & {{m_{x,1}}} & {{m_{y,1}}} & {{m_{x,2}}} & {{m_{y,2}}} & \cdots & {{m_{x,N}}} & {{m_{y,N}}} \cr } } \right]^T} \\$

前3項是機器人位姿，後2N項是 N個路標點的位置。

1. 運動模型

1.1 里程計模型

我比較喜歡採用《自主移動機器人導論》中的里程計模型作為運動模型。具體的，如果t-1時刻機器人的位姿是 ${\xi _{t{\rm{ - }}1}} = \left[ {\matrix{ x & y & \theta \cr } } \right]_{t{\rm{ - }}1}^T$ ，那麼t時刻的機器人位姿為： $\eqalign{ & {\left[ {\matrix{ x \cr y \cr \theta \cr } } \right]_t}{\rm{ = }}{\left[ {\matrix{ x \cr y \cr \theta \cr } } \right]_{t{\rm{ - }}1}} + \left[ {\matrix{ {\Delta s\cos (\theta + \Delta \theta /2)} \cr {\Delta s\sin (\theta + \Delta \theta /2)} \cr {\Delta \theta } \cr } } \right],\; \cr & \left\{ \matrix{ \Delta \theta = {{\Delta {s_r} - \Delta {s_l}} \over b}\; \hfill \cr \Delta s = {{\Delta {s_r}{\rm{ + }}\Delta {s_l}} \over 2} \hfill \cr} \right.,\Delta {s_{l/r}} = {k_{l/r}} \cdot \Delta {e_{l/r}} \cr & \Delta {s_{l/r}} \sim N\left( {\matrix{ {{{\widehat {\Delta s}}_{l/r}},} & {{{\left\| {k{{\widehat {\Delta s}}_{l/r}}} \right\|}^2}} \cr } } \right). \cr} \ \ (1)\\$ ${k_{l/r}}$ 為左右輪系數，把編碼器增量 $\Delta {e_{l/r}}$ 轉化為左右輪的位移， $b$ 是輪間距。左右輪位移的增量 $\Delta {s_{l/r}}$ 服從高斯分佈，均值就是編碼器計算出的位移增量，標準差與增量大小成正比。如果t-1時刻機器人位姿的協方差為 ${{\bf{\Sigma }}_{\xi ,t{\rm{ - }}1}}$

，控制的協方差也就是左右輪位移增量的協方差為 ${{\bf{\Sigma }}_u}$ ，那麼機器人位姿在t時刻的協方差就是： ${{\bf{\Sigma }}_{\xi ,t}} = {{\bf{G}}_\xi }{{\bf{\Sigma }}_{\xi ,t{\rm{ - }}1}}{{\bf{G}}_\xi }^T + {{\bf{G'}}_u}{{\bf{\Sigma }}_u}{{\bf{G'}}_u}^T \ \ (2) \\$ ${{\bf{G}}_\xi }$ 是(1)式關於機器人位姿 ${\xi _{t{\rm{ - }}1}}$ 的雅克比：

${{\bf{G}}_\xi } = {{\partial {\xi _t}} \over {\partial {\xi _{t{\rm{ - }}1}}}} = \left[ {\matrix{ 1 & 0 & { - \Delta s\sin (\theta + \Delta \theta /2)} \cr 0 & 1 & {\Delta s\cos (\theta + \Delta \theta /2)} \cr 0 & 0 & 1 \cr } } \right] \ \ (3) \\$

${{\bf{G'}}_u}$ 是(1)式關於控制） ${\bf{u}} = {\left[ {\matrix{ {\Delta {s_r}} & {\Delta {s_l}} \cr } } \right]^T}$ 的雅克比：

${{\bf{G'}}_u} = {{\partial {\xi _t}} \over {\partial {\bf{u}}}}{\rm{ = }}\left[ {\matrix{ {{1 \over 2}\cos \left( {\theta {\rm{ + }}{{\Delta \theta } \over 2}} \right) - {{\Delta s} \over {2b}}\sin \left( {\theta + {{\Delta \theta } \over 2}} \right)} & {{1 \over 2}\cos \left( {\theta {\rm{ + }}{{\Delta \theta } \over 2}} \right) + {{\Delta s} \over {2b}}\sin \left( {\theta + {{\Delta \theta } \over 2}} \right)} \cr {{1 \over 2}\sin \left( {\theta {\rm{ + }}{{\Delta \theta } \over 2}} \right) + {{\Delta s} \over {2b}}\cos \left( {\theta + {{\Delta \theta } \over 2}} \right)} & {{1 \over 2}\sin \left( {\theta {\rm{ + }}{{\Delta \theta } \over 2}} \right) - {{\Delta s} \over {2b}}\cos \left( {\theta + {{\Delta \theta } \over 2}} \right)} \cr {{1 \over b}} & { - {1 \over b}} \cr } } \right] \ \ (4) \\$

1.2 EKF-SLAM運動更新

上面說的還是隻考慮機器人位姿的情況，但是SLAM系統還需要考慮路標點。擴充套件路標點之後，運動方程為：

$\underbrace {{{\left[ {\matrix{ x \cr y \cr \theta \cr {{m_{x,1}}} \cr {{m_{y,1}}} \cr \vdots \cr {{m_{x,N}}} \cr {{m_{y,N}}} \cr } } \right]}_t}}_{{{\bf{X}}_t}} = \underbrace {\underbrace {{{\left[ {\matrix{ x \cr y \cr \theta \cr {{m_{x,1}}} \cr {{m_{y,1}}} \cr \vdots \cr {{m_{x,N}}} \cr {{m_{y,N}}} \cr } } \right]}_{t{\rm{ - }}1}}}_{{{\bf{X}}_{t{\rm{ - }}1}}} + \underbrace {\left[ {\matrix{ 1 & 0 & 0 \cr 0 & 1 & 0 \cr 0 & 0 & 1 \cr 0 & 0 & 0 \cr 0 & 0 & 0 \cr \vdots & \vdots & \vdots \cr 0 & 0 & 0 \cr 0 & 0 & 0 \cr } } \right]}_{\bf{F}}\left[ {\matrix{ {\Delta s\cos (\theta + \Delta \theta /2)} \cr {\Delta s\sin (\theta + \Delta \theta /2)} \cr {\Delta \theta } \cr } } \right]}_{g\left( {{{\bf{X}}_{t{\rm{ - }}1}},{{\bf{u}}_t}} \right)} \ \ (5) \\$ 系統狀態的均值 ${{\bf{\bar u}}_t}$ 更新利用(5)式，下面看狀態的方差 ${\overline {\bf{\Sigma }} _t}$ 更新。

${\overline {\bf{\Sigma }} _t}{\rm{ = }}{{\bf{G}}_t}{{\bf{\Sigma }}_{t{\rm{ - }}1}}{{\bf{G}}_t}^T + {{\bf{G}}_u}{{\bf{\Sigma }}_u}{{\bf{G}}_u}^T \ \ (6)\\$

${{\bf{G}}_t}$ 是 $g\left( {{{\bf{X}}_{t{\rm{ - }}1}},{{\bf{u}}_t}} \right)$ 關於 ${{\bf{X}}_{t{\rm{ - }}1}}$ 的雅克比：

${{\bf{G}}_t} = \left[ {\matrix{ {{{\bf{G}}_\xi }} & {\bf{0}} \cr {\bf{0}} & {\bf{I}} \cr } } \right] \ \ (7)\\$

${{\bf{G}}_u}$ 是 $g\left( {{{\bf{X}}_{t{\rm{ - }}1}},{{\bf{u}}_t}} \right)$ 關於 ${{\bf{u}}_t}$ 的雅克比：

${{\bf{G}}_u} = {\bf{F}}\;{{\bf{G'}}_u}\ \ (8) \\$

把(6)式展開看一下：

$\eqalign{ {\overline {\bf{\Sigma }} _t} & {\rm{ = }}\left[ {\matrix{ {{{\bf{G}}_\xi }} & {\bf{0}} \cr {\bf{0}} & {\bf{I}} \cr } } \right]{{\bf{\Sigma }}_t}\left[ {\matrix{ {{{\bf{G}}_\xi }^T} & {\bf{0}} \cr {\bf{0}} & {\bf{I}} \cr } } \right] + {\bf{F}}{{{\bf{G'}}}_u}{{\bf{\Sigma }}_u}{{{\bf{G'}}}_u}^T{{\bf{F}}^T} \cr &= \left[ {\matrix{ {{{\bf{G}}_\xi }{{\bf{\Sigma }}_{xx}}{{\bf{G}}_\xi }^T} & {{{\bf{G}}_\xi }{{\bf{\Sigma }}_{xm}}} \cr {{{\left( {{{\bf{G}}_\xi }{{\bf{\Sigma }}_{xm}}} \right)}^T}} & {{{\bf{\Sigma }}_{mm}}} \cr } } \right] + {\bf{F}}{{{\bf{G'}}}_u}{{\bf{\Sigma }}_u}{{{\bf{G'}}}_u}^T{{\bf{F}}^T} \cr} \ \ (9) \\$ 可以看出，運動更新同時影響了機器人位姿的協方差，以及位姿與地圖之間的協方差。

2. 測量模型

首先解決測量值的問題。雖然可以獲得ArUco碼相對於機器人的6自由度位姿資訊，但是為了與書上的觀測統一，本文還是把相機作為Range-bearing感測器使用，也就是轉換成距離 $r$

和角度 $\phi$ 。1個ArUco碼作為一個路標點 ${\bf{m}}$ ，座標為 ${\left[ {\matrix{ {{m_x}} & {{m_y}} \cr } } \right]^T}$ 。

先說一下如何轉化成距離和角度。下圖是示意圖，碼與相機的相對位姿為 ${}_m^c{\bf{T}}$ ，相機與機器人的相對位姿為 ${}_c^r{\bf{T}}$ ，那麼碼相對於機器人的位姿為 ${}_m^r{\bf{T}} = {}_c^r{\bf{T}}{}_m^c{\bf{T}}$ 。 ${}_m^r{\bf{T}}$ 的平移項 $x$ 和 $y$ 就是碼的原點在機器人座標系下的座標。轉化成距離資訊就是 $r = \sqrt {{x^2} + {y^2}}$ ，角度就是 $\phi {\rm{ = atan2}}\left( {y,x} \right)$ 。這樣就得到了測量值 $z = {\left[ {\matrix{ r & \phi \cr } } \right]^T}$ 。這裡再做一個近似假設，認為觀測的方差與距離和角度成線性關係：

${\bf{Q}} = \left[ {\matrix{ {{{\left\| {{k_r}r} \right\|}^2}} & {} \cr {} & {{{\left\| {{k_\phi }\phi } \right\|}^2}} \cr } } \right] \ \ (10)\\$

第 $i$ 個路標點的觀測模型為：

${\bf{z}}_t^i{\rm{ = }}h\left( {{{\bf{X}}_t}} \right){\rm{ + }}{\bf{\delta }}_t^i,\;{{\bf{\delta }}_t} \sim {\cal N}\left( {{\bf{0}},{{\bf{Q}}_t}} \right) \ \ (11)\\$

展開來看：

$\left\{ \matrix{ r_t^i = \sqrt {{{\left( {{m_{x,i}} - x} \right)}^2} + {{\left( {{m_{y,i}} - y} \right)}^2}} \hfill \cr \phi _t^i = {\rm{atan2}}\left( {{m_{y,i}} - y, \ {m_{x,i}} - x} \right) - \theta \hfill \cr} \right.\ \ (12)\\$

根據擴充套件卡爾曼濾波，需要求解觀測 ${\bf{z}}_t^i$ 相對於 ${{\bf{X}}_t}$ 的雅克比 ${\bf{H}}_t^i$ ，實際上一個路標點觀測只涉及到機器人的位姿和這個路標點的座標，組合在一起就是五個量： $\nu = \left[ {\matrix{ x & y & \theta & {{m_{x,i}}} & {{m_{y,i}}} \cr } } \right]$ 。於是，觀測 ${\bf{z}}_t^i$ 相對於 $\nu$ 的雅克比是：

${{\bf{H}}_\nu } = {{\partial h} \over {\partial \nu }} = {1 \over q}\left[ {\matrix{ { - \sqrt q {\delta _x}} & { - \sqrt q {\delta _y}} & 0 & {\sqrt q {\delta _x}} & {\sqrt q {\delta _y}} \cr {{\delta _y}} & { - {\delta _x}} & { - q} & { - {\delta _y}} & {{\delta _x}} \cr } } \right]\left( {\left\{ \matrix{ {\delta _x} = {m_{x,i}} - x \hfill \cr {\delta _y} = {m_{y,i}} - y \hfill \cr} \right.q = {\delta _x}^2 + {\delta _y}^2} \right) \ \ (13)\\$

由於實際的狀態空間是3+2N維的，要求的觀測雅克比應該是2x(3+2N)維的。對(13)進行轉換得到觀測 ${\bf{z}}_t^i$ 相對於全狀態空間 ${{\bf{X}}_t}$ 的雅克比：

${\bf{H}}_t^i{\rm{ = }}{{\bf{H}}_\nu }{{\bf{F}}_i}{\rm{ = }}{1 \over q}\left[ {\matrix{ { - \sqrt q {\delta _x}} & { - \sqrt q {\delta _y}} & 0 & {\sqrt q {\delta _x}} & {\sqrt q {\delta _y}} \cr {{\delta _y}} & { - {\delta _x}} & { - q} & { - {\delta _y}} & {{\delta _x}} \cr } } \right]\left[ {\matrix{ 1 & 0 & 0 & {0 \cdots 0} & 0 & 0 & {0 \cdots 0} \cr 0 & 1 & 0 & {0 \cdots 0} & 0 & 0 & {0 \cdots 0} \cr 0 & 0 & 1 & {0 \cdots 0} & 0 & 0 & {0 \cdots 0} \cr 0 & 0 & 0 & {0 \cdots 0} & 1 & 0 & {0 \cdots 0} \cr 0 & 0 & 0 & {\underbrace {0 \cdots 0}_{2i - 2}} & 0 & 1 & {\underbrace {0 \cdots 0}_{2N - 2i}} \cr } } \right] \ \ (14)\\$

下面就可以按照EKF的框架進行操作了。

$\eqalign{ & {\bf{K}}_t^i = {\overline {\bf{\Sigma }} _t}{\bf{H}}{_t^i}^T{\left( {{\bf{H}}_t^i{{\overline {\bf{\Sigma }} }_t}{\bf{H}}{{_t^i}^T}{\rm{ + }}{{\bf{Q}}_t}} \right)^{ - 1}} \cr & {{\bf{\mu }}_t}{\bf{ = }}{{{\bf{\bar \mu }}}_t} + {\bf{K}}_t^i\left( {{\bf{z}}_t^i - {\bf{\hat z}}_t^i} \right) \cr & {{\bf{\Sigma }}_t} = \left( {{\bf{I}} - {\bf{K}}_t^i{\bf{H}}_t^i} \right){\overline {\bf{\Sigma }} _t} \cr} \ \ (15) \\$

其中， ${\bf{\hat z}}_t^i{\rm{ = }}\left[ \matrix{ \sqrt {{{\left( {{{\bar m}_{x,i}} - \bar x} \right)}^2} + {{\left( {{{\bar m}_{y,i}} - \bar y} \right)}^2}} \hfill \cr {\rm{atan2}}\left( {{{\bar m}_{y,i}} - \bar y,\ {{\bar m}_{x,i}} - \bar x} \right) - \bar \theta \hfill \cr} \right] \ \ (16)\\$

就是由路標點和機器人位姿的均值獲取。對每個觀測到的路標點進行上述操作就完成了觀測更新。

3. 地圖構建

上文所說的操作都是假設路標點的數量是已知的，這個值也可以認為是不知道的，可以邊執行邊加入路標點：當看到一個新的地圖點時就擴充套件狀態空間和協方差。當觀測到一個新的路標點，其觀測為 $z{\rm{ = }}{\left[ {\matrix{ r & \phi \cr } } \right]^T}$ ，根據機器人的位姿可以計算地圖點的座標為： $\left[ {\matrix{ {{m_x}} \cr {{m_y}} \cr } } \right] = \left[ {\matrix{ {\cos (\theta )} & { - \sin (\theta )} \cr {\sin (\theta )} & {\cos (\theta )} \cr } } \right]\left[ {\matrix{ {r\cos \left( \phi \right)} \cr {r\sin \left( \phi \right)} \cr } } \right] + \left[ {\matrix{ x \cr y \cr } } \right] = r\left[ {\matrix{ {\cos \left( {\theta + \phi } \right)} \cr {\sin \left( {\theta + \phi } \right)} \cr } } \right]{\rm{ + }}\left[ {\matrix{ x \cr y \cr } } \right] \ \ (17) \\$ 地圖點的協方差為： ${{\bf{\Sigma }}_m} = {{\bf{G}}_p}{{\bf{\Sigma }}_\xi }{\bf{G}}_p^T + {{\bf{G}}_z}{\bf{QG}}_z^T \ \ (18)\\$ ${{\bf{G}}_p}$ 是(17)式關於機器人位姿的雅克比： ${G_p} = \left[ {\matrix{ 1 & 1 & { - r\sin \left( {\theta + \phi } \right)} \cr 1 & 1 & {r\cos \left( {\theta + \phi } \right)} \cr } } \right] \ \ (19) \\$ ${{\bf{G}}_z}$ 是(17)式關於觀測 $z$ 的雅克比： ${G_z} = \left[ {\matrix{ {\cos \left( {\theta + \phi } \right)} & { - r\sin \left( {\theta + \phi } \right)} \cr {\sin \left( {\theta + \phi } \right)} & {r\cos \left( {\theta + \phi } \right)} \cr } } \right] \ \ (20) \\$ 通過以上各式，算出新路標的均值和協方差，加入到均值向量和協方差矩陣中即可。至此，EKF演算法中所有的模型都已建立完畢。下面給出具體的實施程式碼。

4. 演算法實現

全部工程程式碼：

我利用Falconbot機器人，採集了兩組實驗資料，大家可以在這裡下載：

5. 參考文獻

《概率機器人》

《自主移動機器人導論》

Freiburg SLAM Course:

[PR-3]ArUco EKF SLAM 擴充套件卡爾曼SLAM

1. 運動模型

2. 測量模型

3. 地圖構建

4. 演算法實現

5. 參考文獻

[PR-3]ArUco EKF SLAM 擴充套件卡爾曼SLAM

三輪全向輪底盤SLAM挖坑系列-ekf擴充套件卡爾曼濾波

擴充套件卡爾曼濾波EKF與多感測器融合

演算法解析：擴充套件卡爾曼濾波EKF與KF本質分析

擴充套件卡爾曼濾波（EKF）

初學者的卡爾曼濾波——擴充套件卡爾曼濾波

卡爾曼濾波原理(二)：擴充套件卡爾曼

無人駕駛四擴充套件卡爾曼濾波在目標車輛運動狀態識別中的運用（python程式）

無人駕駛汽車系統入門（二）——高階運動模型和擴充套件卡爾曼濾波

卡爾曼濾波器、擴充套件卡爾曼濾波器、無向卡爾曼濾波器的詳細推導

無人駕駛工程師第二期——P1擴充套件卡爾曼濾波器

初學者的卡爾曼濾波——擴充套件卡爾曼濾波（一）

慣性導航——擴充套件卡爾曼濾波（一）

卡爾曼濾波3

【概率機器人】3.1 卡爾曼濾波、擴展卡爾曼濾波和無跡卡爾曼濾波

無損卡爾曼濾波UKF(3)-預測-生成Sigma點

數據會有一些波動和噪點采用卡爾曼濾波算法進行

卡爾曼濾波實現多項式擬合Matlab

卡爾曼濾波的一個簡單demo：恒定加速度模型

卡爾曼濾波2

[PR-3]ArUco EKF SLAM 擴充套件卡爾曼SLAM

1. 運動模型

2. 測量模型

3. 地圖構建

4. 演算法實現

5. 參考文獻

相關推薦