基于全局特征地圖的由粗到細點云配準算法

摘要：針對現有點云配準算法在非重復掃描式激光雷達上存在精度低、魯棒性差、通用性差等問題，提出一種基于全局特征地圖的由粗到細點云配準算法（CTF-ICP），并實現非重復掃描式激光雷達里程計。該算法利用高斯分布表征局部點云分布，構建全局特征地圖。配準階段包含粗配準和精配準。首先，采用正態分布變換在連續點云幀之間實現幀到幀的粗配準；然后，根據粗配準的結果將當前點云映射到全局特征地圖，并將對應位置的全局特征協方差矩陣的特征值進行歸一化，實現幀到地圖的精配準；最后，將該文算法與其他常用的配準算法進行對比實驗。實驗結果表明：該文算法能夠較好地適應非重復掃描式激光雷達，配準精度和速度比常用的配準算法都有明顯提升；同時，消融實驗證明了由粗到細的點云配準算法以及全局特征地圖的有效性。

關鍵詞：全局特征地圖；由粗到細點云配準算法；非重復掃描式激光雷達里程計；高斯分布；正態分布變換

中圖分類號：TP391" " " " " " "文獻標志碼：A" " " " " "文章編號：1674-2605（2024）02-0003-08

DOI：10.3969/j.issn.1674-2605.2024.02.003

Coarse-to-fine Point Cloud Registration Algorithm Based on" " " " " " " "Global Feature Maps

LI Wen" LIN Xubin

（Guangdong University of Technology， Guangzhou 510006， China）

Abstract： Aiming at the problems of low accuracy， poor robustness， and poor universality of existing point cloud registration algorithms on non-repetitive scanning LiDAR， a coarse-to-fine point cloud registration algorithm based on global feature map （CTF-ICP） is proposed， and a non-repetitive scanning LiDAR odometer is implemented. This algorithm uses Gaussian distribution to characterize the local point cloud distribution and construct a global feature map. The registration stage includes coarse registration and fine registration. Firstly， using normal distribution transformation to achieve frame to frame coarse registration between continuous point cloud frames; Then， based on the coarse registration results， the current point cloud is mapped to the global feature map， and the eigenvalues of the global feature covariance matrix at the corresponding positions are normalized to achieve precise frame to map registration; Finally， a comparative experiment will be conducted between the algorithm proposed in this article and other commonly used registration algorithms. The experimental results show that the algorithm proposed in this paper can adapt well to non-repetitive scanning LiDAR， and the registration accuracy and speed are significantly improved compared to commonly used registration algorithms; Meanwhile， ablation experiments have demonstrated the effectiveness of the coarse-to-fine point cloud registration algorithm and the global feature map.

Keywords： global feature map; coarse-to-fine point cloud registration; non-repetitive LiDAR odometry; Gaussian distribution; normal distribution transformation

0 引言

激光雷達里程計是即時定位與地圖構建（simul-taneous localization and mapping， SLAM）的重要技術之一，通常被視為點云配準問題，其核心是通過點云配準連續地估計兩幀點云之間的位姿變換，進而累計得到里程計。

目前，激光雷達可分為重復掃描式和非重復掃描式2大類。兩者的區別在于當激光雷達靜止時，重復掃描式激光雷達每次掃描得到的點云相同，而非重復掃描式激光雷達每次掃描得到的點云都存在差異。非重復掃描式激光雷達Livox MID-360靜止掃描的連續兩幀點云如圖1所示。其中，連續兩幀點云分別用紅色和藍色表示。

由圖1（a）可以觀察到，非重復掃描式激光雷達掃描點云不一致的表現形式可分為2類：第一類，激光雷達掃描區域不重疊，即對于連續兩幀點云，在三維空間的某個局部，只有其中的一幀存在點云，而另一幀不存在，如圖1（b）所示，在該局部紅色點云和藍色點云覆蓋的區域不重疊，各自集中分布；第二類，激光雷達掃描同一局部的點云位置不穩定，即連續幀在該局部都有點云，但點云的位置不一致，如圖1（c）所示，在該局部紅色點云和藍色點云都存在，但不重合。

目前，點云配準算法根據配準流程可分為直接配準法、基于特征的配準法、基于深度學習的配準法。其中，直接配準法不需要對點云進行特征提取，直接構建優化函數，求解位姿變換，經典算法有迭代最近點（iterative closest point， ICP）[1-2]、正態分布變換（normal distributions transform， NDT）[3]以及它們的變種[4-8]等，該類算法對激光雷達類型的適應性較強，但精度和魯棒性相對較差；基于特征的配準法先在點云中提取特征點，再建立特征點之間的對應關系，通過優化特征點的距離求解位姿變換，經典算法有激光雷達里程計和建圖（LiDAR odometry and mapping， LOAM）[9]、多尺度最小二乘（multi-metric linear least square， MULLS）的激光SLAM[10-12]，但其通用性較差，需要針對不同類型的激光雷達進行相應的配置，才能有效提取特征點，且提取特征點耗時；基于深度學習的配準法利用網絡提取特征點，或直接進行端到端地學習，估計兩個點云的位姿變換[13-14]，但該方法對算力要求較高，難以實際部署。

在實際應用中，單一配準算法的效果相對較差，故形成了分步式的點云配準算法。先通過粗配準提供初始位姿估計，再通過精配準實現精準的點云配準，解決了配準過程中關聯點對應困難、迭代初始要求高等問題。于微波等[15]先利用點云輪廓特征點進行粗配準，再通過廣義迭代最近點（generalized-ICP， GICP）進行精配準，提高了點云配準速度和精度。胡加濤等[16]先利用點云輪廓特征點進行粗配準，再根據法向量進行特征匹配，并通過ICP求解精配準的位姿變換。申鴻等[17]根據室內環境存在大量直線特征數據的特點，建立角度柱狀圖數據，并根據角度柱狀圖的特征信息進行粗配準，解決了ICP算法初始要求高的問題。然而上述算法大多是基于重復掃描式激光雷達提出的，非重復掃描式激光雷達仍存在精度差等問題。

為此，本文提出一種基于全局特征地圖的由粗到細點云配準算法。針對第一類點云不一致的問題，利用局部點云協方差構建一個全局特征地圖，隨著點云不斷地累計擴張和地圖更新，盡可能地保證每個掃描區域都有更新的點云信息；針對第二類點云不一致的問題，采用由粗到細的配準策略，粗配準和精配準階段都以高斯分布表示局部點云，雖然每次掃描的點云位置不同，但對于同一局部，點云在分布理論上具有一致性。

1 由粗到細的點云配準算法

基于全局特征地圖的由粗到細點云配準算法框架如圖2所示。

首先，利用由粗到細的點云配準算法進行連續幀之間的位姿變換估計；然后，以估計的位姿構建和更新全局特征地圖。

1.1" 由粗到細的點云配準

由粗到細的點云配準包括粗配準和精配準2個階段。其中，粗配準階段提供精配準階段的初始位姿估計；精配準階段得到當前幀點云與全局特征地圖的精確位姿變換。

1.1.1" 粗配準階段

粗配準階段采用幀到幀的配準。首先，利用上一時刻點云的全局位姿將當前幀點云轉換到全局坐標系下；然后，利用NDT將當前幀點云與上一幀點云進行幀到幀的粗配準，得到全局坐標系下待配準點云的初始位姿，作為精配準階段的初始位姿。

1.1.2" 精配準階段

精配準階段采用當前幀到地圖的配準。對當前幀進行體素化，每個體素格內用一個高斯分布表示點云。采用特征地圖，點云由一系列的高斯分布表示。假設局部點云為平面結構，基于歸一化的局部協方差矩陣將當前幀點云與全局特征地圖進行精配準。全局特征地圖只需提供體素格點云的均值和協方差，可減小計算開銷，同時降低噪聲影響。

因為點云是基于物體表面的采樣，所以假設每個網格的局部點云都為平面點，將每個體素格的協方差特征值進行歸一化表示，即特征值表示為，其中是一個遠小于1的常數，設置為，建立局部點云平面。假設每個體素格的協方差特征為

（1）

經過歸一化處理的協方差為

（2）

式中：是的特征向量組成的矩陣，的對角元素為的特征值。

精配準階段利用粗配準的初始位姿對當前幀點云進行位姿變換，并投票到全局特征地圖，找到對應體素格的均值和協方差。

當前點與對應體素格均值的空間距離為

（3）

當前幀點云相對上一幀點云的位姿變換優化函數為

（4）

式中：為當前幀在全局特征地圖中相對上一時刻的位姿變換矩陣，采用高斯-牛頓迭代法求解；為當前幀點云精配準的數量；為通過公式（2）歸一化后的全局特征地圖體素格的協方差矩陣，表示體素格的平面。

最終通過位姿變換得到當前時刻點云的全局位姿：

（5）

1.2" 全局特征地圖構建和更新

1.2.1" 全局特征地圖構建

開始時，激光雷達里程計沒有全局特征地圖，需構建一個全局特征地圖。將點云體素化，并計算每個體素格的均值和協方差，計算公式為

（6）

（7）

式中：K為當前網格中點的數量，為網格中的點。

為了提高體素格的搜索效率，避免耗時過多，每個體素格都采用Hash編碼，使每個點根據自身的三維坐標直接定位到網格編號。

1.2.2" 全局特征地圖更新

在當前幀完成配準后，需要用其對全局特征地圖進行擴充和更新，即更新點云體素格的均值和協方差。為了提高效率和魯棒性，只對新增點云數量超過5個的體素格進行更新，新增點云數量不足5個的體素格因信息量有限且協方差對噪聲敏感，不進行更新，但會累積保存新增的點云，直到點云數量大于5個。體素格的均值和協方差的更新公式分別為

（8）

（9）

式中和分別為更新后對應體素格的點云均值和協方差；和分別為體素格原來的點云數量和新增的點云數量；和分別為體素格原來的點云均值和新增的點云均值，采用公式（6）進行計算；和分別為體素格原來的點云協方差和新增的點云協方差，采用公式（7）進行計算。

2 實驗

由于目前沒有用于測試和算法評估的非重復掃描式激光雷達的公開數據集。因此，本文利用地面移動機器人搭載Livox MID-360激光雷達系統，自建了非重復掃描式激光雷達點云序列，共包含5個數據集，如表1所示。

數據集包含室內、室外、靜止和連續移動等不同場景。對于室外連續移動的數據集，難以直接獲取里程計真值（ground truth， GT），因此本文采用精度更高的融合算法得到里程計真值。具體地，本文以Livox MID-360官方推薦的激光雷達和慣性傳感器（inertial measurement unit， IMU）融合的LIO_Livox算法，得到的里程計結果作為評估算法的真值。

實驗在筆記本電腦上進行，核心配置如下： Intel Core i7-12700HQ@2.30 GHz CPU，16 GB RAM，操作系統為Linux，Ubuntu20.04。

采用連續配準累計的激光雷達里程計絕對軌跡誤差（absolute trajectory error， ATE）[18-19]精度評估配準算法。目前，很多開源的基于特征的算法都無法適用于非重復掃描式激光雷達，因此對比算法選用直接配準法中的開源算法，包括ICP、NDT、GICP、體素化的廣義迭代最近點（voxelized GICP， VGICP）[6]。

本實驗包含靜止激光雷達點云配準分析、運動激光雷達點云配準分析、配準耗時評估和對比、消融實驗4部分。

2.1" 靜止激光雷達點云配準分析

2.1.1" 靜止激光雷達里程計漂移對比

為測試本文算法對非重復掃描式激光雷達點云不一致性的魯棒性，將激光雷達靜止，利用ICP、NDT、GICP、VGICP、本文算法CTF-ICP分別在Static-1、Static-2數據集上進行連續配準，獲得里程計結果分別如圖3、4所示。其中圖3（b）、4（b）是靜止激光雷達里程計在靜止位置（坐標原點）的局部放大圖。

由圖3和圖4可以看出：利用ICP、NDT、GICP和VGICP算法估計的位姿都有明顯漂移，其中NDT算法漂移最明顯；而本文算法CTF-ICP僅在原點附近有較小的波動，說明本文算法CTF-ICP對非重復掃描式激光雷達的點云不一致具有較好的魯棒性，不會因為激光雷達的非重復掃描形式而產生漂移。

2.1.2" 靜止激光雷達配準建圖對比

為了更加直觀地展現不同點云配準算法的差異，利用點云配準的結果將點云進行拼接，通過累計得到地圖。由圖3、4可以看出，在Static-1和Static-2數據集中，除了本文算法CTF-ICP外，Static-1中GICP漂移較小 Static-2中VGICP漂移較小，因此只需在Static-1和Static-2數據集上分別對比本文算法CTF-ICP與GICP、VGICP的建圖效果。本文算法CTF-ICP與GICP在Static-1數據集上的建圖效果分別如圖5、6所示，本文算法CTF-ICP與VGICP在Static-2數據集上的建圖效果分別如圖7、8所示。

由圖5～8可以看出：本文算法CTF-ICP構建的點云圖更加清晰，說明了本文算法更加準確；而GICP和VGICP構建的點云圖較模糊，尤其是圖6的室內環境；圖8的室外環境由于累計時間有限，清晰度的區別沒有室內環境明顯，但是依然能夠看出靠近邊緣的物體有重影，如樹木。由此可見，本文算法CTF-ICP的配準精度更高，通過配準拼接的點云重合度更高，點云清晰。

2.2" 運動激光雷達點云配準分析

1） 為進一步評估本文算法CTF-ICP的配準精度，利用地面移動機器人在連續移動的情況下采集點云進行配準，并不斷累計實現里程計，即在Dynamic-1、Dynamic-2、Dynamic-3數據集上進行實驗，并與ICP、NDT、GICP、VGICP等算法進行對比實驗，實驗結果如圖9～11所示。

由圖9～11可以看出，本文算法CTF-ICP的里程計結果與真值誤差更小，而對比算法有明顯的累計漂移。此外，對里程計誤差進行了量化對比，里程計精度ATE對比如表2所示。

由表2可以看出，本文算法CTF-ICP降低了里程計誤差，在Dynamic-2數據集上，ATE降低了30倍以上。另外，本文算法CTF-ICP在Dynamic-1數據集的誤差明顯高于其他2個數據集，并且其他算法也存在同樣的問題。這是因為Dynamic-1數據集有一條筆直的走廊，采用激光雷達點云配準算法存在退化現象。

2） 利用點云配準的結果對點云建圖，表2中除本文算法CTF-ICP外，GICP精度最高，因此僅與GICP進行對比實驗，結果如圖12～17所示。

由圖12、14、16可以看出，本文算法CTF-ICP構建的地圖更清晰，而GICP構建的地圖較模糊。

由圖13、15、17可以看出，本文算法CTF-ICP的配準精度更高。

2.3" 配準耗時評估和對比

本文算法的主要步驟包括輸入點云的預處理（降采樣）、幀到幀粗配準、幀到地圖精配準、地圖更新等。在Dynamic-3數據集中統計每一幀的配準耗時，結果如圖18、表3所示。

由圖18可以看出，本文算法單次配準總耗時不超過60 ms。

由表3可知，本文算法平均總耗時為22.00 ms。其中耗時最多的是幀到幀粗配準，耗時超過一半，幀到地圖精配準和地圖更新耗時較小，兩部分的平均總耗時不足7 ms。

為了進一步評估算法的配準速度，在Dynamic-1、Dynamic-2、Dynamic-3數據集上統計5種點云配準算法的每秒配準幀數（f/s），所有算法都采用單線程模式，結果如表4所示。

由表4可知，本文算法CTF-ICP的配準速度最快，并且遠超其他4種對比算法。

2.4" 消融實驗

本文算法CTF-ICP的關鍵步驟為幀到地圖精配準和全局特征地圖構建更新。為證明這個步驟各自獨立的有效性，在本文提出的框架中，分別去除幀到地圖的精配準和全局特征地圖構建更新，結果如表5所示。

由表5可以看出，無論是去除幀到地圖精配準，還是去除全局特征地圖構建更新，配準效果都明顯下降，說明本文提出的幀到地圖精配準和全局特征地圖構建更新2個核心部分都是有效的。

3 結論

針對現有的點云配準算法大多基于重復掃描式激光雷達提出，對非重復掃描式激光雷達點云配準精度較低的問題，本文提出一種基于全局特征地圖的由粗到細點云配準算法（CTF-ICP），以高斯分布表征全局地圖，并通過由粗到細的配準算法實現點云精準配準。通過實驗驗證：該算法在測試數據集上平均里程計誤差低至0.81%；配準速度最高可達112.37 f/s，最低可達40.91 f/s；在配準精度和速度上都超過現有常用的點云配準算法，且能夠很好地適應非重復掃描式激光雷達。本文算法只涉及前端里程計，但實際使用通常需要配合后端優化，而后端優化需要對歷史的位姿進行糾正，因此下一步工作將深入研究如何高效地對全局特征地圖進行糾正。

參考文獻

[1]ARUN K S， HUANG T S， BLOSTEIN S D. Least-squares fitting of two 3-D point sets[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence， 1987，9 （5）：698-700.

[2]胡成放，丁昊昊，陳德君，等.基于雙目視覺的列車輪對表面缺陷及型面參數檢測方法[J/OL].中國測試，2023：1-9[2023-10-07]. https：//link.cnki.net/urlid/51.1714.TB.20231007.0948.002.

[3]BIBER P， STRASSER W. The normal distributions transform： A new approach to laser scan matching[C]. Proceedings 2003 IEEE/RSJ International on Intelligent Robots and Systems （IROS 2003）， 2003，3： 2743-2748.

[4]HUANG J， TAO B， ZENG F. Point cloud registration algorithm based on ICP algorithm and 3D-NDT algorithm[J]. Interna-tional Journal of Wireless and Mobile Computing， 2022，22（2）： 125-130.

[5]SEGAL A， HAEHNEL D， THRUN S. Generalized-ICP[C]. Robotics： Science and Systems， 2009，2（4）：435.

[6]KOIDE K， YOKOZUKA M， OISHI S， et al. Voxelized GICP for fast and accurate 3D point cloud registration[C]. 2021 IEEE International Conference on Robotics and Automation （ICRA）， 2021：11054-11059.

[7]WANG J， XU M， FOROUGHI F， et al. FasterGICP： Accep-tance-rejection sampling based 3D LiDAR odometry[J]. IEEE Robotics and Automation Letters， 2022，7（1）：255-262.

[8]YOKOZUKA M， KOIDE K， OISHI S， et al. LiTAMIN2： Ultra light LiDAR-based SLAM using geometric approximation applied with KL-divergence[C]. 2021 IEEE International Con-ference on Robotics and Automation （ICRA）， 2021：11619-11625.

[9]ZHANG J， SINGH S. Low-drift and real-time LiDAR odometry and mapping[J]. Autonomous Robots， 2017，41（2）： 401-416.

[10]PAN Y， XIAO P， HE Y， et al. MULLS： Versatile LiDAR SLAM via multi-metric linear least square[C]. 2021 IEEE International Conference on Robotics and Automation （ICRA），" 2021： 11633-11640.

[11]吳耀威，吳自然，趙宇博，等.基于深度相機與激光雷達數據融合SLAM方法研究[J].機電工程技術，2023，52（11）：174-179.

[12]范海廷，杜云剛.基于激光SLAM的移動機器人導航算法研究[J].機床與液壓，2021，49（14）：41-46.

[13]ZHENG C， LYU Y， LI M， et al. LodoNet： A deep neural network with 2D keypoint matching for 3D LiDAR odometry estimation[C]. Proceedings of the 28th ACM International Conference on Multimedia， 2020： 2391-2399.

[14]HORN M， ENGEL N， BELAGIANNIS V， et al. DeepCLR： Correspondence-less architecture for deep end-to-end point cloud registration[C]. 2020 IEEE 23rd International Confe-rence on Intelligent Transportation Systems （ITSC）， 2020：1-7.

[15]于微波，錢柏竹，楊宏韜，等.基于輪廓點的三維點云配準[J]. 組合機床與自動化加工技術，2022（12）：1-5.

[16]胡加濤，吳曉紅，何小海，等.一種基于幾何特征由粗到細點云配準算法[J].科學技術與工程，2020，20（5）：1947-1952.

[17]申鴻，李平，賈丙佳.采用特征預處理ICP算法的機器人運動環境建圖[J].華僑大學學報（自然科學版），2022，43（2）：229-236.

[18]GEIGER A， LENZ P， URTASUN R. Are we ready for autonomous driving？ The KITTI vision benchmark suite[C]. 2012 IEEE Conference on Computer Vision and Pattern Recognition， 2012：3354-3361.

[19]周凱凌，顏禧烽，張建民，等.室內導盲機器人的設計與實現[J].機電工程技術，2021，50（8）：36-39.

作者簡介：

李文，男，1998年生，碩士研究生，主要研究方向：點云配準。E-mail： gdutfsat_lw@163.com

林旭濱，男，1992年生，博士研究生，主要研究方向：即時定位與地圖構建。E-mail： xubin_lin@163.com