基于多約束因子圖優化的無人車定位與建圖方法

牛國臣，王瑜

（中國民航大學 機器人研究所，天津300300）

無人車沿著特定路線行駛，實現如機場擺渡車、觀光車，在工廠或封閉園區巡邏、貨物搬運［1］、衛生清掃以及安保監測［2］等功能，已經成為未來的發展趨勢。定位問題是無人駕駛中的關鍵問題［3］。最常見的定位方法是基于全球導航衛星系統（Global Navigation Satellite System，GNSS），其中全球定位系統（Global Positioning System，GPS）創立時間較早，應用最為廣泛，使用基于載波相位的差分GPS技術RTK定位精度可達到分米級［4］。但是在實際應用中，樹木、樓房建筑物等高大物體的遮擋均會對GPS信號的強度產生影響，導致定位失效。為了克服信號不足的問題，常用卡爾曼濾波器將GPS與慣性導航系統（Inertial Navigation System，INS）耦合［5-6］，但是高精度慣性導航元件較為昂貴，且存在累積誤差。近年來結合高精度地圖（High Definition Map，HD Map）［7］的 無 人 車 匹 配 定 位 發 展 較 為 迅 速［8］，但HD Map的構建需要昂貴的測繪系統和人工標注等復雜的后處理步驟［9］，成本十分昂貴，不適用于特定場景下無人車的大規模應用，構建一個低成本、可靠且在復雜環境下魯棒的定位與建圖系統已成為無人車產業級應用的需求。

為了解決上述問題，機器人導航領域的關鍵技術之一，同時定位與建圖（Simultaneous Localization and Mapping，SLAM）技術被用于解決無人駕駛的定位問題［10-11］。其中，基于激光雷達的三維激光定位與建圖（SLAM）方法不受光照影響，數據量相對較少，創建的地圖精度更高，在無人車中應用更為廣泛。Zhang和Singh［12］提出了一種基于激光雷達的實時定位和地圖（LiDAR Odometry and Mapping，LOAM）構建方法，該方法通過激光雷達來獲取點云圖，并使用非線性最小二乘優化方法來優化角點和平面點的距離，同步實現了高頻率的里程計和低頻率的建圖，實時性較好，但由于缺少后端優化，累積誤差導致所建點云地圖欠準 確，全 局 一 致 性 低。Shan和 Englot［13］在LOAM框架基礎上，提出了改進方案Lego-LOAM，使用迭代最近點（Iterative Closest Point，ICP）算法實現了閉環檢測和全局優化的建圖功能，利用高層次幾何特征提高了空間結構的描述效率，但該方法所建地圖為較稀疏的特征點地圖，難以用于定位。Ji等［14］提出了一種改進LOAM 方案，在LOAM的框架中加入了GPS約束，該方法在局部估計中獲得了較高的準確性，GPS先驗信息可消除部分激光里程計帶來的累積誤差，但點云地圖的“重影”問題明顯。Deschaud［15］提出了一種掃描到模型（Scan to Model）的匹配方法，通過最小化當前點到以隱式移動最小二乘（Implicit Moving Least Squares，IMLS）表示的表面之間的距離來完成位姿估計，實現了穩定準確的點云匹配，但計算量大，難以用于實際系統。Chen等［16］提出了一種基于語義特征的改進LOAM 方案，用于森林中樹木模型的檢測，但該方法僅對環境中樹木的語義特征進行提取，對于無人車應用場景來說，單一的環境特征遠遠不夠，并且還需要考慮大規模點云語義分割帶來的計算量問題。

綜上，以LOAM為首的激光里程計作為應用程度最為廣泛的激光SLAM 主流方案，仍存在大規模場景中無法消除累積誤差、所建的點云地圖無法用于定位的問題。而Lego-LOAM 等增加了閉環約束的改進方案，使系統增加對歷史觀測信息的判斷、保存和校正，提高軌跡的全局一致性，構建較精確的全局地圖，提高地圖精度。增加了GNSS以及地平面等先驗環境信息的方案，為SLAM 系統后端增添約束，可顯著優化激光里程計的誤差，提高姿態精度。在實際應用中，無論是姿態精度還是地圖精度均會影響無人車的定位效果。目前雖然有一些研究提出了較為完整的SLAM方案，但都處于研究階段，未考慮無人車的實際應用場景。

基于此，本文設計了一個低成本、高精度的無人車定位與建圖系統，為滿足實時性，提出一種基于主成分分析（PCA）算法進行點云特征提取與匹配的激光里程計。針對點云地圖的“重影”和激光里程計的累積誤差問題，從因子圖優化角度構建了一個完整的三維激光SLAM 系統，將GNSS（以GPS為例）及地平面等先驗信息和基于聚類特征的閉環檢測使用圖優化框架融合到SLAM系統中，并使用KITTI數據集測試了該系統的性能。

1 系統概述

本文所提出的算法框架如圖1所示。首先，對點云數據進行運動補償、體素濾波和分割等預處理操作；其次，在前端實現一個基于特征匹配的激光里程計，將地平面約束、GPS約束作為一元邊，聚類特征作為閉環約束加入到圖中，在后端進行位姿圖優化，得到最優位姿，增量顯示軌跡并存儲相應幀的點云數據構建點云地圖。

圖1 系統整體結構Fig.1 Overall structure of system

2）IMU坐標系I，以IMU重心為坐標原點，分別沿IMU水平軸和縱向軸設置X軸和Y軸，Z軸垂直于XY平面。

2 SLAM 方案

在本節中將介紹激光SLAM 系統的各個子模塊，包括點云預處理、激光里程計、多約束因子圖和圖優化等4個模塊，并對其理論內容進行推導。

2.1 點云預處理

根據LiDAR的掃描方法，按空間和時間順序記錄這些點以進行遍歷。由于在LiDAR的掃描過程中，車輛已經移動，為此引入IMU 位姿信息［12］對LiDAR運動進行補償，得到去除點云畸變的當前幀。激光點云信息量巨大，在進行幀間匹配之前，將輸入點云S經過體素濾波去除噪點和地面點，對剩下的點云進行歐氏聚類，濾除動態物體等無效點。

本文提出一種輕量級地平面分割方法。設定無人車前進的方向為世界坐標系X軸的正方向，將點云的三維空間XYZ降到二維平面XY，根據激光雷達投影到地面的射線中前后兩點的坡度是否大于事先設定的坡度閾值來判斷點是否為地面點。

激光雷達安裝在車輛頂部并與地面平行，雷達由下至上分布多個激光器，發出一系列放射狀激光束，這些激光束在平地上即表現為一條射線，如圖2所示。

圖2 地平面分割方法示意Fig.2 Schematic diagram of ground plane segmentation method

因雷達安裝高度固定，對于平坦的地面，可以根據LiDAR返回的坐標值得到點到LiDAR的距離，對于任一點i，i∈S有

式中：xi和yi為i點在LiDAR坐標系L下的坐標位置；θi為點相對于車頭正方向（X方向）的夾角；ri為i點到LiDAR的水平距離。

將同一射線上的點按照水平距離ri排序，計算同一條射線上相鄰兩點的坡度。

式中：zi為i點的垂直高度；ai，i+1為i點和i+1點之間的坡度；h為LiDAR的安裝高度。

為了避免地面不平或是LiDAR并未完全平行于地面安裝等微小因素引起的檢測誤差，定義一個閾值threguler，將式（2）結果與此閾值作比較，當ai，i+1＜threguler即將i點歸為地面點。

對去除地面點后的點云進行歐氏聚類，為了達到更好的聚類效果，在以LiDAR為圓心的不同距離區域內使用不同的聚類半徑閾值，效果如圖3所示。

圖3 點云預處理效果Fig.3 Point cloud pretreatment effect

場景中的運動物體嚴重影響建圖過程的準確性，但是從掃描中刪除所有動態對象，需要場景的高水平語義信息，對點云進行語義分割耗時較長，不適用于實時系統。本文通過刪除小型物體來替代動態物體刪除，從場景中丟棄那些尺寸被認為可能是動態物體的對象。將可能是行人、汽車、公交車或者貨車等小的點類去除，移除聚類邊界框X方向上小于10m，Y方向上小于5 m，Z方向上小于4m的點類，保留足夠的大型基礎設施信息，如墻壁、柵欄、立面和樹木（高度超過4 m），確保激光雷達里程計特征匹配過程中角點和邊緣等特征信息足夠。

2.2 激光里程計

LOAM根據激光雷達的特性，按照掃描線的空間順序和時間順序提取特征點。在本文中對其簡化，引入主成分分析計算點云主方向，以滿足計算的實時性和準確性要求。當前幀點云表示為

式中：xi、yi和zi，i∈（1，2，…，n）為經過點云坐標系到世界坐標系的坐標轉換后的點云坐標。

將點云分別投影到X軸、Y軸和Z軸上，計算協方差矩陣為

式中：cov為協方差算子，對每個點分別計算其x坐標、y坐標以及z坐標之間的協方差。例如，cov（x，y）為x坐 標 與y坐 標 之 間 的 方 差，當cov（x，y）＞0時，x和y正相關，cov（x，y）=0時，x和y相互獨立。協方差由式（5）計算：

式中：xi和yi為點云坐標；n為當前幀點云中點的個數。

計算協方差矩陣C的特征值和特征向量，根據特征值將特征向量從大到小排列，組成特征矩陣U。

設點i為在tk處獲得的特征矩陣Uk中的點，R點為i周圍10個連續點的集合，通過圍繞點i的10個矢量之和的范數的大小來評估點i附近局部表面的光滑度。定義光滑度c為

根據點的時間戳將Uk分為4個部分，并根據光滑度c的值對每個部分進行排序，選擇值最大的2個點作為拐角點，值最小的4個點為平面點。

在進行幀間匹配時，同一幀內的2個連續角點可以形成一條邊緣線，3個平面點可以形成一個平面。利用幀間變換矩陣T將k+1幀的點變換回第k幀，則屬于k+1幀的角點和平面點分別到屬于k幀的邊緣線以及平面的距離應該為0，設該距離的向量形式為d，T的初值可由IMU提供的車輛位姿得到。求d的最優值即是一個典型的非線性最小二乘問題。使用Levenberg-Marquardt（L-M）優化方法最小化d，得到最優的T：

2.3 多約束因子圖

因子圖由“頂點”和“邊”組成［17］，具體到SLAM問題中，“頂點”為車輛的位姿，“邊”為位姿之間的約束［18］。為了解決長距離漂移問題，本文提出設有一種包含地平面約束、GPS約束、點云聚類特征閉環約束和前端里程計約束的多約束因子圖，如圖4所示。圖中：Gi為由GPS數據生成的固定節點；Ti為三維位姿變換SE（3）中的車輛位姿節點；π0為地平面節點。

圖4 因子圖結構Fig.4 Structure of factor graph

2.3.1 地平面約束

在2.1節點云預處理中已完成地平面的檢測，將地面局部建模為π=［nx，ny，nz，a］T參數化的平面［19］，nx、ny和nz為平面的法線，a為原點到平面的距離，對于平面上的任一點xi、yi、zi，有xinx+yiny+xinz+a=0。

為了計算車輛姿態和地平面之間的誤差，需要將車輛的姿態節點與固定的地平面節點相連，先進行坐標轉換，用初始時刻的地平面和車輛姿態，得到由車輛姿態估計的地平面，即

采用最小參數法定義平面參數表達式為

式中：arctan（ny/nx）為平面方位角；arctan（nz/n）為平面仰角；a為截距，表示原點到該平面的距離。則位姿節點和地平面節點之間的誤差被定義為

式中：πt為t時刻檢測到的地平面。

2.3.2 GPS約束

為了便于優化，首先將GPS數據轉換為UTM坐標，然后將每個GPS數據與位姿節點相關聯［20］，位姿節點與GPS數據的時間戳對齊，則GPS位置即可作為先驗位置信息，成為位姿節點的一元邊緣。位姿節點的平移矢量與GPS位置之間的誤差定義為

式中：Tt為t時刻的車輛位姿；Gi為GPS數據生成的固定節點。

2.3.3 點云聚類特征閉環約束

在2.1節中已得到一系列點云聚類，分別進行幾何特征和直方圖特征提取，用于隨后的標識和分類［21-22］。提取的點云段特征由特征向量f=［f1f2… fn］表示，每個元素代表局部或全局描述方法。通過計算點云段中的點與其質心之間的差，可以獲得點云段的3D結構張量及其特征值λ1、λ2和λ3。參照SegMatch方法［23］計算點云的線性度（Lλ）、平面度（Pλ）、散射度（Sλ）、全方差（Aλ）、各向異性（Oλ）、特征熵（Eλ）和曲率變化度（Cλ），特征描述符如下

式中：zi為歸一化的特征值。

直方圖特征包含隨機兩點的距離統計、隨機三點組成的三角形面積統計以及隨機三點組成的一個角的角度統計等3種。

在完成聚類特征提取后，先基于點云聚類閉環匹配得到當前幀與地圖的粗匹配，再采用掃描到地圖（Scan to Map）模型［24］，以當前時刻點云幀為中心，按照時間向前和向后各索引數幀點云組成局部地圖，利用正態分布變換（Normal Distribution Transform，NDT）得到當前點云和局部地圖間的精確變換。

為了提高閉環檢測的效率，避免頻繁檢測造成計算量過大問題，設置閉環檢測幀的最小時間差，濾除時間間隔較近的匹配。最終將閉環得到的變換矩陣，作為位姿約束輸入后端進行優化。

2.4 圖優化

本文采用g2o優化庫，對最終建立的位姿圖進行優化［25］。當優化了整個LiDAR運動的位姿圖后，將位姿節點對應的三維點云幀進行拼接，從而得到全局一致的軌跡和地圖。

3 實驗分析

為了驗證本文方法的可行性，選擇無人駕駛數據集進行實驗。目前KITTI數據集是評估SLAM 的最受歡迎的數據集，為本文的實驗評估提供了真實的數據。實驗借助Linux下的機器人操作系統（Robot Operating System，ROS）架構，在具有2.20 GHz×4的i5-5200 CPU和12 GB內存的計算機上運行。

3.1 定位精度

以KITTI數據集提供的真值作為參考，將SLAM 軌跡與真值進行對比，通過計算位姿軌跡與真實軌跡之間的相對位姿誤差（Relative Pose Error，RPE）和絕對軌跡誤差（Absolute Trajectory Error，ATE）來評估本文方法的定位精度。

3.1.1 準確性分析

RPE是指在固定長度的間隔內，估算位姿和真實位姿之間的差值，相當于直接測量激光里程計的誤差。為了體現本系統的定位效果，以KITTI數據集作為輸入數據，采用經典的LOAM 方法和本文方法，分別在短距離運動場景（1 392m）和長距離運動場景（3 714m）2種模式下進行實驗，將里程計數據存儲并解算，每隔50 m 統計一次RPE，以此評價姿態精度。表1中列出了不同運動場景的數據幀數、軌跡長度以及RPE的詳細值，包括最大值、平均值、中值以及均方根值（Root Mean Square Error，RMSE）等。

表1 相對位姿誤差對比Table 1 Comparison of Relative Pose Errors（RPE）

可以看到，無論是在短距離還是長距離運動環境下，本文方法的RPE要明顯低于LOAM 方法，經典的LOAM 方法在短距離運動場景下RMSE為0.39m，但是在長距離運動環境下，里程計精度降低，而本文方法在長距離實驗測試中，RPE的RMSE值可保持在1.5m以下，適用于無人車的應用場景。圖5為本文方法和LOAM方法在長距離運動環境下，RPE值和點云序列之間的關系，為了對比更為明顯，LOAM 方法用虛線表示，本文方法用實線表示。

通過比較可以看出，本文方法對應的曲線更加緩和，相對來說跳變較少，雖然在第12次計算時出現了較大誤差值，但系統的圖約束消除了部分漂移，隨后誤差變化逐漸趨于平穩。與本文方法相比，隨著車輛運動距離的增加，LOAM 方法的累積誤差逐漸增加，曲線增幅逐漸變大，這可說明本文方法在無人車較長距離運動環境中的可靠性更高、穩定性更好。

圖5 相對位姿誤差和點云序列曲線Fig.5 Curves of RPE and point cloud sequence

3.1.2 一致性分析

對于SLAM系統一致性的評估，本文引入絕對軌跡誤差（ATE）指標，根據時間戳對齊位姿的真實值和SLAM系統的估計值，計算每對位姿之間的差值，最后對ATE進行評價。這里同時考慮旋轉和平移帶來的影響。表2中列出了ATE的詳細值，包括最大值、平均值、中值以及RMSE等。

圖6為使用本文方法和LOAM 方法得到的ATE值和點云序列之間的關系。LOAM 沒有GPS、地平面以及點云聚類特征等約束信息，雖然在開始時軌跡誤差可維持在10 m以下，但在運行一段時間后，因為累積誤差，位姿發生偏移，并且這種偏移隨著車輛運動軌跡的增長而遞增；本文方法由于加入了先驗和閉環等信息，使得車輛在運動過程中時時修正位姿的偏移量，從而可以保證車輛運動軌跡更貼近真實軌跡。

表2 絕對軌跡誤差對比Table 2 Comparison of Absolute Trajectory Errors（ATE）

圖7為使用本文方法得到的車輛運動軌跡與參考軌跡的對比，其中SLAM 系統的估計位姿越靠近真實位姿即表示誤差越小。從表1中已知使用經典的LOAM 方法只適合于車輛運動距離較小時，隨著運動時間增長，里程計帶來的漂移誤差隨著LiDAR的運動逐漸增加，使位姿嚴重偏離真實軌跡。與LOAM相比，本文方法所得到的軌跡與真實軌跡基本一致，全局一致性高，可以進一步提高無人車定位與建圖的精度。

圖6 絕對軌跡誤差和點云序列的變化曲線Fig.6 Change curves of ATE and point cloud sequence

圖7 SLAM系統得到的軌跡與參考軌跡對比Fig.7 Comparison between trajectory obtained by SLAM system and reference trajectory

3.2 建圖效果

為了驗證本文方法的大規模建圖效果，將通過SLAM方法得到一系列連續位姿形成的位姿軌跡對應的點云幀轉換為世界坐標系，拼接生成點云地圖，如圖8所示，基于點云的三維地圖可用于后期無人車定位。

在實驗中，圖8（a）、（b）分別為使用LOAM方法和本文方法得到的建圖場景俯視圖，圖中A、B、C均為點云地圖局部細節效果。可以看到，由于缺乏閉環檢測和后端圖優化，圖8（a）中地圖出現了較多的重影和模糊的現象，如圖中框選中區域所示，點云幀之間誤差較大，漂移嚴重甚至導致地圖構建失敗，在框選中C部分，點云幀間匹配丟失，所建地圖未閉合。本文方法所建地圖與真實環境基本吻合，圖8（b）中地圖封閉且完整，邊界清晰，無明顯重影，效果較好。

圖8 點云地圖Fig.8 A point cloud map

4 結 論

本文提出一種基于多約束因子圖優化的三維激光SLAM方案，經實驗驗證可得到以下結論：

1）基于PCA算法和點云特征匹配的激光里程計可以有效應對無人車室外環境定位，該匹配方法簡單且魯棒，實時性好。

2）加入地平面、GPS數據等先驗信息以及點云聚類特征閉環約束的多約束因子圖，降低了前端里程計的累積誤差，解決了構建地圖時的重影問題，同時提高了定位精度。

3）以三維激光SLAM 的代表性方法LOAM與本文方法進行對比驗證，實驗證明，本文方法在長距離實驗測試中，RPE可達到1.5 m以下，適用于無人車的應用場景。

下一步計劃將此系統用到無人車實際系統中，探索復雜環境下高精度的無人車定位與建圖方法。