基于高斯牛頓的局部優化SLAM系統

北京信息科技大學自動化學院，北京 100192

一、前言

同時定位與建圖（Simultaneous Localization and Mapping, SLAM[1]）作為一種基礎技術，是指搭載特定傳感器的主體在沒有環境先驗信息的情況下，在運動過程中建立環境的模型，同時估計自己的運動。從最早的軍事用途（核潛艇海底定位就有了SLAM的雛形）到今天，SLAM已經逐步走入人們的視野。由于計算機視覺技術的快速發展，攝像頭和激光雷達成本的降低，SLAM正在AR、機器人、無人機、無人駕駛等領域發揮重要作用。

近些年來，隨著傳感器和相關SLAM算法的發展，SLAM研究取得了長足的進展。研究的熱點根據傳感器的不同大體上可分為基于激光的SLAM，以及基于視覺的SLAM。視覺SLAM有很豐富的信息量，能夠很好地識別環境，但這種方法制出來的地圖不夠精確，而且由于需要處理的數據過多，無法保證算法的實時性。激光SLAM制圖較為準確，且有很高的實時性，很適合室內小環境的SLAM。

激光傳感器抗干擾性好，所得信息準確。相較于其他主動傳感器，激光測距儀在獲得某一幀的數據時，能夠測量更大的角度和更多的數據點，而且測量的距離信息也更加準確。雖然利用激光雷達作為外部傳感器的機器人SLAM成本高昂，但卻是目前為止最穩定、最可靠的SLAM方式。

現如今，SLAM技術已經展示了廣闊的發展空間，而且在各個領域占據著舉足輕重的地位，這使得SLAM問題的研究極具意義。近年來，在SLAM問題上已經有了大量的研究，目前應用最廣的2D SLAM方法是Hector和Gmapping[2]。Rao-Blackwellized 粒子濾波算法（Rao-Blackwellized Particle Filter, RBPF）[3]是目前對于室內同時定位與建圖的典型解決方案。

SLAM系統可以分為前端和后端兩部分。SLAM前端用于估計機器人實時在線運動，最常用的方法是掃描匹配[4-5]方法。而后端根據前端已經生成的位姿圖來進行優化。主流方案就兩種，基于概率學理論的貝葉斯濾波器（EKF[6-7], PF）以及基于優化的方法。

隨著移動機器人研究的興起，掃描匹配方法被迅速地應用于機器人定位和環境建圖之中。其中，迭代最近點（ICP）[8-9]方法是實現掃描匹配中最常用的一種方法。ICP的主要缺點是需要反復尋找對應點，在應用中收斂速度慢，計算成本很大。

本文采用Sobel算子來計算柵格地圖的梯度，再對其進行雙線性插值獲得地圖上任意點的梯度，通過高斯-牛頓方法來尋找局部最優值，充分利用激光雷達的高更新率，采用較低的計算成本的方法，可以實現可靠精度的建圖和定位的功能。

二、SLAM系統的構成

該SLAM系統主要分為柵格地圖的構造、激光數據匹配求取位姿、更新地圖，最終建立整體地圖。

1、地圖構造

柵格地圖作為一種激光雷達在實際環境中進行定位的已證實方法，可以呈現任意環境。但柵格地圖的離散屬性限制了地圖精度，也不允許直接計算插值或梯度值。這里采用一種雙線性過濾的方案來估計柵格概率值和梯度值，這樣一來柵格地圖單元格值就可以被視為一個連續概率分布。

給定一個連續地圖坐標Pm，其柵格概率值和梯度值可以近似通過距離Pm周圍最近的4個整數坐標P00，P10，P01，P11來計算，Pm地圖概率值M(Pm)具體實現按式（1）進行插值[10]：

這樣我們已經獲得的近似地圖的表達式，對其求導就可以進一步求取地圖的梯度，具體的求梯度的方法是對式（1）分別求偏導，如式（2）和式（3）：

2、激光匹配

掃描匹配是將激光掃描數據與現有的地圖進行對齊的過程。現在的激光雷達具備測量噪聲低和掃描頻率高的特性，所獲取的激光數據比較準確，因此掃描匹配方法可以產生一個很準確的結果。

采用的方法是優化對齊激光數據到現有的地圖中，我們試圖找到激光雷達的位姿ξ=(px,py,φ)T，使以下目標函數式（4）達到最小：

其中，M(Si(ξ))—表示Si(ξ)坐標下為障礙物的概率，1表示100%障礙物，0表示100%空閑區域。

我們想要找到最好的位姿轉換來把激光掃描數據校準到現有的地圖中。其中根據給定機器人的位姿，激光點束的端點在世界坐標下的坐標計算如式（5）所示：

給定一個初值ξ，我們想通過估計Δξ來優化以下測量誤差，使得式（6）趨于零：

通過泰勒展開M(Si(ξ+Δξ))得到式（7）：

這個方程通過設置式（8）關于Δξ的偏導數為零來求最小值：

通過解出Δξ來實現高斯-牛頓最小化問題：

這樣我們就能求出將激光數據對齊到現有地圖中的估計位姿。

3、地圖的更新

在求解出機器人姿態以后，我們已經能夠根據現在的姿態更新地圖了，更新地圖的具體方法為，對于地圖的網格的原始數據：

激光束的頂點（hit），存在障礙物的可能性變高，所以增加一個權重；

激光束上的其他點（miss），存在障礙物的可能性變小，所以減少一個權重；

激光束的始點（original point），存在障礙物的可能性變小，所以減少一個權重。

根據以下公式（12）可以求出障礙物存在的概率：

其中，x—代表網格的原始數據的值；

M—代表障礙物存在的概率。

三、改進獲取占據柵格地圖梯度的近似方法

原來獲取占據柵格地圖概率值和地圖梯度的近似方法是先通過雙線性插值得到柵格地圖的概率值，然后對其求導獲取地圖梯度。柵格地圖的概率值本身就是線性插值近似的，再對其求導得到的地圖梯度的誤差就會更大。于是，參照獲取地圖概率值的方式，通過圖像獲取梯度的方法先獲取離散的梯度值矩陣，再對其進行插值計算。

這里采用Sobel算子來求取柵格地圖的梯度，在x，y兩個方向上的Sobel算子模板為：

通過對柵格地圖M的二維矩陣用Sobel算子作為相關核進行濾波，可以獲取柵格地圖梯度的離散值，gx和gy分別為x，y兩個方向上的地圖梯度矩陣。然后和獲取地圖概率值一樣，再對柵格地圖梯度值進行插值：

原有方法對柵格地圖概率值M直接求導獲取地圖梯度。改進獲取地圖梯度的方法后，利用Sobel算子作為相關核和柵格地圖概率值M進行相關運算，求出地圖梯度的離散矩陣gx和gy，再對其進行雙線性插值。采用原有方法和改進方法分別對模擬實驗數據進行建圖測試，效果圖見圖1和圖2。

通過對照圖1和圖2的效果，可以明顯看出改進后的建圖精度得到了提升。

四、實驗

為了進一步驗證改進方法，在實驗室環境下利用機器人采集真實激光雷達掃描數據，進行建圖實驗。

依照上述SLAM系統的框架邏輯，首先建立采用一種可通過雙線性過濾的方案來估計柵格概率值和梯度值的近似連續分布的柵格地圖，然后根據高斯-牛頓優化方法來尋求能獲得當前觀測值的柵格地圖上的最優位姿，從而確定兩幀數據的位姿關系，最后把當前的觀測數據更新到柵格地圖里，為下一次激光數據的匹配做準備，實現了同時定位與建圖的功能。

為了測試編寫算法的效果，在實驗室搭建環境，機器人采集數據，進行建圖效果的測試。機器人的激光雷達采用的是HOKUYO公司的UTM-30LX激光掃描測距儀，該產品擁有30m，270°測量范圍，DC12V輸入，采用ToF（Time of Flight）激光時間飛行原理進行距離測量。

在實驗室搭建如圖3所示環境，利用手柄控制機器人的運動，使機器人的激光雷達能夠逐漸掃描到整個周圍環境，尋求把每一幀新的掃描數據對齊到地圖中的最優位姿，根據位姿更新現有地圖。當機器人的激光雷達獲取了環境中的所有數據信息，就可以完整地構建出室內環境的二維地圖。建圖效果如圖4所示。

五、結論

本文充分利用激光雷達的高更新率，采用較低的計算成本的SLAM方法。改進了獲取柵格地圖梯度的方法，利用Sobel算子和柵格地圖概率值矩陣進行互相關運算，求出地圖梯度的離散矩陣，再對其進行雙線性插值，獲得地圖上任意點的梯度。通過高斯-牛頓方法來尋求每一幀新的觀測數據對齊到現有地圖的最優位姿，確定了機器人的位姿就可以根據位姿把觀測數據更新到地圖中，從而建立環境地圖。