姜晶營 路銘 王子鈺 李鈺澤 邊亮亮

摘要：針對構建點云地圖存在的位姿漂移誤差問題，本文提出了一種基于因子圖優(yōu)化的點云地圖構建的方法，利用回環(huán)約束與GNSS（global navigation satellite system）約束消除了建圖的漂移誤差。通過履帶機器人平臺在實際園區(qū)場景中進行了數據采集和實驗驗證。實驗結果表明：該方法利用因子約束對歷史位姿軌跡進行優(yōu)化可進一步提高點云地圖的局部結構精度與一致性，為履帶機器人提供可靠的定位與導航數據。

關鍵詞：因子圖優(yōu)化;履帶機器人;點云地圖

引言

隨著科技技術的發(fā)展，無人駕駛技術廣泛應用于各行各業(yè)。履帶機器人以其優(yōu)越的機動性能而得到發(fā)展，可應用于結構化與非結構化的道路上，可爬坡、越障，具有較好的環(huán)境適應性。當前履帶機器人是無人駕駛技術發(fā)展的重要載體，通過兩個技術的結合，可執(zhí)行特殊場景下的任務。履帶機器人實現巡邏偵查的關鍵技術則是定位與地圖構建技術，利用定位與建圖技術可為無人駕駛載體提供先驗的地圖信息，這在高精定位與導航中都發(fā)揮著重要作用。其中基于激光雷達的定位與建圖技術受環(huán)境影響小，具有高可靠性而得到了充分的發(fā)展。

為了實現低成本的履帶機器人的高精度激光SLAM（simultaneous localization and mapping）地圖構建，本文設計了一種基于因子圖優(yōu)化的多傳感器融合定位與地圖構建方法，算法框架如圖1所示。首先是傳感器的標定與數據預處理，消除物理安裝所帶來的誤差及提供平滑的點云數據;然后是基于NDT（normal distributions transform）算法的點云地圖構建;最后是利用因子圖對歷史位姿軌跡進行優(yōu)化，消除構建地圖的漂移誤差。通過本文方法可降低構建點云地圖的重投影誤差，提高點云地圖的一致性。

1 基于因子圖優(yōu)化的點云建圖方法

1.1點云地圖構建

在多傳感器融合的方法中，需要各將各個傳感器基于自身的坐標進行標定，消除傳感器物理安裝上的誤差。在履帶機器人運行的過程中，激光雷達點云會產生畸變，其原因是激光雷達點云的采集不是瞬時獲得的。為此采用IMU（inertial measurement unit）測量值對激光雷達點云進行去畸變，為點云地圖的構建提供一個平滑的點云數據。激光雷達點云數據量大，對去畸變后的點云進行體素網格濾波進行降采樣處理，在濾除噪點的同時極大的降低了計算量，降采樣的頻率設為0.9m。最后將降采樣的點云數據通過NDT匹配算法進行累積建圖。

1.2 因子圖優(yōu)化

因子圖是一種基于非線性最小二乘優(yōu)化的概率圖，是SLAM中后端優(yōu)化的主流技術之一。因子圖一般可用來實現即插即用的多源傳感器觀測數據的輸入融合，并且這一特點極大的降低了構建地圖時的計算量。因子圖在定位與建圖技術問題中可以定義為當前位姿與歷史位姿的約束問題。在因子圖中，本文方法加入了GNSS約束和回環(huán)約束，如圖2所示。

回環(huán)約束

在大場景建圖的過程中，長時間的增量式構建點云地圖存在位姿漂移誤差的問題，而回環(huán)約束則是解決該問題的主要方法之一?；丨h(huán)約束是實時點云位姿與歷史點云位姿的約束關系，利用該約束關系可消除建圖過程中產生的累積誤差。在本方法由文獻的回環(huán)檢測所啟發(fā)，Wan等人雖然融合了幾何特征與強度特征的ISC（Intensity Scan Context）回環(huán)檢測，但是其強度只取了一個面元中的最大值，這會使得其丟失了其它點的強度信息。本文方法對面元的平均強度進行編碼，可增強特征描述子的可靠性。在接收到實時雷達點云時，對激光雷達點云劃分為面元，進行上下文掃描，將激光雷達的幾何特征和強度特征編碼到二維圖像上。在全局地圖構建的過程中，始終保持對歷史軌跡的維護，在進行回環(huán)約束檢測時，對實時的點云進行特征描述子檢測，再將實時點云的描述子與候選點云的描述子進行匹配。

GNSS約束

當接收到GNSS的測量值時，首先利用通用橫墨卡托格網系統(tǒng)（Universal Transverse Mercator Grid System，UTM）將GNSS測量值轉換到激光雷達坐標系下的位姿;然后通過計算當前GNSS的位置殘差。當GNSS測量值小于經驗值時，將GNSS約束添加到因子圖中，對當前位姿進行約束，消除建圖的位姿漂移誤差。

2 實驗結果與分析

實驗使用履帶機器人作為實驗平臺。平臺設備主要有線控底盤，聯適導航R60S-U GNSS接收機，速騰16線激光雷達與HWT905 IMU等傳感器。程序運行在工控機配置為Inteli7-9750 CPU，8GB內存，環(huán)境為Linux系統(tǒng)下的機器人操作系統(tǒng)（ROS，Robot Operating System）。在實驗驗證中，我們在校園真實場景中作了分析驗證。

2.1數據采集與建圖效果

在校園場景中采集了數據進行建圖效果分析。實際測試場景如圖3（a）所示，構建的三維點云地圖如圖3（b）所示。點云地圖去除地面點，測試場景主要為非結構化道路，建筑物與樹木較多。在采集數據的過程中，通過遙控控制履帶機器人的行進，測試道路是平坦的園區(qū)道路，軌跡長度為0.86km，履帶機器人的平均運行速度為1.2m/s，激光雷達幀數為7132。

2.2定位精度分析

如圖4所示，針對不同算法的位姿軌跡進行分析，并以GNSS測量值為真值進行對比。圖4（b）是圖4（a）中方框的局部顯示圖。在圖4（a）中，NDT算法與其他算法相比，其軌跡具有明顯的漂移，其原因是沒有回環(huán)約束的優(yōu)化，累積的漂移誤差無法消除;而LeGO-LOAM算法[4]加入回環(huán)約束優(yōu)化位姿軌跡，在一定程度上消除了位姿漂移誤差，但在局部軌跡處與GNSS真值還存在一定的誤差，如圖4（b）所示。本方法通過添加回環(huán)約束與GNSS約束，極大的消除了位姿漂移誤差，位姿軌跡與GNSS真值軌跡基本一致，均優(yōu)于NDT算法與LeGO-LOAM算法。

在量化分析時采用了絕對軌跡誤差（ATE，absolute trajectory error）進行分析，如表1所示，NDT算法達到了31.22，而本方法的均方根誤差（RMSE，root mean square error）為0.87，優(yōu)于LeGO-LOAM的1.57。

3 結論

針對園區(qū)場景下構建點云地圖存在的位姿漂移誤差的問題，提出了一種基于因子圖優(yōu)化的NDT點云地圖構建方法，應用于園區(qū)場景中的位姿估計與地圖構建。在開源算法NDT的基礎上加入回環(huán)約束與GNSS約束，消除了長時間累積建圖過程中產生的位姿漂移誤差。為了驗證所提出算法的性能，與LeGO-LOAM等經典算法進行了對比實驗。實驗結果表明所提出的方法有效的提高了點云地圖的局部結構精度與全局一致性。在下一步的工作中，需要研究多傳感器數據融合算法優(yōu)化，進一步提高算法的實時性與精度。

參考文獻：

[1]林俊欽，韓寶玲，羅慶生，趙嘉珩，葛卓.基于NDT匹配和改進回環(huán)檢測的SLAM研究[J].光學技術，2018，44（02）：152-157.

[2]付光耀，周海，卞春江，陳宇.基于閉環(huán)檢測的三維點云地圖創(chuàng)建[J].傳感器與微系統(tǒng)，2020，39（9）：4.

[3]Wang H，Wang C，Xie L . Intensity Scan Context：Coding Intensity and Geometry Relations for Loop Closure Detection[J].? 2020：2095-2101.

[4]Shan T，Englot B. Lego-loam：Lightweight and ground-optimized lidar odometry and mapping on variable terrain.32 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS） 2018;4758-4765.

作者簡介：

通訊作者：路銘（1981.10.6），男，滿族，博士/研究生，北京聯合大學應用科技學院 講師，高頻開關電源拓撲結構及其控制策略、無人駕駛車輛電氣系統(tǒng)架構優(yōu)化。

姜晶營（1998.01.13），女，漢族，河北，碩士研究生，北京聯合大學應用科技學院，研究方向為無人駕駛技術職業(yè)教育。

王子鈺（2000.07.12），女，滿族，遼寧，本科生，北京聯合大學機器人學院。

李鈺澤（1999.05.05），男，漢族，河南，本科生，北京聯合大學機器人學院。

邊亮亮（1999.05.04），男，漢族，甘肅，本科生，北京聯合大學應用科技學院。

基金項目：202101北京聯合大學“啟明星”大學生科技創(chuàng)新項目。