SLAM概念學習之隨機SLAM演算法

標籤：儲存   http   分享   初始化   學習   更新   .com   座標   存在   

　　這一節，在熟悉了Featue maps相關概念之後，我們將開始學習基於EKF的特徵圖SLAM演算法。

　　1. 機器人，圖和增強狀態向量

      隨機SLAM演算法一般儲存機器人位姿和圖中的地標在單個狀態向量中，然後通過一個遞迴預測和量測過程來估計狀態參數。其中，預測階段通過增量航跡估計來處理機器人的運動，並增加了機器人位姿不確定性估計。當再次觀測到Map中儲存的特徵後，量測階段，或者叫更新階段開始執行，這個過程可以改善整個的狀態估計。當機器人在運動過程中觀測到新特徵時，便通過一個狀態增強過程將新觀測的特徵添加到狀態向量中。

　　機器人的狀態，即一個相對於參考笛卡爾座標系的自身位姿參數通過均值和共變數來定義：

（1）

（2）

　　圖的共變數矩陣Pm 包含特徵之間的互相關資訊（即非對角項），這些交叉關聯資訊表徵了每個特徵對Map中其他特徵相關資訊的依賴。由於特徵的位置是靜態，當周圍的環境也即Map是剛性的話，特徵之間的交叉關聯將會隨著的特徵的二次觀測增強。圖中的特徵可以表示如下

（3）

（4）

 　　SLAM中的圖通過一個串聯了機器人位姿和特徵圖狀態的增強顯示狀態向量定義，1所示。這是很有必要的，因為一致的SLAM依賴於機器人位姿和圖之間相關性Pvm

（5）

（6）

　　

 

                                                              Fig. 1 增強狀態向量.

　　需要注意的是，狀態的初始化時通常為和.也就是說，還沒有特徵被機器人觀測到，並且初始時刻機器人位於參考座標系的原點。

　　2. 預測階段

      隨機SLAM演算法的過程模型根據航跡推算運動估計來確定機器人相對於先前時刻位姿的運動，圖特徵仍然保持靜止。這個模型對狀態估計的影響存在於狀態向量部分和狀態共變數矩陣中的以及項。而和這些圖特徵相關項目仍保持不變。

　　估計帶來的機器人（這裡以移動小車為例）位姿變化和共變數通常用車輪的編碼器測量和小車對應的運動學模型擷取。本文裡面的位姿改變通過基於雷射測距的航跡推算擷取，該演算法可以通過批處理資料關聯演算法來找出序列雷射掃描之間的相對位姿關係。

 

                                                              Fig. 2 機器人位姿變化示意.

　　因此，預測的增強顯示狀態可以表示為

（7）

　（8）

　　其中，雅各比矩陣和的定義為

　　（9）

　　（10）

　　雅各比矩陣和定義為

        （11）

　　    （12）

 

　　因為雅各比矩陣僅僅影響機器人的共變數矩陣和對應的交叉相關性，方程（8）用下式高效地實現

           （13）

　　3. 更新階段

　　如果已經儲存在圖中的估計特徵被距離方位感應器二次觀測到，此時對應的量測量可以表示為

（14）

 （15）

　　其中，量測量分別表示對應的距離值和相對於觀測機器人的偏轉角度。為量測共變數。感應器擷取的量測值和圖之間的關聯可以通過下面的方程表示

（16）

　　Kalman增益可以得到

（17）（18）

  （19）

　　其中，雅各比矩陣為

（20）

　　對於包含許多特徵的SLAM圖，雅各比矩陣將會含有許多零項，這會使得方程（18）和（19）的計算非常高效。

　　隨後我們可以從更新過程得到一個後驗SLAM估計

（21/22）

 　　實踐表明，如果幾個獨立的量測同時可用，即知道，便有可能擷取更加精確的更新。

 

　　4. 狀態增強

　　當機器人在環境中漫遊時，一旦新特徵被觀測到，就必須儲存到圖中。接下來將給出新觀測到的特徵初始化方法，首先，狀態向量和對應的共變數矩陣通過新的量測的極值增強為

（23）

（24）

　　量測z 通過函數被轉換位全域的笛卡爾座標系特徵位置，對應的變換表示如下

（25）

　　可以通過線性化函數將增強狀態初始化

   （26/27）

　　其中，雅克比矩陣表示為

     （28）

　　與特徵的儲存相比，特徵的刪除非常簡單，只需將特徵對應的元素直接從狀態向量刪除，同時將相關的行和列中的項目從共變數矩陣中刪除。

SLAM概念學習之隨機SLAM演算法