基于多傳感器融合的系統(tǒng)自我定位與地圖重建

郝 睿，李 瑞，史瑩晶，龔美鳳，張智容

(1.電子科技大學(xué) 自動化工程學(xué)院，四川 成都 611731；2.電子科技大學(xué)長三角研究院(湖州)，浙江 湖州 313001；3.重慶大學(xué) 自動化學(xué)院，重慶 400044)

0 引言

隨著傳感器技術(shù)和人工智能的不斷發(fā)展,諸多全新的移動測量系統(tǒng)空間數(shù)據(jù)獲取技術(shù)應(yīng)運(yùn)而生,其中自身位姿數(shù)據(jù)和運(yùn)動軌跡信息是完成空間數(shù)據(jù)獲取的基礎(chǔ),其準(zhǔn)確性直接影響到三維空間場景數(shù)據(jù)的精度及有效性。但是當(dāng)面臨各種復(fù)雜環(huán)境時,如何充分發(fā)揮傳感器的優(yōu)勢,通過自我定位和地圖構(gòu)建(Simultaneous Localization and Mapping,SLAM)算法提高移動測量系統(tǒng)整體的精度和魯棒性,成為近年來移動測量領(lǐng)域的研究熱點(diǎn)。

針對以上問題,本文研究設(shè)計(jì)基于激光雷達(dá)(LiDAR)、相機(jī)和慣性測量單元(IMU)多傳感器融合的移動背包測量系統(tǒng),目的在于完成各種環(huán)境中系統(tǒng)的精準(zhǔn)自我運(yùn)動估計(jì)和對目標(biāo)環(huán)境的三維稠密建圖。主要工作如下:

① 構(gòu)建“傳感器聯(lián)合—激光慣導(dǎo)前端定位器—激光視覺后端三維重建—平臺”的多傳感融合SLAM架構(gòu)體系。研究多傳感器融合帶來的描述一致性、位置姿態(tài)的準(zhǔn)確性和實(shí)時建圖的有效性等問題。

② 對多傳感器融合SLAM算法加以改進(jìn),提出了一種基于因子圖優(yōu)化框架的雷達(dá)慣導(dǎo)融合定位算法。首先,改進(jìn)點(diǎn)云提取及預(yù)處理的方式;其次,圍繞幀間配準(zhǔn)算法時效性和精度問題,設(shè)計(jì)關(guān)鍵幀選取策略和點(diǎn)云配準(zhǔn)算法,構(gòu)建多個約束因子在局部滑動窗口中進(jìn)行聯(lián)合優(yōu)化,得到初步定位值最后設(shè)計(jì)基于幀到地圖(scan-to-submap)的位姿優(yōu)化方法,優(yōu)化定位結(jié)果。

③ 對激光-視覺融合方法進(jìn)行改進(jìn)。在后端閉環(huán)檢測圖優(yōu)化部分,利用視覺詞袋輔助三維激光雷達(dá)完成閉環(huán)候選幀選取,同時基于三維激光雷達(dá),進(jìn)一步融合視覺信息豐富雷達(dá)點(diǎn)云的紋理特征,基于融合點(diǎn)云構(gòu)建新的點(diǎn)云表達(dá)形式,進(jìn)行點(diǎn)云網(wǎng)格化實(shí)現(xiàn)稀疏點(diǎn)云稠密化重建效果,以便更好地對移動測量系統(tǒng)周圍環(huán)境進(jìn)行評估。

最終設(shè)計(jì)系統(tǒng)整體評估策略。通過研究領(lǐng)域內(nèi)公開的標(biāo)準(zhǔn)數(shù)據(jù)集(KITTI)進(jìn)行多場景實(shí)驗(yàn),從軌跡精度、算法對比地圖一致性等多個角度,全面分析本文SLAM方案中位姿估計(jì)方法的精確性和三維重建的有效性。同時設(shè)計(jì)構(gòu)建出低成本高性能的移動背包掃描系統(tǒng)(BLS),通過本地?cái)?shù)據(jù)集驗(yàn)證該移動背包系統(tǒng)的有效性和適用性。

1 相關(guān)工作

早在1986年的IEEE機(jī)器人與自動化國際會議(ICRA)上,為了將基于估計(jì)理論的方法引入到移動測量系統(tǒng)的運(yùn)動估計(jì)與建圖問題中,Cheeseman等提出了自我定位和地圖構(gòu)建的概念。經(jīng)過30余年的理論創(chuàng)新和傳感器技術(shù)發(fā)展,同時伴隨著電子探測、智能機(jī)器人和自動駕駛等領(lǐng)域的興起,SLAM作為這些領(lǐng)域中基本的技術(shù)模塊,完成了眾多的工程實(shí)踐和應(yīng)用推廣任務(wù),成為許多高校實(shí)驗(yàn)室和企業(yè)的研究重點(diǎn)。

為此,研究者提出了使用諸如激光雷達(dá)等有源傳感器來完成相關(guān)SLAM任務(wù)。通過傳感器獲取現(xiàn)實(shí)世界數(shù)據(jù),反映空間實(shí)體單元的幾何信息,記錄實(shí)體表面在觀測點(diǎn)上的物理參數(shù),其中包括觀測物體最基本的各離散點(diǎn)三維坐標(biāo),單元表面的紋理特性等;將此類觀測到的數(shù)據(jù)進(jìn)行解算來得到自身狀態(tài)結(jié)果和周圍環(huán)境信息。由于目前系統(tǒng)所面臨的各種復(fù)雜環(huán)境,難以在使用單一傳感器的前提下保證實(shí)時完成環(huán)境中的SLAM任務(wù)?；诩兗す饫走_(dá)的SLAM系統(tǒng)由于雷達(dá)的更新頻率低(5～30 Hz),在快速運(yùn)動場景下性能較低,以及雷達(dá)點(diǎn)云的稀疏性且其基于幾何呈現(xiàn)的特點(diǎn),在具有重復(fù)結(jié)構(gòu)(如隧道或狹窄走廊)的環(huán)境中容易導(dǎo)致SLAM任務(wù)失敗;基于純視覺的SLAM系統(tǒng)由于傳感器的光敏性特點(diǎn)和不穩(wěn)定性,當(dāng)環(huán)境信息退化時,系統(tǒng)性能會產(chǎn)生明顯的弱化[1]。

經(jīng)典激光SLAM方案LOAM由Zhang等[2]于2014年提出,作為基于幾何三維激光雷達(dá)的SLAM方案,目前在 KITTI 數(shù)據(jù)集上測試狀態(tài)軌跡精度位列第二。但由于缺少閉環(huán),對于累積誤差和發(fā)生位置重訪的時候無法進(jìn)行位姿信息的更新。對此,當(dāng)前激光SLAM領(lǐng)域內(nèi)的研究學(xué)者基于LOAM展開了多種SLAM方案的研究,通過在雷達(dá)前端定位部分融入高頻IMU(100～500 Hz)數(shù)據(jù)去除由雷達(dá)連續(xù)時間運(yùn)動產(chǎn)生的畸變。2018年Shan等[3]提出的開源LeGo-LOAM算法,提出了具有閉環(huán)檢測和后端優(yōu)化的激光慣導(dǎo)融合SLAM方案,團(tuán)隊(duì)隨之在2020年提出了基于滑窗的激光雷達(dá)慣導(dǎo)緊耦合SLAM方案LIO-SAM,該方案構(gòu)建因子圖并將異源傳感器測量值作為因子合并到系統(tǒng)中,實(shí)現(xiàn)高精度、實(shí)時的移動系統(tǒng)軌跡估計(jì)和地圖構(gòu)建[4]。

在SLAM實(shí)際任務(wù)中,為了能夠有效地融合多個傳感器,需要考慮多傳感器數(shù)據(jù)融合和時空同步。激光SLAM對掃描環(huán)境的特征具有較強(qiáng)依賴性,在連續(xù)運(yùn)動期間的不同時間會存在點(diǎn)云的運(yùn)動失真,點(diǎn)云掃描配準(zhǔn)在非結(jié)構(gòu)化場景中容易失敗;視覺SLAM雖然在位置重訪閉環(huán)檢測方面取得很大進(jìn)展,但在能見度降低,以及不同的視角導(dǎo)致同一場景外觀的變化等感知退化場景下,產(chǎn)生的特征點(diǎn)稀疏性會導(dǎo)致數(shù)據(jù)關(guān)聯(lián)模糊和感知理解混疊[5]。而IMU因具備模塊小成本低、自主感知性強(qiáng)、高精度(每秒數(shù)千個樣本)等優(yōu)勢,可以通過自積分跟蹤載體姿態(tài),穩(wěn)定的IMU能為系統(tǒng)提供關(guān)鍵且可靠的位姿信息。當(dāng)前多傳感器的融合也更偏向于低成本和高性能的結(jié)合,在融合方案中更加合理利用各傳感器數(shù)據(jù),同時減少傳感器自身給系統(tǒng)整體帶來的影響,使得系統(tǒng)整體在性價比高的方案下,能夠保證系統(tǒng)性能以及穩(wěn)定性。

隨著信息技術(shù)研究的深入、傳感器的迭代及數(shù)字化城市等相關(guān)技術(shù)的發(fā)展,高精地圖的重要性也日益顯現(xiàn)。與僅包含寬、高信息的二維圖像相比,三維地圖重建技術(shù)的發(fā)展將移動測量系統(tǒng)的感知提高了一個維度。從傳感器融合角度來看,微軟將紅外光傳感器與單目相機(jī)相結(jié)合,在2009年推出了KinectV1,其中空間距離信息由紅外光傳感器主動提供,環(huán)境紋理信息由被動傳感器相機(jī)提供。這類融合方式的出現(xiàn)使得室內(nèi)三維重建技術(shù)得到了迅速的發(fā)展。近年來已經(jīng)開發(fā)了將LiDAR和相機(jī)數(shù)據(jù)結(jié)合起來進(jìn)行3D映射或重建的各種算法。使用LiDAR數(shù)據(jù)進(jìn)行重建已用于不同的項(xiàng)目,來自ETH ASL實(shí)驗(yàn)室2020年發(fā)布的基于體積元素和基于截?cái)嗟膸Х柧嚯x函數(shù)(TSDF)的建圖工作[6],在機(jī)器人的避障、導(dǎo)航領(lǐng)域的三維稠密重建上有重要作用。

2 多傳感器融合定位與建圖

本地移動BLS系統(tǒng)信息采集主要由VLP-16激光雷達(dá)、單目相機(jī)和六自由度(6-DOF)IMU組成,背包平臺攜帶由電源和在CPU上、Ubuntu開發(fā)環(huán)境下的筆記本電腦,共同組成信息處理系統(tǒng)。系統(tǒng)整體具備移動采集、自我定位與建圖能力,如圖1所示。BLS系統(tǒng)的平均運(yùn)動速度為1 m/s,由于平臺沒有搭載全球?qū)Ш叫l(wèi)星系統(tǒng)(GNSS)類相關(guān)傳感器,無法獲取運(yùn)動軌跡真實(shí)值。因此在本地?cái)?shù)據(jù)采集過程中,將有意識走出一個規(guī)則區(qū)域,形成路線閉環(huán),以此定性評估系統(tǒng)整體定位效果和建圖效果。

圖1 背包掃描系統(tǒng)Fig.1 Backpack scanning system

本文所設(shè)計(jì)的背包式移動測量系統(tǒng)旨在自動收集和解算傳感器觀測數(shù)據(jù),完成自我運(yùn)動估計(jì)和環(huán)境重建。按照“多傳感器聯(lián)合—位姿定位—閉環(huán)檢測—位姿地圖更新—點(diǎn)云網(wǎng)格化—三維地圖重建”的算法流程,構(gòu)建包含“傳感器硬件—激光慣導(dǎo)前端定位器—激光視覺后端三維重建—平臺”的SLAM總體技術(shù)框架?？傮w框架如圖2所示。

圖2 多傳感器融合算法框架Fig.2 Multi-sensor fusion algorithm framework

2.1 多傳感器數(shù)據(jù)融合

定位就是基于系統(tǒng)傳感器觀測與測量數(shù)據(jù),通過數(shù)據(jù)融合來解算系統(tǒng)的運(yùn)動狀態(tài)。本節(jié)將具體描述所提出的多傳感器融合定位與地圖重建2個模塊,首先闡述基于因子圖優(yōu)化的LiDAR、慣導(dǎo)數(shù)據(jù)融合方法: 本文引入關(guān)鍵幀和增量式優(yōu)化的方式求解系統(tǒng)運(yùn)動狀態(tài),通過高效的滑動窗口方法,構(gòu)建包含固定數(shù)量點(diǎn)云幀的局部地圖;然后將新的掃描關(guān)鍵幀注冊到子關(guān)鍵幀集合中[7],同時固定觀測誤差項(xiàng)也會被限制在一定范圍內(nèi);最后對狀態(tài)估計(jì)中的各個狀態(tài)量進(jìn)行建模和求解,在接收到新的傳感器觀測信息后,根據(jù)傳感器各自的觀測方程以及相應(yīng)的代價函數(shù)對狀態(tài)量進(jìn)行邊緣化,從而實(shí)現(xiàn)多傳感器的實(shí)時數(shù)據(jù)融合。

此外,由于系統(tǒng)中各個傳感器都有自己的時間基準(zhǔn),且采樣頻率各異,所以需要保證傳感器融合的時間信息同步。為了綜合運(yùn)用整體軟硬件算法,本系統(tǒng)將系統(tǒng)各功能模塊節(jié)點(diǎn)化。首先在硬件層面使用開源操作系統(tǒng)(Robot Operating System,ROS)發(fā)布—訂閱式的通信機(jī)制,包括異步話題與同步服務(wù)主要通信機(jī)制。利用ROS自身提供的時鐘信號,通過驅(qū)動控制程序和觸發(fā)傳感器響應(yīng),將接收到的傳感器觀測值統(tǒng)一到ROS通信的規(guī)范格式下,參照系統(tǒng)時間篩選出鄰近時間內(nèi)的傳感器采樣數(shù)據(jù),從而近似實(shí)現(xiàn)不同傳感器節(jié)點(diǎn)的時間同步[8]。在軟件層面,設(shè)計(jì)算法來同步循環(huán)多個傳感器所發(fā)布的原始數(shù)據(jù)。主要思想為基于每次更新的LiDAR點(diǎn)云幀序列,將其當(dāng)前時刻作為插入的時間戳,通過插值計(jì)算獲得除雷達(dá)以外其他傳感器的同一時刻等效信息[9]。

2.2 多傳感器融合定位里程計(jì)實(shí)現(xiàn)

當(dāng)前激光SLAM定位部分存在的主要問題包括點(diǎn)云信息的獲取具有較強(qiáng)的環(huán)境特征依賴性,定位的效率與精度依賴于系統(tǒng)位姿估計(jì)初值的精度以及閉環(huán)檢測算法的回召率和精確率。為了解決以上問題,并且在具有挑戰(zhàn)性的環(huán)境下保證系統(tǒng)魯棒性和計(jì)算效率,本文基于因子圖優(yōu)化思想,設(shè)計(jì)以移動測量系統(tǒng)為中心的LiDAR-慣導(dǎo)定位算法。設(shè)計(jì)和改進(jìn)模塊包括以下三方面:

① 改進(jìn)原始點(diǎn)云預(yù)處理方式和關(guān)鍵幀選取策略。融合高頻IMU來線性插值輔助點(diǎn)云運(yùn)動畸變校正,并對點(diǎn)云去冗去噪降低配準(zhǔn)誤差;圍繞幀間配準(zhǔn)算法時效性和精度問題,改進(jìn)用于點(diǎn)云配準(zhǔn)的關(guān)鍵幀選取策略,相比于傳統(tǒng)的點(diǎn)云配準(zhǔn)方法,大大增加了計(jì)算速度,減少了點(diǎn)云配準(zhǔn)所需要的時間。

② 構(gòu)建基于正態(tài)分布變換(NDT)的點(diǎn)云多態(tài)配準(zhǔn)算法。完成幀間配準(zhǔn),建立雷達(dá)相對變換位姿因子,設(shè)計(jì)基于幀到子地圖的局部位姿優(yōu)化方法,優(yōu)化定位結(jié)果,保持計(jì)算效率。

③ 構(gòu)建因子圖模型。將IMU的定位結(jié)果,激光雷達(dá)掃描配準(zhǔn)得到的幀間變換結(jié)果以及發(fā)生閉環(huán)檢測時的位姿變換結(jié)果,分別傳遞到因子圖優(yōu)化中,通過聯(lián)合優(yōu)化得到的系統(tǒng)定位值,生成系統(tǒng)初步定位軌跡和地圖。

本文基于因子圖優(yōu)化框架,根據(jù)輸入的點(diǎn)云幀序列、慣導(dǎo)測量序列和相機(jī)觀測序列,估計(jì)系統(tǒng)運(yùn)動狀態(tài)量,包括關(guān)鍵幀的6-DOF位姿,LiDAR掃描點(diǎn)云值以及IMU的偏置誤差。最終估計(jì)出移動測量系統(tǒng)的狀態(tài)及其軌跡。在一般的局部優(yōu)化過程中,只有滑動窗口中的雷達(dá)相對位姿變換因子和IMU預(yù)積分因子參與,滑動窗口外的歷史狀態(tài)信息則進(jìn)行邊緣化處理,若發(fā)生了位置重訪,則閉環(huán)檢測因子以相對位姿變換因子的形式加入?yún)⑴c聯(lián)合優(yōu)化。當(dāng)新的傳感器觀測序列進(jìn)入時,子圖和優(yōu)化窗口按照時間發(fā)展同時向前滑動[10]。對此構(gòu)建出LiDAR-IMU融合定位的無約束優(yōu)化目標(biāo)函數(shù)為:

(1)

將IMU數(shù)據(jù)預(yù)積分測量值與LiDAR點(diǎn)云數(shù)據(jù)配準(zhǔn)分別得出的幀數(shù)據(jù)之間的位姿約束,以及在發(fā)生位置重訪時產(chǎn)生的閉環(huán)檢測約束,使用基于因子圖的優(yōu)化庫(GTSAM)的優(yōu)化方式融合[11],得到載體更高精度的位置姿態(tài)信息。式中包含LiDAR、IMU傳感器的殘差范數(shù)。其中先驗(yàn)因子代價函數(shù)提供了當(dāng)前優(yōu)化窗口中待優(yōu)化狀態(tài)與歷史邊緣化狀態(tài)之間的約束為:

(2)

雷達(dá)相對位姿變換因子代價函數(shù)表示為式(3),描述了滑動優(yōu)化窗口中相鄰LiDAR掃描關(guān)鍵幀之間的位姿運(yùn)動變換約束,其中閉環(huán)檢測代價函數(shù)則依靠雷達(dá)相對位姿變換因子代價函數(shù)來實(shí)現(xiàn)。

(3)

IMU預(yù)積分因子代價函數(shù)如式(4)所示,描述了優(yōu)化窗口中相鄰LiDAR掃描關(guān)鍵幀之間,慣導(dǎo)測量狀態(tài)和隨機(jī)游走偏移量之間的運(yùn)動約束。

(4)

關(guān)鍵幀信息包括自身6-DOF位姿、LiDAR測量數(shù)據(jù)以及觀測圖像,在完成幾何、視覺層面的閉環(huán)候選幀選取后,其中基于幾何特征點(diǎn)配準(zhǔn)方式的激光SLAM閉環(huán)檢測,將新的點(diǎn)云幀注冊到與圖中歷史節(jié)點(diǎn)相關(guān)的所有其他幀中,以找到匹配項(xiàng)。同時,視覺通過詞袋(BoW)模型比較圖像之間的相似度,來判斷閉環(huán)檢測的發(fā)生。如果檢索到閉環(huán)候選幀,則將圖像的特征描述符與幾何檢測出的候選幀配準(zhǔn),以進(jìn)一步驗(yàn)證閉環(huán)候選幀的正確性[12]。

當(dāng)新的狀態(tài)節(jié)點(diǎn)xk添加到因子圖中,通過NDT將子圖配準(zhǔn)得到的當(dāng)前幀與候選幀2幀相對位姿,即閉環(huán)約束因子[13],融入到當(dāng)前SLAM系統(tǒng)中。主要流程如下:

① 尋找歐幾里德空間上與xk距離相近的關(guān)鍵幀;

② 如圖3所示,假設(shè)xi為當(dāng)前閉環(huán)候選幀,通過NDT點(diǎn)云配準(zhǔn)xk時刻的激光掃描特征Xk到附近的關(guān)鍵幀F(xiàn)i-m,…,Fi,…,Fi+m,本文定義m=12,得到當(dāng)前狀態(tài)xk節(jié)點(diǎn)與候選閉環(huán)節(jié)點(diǎn)即歷史狀態(tài)節(jié)點(diǎn)之間的變換關(guān)系ΔTi,k;

圖3 多傳感器融合因子圖Fig.3 Multi-sensor fusion factor map

(a)標(biāo)定環(huán)境

(c)標(biāo)定效果驗(yàn)證

③ 將ΔTi,k根據(jù)相對位姿因子構(gòu)建方法,構(gòu)建為閉環(huán)因子添加到系統(tǒng)因子圖中。

以上工作完成了累積誤差消除,解決了位置重訪問題。 將構(gòu)建的約束因子放入GTSAM庫的因子圖中進(jìn)行優(yōu)化融合,最終實(shí)現(xiàn)了系統(tǒng)在不同環(huán)境下的可靠的自我估計(jì)定位里程計(jì)功能。

2.3 多傳感器融合稠密建圖

在通過多傳感器融合完成系統(tǒng)基本運(yùn)動定位和點(diǎn)云地圖構(gòu)建后,需要對周圍環(huán)境進(jìn)行進(jìn)一步描述。 本節(jié)具體闡述基于多傳感器融合的稠密建圖方案,主要包括基于因子圖優(yōu)化的軌跡地圖更新方式以及LiDAR-相機(jī)進(jìn)一步融合,生成紋理化雷達(dá)點(diǎn)云的網(wǎng)格化過程。

首先保證LiDAR和相機(jī)的數(shù)據(jù)同步,對二者進(jìn)行外參標(biāo)定,通過視覺與雷達(dá)的共有特征進(jìn)行3D-3D點(diǎn)對匹配,用剛體的平移變換和旋轉(zhuǎn)變換來描述三維激光雷達(dá)和相機(jī)在空間中的相對位置關(guān)系,最終實(shí)現(xiàn)融合。原理如下:

(5)

式中:(u,v)表示由相機(jī)捕獲的圖像數(shù)據(jù),(x,y,z)表示激光雷達(dá)捕獲的三維點(diǎn)陣云,其目標(biāo)是建立一個轉(zhuǎn)化矩陣M,將三維點(diǎn)映射到二維點(diǎn) 。

在已知傳感器點(diǎn)的對應(yīng)關(guān)系的情況下,采用Kabsch算法[14]進(jìn)行點(diǎn)對間的RT矩陣運(yùn)算。設(shè)P和Q是同一個目標(biāo)在不同坐標(biāo)系下的2組點(diǎn),pi和qi是集合內(nèi)的第i個點(diǎn)。令2個坐標(biāo)系的旋轉(zhuǎn)為R,平移為T,Kabsch算法求解下述優(yōu)化問題:

(6)

得出標(biāo)定后的R、T矩陣后,將激光雷達(dá)點(diǎn)云作剛體變換轉(zhuǎn)移至相機(jī)坐標(biāo)系下,再投影至圖像坐標(biāo)系中,最終返回有色點(diǎn)云至雷達(dá)坐標(biāo)系中。

完成全局位姿與地圖的更新后,返回當(dāng)前點(diǎn)云幀集合與閉環(huán)檢測后得到的變換矩陣。在點(diǎn)云配準(zhǔn)環(huán)節(jié)中,當(dāng)目標(biāo)點(diǎn)云為空或目標(biāo)點(diǎn)云過小時,設(shè)置返回空的配準(zhǔn)候選集條件,以此判斷是否存在配準(zhǔn)點(diǎn)云幀,從而確定是否發(fā)生了位置重訪。當(dāng)其為空時,說明沒有得到點(diǎn)云配準(zhǔn),從而也不存在發(fā)生閉環(huán),此時將原始點(diǎn)云轉(zhuǎn)換合并到目標(biāo)點(diǎn)云中[15],發(fā)布更新的點(diǎn)云地圖和軌跡。

結(jié)合生成的雷達(dá)里程計(jì),采用比較傳統(tǒng)的方式將里程計(jì)和點(diǎn)云信息進(jìn)行增量式拼接建圖,將點(diǎn)云信息合成到TSDF建圖,降低地圖的內(nèi)存開銷并保證了實(shí)時更新。參考Nieβner等[16]在2013年的工作,最終利用實(shí)時的雷達(dá)里程計(jì)和著色點(diǎn)云信息更新周圍環(huán)境。主要過程如下:

① 將輸入的點(diǎn)云按照一定分辨率劃分為立方體,又稱為體素。由于掃描得到的三維空間內(nèi)外部部分信息是空白的,在空間劃分過程中,為了避免空間資源浪費(fèi),提高計(jì)算效率,采用八叉樹(Octree)非均勻劃分,將算法的復(fù)雜度降至O(log8N)。根據(jù)掃描數(shù)據(jù)的最大空間尺寸,建立首個立方體作為根節(jié)點(diǎn)(即邊界區(qū)域)包含所有三維空間信息。若立方體內(nèi)包含空間點(diǎn),從根節(jié)點(diǎn)開始,將包含掃描物體節(jié)點(diǎn)均勻劃分為8個子節(jié)點(diǎn),遞歸迭代所有立方體直至滿足終止條件;若不包含點(diǎn)或已達(dá)到最大深度則不進(jìn)行劃分。

② 構(gòu)建哈希表進(jìn)行體素網(wǎng)格的散列存儲,每個體素網(wǎng)格大小固定,同時用連續(xù)的內(nèi)存空間儲存每一個體素塊。轉(zhuǎn)化三維空間中劃分構(gòu)造的體素g到世界坐標(biāo)系下,得到三維空間點(diǎn)坐標(biāo)p。通過使用哈希表存儲體素網(wǎng)格,提高時間效率,此過程的算法時間復(fù)雜度從O(n)降到O(1)。

③ 對每一個雷達(dá)線束下的終端體素只執(zhí)行一次光線投射,這加速了TSDF中地圖信息更新[17]的速度,大大提高了算法的時間效率。根據(jù)第i幀紋理化點(diǎn)云的當(dāng)前系統(tǒng)位姿矩陣:

P=[p0p1…pn+k+1]。

(7)

由T(g,i)-1p求得p在LiDAR坐標(biāo)系的投影點(diǎn),同時反投影得點(diǎn)云幀中對應(yīng)像素點(diǎn)x。

④ 通過式(8)計(jì)算點(diǎn)p沿投影射線到掃描物體表面的最近距離:

sdfi(p)=‖T(g,i)-1p‖-Ri(x)。

(8)

(9)

3 實(shí)驗(yàn)驗(yàn)證與結(jié)論

基于上述內(nèi)容,設(shè)計(jì)系統(tǒng)整體評估策略。通過研究領(lǐng)域內(nèi)公開的標(biāo)準(zhǔn)數(shù)據(jù)集KITTI進(jìn)行多場景實(shí)驗(yàn),從軌跡精度、算法對比和地圖一致性等多個角度,全面分析本文SLAM方案中位姿估計(jì)方法的精確性和三維重建的有效性。此外,在移動測量系統(tǒng)上進(jìn)行了實(shí)際應(yīng)用,對其進(jìn)行綜合分析,驗(yàn)證了系統(tǒng)的可靠性與穩(wěn)定性。

3.1 公開數(shù)據(jù)集評估實(shí)驗(yàn)

基于標(biāo)準(zhǔn)數(shù)據(jù)集KITTI的系統(tǒng)軌跡精度評估,該數(shù)據(jù)集可以很好地觀察移動測量平臺在各種任務(wù)環(huán)境下的感知情況。分別在不同類型序列中完成對比實(shí)驗(yàn),展示相對于軌跡真值,改進(jìn)后的系統(tǒng)前端算法初步定位效果。此外,進(jìn)一步對比閉環(huán)優(yōu)化后的軌跡效果(帶*號),闡述閉環(huán)檢測和因子圖優(yōu)化對累積誤差消除做出的貢獻(xiàn),并對優(yōu)化前后系統(tǒng)定位算法的魯棒性展開分析。

00、06為環(huán)境特征明顯的城鎮(zhèn)序列,閉環(huán)較多且掃描時間較長。其中00序列檢測到的閉環(huán)數(shù)目較多,06序列則容易發(fā)生假性閉環(huán),同時這2個序列容易產(chǎn)生較大的累積誤差。

從實(shí)驗(yàn)過程中效果分析,以及最終結(jié)果圖5和圖6所示,00、06序列閉環(huán)檢測準(zhǔn)確率較高,在原本發(fā)生位置重訪的區(qū)域上完成了閉環(huán)。相較于初始定位的系統(tǒng)運(yùn)動軌跡,在長時間運(yùn)動下以及無閉環(huán)條件下帶來的誤差累積,軌跡發(fā)生了明顯偏移。全局優(yōu)化后累積誤差基本消除,其運(yùn)動軌跡也更接近于真值,說明后端閉環(huán)檢測優(yōu)化達(dá)到了預(yù)期效果。

圖5 00、09序列的系統(tǒng)軌跡精度圖與點(diǎn)云地圖Fig.5 System trajectory accuracy map and point cloud map of 00 and 09 sequences

圖6 06、10序列的系統(tǒng)軌跡精度圖與點(diǎn)云地圖Fig.6 System trajectory accuracy map and point cloud map of 06 and 10 sequences

由圖5可以看出,重建后的環(huán)境建筑物輪廓清晰,樹木、車輛等物體描述基本準(zhǔn)確,局部點(diǎn)云地圖如圖A、B、C、D所示,證明了所建點(diǎn)云地圖的有效性。

09、10為城鄉(xiāng)結(jié)合環(huán)境序列,環(huán)境特征較為復(fù)雜,環(huán)境變換較多,綜合了B,C類型場景特點(diǎn)。09序列前期沒有閉環(huán),實(shí)際的和檢測到的閉環(huán)均只有1個,位于掃描起始和終止區(qū)域,連續(xù)且長時間的彎道給系統(tǒng)帶來的軌跡漂移,在閉環(huán)后顯示累積誤差大幅度消除并回到初始位置。序列10類似于序列01,受到有限閉環(huán)的影響,連續(xù)無閉環(huán)后的運(yùn)動軌跡也略微變差,但速度稍慢,整體軌跡精度可觀。

為了客觀評估系統(tǒng)在定位層面的精度以及算法的魯棒性,將采用業(yè)內(nèi)通用的相對位姿誤差(Relative Pose Error,RPE)評估指標(biāo)。本文取100 m長度相對平移誤差(%)的平均值作為評估指標(biāo),與經(jīng)典激光SLAM方案LOAM以及目前主流的多傳感器融合方案LeGO_LOAM[3]、DVL_SLAM[18]和Hdl-graph-slam[19]進(jìn)行軌跡精度對比評估。目的是驗(yàn)證當(dāng)前定位算法精度是否能夠匹配現(xiàn)有主流算法以及本文算法的魯棒性。同時為了避免和對比的SLAM方案中的原實(shí)驗(yàn)數(shù)據(jù)相違背,直接引用對應(yīng)方案論文中的實(shí)驗(yàn)結(jié)果,并通過開源評估工具來完成實(shí)驗(yàn)數(shù)據(jù)評估[20],具體評估結(jié)果如表1所示。

表1 RPE評估結(jié)果:相對平移誤差

由表1中RPE評估結(jié)果可知,LOAM總體具有最高的精度。本文是基于Hdl-graph-slam方案展開的研究,該方案為早期多傳感器融合3D SLAM方案,由于在點(diǎn)云采集過程中,需要提取地面點(diǎn)以及進(jìn)行點(diǎn)云的降采樣,通過提取關(guān)鍵幀建立各約束完成位姿估計(jì),因此在高速公路數(shù)據(jù)集01序列和04無閉環(huán)的鄉(xiāng)村序列效果相對于其他方案較差,表內(nèi)評估數(shù)據(jù)定義為失效。

通過以上分析可以得出,設(shè)計(jì)調(diào)整前端定位和后端建圖的策略后,本文定位在大多數(shù)序列上精度接近LOAM,且在魯棒性上略有提高。

對比現(xiàn)有基于LOAM的另外2個SLAM方案,也同樣具備較高的軌跡精度,誤差維持在米級范圍內(nèi)。其中LeGO-LOAM在01高速公路數(shù)據(jù)集序列失效,初步分析因?yàn)槠淝岸藥錅?zhǔn)是基于特征點(diǎn)完成的,且經(jīng)過2次特征點(diǎn)提取,基于LOAM的點(diǎn)對配準(zhǔn)誤差思想來估計(jì)系統(tǒng)狀態(tài),而01序列環(huán)境特征點(diǎn)稀疏,不利于該方案的特征提取配準(zhǔn)估計(jì)。本文仍舊將其01序列的評估數(shù)據(jù)納入綜合評估,暫認(rèn)為無效。綜上所述,對于本文定位的穩(wěn)定性和精確度,特別在閉環(huán)檢測和后端全局優(yōu)化后,系統(tǒng)整體精度得到可觀的提高,證明了閉環(huán)優(yōu)化對系統(tǒng)整體性能的有效性。

由于軌跡精度和建圖效果相輔相成,下面進(jìn)行雷達(dá)點(diǎn)云地圖和網(wǎng)格(Mesh)地圖的一致性分析。圖7展示的是發(fā)生多次閉環(huán)的城鎮(zhèn)序列00,從點(diǎn)云圖效果來看,在發(fā)生位置重訪時,其閉環(huán)準(zhǔn)確率高,所構(gòu)建環(huán)境的點(diǎn)云地圖結(jié)構(gòu)無誤。根據(jù)地圖局部效果A、B所示,紋理化后的雷達(dá)點(diǎn)云基本具備了識別環(huán)境屬性的能力。地圖局部效果C、D則進(jìn)一步展示了通過點(diǎn)云網(wǎng)格化后的車輛、墻壁、灌木叢和房屋等環(huán)境信息。圖7通過坐標(biāo)系轉(zhuǎn)換,將雷達(dá)點(diǎn)云地圖和重建Mesh地圖統(tǒng)一到世界坐標(biāo)系下,進(jìn)行顯示分析,可以觀察到2種類型點(diǎn)云重建的一致性,標(biāo)識的A、B、C、D四處稀疏點(diǎn)云和稠密地圖基本重合。此外,局部效果圖為了具體顯示出三維重建效果,將點(diǎn)云顯示體積設(shè)置為0.01。從車輛、墻壁、灌木叢以及環(huán)境整體的三維重建效果來看,提出的方案可基本完成大環(huán)境下的稠密地圖重建。

圖7 07 序列點(diǎn)云與 Mesh 圖聯(lián)合效果Fig.7 Joint effect of 07 sequence point cloud and Mesh map

3.2 本地移動背包系統(tǒng)實(shí)驗(yàn)評估

本文設(shè)計(jì)和構(gòu)建了低成本高性能的BLS系統(tǒng),通過本地?cái)?shù)據(jù)集驗(yàn)證了該移動測量平臺的有效性和適用性。最終可知該系統(tǒng)整體對設(shè)定高級任務(wù)目標(biāo),在環(huán)境檢查、人機(jī)交互和城市重建方面有一定的作用和意義。

圖8展示了在室外復(fù)雜變化環(huán)境下的系統(tǒng)狀態(tài)估計(jì)效果,主要圍繞電子科技大學(xué)眾創(chuàng)空間生成軌跡和地圖,總長度約400 m。

圖8 本地室外狀態(tài)估計(jì)效果定性分析與Mesh地圖重建Fig.8 Qualitative analysis of local outdoor state estimation effect and Mesh map reconstruction

如圖8(a)衛(wèi)星實(shí)景圖所示,環(huán)境中包括空曠大道和小樹林,地形較為復(fù)雜且不平坦。由圖8(b)可見,移動測量系統(tǒng)基本構(gòu)建出環(huán)境中有關(guān)建筑物、樹木和道路的相應(yīng)特征點(diǎn)云效果,在起始、終止點(diǎn)處軌跡誤差達(dá)到厘米級。實(shí)際運(yùn)行過程中的系統(tǒng)實(shí)時性不差,基本保證人和軌跡同步一致。整體說明系統(tǒng)方案面對較復(fù)雜、非結(jié)構(gòu)性強(qiáng)的環(huán)境時,狀態(tài)估計(jì)具有較強(qiáng)的魯棒性。

如圖8(c)所示,局部效果圖中基本包含環(huán)境中樹木、建筑物和草地等結(jié)構(gòu)的顏色信息和幾何結(jié)構(gòu)信息。室外環(huán)境下,由于雷達(dá)點(diǎn)的稀疏性和受到單目相機(jī)的視野區(qū)域限制,可重建基本Mesh地圖,若需要豐富信息,需要進(jìn)一步添加點(diǎn)云地圖。

之后,為了驗(yàn)證室內(nèi)連續(xù)不同環(huán)境下本地BLS系統(tǒng)的魯棒性,完成了在室內(nèi)、走廊和樓梯3種連續(xù)不同環(huán)境的定位和地圖重建實(shí)驗(yàn)。其中紅色箭頭示意Mesh地圖下更新的當(dāng)前系統(tǒng)定位信息,整體效果如圖9所示。

(a)樓梯與走廊環(huán)境

(c)稠密地圖重建效果

室內(nèi)環(huán)境結(jié)構(gòu)主要為樓梯、走廊和室內(nèi)墻壁,從室內(nèi)走向樓梯間并完成路徑往返,總路程約50 m。由于周圍環(huán)境紋理色彩信息多為白色,環(huán)境結(jié)構(gòu)單一且重復(fù),主要查看環(huán)境重建結(jié)構(gòu)。對于樓梯間和走廊重建效果,整體基本沒有發(fā)生很大的效果偏差,結(jié)合狀態(tài)估計(jì)結(jié)果和生成的LiDAR點(diǎn)云地圖可見,路徑返回過程中地圖和軌跡發(fā)生修正,樓道輪廓直觀清晰,整體點(diǎn)云地圖中無其他雜亂點(diǎn),室內(nèi)和墻壁重建連接在一起,整體基本達(dá)到期待的重建效果。

4 結(jié)束語

本文基于多傳感器融合的方法,研究高性能低成本的移動背包測量系統(tǒng),并對于當(dāng)前多傳感器SLAM方案中存在的相關(guān)問題進(jìn)行研究與改進(jìn),對系統(tǒng)獲取各種環(huán)境條件下的準(zhǔn)確定位結(jié)果和豐富的空間信息具有重要意義?？偨Y(jié)如下:

① 研究了多傳感器融合帶來的描述一致性、位置姿態(tài)的準(zhǔn)確性和建圖的有效性等問題,按照“多傳感器關(guān)聯(lián)—位姿定位—閉環(huán)檢測—位姿地圖更新—點(diǎn)云網(wǎng)格化—三維地圖重建”的算法流程,構(gòu)建“平臺—激光慣導(dǎo)前端定位器—激光視覺后端稠密建圖”的多傳感融合SLAM方案,集成了激光雷達(dá)、單目相機(jī)和慣性測量單元等多個傳感器。

② 改進(jìn)多傳感器融合SLAM算法,提出了一種基于因子圖優(yōu)化模型的多傳感器融合算法。在前端定位模塊,構(gòu)建因子圖模型融合雷達(dá)慣導(dǎo)信息,設(shè)計(jì)構(gòu)建多個約束因子在一個局部滑動窗口中聯(lián)合優(yōu)化,得到初步定位值;針對動態(tài)物體的干擾,設(shè)計(jì)位姿再優(yōu)化,完成雷達(dá)相對位姿變換約束因子的構(gòu)建。

③ 發(fā)展一種激光-視覺后端融合方法,在后端閉環(huán)檢測圖優(yōu)化模塊,融合視覺BoW到激光SLAM閉環(huán)檢測中,將幾何-視覺閉環(huán)檢測有機(jī)結(jié)合,設(shè)計(jì)基于幾何-視覺的聯(lián)合閉環(huán)檢測方法降低系統(tǒng)累積誤差。構(gòu)建閉環(huán)約束因子,基于因子圖模型,聯(lián)合約束因子完成全局優(yōu)化,實(shí)現(xiàn)位姿地圖的更新。即使在快速運(yùn)動條件或特征不足的情況下,也能完成高精度定位并保證全局地圖一致性。同時,為了獲取更豐富的環(huán)境信息以及提高系統(tǒng)的泛用拓展性,基于三維激光雷達(dá),進(jìn)一步融合視覺信息豐富雷達(dá)點(diǎn)云的紋理特征,進(jìn)行點(diǎn)云網(wǎng)格化實(shí)現(xiàn)稀疏點(diǎn)云稠密化重建效果。

④ 完成KITTI和本地移動背包2個不同移動測量平臺的多場景實(shí)驗(yàn),針對本文融合方案與現(xiàn)有多傳感器融合方案進(jìn)行定性定量的軌跡精度分析,并實(shí)現(xiàn)點(diǎn)云地圖和三維重建地圖的一致性表達(dá),綜合分析平臺算法效果,最終驗(yàn)證系統(tǒng)算法的魯棒性和平臺的適用性。