基于高斯牛頓的局部優(yōu)化SLAM系統(tǒng)

北京信息科技大學(xué)自動化學(xué)院，北京 100192

一、前言

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

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

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

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

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

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

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

二、SLAM系統(tǒng)的構(gòu)成

該SLAM系統(tǒng)主要分為柵格地圖的構(gòu)造、激光數(shù)據(jù)匹配求取位姿、更新地圖，最終建立整體地圖。

1、地圖構(gòu)造

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

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

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

2、激光匹配

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

采用的方法是優(yōu)化對齊激光數(shù)據(jù)到現(xiàn)有的地圖中，我們試圖找到激光雷達的位姿ξ=(px,py,φ)T，使以下目標函數(shù)式（4）達到最?。?/p>

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

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

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

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

這個方程通過設(shè)置式（8）關(guān)于Δξ的偏導(dǎo)數(shù)為零來求最小值：

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

這樣我們就能求出將激光數(shù)據(jù)對齊到現(xiàn)有地圖中的估計位姿。

3、地圖的更新

在求解出機器人姿態(tài)以后，我們已經(jīng)能夠根據(jù)現(xiàn)在的姿態(tài)更新地圖了，更新地圖的具體方法為，對于地圖的網(wǎng)格的原始數(shù)據(jù)：

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

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

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

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

其中，x—代表網(wǎng)格的原始數(shù)據(jù)的值；

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

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

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

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

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

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

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

四、實驗

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

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

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

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

五、結(jié)論

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