SLAM激光點云整體精配準位姿圖技術

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

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

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

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

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

1 位姿圖優化理論基礎

1.1 位姿圖表達

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

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

1.2 優化求解

(1)

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

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

(2)

式中，ρH為Huber損失函數[23]。

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

2 基于位姿圖的點云優化

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

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

2.1 預處理

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

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

2.2 位姿圖構建

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

(3)

式中，si=[sixsiysiz]T是待配準點云中的點；di=[dixdiydiz]T是目標點云中的對應點；R代表旋轉矩陣；t代表平移參數；ni=[nixniyniz]T為點si的法向量。

(4)

式中，R(q)表示四元數q對應的旋轉矩陣；邊ei,j的實際意義為在點云ci對應的局部坐標系下，兩視配準后的點云cj與點云ci的掃描中心的相對位姿關系。

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

(5)

(6)

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

結合平移和旋轉殘差的總體殘差為

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

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

(7)

式中，wt和wr分別為平移和旋轉的權重。

2.3 分層次優化

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

預處理后，將點云和軌跡按時間間隔ts分段，每一小段視為一個局部區域，其中含有若干幀點云。在局部區域層次上進行點云優化時，首先使用分段后的軌跡構建局部區域內的位姿圖頂點。然后，在每個分段區域內通過對每幀點云的外包盒進行相交檢測，找出具有重疊關系的點云，使用點到切平面ICP對重疊點云進行配準，每次成功進行重疊點云的配準后，使用2.2節中描述的方法構建位姿圖中對應頂點間的一條邊。局部區域內的位姿圖構建完成后，對位姿圖進行解算，獲得優化后的分段軌跡，對分段內點云進行修正。

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

3 試驗與分析

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

表1 試驗數據說明

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

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

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

根據優化后獲得的軌跡對點云進行修正，得到整體精配準后的點云。以時間間隔ts=3 s時的點云優化結果為例，圖5、圖6為點云數據優化前后場景的定性對比。從圖5(a)、(b)中1，(c)、(d)中5，圖6(c)、(d)中5、6，(e)、(f)中8、9的對比可知，優化后點云中的物體輪廓更加清晰；從圖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的對比可知，同一物體存在多個掃描結果的現象也得到了消除。

圖6 數據2優化前后對比Fig.6 Data 2 scenes before and after optimization

(8)

(9)

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

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

Tab.2TheRMSEbeforeandafteroptimizationindifferentvaluesofts

分段時間間隔ts/s數據1的RMSE/cm數據2的RMSE/cm優化前優化后優化前優化后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)可以看出，局部優化用時隨分段時間間隔ts的增大近似線性增長，而整體優化用時則隨著時間間隔ts的增大，開始時快速減小，之后減小速度逐漸減慢。在時間間隔ts等于2 s時，數據1和數據2優化總用時最少。從表2中可以看出，在不同的時間間隔ts下，點云的精度計算結果沒有明顯變化，說明了本文方法的優化效果受不同時間間隔ts的影較小。其中，優化前的RMSE的變化，是由于時間間隔ts的改變，計算得到的分段點云不同，導致獲取的同名點對不同造成的。數據1優化后，不同時間間隔下平均點云RMSE從10.7 cm變為4.5 cm，減小了58%，在ts=3 s時優化后RMSE最小為4.3 cm；數據2優化后，不同時間間隔下平均點云RMSE從14.4 cm變為6.8 cm，減小了53%，在ts=1 s時，優化后RMSE最小為6.6 cm，說明優化后點云精度明顯提高，驗證了本文方法的有效性。

4 結 論

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