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

　　摘要:為了提高遮擋環(huán)境下三維目標點云的識別率,提出將聚類視點特征直方圖(CVFH)算法與方向特征直方圖(SHOT)算法相結(jié)合的特征融合識別(CV-SHOT)算法。利用 CVFH 特征對分割的場景點云進行快速粗識別,提取相似目標點云 SHOT 特征并獲得模型-場景對應點集,通過引入 3D霍夫投票機制對場景目標進行精確識別并獲得點云目標初始位姿。基于最近點迭代(ICP)算法實現(xiàn)目標精確定位以及位姿估計并搭建實驗環(huán)境,測試單物體場景以及多物體部分遮擋場景。結(jié)果表明:CV-SHOT 算法識別率達到90%以上,比傳統(tǒng)的點云識別算法識別率明顯提高、魯棒性更強,可對室內(nèi)復雜場景中目標有效識別與位姿估計。

　　關(guān)鍵詞:機器視覺;三維目標識別;霍夫投票;點云分割;點云配準;位姿估計

　　王青; 賈秀海; 葉明露; 王啟宇; 盛曉超 西安工程大學學報 2021-12-31

　　0 引 言

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

　　針對在復雜場景下點云目標被遮擋和含有噪聲時目標識別率低的問題,本文結(jié)合 CVFH 與方向特征直方圖(SHOT)描述子[9],提出一種改進的 CVSHOT 識別算法,并使用霍夫投票算法優(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ū)域,然后進行目標點云的識別。空間坐標軸各個方向上的距離閾值設(shè)置: Xmin ≤x ≤ Xmax Ymin ≤y ≤Ymax Zmin ≤z ≤Zmax ì î í ï ï ï ï (1) 式中:(Xmin,Xmax)、(Ymin,Ymax)、(Zmin,Zmax)閾值可通過計算獲得。 點云數(shù)據(jù)直通濾波效果如圖 1 所示。

　　圖1中,(a)為原始數(shù)據(jù)點云,其冗余的點云信息主要是背景信息。取Z 方向(-1,0)范圍內(nèi)的點云數(shù)據(jù),直通濾波后的效果圖如(b)所示。濾波前后感興趣區(qū)域的點云數(shù)量、形狀、位置未發(fā)生變化, 原始點云的點數(shù)量為223778,直通濾波后的點數(shù)量為72103,極大地減少了點的數(shù)量,節(jié)約了計算資源。在使用直通濾波的過程中,可以根據(jù)實際情況合理設(shè)置方向閾值,在減少點云數(shù)量和保留感興趣區(qū)域之間達到平衡。深度相機獲取的點云數(shù)據(jù)比較稠密,為了提高后續(xù)點云處理的速度,在保留物體特征信息的基礎(chǔ)上,采用體素濾波算法[14]對點云進行下采樣。體素濾波算法:根據(jù)點云數(shù)據(jù)的特點建立三維體素柵格,組成多個微小立方體,采用小立方體的重心替代該立方體內(nèi)的點,對點云數(shù)據(jù)進行稀疏化。該方法簡單高效,不需要建立復雜的拓撲結(jié)構(gòu),滿足三維點云曲面快速重構(gòu)的需求。每個小立方體重心 (xc,yc,zc)的計算公式為 xc = ∑ n i=1 xi n yc = ∑ n i=1 yi n zc = ∑ n i=1 zi n ì î í ï ï ï ï ï ï ï ï ï ï ï ï (2) 式中:n 為小立方體中的點云數(shù)量,(xi,yi,zi)為小立方體中的第i個點。體素內(nèi)的點用小立方體重心表示,以完成點云數(shù)據(jù)的下采樣。

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

　　1.2 載物臺平面去除

　　采用隨機采樣一致(RANSAC)算法[15]去除載物臺平面。RANSAC 算法通過迭代方式在含有外部點的點云數(shù)據(jù)中估計并優(yōu)化數(shù)學模型,將數(shù)學模型設(shè)置為平面模型,將載物臺平面與場景物體分離。根據(jù)實驗 室 場 景 的 復 雜 度,設(shè) 置 最 大 迭 代 次 數(shù) 為 50,距離閾值為0.01,對場景進行多次平面濾除,去除載物臺平面效果如圖3所示。相比左圖,右圖載物臺主平面幾乎被完全濾除,只有零散的幾處小平面點云以及離群點未被濾除,平面上的物體點云形狀、輪廓保持不變。

　　1.3 離群點濾除

　　為了去除因深度相機采集產(chǎn)生的稀疏離群點、點云邊緣噪聲以及分割載物臺平面留下的離群點, 并同時降低相互遮擋物體的連接性,采用統(tǒng)計離群點算法[16]進行點云數(shù)據(jù)處理,鄰域平均距離的概率密度函數(shù),即 f(li)= 1 2πσ exp - (li -u)2 2σ2 æ è ç ö ø ÷ ,i=1,2,3,… (3) 式中:li 為任意點的鄰域平均距離。該算法對每個點的k 近鄰點進行分析,k 設(shè)置為50。如果當前點距離k近鄰點的平均距離超過整個點云數(shù)據(jù)點之間平均距離的一個標準差以上,則視該點為離群點,離群點濾波效果如圖4所示。

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

　　1.4 點云場景分割

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

　　在點 云 維 度 空 間 建 立 數(shù) 據(jù) 索 引 樹 形 結(jié) 構(gòu) (KDTree),利用 KDTree的最近鄰查詢算法加速歐式聚類的過程。歐式聚類是基于歐式距離判斷是否進行聚類的算法,點云三維空間中,點 (x1,y1,z1) 與點(x2,y2,z2)的歐氏距離,即 dE = (x1 -x2)2 + (y1 -y2)2 + (z1 -z2)2 (4) 首先 在 搜 索 空 間 中 選 取 一 點 p,然 后 利 用 KDTree數(shù)據(jù)結(jié)構(gòu)在搜索范圍內(nèi)找到k 個離p 點最近的點,當搜索點的歐式距離小于設(shè)定閾值時,則被聚類到集合A 中。當集合A 中的點數(shù)不再增加,則完成歐式聚類,否則選取集合A 中除p點以外的點, 重復上述過程,直到 A 中的點數(shù)不再增加,即完成聚類分割。

　　圖5為采用歐式聚類算法分割后的效果圖。從圖5可以看出,當場景中的物體不相互粘連時,場景中的牛奶盒、茶葉罐、布仔、可樂罐被有效聚類。能夠滿足點云目標識別的要求。

　　2 目標識別與位姿估計

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

　　2.1 霍夫投票算法

　　3D霍夫變換用于檢測平面、圓柱、球體以及不規(guī)則幾何體[8]。以霍夫投票的票數(shù)為判斷依據(jù),實現(xiàn)目標點云的識別,并利用關(guān)鍵點局部坐標系的唯一性,獲取識別目標的初始位姿。本文通過計算模型、場景關(guān)鍵點的局部參考系,利用 SHOT 描述子得到的模型-場景對應點集作為投票的特征點,提高霍夫投票的準確性。相機拍攝角度或場景物體的移動,使場景中的待識別物體模型發(fā)生旋轉(zhuǎn)平移,因此在霍夫投票之前, 需要將模型參考向量坐標和場景坐標轉(zhuǎn)換到同一個三維空間中,參考向量空間坐標轉(zhuǎn)換如圖6所示。

　　選取模型質(zhì)心參考點為 CM ,對于每個模型特征點FM i ,計算參考點CM 和特征點FM i 的參考向量 VM i,G ,計算公式為 VM i,G =CM -FM i 為了使參考向量具有旋轉(zhuǎn)平移不變性,模型坐標的參考向量VM i,G 必須轉(zhuǎn)換到相應特征點的局部坐標下,模型局部參考向量VM i,L 轉(zhuǎn)換公式即 VM i,L =RM GL ·VM i,G式中:RM GL 為旋轉(zhuǎn)矩陣,RM GL =[LM i,xLM i,yLM i,z]T,矩陣中每一行分別是3個坐標方向下特征點FM i 及其鄰域內(nèi)點構(gòu)建的局部坐標的單位特征向量。在局部坐標中參考向量具有旋轉(zhuǎn)平移不變性, 則局部坐標下的模型參考向量VM i,L 與場景參考向量 VS i,L 相等,即VS i,L =VM i,L 。最后,全局坐標下的參考向量VS i,G 計算公式: VS i,G =RS LG ·VS i,L +FS j 式中:RS LG 為旋轉(zhuǎn)矩陣,RS LG =[LS j,xLS j,yLS j,z],其每一列分別是3個坐標方向下特征點FS j 及其鄰域內(nèi)點構(gòu)建的局部坐標的單位特征向量。 通過上述變換, 特征點FS j 可以利用參考向量VS i,G 進行霍夫空間投票,將目標識別的問題轉(zhuǎn)化為目標上每個參考向量在霍夫空間上對質(zhì)心投票的問題。

　　對模型-場景的對應點通過上述轉(zhuǎn)換過程進行投票,利用場景中的匹配點確定每次投票的一個質(zhì)心位置,根據(jù)特征向量VS i,G 的投票數(shù)值確定目標質(zhì)心的真實位置。在投票過程中,設(shè)計1個3D 投票數(shù)組進行投票計數(shù),每對質(zhì)心位置投票1次,此數(shù)組對應位置上計數(shù)器加1,循環(huán)投票計數(shù)直到所有特征點完成投票,通過投票峰值以及目標質(zhì)心的位置識別場景中的目標。

　　2.2 目標識別

　　CVFH 是 VFH 的擴展,可有效獲取復雜場景下遮擋目標的特征,CVFH 將待識別的目標點云表面劃分為多個平滑且連續(xù)的區(qū)域,在每個區(qū)域中生成 VFH。因此,一個目標的識別可以由多個平滑且連續(xù)區(qū)域的 CVFH 特征表示。方向直方圖特征(SHOT)描述子將簽名法和直方圖法組合描述點云局部特征,具有旋轉(zhuǎn)及尺度不變性、對點云密度不敏感等特性。在特征點處建立半徑為R 的鄰域空間,將鄰域空間沿縱向劃分為8 份,沿半徑劃分為2份,沿高度劃分為2份,鄰域空間劃分為32份,計算每份中特征點法線ni 與特征點法線np 的夾角余弦值cosθ,將每份空間中的余弦值劃分為11個單元用直方圖統(tǒng)計,則每個特征點的維數(shù)為32×11=352維,其中兩法線夾角余弦值 cosθ,即 cosθ=ni·np (5) 將 CVFH 和 SHOT 描 述 子 結(jié) 合 進 行 目 標 識別。首先對場景點云使用 CVFH 特征進行快速初步識別,得到相似的k 個目標,極大地縮小了目標搜索空間,提高了識別速度;使用 SHOT 描述子進行進一步識別,獲得模型-目標的初始對應點集;計算關(guān)鍵點的局部參考系,采用霍夫投票算法使匹配的點集生成投票向量;最后通過霍夫投票數(shù)濾除匹配的偽對應點,實現(xiàn)目標識別。

　　2.3 位姿估計

　　完成場景中目標識別后,需要進行點云配準,獲得目標物體的精確位姿。點云配準是不斷迭代優(yōu)化的過程,經(jīng)典的ICP算法通過迭代最近點實現(xiàn)點云配準,但是當匹配的點云間位姿相差較大時,容易陷入局部最大值,因此選取霍夫投票高的目標位姿作為配準的初始位姿,實現(xiàn)粗配準,再采用ICP 算法進行精確配準,得到模型庫目標到場景目標的旋轉(zhuǎn)矩陣R 與平移向量t。 ICP算法通過在匹配點集中不斷迭代搜索最近點間的距離平方和獲得最優(yōu)的剛性變換,歐式距離平方和計算公式: E(R,t)= 1 Np∑ Np i=1 ‖xi -Rpi -t‖2 (6) 式中:Np 為匹配點數(shù),xi、pi 分別為模型、場景點。 ICP算法通過不斷迭代,使最近點距離平方和不斷收斂,直到達到設(shè)定的距離閾值或者迭代次數(shù), 并輸出最優(yōu)的旋轉(zhuǎn)矩陣R 和平移向量t。設(shè)α、β、γ 分別為坐標軸的旋轉(zhuǎn)角度,tx 、ty、tz 分別為坐標軸的平移向量,則六自由度參數(shù)為 ψ=(α,β,γ,tx ,ty,tz)

　　3 結(jié)果與分析

　　3.1 算法框架

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

　　3.1.1 離線訓練。

　　Step1 建立目標點云數(shù)據(jù)集模型庫。在實驗室真實場景下,使用深度相機采集單個目標物體各個方 向 的 幾 何、顏 色 信 息,利 用 IntelRealSense SDK 生成目標點云,采集100個目標點云數(shù)據(jù)構(gòu)成模型庫。 Step2 在 目 標 點 云 空 間 中 建 立 KDTree結(jié)構(gòu),計算模型庫中各個點云的 CVFH 描述子,建立物體類別數(shù)據(jù)文件。

　　3.1.2 在線識別。

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

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

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

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

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

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

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

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

　　旋轉(zhuǎn)平移矩陣:M =t(tx ,ty,tz)·R(α,β,γ)= 0.99996 -0.00519 0.00773 0.00503 0.00508 0.99988 0.01464 0.00991 -0.00780 -0.01460 0.99987 -0.00115 0 0 0 1 æ è ç ç ç çç ö ø ÷ ÷ ÷ ÷÷ 轉(zhuǎn)化為六自由度位姿: ψ=(α,β,γ,tx ,ty,tz)= (-0.015 0.008 0.005 0.005 0.010 -0.001)

　　3.2 實驗平臺搭建

　　計算機操作系統(tǒng)為 window10,硬件配置為Intel (R)Core(TM)i3-4005U CPU @ 1.70 GHz (1700MHz),運行內(nèi)存為8GB,結(jié)合PCL1.9.1點云庫,在 VisualStudio2019中編譯運行程序。真實場景中的點云采集設(shè)備使用英特爾公司的 RealSense D435i深度相機,采集實驗室場景制作數(shù)據(jù)集。

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

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

　　從表1可以看出,本文算法識別率比 C-SHOT 算法提高了約3%,相比 C-SHOT、CSHOT-VFH 算法,修正了目標與模型點云位置、形狀相似下目標分割不準確造成的誤匹配,使目標識別率進一步提高。考慮實際生產(chǎn)環(huán)境的復雜性,對多物體(除布仔, 多個相似物品)以及對布仔進行部分遮擋的復雜場景進行識別。同樣,采集200個真實環(huán)境中的復雜場景點云圖作為數(shù)據(jù)來源,多物體識別結(jié)果見表2。

　　從表2可以看出,現(xiàn)有識別算法在復雜環(huán)境下的識別率明顯降低,本文算法通過兩步法識別,利用 SHOT 特征修 正 識 別,降 低 了 相 似 目 標 的 誤 識 別率,識別率顯著提高,高達93.5%,驗證了本文算法對復雜環(huán)境有一定的魯棒性。在位姿估計階段,為了檢驗目標位姿的準確性與穩(wěn)定性,以估計場景中布仔為例,首先對場景中布仔添加不同程度的遮擋,即占原始布仔點云數(shù)據(jù)量的百分比,遮擋強度為無遮擋、10%、20%、30%,不同遮擋程度下的布仔遮擋場景如圖9所示。

　　通過采集布仔不同程度遮擋的場景,測試位姿估計算法以及ICP算法[10],計算并統(tǒng)計布仔的配準耗時與配準得分,不同遮擋程度下布仔配準性能見表3。其中配準得分為配準過程完成后對應點集之間距離平方和的平均值,得分越低,表明配準效果越好。配準得分計算公式: S= 1 n∑ n i=0 (pj -qj)2 (7) 式中:pj 為模型中的點,qj 為模型點pj 在場景中所對應的點,n 為對應點的數(shù)量。

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

　　4 結(jié) 論

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

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

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