SLAM系統原理推導

• 1.SLAM 基礎知識
• • 1.1 SLAM 系統概率模型
  • 1.2 里程計運動模型
  • 1.3 激光雷達測距模型
  • 1.4 系統觀測模型
• 2 SLAM實例
• • 2.1 粒子濾波算法原理

1.SLAM 基礎知識

??SLAM 系統框架如圖1 所示。包括兩個主要傳感器：激光雷達與輪式里程計。其中激光雷達用來獲取環境信息，通過激光掃描可以測量周圍 360°環境的障礙物信息。輪式里程計則通過移動底盤的電機編碼器持續獲取機器人的里程信息。

??激光雷達的作用主要有兩個方面：一方面可以為構圖算法提供點云數據。當構圖算法獲取足夠的點云數據后，即可構建出以機器人為中心，以雷達射程為半徑的局部地圖。另一方面可以通過系統觀測模型修正貝葉斯濾波器的預測位姿，提高濾波估計的機器人位姿精度。輪式里程計主要作用則是為 SLAM 系統提供里程信息。通過輪式里程計運動學模型，可以將里程信息轉化為機器人位姿變化信息，送入貝葉斯濾波器初步計算出預測位姿。 預測精度完全依賴于輪式里程計提供的數據精度。而輪式里程計的測量精度對于獲取精確的機器人位姿來講遠遠不夠。因此，需要用到系統觀測模型對預測結果進行修正。經過修正并加權求和得到的機器人位姿即為當前估計的最佳位姿。在已知最佳位姿的條件下，局部地圖即可更新至全局地圖中。全局地圖進一步進行高斯模糊，構建新的觀測模型。至此整個 SLAM系統形成完整的閉環系統。

1.1 SLAM 系統概率模型

??在 SLAM 系統框架中， 采用貝葉斯濾波器為 SLAM 系統的后端優化算法提供概率模型。 貝葉斯濾波的原理是基于已經發生的事件對未發生事件進行估計，求解未發生事件的最可能概率分布。貝葉斯濾波的推導如圖 2 所示。

??在貝葉斯濾波器構建的 SLAM 概率模型基礎上， 發展出了兩大分支： 基于卡爾曼濾波的 SLAM 算法， 主要代表有 EKF-SLAM， UKF-SLAM 等； 以及基于粒子濾波的 SLAM 算法， 主要代表有 RBPF-SLAM， FastSLAM 等。 其中基于卡爾曼濾波的 SLAM 算法要求噪聲必須服從高斯分布且系統非線性不能太大；相反基于粒子濾波的 SLAM 算法適用于非線性較大的系統， 但受限于粒子退化與粒子耗散這一對耦合問題。

1.2 里程計運動模型

??機器人底盤分為兩輪差速底盤和三輪全向底盤。兩輪差速底盤其結構簡單，造價較低，并且建立的模型相對簡單，因此被應用于各種類型的機器人生產中。兩輪差速底盤的運動學模型如圖 3 所示。

該模型將機器人的任意移動視為某一圓心做圓弧運動，其中 d 為兩輪距離的二分之一， r 為底盤中心做圓弧運動的半徑。設左輪速度為 $v_L$，右輪速度為$v_R$, 底盤中心的角速度為 ω, 若已知 $v_L$,$v_R$,可以由式(1)計算出底盤運動中心的角速度 ω 和線速度 v。

??假設機器人在里程計數據更新的兩時刻之間的極短時間 $\Delta t$內作勻速運動，則相鄰兩時刻之間機器人位移由式(2)給出。

式中： dx ,dy ,$d\theta$ 分別為機器人坐標系下沿 x 軸方向位移，沿 y 軸方向位移，以及以 z 軸為旋轉軸的逆時針旋轉角度。式(2)給出的是在機器人坐標系下的位移。機器人坐標系與世界坐標系的關系是由機器人位姿聯系起來的，如圖 2-4 所示。 圖中$\left(x,y,\theta \right)$為機器人在世界坐標系下的位姿表示。

??可以通過機器人位姿 $\left(x,y,\theta \right)$將機器人在機器人坐標系的位移轉換到世界坐標系中。最終計算的機器人運動模型由式(3)給出：

1.3 激光雷達測距模型

??激光雷達的測距模型主要有以下兩種：三角測距與 TOF（ Time of Flight）測距。其中基于三角測距模型的激光雷達實物圖如圖 5 a 所示。圖中左邊小孔為激光發射器，右邊稍大的孔為接受激光的光學攝像頭。 三角測距模型主要原理為三角形的角邊角定理，激光發射器位置與攝像頭光心以及掃描點構成的三角形如圖 5 b 所示。

??圖 5b 中 $O_1$點為激光發射器， $O_2$點為攝像頭光心軸，光心軸旁邊的三角形代表用來捕捉反射光斑的相機模型。激光發射器發射的激光由紅色虛線表示，相機接收的反射激光由藍色虛線表示。 A 點表示激光擊中的障礙物位置,障礙物A 在成像平面上的位置用 $A_1$ 表示。其中線段$O_1$ $O_2$長度已知；由于激光垂直于直線 $O_1$ $O_2$ 發射，$\angle A{O}_1{O}_2$ 為 90°；$\angle O_1{O}_{2}A$ 可以通過成像點 $A_1$ 的位置獲取。在三角形 $O_1{O}_{2}A$中已知兩內角與一條邊長，即可通過正弦定理求出三角形另外一邊 $O_{1}A$的長度，見式(4)。 線段 $O_{1}A$ 即為障礙物 A 到雷達的距離。

單線激光雷達通過電機驅動雷達測距核心進行順時針旋轉，最高可達每秒10Hz。旋轉過程中不斷發射激光束，基于三角測距模型獲取 360°方向的障礙物距離信息，完成激光掃描數據的封裝。

1.4 系統觀測模型

??激光雷達的觀測模型包括光束模型和似然場模型。其中似然場模型的基本原理是對地圖進行高斯模糊（如圖6 所示），利用 SLAM算法輸出的環境地圖， 對于地圖上存在障礙物的位置， 以該點為中心， 以地圖上其他點到該點的歐氏距離為變量來構造高斯分布函數，設 d 表示激光測量終點坐標$\left(x_{zt},y_{zt}\right)$與地圖 m 上最近的障礙物之間的歐式距離，那么激光雷達測量的概率分布可以由一個以 0 為中心，方差為 d 的高斯函數$\epsilon (d)$給出，見式(2-5)：

??依據地圖提供的障礙物信息可以獲取地圖上全部點對應的高斯分布函數值，并將該值作為地圖上的似然評分，評分越高則代表掃描點與障礙物點越接近，其測量結果也就越準確。
??地圖經高斯模糊后， 環境$z_t$的期望值相對于位姿來講都是連續的，相比于光束模型，似然場模型不會因為環境形狀變化不規律導致觀測期望突變，因此似然場模型適合結構化與非結構化的環境，增加了該模型的適用場景。此外，對于似然場每一點的概率都可以預先計算，在實際 SLAM 算法執行過程中計算式僅需要對預先計算的似然場概率分布進行查表即可，計算量可以忽略不計，提高了算法的實時性。

2 SLAM實例

2.1 粒子濾波算法原理

??粒子濾波算法使用貝葉斯估計的思想， 用粒子分布去近似機器人位姿的概率分布，如圖7 所示。
??首先將一定數量的粒子均勻撒在可能分布的空間中，作為粒子濾波的初始狀態$x_0$ 的概率分布。按照貝葉斯估計的流程，可以通過里程計運動模型求得t時刻的機器人位姿 $x_t$ ，作為預測粒子集。 再根據激光雷達觀測模型對預測粒子集中每一個粒子賦予權值， 粒子權值越高， 則代表該粒子與實際位置的誤差越小。最終對所有粒子根據其權值進行加權平均，即可得到濾波后的位姿估計結果。
??SLAM 中主要任務是機器人的自主定位與環境地圖的建立。 其中在已知機器人的位姿信息的條件下計算環境地圖存在閉式解。 因此 SLAM 問題的主要難點在于如何獲取機器人位姿信息。通過粒子濾波得到的機器人位姿$x_{1:t}$，與激光雷達的歷史觀測信息$z_{1:t}$可以直接計算得到環境的柵格地圖，下面主要介紹基于粒子濾波器獲取機器人位姿的算法步驟。
??粒子濾波算法的基本流程如下：

用粒子集表示機器人的位姿信息： $X_t=\left\{\left(x_t^i,w_t^i\right)|i=1,...,n\right\}$， 其中每一個粒子包含兩個參數： 機器人位姿 $x_t^i$ ，粒子權重 $w_t^i$。

將總數為 N 的粒子集隨機且均勻地撒在所有可能的位姿空間內，作為粒子濾波算法的初始狀態 。

已知前一時刻的粒子集狀態 $X_{t?1}=\left\{\left(x_{t?1}^i,w_{t?1}^i\right)|i=1,...,n\right\}$， 每一個粒子通過建議分布 $x_t^i～p(x_t|u_t,x_{t?1}^i)$ 計算當前時刻的位姿信息。 這里建議分布采用里程計運動模型。其中 Δx, Δy, Δθ 為里程計信息 $u_t$；$N_x$, $N_y$, $N_{\theta }$為里程計的測量噪聲，噪聲服從均值為 0 的高斯分布。最后粒子在建議分布進行隨機采樣，見式(6)。

4. 評估每一個粒子的權重：
??機器人實際位姿分布未知，粒子集內每一個位姿都可能是機器人的實際位姿。定義粒子權重$w_t^i$，用來描述粒子的預測分布與實際分布的匹配程度，粒子權重定義見式(7)，權重越大，表明粒子的預測分布實際分布匹配程度越高。

??經貝葉斯公式推導，可以得到權重的計算式，見式(8):

??由于粒子權重與機器人在$x_t$位姿下獲取觀測數據$z_t$的置信度成正比，其系數$\eta$為歸一化因子。因此首先將每個粒子對應的$p(z_t|x_t^i)$ 求出，計算出歸一化因子$\eta$ （見式）即可通過式()求出歸一化后的粒子權重。

計算出機器人位姿估計結果，見式(10)。

6. 將得到的當前時刻粒子集$X_t$ 替換前一時刻粒子集$X_{t?1}$ ，返回步驟（3）進行迭代計算。
7. 通過上述步驟可以迭代計算出機器人在每一時刻的位姿分布情況，并通過式(11)計算出每一時刻下的機器人位姿估計結果。通過此結果可以進一步計算出環境地圖，完成即時定位與地圖構建的任務。

總結

以上是生活随笔為你收集整理的SLAM系统原理推导的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。