SLAM激光點云整體精配準(zhǔn)位姿圖技術(shù)

閆 利，戴集成，譚駿祥,劉 華，陳長軍

1. 武漢大學(xué)測繪學(xué)院，湖北 武漢 430079； 2. 武漢大學(xué)測繪遙感信息工程國家重點實驗室，湖北 武漢 430079

激光掃描(light detection and ranging,LiDAR)技術(shù)作為一種重要的三維空間數(shù)據(jù)獲取技術(shù)，可以直接快速地獲取物體表面三維信息[1]，廣泛應(yīng)用于測繪[2]、機器人[3]和無人駕駛[4]等領(lǐng)域。SLAM技術(shù)源于機器人領(lǐng)域，中文譯為同步定位與制圖技術(shù)[5]。將LiDAR與SLAM相結(jié)合，通過集成LiDAR掃描儀、IMU(inertial measurement unit)等傳感器可在掃描平臺移動過程中實時獲取位置、姿態(tài)信息以及場景的空間信息。基于SLAM技術(shù)的激光掃描可以高效完成室內(nèi)空間或室外小場景的三維點云數(shù)據(jù)獲取，近年來在測繪領(lǐng)域得到了廣泛的關(guān)注和應(yīng)用[6]。

由于SLAM定位對實時性的要求，無法充分利用多余觀測信息對位姿信息和點云數(shù)據(jù)進(jìn)行優(yōu)化，致使激光點云數(shù)據(jù)中存在內(nèi)部不一致問題，影響點云后續(xù)處理與應(yīng)用。實時SLAM點云優(yōu)化，通常先對點云進(jìn)行分段處理，然后通過閉環(huán)檢測找出重疊點云構(gòu)成的閉合環(huán)，以此構(gòu)建位姿圖進(jìn)行優(yōu)化[7-10]。這類算法存在閉合環(huán)檢測錯誤的可能，且無法處理局部區(qū)域內(nèi)部的誤差[11]。測繪領(lǐng)域?qū)θS激光點云數(shù)據(jù)精度要求高，對數(shù)據(jù)實時性要求較低，可以對點云進(jìn)行離線的全局優(yōu)化，從而改善點云內(nèi)部不一致，提高點云的相對精度。目前的點云全局優(yōu)化主要針對地面靜態(tài)式掃描激光掃描點云，方法是將一站點云視為一個視圖，利用閉合環(huán)條件約束對多視點云進(jìn)行配準(zhǔn)和優(yōu)化。文獻(xiàn)[12—13]提出一種基于光束法區(qū)域網(wǎng)平差進(jìn)行點云整體配準(zhǔn)方法，但需要利用靶標(biāo)；文獻(xiàn)[14]提出了一種嚴(yán)密的閉合條件約束配準(zhǔn)方法，使用點云間的坐標(biāo)轉(zhuǎn)換參數(shù)構(gòu)建誤差方程，進(jìn)行優(yōu)化；文獻(xiàn)[15—16]提出一種通過將點云分為兩份分別配準(zhǔn)，比較轉(zhuǎn)換參數(shù)的點云配準(zhǔn)質(zhì)量的評價方式；文獻(xiàn)[17]提出了一種利用所有重疊點云對應(yīng)點作為觀測值同時解算配準(zhǔn)結(jié)果的全局優(yōu)化的算法。

SLAM點云為移動式實時掃描點云，每一幀點云為一個視圖，點云視圖數(shù)多，重疊關(guān)系復(fù)雜。為了充分利用重疊點云中的冗余信息對SLAM點云進(jìn)行精度優(yōu)化，本文提出一種基于位姿圖優(yōu)化理論[18-19]的離線整體精配準(zhǔn)方法。該方法以實時獲取的平臺軌跡的位姿信息為初始條件，通過迭代最近點算法(iterative closest point,ICP)[20]對在不同時刻掃描的重疊點云進(jìn)行配準(zhǔn)，形成對掃描平臺軌跡的約束并對其進(jìn)行優(yōu)化，利用優(yōu)化后的軌跡對點云進(jìn)行修正。為提高計算效率，本文采用分層優(yōu)化策略，即將優(yōu)化分解為局部區(qū)域優(yōu)化和整體優(yōu)化兩個層次。

1 位姿圖優(yōu)化理論基礎(chǔ)

1.1 位姿圖表達(dá)

圖優(yōu)化理論是一種將圖論和非線性優(yōu)化相結(jié)合的理論，通過將優(yōu)化問題轉(zhuǎn)化為圖形式進(jìn)行表達(dá)，從而直觀地反映出優(yōu)化變量與約束條件之間的關(guān)系，便于拓?fù)浣Y(jié)構(gòu)操作和構(gòu)造目標(biāo)函數(shù)[21]。用G={V,E}表示圖結(jié)構(gòu)，V代表圖的頂點，E表示邊。針對優(yōu)化問題，圖的頂點代表待優(yōu)化參數(shù)，邊代表參數(shù)之間的約束條件。圖優(yōu)化的目標(biāo)是最小化所有約束代價，進(jìn)而通過優(yōu)化解法獲得最佳待求參數(shù)。位姿圖(見圖1)是優(yōu)化圖的一種具體形式，本文將其用于解決具有復(fù)雜點云視圖結(jié)構(gòu)的軌跡優(yōu)化問題。

圖1 位姿圖示例Fig.1 Pose graph

1.2 優(yōu)化求解

(1)

式中，n為軌跡中的位姿總數(shù)。如果頂點vi、vj間不存在邊ei,j作為約束時，則殘差r(vi,vj,ei,j)=0。

位姿圖優(yōu)化作為一種全局優(yōu)化方法，其中每一條邊都會直接或間接對所有頂點的估計值產(chǎn)生影響，一個錯誤的約束就足以導(dǎo)致優(yōu)化質(zhì)量的下降，同時降低識別粗差的能力[22]。在構(gòu)建位姿圖的邊時，需要對大量點云進(jìn)行配準(zhǔn)，周圍環(huán)境中存在的對稱、重復(fù)結(jié)構(gòu)，噪聲，點云重疊度低等因素，都會造成配準(zhǔn)失敗。為提高解算的穩(wěn)健性，減小異常值對結(jié)果的影響，本文通過附加損失函數(shù)對式(1)進(jìn)行改進(jìn)

(2)

式中，ρH為Huber損失函數(shù)[23]。

由于目標(biāo)函數(shù)為非線性函數(shù)，可使用L-M(Levenberg-Marquardt)優(yōu)化方法[24]進(jìn)行解算，獲得掃描平臺軌跡的最優(yōu)位姿參數(shù)，進(jìn)而根據(jù)優(yōu)化后的軌跡對點云進(jìn)行修正，達(dá)到改善點云質(zhì)量的目的。

2 基于位姿圖的點云優(yōu)化

本文所使用的點云優(yōu)化方法是通過對掃描平臺的軌跡進(jìn)行全局優(yōu)化以提高點云精度，方法流程如圖2所示。過程包括數(shù)據(jù)預(yù)處理、局部區(qū)域點云優(yōu)化和整體點云優(yōu)化3部分。預(yù)處理的目的是去除對點云配準(zhǔn)貢獻(xiàn)小的點，如離群點，并估計點云配準(zhǔn)所需的法向量。預(yù)處理后按一定的時間間隔將點云和軌跡分為若干小段，每一小段視為一個局部區(qū)域，其含有若干幀點云。為了處理局部區(qū)域內(nèi)點云誤差，以單幀點云為配準(zhǔn)單元，通過外包盒尋找點云之間的重疊關(guān)系，對重疊點云進(jìn)行配準(zhǔn)。SLAM激光點云經(jīng)過實時算法的處理，點云間可認(rèn)為已經(jīng)完成粗配準(zhǔn)，可以直接使用ICP算法進(jìn)行配準(zhǔn)。利用配準(zhǔn)結(jié)果對軌跡進(jìn)行約束，構(gòu)建位姿圖對軌跡進(jìn)行優(yōu)化，完成局部區(qū)域內(nèi)點云優(yōu)化。最后將優(yōu)化后的局部區(qū)域點云合并，合并后的每一段點云視為配準(zhǔn)單元，采用類似局部區(qū)域優(yōu)化的方式完成整體點云的全局優(yōu)化。

圖2 SLAM點云整體精配準(zhǔn)方法Fig.2 Global fine registration method of SLAM point

2.1 預(yù)處理

為了提高配準(zhǔn)的可靠性和效率，首先采用距離濾波和統(tǒng)計距離閾值濾波對點云數(shù)據(jù)進(jìn)行預(yù)處理，剔除掃描距離過大點和孤立點。隨著掃描距離的增大，點云的精度和密度都會下降，同時噪聲增多，根據(jù)掃描儀的有效掃描距離和掃描環(huán)境，設(shè)置距離閾值Dmax，去除掃描距離大于Dmax的點。為了去除孤立點，避免其對之后的法向量計算造成影響，對點云進(jìn)行統(tǒng)計距離閾值濾波[25]。該方法計算每個點到最鄰近點的距離的平均值μ和標(biāo)準(zhǔn)差σ，去除距離最鄰近點距離超出μ±σ的點。最后，在預(yù)處理中計算每個點的法向量，通過對各點鄰域內(nèi)坐標(biāo)的協(xié)方差矩陣進(jìn)行特征值分解，其中最小的特征值對應(yīng)的特征向量為該點的法向量[26]。

(4)中秋節(jié)，將近正午的光景，在北平曾家舊宅的小花廳里，一切都還是靜幽幽的，屋內(nèi)悄無一人，只聽見靠右墻長條案上一架方棱棱的古老蘇鐘遲緩低郁地邁著他“嘀塔嘀嗒”的衰弱的步子，屋外，主人蓄養(yǎng)的白鴿成群地在云霄里盤旋，時而隨著秋風(fēng)吹下一片冷冷的鴿哨響，異常嘹亮悅耳，這銀笛一般的天上音樂使久羈在暗屋里的病人也不禁抬起頭來望望：從后面大花廳一排明凈的敞窗望過去，正有三兩朵白云悠然浮過蔚藍(lán)的天空。

2.2 位姿圖構(gòu)建

由于SLAM激光點云在獲取時存在定位誤差，導(dǎo)致對同一區(qū)域掃描時獲取的不同幀點云之間無法完全重合，通過對這些點云進(jìn)行配準(zhǔn)可以構(gòu)建對軌跡中位姿的約束。針對室內(nèi)環(huán)境高度結(jié)構(gòu)化的特點，本文選擇使用點-切平面ICP算法對點云進(jìn)行配準(zhǔn)[27]，通過采用最小化點到目標(biāo)點云對應(yīng)點切平面的距離完成配準(zhǔn)。作為一種兩視點云精配準(zhǔn)的方法，相比于普通ICP算法，點-切平面ICP能夠建立更精確的同名點，收斂速度較快、精度更高[28]，其算法模型為[29]

(3)

式中，si=[sixsiysiz]T是待配準(zhǔn)點云中的點；di=[dixdiydiz]T是目標(biāo)點云中的對應(yīng)點；R代表旋轉(zhuǎn)矩陣；t代表平移參數(shù)；ni=[nixniyniz]T為點si的法向量。

(4)

式中，R(q)表示四元數(shù)q對應(yīng)的旋轉(zhuǎn)矩陣；邊ei,j的實際意義為在點云ci對應(yīng)的局部坐標(biāo)系下，兩視配準(zhǔn)后的點云cj與點云ci的掃描中心的相對位姿關(guān)系。

rtran(vi,vj,ei,j)=R(qi)T(ti-tj)-pei,j

(5)

(6)

式中，rtran(vi,vj,ei,j)、rrota(vi,vj,ei,j)分別為平移和旋轉(zhuǎn)對應(yīng)的殘差；pei,j和qei,j分別為邊ei,j的平移和旋轉(zhuǎn)部分；vec(q)為四元數(shù)的虛數(shù)部分[qxqyqz]。

結(jié)合平移和旋轉(zhuǎn)殘差的總體殘差為

r(vi,vj,ei,j)=wt‖rtran(vi,vj,ei,j)‖2+

wr‖rrota(vi,vj,ei,j)‖2

(7)

式中，wt和wr分別為平移和旋轉(zhuǎn)的權(quán)重。

2.3 分層次優(yōu)化

由于SLAM激光點云包含大量的掃描幀數(shù)，且相互之間存在大量重疊，若要一次性構(gòu)建整體點云的位姿圖，需要在各幀點云間進(jìn)行大量配準(zhǔn)，導(dǎo)致優(yōu)化計算耗時過長。因此，通過將點云優(yōu)化分解為局部區(qū)域和整體兩個層次進(jìn)行，以減小構(gòu)建位姿圖時的計算量，提高算法效率，過程如圖3所示。

預(yù)處理后，將點云和軌跡按時間間隔ts分段，每一小段視為一個局部區(qū)域，其中含有若干幀點云。在局部區(qū)域?qū)哟紊线M(jìn)行點云優(yōu)化時，首先使用分段后的軌跡構(gòu)建局部區(qū)域內(nèi)的位姿圖頂點。然后，在每個分段區(qū)域內(nèi)通過對每幀點云的外包盒進(jìn)行相交檢測，找出具有重疊關(guān)系的點云，使用點到切平面ICP對重疊點云進(jìn)行配準(zhǔn)，每次成功進(jìn)行重疊點云的配準(zhǔn)后，使用2.2節(jié)中描述的方法構(gòu)建位姿圖中對應(yīng)頂點間的一條邊。局部區(qū)域內(nèi)的位姿圖構(gòu)建完成后，對位姿圖進(jìn)行解算，獲得優(yōu)化后的分段軌跡，對分段內(nèi)點云進(jìn)行修正。

完成局部層次的優(yōu)化后，對每個分段內(nèi)的點云進(jìn)行合并，結(jié)果如圖3(b)所示。以合并后的點云為配準(zhǔn)單元進(jìn)行配準(zhǔn)，計算配準(zhǔn)時建立的同名點對數(shù)與目標(biāo)點云點數(shù)的比值α作為有效重疊度，在整體點云位姿圖構(gòu)建時通過閾值αmin去除有效重疊度低的點云間的不可靠配準(zhǔn)結(jié)果。位姿圖解算后，根據(jù)優(yōu)化后的整體軌跡對點云進(jìn)行修正，完成對整體點云的優(yōu)化。

3 試驗與分析

為了驗證本文算法的有效性，本文采用Google公司的Cartographer算法[9]示例數(shù)據(jù)作為試驗數(shù)據(jù)對本文算法的效果進(jìn)行驗證。該數(shù)據(jù)包含數(shù)據(jù)1和數(shù)據(jù)2兩部分，由裝備了兩個VelodyneVLP-16LiDAR掃描儀和IMU的3D LiDAR背包獲取，掃描地點為德意志科技博物館，圖4為所用試驗數(shù)據(jù)的俯視圖。數(shù)據(jù)1為單個樓層掃描結(jié)果，數(shù)據(jù)2為3個樓層掃描結(jié)果，并包含一個大型展廳，數(shù)據(jù)具體參數(shù)如表1所示。試驗的計算機環(huán)境為IntelXeonE5-2660 v3 @2.60 GHz，128 GB內(nèi)存。

表1 試驗數(shù)據(jù)說明

在預(yù)處理階段，根據(jù)掃描儀的性能和室內(nèi)環(huán)境的尺度，將點云的最大有效距離閾值Dmax設(shè)為20 m，去除超出距離閾值的點。局部區(qū)域優(yōu)化時，按時間間隔ts對預(yù)處理后的點云和軌跡進(jìn)行分段，分別在每段點云內(nèi)部通過ICP配準(zhǔn)構(gòu)建局部區(qū)域的位姿圖。在位姿圖殘差計算時，參考Cartographer算法[9]中的設(shè)置，將平移和旋轉(zhuǎn)代價的權(quán)值wt、wr分別設(shè)為1、100，將每段點云中第1幀點云對應(yīng)的頂點設(shè)為基準(zhǔn)，使用L-M算法進(jìn)行解算。整體點云優(yōu)化時，對局部區(qū)域內(nèi)優(yōu)化后的點云進(jìn)行合并，在整體點云范圍內(nèi)搜索重疊點云進(jìn)行配準(zhǔn)，為了保證配準(zhǔn)的可靠性，將最小有效重疊度αmin的值設(shè)為0.05，進(jìn)行整體點云的全局位姿圖構(gòu)建。

圖3 分層次配準(zhǔn)過程(每種顏色代表一個分段區(qū)域內(nèi)的點云)Fig.3 The process of hierarchical registration(each color represents the point cloud in one segmentation

圖4 Google Cartographer算法德意志博物館示例數(shù)據(jù)Fig.4 Google Cartographer sample data set in Deutsches Museum

根據(jù)優(yōu)化后獲得的軌跡對點云進(jìn)行修正，得到整體精配準(zhǔn)后的點云。以時間間隔ts=3 s時的點云優(yōu)化結(jié)果為例，圖5、圖6為點云數(shù)據(jù)優(yōu)化前后場景的定性對比。從圖5(a)、(b)中1，(c)、(d)中5，圖6(c)、(d)中5、6，(e)、(f)中8、9的對比可知，優(yōu)化后點云中的物體輪廓更加清晰；從圖5(a)、(b)中3、4，(c)、(d)中6，圖6(c)、(d)中7，(e)、(f)中10、11的對比可知，點云中的墻面的厚度變??；從圖5(a)、(b)中2，圖6(a)、(b)中1、2、3、4的對比可知，同一物體存在多個掃描結(jié)果的現(xiàn)象也得到了消除。

圖6 數(shù)據(jù)2優(yōu)化前后對比Fig.6 Data 2 scenes before and after optimization

(8)

(9)

為了分析不同的分段時間間隔ts取值對點云優(yōu)化的用時和優(yōu)化精度的影響，以1 s為間隔對ts取值進(jìn)行點云全局優(yōu)化，統(tǒng)計優(yōu)化用時，并計算優(yōu)化前后整體點云所有重疊幀間的對應(yīng)點對距離RMSE，結(jié)果如圖7和表2所示。

圖7 不同時間間隔ts取值下優(yōu)化用時Fig.7 The time cost before and after optimization in different values of ts

Tab.2TheRMSEbeforeandafteroptimizationindifferentvaluesofts

分段時間間隔ts/s數(shù)據(jù)1的RMSE/cm數(shù)據(jù)2的RMSE/cm優(yōu)化前優(yōu)化后優(yōu)化前優(yōu)化后110.54.814.26.6210.54.514.36.7310.74.314.46.7410.94.414.46.8511.04.514.57.0平均值10.74.514.46.8

由圖7(a)、(b)可以看出，局部優(yōu)化用時隨分段時間間隔ts的增大近似線性增長，而整體優(yōu)化用時則隨著時間間隔ts的增大，開始時快速減小，之后減小速度逐漸減慢。在時間間隔ts等于2 s時，數(shù)據(jù)1和數(shù)據(jù)2優(yōu)化總用時最少。從表2中可以看出，在不同的時間間隔ts下，點云的精度計算結(jié)果沒有明顯變化，說明了本文方法的優(yōu)化效果受不同時間間隔ts的影較小。其中，優(yōu)化前的RMSE的變化，是由于時間間隔ts的改變，計算得到的分段點云不同，導(dǎo)致獲取的同名點對不同造成的。數(shù)據(jù)1優(yōu)化后，不同時間間隔下平均點云RMSE從10.7 cm變?yōu)?.5 cm，減小了58%，在ts=3 s時優(yōu)化后RMSE最小為4.3 cm；數(shù)據(jù)2優(yōu)化后，不同時間間隔下平均點云RMSE從14.4 cm變?yōu)?.8 cm，減小了53%，在ts=1 s時，優(yōu)化后RMSE最小為6.6 cm，說明優(yōu)化后點云精度明顯提高，驗證了本文方法的有效性。

4 結(jié) 論

基于SLAM技術(shù)的激光掃描可以高效地獲取目標(biāo)場景的三維信息，但精度受限，可通過全局優(yōu)化的方法提高SLAM激光點云的精度。由于SLAM激光點云具有重疊度高，幀數(shù)多的特點，導(dǎo)致配準(zhǔn)關(guān)系復(fù)雜，計算量大，傳統(tǒng)的點云優(yōu)化方法不適用于SLAM激光點云的全局優(yōu)化。本文針對SLAM激光點云相對精度優(yōu)化問題，提出了一種分層次的點云整體精配準(zhǔn)算法。通過對獲取的點云進(jìn)行分段將全局位姿圖的構(gòu)建分為兩個層次。首先在分段內(nèi)部使用點-切平面ICP進(jìn)行配準(zhǔn)，構(gòu)建局部位姿圖對點云進(jìn)行優(yōu)化，保證分段內(nèi)部點云的一致性，再對分段點云進(jìn)行配準(zhǔn)，構(gòu)建全局位姿圖，對整體點云進(jìn)行優(yōu)化。通過試驗，對算法的可行性和有效性進(jìn)行了驗證。試驗結(jié)果表明，優(yōu)化后點云同名點對間的距離中誤差減小了約50%，點云中重復(fù)掃描間的差異減小，物體輪廓更加清晰，說明點云的內(nèi)部一致性得以改善。