三維視覺下的目標識別與位姿估計方法

王 青，賈秀海，葉明露，王啟宇，盛曉超

(西安工程大學 機電工程學院，陜西 西安 710048)

0 引 言

基于深度相機采集的三維點云數(shù)據(jù)不僅包括物體表面的幾何特征、顏色信息，而且還包括深度信息[1]，即采集的信息更接近現(xiàn)實場景，因此基于三維點云數(shù)據(jù)的物體識別與位姿估計技術是目前機器視覺領域研究的熱點之一，被廣泛用于機器人環(huán)境感知和導航、無人汽車自動駕駛、自動裝配及虛擬現(xiàn)實等領域[2]。文獻[3]使用快速點特征直方圖計算局部特征，相比點特征直方圖[4]，極大地提高了點云的計算速度，并提出了一種基于樣本一致性的初始對齊算法，解決了點云配準效率低的問題，但對噪聲點云數(shù)據(jù)的魯棒性較差。文獻[5]利用視點特征直方圖(viewpoint feature histogrcm,VFH)獲取待配準點云特征，采用KNN算法和ICP算法估計物體位姿，具有較強的穩(wěn)健性。文獻[6]利用CVFH識別物體，可有效識別遮擋的點云以及包含噪聲的點云。文獻[7]通過使用點對特征對在散亂堆放場景中的物體進行點云匹配和位姿估計，采用投票策略進行模板匹配，在仿真環(huán)境下抓取準確率達到了97.1%，但該算法沒有涉及對復雜場景中多物體的識別。文獻[8]為了識別復雜場景中的多個目標，提出一種基于改進法矢的C-SHOT特征識別目標，使用LM-ICP實現(xiàn)目標點云的位姿估計，對顏色鮮明、區(qū)別性強的物體識別率達到了99%，但對表面光滑、顏色相近的物體識別率較低。

針對在復雜場景下點云目標被遮擋和含有噪聲時目標識別率低的問題，本文結合CVFH與SHOT描述子[9]，提出一種改進的CV-SHOT識別算法，并使用霍夫投票算法優(yōu)化ICP算法[10-11]，對復雜場景中目標進行識別和位姿估計。

1 點云數(shù)據(jù)處理

由于環(huán)境光線、相機本身誤差等原因，深度相機采集的點云數(shù)據(jù)會包含數(shù)據(jù)噪點[12]，并且除了物體點云信息外，還存在多余的背景信息、載物臺平面信息等。為了得到有效的分割聚類以及提高后續(xù)的識別效率，需要去除多余點云。

1.1 數(shù)據(jù)預處理

針對深度相機采集的原始點云數(shù)據(jù)，為了快速找到感興趣區(qū)域，采用點云直通濾波算法[13]去除冗余的背景信息，可以極大地減少點云數(shù)量，完整地保留目標特征的信息。待識別的目標點云與相機的距離保持不變，故可以截取空間坐標軸方向一定范圍內(nèi)的點云數(shù)據(jù)作為感興趣區(qū)域，然后進行目標點云的識別?？臻g坐標軸各個方向上的距離閾值設置：

(1)

式中：(Xmin,Xmax) 、(Ymin,Ymax)、(Zmin,Zmax)閾值可通過計算獲得。點云數(shù)據(jù)直通濾波效果如圖1所示。

(a)原始數(shù)據(jù)點云 (b)直通濾波后的效果圖 1 直通濾波Fig.1 Through filtering

圖1(a)為原始數(shù)據(jù)點云，其冗余的點云信息主要是背景信息。取Z方向(-1，0)范圍內(nèi)的點云數(shù)據(jù)，直通濾波后的效果如圖1(b)所示。濾波前后感興趣區(qū)域的點云數(shù)量、形狀、位置未發(fā)生變化，原始點云的點數(shù)量為223 778，直通濾波后的點數(shù)量為72 103，極大地減少了點的數(shù)量，節(jié)約了計算資源。在使用直通濾波的過程中，可以根據(jù)實際情況合理設置方向閾值，在減少點云數(shù)量和保留感興趣區(qū)域之間達到平衡。

深度相機獲取的點云數(shù)據(jù)比較稠密，為了提高后續(xù)點云處理的速度，在保留物體特征信息的基礎上，采用體素濾波算法[14]對點云進行下采樣。

體素濾波算法：根據(jù)點云數(shù)據(jù)的特點建立三維體素柵格，組成多個微小立方體，采用小立方體的重心替代該立方體內(nèi)的點，對點云數(shù)據(jù)進行稀疏化。該方法簡單高效，不需要建立復雜的拓撲結構，滿足三維點云曲面快速重構的需求。每個小立方體重心(xc,yc,zc)的計算公式為

(2)

式中：n為小立方體中的點云數(shù)量；(xi,yi,zi)為小立方體中的第i個點。體素內(nèi)的點用小立方體重心表示，以完成點云數(shù)據(jù)的下采樣。體素濾波如圖2所示。

圖 2 體素濾波Fig.2 Voxel filtering

從圖2可以看出，體素濾波中小立方體邊長為5 mm，濾波前后點云的位置、形狀保持不變，濾波前的數(shù)量為5 713，濾波后的數(shù)量為1 168，點云數(shù)量減少極大，提高了后續(xù)點云局部特征的計算速度。小立方體邊長可根據(jù)場景情況適當調(diào)節(jié)，在減少點云數(shù)量與保持點云輪廓信息之間達到平衡。

1.2 載物臺平面去除

采用隨機抽樣一致(random sample consensus,RANSAC)算法[15]去除載物臺平面。RANSAC算法通過迭代方式在含有外部點的點云數(shù)據(jù)中估計并優(yōu)化數(shù)學模型，將數(shù)學模型設置為平面模型，將載物臺平面與場景物體分離。根據(jù)實驗室場景的復雜度，設置最大迭代次數(shù)為50，距離閾值為0.01，對場景進行多次平面濾除，去除載物臺平面效果如圖3所示。

圖 3 去除載物臺平面Fig.3 The stage plane removal

圖3中，相比左圖，右圖載物臺主平面幾乎被完全濾除，只有零散的幾處小平面點云以及離群點未被濾除，平面上的物體點云形狀、輪廓保持不變。

1.3 離群點濾除

為了去除因深度相機采集產(chǎn)生的稀疏離群點、點云邊緣噪聲以及分割載物臺平面留下的離群點，并同時降低相互遮擋物體的連接性，采用統(tǒng)計離群點算法[16]進行點云數(shù)據(jù)處理，鄰域平均距離的概率密度函數(shù)，即

(3)

式中：li為任意點的鄰域平均距離。該算法對每個點的k近鄰點進行分析，k設置為50。如果當前點距離k近鄰點的平均距離超過整個點云數(shù)據(jù)點之間平均距離的一個標準差以上，則視該點為離群點，離群點濾波效果如圖4所示。

圖 4 統(tǒng)計離群點濾波Fig.4 Statistical outlier filtering

圖4中，經(jīng)過統(tǒng)計離群點濾波后，相比左圖，右圖中的離群點被濾除，相互接觸的物體也被有效區(qū)別，得到了比較規(guī)則、光滑的多個物體點云聚類，可使后續(xù)的點云得到有效分割。

1.4 點云場景分割

點云分割通常依據(jù)點云的法線、幾何特征、顏色等信息將點云分割為互不相交的多個子集。當場景物體經(jīng)過點云預處理后，點云數(shù)據(jù)量急劇降低，各物體的連接性明顯降低，此時采用歐式聚類[17]分割算法，相比于區(qū)域生長分割[18]、超體聚類分割[19-20]、最小分割[21]算法，可有效快速分割場景，分割實時性滿足目標識別的需要。

在點云維度空間建立數(shù)據(jù)索引樹形結構(KDTree)，利用KDTree的最近鄰查詢算法加速歐式聚類的過程。歐式聚類是基于歐式距離判斷是否進行聚類的算法，點云三維空間中，點(x1,y1,z1)與點(x2,y2,z2)的歐氏距離，即

(4)

首先在搜索空間中選取一點p，然后利用KDTree在搜索范圍內(nèi)找到k個離p點最近的點，當搜索點的歐式距離小于設定閾值時，則被聚類到集合A中。當集合A中的點數(shù)不再增加，則完成歐式聚類，否則選取集合A中除p點以外的點，重復上述過程，直到A中的點數(shù)不再增加，即完成聚類分割。圖5為采用歐式聚類算法分割后的效果圖。

圖 5 場景分割Fig.5 Scene segmentation

從圖5可以看出，當場景中的物體不相互粘連時，場景中的牛奶盒、茶葉罐、布仔、可樂罐被有效聚類。能夠滿足點云目標識別的要求。

2 目標識別與位姿估計

在點云目標識別過程中，最重要的是三維描述子的設計，一個目標能否被有效識別，很大程度上取決于三維描述子獲取目標特征信息的準確性與完整性，三維描述子具有分辨率不變性、強魯棒性和旋轉不變性[22-23]等特點。三維特征按照空間搜索范圍分為局部特征和全局特征[24]。局部特征描述子是對點云數(shù)據(jù)局部特征的描述，不需要對場景點云進行分割，直接計算場景物體局部特征，并與模型庫完成匹配，識別速度快、具有旋轉尺度不變性，但對點云噪聲比較敏感；全局特征是對整個點云數(shù)據(jù)特征的描述，容易忽略細節(jié)信息，為了提高識別率，需要對場景點云進行適當分割。面對復雜環(huán)境，特別是物體相互遮擋、存在相似目標的場景中，本文提出一種復合描述子CV-SHOT，將全局特征的CVFH與局部特征的SHOT描述子結合起來，通過粗識別-精匹配兩步法識別目標，并估計目標在場景中的位姿。

2.1 霍夫投票算法

3D霍夫變換用于檢測平面、圓柱、球體以及不規(guī)則幾何體[8]。以霍夫投票的票數(shù)為判斷依據(jù)，實現(xiàn)目標點云的識別，并利用關鍵點局部坐標系的唯一性，獲取識別目標的初始位姿。本文通過計算模型、場景關鍵點的局部參考系，利用SHOT描述子得到的模型-場景對應點集作為投票的特征點，提高霍夫投票的準確性。

相機拍攝角度或場景物體的移動，使場景中的待識別物體模型發(fā)生旋轉平移，因此在霍夫投票之前，需要將模型參考向量坐標和場景坐標轉換到同一個三維空間中，參考向量空間坐標轉換如圖6所示。

圖 6 空間坐標轉換Fig.6 Space coordinate conversion

2.2 目標識別

CVFH是VFH的擴展，可有效獲取復雜場景下遮擋目標的特征，CVFH將待識別的目標點云表面劃分為多個平滑且連續(xù)的區(qū)域，在每個區(qū)域中生成VFH。因此，一個目標的識別可以由多個平滑且連續(xù)區(qū)域的CVFH特征表示。

SHOT描述子將簽名法和直方圖法組合描述點云局部特征，具有旋轉及尺度不變性、對點云密度不敏感等特性。在特征點處建立半徑為R的鄰域空間，將鄰域空間沿縱向劃分為8份，沿半徑劃分為2份，沿高度劃分為2份，鄰域空間劃分為32份，計算每份中特征點法線ni與特征點法線np的夾角余弦值cosθ，將每份空間中的余弦值劃分為11個單元用直方圖統(tǒng)計，則每個特征點的維數(shù)為32×11=352維，其中兩法線夾角余弦值cosθ，即

cosθ=ni·np

(5)

將CVFH和SHOT描述子結合進行目標識別。首先對場景點云使用CVFH特征進行快速初步識別，得到相似的k個目標，極大地縮小了目標搜索空間，提高了識別速度；使用SHOT描述子進行進一步識別，獲得模型-目標的初始對應點集；計算關鍵點的局部參考系，采用霍夫投票算法使匹配的點集生成投票向量；最后通過霍夫投票數(shù)濾除匹配的偽對應點，實現(xiàn)目標識別。

2.3 位姿估計

完成場景中目標識別后，需要進行點云配準，獲得目標物體的精確位姿。點云配準是不斷迭代優(yōu)化的過程，經(jīng)典的ICP算法通過迭代最近點實現(xiàn)點云配準，但是當匹配的點云間位姿相差較大時，容易陷入局部最大值，因此選取霍夫投票高的目標位姿作為配準的初始位姿，實現(xiàn)粗配準，再采用ICP算法進行精確配準，得到模型庫目標到場景目標的旋轉矩陣R與平移向量t。

ICP算法通過在匹配點集中不斷迭代搜索最近點間的距離平方和獲得最優(yōu)的剛性變換，歐式距離平方和計算公式：

(6)

式中：Np為匹配點數(shù);xi、pi分別為模型、場景點。

ICP算法通過不斷迭代，使最近點距離平方和不斷收斂，直到達到設定的距離閾值或者迭代次數(shù)，并輸出最優(yōu)的旋轉矩陣R和平移向量t。設α、β、γ分別為坐標軸的旋轉角度，tx、ty、tz分別為坐標軸的平移向量，則六自由度參數(shù)為

ψ=(α,β,γ,tx,ty,tz)

3 結果與分析

3.1 算法框架

目標識別算法分為離線訓練與在線識別2個階段，CV-SHOT算法流程如圖7所示。

圖 7 CV-SHOT算法流程Fig.7 CV-SHOT algorithm flow

3.1.1 離線訓練。

1) 建立目標點云數(shù)據(jù)集模型庫。在實驗室真實場景下，使用深度相機采集單個目標物體各個方向的幾何、顏色信息，利用Intel RealSense SDK生成目標點云，采集100個目標點云數(shù)據(jù)構成模型庫。

2) 在目標點云空間中建立KDTree結構，計算模型庫中各個點云的CVFH描述子，建立物體類別數(shù)據(jù)文件。

3.1.2 在線識別。

1) 將原始場景點云進行點云預處理，采用歐式聚類分割算法分割場景物體，得到有效的多個聚類，組成點云集Q。

2) 設置初步識別時匹配的目標模型數(shù)量為k，取k=3，卡方檢測閾值為D，計算點云集Q中第i個聚類的CVFH特征(i=1,2,3,…)，對模型庫進行k近鄰搜索，利用訓練建立的KDTree結構進行近似查找，搜索得到k個小于卡方檢測閾值D的相似點云集合q，完成初步的識別。

3) 將包含k個點云聚類的相似點云集q按卡方距離從小到大排序，采用體素濾波算法提取點云關鍵點，對點云數(shù)據(jù)稀疏化處理。

4) 分別計算SHOT描述子，使用KDTree FLANN方法匹配，通過最近鄰搜索得到相似點，當相似點對的平方距離S<0.25時，則為模型-場景的對應點，遍歷整個點云聚類qm(0

5) 計算關鍵點局部參考坐標系，采用3D霍夫投票算法去除偽匹配點對，精確識別目標，并獲得初始位姿。

6) 根據(jù)獲得的初始位姿，采用經(jīng)典的ICP算法進行點云目標的精確配準，得到場景中目標的最終位姿估計。轉到步驟2，進行識別點云集合Q中下一個候選點云聚類。

7) 在場景中被識別到的目標用綠色標識，并輸出其六自由度位姿；迭代下一幀場景點云。

以識別真實場景中布仔為例，運行本文算法，有效地識別目標并計算布仔位姿。識別效果如圖8所示。

圖 8 識別效果圖Fig.8 Recognition effect diagram

旋轉平移矩陣：

M=t(tx,ty,tz)·R(α,β,γ)=

轉化為六自由度位姿：

ψ=(α,β,γ,tx,ty,tz)=

(-0.015 0.008 0.005 0.005 0.010 -0.001)3.2實驗平臺搭建

計算機操作系統(tǒng)為windows 10，硬件配置為Intel(R) Core(TM) i3-4005U CPU@ 1.70 GHz，運行內(nèi)存為8 GiB，結合PCL 1.9.1 點云庫，在 Visual Studio 2019中編譯運行程序。真實場景中的點云采集設備使用英特爾公司的RealSense D435i深度相機，采集實驗室場景制作數(shù)據(jù)集。

3.3 數(shù)據(jù)集

以識別布仔為例，在布仔場景中分別采用C-SHOT算法[8]、CSHOT-VFH算法[25]、本文三維視覺識別算法進行測試。使用深度相機RealSense D435i采集200個真實環(huán)境中的單物體點云圖作為數(shù)據(jù)來源，單物體識別結果見表1。

表 1 單物體識別結果

從表1可以看出，本文算法識別率比C-SHOT算法提高了約3%，相比C-SHOT、CSHOT-VFH算法，修正了目標與模型點云位置、形狀相似下目標分割不準確造成的誤匹配，使目標識別率進一步提高。

考慮實際生產(chǎn)環(huán)境的復雜性，對多物體(除布仔，多個相似物品)以及對布仔進行部分遮擋的復雜場景進行識別。同樣，采集200個真實環(huán)境中的復雜場景點云圖作為數(shù)據(jù)來源，多物體識別結果見表2。

表 2 多物體識別結果

從表2可以看出，現(xiàn)有識別算法在復雜環(huán)境下的識別率明顯降低，本文算法通過兩步法識別，利用SHOT特征修正識別，降低了相似目標的誤識別率，識別率顯著提高，高達93.5%，驗證了本文算法對復雜環(huán)境有一定的魯棒性。

在位姿估計階段，為了檢驗目標位姿的準確性與穩(wěn)定性，以估計場景中布仔為例，首先對場景中布仔添加不同程度的遮擋，即占原始布仔點云數(shù)據(jù)量的百分比，遮擋強度為無遮擋、10%、20%、30%，不同遮擋程度下的布仔遮擋場景如圖9所示。

(a) 無遮擋 (b) 10%遮擋 (c) 20%遮擋 (d) 30%遮擋圖 9 不同遮擋程度下的布仔Fig.9 The fabric objects in under different degrees of occlusion

通過采集布仔不同程度遮擋的場景，測試位姿估計算法以及ICP算法[10]，計算并統(tǒng)計布仔的配準耗時與配準得分，不同遮擋程度下布仔配準性能見表3。

其中配準得分為配準過程完成后對應點集之間距離平方和的平均值，得分越低，表明配準效果越好。配準得分計算公式：

(7)

式中：pj為模型中的點;qj為模型點pj在場景中所對應的點;n為對應點的數(shù)量。

表 3 不同遮擋程度下布仔配準性能

從表3可以看出，隨著布仔遮擋程度的增加，相比ICP算法，本文算法的配準得分與配準耗時增加幅度較小，配準得分均小于ICP算法，且得分保持在10 μm級別，配準時間也有所下降，因此，本文提出的位姿估計算法在存在目標部分遮擋情況下仍能保持較好的配準效果，對復雜場景具有一定的魯棒性。

4 結 論

1) 面對復雜場景下的三維目標識別，本文改進了傳統(tǒng)的CVFH識別算法，提出了一種將CVFH算法與SHOT算法相結合的特征融合識別算法，并將3D霍夫變換與ICP算法結合，實現(xiàn)了復雜環(huán)境下的目標識別與位姿估計。

2) 在單物體場景、部分遮擋的多物體場景中，驗證了本文算法比傳統(tǒng)識別算法識別率有效提高，達到了90%以上。

3) 對相同位置、不同復雜場景中的目標進行位姿估計，本文位姿估計算法準確度較高，可滿足對三維目標有效識別與定位的需求。