陳仲生，李潮林，左 旺，侯幸林

（1.常州工學院汽車工程學院，常州 213032；2.湖南工業(yè)大學電氣與信息工程學院，株洲 417002）

前言

目前自動駕駛的4 大關(guān)鍵技術(shù)包括感知技術(shù)、決策技術(shù)、路徑規(guī)劃和運動控制，其中環(huán)境感知是智能車輛自主行駛的基礎(chǔ)和前提。特別是在自動駕駛環(huán)境感知系統(tǒng)中，如何獲取高精度實時路況數(shù)據(jù)是決定自動駕駛系統(tǒng)行車安全的關(guān)鍵。近年來，激光雷達、毫米波雷達等先進傳感器技術(shù)正成為汽車自動駕駛領(lǐng)域的重要感知手段，其輸出的點云已成為描述車輛所處三維世界的主要數(shù)據(jù)格式［1］。但是實際應(yīng)用中，受傳感器視場角限制，單個傳感器往往只能獲得有限視野范圍內(nèi)的點云，需要采用配準算法才能生成完整場景的三維點云，以便支持后續(xù)的物體識別、分類及路徑規(guī)劃。點云配準就是估計兩幀掃描點云之間的變換矩陣，將不同條件下采集的點云數(shù)據(jù)變換到同一坐標系下，實現(xiàn)點云數(shù)據(jù)的拼接或融合以獲得完整的3D 數(shù)字模型，它是自動駕駛點云數(shù)據(jù)處理不可缺少的重要前序步驟［2］。

點云配準也是計算機視覺領(lǐng)域的研究熱點之一。Huang 等［3］對點云配準研究進展進行了綜述與展望，根據(jù)采集設(shè)備類型分為同源點云配準和跨源點云配準，根據(jù)發(fā)展階段分為傳統(tǒng)的配準方法和基于學習的配準方法。傳統(tǒng)點云配準方法主要基于優(yōu)化思想，包含粗配準和精配準兩個階段：點云粗配準目的是對任意初始位置的兩個點云進行粗略的配準使其大致對齊，為后續(xù)精配準提供良好的初始位置，主要包括基于全局搜索思想的配準和基于幾何特征描述的配準兩大類算法；精配準是在粗配準的基礎(chǔ)上，進行更精確、更細化的配準。基于學習的配準方法是從點云數(shù)據(jù)中學習出三維特征實現(xiàn)配準，其不足是需要大量訓練數(shù)據(jù)，對于未知場景配準性能較差［3］，這在自動駕駛這種實時性要求高、未知場景多的應(yīng)用場合中存在較大的局限性。

應(yīng)用最廣的點云精配準算法是Besl 和Mckay［4］提出的迭代最近點（iterative closest point，ICP）算法，但它對點云初始位置要求較高，需要兩個點云的重疊度高且數(shù)據(jù)量相近，否則容易陷入局部最優(yōu)解，而且存在迭代速度慢、配準時間長等不足；Magnusson等［5］提出了另一種經(jīng)典的正態(tài)分布變換（normal distributions transform，NDT）配準算法，NDT 算法的配準效果與ICP 算法相似，其改進實質(zhì)等同于將ICP算法柵格化。可以看出，ICP、NDT 算法及其變體大多需要點云初始位置大致對齊，這種簡單的點與點對應(yīng)導致魯棒性較差，為此人們開始研究更加魯棒的對應(yīng)，比如點對面、面對面和特征對特征，常見的點云特征描述有快速點特征直方圖（fast point feature histograms，F(xiàn)PFH）描述子、內(nèi)部形狀描述子（intrinsic shape signature，ISS）［6］、三維Harris 描述子（harris 3D，H3D）［7］等，其中ISS是一種表示立體幾何形狀的算法，具有豐富的幾何信息，用于關(guān)鍵點提取時具有快速、高效、穩(wěn)定等優(yōu)點。同時，人們研究了不同的粗配準算法，比如Aiger 等［8］提出的全等四點集（4-points congruent sets，4PCS）經(jīng)典粗配準算法，不需要提供初始位姿就能為精配準提供良好的初始位姿，缺點是計算速度慢；為此Mellado 等［9］提出了一種Super4PCS 算法，它在提取四點集的同時去除錯誤的點集，降低了一定的時間復(fù)雜度。Super4PCS算法的特點是不需要初始位姿，但計算速度仍然不快，可以作為粗配準算法為ICP 等精配準算法提供良好的初始位姿。為了提高點云配準的速度和正確率，近年來人們提出了不同的點云配準增強方法。李仁忠等［10］將ISS 與ICP 算法結(jié)合，提高了ICP 算法的配準精度，但仍然依賴初始位姿；宗曉萍和吳巖［11］提出一種基于FPFH 特征的ICP 改進算法，可以提高配準的精確度；單麗杰等［12］利用法向量夾角提取特征點，提高ICP 算法的配準速度；王月海等［13］采用自適應(yīng)體素網(wǎng)格濾波法對點云進行處理，一定程度上提升了配準效率；范強等［14］將H3D 與NDT 相結(jié)合，提升了配準精度，但魯棒性不高。綜合來看，已有改進ICP 配準算法雖然在一定程度上提高了配準精度和速度，但目前大多數(shù)4PCS 和ICP 變體算法仍無法滿足自動駕駛等實時性要求高的應(yīng)用場合［15］。

進一步分析可知，限制4PCS 和ICP 及其變體算法配準速度的主要原因在于尋找?guī)缀涡螤钕嗨频狞c集以及驗證其變換矩陣均比較費時。為此，本文引入點云雙重下采樣來快速減少點云數(shù)目，并融合ISS、Super4PCS 和ICP 算法的優(yōu)點，研究一種改進的快速點云配準方法，其特點是采用雙重下采樣快速降低點云數(shù)據(jù)，同時利用ISS 算法來優(yōu)化Super4PCS算法，降低時間復(fù)雜度，同時提供良好的初始位姿，以提高整個算法的配準速度和精度，最后利用斯坦福點云數(shù)據(jù)集和自動駕駛點云數(shù)據(jù)集進行驗證。

1 改進配準算法的基本框架

針對已有點云配準算法存在速度低的問題，本文提出的改進型雙重下采樣增強的點云快速配準算法基本框架如圖1 所示，主要包括以下3 個步驟：點云數(shù)據(jù)雙重下采樣、ISS-Super4PCS粗配準和線性最小二乘優(yōu)化ICP 精配準。在雙重下采樣階段，首先使用體素下采樣對原始點云數(shù)據(jù)進行粗采樣，快速減少點云數(shù)目，然后使用基于法向量的鄰域夾角進行精采樣，充分保留點云數(shù)據(jù)的原始特征；在粗配準階段，首先提取雙重下采樣后兩個點云的ISS 特征點，減少Super4PCS 算法的一致性4 點搜索范圍，然后基于ISS 特征點利用Super4PCS 算法求出粗配準變換矩陣；在精配準階段，利用線性最小二乘優(yōu)化ICP 配準算法對粗配準的結(jié)果進一步優(yōu)化，實現(xiàn)精配準。

圖1 改進型配準算法基本框架

上述點云配準改進算法的優(yōu)點在于：通過雙重下采樣操作可以快速減少點云的數(shù)據(jù)量，從而大大降低后續(xù)配準過程的時間復(fù)雜度；同時，保留了Super4PCS算法的魯棒性。因此，該算法同時具有魯棒性好、配準精度高、配準速度更快的優(yōu)點。

2 點云數(shù)據(jù)雙重下采樣

實際應(yīng)用中點云的數(shù)據(jù)量往往很大，直接進行處理不僅計算量大，而且配準效率低。為此本文提出對點云數(shù)據(jù)進行雙重下采樣，包括粗采樣和精采樣，以降低后續(xù)點云配準的時間復(fù)雜度。

2.1 基于體素下采樣的粗采樣

已有研究表明，體素格濾波器在進行向下采樣的同時不會破壞點云的數(shù)據(jù)結(jié)構(gòu)［16］，故采用體素下采樣方法進行粗采樣。體素下采樣是根據(jù)輸入點云數(shù)據(jù)創(chuàng)建一個三維的體素柵格，每個體素柵格用柵格內(nèi)全部點的質(zhì)心來表示，其計算表達式為

式中：Xi、Yi、Zi分別是第i點的3 個坐標；m是體素柵格內(nèi)點的數(shù)量。

2.2 基于鄰域點云法向夾角下采樣的精采樣

體素下采樣雖然不破壞點云的數(shù)據(jù)結(jié)構(gòu)，但是如果體素柵格過大就會損失點云的特征。為此，采用基于鄰域點云法向夾角的下采樣方法對粗采樣點進行精采樣，進一步減少點云數(shù)量，且保留點云原始特征。

基于法向夾角的下采樣是計算一個點在一個鄰域內(nèi)與其他所有點法向夾角的平均值，如果該均值大于設(shè)置的閾值，則認為該點是特征點，需要保留，計算公式為

式中：θ是平均夾角；m是鄰域內(nèi)所有點的數(shù)量；α是選取點的法向量；β是鄰域內(nèi)其他點的法向量。需要指出的是，這里閾值的選取是精采樣中的重要環(huán)節(jié)，需要考慮兩個因素：一是后續(xù)配準速度要求精采樣后的點數(shù)不能太多，故閾值不能過大；二是后續(xù)配準精度要求精采樣后的點數(shù)不能太多，故閾值不能過小。但目前如何具體設(shè)置閾值仍缺乏統(tǒng)一標準，往往需要針對實際點云數(shù)據(jù)大小及其配準性能要求采用嘗試方式來確定。

3 改進配準算法設(shè)計

3.1 ISS-Super4PCS粗配準

由于Super4PCS 算法對初始位姿具有很好的魯棒性，故選用它進行粗配準。但為了保留點云的原始特征信息，雙重下采樣后的點云數(shù)據(jù)不可能降到最低，直接進行配準計算量仍然較大。針對此問題，常用的解決方法是提取特征點集后再進行配準，可以大幅減少點云數(shù)量、降低時間復(fù)雜度。特別是在常見的幾種特征描述子中，ISS 具有豐富的幾何信息，有利于提高點云配準精度。為此本文提出將ISS和Super4PCS 結(jié)合起來進行粗配準，其改進之處在于引入ISS 算法將下采樣后的初始點云縮減到ISS特征點集，使得Super4PCS 粗配準在特征點集上進行，可以大大縮短四點對提取和匹配時間。

3.1.1 ISS特征點提取

針對雙重下采樣后的點云數(shù)據(jù)，ISS特征點提取的主要步驟如下。

（1）對每一個點Pi設(shè)定一個搜索半徑r。

（2）計算查詢點Pi與鄰域內(nèi)各點的歐氏距離，并設(shè)定權(quán)值Wij，權(quán)值表達式為

（3）計算每個查詢點Pi與鄰域內(nèi)所有點的協(xié)方差矩陣cov(Pi)：

3.1.2 Super4PCS粗配準

與已有方法不同，這里Super4PCS 粗配準是在ISS特征點集上進行的。記點云P、Q分別為源ISS特征點集和目標ISS 特征點集，則Super4PCS 粗配準主要步驟如下。

（1）在源ISS 特征點集中選取4 個共面不共線的4 個點a、b、c、d作為點基（必須是P、Q重疊區(qū)域中的點），且ab和cd相交于點e，夾角為θ，利用式（6）計算兩個比率和兩個點距。

對于Q中每個點qi，以它為球心分別以λ1、λ2半徑做球面，設(shè)置閾值ε、qi與球面[λ1-ε，λ1+ε]范圍內(nèi)的點滿足S1，qi與球面[λ2-ε，λ2+ε]范圍內(nèi)的點滿足S2。

（3）針對S1和S2中的每個對點，連線并存儲對應(yīng)的方向向量角，再基于仿射不變性原理分別計算由比率r1、r2確定的交點e1、e2。

（4）在S1和S2中搜索與e1、e2近似對等的點，且兩點對連線的夾角近似等于θ，則在目標ISS 特征點集中找到點基對應(yīng)的四點集。

（5）根據(jù)對應(yīng)點集和點基計算變換矩陣T，利用T對源ISS 特征點進行變換，統(tǒng)計變換后的點云與目標ISS 特征點集中最近點距離小于某個閾值δ的點的數(shù)目，該數(shù)目表征T的質(zhì)量。迭代進行上述4個步驟，直至找到最優(yōu)的T使得兩個ISS特征點集足夠接近，此時T即是粗配準的結(jié)果。

3.2 線性最小二乘優(yōu)化ICP精配準

經(jīng)過ISS-Super4PCS粗配準后，源點云和目標點云具有良好的初始位姿，再采用點到面ICP 算法進行精配準，可以減少對點云初始位姿的依賴，提高配準精度。具體來說，就是將雙重下采樣后的點云（以下稱D-源點云和D-目標點云）和粗配準變換矩陣T輸入到ICP 配準算法中，用于源點云與目標點云的精配準。

經(jīng)典的點到面ICP 配準算法中常使用非線性最小二乘法來求解，但當兩個點云之間相對旋轉(zhuǎn)角度小于30°時，可以采用線性最小二乘優(yōu)化逼近非線性最小二乘優(yōu)化。本文中經(jīng)過上述粗配準后，D-源點云與D-目標點云之間的位置已經(jīng)非常接近，相對旋轉(zhuǎn)角度可近似為0，為此本文采用了線性最小二乘優(yōu)化求解方法，它不但可以加快ICP 配準速度，而且可以進一步提高配準精度。具體步驟如下。

（1）利用粗配準變換矩陣T對D-源點云進行變換，得到精配準的初始D-源點云。

（2）對D-源點云中的任意點pi，在D-目標點云中搜索與pi相應(yīng)的最近點qi，pi與qi構(gòu)成對應(yīng)點對，從而形成對應(yīng)點對集。

（3）以對應(yīng)點對平均距離最小為目標函數(shù)，采用線性最小二乘優(yōu)化計算得到D-源點云與D-目標點云之間的旋轉(zhuǎn)矩陣R和平移矩陣Γ，由R和?？梢缘玫絼傮w變換矩陣M。

（4）利用剛體變換矩陣M對初始D-源點云進行變換，得到新的D-源點云，再重復(fù)步驟（2）。

（5）設(shè)定閾值ε與迭代次數(shù)k，當對應(yīng)點相對平均距離小于閾值或總迭代次數(shù)大于k時，則迭代結(jié)束；否則依次執(zhí)行步驟（2）～步驟（5），直至獲得最優(yōu)的剛體變換矩陣M，從而完成精配準。

4 基于標準點云數(shù)據(jù)集的算法驗證

為了驗證本文提出的點云改進配準算法，分別選取公開的斯坦福點云數(shù)據(jù)集和自動駕駛場景Kitti數(shù)據(jù)集作為目標點云。硬件環(huán)境為Intel（R）Core（TM）i5-10400F CPU@ 2.90GHz 處理器/8GB 內(nèi)存/64位win10操作系統(tǒng)，軟件環(huán)境為Visual Studio2019/開源點云庫PCL1.11.1/C++語言。

4.1 基于斯坦福點云數(shù)據(jù)的配準算法驗證

采用的斯坦福點云數(shù)據(jù)包括兔子（bunny）點云模型、龍（dragon）點云模型和犰狳（armadillo）點云模型，再人工對上述點云模型進行變換作為源點云。兔子點云模型1 為沿X軸平移0.4 m、繞Y軸旋轉(zhuǎn)45°；兔子點云模型2 為沿X軸反方向平移0.2 m，繞Y軸旋轉(zhuǎn)30°；龍點云模型1 為沿Y軸平移0.4 m、繞X軸旋轉(zhuǎn)45°；龍點云模型2 為沿X軸反方向平移0.2 m，繞Y軸旋轉(zhuǎn)30°；犰狳點云模型1 為沿X軸平移0.4 m、繞Y軸旋轉(zhuǎn)45°；犰狳點云模型2 為沿X軸反方向平移0.2 m，繞Y軸旋轉(zhuǎn)30°。

為了評價點云配準算法的性能，這里選取對應(yīng)點相對距離的均方根（RMSE）作為配準誤差度量指標，RMSE越小，表明配準精度越高；同時計算配準過程所用的時間，用于度量配準的速度。3 種斯坦福點云模型的最終配準結(jié)果如表1 所示（紅色是目標點云、綠色是源點云），且粗配準和精配準結(jié)果分別如表2 和表3 所示，其中精采樣的閾值設(shè)為10°。從表1～表3可以看出：對于不同初始位姿的源點云，均獲得了較好的配準結(jié)果，驗證了本文算法的魯棒性；針對不同的點云模型，均獲得了很好的配準結(jié)果，從而驗證了本文算法的配準精度。

表1 3類斯坦福點云模型

表2 3類斯坦福點云模型粗配準結(jié)果

表3 3類斯坦福點云模型精配準結(jié)果

為了進一步驗證本文算法的優(yōu)越性，這里將本文算法與已有文獻中的改進點云配準算法進行對比。驗證數(shù)據(jù)集為斯坦福兔子點云模型，對比的算法既包括與本文算法相近的ISS+ICP 算法和Super4PCS+ICP 算法，又包括其他文獻里的NDTICP 算法［17］和SAC-IA+ICP 算法［18］，對比驗證計算結(jié)果如表4所示。

由表4 可以看出：本文算法的配準精度明顯高于ISS+ICP 和Super4PCS+ICP，分別提高了約2 個和3 個數(shù)量級，配準速度分別約為ISS+ICP、Super4 PCS+ICP 的47.66%和41.96%，配準速度顯然更快；本文算法的配準精度也明顯高于NDT+ICP 和SAC-IA+ICP，分別提高了2 個和1 個數(shù)量級，配準速度分別 約 為NDT+ICP 和SAC-IA+ICP 的33.33% 和11.11%，配準速度也更快。上述對比結(jié)果充分驗證了本文算法的優(yōu)越性。

表4 與已有配準算法對比結(jié)果

4.2 基于自動駕駛Kitti數(shù)據(jù)集的配準算法驗證

Kitti數(shù)據(jù)集由德國卡爾斯魯厄理工學院和豐田美國技術(shù)研究院聯(lián)合制作的，也是目前國際上自動駕駛場景下常用的評測數(shù)據(jù)集［19］。Kitti數(shù)據(jù)集的采集平臺裝配有2臺灰度攝像機、2臺彩色攝像機、1臺Velodyne 64 線3D 激光雷達、4 個光學鏡頭以及1 個GPS導航系統(tǒng)。這里選用Kitti數(shù)據(jù)集的街道點云數(shù)據(jù)集作為目標點云，再將其沿X軸正方向移動5 m、繞Y軸轉(zhuǎn)動30°變換作為源點云，如圖2所示（其中綠色為源點云，紅色是目標點云），然后利用本文提出的算法進行配準（其中精采樣的閾值設(shè)為15°），結(jié)果如表5 所示，可以看出在街道點云數(shù)據(jù)集上同樣驗證了該算法的配準速度快。

圖2 街道源點云和目標點云數(shù)據(jù)集

表5 自動駕駛街道點云模型配準結(jié)果

5 結(jié)論

針對當前ICP 及其變體配準算法存在的對初始位姿要求較高、配準速度慢等問題，本文提出了一種雙重下采樣方法對初始點云數(shù)據(jù)進行預(yù)處理，在保留點云原始特征的同時大大減少了點云數(shù)據(jù)量；然后集成ISS、Super4PCS 和ICP 算法的優(yōu)點，研究了ISS-Super4PCS粗配準方法和線性最小二乘優(yōu)化ICP精配準算法，并利用斯坦福點云數(shù)據(jù)集和自動駕駛Kitti 點云數(shù)據(jù)集進行了實驗測試和對比驗證，結(jié)果表明本文算法具有更高的配準精度和更快的配準速度，更適用于自動駕駛領(lǐng)域的點云配準。今后，將進一步對本文算法中的閾值設(shè)定問題進行研究，降低閾值對算法配準性能的影響。