基于6D 位姿識別面向任意物體的智能人-機協(xié)同遞送

張樹忠，朱 祺，張 弓?，陳旭飛，楊 根，吳月玉，齊春雨，邸 思

1) 福建理工大學(xué)福建省智能加工技術(shù)及裝備重點實驗室，福州 350108 2) 廣州先進技術(shù)研究所，廣州 511458 3) 中國科學(xué)院大學(xué)，北京 100049

日常實踐中存在大量人與人之間多樣性的物體遞送需求. 例如：在汽車生產(chǎn)線上，工人需要提取零件將其交付給同事；醫(yī)療手術(shù)中，助手接過醫(yī)生用完的手術(shù)工具；家庭生活中，子女幫助臥床的老人傳遞水杯. 隨著協(xié)作機器人的廣泛應(yīng)用，這些看似簡單卻耗時耗力的任務(wù)都可讓協(xié)作機器人作為人類的同事自然而有效地協(xié)同完成，這就是人–機協(xié)同（Human–robot collaboration，HRC）[1].

現(xiàn)有人–機協(xié)同遞送均需要專業(yè)設(shè)備配合[2-3].王憲偉等[4]采用骨骼信息和Red green blue-depth(RGB-D)信息相結(jié)合的方式構(gòu)建人機物體傳遞意圖判別模型，有效地區(qū)分了相同姿態(tài)表達的不同意圖. Chan 等[5]使用距離最小化方法實現(xiàn)了機器人自適應(yīng)選擇遞送方向，實現(xiàn)人–機遞送，但該方法未考慮遞送物體的多樣性. Liu 等[6]使用點云的方式進行目標物體定位，雖有較好的泛化能力，但缺乏準確定位能力，不適于精確抓取場景，如無法選擇抓取藥瓶的瓶蓋、瓶身，也無法準確地抓取手術(shù)刀的刀柄. 基于神經(jīng)網(wǎng)絡(luò)技術(shù)的6D 位姿計算是解決此類問題的有效方法[7]. 例如，Xiang 等[8]提出將Pose convolutional neural network (PoseCNN)網(wǎng)絡(luò)用于計算目標物體的平移矩陣和旋轉(zhuǎn)矩陣，目標物體的平移矩陣通過相機的內(nèi)外參數(shù)計算得到，旋轉(zhuǎn)矩陣通過卷積網(wǎng)絡(luò)訓(xùn)練得出. Wu 等[9]通過神經(jīng)網(wǎng)絡(luò)預(yù)測位姿的間接特征，再計算得到物體的6D 位姿. 根據(jù)間接計算方式不同，6D 位姿計算可以分為N點透視法（Perspective-n-point，PnP）[10]、特征描述方法[11]和模型擬合方法[12].

因此，本文提出一種基于6D 位姿識別面向任意物體的智能人–機協(xié)同遞送系統(tǒng). 通過識別空間中目標物體的6D 位姿并選取相應(yīng)的抓取姿勢，精確定位待遞送物體位姿實現(xiàn)準確抓??；基于點云重建系統(tǒng)制作自定義數(shù)據(jù)集，實現(xiàn)了面向任意物體的精準識別，擴展了遞送系統(tǒng)使用場景；進一步通過坐標轉(zhuǎn)換，將識別物體位姿從物體坐標系轉(zhuǎn)換到機器人基坐標系中，實現(xiàn)了從理論識別到實際抓取的過渡.

1 總體技術(shù)流程

針對遞送過程中待抓取物體的定位問題，本文采用間接法估計目標物體的6D 位姿：首先使用殘差網(wǎng)絡(luò)（Residual network，ResNet）對目標圖片進行關(guān)鍵點向量預(yù)測以及語義分割[13]；其次根據(jù)隨機抽樣一致（Random sample consensus，RANSAC）投票機制對所有關(guān)鍵點進行評分，再利用最遠點采樣法（Farthest point sampling，F(xiàn)PS）確定關(guān)鍵點的最終位置[14-15]；然后利用PnP 方法對求得的關(guān)鍵點進行解算，求得物體在空間中的位姿；再通過標定手眼系統(tǒng)，將識別位姿轉(zhuǎn)換到世界坐標系中進行抓?。蛔詈笸ㄟ^人–機遞送實驗，驗證了該系統(tǒng)可行性和有效性. 具體的技術(shù)流程如圖1 所示.

圖1 人–機協(xié)同遞送系統(tǒng)技術(shù)流程Fig.1 Technical flow of the human–robot collaborative handover system

2 基于ResNet 的物體6D 位姿識別網(wǎng)絡(luò)

2.1 基于ResNet 的向量場預(yù)測和語義分割

理論上，神經(jīng)網(wǎng)絡(luò)的層數(shù)越多，其性能也會越好；但在實際中，過深的網(wǎng)絡(luò)會產(chǎn)生退化問題，性能不增反減. He 等[13]提出了ResNet 結(jié)構(gòu)來解決網(wǎng)絡(luò)退化問題. 該結(jié)構(gòu)在普通網(wǎng)絡(luò)中新增一條分支，將輸入直接疊加到輸出特征中.

以ResNet-18 為主要結(jié)構(gòu)對輸入的RGB 圖像進行訓(xùn)練（圖1），對其進行語義分割和關(guān)鍵點向量預(yù)測. 其結(jié)構(gòu)主要包括全連接層、池化層和兩個卷積層組成的殘差塊. 語義分割即將圖片中的像素點進行分類，從中挑選出目標物體的像素點；關(guān)鍵點向量預(yù)測輸出每個像素點到關(guān)鍵點的方向. 與傳統(tǒng)方法中直接預(yù)測關(guān)鍵點的位置相比，該方法使網(wǎng)絡(luò)集中預(yù)測目標物體特征，直接根據(jù)可見部分預(yù)估關(guān)鍵點位置，避免關(guān)鍵點被遮擋時預(yù)測不準確.

2.2 RANSAC 投票

定義語義分割中識別的待抓取物體的某一像素點坐標為p，某一2D 關(guān)鍵點k坐標為xk，則網(wǎng)絡(luò)中預(yù)測p點的向量為vk(p)，表達式為：

根據(jù)語義分割和關(guān)鍵點向量預(yù)測，可在RANSAC投票中生成假設(shè)關(guān)鍵點. RANSAC 投票從一組包含了干擾點和關(guān)鍵點的數(shù)據(jù)集中，通過迭代的方式估計關(guān)鍵點參數(shù)的數(shù)學(xué)模型. 由于RANSAC 投票機制并不能每次都準確地確定模型，需要進行多次迭代來提高正確率.

首先通過語義標簽確認所有屬于目標物體O的像素點，找到隨機兩個像素對應(yīng)向量的交點；重復(fù)此過程N次，得到一組假設(shè)關(guān)鍵點的集合{hk,i|i=1, 2,···,N}；設(shè)目標物體O 的像素點為集合hk,i，計算 RANSAC 投票分數(shù)wk,i：

式中，Ⅱ為指標函數(shù)；θ為人工定義的一個閾值，選取0.99. 某點的投票分數(shù)越高代表其作為關(guān)鍵點的可能性越高，生成關(guān)鍵點的空間概率分布. 假設(shè)關(guān)鍵點xk的均值為μk，協(xié)方差為則：

為使該算法可以檢測物體的不同角度，可以使用最遠點采樣法將關(guān)鍵點均勻分布在目標物體O 的表面. FPS 算法選取目標物體中心點為初始關(guān)鍵點，然后尋找離該關(guān)鍵點最遠的一個點，將其添加至關(guān)鍵點集合A中，重復(fù)多次，直到該集合中的點達到K個，在多次訓(xùn)練中K=8 時效果最佳.

2.3 PnP 求解

PnP 是求解3D 點到2D 點轉(zhuǎn)換關(guān)系的方法，即已知世界坐標系中n個關(guān)鍵點及其在2D 圖像坐標系中對應(yīng)關(guān)鍵點坐標，求得相機位姿的方法. 目前求解PnP 問題主要有直接線性變換[16]、非線性優(yōu)化[17]、P3P（Perspective-3-points）[18]和EPnP （Efficient perspective-n-points）方法[19]. 直接線性變換只考慮了線性意義下的最優(yōu)解沒有考慮幾何約束問題；非線性優(yōu)化將PnP 問題構(gòu)建為一個關(guān)于重投影誤差的非線性最小二乘問題，該方法準確率較高但計算量較大，無法滿足人–機協(xié)同遞送中的實時性要求；P3P 無法利用多于三組匹配點的信息，容易造成缺解問題；EPnP 方法使用迭代的方式對相機位姿進行優(yōu)化，消除誤差，是目前的主流. 在EPnP 方法的基礎(chǔ)上，引入空間概率分布，采用萊文伯格–馬夸特算法[20]（Levenberg–Marquardt algorithm）代替高斯–牛頓迭代法（Gauss–Newton iteration method）進行迭代計算，優(yōu)化了計算過程，可增加位姿識別的準確性[20-21].

PnP 算法如圖2 所示（以馬克杯為例），首先定義世界坐標系中的n個3D 點{pwi|i=1, 2,···,n}及其對應(yīng)的圖像坐標系中的2D 關(guān)鍵點{pci|i=1, 2,···,n}.世界坐標系和圖像坐標系的關(guān)鍵點可以分別使用4 個控制點{cwj|i=1, 2, 3, 4}和{ccj|i=1, 2, 3, 4}的權(quán)重歸一化來表示：

圖2 PnP 算法示意圖Fig.2 Schematic diagram of the PnP algorithm

式中，αij為均質(zhì)重心坐標（Homogeneous barycentric coordinates），其在世界坐標系和相機坐標系中的數(shù)值一致；w 為世界坐標系；c 為相機坐標系；i和j為計數(shù)點. 原本的3D-2D 問題轉(zhuǎn)換為3D-3D 問題，即求解兩組空間三維點之間的位姿變換. 2D圖像關(guān)鍵點可表示為：

式中，K為相機內(nèi)參矩陣；fu和fv為相機內(nèi)參矩陣中的焦距參數(shù)；uc和vc為相機坐標系的中心坐標位置；xj、yj和zj為空間中第j個3D 點的三維坐標.由式（4）和（5）聯(lián)立可得：

將n個關(guān)鍵點串聯(lián)，可得：

式中，M為聯(lián)立矩陣；x為控制點坐標.

控制點在攝像頭坐標系中的坐標為x=[c1cT,c2cT,c3cT,c4cT]T，即：

式中，c1cT、c2cT、c3cT和c4cT表示相機坐標系中的點；T 為轉(zhuǎn)置；vi是M中N個零特征值所對應(yīng)的特征向量；βi為求解量. 原算法在計算出βi的初始值后可以用高斯–牛頓迭代法對其進行迭代求解，推導(dǎo)旋轉(zhuǎn)矩陣R和平移矩陣T. 基于RANSAC 的投票得到關(guān)鍵點xk的均值μk和協(xié)方差為∑k. 通過最小化馬哈諾比斯距離計算其6D 位姿矩陣：

式中，Xk為關(guān)鍵點在世界坐標系中的三維坐標；~x為關(guān)鍵點在圖像坐標系中的二維坐標； π為透視投影函數(shù)；旋轉(zhuǎn)矩陣R和平移矩陣T的初始值由EPnP算法得出. 現(xiàn)使用萊文伯格–馬夸特算法求解.

鑒于攝像頭為固定安裝，則將求得的RT齊次矩陣求逆后得到目標物體坐標系在攝像頭坐標系中的位姿rt.

3 改進的物體數(shù)據(jù)集制作

常用的6D 位姿識別數(shù)據(jù)集有Yale-CMU-Berkeley (YCB)[22]、 Texture-Less (T-LESS)[23]和Line-Mod[24]，但僅限于某幾樣物體，不能完全覆蓋日常使用場景. 而且由于人–機遞送中所涉及的物體形狀不規(guī)則、光學(xué)特性不統(tǒng)一以及相機存在局限性等因素，基于Augmented reality university of Cordoba(ArUco)標定板[25]的三維重建方法效果不佳.

為此，本文提出一種改進的數(shù)據(jù)集制作方法，即以實際物體為參考進行3D 建模并將其轉(zhuǎn)換為點云模型，然后將物體原始點云模型與第一幀RGB圖像和深度圖像構(gòu)建的殘缺點云重合定位，使完整、準確的點云圖像代替原本三維重建中效果不佳的點云模型，如圖3 所示.

圖3 匹配點云模型Fig.3 Point cloud matching model

由于手動調(diào)整位姿矩陣無法使其完全重合，引入了迭代最近點算法（Iterative closest point，ICP）進行點云匹配[26]. 該算法通過迭代對應(yīng)點搜尋和最小化點對整體距離以估計變換矩陣，無需提取特征點且精度較高，但容易陷入局部極小值. 因此在重合定位過程中，先手動調(diào)整使完整點云與殘缺點云大部分重合，再進行ICP 算法配準，以避免陷入局部最優(yōu). 配準點云后，通過ArUco 標定板輸出語義分割圖像和標準位姿，從而為神經(jīng)網(wǎng)絡(luò)訓(xùn)練提供數(shù)據(jù).

與手動標注相比，ICP 算法簡化了數(shù)據(jù)集的制作過程，可面向各種場景. 圖4 為自制數(shù)據(jù)集的訓(xùn)練結(jié)果（為增加可靠性均進行了兩次訓(xùn)練）. 其中虛線部分為第一次數(shù)據(jù)，實線部分為第二次數(shù)據(jù).從圖中可以看出，訓(xùn)練損失和投票訓(xùn)練損失在第30000 次迭代時接近最小值；測試損失和投票測試損失在第80 次迭代時接近最小值；ADD （Average 3D distance）矩陣（預(yù)測位姿和真實位姿之間的平均距離）和2D 投影矩陣(預(yù)測位姿和真實位姿的3D 模型點投影之間的平均距離)來衡量預(yù)測位姿的準確性，且二者都在第80 次迭代時接近最大值1.

圖4 自制數(shù)據(jù)集訓(xùn)練結(jié)果. (a) 訓(xùn)練損失；(b) 投票訓(xùn)練損失；(c) 測試損失；(d) 投票測試損失；(e) ADD 矩陣；(f) 2D 投影矩陣Fig.4 Training results of the self-made dataset: (a) train loss; (b) train vote loss; (c) test loss; (d) test vote loss; (e) ADD metric; (f) 2D projection metric

4 視覺系統(tǒng)標定及坐標轉(zhuǎn)換

標定在視覺系統(tǒng)中是不可或缺的，其精度決定遞送的準確性. 本文從相機成像原理分析開始，對遞送系統(tǒng)進行相機內(nèi)參及機器人手眼標定，將在神經(jīng)網(wǎng)絡(luò)中識別的物體位姿轉(zhuǎn)換為世界坐標系中的位姿，以進行人–機協(xié)同物體遞送實驗，如圖5所示.

圖5 物體坐標系到基坐標系轉(zhuǎn)換過程Fig.5 Transformation process of the object coordinate system to base coordinate system

4.1 手眼標定

為求解機器人基坐標系與相機坐標系之間的轉(zhuǎn)換關(guān)系，采用眼在手外（Eye on hand）的形式[27]，即相機固定在機器人以外的基座上，不隨機器人一起運動.

相機坐標系為Oc，標定板坐標系為Ow，機器人末端坐標系為Oe，機器人基坐標系為Ob. 以及標定板坐標系到相機坐標系的位姿變換為Twc，相機坐標系到機器人基坐標系的位姿變換為Tcb，機器人基坐標系到機器人末端坐標系的位姿變換為Tbe. 在標定過程中，將標定板固定在機器人末端，使其相對位姿保持不變. 在機器人運動到某一位姿時，標定板上的一點在標定板坐標系Ow中的坐標為P1，其在機器人末端坐標系Ob中的坐標為P3. 因而初始和變換位置后的相對關(guān)系為：

對上式進行聯(lián)立轉(zhuǎn)換，可得：

通過多次變換機器人末端位姿，得到目標矩陣Tcb的值. 本文利用ROS（Robot operating system）中的ros_aruco 功能包，獲取標定板坐標系到相機坐標系的轉(zhuǎn)換關(guān)系Twc；通過遨博機器人SDK（Software development kit）獲取機器人基坐標系到機器人末端坐標系的位姿變換Tbe. 代入Tsai–Lenz 算法[28]中，求解相機坐標系到機器人基坐標系的位姿變換Tcb，其結(jié)果如式（12）所示：

4.2 坐標轉(zhuǎn)換及改進的抓取方案

針對遞送對象的不同，為選取合適的抓取位姿，在物體坐標系中選取可以完全覆蓋目標物體的正方體結(jié)構(gòu)，其八個角點{CiO|i=1, 2,···, 8}即為目標點，如圖5 所示.

基于前文所用PnP 算法求得的目標物體坐標系與相機坐標系的轉(zhuǎn)換矩陣rt，結(jié)合手眼標定得到的相機坐標系與機器人基坐標系的轉(zhuǎn)換矩陣Tcb，可求出機器人基坐標系中的角點坐標{CiR|i=1,2,···, 8}：

為增加遞送系統(tǒng)的魯棒性，將抓取時的位置選取和姿勢選取分開計算，以確保即使抓取姿勢有誤，也不影響最終效果. 如圖5所示，假定最佳抓取點為水杯中心位置，且平行于軸的方向為最佳抓取姿勢. 則計算點1和7的坐標的平均值為抓取位置點；選取軸為z軸正方向，軸為y軸正方向，軸為x軸正方向，以組成抓取坐標系.通過計算逆解，使機器人末端原點到達抓取點，且機器人末端坐標系與抓取坐標系平行，即可完成標準位姿抓取.

5 人–機遞送實驗

5.1 實驗平臺及流程

人–機協(xié)同遞送實驗平臺主要包括：Intel Real-Sense D435i 攝像頭，其可作為人–機協(xié)同系統(tǒng)的“眼睛”，負責拍攝實時圖像并將其傳遞給工作站處理；搭載了NVDIA 3090 GPU 的高性能工作站，可作為系統(tǒng)中樞快速識別目標物體的位姿，同時負責數(shù)據(jù)后續(xù)處理和傳輸；遨博（AUBO）i5 協(xié)作機器人，負責接受物體位姿數(shù)據(jù)，并進行遞送操作，如圖6 所示.

圖6 人–機協(xié)同遞送實驗平臺Fig.6 Human–robot collaborative handover experimental platform

人–機協(xié)同物體遞送操作的主要流程為：（1）人類手持目標物體出現(xiàn)在攝像機和機器人的協(xié)同工作空間；（2）待人類姿勢穩(wěn)定后，拍攝照片并將其輸入至神經(jīng)網(wǎng)絡(luò)以進行位姿識別；（3）將識別后的位姿轉(zhuǎn)換到機器人基坐標系，計算逆解后傳輸給機器人；（4）機器人運行到目標物體位置附近從人類手中抓取物體，然后將其放到指定位置.

5.2 實驗結(jié)果及分析

鑒于人–機協(xié)同物體遞送中的諸多情況，如人類身高的多樣性會導(dǎo)致物體在空間中位置和姿態(tài)不同以及人類服裝顏色對識別的干擾，選取4 位體態(tài)和穿著不一的志愿者，每位均以不同的姿態(tài)、手持LineMod 數(shù)據(jù)集的寵物貓物體，各自進行20 次遞送實驗. 其遞送過程包括：識別、待抓取、已抓取、釋放和放置；物體位姿包括：朝右上方、正前方和左下方，如圖7 所示.

圖7 不同物體位姿情況下的遞送流程（LineMod）. (a) 朝右上方; (b) 正前方; (c) 左下方Fig.7 Handover process (LineMod) at different object postures: (a) top right; (b) forward; (c) lower left

同時，為驗證自制數(shù)據(jù)集的可靠性，以及人–機協(xié)同物體遞送系統(tǒng)的魯棒性，也針對自制數(shù)據(jù)集中的馬克杯物體，進行了與LineMod 數(shù)據(jù)集中物體相同模式的遞送實驗，共80 次，如圖8 所示.

圖8 不同物體位姿情況下的遞送流程（自制）. (a) 朝右上方; (b) 正前方; (c) 左下方Fig.8 Handover process at different object postures: (self-made dataset): (a) top right; (b) forward; (c) lower left

為客觀量化抓取標準，除誤差距離外，還設(shè)定了位置和姿勢兩種衡量標準. 其中，若機器人運動到最終位置后，關(guān)閉夾爪時能保持抓緊物體而不掉落，即位置達標；若機器人抓取物體時，機器人第六軸與目標物體保持平行，則判斷本次抓取姿勢達標.

為保障志愿者安全，實驗中將機器人末端速度僅設(shè)置為0.5 m·s–1. 結(jié)果表明：神經(jīng)網(wǎng)絡(luò)平均識別時間為4.3 s，遞送過程平均總時長約為30 s. 當遞送物體的位置和姿勢同時達標時，則判定實驗成功. 面向LineMod 數(shù)據(jù)集和自制數(shù)據(jù)集中物體的人–機協(xié)同遞送實驗，其結(jié)果如表1 所示.

表1 人–機協(xié)同遞送實驗結(jié)果Table 1 Experimental results of human–robot collaborative handover

從表中可以看出，面向LineMod 數(shù)據(jù)集的物體遞送：平均成功率為88%；平均誤差距離為0.925 cm；在不考慮抓取姿勢是否準確的情況下抓取成功率高達95%，基本達到了人–機協(xié)同遞送要求. 面向自制數(shù)據(jù)集的物體遞送：平均成功率為76%、誤差距離平均值為1.97 cm；在不考慮抓取姿勢是否達標的情況下，遞送成功率可達89%. 相對于LineMod數(shù)據(jù)集，其抓取成功率和平均誤差距離雖然有所降低，但自制數(shù)據(jù)集擴展了人–機協(xié)同遞送系統(tǒng)的多樣性，節(jié)省了面向新的抓取環(huán)境和對象的準備和計算時間，也是未來人–機協(xié)同遞送系統(tǒng)的發(fā)展方向之一.

6 結(jié)論

本文提出一種基于6D 位姿識別面向任意物體的智能人–機協(xié)同遞送系統(tǒng). 引入基于ResNet 算法的物體6D 位姿識別網(wǎng)絡(luò)，實現(xiàn)了待遞送物體位姿的精確識別；提出改進的被遞送物體數(shù)據(jù)集的制作方法，實現(xiàn)了面向任意物體的精準識別；開展從識別圖像坐標系到機器人基坐標系的轉(zhuǎn)換，實現(xiàn)了從理論識別到實際抓取的過渡；進行了面向LineMod 數(shù)據(jù)集和自制數(shù)據(jù)集的物體遞送對比實驗，具有較好的實用性，應(yīng)用前景廣泛. 鑒于自制數(shù)據(jù)集的成功率較低，難以進行隨動性的人–機遞送，未來的研究中，會進一步優(yōu)化自制數(shù)據(jù)集的技術(shù)流程，提升位姿識別的準確性. 同時增加人–機隨動遞送機制，以提升系統(tǒng)的魯棒性.