自研激光雷達三維點云配準技術(shù)

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

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

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

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

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

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

1 三維點云數(shù)據(jù)獲取系統(tǒng)

三維點云數(shù)據(jù)獲取系統(tǒng)由激光雷達系統(tǒng)和圖像采集系統(tǒng)構(gòu)成，如圖1所示。自研三維掃描激光雷達系統(tǒng)包括4個組成單元：激光發(fā)射與接收單元、掃描單元、控制單元和測距單元。激光發(fā)射與接收單元由激光器、望遠鏡、光電探測器組成，設(shè)計同軸光路采集激光發(fā)射脈沖、回波脈沖和觸發(fā)信號。掃描單元用于實現(xiàn)三維掃描功能，垂直方向360°掃描采用步進電機和45°掃描轉(zhuǎn)鏡實現(xiàn)，水平方向360°掃描采用高精度數(shù)位云臺實現(xiàn)?？刂茊卧暮诵牟考楝F(xiàn)場可編程門陣列（FPGA），用于設(shè)置激光器參數(shù)、程控增益控制、步進電機控制和數(shù)據(jù)采集等。測距單元包括自動增益控制電路和時刻鑒別電路。系統(tǒng)具體設(shè)計及描述參見筆者團隊前期發(fā)表的論文［15-18］。

圖1 三維點云數(shù)據(jù)獲取系統(tǒng)Fig.1 Three-dimensional point cloud data acquisition system

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

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

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

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

2.1 兩種點云數(shù)據(jù)獲取

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

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

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

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

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

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

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

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

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

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

將關(guān)鍵點根據(jù)關(guān)鍵點質(zhì)量Qk由大到小排序。由于部分距離相近的關(guān)鍵點容易造成數(shù)據(jù)信息的重復，因此設(shè)置最小距離閾值，消除最小距離范圍內(nèi)的其他關(guān)鍵點，僅保留最優(yōu)關(guān)鍵點。

2.3 關(guān)鍵點特征提取與匹配

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

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

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

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

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

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

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

2）利用KNN算法全局搜索，在參考點云中搜索與變換后的目標點云xtrans最鄰近的點yk，計算出尺度因子Strans、旋轉(zhuǎn)矩陣Rtrans、平移向量Ttrans，使得目標函數(shù)誤差最?。?/p>

3）兩次迭代間參數(shù)的變化關(guān)系為

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

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

式中：Qrms為均方根誤差。

3 實驗結(jié)果與分析

3.1 仿真實驗驗證與分析

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

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

rk=［2.5 5 7.5 10 12.5］

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

為了驗證不同尺度因子的特征匹配實驗結(jié)果，4種目標尺度因子設(shè)置為0.5、1、2、10，尺度因子定義為目標點云與參考點云的比值?；谏鲜鲫P(guān)鍵點提取結(jié)果，進行網(wǎng)格化采樣（20×20）得到400維特征向量，利用PCA方法對特征向量降維。匹配結(jié)果如圖4所示，結(jié)果顯示該算法對不同尺度因子的目標都有良好的匹配結(jié)果。

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

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

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

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

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

理論值μ 最優(yōu)尺度因子S 最優(yōu)旋轉(zhuǎn)矩陣R 最優(yōu)平移向量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所示。利用自研三維掃描激光雷達系統(tǒng)采集三維點云，設(shè)置激光發(fā)射頻率為5 k Hz、電機轉(zhuǎn)速為2 r/s、云臺轉(zhuǎn)速為0.37（°）/s。獲取激光雷達點云28 136個，垂直分辨率為0.144°，水平分辨率為0.185°。采用攝像機（型號MER-1070-14U3C）拍攝多視點圖像，圖像像素為3 264×2 448，并且利用SFM 技術(shù)實現(xiàn)圖像三維重建，獲取圖像重建點云12789個。

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

3.2.2 實驗案例分析

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

圖7 真實場景關(guān)鍵點提取結(jié)果Fig.7 Key point extraction results of real scene

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

圖8 真實場景的關(guān)鍵點匹配結(jié)果Fig.8 Key point matching results of real scene

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

參數(shù) S-ICP算法 FMSR 算法最優(yōu)尺度因子S 3.000 3.300最優(yōu)旋轉(zhuǎn)矩陣R （1.683，-0.035，0.935） （1.561，-0.044，-2.214）最優(yōu)平移向量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 結(jié) 論

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

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