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

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

0 引言

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

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

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

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

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

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

1 相關工作

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

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

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

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

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

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

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

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

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

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

2.1 多傳感器數據融合

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

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

2.2 多傳感器融合定位里程計實現

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

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

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

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

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

(1)

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

(2)

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

(3)

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

(4)

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

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

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

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

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

(a)標定環(huán)境

(c)標定效果驗證

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

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

2.3 多傳感器融合稠密建圖

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

首先保證LiDAR和相機的數據同步,對二者進行外參標定,通過視覺與雷達的共有特征進行3D-3D點對匹配,用剛體的平移變換和旋轉變換來描述三維激光雷達和相機在空間中的相對位置關系,最終實現融合。原理如下:

(5)

式中:(u,v)表示由相機捕獲的圖像數據,(x,y,z)表示激光雷達捕獲的三維點陣云,其目標是建立一個轉化矩陣M,將三維點映射到二維點 。

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

(6)

得出標定后的R、T矩陣后,將激光雷達點云作剛體變換轉移至相機坐標系下,再投影至圖像坐標系中,最終返回有色點云至雷達坐標系中。

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

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

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

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

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

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

(7)

由T(g,i)-1p求得p在LiDAR坐標系的投影點,同時反投影得點云幀中對應像素點x。

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

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

(8)

(9)

3 實驗驗證與結論

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

3.1 公開數據集評估實驗

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

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

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

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

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

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

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

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

表1 RPE評估結果:相對平移誤差

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

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

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

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

圖7 07 序列點云與 Mesh 圖聯合效果Fig.7 Joint effect of 07 sequence point cloud and Mesh map

3.2 本地移動背包系統(tǒng)實驗評估

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

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

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

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

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

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

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

(c)稠密地圖重建效果

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

4 結束語

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

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

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

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

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