基于圖優化的二維激光SLAM 研究

翁瀟文，李 迪，柳俊城

（華南理工大學 機械與汽車工程學院，廣州510640）

生產環境的平面圖是移動機器人進行自主導航的依據，通常由人工進行測量，然后利用計算機輔助設計CAD（computer aided design）整理而成。人工測量通常會將障礙物邊緣視為直線進行計算，存在一定的誤差，還有耗費時間長，測量難度大，后續處理困難等缺點。 移動機器人運動過程中的同時定位與地圖構建SLAM 算法的提出正是為了解決這一問題，為移動機器人自主導航奠定基礎。 SLAM問題可以描述為：在未知環境中移動機器人從隨機起點開始運動，根據運動過程中得到的傳感器數據預估推測機器人本體的位姿， 構建出環境地圖，同時標示出機器人在地圖中的位姿[1]。

1 SLAM 及其問題

1.1 SLAM 分類

目前，主流的同時定位與地圖構建SLAM 算法可根據主要傳感器的不同而分為2 種，一種是基于激光傳感器的SLAM 算法， 其特點是距離測量準確，數據處理比較簡單。 常見的激光傳感器有機械式激光雷達、固態雷達等。 另一種是基于視覺傳感器的SLAM 算法，具有應用場景豐富，適用于復雜動態場景，適合多機協作，等特點。 常見的傳感器有單目攝像頭、雙目攝像頭、紅外攝像頭等[2]。

根據算法原理的不同，SLAM 算法可分為濾波法和圖優化法。 濾波法是根據濾波算法，如擴展卡爾曼濾波、粒子濾波、無損卡爾曼濾波等，對每一幀觀測數據進行處理后推測出機器人位姿，進而將觀測數據拼合而成環境地圖。 圖優化法則是根據歷史所有觀測數據，以機器人的位姿作為點，位姿之間的關系作為邊構建圖，而后調整機器人位姿盡量滿足邊的約束，最后得出環境地圖。 隨著待建地圖尺寸的擴大， 圖優化法比濾波法的計算量增長更慢，內存占用少，并且更容易達成閉環[3-4]。

在此， 以虛擬機器人試驗平臺V-REP（virtual robot experimentation platform） 為基礎搭建SLAM算法仿真驗證平臺，提出一種基于圖優化理論的激光SLAM 算法，并在仿真環境和實際環境中驗證算法可行性。

1.2 SLAM 問題

在二維地圖中，機器人的位姿可以用向量（x，y，θ）（其中，x 和y 分別為機器人的橫、 縱坐標；θ 為機器人轉過的角度）表示，建立觀測方程和運動方程，即

式（1）中：xt，yj，zt，j分別為t 時刻時，機器人的位姿、第j 個路標的位置、 傳感器所獲得的數據；vt，j為本次觀測中的噪聲。 觀測方程（1）描述了機器人觀測到路標的過程。 式（2）中：ut為t 時刻機器人運動傳感器的輸入， 也可以理解為機器人的位移；xt和xt-1分別為t 時刻及其上一時刻機器人的位姿；wt為噪聲。 運動方程（2）描述了機器人運動的過程。

采用觀測方程和運動方程將SLAM 問題以數學的形式表達。SLAM 問題可以歸結為：當知道運動傳感器的數據u 及激光傳感器的讀數z 時， 同時求解定位問題（即求解x 值）和建圖問題（即求解y值）。

因此，可以把SLAM 問題建模成一個狀態估計問題，即通過帶有噪聲的傳感器測量數據，估計動態系統內部隱藏的狀態變量，如圖1 所示。

圖1 SLAM 算法模型Fig.1 SLAM algorithm model

2 SLAM 算法

在圖1 所示SLAM 問題模型中， 已知量u，z 的值與真實值間存在一定誤差，具體表現為在閉環的運動建圖過程中：

根據式（3）、式（4）所示的SLAM 模型，需要求出機器人的位姿x 和路標的位置y， 并使測量值盡可能地接近真實值，所以其本質為求解一個最小二乘問題。

這種將待求解的量作為節點，已知量作為約束建立成邊，節點和邊構造成圖，通過求解非線性最小二乘問題使圖的總誤差最小的方法稱為圖優化法。 在SLAM 問題中，圖優化法相比于傳統的濾波法，具有計算速度快，能構造回環，地圖累積誤差較小等優點。

算法將輸入的激光傳感器數據依據數據量和測量范圍分割為多個子數據集，在每個子數據集中采用激光數據配準的方法求解SLAM 問題，得到一個局部地圖，稱為子地圖。 子地圖具有范圍小、累積誤差小、計算量少等特點。 子地圖之間存在一定的累積誤差，依據圖優化的理論，將子地圖數據（云數據和在全局地圖中的位姿）作為圖的節點，觀測數據和回環約束作為邊構造圖，經優化后使子地圖集與真實環境的總誤差最小， 然后更新子地圖數據，得出環境地圖。

2.1 點云預處理

子地圖的尺寸需要根據待測環境進行調整。 子地圖尺寸過小可能導致子地圖之間累積誤差過大，會導致子地圖數目過多，使得回環檢測難度上升，甚至無法完成回環；子地圖尺寸過大則可能導致單幀子地圖內的位姿估算難度上升，子地圖的準確性下降。 在確定子地圖的尺寸后，首先對激光點云數據進行尺寸濾波，舍棄超出子地圖范圍的點云數據。

經過初次濾波后的激光雷達點云數據，因其具有數據量大、存在隨機誤差、重復數據多等特點，需要再次進行點云濾波。 此次濾波采用體素濾波思想對點云數據進行處理。

體素濾波可以使點云在保持形狀特征的基礎上大幅度減少點云數據，減輕后續計算負荷。 其原理為，在點云數據中創建大小一致的虛擬三維體素柵格，以每個體素中點云的重心近似表示體素中所有的點云數據。 雖然以重心點表示比采用體素中心表示的計算量大，但對點云的形狀特征保存更加準確。 具體操作如下：

定義最大邊長和最小體素數，以最大體素邊長開始進行體素濾波，比較濾波后的體素數是否大于等于最小體素數， 沒有則體素邊長減半進行濾波，直至濾波結果大于等于最小體素數。 最大體素邊長和最小體素數的選取， 需要根據待測環境進行調整，體素數過少則點云喪失形狀特征，后續難以檢測回環；體素過多則無法減輕后續計算壓力，失去濾波的意義。

2.2 構建子地圖

首先，定義“激光數據配準”，以求解2 幀雷達數據或者雷達數據與已有地圖之間的位移矩陣（平移和旋轉）。 激光數據配準后，可使機器人在不同時刻、不同位姿下測得的點云數據統一到一個坐標系中，這就是構建地圖的基本原理。 子地圖的構建就是一個重復迭代配準激光數據的過程，根據計算出的最佳位姿估計將點云數據插入地圖相應的位置，從而構建基礎模型，即

式中：xt和xt-1為當前時刻和上一時刻機器人的位姿；zt為當前時刻所有點云數據的集合，每個點云數據是一個矢量， 代表該點相對機器人的距離和角度；mt-1為添加上一時刻數據后的地圖即現有地圖；ut-1為上一時刻到這一時刻的運動量；p（zt|xt，mt-1）為觀測概率，即求解在已知地圖下，獲得某一觀測值zt時，可能性最大的機器人位姿；p（xt|xt-1，ut-1）為運動概率， 即已知機器人上一時刻位姿和位移量，求解當前時刻可能性最大的機器人位姿。 最后，根據當前位姿， 將對應的點云數據經平移旋轉后，添加到子地圖相應位置。

在子地圖得到一定數量的點云數據后，將子地圖柵格化。 根據點云的位置，不可通行區域（或障礙物）以黑色表示，灰度為0；可通行區域以白色表示，灰度為255；未知區域灰度為128。 至此子地圖構建完畢。

2.3 構建全局地圖

由于每一幀子地圖都是單獨構建的，相互之間存在誤差，為消除這一誤差，需要對子地圖進行匹配。 在檢測到匹配度足夠高的子地圖后，可以認為在這2 幀子地圖中機器人觀測到了同一路標， 即2幀地圖描述了同一場景，機器人經過這段時間的位移后回到之前的坐標附近，即為回環檢測。 匹配度采用迭代最近點算法ICP（iterated closest points）進行衡量，即

式中：p 和p′為匹配的點云對；R 為旋轉矩陣；t 為平移矩陣；J 為誤差平方和。 當J 足夠小時認為完成匹配，根據J 值衡量匹配度，J 值越小匹配度越高，J 值為0 時2 幀點云數據完全一致。 然后，計算最小誤差下2 幀激光點云圖的協方差矩陣， 其逆記為Ω，稱為信息矩陣。 信息矩陣也可以用于衡量匹配度。

檢測到回環后，根據圖優化的思想，構建待優化的圖，以子地圖相對于全局地圖的位姿xt作為點，在不同子地圖中的觀測數據所產生的約束作為邊，在整個過程中所產生的點與邊組成圖。 其中，圖的邊可分為運動模型約束和觀測模型約束2 種，分別為

式中：xt為機器人的位姿；m 為靜態的環境地圖；ut和zt分別為機器人的運動量（速度、轉向等）、觀測量（由外部傳感器獲取）。 它們都是SLAM 問題中的已知量。 運動模型約束可以理解為子地圖之間的相對位姿關系，觀測模型約束可以理解為觀測到同一路標時子地圖的相對關系。 在此僅采用激光傳感器，故不存在運動模型約束，只需考慮觀測模型約束構造優化方程式。

根據點和邊約束構建圖優化目標方程，即

這是一個非線性最小二乘問題， 其求解過程為：步驟1 根據目標方程求解點（位姿）初始的估計值u0；步驟2 對信息矩陣Ω 降維，求優化的梯度方向；步驟3 梯度方向迭代，直至收斂，返回優化結果。 在程序中，采用通用圖優化算法G2O（general graph optimization）， 對這一圖優化問題進行求解。 G2O 是一個開源的C++算法框架，用于求解用圖表示的優化問題，其具有計算高效、代碼簡單、通用性好等優點。

經過優化后，全局地圖與環境的誤差降低到了理論上的最小值，并且全局地圖構成回環，提高了全局地圖的準確性。

3 試驗過程

試驗分別在V-REP（如圖2 所示）和現實環境（如圖3 所示）中進行。

圖2 仿真環境Fig.2 Simulation environment

在現實環境中， 以全向輪AGV 為移動機器人平臺，使用基于三角測距原理的激光傳感器，頻率為10 Hz， 傳感器通過串口傳輸數據。 機器人通過TCP/IP 的方式與ROS 進行通信， 操作系統為Ubuntu 16.04，ROS 版本為kinetic。 試驗中，待測環境為擺放有不規則障礙物的室內環境，房間面積為20 m×10 m。

VREP 是由Coppelia Robotics GmbH 開發的三維機器人集成開發環境。VREP 可跨平臺使用，支持7 種編程語言，每個模塊可單獨控制，自帶通用計算模塊，提供與機器人操作系統的通信接口。在VREP中搭建算法驗證環境，能夠縮短開發周期，節約開發成本，方便后續代碼移植。

4 結果分析

環境地圖和試驗結果分別如圖4，圖5 所示。

圖4 仿真環境算法結果Fig.4 Algorithm result in the simulation environment

在現實環境和仿真環境中，以圖中5 段待測距離，分別為左側桌子長度、左下實驗臺長度和寬度、房間總長度和總寬度，計算地圖中對應距離與實際距離的誤差，試驗結果見表1。

試驗結果表明，該算法在仿真試驗和實際試驗中，均可較為準確地完成地圖建立以及推算自身位姿的工作，誤差波動小，因此仿真環境可以作為現實環境的代替品對算法進行驗證。 由于現實環境存在地面不平、車輪打滑等隨機影響，致使現實環境下結果的誤差要大于仿真環境。

表1 測量誤差Tab.1 Measurement error

試驗過程由于受激光傳感器的限制，與傳感器不在同一平面的障礙物無法檢測。 對此，可以通過調整傳感器高度，重復掃描環境以建立更準確的地圖。

5 結語

所提出的基于圖優化的移動機器人同步定位與地圖構建算法，在仿真環境和實際環境中對算法進行了驗證。 傳統粒子濾波優化方法需要維護全局地圖中的所有粒子，隨著地圖范圍的增大，需要維護的粒子數增多， 對計算機的性能有較高的要求。而圖優化方法不需要地圖中每一個粒子的信息，減少了累積誤差，提高了有效性和魯棒性。