【SLAM】"同時定位與地圖創建"在無人車領域的應用探索

版權聲明：本文爲博主原創文章，歡迎轉載。轉載請註明出處http://blog.csdn.net/jsgaobiao https://blog.csdn.net/jsgaobiao/article/details/72867166

【SLAM】”同時定位與地圖創建”在無人車領域的應用探索

畢業答辯總算結束了，主要研究的是GraphSLAM進行地圖創建的可行性。其中，理論部分繼承了前一篇博客的內容，實驗部分在室內室外兩個場景下分別進行了嘗試，最終結果還是不錯的。
下面我按照答辯的順序，簡要梳理一下畢設的思路。

研究背景

同時定位與地圖創建也被稱爲SLAM（Simultaneous Localization and Mapping），簡單來說就是機器人在自身位置不確定的情況下，探索靜態的未知環境，並且同時建立地圖和定位的技術。
它的應用平臺也十分廣泛，從家用的掃地機器人到道路上行駛的無人車，都離不開SLAM技術的支持。
那麼我們爲什麼要研究SLAM在無人車上的應用呢？上文也說了，目的是爲了進行高精度的地圖創建。在城市道路上行駛的無人車需要對自身有較爲準確的定位，而GPS會受到高樓、樹木等干擾，一般會有10m左右範圍的誤差，這一誤差對於無人車而言是不可接受的：比如環路主路和輔路常常只隔了一個車道（2m左右）的距離。因此，如果有一個高精度地圖，無人車根據傳感器觀測與地圖進行實時匹配定位，就可以將定位精度提高到車道級別，滿足實際需求。

SLAM過程示意

下面用一副圖片簡單的描述一下SLAM的過程：

 無人車按照紅色箭頭所示的路徑行駛，在這個過程中，它會通過各種傳感器（GPS、慣導、編碼器等）對自身進行定位，但是這個定位肯定是存在誤差的，我們可以認爲這個誤差服從高斯分佈，就用圖中黑色的模糊點雲表示。無人車行駛的過程中，定位的累計誤差會越來越大。同時，外部傳感器可以觀測到環境中的很多參照物，也被稱爲Landmark，觀測車輛和Landmark的相對位置可以爲SLAM提供很多有用的信息。 另外，當無人車繞了一圈回到原點的時候，就產生了一個閉環。在SLAM問題中，無人車此時必須知道自己回到了原點，並且根據之前的觀測校準當前的位姿。閉環成功之後，可以消除累計誤差。在地圖和車輛行駛軌跡沒有真值的時候，閉環檢測是評判SLAM方法好壞的重要標準之一。

SLAM問題定義

SLAM問題可以用這張圖來形象的表示。

 圖中的xt$x_t$ 表示無人車的位姿，m$m$ 表示靜態的環境地圖，它們是SLAM問題中的未知量，而ut$u_t$ 和 zt$z_t$ 則分別是無人車的控制量（速度、轉向等）和觀測量（由外部傳感器獲取），它們是SLAM問題中的已知量。 SLAM問題面臨的主要難點有： - **控制量和觀測量都存在誤差**，想通過帶誤差的已知量去估計精確的地圖和定位 - **製圖和定位兩個部分相互依賴**，類似“雞生蛋和蛋生雞”的關係 - **閉環檢測** 除了這幾個SLAM問題本身的難點以外，在無人車上應用SLAM還面臨着一些特殊的問題： - **動態環境**：無人車行駛的環境中充滿了行人、車輛等動態物體，而SLAM數學模型中定義的地圖是靜態的 - **室外場景的結構性很差**：相比於SLAM通常應用的室內場景，室外比較少有清晰的牆壁、走廊這些特徵 - **室外環境複雜**：這點顯而易見，想要從環境中提取有效的特徵進而用於定位和建圖是一件非常困難的事情

基於圖模型的SLAM方法

SLAM主要有兩種技術路線：濾波器模型SLAM和圖模型SLAM，其中圖模型的也被稱爲GraphSLAM或Graph-Based SLAM。
本課題主要研究的是GraphSLAM，它的特點是先將SLAM問題轉化爲圖模型，再將圖模型的優化轉化爲非線性最小二乘問題。
這個過程中引入了兩種約束，在上一篇博客中也提到過，它們分別是：
運動模型約束

(xt−g(ut,xt−1))TR−1t(xt−g(ut,xt−1))$$(x_t-g(u_t,x_{t-1}){)}^T{R}_t^{-1}(x_t-g(u_t,x_{t-1}))$$

觀測模型約束

(zit−h(xt,mj,i))TQ−1t(zt−h(xt,mj,i))$$(z_t^i-h(x_t,m_j,i){)}^T{Q}_t^{-1}(z_t-h(x_t,m_j,i))$$

對應到剛纔的圖中就是這樣：

 其中紅色的箭頭對應了運動模型約束，它們是由車輛的位移產生的；黑色虛線對應了觀測模型約束，他們是由外部傳感器的觀測所產生的。這些約束構成了無人車位姿節點xt$x_t$ 之間的約束邊，將它們綜合到一起就得到了GraphSLAM的優化目標函數：

JGraphSLAM=xT0Ω0x0+∑t(xt−g(ut,xt−1))TR−1t(xt−g(ut,xt−1))+∑t∑i(zit−h(xt,mj,i))TQ−1t(zt−h(xt,mj,i))$$J_{GraphSLAM}=x_0^T{\mathrm{\Omega }}_0{x}_0+\sum _t(x_t-g(u_t,x_{t-1}){)}^T{R}_t^{-1}(x_t-g(u_t,x_{t-1}))+\sum _t\sum _i(z_t^i-h(x_t,m_j,i){)}^T{Q}_t^{-1}(z_t-h(x_t,m_j,i))$$

得到優化函數後，調用Ceres、g2o、SPA等後端優化庫就可以很方便的進行優化了。

系統框架

本課題的系統框架如下圖所示：

 本研究在理解GraphSLAM算法的基礎上，使用無人車平臺在室內室外場景下進行了實驗，目的是爲了進行高精度地圖的創建。在算法實現方面，GraphSLAM的前端主要參考了開源項目OpenKarto，我將其改爲了離線版本，通過ROS系統進行通信、可視化等；後端優化部分使用了SPA。

室內實驗

室內試驗的平臺使用的是小型的無人車POSS-Mini，大概一臺筆記本電腦的大小。實驗場景在逸夫樓二層的環形走廊。
下圖是建圖的對比：左圖是航位推算法結合激光幀匹配的結果，右圖是GraphSLAM的結果。

 下面是紅圈部分的放大圖：

 將建好的圖模型可視化的效果：其中紅色的點是圖模型中的節點，也就是車輛的位姿；藍色的邊是節點之間的約束，可以看到閉環處，開頭和結尾軌跡的節點之間由於閉環檢測產生的約束邊。 

室外實驗

室外實驗使用了實驗室的Jeep牧馬人作爲無人車平臺，路線是繞着理科樓羣環繞兩週，大概2km左右的距離：

 室外建圖的結果，以及局部對比圖：



總結與展望

本課題在理解了GraphSLAM算法的基礎上，使用無人車進行了實驗和探討，研究了其在室內室外兩種環境下進行高精度地圖創建的可行性。從實驗結果來看，建圖的結果還是不錯的，但是仍然有可以改進的空間。
今後可以嘗試在圖模型中加入GPS約束，提高閉環檢測的魯棒性。
更長期的來看，還可以研究GraphSLAM在長期（Long-Term）動態環境中如何有效地更新和維護的問題。

這次借這個機會簡單的總結一下之前的工作，也希望之後的研究和生活能夠一切順利吧~

最後，可愛的小胖刺鎮樓~