基于關(guān)鍵點數(shù)量及分布的多視角位姿觀測融合方法研究

孫錚 ，姜濤 ，2

（1.長春理工大學(xué) 機電工程學(xué)院，長春 130022；2.長春理工大學(xué) 重慶研究院，重慶 401135）

工業(yè)領(lǐng)域的快速發(fā)展對生產(chǎn)自動化水平提出了更高的要求。作為視覺SLAM領(lǐng)域與Bin Picking領(lǐng)域的基礎(chǔ)與核心，復(fù)雜工業(yè)應(yīng)用場景中多目標物體精確定位與姿態(tài)估計問題得到了學(xué)術(shù)界的極大關(guān)注。

以LineMode[1]為代表的基于模板匹配的位姿估計方法由于模型投影數(shù)據(jù)庫大小的限制，在應(yīng)對遮擋等復(fù)雜場景時準確度較低。而基于深度學(xué)習理論的位姿檢測方法由于其在光照變化以及場景混疊下的優(yōu)異性能得以廣泛應(yīng)用，這類方法依據(jù)輸入數(shù)據(jù)形式的不同分為：基于點云配準的位姿估計和基于RGB圖像匹配的位姿估計[2]。基于點云配準的位姿估計方法依賴于所獲場景點云精度，在采集信息豐富的條件下可獲得較好的位姿估計結(jié)果。JOHNSON等人[3]提出利用3D點云空間分布進行特征描述進而進行位姿匹配的方法Spin Images，該方法將定區(qū)域點云轉(zhuǎn)換為2D的旋轉(zhuǎn)圖像，度量匹配場景與模型中的2D旋轉(zhuǎn)圖像相似性。WANG C[4]提出的DenseFusion方法通過特征融合的方法將RGB圖像與深度圖像進行了統(tǒng)一處理，利用二維信息增強每個三維點的信息，該方法在高度混亂的場景具有較高的運算速度。但是，基于點云匹配的位姿估計方法由于點云性質(zhì)，難以對場景中存在嚴重遮擋的多目標物體產(chǎn)生良好的位姿評估結(jié)果，而基于RGB圖像匹配的位姿估計算法由于RGB圖像較小的數(shù)據(jù)處理量以及相較而言較為穩(wěn)定的信息表征能力在6Dof剛體位姿檢測領(lǐng)域有著較好的表現(xiàn)。RAD等人[5]提出的BB8算法通過兩次分割的方法預(yù)測對象3D邊界框角點對應(yīng)的2D投影進而整體性的回歸3D姿態(tài)，訓(xùn)練過程中使用受限范圍內(nèi)旋轉(zhuǎn)對象作為輸入圖片，巧妙地解決了“近似對稱”問題。相較于通過單視角圖片預(yù)測評估位姿，融合多視點信息進而評估物體位姿的方法對遮擋場景敏感性更低。SU等人[6]提出的MVCNN利用三維模型在環(huán)形12視角下的二維渲染圖融合提取三維形狀描述子實現(xiàn)對三維圖像的分類識別。

綜上所述，針對工業(yè)遮擋場景中物體位姿難識別問題，提出了一種基于關(guān)鍵點數(shù)量及分布的融合多視角位姿評估結(jié)果的多視角位姿檢測方法。通過相機與UR10機械臂組合所構(gòu)建的手眼系統(tǒng)采集場景RGB圖像。利用機械臂的移動特性，在根據(jù)關(guān)鍵點分布生成的觀測位點組下對待檢測物體進行觀測，并融合位姿結(jié)果。經(jīng)實驗驗證，在強遮擋場景下，多視角位姿檢測結(jié)果較單視角位姿檢測結(jié)果更加可靠。

1 基于手眼系統(tǒng)的多視角位姿檢測方法

圖1為提出的基于手眼系統(tǒng)的多視角位姿檢測方法，三維視覺測量系統(tǒng)有三個主要任務(wù)：（1）利用手眼系統(tǒng)獲取場景RGB信息，并基于像素投票理論采用深度學(xué)習的方式生成關(guān)鍵點假設(shè)；（2）根據(jù)關(guān)鍵點數(shù)量，選擇視角檢測模式，依據(jù)關(guān)鍵點分布選擇多視角觀測位點組，控制手眼系統(tǒng)移至觀測位點組拍攝當前幀；（3）利用PnP算法求解目標物體各視點下位姿，并將各位姿統(tǒng)一還原至手眼系統(tǒng)基坐標系下通過回歸算法實現(xiàn)位姿融合。

圖1 基于手眼系統(tǒng)的二次位姿檢測方法

1.1 基于像素投票理論的關(guān)鍵點檢測

在遮擋場景下推斷具有良好空間表述性能關(guān)鍵點的基本思路是：利用深度殘差網(wǎng)絡(luò)構(gòu)建像素級向量場表示并對場景進行語義分割，利用隨機采樣一致性剔除離群值，通過投票得到關(guān)鍵點假設(shè)。該方法利用剛體局部特征與剛體整體之間的空間關(guān)系，使得算法在遮擋下仍可推斷非可視區(qū)域的關(guān)鍵點分布，魯棒性較高。

首先，通過最遠點采樣法在異形物體三維模型表面選擇良好空間結(jié)構(gòu)表征能力的關(guān)鍵點，在模型點云Ωα選擇初始點及距其最遠點后，可根據(jù)歐式距離比較選擇第m點，計算公式如下：

通過遞推的形式減少迭代過程中的重復(fù)計算：

為檢測二維場景圖像中目標物體關(guān)鍵點，在像素投票網(wǎng)絡(luò)[7]的基礎(chǔ)上，選擇 ResNext[8]網(wǎng)絡(luò)作為backbone，在保持精度的同時以降低原網(wǎng)絡(luò)的復(fù)雜度。

網(wǎng)絡(luò)對場景圖像進行語義分割，為使其達到像素級精度，對逐個像素計算Softmax分類，使用帶有權(quán)重形式的多分類交叉熵損失函數(shù)訓(xùn)練，使得網(wǎng)絡(luò)更加針對工業(yè)場景檢測。

式中，y為網(wǎng)絡(luò)輸出結(jié)果；class為實際類別的索引值，其并不直接參與計算；weight[class]為設(shè)置的類別權(quán)重。

構(gòu)建像素級向量場，以向量歸一化的形式τk(xi)表示二維圖像中具有特定語義標簽的像素點xi指向具有相同語義的第k個關(guān)鍵點xk的方向：

式中，||·||2為2-范數(shù)運算符，表述兩像素間的歐式距離。

選擇對離群點不敏感的smoothl1損失函數(shù)來學(xué)習單位向量，進一步防止由離群點可能造成的梯度爆炸問題。

在同語義分割區(qū)域隨機選取兩個像素，并將二者對應(yīng)歸一化向量交點作為關(guān)鍵點位置假設(shè)，重復(fù)n次構(gòu)建關(guān)鍵點假設(shè)數(shù)據(jù)集利用隨機采樣一致性[9]剔除離群預(yù)測，得到關(guān)鍵點分布，并得到第j次所得關(guān)鍵點假設(shè)投票得分，其定義為：

式中，Ι為指示函數(shù)，表示元素從屬關(guān)系；θ為判別閾值。選擇投票得分最高的K個點作為通過圖像檢測的關(guān)鍵點。

1.2 基于關(guān)鍵點數(shù)量分布的手眼系統(tǒng)二次移動觀測策略

為降低遮擋造成的影響，BB8、Segmentationdriven[10]與 Oberweger[11]等 算法 通 過 對 輸 入 圖 像網(wǎng)格劃分完成區(qū)域分割，根據(jù)分割結(jié)果推斷表述目標物體關(guān)鍵點，實現(xiàn)了從場景到物體局部特征的表述。

受其啟發(fā)，期待通過物體局部特征表述其所處場景空間信息，提出了一種根據(jù)可視關(guān)鍵點數(shù)量及分布推斷空間視角信息豐富程度的方法，并以此來指導(dǎo)手眼系統(tǒng)的移動采集。

設(shè)經(jīng)初始觀測位點下經(jīng)1.1節(jié)評估所得可視關(guān)鍵點數(shù)量為Nk，根據(jù)可視關(guān)鍵點數(shù)量判斷視角空間信息豐富度情況，將位姿評估策略分為四種情況：

情況一：目標物體本身存在結(jié)構(gòu)自遮擋，當可視關(guān)鍵點數(shù)量Nk≥6，屬于單視角無遮擋觀測，直接調(diào)用1.3節(jié)位姿回歸算法進行位姿估計。

情況二：6＞Nk≥4，其至少包含某一視角下全部信息，屬于單視角觀測弱遮擋觀測，利用可視關(guān)鍵點位置判別法為其分配觀測位點組。

情況三：4＞Nk＞0，其于該視角下所得目標物體結(jié)構(gòu)幾何信息過少，屬于單視角觀測強遮擋觀測，利用可視關(guān)鍵點位置判別法為其分配觀測位點組。

情況四：Nk=0，依據(jù)FPS算法中最遠距視界信息思想為其分配觀測位點組D2中視點P3,2，重新拍攝計算其中關(guān)鍵點并對比情況二與情況三判定其觀測模式。若皆不屬于此二類，則判定該場景下無待檢測對象。

針對第二、三、四種情況下二次觀測需求，融合MVCNN中提出的12個全視角觀測模式以及物體多視圖表示中使用的空間正十二面體投影模式，在空間三維球坐標系下構(gòu)建三層錯位五邊形視點作為觀測位點組，如圖2（b）所示。記第i層第j個視點為Pi,j。由于考慮到機械臂移動位移極限及視點分布結(jié)構(gòu)特性，舍棄了遠視點P2,3、P2,4、P3,3，P4,3，圖2為手眼系統(tǒng)觀測位點投影分布。

圖2 觀測視點投影分布

觀測位點可由圖2（c）所示的無向圖G(V,E)表示，共分四組：第一組D1={P2,1,P2,2,P3,1,P4,1}；第二組D2={P2,2,P3,2,P4,2}；第三組D3={P2,5,P3,4,P4,4}；第四組D4={P2,5,P2,1,P3,5,P4,5}；D1與D4在形狀上構(gòu)成內(nèi)心四點視角，D2與D3由于視點限制構(gòu)成均分內(nèi)心三點視角，其視角盲區(qū)由空間三維球坐標系漸變特性彌補。

將目標物體根據(jù)1.1節(jié)所得可視關(guān)鍵點與不可視關(guān)鍵點像素坐標(uk,vk)擬合形成邊框包圍盒：

定義?γ為網(wǎng)格分割后的包圍區(qū)域，以矩陣的形式表示包圍盒各網(wǎng)格元素值。將?γ中關(guān)鍵點順序連接，構(gòu)造粗略擬合物體形貌的網(wǎng)格分割單元圖像??。對??中單目標物體可視關(guān)鍵點單元、可視網(wǎng)格單元、不可視網(wǎng)格單元、不可視關(guān)鍵點單元以及?γ中另包含的無效單元賦不同的矩陣A元素值，如圖3所示。

圖3 目標物體網(wǎng)格單元分割

分別判別粗擬合網(wǎng)格分割單元像素橫坐標uc,k與檢測場景位置分割中線關(guān)系，判別公式如下：

式中，Δs為中線右側(cè)網(wǎng)格單元投票分數(shù)Nr與左側(cè)網(wǎng)格單元投票分數(shù)Nl數(shù)值差值；λc為網(wǎng)格單元性質(zhì)權(quán)重；W為觀測區(qū)像素寬度。

當兩側(cè)差值Δs＞0時，判定物體于空間右側(cè)具有更充分可視信息，選擇右視位點組Dr={D1,D2}。反之選擇左視位點組Dl={D3,D4}。

根據(jù)網(wǎng)格單元圖像中不可視關(guān)鍵點與1.1節(jié)中檢測所得其他物體語義分割圖像像素位置關(guān)系，判別其遮擋情況：

式中，??為粗擬合網(wǎng)格包圍區(qū)域；δs為互遮擋判別閾值；ηc為網(wǎng)格單元性質(zhì)權(quán)重；so(??)為粗擬合類別差 異；分別 為某點下神經(jīng)網(wǎng)絡(luò)對區(qū)域??中真實可視物體OT與待檢測目標物體Op所輸出的歸一化類別概率。

若不滿足判別條件則判定其為結(jié)構(gòu)自遮擋，初視角下結(jié)構(gòu)信息充足，無需考慮其視點深度造成的位點選擇影響，選擇前置觀測位點組Df={D1,D4}。反之待檢測目標物體存在互遮擋，初始視角下結(jié)構(gòu)信息不足，選擇后置觀測位點組Db={D2,D3}。

選擇左右位點判別與前后位點判別交集D?=[Dr∨Dl]∩[Df∨Db]為所選觀測位點組，控制手眼系統(tǒng)采集所選位點組下各視點場景信息，對二次觀測下各視角場景采集圖像使用1.1節(jié)中方法分別評估關(guān)鍵點。

1.3 多視角位姿融合與還原

對1.2節(jié)所獲二維關(guān)鍵點使用EPnP算法構(gòu)建其與三維關(guān)鍵點間的映射關(guān)系，完成對各視點下位姿的求解，得到物體坐標系Oobj到相機坐標系Oc間的變換Tobj2c。為保證后續(xù)優(yōu)化過程中數(shù)據(jù)的一致性，將各視點下位姿求解結(jié)果轉(zhuǎn)換至統(tǒng)一的機械臂基坐標系下進行表示。

在空間變換過程中，通過Kalibr標定相機內(nèi)參，獲取相機坐標系與像素坐標系變換關(guān)系；通過easy_handeye進行手眼標定，獲取末端執(zhí)行器坐標系Oe與相機坐標系Oc間變換關(guān)系Tc2e。通過對ROS系統(tǒng)下的直接訪問，獲取末端執(zhí)行器坐標系Oe與機械臂基坐標系Ob關(guān)系Te2b。通過鏈式傳遞完成關(guān)系變換：

對轉(zhuǎn)換至統(tǒng)一的機械臂基坐標系下表示的多視角位姿估計結(jié)果進行融合，將多視角位姿融合問題拆分為由向量空間描述的位置回歸融合與由特殊正交群表述的姿態(tài)回歸融合。

設(shè)位置向量?∈?3，向量描述的位置回歸融合可表示為：

對比黎曼距離與Frobenius標準范數(shù)對兩個SO(3)元素之間距離的表述，選擇幾何表述更自然的黎曼距離衡量最短測地線弧長，將姿態(tài)回歸融合問題表述為：

式中，Q∈SO(3)為各視角下姿態(tài)表示；Qp為姿態(tài)回歸值；ωi為各距離值權(quán)重；d(Q,Qi)為黎曼距離；log(Q)為旋轉(zhuǎn)矩陣主對數(shù)函數(shù)，可表示為：

使用Manton算法[12]將各位姿觀測偏差的so(3)均值作為更新量，迭代求解數(shù)值解。

將Karcher均值Qp賦初值：

計算其切空間均值：

更新當前姿態(tài)：

式中，exp為矩陣指數(shù)函數(shù)，表示SO(3)與所對應(yīng)的so(3)間的指數(shù)映射。

重復(fù)求解式（16）與式（17），直至切空間均值q的低于初始設(shè)定迭代止斷容差ε，并將最終所得Karcher均值Qp作為姿態(tài)回歸結(jié)果[13]。對比通過SE(3)直接對多重估計結(jié)果回歸的方法，將位姿融合問題先分解后回歸的方法在面對擾動時回歸值與真實值均方根誤差變化更小。

圖4 均方根誤差對比

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

2.1 單視角位姿檢測網(wǎng)絡(luò)訓(xùn)練與結(jié)果分析

為驗證網(wǎng)絡(luò)在單視角遮擋場景下位姿檢測結(jié)果精度，設(shè)計驗證實驗。網(wǎng)絡(luò)部署在Ubuntu16.04操作環(huán)境，使用i7-10700K CPU以及NVIDIA GeForce RTX 3060（12G）顯卡。利用Pytorch框架運行深度學(xué)習檢測算法，主要采用OpenCV計算機視覺庫，Eigen矩陣運算庫等開源視覺庫實現(xiàn)算法。在公開數(shù)據(jù)集LINEMOD與自建數(shù)據(jù)集上進行單次位姿檢測網(wǎng)絡(luò)訓(xùn)練，通過TensorboardX實現(xiàn)訓(xùn)練數(shù)據(jù)可視化。

網(wǎng)絡(luò)訓(xùn)練過程輸入圖片尺寸640×480，選擇RAdam算法作為優(yōu)化器，并利用其對初始學(xué)習率不敏感的特性，設(shè)置初始學(xué)習率為0.01，迭代輪次設(shè)置為240，訓(xùn)練批大小設(shè)置為8。圖5（a）顯示網(wǎng)絡(luò)訓(xùn)練過程中損失值變化，隨迭代增加，訓(xùn)練損失迅速下降。圖5（b）與圖5（c）顯示了位姿測試精度在訓(xùn)練過程中的變化，分別度量的是位姿真值與位姿估值下模型點云間的平均距離，符合設(shè)定閾值的點數(shù)占比和位置姿態(tài)估值與真值符合容差5 cm與5°間占比[14]。觀察可知，訓(xùn)練圖片空間表征信息充足，所得網(wǎng)絡(luò)在無遮擋場景下位姿評估誤差較低。

圖5 網(wǎng)絡(luò)訓(xùn)練損失及位姿度量變化

使用訓(xùn)練后的模型在Occluded LINEMOD遮擋數(shù)據(jù)集與上述數(shù)據(jù)集上分別測試位姿檢測結(jié)果，使用邊框包圍盒的方式顯示標注位姿真值與位姿估值，如圖6所示。

圖6 單視角遮擋場景下位姿可視化結(jié)果

表1給出了測試過程中各數(shù)據(jù)集下多種目標檢測物體位姿精度評估均值，觀察可知在無遮擋條件下位姿檢測結(jié)果精度較高，在場景遮擋的情況下，僅通過單一視角圖片對位姿進行預(yù)估與真實位姿存在較大誤差。

表1 各數(shù)據(jù)集下位姿評估結(jié)果

表2給出了在Occluded LINEMOD遮擋數(shù)據(jù)集下測試過程中不同關(guān)鍵點表示方式對位姿評估精度ADD（-S）與圖片處理速度的影響。BBX-8、FPS-4與FPS-8分別表示8點邊框包圍法、4點最遠采樣法與8點最遠采樣法三種關(guān)鍵點表征方法。實驗結(jié)果表明，不同關(guān)鍵點檢測與表征方式對算法運行速度并無明顯影響，同時通過8點最遠點采樣法所得遮擋位姿評估結(jié)果具有更高精度，即該關(guān)鍵點檢測法具有更強的空間信息表征能力。

表2 不同關(guān)鍵點表示方式下位姿評估結(jié)果

2.2 二次位姿檢測與結(jié)果分析

為驗證基于關(guān)鍵點數(shù)量及分布的融合多視角位姿評估結(jié)果的二次位姿預(yù)估方法相較于單視角位姿檢測方法的精度提升，構(gòu)建機械臂手眼系統(tǒng)實驗平臺。硬件部分主要由：UR10機械臂、Intel Realsense D435i深度相機組成。系統(tǒng)框架基于ROS機器人控制系統(tǒng)搭建。

圖7展示了二次位姿檢測策略下單組實驗中關(guān)鍵點檢測、觀測策略下視點拍攝、位姿結(jié)果可視化的過程。如圖7（a）所示，對存在互遮擋與自遮擋的場景物體通過手眼系統(tǒng)進行檢測。將初始視點拍攝結(jié)果圖7（b）作為1.1節(jié)構(gòu)建的網(wǎng)絡(luò)輸入，所輸出關(guān)鍵點分布情況如圖7（c），可知過少的可視關(guān)鍵點無法充分表征物體空間信息。

圖7 實驗場景及結(jié)果

基于其關(guān)鍵點數(shù)量及分布檢測結(jié)果，依據(jù)1.2節(jié)所述理論為其分配觀測位點組D3，進而控制手眼系統(tǒng)在P2,5、P3,4、P4,4視點下拍攝，對所采集的圖像進行位姿評估。圖7（d）、圖7（e）、圖7（f）為多視角位姿評估中多視點下位姿評估可視化結(jié)果。由圖7（f）可知，截斷情況下位姿評估可視化邊框包圍盒與真值包圍盒沒有較好的重合，單視角位姿評估結(jié)果存在一定誤差。對多視角下各位姿評估結(jié)果進行融合，并與單次位姿識別結(jié)果進行對比，精度誤差對比結(jié)果如表3所示。

表3 位姿檢測精度對比

表3展示了兩種位姿評估模式所得位置姿態(tài)與ArUco碼標定真值間偏差絕對值，以及通過ADD、ADD-S評估所得位姿準確性。

對比多視角位姿融合結(jié)果與初始視點單視角位姿評估結(jié)果，同組實驗中位置度量精度提升了0.069 m，姿態(tài)誤差降低了0.155 rad，平均距離度量提高了66.63%?？芍?，多視角位姿融合策略對強遮擋場景中位置與姿態(tài)修正效果明顯。

對比多視角位姿融合結(jié)果與多視角觀測模式下P3,4單視點位姿評估結(jié)果，采用位姿融合策略后位置度量精度提升了0.004 m，姿態(tài)誤差降低了0.008 rad。對比結(jié)果表明，位姿融合策略對于弱互遮擋場景中位姿結(jié)果仍有不錯的修正效果。

在相同的實驗平臺下，改變檢測物體的種類及位置姿態(tài)，進行20組（8類物體）實驗，實驗結(jié)果如表4所示。數(shù)據(jù)表明，位姿評估過程中姿態(tài)平均偏差相較于位置平均偏差對遮擋更為敏感，所采用的二次位姿檢測方法相較于單視點檢測對姿態(tài)偏差修正效果更為明顯。

表4 位姿檢測精度偏差均值對比

上述檢測實驗驗證了基于關(guān)鍵點分布的融合多視角位姿評估結(jié)果的二次位姿預(yù)估方法在遮擋場景下的算法有效性。在測量操作方面，提出的方法集成于ROS框架下，各檢測移動步驟皆通過手眼系統(tǒng)實現(xiàn)，自動化程度及測量效率更高。

3 結(jié)論

提出了一種基于關(guān)鍵點數(shù)量及分布的融合多視角位姿評估結(jié)果的多視角位姿檢測方法，通過在關(guān)鍵點檢測步驟后引入二次網(wǎng)格分割思想，根據(jù)其于分割網(wǎng)格上的位置數(shù)量及分布選擇手眼系統(tǒng)多視角觀測位點組，并對手眼系統(tǒng)各視點下位姿檢測結(jié)果進行融合作為位姿檢測最終結(jié)果。相較于傳統(tǒng)的單視角位姿檢測方法，利用手眼系統(tǒng)進行多視角位姿檢測所得結(jié)果在復(fù)雜場景下算法魯棒性更高。經(jīng)實驗驗證，在強遮擋場景下，融合多視角位姿檢測結(jié)果所得位姿檢測較單視角下位姿檢測結(jié)果精度更高。