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

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

關(guān)鍵詞：因子圖優(yōu)化;履帶機(jī)器人;點(diǎn)云地圖

引言

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

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

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

1.1點(diǎn)云地圖構(gòu)建

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

1.2 因子圖優(yōu)化

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

回環(huán)約束

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

GNSS約束

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

2 實(shí)驗(yàn)結(jié)果與分析

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

2.1數(shù)據(jù)采集與建圖效果

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

2.2定位精度分析

如圖4所示，針對不同算法的位姿軌跡進(jìn)行分析，并以GNSS測量值為真值進(jìn)行對比。圖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）進(jìn)行分析，如表1所示，NDT算法達(dá)到了31.22，而本方法的均方根誤差（RMSE，root mean square error）為0.87，優(yōu)于LeGO-LOAM的1.57。

3 結(jié)論

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

參考文獻(xiàn)：

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

[2]付光耀，周海，卞春江，陳宇.基于閉環(huán)檢測的三維點(diǎ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），男，滿族，博士/研究生，北京聯(lián)合大學(xué)應(yīng)用科技學(xué)院 講師，高頻開關(guān)電源拓?fù)浣Y(jié)構(gòu)及其控制策略、無人駕駛車輛電氣系統(tǒng)架構(gòu)優(yōu)化。

姜晶營（1998.01.13），女，漢族，河北，碩士研究生，北京聯(lián)合大學(xué)應(yīng)用科技學(xué)院，研究方向?yàn)闊o人駕駛技術(shù)職業(yè)教育。

王子鈺（2000.07.12），女，滿族，遼寧，本科生，北京聯(lián)合大學(xué)機(jī)器人學(xué)院。

李鈺澤（1999.05.05），男，漢族，河南，本科生，北京聯(lián)合大學(xué)機(jī)器人學(xué)院。

邊亮亮（1999.05.04），男，漢族，甘肅，本科生，北京聯(lián)合大學(xué)應(yīng)用科技學(xué)院。

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