基于改進 FPFH-ICP 的車載激光雷達點云配準方法

摘要：為了改善傳統車載激光雷達點云配準方法準確度低、計算速度慢的問題，提出了一種基于快速點特征直方圖（fast point feature histograms， FPFH ）初始匹配與改進迭代最近點（iterative closest point， ICP）精確配準相結合的改進 FPFH -ICP 配準算法。配準前使用體素濾波器和 statistical-outlier-removal 濾波器進行預處理；采用 FPFH 提取點云特征，基于采樣一致性（sample consensus initial alignment， SAC-IA ）進行初始配準，為精確配準提供良好的位姿信息；建立 K-D 樹并在傳統 ICP 配準算法的基礎上添加法向量閾值，對車載激光雷達點云數據進行精確配準；在4種不同場景的實驗中，改進 FPFH -ICP 配準比 ICP 配準的均方根誤差和配準用時分別平均減少了7.56%和41.22%，比點特征直方圖（point feature histograms， PFH ）配準的均方根誤差和配準用時分別平均減少了30.28%和18.95%，表明改進的 FPFH -ICP 能夠對車載激光雷達點云數據實現精確且高效的配準。

關鍵詞：車載激光雷達；點云配準；快速點特征直方圖；法向量閾值；迭代最近點

中圖分類號：U461.91文獻標志碼：A文章編號：1000-582X（2023）05-001-10

Point cloud registration of vehicle-mounted lidar based on improved FPFH-ICP algorithm

JIANGFengyang1a， 1b ， LIUYonggang1a， 1b ， CHENZhihang1a， 1b ， CHENZheng2（1a . College of Mechanical and Vehicle Engineering;1b . State Key Laboratory of MechanicalTransmissions， Chongqing University， Chongqing 400044， P. R . China;2. Faculty of Transportation Engineering， Kunming University of Science and Technology， Kunming 650500， P. R . China）

Abstract： Tosolvetheproblemsoflowaccuracyandslowcalculatingspeedof thetraditionalpointcloud registration methods for vehicle-mounted lidar， an improved FPFH -ICP registration combining fast point feature histograms （FPFH） initialmatchingwithimprovediterativeclosestpoint （ICP） accurateregistrationwas proposed . Firstly，voxelgridandstatistical-outlier-removalfilterwereusedforpreprocessingdatabefore registration . Then， based on sample consensus initial alignment （SAC-IA）， FPFH was used for initial registration toprovidegoodposeinformationforaccurateregistration . Finally，aK-Dtreewasestablished，andanormal vector thresholdwasaddedtotraditionalICP registrationforaccurateregistration . Intheexperimentsof four different scenarios， the root mean square error and registration time of the improved FPFH -ICP registration werereducedby 7.56% and 41.22% ，respectively，comparedwithICPregistration，andby 30.28% and 18.95% ， respectively， compared with point feature histograms （PFH） registration， suggesting that the improved FPFH -ICP registration can achieve accurate and efficient registration of point cloud data of vehicle-mounted lidar.

Keywords： vehicle-mounted lidar; point cloud registration; fast point feature histograms; normal vector threshold; iterative closest point

車載激光雷達安裝在快速移動的智能汽車上，持續采集車輛前方的道路環境信息，由于對同一障礙物在不同地點、不同角度采集的點云數據存在一定的偏差，需要對車載激光雷達采集到的點云數據進行配準。同時，車載激光雷達每次掃描的點云數據量有限，距離車載激光雷達較遠處的點云密度較低；天氣、遮擋、振動及安裝誤差等原因也會降低車載激光雷達采集點云數據的精度。因此，點云配準方法需保證在低精度、低密度條件下也具有良好的適應性。當前關于點云配準的研究主要集中在正態分布變換法（normal distribution transform，NDT ）[1?2]、隨機采樣一致性法（random sample consensus，RANSAC ）[? ]34和 ICP[? ]56等配準算法的改進。

Li 等[7]提出一種基于正態分布變換單元的改進 RANSAC 方法 ， 用每個單元內的正態分布對觀察點進行建模。楊玉澤等[8]提出 FPFH 初始匹配和 NDT 精確配準結合的配準方法，在犧牲配準效率的前提下提高了精度。相較于上述方法，ICP 配準作為當前應用最廣泛的點云配準方法，應用最小二乘法迭代計算獲取最優的變換矩陣[9]。但是該算法存在計算難、效率低、依賴初始變換估計且易陷于局部最優的不足，很多學者對該算法進行了改進。Liu 等[10]設計了一種隨機策略來計算兩點云之間變換矩陣的初始值，結合 ICP 自動地實現點云配準，但該方法的魯棒性有待驗證。Li 等[11]將重疊率、角度、距離和噪聲作為 ICP 的影響因素，利用4個數據集對點對點和點對平面 ICP 的有效性、魯棒性、準確性和效率進行了評估。Shi 等[12]將 NDT 與 ICP 結合，提高了配準效率。Zaganidis等[13]提出語義輔助正態分布變換配準點云，但應用范圍有限。Li 等[14]建立高斯混合模型來擬合點云，雖提高了配準精度但降低了配準效率。

為進一步提高車載激光雷達點云配準的效率和精度，同時滿足在低精度、低密度條件下的配準需求，提出了一種基于 FPFH 初始匹配與改進 ICP 精確配準相結合的改進 FPFH -ICP 配準方法。首先對激光雷達點云數據進行預處理，并采用 FPFH提取點云特征，基于 SAC-IA 進行初始配準，然后建立 K-D 樹并在傳統 ICP 配準算法的基礎上添加法向量閾值進行精確配準，最后在不同場景下與 ICP 配準和 PFH 配準進行對比實驗，驗證了所提方法對車載激光雷達點云數據的配準效果。

1 基于 FPFH特征的初始配準

初始配準可減小點云對應點對間在平移與旋轉上的錯位，為點云的精確配準提供良好的位姿。使用體素濾波器和 statistical-outlier-removal 濾波器對點云數據進行預處理，采用 FPFH特征描述子提取點云特征，使用 SAC-IA 進行車載激光雷達點云數據的初始配準。

1.1FPFH 特征描述子

3D 點云特征描述和提取是點云數據處理最關鍵的部分，點云配準的效果直接受到特征描述和提取的影響。FPFH 特征描述子基于 PFH 進行改進，解決了 PFH在處理密集點云時效率低下的問題，提供的信息具有旋轉不變性。車載激光雷達點云數據量較龐大，采用 FPFH特征描述子進行點云特征的描述和提取，其工作原理如圖1所示[15]。

計算過程如下：

1）計算查詢點pq與其 k 鄰域點 pk 間的特征元素，該結果成為簡化的點特征直方圖；

2）重新確定每個點的 k鄰域，使用鄰近簡化點特征直方圖的值得出pq的 FPFH 。

FPFH （ pq ）= SPFH （ pq ）+SPFH （ pk ），（1）

式中：權重wk表示查詢點pq與其 k 鄰域點pk 間的距離；SPFH （simple point feature histograms）為簡化的點特征直方圖，表示查詢點pq與其 k鄰域點間對應法線構成的角度集合。

1.2SAC-IA 初始配準

SAC-IA 初始配準無需獲取點云數據對應關系的全部組合，通過 FPFH點云特征描述子從大量候選對應關系中獲取最優的初始變換矩陣估計，避免了傳統方法依賴點云數據旋轉不變性過程中出現計算困難、易陷于局部最優的弊端。SAC-IA 初始配準的過程為：

1）在源點云S 中選取 x 個樣本數據點，所選樣本數據點兩兩之間的距離應大于所設定的距離閾值ω d，以保證所選的 x個樣本數據點具備不同的FPFH 特征；

2）在目標點云 T 中找出與樣本數據點具備相似 FPFH 特征的點云數據點，對其進行存儲并進行隨機采樣，用它們之間的對應關系來代表所有樣本數據點之間的對應關系；

3）計算這些樣本數據點對的變換參數，得出平移和旋轉矩陣，并利用 Huber 評價函數判定是否為最優變換矩陣，評價函數設定為：

式中：a 為樣本數據點對變換后的距離差；ωd為超參數。

重復上述過程，當 Huber 評價函數最小時，所得變換矩陣即為最優。

2 基于改進 ICP 的精確配準

經過 SAC-IA 初始配準的點云數據已經有了較好的位姿，但要獲得更為精準的結果還需進行精確配準。 ICP 算法通過最小二乘法迭代獲取最優變換矩陣，有著計算直觀簡單的優勢，但也存在迭代次數過多易陷于局部最優的不足。改進 FPFH -ICP 配準通過在傳統 ICP 配準算法的基礎上增加 K-D 樹和法向量閾值實現車載激光雷達點云數據的精確配準。

2.1K-D 樹建立

傳統 ICP配準花費大量時間在最近點搜索上，且需要多次重復迭代，計算效率較低。K-D 樹是帶約束的二分查找樹，常用于搜索和存儲多維空間中的數據點，可降低 ICP在查詢最近點時的計算復雜度，加速 ICP 配準的迭代過程，其工作原理如圖2所示。

2.2 法向量閾值

需要進行精確配準的車載激光雷達源點云與目標點云間的拓撲關系是一致的，因此，可選用對應點對的法向量夾角來判定是否為點云數據正確的對應點對sp與tq。從車載激光雷達源點云數據集 S 和目標點云數據集 T 中，計算各匹配點法向量夾角的余弦值 cos θ，當大于法向量閾值ωv時，則認定為正確的匹配點對，否則剔除錯誤點對。由此便提高了車載激光雷達點云的配準精度，剔除錯誤點對在一定程度上也加快了配準速度。

式中：θ為對應點對sp和tq法向量 np 與 nq 的夾角；ωv為設定的法向量閾值。

2.3 基于改進 ICP 精確配準原理

點云配準的目的是源點云數據集 S 和目標點云數據集 T 通過變換矩陣（ R ，t ）變換后，使源點云與目標點云中的數據點對應。

式中：t為平移矩陣；R 為旋轉矩陣。

當目標函數

為最小值時，t與 R 為最優，這里的 n 為目標點云數據點的個數。

改進后的 ICP精確配準對初始變換后的源點云和目標點云建立 K-D 樹加速近鄰搜索，通過法向量閾值ω v 剔除錯誤的點對，使用奇異值分解法計算變換矩陣（ R ，t ），使目標函數 E（ R ，t ）最小，循環迭代從而獲得最優的配準效果，其工作原理如圖3所示。

3 實驗結果與分析

KITTI 數據集是當前自動駕駛領域廣泛應用的計算機視覺算法評測數據集。從 KITTI 實車采集的數據集中選取4個不同的場景，對預處理后的點云數據分別采用 ICP、PFH 和改進 FPFH -ICP 進行車載激光雷達點云數據配準，驗證所提算法的配準效果。

3.1 點云數據預處理

車載激光雷達采集到的點云數據摻雜了部分噪聲點和離群點，而且龐大的點云數據量會降低計算機的處理速度，所以在點云配準前需進行預處理過程[16]。基于體素濾波器對激光雷達點云數據進行下采樣，采用 statistical-outlier-removal 濾波器剔除偏移有效點云的離群點，結果如圖4所示。

由圖4可知，車載激光雷達點云數據經預處理后明顯變得更加稀疏，源點云數據點的個數由120119變為6400，目標點云數據點的個數由119510變為6431，在不改變原有數據結構的前提下精簡了數據，而且從圖中的圓圈標記位置看出，點云數據經預處理后有效剔除了離群點，為提升點云配準效果奠定了基礎。

3.2 點云配準結果

點云配準的效率和精度是評估點云配準效果的重要指標，配準效率由配準時間評估，配準精度采用均方根誤差（root mean square error，RMSE ）進行評估。

式中：ns 為源點云數據點的個數；ei為源點云與目標點云中對應點對間的距離誤差；si為源點云中的數據點，依賴剛體變換矩陣變換到目標點云中；ti為目標點云中的與si對應的數據點。

場景1獲取的車載激光雷達點云數據有著旋轉偏移量較小而平移偏移量較大的特點，初始源點云與目標點云的數據點數量分別為120308和120636，將預處理后的點云數據分別采用 ICP、PFH 和改進 FPFH -ICP 進行配準，結果如圖5所示。

由圖5和表1得出，在場景1中改進 FPFH -ICP 配準均方根誤差為0.0374 m，其精度優于傳統 ICP 配準和 PFH 配準，配準時間為15.163 s，比 ICP 配準和 PFH 配準所用配準時間少。由此可得出，改進 FPFH -ICP 配準適用于在車載激光雷達點云數據平移偏移量較大而旋轉偏移量較小的場景。

與場景1不同，場景2的車載激光雷達點云數據有著平移偏移量較小而旋轉偏移量較大的特點，初始源點云和目標點云數據點的數量分別為120119和119510，將預處理后的點云數據分別采用 ICP、PFH 和改進 FPFH -ICP 進行配準，結果如圖6所示。

由圖6和表2得出，采用 ICP 配準和改進 FPFH -ICP 配準的源點云與目標點云配準效果良好，配準精度均優于 PFH 配準，但改進 FPFH -ICP 的配準效率更高，所用配準時間最短。由此可得出，在平移偏移量較小而旋轉偏移量較大的場景中，改進 FPFH -ICP 配準效果最佳。

場景3選取自彎道處的車載激光雷達點云數據，平移和旋轉偏移量均較小。初始源點云與目標點云的數據點數量分別為118679和118464，將預處理后的點云數據分別采用 ICP、PFH 和改進 FPFH -ICP 進行配準，結果如圖7所示。

由圖7和表3得出，在場景3中改進 FPFH -ICP 配準的均方根誤差為0.0531 m，ICP 配準的精度與 FPFH - ICP 接近，但 ICP 配準所消耗的時間最長，配準效率最低，PFH 配準的精度最差。改進 FPFH -ICP 同樣適用于彎道處平移和旋轉偏移量均較小的車載激光雷達點云數據的配準。

場景4采用了較為稀疏的點云數據，以此來驗證當車載激光雷達在低精度、低密度條件下的配準效果。初始源點云的數據點的數量為20911，目標點云的數據點的數量為20765，不同配準方法的配準效果如圖8和表4所示。

由圖8和表4得出，在場景4中改進 FPFH -ICP 配準的均方根誤差為0.0599 m，配準精度最高；所用配準時間最短，不到 ICP 配準時間的50%。因此，改進 FPFH -ICP 配準可以較好地適應低精度、低密度條件。

圖9表明，改進 FPFH -ICP 配準方法分別在不同的4個場景中RMSE 和配準時間均低于 ICP 配準和 PFH 配準，證明了提出的改進 FPFH -ICP 配準方法有較高的配準精度和配準效率，具有良好的魯棒性。

4 結論

為了改善車載激光雷達點云配準方法計算效率低、依賴初始變換估計等問題，采用 FPFH 特征，基于 SAC-IA 方法進行初始配準，在傳統 ICP 配準算法的基礎上增加 K-D 樹和法向量閾值進行精確配準。最后利用 KITTI數據集中車載激光雷達實車采集的不同場景下的點云數據，與 ICP 配準和 PFH 配準進行對比實驗。結果表明，改進 FPFH -ICP 配準相比于 ICP 配準均方根誤差平均減少7.56%，配準用時平均減少41.22%，配準效率顯著提高；相比于 PFH 配準均方根誤差平均減少30.28%，配準用時平均減少18.95%，配準精度顯著提高。由此得出，所提的改進 FPFH -ICP 配準方法能夠對車載激光雷達點云數據實現精確且高效的配準，為后續智能車的決策規劃提供良好的數據支持。

參考文獻

[1]范強 ， 劉鵬 ， 楊俊 ， 等.基于3D -Harris 與 FPFH 改進的3D -NDT 配準算法[J].圖學學報 ， 2020， 41（4）：567-575.

Fan Q ， Liu P， Yang J， et al. Improved 3D -NDT point cloud registration algorithm based on 3D -Harris and FPFH [J]. Journal of Graphics， 2020， 41（4）：567-575.（in Chinese）

[2] 袁志聰 ， 魯鐵定 ， 劉瑞.一種基于 BFGS修正的正態分布變換點云配準方法[J].測繪通報 ， 2020（10）：38-42.

YuanZC ，LuTD ，LiuR . A normaldistributiontransformpointcloudregistrationmethodbasedonBFGScorrection[J]. Bulletin of Surveying and Mapping， 2020（10）：38-42.（in Chinese）

[3] Kang G H ， Zhang Q ， Wu J Q ， et al. Pose estimation of a non-cooperative spacecraft without the detection and recognition ofpoint cloud features[J]. Acta Astronautica， 2021， 179：569-580.

[4] KoguciukD . Parallel RANSAC for point cloud registration[J]. Foundations of Computing and Decision Sciences， 2017， 42（3）：

[5] 易倩 ， 鐘浩宇 ， 劉龍 ， 等.基于 ROI-RSICP 算法的車輪廓形動態檢測[J].中國激光 ， 2020， 47（11）：154-165.

Yi Q ， Zhong H Y， Liu L ， et al. Dynamic inspection of wheel profile based on ROI-RSICP algorithm[J]. Chinese Journal of Lasers， 2020， 47（11）：154-165.（in Chinese）

[6] 王飛 ， 劉如飛 ， 任紅偉 ， 等.利用道路目標特征的多期車載激光點云配準[J].測繪科學技術學報 ， 2020， 37（5）：496-502.

WangF， LiuRF， RenHW，etal. Multi-stagevehicle-mountedlaser pointcloudregistration usingroadtargetfeatures[J]. Journal of Geomatics Science and Technology， 2020， 37（5）：496-502.（in Chinese）

[7] Li L ， Yang F， Zhu HH ， et al. An improved RANSACfor 3D point cloud planesegmentation based on normal distributiontransformation cells[J]. Remote Sensing， 2017， 9（5）：433.

[8] 楊玉澤 ， 孫英偉 ， 林文樹.基于 FPFH 特征和 NDT算法的樹木點云配準[J].西北林學院學報 ， 2019， 34（5）：141-146.

Yang Y Z ， Sun Y W， Lin W S . Tree point cloud registration based on FPFH feature and NDT algorithm[J]. Journal of Northwest Forestry University， 2019， 34（5）：141-146.（in Chinese）

[9] WuZZ ，ChenHC ，DuSY，etal. CorrentropybasedscaleICPalgorithmforrobustpointsetregistration[J]. PatternRecognition， 2019， 93：14-24.

[10] Liu J B ， Guo P Y， Sun X L . An automatic 3D point cloud registration method based on biological vision[J]. Applied Sciences，2021， 11（10）：4538.

[11] Li P， Wang RS ， Wang Y X ， et al. Evaluation of the ICP algorithm in 3D point cloud registration[J]. IEEE Access， 2020， 8：68030-68048.

[12] Shi X Y， Peng J J， Li J P， et al. The iterative closest point registration algorithm based on the normal distribution transformation[J]. Procedia Computer Science， 2019， 147：181-190.

[13] ZaganidisA ，SunL ，DuckettT，etal. Integratingdeepsemanticsegmentationinto 3-Dpointcloudregistration[J]. IEEERobotics and Automation Letters， 2018， 3（4）：2942-2949.

[14] Li Q S ， Xiong R ， Vidal-Calleja T. A GMM based uncertainty model for point clouds registration[J]. Robotics and AutonomousSystems， 2017， 91：349-362.

[15] Zheng L ， Li Z K . Virtual namesake point multi-source point cloud data fusion based on FPFH feature difference[J]. Sensors，2021， 21（16）：5441.

[16]謝德芳 ， 陳叢桂 ， 周聰 ， 等.基于 RANSAC 的點云配準優化算法[J].機電工程技術 ， 2020， 49（9）：38-39，67.

Xie D F， Chen C G ， Zhou C ， et al. Optimization algorithm of point cloud registration based on RANSAC [J]. Mechanical amp; Electrical Engineering Technology， 2020， 49（9）：38-39，67.（in Chinese）

（編輯羅敏）