自研激光雷達三維點云配準技術

呼延嘉玥，徐立軍，李小路

（北京航空航天大學 儀器科學與光電工程學院，北京100083）

近年來，激光雷達三維掃描技術發展迅速，是獲取空間三維信息最重要的方法之一。因為其便捷快速、測量范圍廣、精度高的特點，被廣泛應用于數字城市、地形測繪、文物保護、自然災害監測等領域［1-2］。傳統獲取物體表面三維信息的方法，主要包括基于激光雷達的三維掃描方法［3］和基于圖像的三維重建方法［4］。

基于TOF（Time of Flight）三維掃描激光雷達，計算光的飛行時間獲取距離信息，可以解算物體的空間三維坐標。這種方法測距精度高、工作距離遠、操作簡單，但無法為重建場景提供紋理信息，且存在位置限制和遮擋等問題，通常難以獲取完整表面［5］。基于圖像數據三維重建方法，獲取某一物體多視角下的圖像，利用三角測量原理計算圖像像素間視差，從而解算物體的三維幾何信息。這種方法雖然能夠獲取豐富的紋理信息，但重建點數量少且精度低，當圖像質量不佳時，重建效果較差［6］。激光雷達與圖像相結合的三維重建方法兼具兩者優勢，因此受到國內外學者關注，已在紋理映射、導航定位等領域得到廣泛應用。實現激光雷達點云與圖像重建點云的空間配準技術，則是三維重建技術研究的關鍵。

空間點云配準技術分為初始配準和精確配準2個步驟，初始配準能夠初步減小兩組點云的旋轉和平移錯位，精確配準能夠使兩組點云之間的配準誤差最小［7］。目前，最常用的點云配準方法是由Besl和Mckay［8］在1992年提出的迭代的最近點（Iterative Closest Point，ICP）算法。該算法針對目標數據集中的每個目標點，尋找參考數據集中與其距離最小的對應參考點，通過迭代算法估計旋轉矩陣、平移向量。然而，該算法存在計算量大、效率低、對初始位置要求高等不足，且忽視了2組點云之間的尺度因子［9］。Chen和Medioni［10］提出點對面搜索最近點的配準方法，降低原始ICP算法對初始位置的要求，加快算法收斂速度。Rusinkiewicz和Levoy［11］詳細分析ICP算法各個步驟，并提出相應的改進算法。戴靜蘭等［12］提出基于曲率特征的改進ICP算法，鄒際祥［13］提出基于K-D樹（K-Dimensional tree）搜索最近點的方法，加快了最鄰近點的查找速度。

本文研究了一種改進的快速多尺度因子（Fast Multi-Scale Registration，FMSR）點云配準方法，該方法引入激光雷達點云和圖像重建點云之間的尺度因子，提升空間點云配準技術的配準精度和計算效率，并將該方法最終應用于自研激光雷達系統所得點云數據。初始配準使用基于尺度自適應關鍵點質量（Adaptive Scale Key Point Quality，ASKQ）的點云特征提取算法［14］提取出關鍵點，實現關鍵點的特征提取與匹配，利用特征匹配對計算初始參數，提升初始配準的配準精度。精確配準采用K-鄰近（K-Nearest Neighbors，KNN）算法搜索相鄰近點，無需對點云進行網格劃分，減少算法的運行時間，實現快速、準確的配準。

1 三維點云數據獲取系統

三維點云數據獲取系統由激光雷達系統和圖像采集系統構成，如圖1所示。自研三維掃描激光雷達系統包括4個組成單元：激光發射與接收單元、掃描單元、控制單元和測距單元。激光發射與接收單元由激光器、望遠鏡、光電探測器組成，設計同軸光路采集激光發射脈沖、回波脈沖和觸發信號。掃描單元用于實現三維掃描功能，垂直方向360°掃描采用步進電機和45°掃描轉鏡實現，水平方向360°掃描采用高精度數位云臺實現。控制單元的核心部件為現場可編程門陣列（FPGA），用于設置激光器參數、程控增益控制、步進電機控制和數據采集等。測距單元包括自動增益控制電路和時刻鑒別電路。系統具體設計及描述參見筆者團隊前期發表的論文［15-18］。

圖1 三維點云數據獲取系統Fig.1 Three-dimensional point cloud data acquisition system

圖像采集系統采用攝像機（型號MER-1070-14U3C）拍攝多視點下的圖像。相機鏡頭（型號M7528-MP2）焦距為75 mm，工作距離為0.3 m到無窮遠處。該圖像采集系統具有1 070萬像素分辨率，圖像數據通過USB3.0數據接口傳輸。

2 基于三維點云的空間配準

基于激光雷達點云與圖像重建點云的空間點云配準技術，主要包括點云獲取、點云處理和點云融合3個步驟，本文實現點云配準的具體流程如圖2所示。首先，激光雷達系統與圖像采集系統采集激光雷達點云和二維圖像數據，并利用SFM（Structure from Motion）技術從圖像中獲取三維重建點云。其次，利用ASKQ算法選取關鍵點，提取關鍵點特征與匹配。然后，利用提取出的匹配對求解初始配準參數，利用基于快速多尺度因子的三維點云配準算法實現精確配準。最后，進行數值仿真和實驗，分析驗證算法有效性，評價配準精度。

圖2 點云配準流程圖Fig.2 Flowchart of point cloud registration

2.1 兩種點云數據獲取

點云數據獲取過程包括2個部分：基于三維掃描激光雷達的三維坐標解算和基于圖像的三維坐標重建。

為了獲取三維掃描激光雷達點云，筆者團隊設計了點云實時獲取與顯示的上位機軟件，該軟件通過USB接口實時獲取測距和掃描模塊相關數據，計算每束激光脈沖對應的水平方向掃描角、垂直方向掃描角和激光腳點的距離信息，最后利用激光雷達方程解算出激光腳點的三維坐標。

為了獲取圖像三維重建點云，首先利用相機拍攝同一物體在不同視角的二維圖像。然后基于SIFT（Scale-Invariant Feature Transform）算法提取出圖像特征，并基于RANSAC（RANdom SAmple Consensus）算法實現特征匹配。最后利用SFM技術獲取三維重建點云。

2.2 基于ASKQ的點云特征提取算法

針對已獲取的2組點云，本文采用基于ASKQ的點云特征提取算法提取關鍵點，評估關鍵點質量并且排序，從中選取質量最優關鍵點。該方法的主要步驟如下：

首先，對2組點云中每個采樣點v，選取以v的坐標為球心，r為半徑的球體內的全部點云信息，并由局部表面矩陣Lj=［xj，yj，zj］T（j=1，2，…，m）表示。

其次，初步選取關鍵點。利用霍特林變換在局部表面建立局部坐標系，得到2個主軸方向。將每個局部表面矩陣L變換到局部坐標系中，使其與主軸方向對齊，并計算出物理量主軸比δ為

式中：X、Y分別為變換后局部表面矩陣的x分量和y分量。如果δ=1，則該局部表面為普通對稱表面；如果δ＞1，則為非對稱表面。根據采樣點v的不同半徑rk計算主軸比δk，當主軸比δk中存在極大值，對應的采樣點v視為關鍵點，極大值對應半徑rk為最佳尺度。

最后，評估提取出的關鍵點質量并排序，從中選取最優關鍵點。關鍵點質量取決于關鍵點附近局部表面的主曲率。基于變換后的局部表面矩陣所含數據，進行曲面擬合，得到擬合曲面C，并對曲面C進行n×n的采樣，計算關鍵點質量Qk為

式中：K=κ1κ2為高斯曲率，κ1和κ2為擬合曲面C上每點的主曲率。

將關鍵點根據關鍵點質量Qk由大到小排序。由于部分距離相近的關鍵點容易造成數據信息的重復，因此設置最小距離閾值，消除最小距離范圍內的其他關鍵點，僅保留最優關鍵點。

2.3 關鍵點特征提取與匹配

由擬合曲面C采樣提取的特征向量具有旋轉不變性和尺度不變性，本文選擇該特征向量作為形狀特征。形狀特征的向量維數通常較高，存在占用大量內存、耗費大量時間等問題，因此使用主成分分析（Principal Component Analysis，PCA）方法對形狀特征進行降維，將形狀特征投影到PCA子空間中。

利用降維后的2組形狀特征實現關鍵點特征匹配。其中，由激光雷達點云獲取形狀特征為fp，由圖像重建點云獲取形狀特征為fq，兩組形狀特征之間的誤差er為

式中：er在0～π/2之間分布，0代表兩組形狀特征向量完全相同，π/2代表兩組形狀特征向量完全不同或正交。當er取得最小值時，則為最佳匹配。

為了實現多尺度點云之間的精確配準，本文對ICP算法引入尺度因子，研究基于快速多尺度因子的三維點云配準算法。該算法選取目標點云與參考點云之間的誤差和作為目標函數，最小化目標函數，求解2組點云之間最優尺度因子S、最優旋轉矩陣R和最優平移向量T。目標函數為

式中：xk為目標點云；N為目標點云數量；yk為參考點云中與目標點云xk相對應的點。為了保證目標點云中每個點都能在參考點云搜索到對應點，通常選取數量少、精度低的圖像重建點云為目標點云，選取數量多、精度高的激光雷達點云為參考點云。

利用2.2節和本節所述的ASKQ算法獲取關鍵點的特征匹配對，計算初始尺度因子S0、初始旋轉矩陣R0和 初 始 平 移 向 量T0［19］。當 確 定 初始值后，利用多次迭代實現點云的精確配準。迭代過程如下：

1）對目標點云進行尺度因子為S0、旋轉矩陣為R0和平移向量為T0的初始變換，得到變換后的目標點云為xtrans。

2）利用KNN算法全局搜索，在參考點云中搜索與變換后的目標點云xtrans最鄰近的點yk，計算出尺度因子Strans、旋轉矩陣Rtrans、平移向量Ttrans，使得目標函數誤差最小：

3）兩次迭代間參數的變化關系為

將Si+1、Ri+1、Ti+1作為初始值重復步驟1）、步驟2），當尺度因子變化量ΔS=Si+1-Si、旋轉矩陣變化量ΔR=Ri+1-Ri、平移向量變化量ΔT=Ti+1-Ti均小于設定閾值時，停止迭代過程，輸出即為最優尺度因子S，最優旋轉矩陣R與最優平移向量T。

4）利用均方根（Root Mean Square，RMS）誤差分析評價配準精度，計算式為

式中：Qrms為均方根誤差。

3 實驗結果與分析

3.1 仿真實驗驗證與分析

為了驗證ASKQ算法對于關鍵點提取、特征提取與匹配的有效性，本文使用UWA（University of Western Australia）提供的點云數據庫進行仿真實驗驗證。該點云數據庫采用Minolta Vivid 910激光掃描儀，獲取真實目標點云，空間分辨率為640×480。本實驗選用的數據集包含點云30 165個，點云間隔為0.366 mm。

關鍵點質量Qk常作為選取高質量關鍵點的指標。Qk的值越大，表示關鍵點的質量越高，但相對應的是關鍵點的數量減小，平坦目標表面上的關鍵點將被經驗性剔除。本實驗將閾值設定為Qk＞4，將算法中自適應尺度設定為

rk=［2.5 5 7.5 10 12.5］

本實驗設置4種尺寸目標（小雞模型），分別為5、10、20、100 cm，關鍵點提取的結果如圖3所示。結果顯示當目標尺寸越大，關鍵點數量越多，并且關鍵點分布均勻。

為了驗證不同尺度因子的特征匹配實驗結果，4種目標尺度因子設置為0.5、1、2、10，尺度因子定義為目標點云與參考點云的比值。基于上述關鍵點提取結果，進行網格化采樣（20×20）得到400維特征向量，利用PCA方法對特征向量降維。匹配結果如圖4所示，結果顯示該算法對不同尺度因子的目標都有良好的匹配結果。

最后，利用ASKQ算法提取的關鍵點的特征匹配對計算初始配準參數，并利用FMSR算法實現4組目標的精確配準，配準結果如圖5所示。結果表明該算法對4組尺度因子不同的目標均有良好的精確配準結果。

表1為FMSR算法的精確配準結果，從表中得出以下結論：①對尺度因子為0.5、1、2、10的4組目標，利用FMSR算法計算的最優尺度因子S均與理論值μ接近；②由于4組目標具有固定的相對位置，配準得到的4組最優旋轉矩陣R一致，最優平移向量T成比例關系，且比例系數與尺度因子相同，表明FMSR算法精確配準結果具有穩定性；③4組目標的配準精度均為0.002 m，說明FMSR算法配準精度具有穩定性；④4組目標的算法運行時間不同，算法運行時間與初始配準的精度有關，初始特征匹配對的精度越高，算法運行越快。

圖3 多尺度小雞模型的關鍵點提取結果Fig.3 Key point extraction results of multi-scale chicken model

圖4 多尺度小雞模型的關鍵點匹配結果Fig.4 Key point matching results of multi-scale chicken model

圖5 多尺度小雞模型精確配準結果Fig.5 Fine registration results of multi-scale chicken model

理論值μ 最優尺度因子S 最優旋轉矩陣R 最優平移向量T 均方根誤差Q rms/m 時間/s 0.5 0.487 （-0.483，0.010，-0.049） （2.659，-145.708，-25.783）0.002 4.043 1 0.977 （-0.483，0.010，-0.049） （5.338，-291.396，-51.402） 0.002 2.601 2 1.954 （-0.483，0.010，-0.049） （10.679，-582.779，-102.807） 0.002 2.509 10 9.768 （-0.483，0.010，-0.049） （53.439，-2 913.906，-513.807）0.002 2.489

3.2 真實實驗驗證與分析

3.2.1 實驗描述

真實場景實驗的目標為北京航空航天大學晨興音樂廳（尺寸為20.30 m×7.85 m×26.56 m），實驗場景如圖6所示。利用自研三維掃描激光雷達系統采集三維點云，設置激光發射頻率為5 k Hz、電機轉速為2 r/s、云臺轉速為0.37（°）/s。獲取激光雷達點云28 136個，垂直分辨率為0.144°，水平分辨率為0.185°。采用攝像機（型號MER-1070-14U3C）拍攝多視點圖像，圖像像素為3 264×2 448，并且利用SFM 技術實現圖像三維重建，獲取圖像重建點云12789個。

圖6 北京航空航天大學晨興音樂廳建筑的實驗現場Fig.6 Building experimental site of Chenxing Concert Hall in Beihang University

3.2.2 實驗案例分析

本實驗中，最小距離閾值設定為Qk＞4，自適應尺度的選取范圍rk=［25 20 15 10 5 1 0.5］。激光雷達點云和圖像重建點云的關鍵點提取結果如圖7（a）和圖7（b）所示。由圖可知：①關鍵點僅取決于局部表面的坐標變化，不受目標整體幾何形狀的影響，因此ASKQ算法提取出的關鍵點分布均勻。②關鍵點由局部坐標系提取得出，具有局部不變性，因此關鍵點魯棒性佳，不隨旋轉、平移及尺度因子的變化而改變。③圖7（b）中設定的最小距離閾值小于圖7（a），因此關鍵點數量較多。由于圖像重建點云密度低于激光雷達點云密度，應從圖像重建點云中提取更多關鍵點，尋找激光雷達點云的最佳匹配。

圖7 真實場景關鍵點提取結果Fig.7 Key point extraction results of real scene

通過網格化采樣提取出關鍵點的形狀特征，使用PCA方法降維，并對降維后的形狀特征實現匹配。本實驗提取出關鍵點的特征匹配對22對，匹配結果如圖8所示，結果表明所提出算法具有良好匹配效果。

圖8 真實場景的關鍵點匹配結果Fig.8 Key point matching results of real scene

最后，利用關鍵點的特征匹配對計算初始尺度因子S0、初始旋轉矩陣R0和初始平移向量T0，為FMSR算法提供初始參數，最終通過迭代求解實現精確配準。本文選取S-ICP（Scale-Iterative Closest Point）算法為對比算法，利用所提出的FMSR算法和S-ICP算法配準同一目標。實驗目標和2種算法的點云配準結果如圖9所示，2種算法配準結果的對比如下：在屋頂上方和墻壁右側位置，應用S-ICP算法的配準結果存在部分缺失，而采用所提出的FMSR算法則有較優的配準結果。表2結果表明，S-ICP算法的均方根誤差為0.325 m，FMSR算法的均方根誤差為0.194 m，表明FMSR算法的配準精度高于S-ICP算法，配準精度提高了0.131 m。且運行時間由23.212 s降低為16.207 s，表明FMSR算法能夠減小運行時間（30%），提高計算效率。

參數 S-ICP算法 FMSR 算法最優尺度因子S 3.000 3.300最優旋轉矩陣R （1.683，-0.035，0.935） （1.561，-0.044，-2.214）最優平移向量T （-39.233，-29.415，2.564） （17.394，12.485，-0.957）均方根誤差Q rms/m 0.325 0.194時間/s 23.212 16.207

4 結 論

為了提高激光雷達點云和圖像重建點云的空間點云配準精度，自主搭建三維掃描激光雷達系統，主要性能包括：點云垂直角分辨率0.144°，水平角分辨率0.185°，點云密度300 pt/m2（pt/m2代表每平方米的點數）；結合基于ASKQ的點云特征提取算法和基于FMSR的點云配準算法，實現激光雷達點云和圖像重建點云的初始配準和精確配準。

ASKQ算法為點云配準的初始配準過程提供高質量匹配對，從而計算出精確的初始尺度因子S0、初始旋轉矩陣R0和初始平移向量T0，為精確配準提供準確的初始參數。利用FSMR點云配準算法進行精確配準，通過KNN算法對2組點云進行全局搜索，多次迭代計算得到最優尺度因子S，最優旋轉矩陣R和最優平移向量T。通過數值仿真和實驗結果表明，該算法針對空間目標（20.30m×7.85 m×26.56 m）配準精度達到0.194 m，運行時間為16.207 s。FMSR算法有效提升空間點云配準精度和計算效率（30%）。上述方法可以為空間場景重建和紋理匹配提供算法基礎。