基于法向量投票的點云配準方法

摘 要： "針對多平面結構的物體，傳統的點特征點云配準方法存在魯棒性差、易收斂到局部最優解等問題，提出了一種基于法向量投票的點云配準方法。用平面特征代替點特征作為配準基元，建立基于平面的坐標轉換模型。首先構建kd-tree，計算各點的法向量，并將法向量轉換到霍夫空間進行投票，提取平面特征；然后將單位四元數作為特征描述算子，以同名平面特征作為約束條件，根據最小二乘平差原則，求解點云之間的位姿變換關系。實驗結果表明：相較于其他兩種方法，提出方法對初始位置沒有依賴性，在配準過程中可以有效避免局部最小陷阱，并且配準精度得到了提高。

關鍵詞： "點云配準； 法向量； 平面特征； 四元數

中圖分類號： "TP391 """文獻標志碼： A

文章編號： "1001-3695（2022）02-056-0637-04

doi：10.19734/j.issn.1001-3695.2021.06.0236

Point cloud registration method based on normal vector voting

Zhou Ying， Lin Yi

（School of Artificial Intelligence amp; Computer Science， Jiangnan University， Wuxi Jiangsu 214122， China）

Abstract： "Aiming at the problems of poor robustness and easy convergence to local optimal solution of traditional point cloud registration methods for objects with multi-planar structures，this paper proposed a point cloud registration method based on normal vector voting.It used planar features instead of point features as registration primitives to establish a coordinate transformation model.Firstly，it constructed kd-tree，and calculated the normal vector of each point，converted the normal vector to the Hough space and voted to extract the planar features.Then，it used the unit quaternions as feature description operators，and used conjugate planes as constraints.According to the least square adjustment principle，it solved the pose transformation relationship between point clouds.Experimental results show that，compared with the other two methods，the proposed method does not depends on the initial position，and can effectively avoid local minimum trapping in the registration process，and improves the registration accuracy.

Key words： "point cloud registration； normal vector； plane feature； quaternion

0 引言

逆向工程是對產品設計過程的一種描述［1］，根據已有的產品，反向推出產品的設計和生產過程，被廣泛應用于模型設計、文物修復、醫學研究［2～4］等領域。三維掃描技術為逆向工程的實現提供了一個簡單快捷的方法，是目前常用的方法之一。用三維激光掃描儀在不同的測量站對目標產品進行掃描，得到點云數據，再對點云進行配準，得到目標產品的三維模型。其中，最重要的環節是點云配準。點云配準是求解兩片或多片點云之間的旋轉平移矩陣，使其統一到同一個坐標系中，形成一個完整的點云數據［5］，即目標產品的三維模型。目前比較成熟的點云配準方法主要有基于點的配準—最近點迭代法（iterative closest points，ICP）［6～8］、基于數字特征的配準—正態分布變換（normal distributions transform，NDT） ［9～11］和基于特征的配準三種。ICP算法是研究最多且應用最廣的一種算法［12］，它的核心思想是尋找兩片點云之間歐氏距離最短的點，以點到點的距離作為誤差方程，利用最小化均方誤差來迭代求解最優值。它的不足點在于尋求的是局部最小值和魯棒性差。對此，李傳龍等人［13］提出了基于改進的動態差分進化算法，首先根據曲率值和法向量提取特征點，再利用動態差分算法進行粗配準得到初始匹配參數，最后利用ICP進行細配準，提高了算法的搜索速度。余大梅等人［14］對原始的ICP進行了改進，推導了配準殘差分布函數權重函數，用加權點對殘差代替傳統的最近點對距離，在速度和精度上有了改進，具有很好的魯棒性。NDT算法的核心思想是將點云柵格化，計算每個體素內的概率密度函數，利用正態分布對點云進行預測。它的不足之處同樣是在沒有合適的初值情況下容易陷入局部最優。

由于平面特征的穩定性和特殊性廣泛存在于物體的表面，即使被遮擋物擋住一部分，也不影響對平面特征的提取，既降低了復雜度又提高了精度。文獻［15］提出了基于平面特征和KNNS的點云配準算法，該方法對ICP算法進行了改進，在平面上利用K-最近鄰搜索算法提取同名點，提高了配準精度，加快了搜索速度。該方法用到了平面特征，但是并不是用平面進行配準，而是利用平面上的點進行配準，這使得平面特征的作用沒有最大化，而且點配準容易造成局部最優值。文獻［16］提出了基于點面關系的點云配準算法，該方法利用直線特征代替傳統點特征，借助四元數實現點云配準，不足之處是沒有考慮到直線夾角的正負性。

針對以上問題，本文提出了一種基于法向量投票的點云配準方法。該方法利用平面特征代替傳統的點特征，將單位四元數作為特征描述算子，以同名平面特征作為約束條件，求解點云之間的位姿變換關系。仿真和對比實驗表明，該方法適用于具有平面特征的物體，而且很好地避免了局部最優值。

1 基于法向量的平面提取

針對傳統霍夫變換提取平面速度慢的問題，本文提出了一種基于法向量的霍夫投票方法。傳統的霍夫變換提取平面需要計算在三維霍夫空間中每個點在不同角度下的平面到原點的距離，然后對距離進行投票，所以投票的次數不僅與點的數目有關，還與空間劃分間隔有關。而本文方法只需要對每個點進行一次投票，總共只需要投票 N 次，因此極大地縮短了運行時間。算法過程包括計算法向量、Hough空間變換、法向量投票以及峰值檢測四部分，具體流程如下：

a）令霍夫空間為 （θ，φ，ρ） ，其中， φ 為平面與 x 軸的夾角； θ 為平面與 z 軸的夾角； ρ 為平面到原點的距離。

b）計算點云中每個點到原點的距離，得到最大值 ρ max和最小值ρ min。然后選擇合適的劃分間隔s θ、s φ、s ρ 對霍夫空間 （θ，φ，ρ） 進行劃分，得到霍夫投票器 （n θ，n φ，n ρ） 。

c）查找點 p "i=（x i，y i，z i） T的 k 個近鄰點，數據結構采用kd-tree，用最小二乘法擬合平面，求出該點的法向量 n "i=（n xi，n yi，n zi） T。然后單位化 n "i ，并計算該點到原點的距離 d i 。

d）根據點 p "i的法向量 n "i 計算 θ i、φ i、ρ i 的值，公式如下：

ρ i=d i

i= arccos "n zi ρ i

φ i= arctan "n yi n xi """"（1）

e）根據點 p "i在霍夫空間的位置（θ i，φ i，ρ i） ，計算點 p "i 在霍夫投票器中的位置 （n θi，n φi，n ρi） ，并在投票器中進行投票。

f）重復步驟c）～e），完成對所有點的投票。

g）對霍夫投票器進行局部峰值檢測，得到的峰值點即為潛在的平面。

2 點云配準

2.1 四元數與旋轉矩陣

四元數是一種高階復數，公式為

q =q 0+q 1 i +q 2 j +q 3 k ""（2）

其中： i、j、k 是基元；q 0、q 1、q 2、q 3 是實數。

當 q2 0+q2 1+q2 2+q2 3=1時， q 為單位四元數。四元數與旋轉矩陣之間的關系如下：

R = "q2 0+q2 1-q2 2-q2 3 2（q 1q 2-q 0q 3） 2（q 1q 3+q 0q 2） 2（q 1q 2+q 0q 3） q2 0-q2 1+q2 2-q2 3 2（q 2q 3-q 0q 1） 2（q 1q 3-q 0q 2） 2（q 2q 3+q 0q 1） q2 0-q2 1-q2 2+q2 3 """"（3）

2.2 對應平面的查找

上一步檢測出來平面之后，要進行對應平面的查找。當點云 P 經過旋轉 R和平移T 變換為P′ 時，在這一過程中，沒有發生形態改變，根據旋轉不變性，可以得到各平面之間的夾角屬于不變量。因此可以根據各平面之間的夾角來進行對應平面的查找。

2.3 平面配準

設平面 a和b 的單位法向量分別為 n "a=（n ax，n ay，n az） T， n "b=（n bx，n by，n bz） T。對于平面 a 上任意點 p "a=（x a，y a，z a） T， p "a到原點的距離為d a= n T "a· p "a。 p "a經過旋轉 R和平移T 變換為平面b上一點 p "b，假設 p "b到原點的距離為d b ，可以得到如下關系：

n "b= Rn "a

d b= n T "b· p "b=（ Rn "a） T·（ Rp "a+ T ）=d a+ n T "b T """"（4）

其中： R = ""r 11 r 12 r 13

r 21 r 22 r 23

r 31 r 32 r 33 ""， T =（t x，t y，t z） T。 式中用到了旋轉矩陣的性質 R T= R -1，即 R T R-1=I 。由式（4）得到誤差方程為

v x=r 11n ax+r 12n ay+r 13n az-n bx

v y=r 21n ax+r 22n ay+r 23n az-n by

v z=r 31n ax+r 32n ay+r 33n az-n bz

v d=d a+t x（r 11n ax+r 12n ay+r 13n az）+t y（r 21n ax+r 22n ay+r 23n az）+ t z（r 31n ax+r 32n ay+r 33n az）-d b """（5）

對式（5）進行線性化，得到

V=BX-L ""（6）

其中： V =（v x，v y，v z，v d） T， X =（q 0，q 1，q 2 ，q 3，t x，t y） T， L =-（v0 x，v0 y，v0 z，v0 d） T， B = """"v x "q 0 """v x "q 1 """v x "q 2 """v x "q 3 "0 0 0

v y "q 0 """v y "q 1 """v y "q 2 """v y "q 2 "0 0 0

v z "q 0 """v z "q 1 """v z "q 2 """v z "q 2 "0 0 0

v d "q 0 """v d "q 1 """v d "q 2 """v d "q 2 """v d "t x """v d "t y """v d "t z """"。

若從點云中提取的平面數目為 n 對，則根據上式可以列出 4n個方程，其中 V 為4n×1 的殘差向量， B 為4n×7的系數矩陣， L 為4n×1 的常數項矩陣。又由于單位四元數滿足條件

q2 0+q2 1+q2 2+q2 3=1 ""（7）

對其線性化，得到

CX+W "x=0 "nbsp;（8）

其中： C =［2q 0 2q 1 2q 2 2q 3 0 0 0］， W "x=q2 0+q2 1+q2 2+q2 3-1 。

聯立式（6）（8），利用附有限制條件的間接平差模型來對四元數和平移參數進行求解，使得 V T PV =min。求解過程如下：

a）令初值為 q0 0=1，q0 1=q0 2=q0 3=0，t0 x=t0 y=t0 z=0 。

b）將初值代入以下方程進行求解：

B T PBX+C T K "s+ B T PL=0

CX+W "s=0 """（9）

其中： P 為觀測值權值。求解方程得到

X=（N -1 bb- N -1 bb C T N -1 cc CN -1 bb） W-N -1 bb C T N -1 cc W "x ""（10）

其中： N "bb= B T PB，W=B T PL，N "cc= CN -1 bb C T。更新 q 0、q 1、q 2、q 3、t x、t y、t z 的值。

c）判斷平差值是否小于閾值。若小于或等于閾值，則停止迭代；若大于閾值，則將c）中的值代入b）中計算，直到滿足條件或者達到最大迭代次數。

d）迭代終止后，將得到的 q 0、q 1、q 2、q 3 代入式（3）中，求出旋轉矩陣 R，平移矩陣T =（t x，t y，t z） T。

3 實驗分析

本文共設計了兩組實驗來驗證基于法向量投票的點云配準方法的正確性，并用文獻［14］提出的改進ICP算法和文獻［15］提出的KNNS算法進行對比。

3.1 仿真實驗

采用編譯器生成一個四面體的點云模型，并對四面體進行旋轉平移變換，將變換前后兩個四面體放在同一個坐標系下，如圖1（a）所示。利用本文提出的基于法向量的平面提取方法分別對變換前后兩個四面體提取平面特征，用不同的顏色區別不同的平面，如圖1（b）（c）所示，（b）是變換前四面體的平面提取示意圖，（c）是變換后四面體的平面提取示意圖。表1是變換前四面體的平面方程，左側是實際值，右側是估計值。對比表1中的實際值和估計值，發現兩者相差并不大，說明本文提出的平面提取方法是可行的。表2是用本文方法計算出變換后四面體的平面方程的估計值。

基于表1、2所示的四面體平面特征數據，采用本文方法對點云進行配準，配準之后的圖像如圖2（a）所示，同時對配準后的四面體提取平面特征，并計算同名平面之間的誤差，如表3所示。可以看出，配準后得到的平面特征在方向向量的偏差上是比較小的。為了驗證所提方法的有效性，用改進的ICP和KNNS方法進行實驗，配準后的圖形如圖2（b）（c）所示，配準結果如表4所示，前三行是實際的旋轉平移變換矩陣，后面的分別是三種方法配準的旋轉矩陣和平移矩陣。其中，對于改進的ICP方法，最大迭代次數設置為50，兩次迭代后的誤差之差的閾值設置為0.001，圖3是每一次迭代后的配準誤差，當迭代到第21次的時候，迭代終止。對于KNNS方法，使用的共面平面特征和本文方法相同，如表1、2所示，然后再利用共面點進行K鄰近搜索。由圖2可見，除了本文方法配準成功之外，其他兩種方法都配準失敗了。原因是改進的ICP方法采用了歐氏距離最近點作為配對點，對初始值有很強的依賴性，由圖1（a）可見，這兩片點云的重疊部分較少，對于非重疊部分的點，在尋找對應點的時候，忽略了點云的結構信息，陷入了局部最優解，導致配準失敗。KNNS方法采用共面點集中心區域的點進行配準，當平面距離較遠時，在找尋對應點對的時候陷入了局部最優值，導致配準失敗。

3.2 實測數據

針對本文提出的方法，在實驗室搭建3D環境進行測試，本實驗采用的是PRINCE775手持式三維掃描儀，如圖4所示，從兩個不同的站點對桌面上的物體進行掃描，獲取兩片點云數據。對得到的點云預處理之后，將兩片點云放到同一個坐標系中，作為初始點云進行配準，如圖5（a）所示。

實驗時，采用本文提出來的方法分別從兩個測量站中提取五對同名平面，在此基礎上借助四元數進行配準，計算得到基準站和待配準站之間的變換參數，配準結果如圖5（b）所示。為進一步驗證所提算法的正確性，采用改進的ICP和KNNS算法進行實驗，配準后的圖形如圖5（c）和（d）所示，對比配準結果如表5所示，配準殘差如表6所示。其中，對于改進的ICP方法，最大迭代次數設置為50，兩次迭代后的誤差之差的閾值設置為0.000 1，圖6是每一次迭代后的配準誤差，當迭代到第25次的時候，迭代終止。對于KNNS方法，使用的共面平面特征和本文方法相同，然后再利用平面上的點進行鄰近K搜索算法。由于鄰近K進行線性掃描的復雜度是 O（n），本文采用的是樹型結構，復雜度為O （log "n ），所以在KNNS方法進行配準的時候，構建kd-tree進行數據查找，縮短計算時間。

由表5、6可以看出，三種方法的配準結果接近，本文算法在單位方向向量的偏差中誤差與改進的ICP和KNNS算法相差不大，但是在距離偏差中誤差稍優于改進的ICP和KNNS算法。主要是因為本文同步求解旋轉矩陣和平移矩陣，沒有依賴關系，而其他兩種算法是先計算旋轉矩陣，再計算平移矩陣，平移矩陣的求解依賴于旋轉矩陣，對誤差進行了累積。

4 結束語

本文在分析了傳統點特征點云配準的缺點后，提出了一種基于法向量投票的點云配準方法，用平面特征代替點特征作為配準基元，建立基于平面的坐標轉換模型。該方法對傳統的霍夫變換進行了改進，降低了時間復雜度，然后在提取同名平面特征的基礎上，利用四元數進行配準，充分利用了平面特征，提高了精度，并且降低了對初始值的依賴，有效地避免了由初始位置不理想造成的局部最優值，為實現產品的逆向工程提供了一種思路。另外，霍夫投票同樣適用于其他三維物體形狀的檢測，如球面、圓柱等，但是要對不同的形狀進行分析，找出相同的特征，最后對特征進行投票。

參考文獻：

［1］ "Yang I T，Shin M S，Acharya T D.A technology on reverse engineering of structure using 3D scanner［J］. Journal of Industrial Technology ，2015， 35 ：47-53.

［2］ Yanamandra K，Chen Guanlin，Xu Xianbo， et al. Reverse engineering of additive manufactured composite part by toolpath reconstruction using imaging and machine learning［J］. Composites Science and Technology ，2020， 198 ：108318.

［3］ 石征錦，富斯盟，呂鑫，等.三維點云配準算法在醫療手術中的應用［J］.光電技術應用，2019， 34 （5）：53-57. （Shi Zhengjin，Fu Simeng，Lyu Xin， et al. Application of 3D point cloud registration algorithm in medical surgery［J］. Electro-Optic Technology Application， 2019， 34 （5）：53-57.）

［4］ George P，Nectarios V，Markos P， et al. Individualized ophthalmic exoplants by means of reverse engineering and 3D printing technologies for treating high myopia complications with macular buckles［J］. Biomimetics ，2020， 5 （4）：article No.54.

［5］ 陳學偉，朱耀麟，武桐，等.基于SAC-IA和改進ICP算法的點云配準技術［J］.西安工程大學學報，2017， 31 （3）：395-401. （Chen Xuewei，Zhu Yaolin，Wu Tong， et al. The point cloud registration technology based on SAC-IA and improved ICP［J］. Journal of Xi’an Polytechnic University ，2017， 31 （3）：395-401.）

［6］ Shi Guiguan，Gao Xuguang，Dang Xinghua.Improved ICP point cloud registration based on kd-tree［J］. International Journal of Earth Sciences and Engineering， 2016， 9 （5）：2195-2199.

［7］ 趙明富，黃錚，宋濤，等.融合采樣一致性和迭代最近點算法的點云配準方法［J］.激光雜志，2019， 40 （10）：45-50. （Zhao Mingfu，Huang Zheng，Song Tao， et al. Point cloud registration method based on sample consensus alignment and iterative closest point algorithm［J］. Laser Journal ，2019， 40 （10）：45-50.）

［8］ Zhang Runmei，Wu Yulu，Zhang Guangbin， et al. Study on Huizhou architecture of point cloud registration based on optimized ICP algorithm［J］. IOP Conference Series：Earth and Environmental Science ，2018， 128 （1）：article ID 012098.

［9］ 荊路，武斌，李先帥.基于SAC-IA和NDT融合的點云配準方法［J］.大地測量與地球動力學，2021， 41 （4）：378-381. （Jing Lu，Wu Bin，Li Xianshuai.Point cloud registration method based on SAC-IA and NDT fusion［J］. Journal of Geodesy and Geodynamics ，2021， 41 （4）：378-381.）

［10］ 余洪山，付強，孫健，等.面向室內移動機器人的改進3D-NDT點云配準算法［J］.儀器儀表學報，2019， 40 （9）：151-161. （Yu Hongshan，Fu Qiang，Sun Jian， et al. Improved 3D-NDT point cloud registration algorithm for indoor mobile robot［J］. Chinese Journal of Scientific Instrument ，2019， 40 （9）：151-161.）

［11］ 袁志聰，魯鐵定，劉瑞.一種基于BFGS修正的正態分布變換點云配準方法［J］.測繪通報，2020， 4 （10）：38-42. （Yuan Zhicong，Lu Tieding，Liu Rui.A normal distribution transform point cloud registration method based on BFGS correction［J］. Bulletin of Surveying and Mapping ，2020， 4 （10）：38-42.）

［12］ 宗文鵬，李廣云，李明磊，等.激光掃描匹配方法研究綜述［J］.中國光學，2018， 11 （6）：914-930. （Zong Wenpeng，Li Guangyun，Li Minglei， et al. A survey of laser scan matching methods［J］. Chinese Optics ，2018， 11 （6）：914-930.）

［13］ 李傳龍，佃松宜，劉海亮.基于改進動態差分進化算法的點云配準［J］.電光與控制，2019， 26 （3）：59-64. （Li Chuanlong，Tian Songyi，Liu Hailiang.Point cloud registration based on improved dynamic differential evolution algorithm［J］. Electronics Optics and Control ，2019， 26 （3）：59-64.）

［14］ 余大梅，趙闖姓.基于配準殘差分布函數點對定權的ICP算法研究［J］.地理空間信息，2020， 18 （6）：48-52，6，7. （Yu Damei，Zhao Chuangxing.Research on ICP algorithm of point pair weighting based on registration residual distribution function［J］. Geospatial Information ，2020， 18 （6）：48-52，6，7.）

［15］ 陳西江，花向紅，魯鐵定，等.利用平面特征和KNNS提高點云配準效率［J］.中國礦業大學學報，2014， 43 （6）：1149-1154. （Chen Xijiang，Hua Xianghong，Lu Tieding， et al. Improving the efficiency of point cloud registration based on planar characteristics and KNNS［J］. Journal of China University of Mining and Technology ，2014， 43 （6）：1149-1154.）

［16］ 柴雙武，楊曉琴.基于點面關系構建的線基元點云配準算法［J］.大地測量與地球動力學，2020， 40 （4）：405-410. （Chai Shuangwu，Yang Xiaoqin.A linear feature point cloud registration algorithm based on the relation between point and plane［J］. Journal of Geodesy and Geo-dynamics ，2020， 40 （4）：405-410.）