基于3D目標(biāo)跟蹤算法的機(jī)器人手眼協(xié)調(diào)研究

段肖，馬鋼，危輝

（復(fù)旦大學(xué) 計(jì)算機(jī)科學(xué)技術(shù)學(xué)院認(rèn)知算法模型實(shí)驗(yàn)室，上海 200433）

機(jī)器人手眼協(xié)調(diào)[1-4]是指機(jī)器人感受器與控制器之間的協(xié)調(diào)控制策略。傳統(tǒng)流水線上的工業(yè)機(jī)器人，它們面對(duì)固定不變的任務(wù)，因而其操作流程和動(dòng)作序列在設(shè)定后就可以保持不變，因此即使是盲操作(無(wú)需反饋)也足以完成任務(wù)。而類似于服務(wù)機(jī)器人的非工業(yè)流水線機(jī)器人所面對(duì)的環(huán)境和任務(wù)是不可預(yù)知的，這就需要實(shí)時(shí)感知這些狀態(tài)的變化，并做出正確的調(diào)整。高級(jí)哺乳動(dòng)物，如人應(yīng)對(duì)這種任務(wù)的能力就特別強(qiáng)。基于Goodale和Milner提出的視覺(jué)雙通路理論[5-6]，我們大腦是這樣工作的，視覺(jué)信息經(jīng)過(guò)腹側(cè)通路產(chǎn)生知覺(jué)，然后背側(cè)系統(tǒng)進(jìn)行視覺(jué)引導(dǎo)。信息加工能夠在短時(shí)間內(nèi)完成，保證了由視覺(jué)引導(dǎo)的動(dòng)作是實(shí)時(shí)且動(dòng)態(tài)調(diào)整的。

因此，帶反饋和閉環(huán)控制的手眼協(xié)調(diào)為完成這種具有各種可變性的機(jī)器人抓取任務(wù)提供了可能，使它們變得更加智能，不再局限于一成不變的環(huán)境和任務(wù)中。

機(jī)器人手眼協(xié)調(diào)的前提是準(zhǔn)確的三維感知以獲取目標(biāo)對(duì)象的狀態(tài)。物體跟蹤[7]是機(jī)器人領(lǐng)域的核心技術(shù)，三維物體位姿追蹤通過(guò)輸入的圖像序列分析確定目標(biāo)物體在三維空間的位姿參數(shù)，該參數(shù)包括位置分量和姿態(tài)角。針對(duì)剛性物體位姿追蹤已有多種研究思路，并取得很大進(jìn)展。主要分為以下幾類方法：

基于特征匹配[8-10]的方法，這種方法傾向于檢測(cè)圖像中的邊緣、角點(diǎn)等基于梯度的特征，再將這些特征與3D模型匹配進(jìn)行位姿估計(jì)。這種方法要求檢測(cè)對(duì)象具有良好的紋理，同時(shí)受運(yùn)動(dòng)模糊、低光照條件以及與攝像機(jī)距離等條件的限制。

基于學(xué)習(xí)[11]的方法，目前深度學(xué)習(xí)技術(shù)在位姿估計(jì)[12-14]和目標(biāo)跟蹤領(lǐng)域已取得很大進(jìn)展，但在位姿追蹤領(lǐng)域的研究工作較少。 Crivellaro等[11]首次提出利用深度神經(jīng)網(wǎng)絡(luò)來(lái)實(shí)現(xiàn)姿態(tài)跟蹤，其核心思想是根據(jù)物體的三維模型，選擇模型的某些部件作為被追蹤的目標(biāo)。當(dāng)跟蹤器運(yùn)行時(shí)，跟蹤器會(huì)首先定位這些部件在圖像中的二維投影位置，然后利用此二維投影估計(jì)被追蹤部件的三維位姿，物體的位姿則通過(guò)被追蹤部件的三維位姿來(lái)恢復(fù)。這種方法需要為每個(gè)物體的各個(gè)部件進(jìn)行人工標(biāo)記，同時(shí)需要單獨(dú)訓(xùn)練多個(gè)網(wǎng)絡(luò)。使用神經(jīng)網(wǎng)絡(luò)，不可避免要對(duì)特定環(huán)境收集和標(biāo)注大量樣本，使用大量的GPU資源用于訓(xùn)練，同時(shí)這種方法不具備足夠的擴(kuò)展性，只能用于特定的環(huán)境和物體。若將其用于機(jī)器人還要同時(shí)考慮算力、機(jī)器人成本、便攜性、大量參數(shù)的易操作性能否滿足神經(jīng)網(wǎng)絡(luò)模型的需求。

基于區(qū)域[15-19]的方法，更適應(yīng)于復(fù)雜場(chǎng)景中的對(duì)象。此類方法假設(shè)目標(biāo)和背景區(qū)域之間具有不同的圖像統(tǒng)計(jì)信息，基于合適的統(tǒng)計(jì)外觀模型和3D形狀先驗(yàn)，使用分割模型在當(dāng)前圖像上提取目標(biāo)對(duì)象輪廓，同時(shí)再對(duì)姿態(tài)預(yù)先參數(shù)化的形狀先驗(yàn)渲染得到輪廓。通過(guò)不斷改變用于合成投影的3D模型的位姿參數(shù)來(lái)最小化兩個(gè)輪廓的差異，從而得到目標(biāo)對(duì)象的位姿。

目前機(jī)器人手眼協(xié)調(diào)的實(shí)現(xiàn)大都基于標(biāo)定的方法[2-4，20-25]，通過(guò)標(biāo)定相機(jī)的內(nèi)參數(shù)與外參數(shù)，得到機(jī)器人手眼關(guān)系，構(gòu)建圖像空間與機(jī)器人操作空間的映射。這種方式下，一旦標(biāo)定工作完成，相機(jī)的位置也將固定，除非進(jìn)行重新標(biāo)定。標(biāo)定工作是繁瑣的，同時(shí)不符合人類“Sensor-Actor”的方式。

受人類手眼協(xié)調(diào)的啟發(fā)，本文利用基于區(qū)域的位姿跟蹤算法，同時(shí)跟蹤機(jī)械臂夾持器和目標(biāo)物體。在不進(jìn)行手眼關(guān)系標(biāo)定的前提下，我們基于位姿跟蹤算法計(jì)算夾持器與目標(biāo)物體的相對(duì)位置關(guān)系，根據(jù)它們的相對(duì)位姿，求解機(jī)械臂運(yùn)動(dòng)的目標(biāo)位置，最后通過(guò)MoveIt模塊規(guī)劃?rùn)C(jī)械臂運(yùn)動(dòng)路徑，實(shí)時(shí)引導(dǎo)機(jī)械臂靠近目標(biāo)物體。實(shí)驗(yàn)表明，本文改進(jìn)的位姿跟蹤算法能夠應(yīng)用于機(jī)械臂環(huán)境，并且具有足夠的魯棒性。

1 基于區(qū)域的物體位姿跟蹤

1.1 問(wèn)題描述

基于區(qū)域的位姿追蹤算法基于兩個(gè)假設(shè)：1)物體和背景具有不同的圖像統(tǒng)計(jì)數(shù)據(jù)；2) 已知目標(biāo)物體的3D模型，即形狀先驗(yàn)。首先需要初始化模型的位姿，即對(duì)3D模型賦予初始姿態(tài)參數(shù)，進(jìn)而得到模型在此位姿下的投影輪廓。基于區(qū)域的位姿跟蹤算法通過(guò)定義能量函數(shù)來(lái)最小化目標(biāo)物體真實(shí)輪廓和投影輪廓之間的差異，獲得目標(biāo)物體在相機(jī)坐標(biāo)系下的真實(shí)位姿。

物體的位姿估計(jì)實(shí)質(zhì)上是求解3D模型坐標(biāo)系到相機(jī)坐標(biāo)系的變換，該變換用齊次坐標(biāo)可以表示為矩陣T=[R|t]，其中R是描述空間旋轉(zhuǎn)的矩陣，t是描述空間位移的向量。通過(guò)相機(jī)標(biāo)定可以得到相機(jī)內(nèi)參矩陣：

根據(jù)式(1)將3D模型頂點(diǎn)Xˉi=[XˉiYˉiZˉi]T∈ R3,i=1,2,···,n投影至圖像：

其 中 π (X)=[X/Z,Y/Z]T，X?=[XYZ1]是X的 齊 次表示。

3D目標(biāo)位姿跟蹤過(guò)程如下。

1) 結(jié)合相機(jī)內(nèi)參矩陣K和物體3D模型M，利用Aruco marker初始化物體的位姿T=T0；

2) 將需要跟蹤的物體的3D模型根據(jù)位姿T投影到圖像平面，然后根據(jù)投影輪廓構(gòu)建區(qū)域分割線；

3) 針對(duì)每組區(qū)域分割線，求解局部分割模型；

4) 通過(guò)高斯-牛頓法對(duì)物體進(jìn)行位姿優(yōu)化，得到物體優(yōu)化后的位姿T；

5) 重新進(jìn)行2)，直到停止跟蹤。

1.2 初始位姿估計(jì)

算法開始階段，需要粗略估計(jì)目標(biāo)物體在相機(jī)坐標(biāo)系下的位姿，本文借助Aruco marker[26]來(lái)進(jìn)行初始位姿估計(jì)。PnP（perspective-n-point）問(wèn)題即根據(jù)幾組已知坐標(biāo)系下的3D空間點(diǎn)，及對(duì)應(yīng)圖像上的2D點(diǎn)，可以用于求解相機(jī)位于該坐標(biāo)系下的位置和旋轉(zhuǎn)角，即矩陣T=[R|t]。常見用于解決PnP問(wèn)題的算法包括 P3P、EPnP[27]、DLT、RPnP、OI等。其中EPnP算法通常設(shè)置4個(gè)控制點(diǎn)，即將3D坐標(biāo)表示為一組虛擬的控制點(diǎn)的加權(quán)和。本文利用Aruco標(biāo)志物的4個(gè)角點(diǎn)計(jì)算P4P 問(wèn)題來(lái)估計(jì)ArUco二維碼在相機(jī)坐標(biāo)系下的位姿，從而粗略估計(jì)目標(biāo)物體在相機(jī)坐標(biāo)系下的初始位姿參數(shù)。

1.3 分割模型

基于區(qū)域的位姿跟蹤算法RBOT[18]對(duì)模型投影輪廓的每個(gè)點(diǎn)都建立半徑為r的圓域，然后計(jì)算每個(gè)圓域的前景和背景顏色直方圖，構(gòu)建外觀模型。以?shī)A持器為例，如圖1所示，在跟蹤過(guò)程將計(jì)算大量重疊的圓域，這需要維護(hù)和更新大量的局部顏色直方圖。若減小半徑r，重疊域減小，每個(gè)局部區(qū)域計(jì)算的像素量也會(huì)減少。而由較少的像素計(jì)算出的局部顏色直方圖會(huì)降低算法的穩(wěn)定性，從而導(dǎo)致跟蹤失敗。

圖1 經(jīng)典局部外觀模型Fig.1 Classic local appearance model

為了提高RBOT算法[18]的性能，在此基礎(chǔ)上，本文提出在圓域的分割線上提取顏色特征的方法。如圖2所示，建立以ci為圓心，半徑為r的圓域，然后以該點(diǎn)的法線為基礎(chǔ)構(gòu)建4條分割線，將圓域平均分塊。以每個(gè)圓域及其相鄰的左右兩個(gè)圓域?yàn)橐唤M，對(duì)12條分割線上的像素點(diǎn)分別統(tǒng)計(jì)前景和背景局部顏色直方圖。由圖3可以看到，相鄰區(qū)域內(nèi)計(jì)算的重復(fù)像素點(diǎn)大大減少。

圖2 局部區(qū)域分割線Fig.2 Cutting line of local area

圖3 基于分割線的局部外觀模型Fig.3 Local appearance model based on cutting line

在像素獨(dú)立的前提下，使用類似RBOT算法的局部能量函數(shù)：

其中，

式中：N表示分組個(gè)數(shù)；Bn標(biāo)識(shí)像素x是否在分割線上；和為所有局部顏色直方圖的平均后驗(yàn)。其中，

1.4 迭代更新

對(duì)于每一幀圖像，根據(jù)式(2)將物體模型的頂點(diǎn)投影到圖像上，形成投影輪廓。在出現(xiàn)跟蹤偏差或遮擋情況下，若只計(jì)算每幀圖像的顏色直方圖，算法將產(chǎn)生較大的誤差并導(dǎo)致跟蹤失敗。如果上一幀圖像的位姿估計(jì)結(jié)果正確，則可使用如下線性聯(lián)合的方式來(lái)更新前景和背景模型：

一般而言， αf=0.1， αb=0.2。這種更新方式保證當(dāng)前結(jié)果的準(zhǔn)確性和時(shí)序的一致性。

考慮以下情況：圖像兩幀之間由于光照等原因產(chǎn)生較大顏色差異。若更新時(shí)仍然以上一幀圖像的統(tǒng)計(jì)模型為主，位姿更新時(shí)將會(huì)產(chǎn)生較大偏差，繼而影響后續(xù)跟蹤。針對(duì)此問(wèn)題，本文提出如下的聯(lián)合更新方式：

其中，i∈{f,b},α+β+γ=1。

兩幀圖像之間物體的位姿變化微乎其微，但圖像特征發(fā)生巨大差異。因此我們利用上一幀的位姿參數(shù)將模型投影到當(dāng)前幀，計(jì)算其前景與背景的似然P′t(y|Min)。這消除Pt-1(y|Min)帶來(lái)的錯(cuò)誤引導(dǎo)，為了不丟失上一幀的顏色分布，我們保留了此項(xiàng)。

2 3D目標(biāo)跟蹤實(shí)現(xiàn)手眼協(xié)調(diào)

機(jī)器人手眼協(xié)調(diào)研究的是機(jī)器人感受器與控制器之間的協(xié)調(diào)控制策略，如圖4所示。

圖4 手眼協(xié)調(diào)系統(tǒng)結(jié)構(gòu)Fig.4 Hand-eye coordination system architecture

決策控制部分根據(jù)任務(wù)和當(dāng)前機(jī)器人及目標(biāo)物體的狀態(tài)，進(jìn)行運(yùn)動(dòng)規(guī)劃，控制機(jī)器人運(yùn)動(dòng)；視覺(jué)反饋環(huán)節(jié)通過(guò)攝像機(jī)感知外界環(huán)境、機(jī)器人狀態(tài)等的變化，進(jìn)而通過(guò)視覺(jué)處理使機(jī)器人做出新的決策調(diào)整。

本文手眼協(xié)調(diào)系統(tǒng)由機(jī)械臂硬件系統(tǒng)、單目相機(jī)、夾持器和控制計(jì)算機(jī)構(gòu)成。機(jī)械臂硬件系統(tǒng)包括控制柜、示教器和機(jī)械臂本體。在機(jī)械臂末端，我們安裝了基于伺服電機(jī)的電動(dòng)夾爪。實(shí)驗(yàn)環(huán)境如圖5所示，SD700E機(jī)械臂本體固定在桌面，相機(jī)固定在桌面上方，桌面上放置待抓取物體，待抓取物體為易拉罐、犀牛玩具等物體。

圖5 手眼協(xié)調(diào)系統(tǒng)Fig.5 Hand-eye coordination system

首先機(jī)械臂的運(yùn)動(dòng)根據(jù)運(yùn)動(dòng)學(xué)方程，得到機(jī)械臂六軸的變換關(guān)系。由于本系統(tǒng)在原機(jī)器人的基礎(chǔ)上，增加一截電動(dòng)手爪，因此需要在運(yùn)動(dòng)學(xué)方程中添加機(jī)器人的連桿參數(shù)，使其可以被看做一個(gè)理想的笛卡爾元件。表1為SD700E機(jī)械臂D-H參數(shù)[28]。其中 θ表示關(guān)節(jié)，d表示桿件長(zhǎng)度，a表示桿件連接位移，α表示桿件之間扭轉(zhuǎn)角度。

表1 D-H參數(shù)Table 1 D-H parameters

根據(jù)改進(jìn)的D-H表示法，利用式(11)的變換矩陣將坐標(biāo)系 {i-1}變換到坐標(biāo)系 {i}：

將上述變換矩陣展開為

式中：cθi表示 θi的 余弦值；sθi表示 θi的正弦值。

手眼協(xié)調(diào)的實(shí)現(xiàn)通常以系統(tǒng)標(biāo)定為前提，通過(guò)手眼關(guān)系標(biāo)定，得到相機(jī)與機(jī)械臂坐標(biāo)系的變換關(guān)系。如圖6所示，通過(guò)標(biāo)定得到相機(jī)坐標(biāo)系與機(jī)械臂基坐標(biāo)系的變換矩陣，當(dāng)視覺(jué)系統(tǒng)得到目標(biāo)物體的狀態(tài)時(shí)，由獲得物體在機(jī)械臂坐標(biāo)空間的位置，進(jìn)而控制機(jī)械臂運(yùn)動(dòng)。這種方式無(wú)法應(yīng)對(duì)彈性任務(wù)，同時(shí)一旦標(biāo)定成功之后，攝像機(jī)(眼)將永遠(yuǎn)被固定，除非重新進(jìn)行標(biāo)定。

圖6 手眼標(biāo)定(眼在手外)Fig.6 Hand-eye calibration (eye outside the hand)

利用物體位姿跟蹤算法，同時(shí)跟蹤機(jī)械臂夾持器和目標(biāo)物體，利用二者相對(duì)位置關(guān)系，實(shí)現(xiàn)機(jī)械臂邊看邊做的動(dòng)態(tài)反饋控制。

3 運(yùn)動(dòng)規(guī)劃

ROS 為機(jī)器人開發(fā)提供了一個(gè)統(tǒng)一的開發(fā)和測(cè)試平臺(tái)，MoveIt 是 ROS 中集成的一個(gè)功能強(qiáng)大的軟件開發(fā)包，使用它能夠方便地進(jìn)行運(yùn)動(dòng)規(guī)劃，同時(shí)它兼?zhèn)淙S感知、控制、導(dǎo)航等功能。根據(jù)機(jī)器人的 URDF 模型，利用 MoveIt 可以在綜合考慮場(chǎng)景信息基礎(chǔ)上，完成機(jī)器人運(yùn)動(dòng)規(guī)劃及避障的相關(guān)計(jì)算。我們利用ROS MoveIt結(jié)合實(shí)時(shí)位姿跟蹤，完成機(jī)械臂自主導(dǎo)航任務(wù)。

ROS中還提供 Gazebo 物理仿真工具，它不僅能夠模擬機(jī)器人的理想運(yùn)動(dòng)，還可以為機(jī)器人加入質(zhì)量、慣性、摩擦系數(shù)等物理屬性。因此我們針對(duì)實(shí)驗(yàn)的機(jī)械臂在Gazebo 環(huán)境下搭建了仿真實(shí)驗(yàn)平臺(tái)，如圖7所示。

圖7 Gazebo仿真環(huán)境Fig.7 Gazebo simulation environment

在ROS仿真環(huán)境下，利用MoveIt進(jìn)行運(yùn)動(dòng)規(guī)劃使機(jī)械臂根據(jù)規(guī)劃的路徑運(yùn)動(dòng)到目標(biāo)位置。仿真結(jié)果如圖8所示。

圖8 仿真環(huán)境運(yùn)動(dòng)規(guī)劃Fig.8 Motion planning in the simulation environment

同時(shí)，利用MoveIt并結(jié)合機(jī)械臂底層通信實(shí)現(xiàn)真實(shí)環(huán)境下機(jī)械臂的運(yùn)動(dòng)規(guī)劃與執(zhí)行，主要流程和實(shí)驗(yàn)效果如圖9所示。

圖9 真實(shí)環(huán)境運(yùn)動(dòng)規(guī)劃Fig.9 Motion planning in the real world

1) 確定機(jī)械臂運(yùn)動(dòng)目標(biāo)；

2) MoveIt完成運(yùn)動(dòng)規(guī)劃得到目標(biāo)關(guān)節(jié)軌跡；

3) 通過(guò)socket接口和控制器連接，將關(guān)節(jié)軌跡發(fā)送給控制器；

4) 控制器執(zhí)行插補(bǔ)運(yùn)算，將結(jié)果周期性發(fā)送給電機(jī)驅(qū)動(dòng)器；

5) 驅(qū)動(dòng)器完成閉環(huán)控制后，由電機(jī)跟隨輸入指令；

6) 控制器反饋實(shí)時(shí)狀態(tài)到MoveIt。

4 實(shí)驗(yàn)

4.1 位姿跟蹤實(shí)驗(yàn)

為了驗(yàn)證改進(jìn)后的跟蹤算法的準(zhǔn)確度，我們分別錄制了機(jī)械臂運(yùn)動(dòng)的局部區(qū)域視頻序列、完整跟蹤測(cè)試序列以及目標(biāo)物體的運(yùn)動(dòng)序列。視頻序列分辨率為640×480，畫面中包括物體的平移、旋轉(zhuǎn)、運(yùn)動(dòng)模糊以及光照不均等現(xiàn)象。

對(duì)于圖10(a)機(jī)械臂的局部區(qū)域視頻序列，可以看到夾持器在運(yùn)動(dòng)時(shí)顏色特征發(fā)生了明顯的變化，尤其體現(xiàn)在旋轉(zhuǎn)過(guò)程，造成了上下兩幀圖像明暗、顏色差異較大。 對(duì)每一幀圖像，將模型根據(jù)位姿估計(jì)結(jié)果渲染到圖像上，模型渲染結(jié)果與圖像中目標(biāo)物體越吻合，則結(jié)果越準(zhǔn)確。圖10(b)和圖10(c) 展示了RBOT算法和本文算法的表現(xiàn)差異，當(dāng)夾持器在空間中運(yùn)動(dòng)時(shí)，RBOT算法出現(xiàn)偏差，并導(dǎo)致后面的跟蹤完全丟失。由于本文算法改進(jìn)了用于提取顏色特征的局部區(qū)域以及顏色似然的更新方式，在整個(gè)夾持器旋轉(zhuǎn)過(guò)程都能準(zhǔn)確跟蹤。

圖10 夾持器跟蹤結(jié)果對(duì)比Fig.10 Comparison of gripper tracking results

圖11給出了RBOT算法與本文算法在兔子模型上的跟蹤結(jié)果對(duì)比。按照從左到右的順序依次為相機(jī)采集的視頻序列第20幀、第672幀、第780幀和第910幀。兩個(gè)算法在該模型數(shù)據(jù)上的跟蹤效果表現(xiàn)相對(duì)較好，但在第910幀時(shí)，從圖11(b)最右圖中的紅色圓圈標(biāo)注部分可以看出，RBOT[18]算法渲染的結(jié)果與真實(shí)模型偏差較大。主要原因是基于局部圓形區(qū)域的方法會(huì)導(dǎo)致圓域之間出現(xiàn)大量重疊，鄰圓域內(nèi)上下文信息相似性較高，無(wú)法準(zhǔn)確分割物體輪廓。而本文提出的局部圓域分割線方法，可以在物體輪廓的法線、切線及對(duì)角線方向上構(gòu)建局部分割線，可以對(duì)輪廓進(jìn)行更精準(zhǔn)的定位，充分捕獲局部區(qū)域的上下文信息，實(shí)現(xiàn)更為精準(zhǔn)的位姿跟蹤。從圖11(c)最右圖可以看出本文提出的局部區(qū)域分割線位姿跟蹤效果更優(yōu)。

圖11 兔子跟蹤結(jié)果對(duì)比Fig.11 Comparison of rabbit tracking results

圖12(a)和12(b)分別給出了本文算法在企鵝模型和易拉罐模型上的跟蹤效果。從圖中可以看出，根據(jù)算法估計(jì)的位姿參數(shù)將模型渲染到圖像上的效果與物體貼合緊密，較為準(zhǔn)確地估計(jì)出物體的位姿，本文算法在物體跟蹤過(guò)程具有較好的準(zhǔn)確度和魯棒性。

圖12 本文算法其他模型跟蹤結(jié)果Fig.12 Tracking results of other models with our algorithm

為了定義位姿估計(jì)的誤差Ep,我們將物體模型的頂點(diǎn)分別根據(jù)估計(jì)的位姿 [R,t]和真實(shí)位姿[R*,t*]進(jìn)行變換，然后計(jì)算變換后坐標(biāo)點(diǎn)的歐氏距離：

式中：xi表示模型的頂點(diǎn)；m表示頂點(diǎn)總數(shù)。

我們統(tǒng)計(jì)了夾持器位姿跟蹤實(shí)驗(yàn)和兔子模型位姿跟蹤實(shí)驗(yàn)位的姿估計(jì)誤差。如圖13所示，夾持器模型的位姿估計(jì)平均誤差為6.43 mm，兔子模型的平均誤差為5.68 mm。可以看出夾持器模型誤差與兔子模型相比較大，且波動(dòng)明顯，這主要由于夾持器支架與夾持器具有相似的顏色特征。同時(shí)二者都存在一些誤差較大的幀，主要由運(yùn)動(dòng)模糊或者遮擋導(dǎo)致。總體而言，本文算法滿足實(shí)驗(yàn)精度需求。

圖13 位姿跟蹤誤差統(tǒng)計(jì)Fig.13 Error statistics for pose tracking

此外，由于本文提出的局部區(qū)域分割線算法減少了重復(fù)像素的計(jì)算，本文算法的跟蹤速度也有了一定的提升，如圖14所示，本文算法在性能一般的處理器上運(yùn)行，與RBOT算法相比，每秒提升了0.9～3.5幀。

圖14 本文算法與RBOT算法速度對(duì)比結(jié)果Fig.14 Speed comparisons between our algorithm and RBOT algorithm

4.2 機(jī)器人手眼協(xié)調(diào)實(shí)驗(yàn)

機(jī)器人手眼協(xié)調(diào)實(shí)驗(yàn)的目標(biāo)是基于位姿跟蹤算法完成機(jī)械臂自主導(dǎo)航任務(wù)，讓夾持器自動(dòng)靠近目標(biāo)物體，實(shí)現(xiàn)手眼協(xié)調(diào)系統(tǒng)。 為了驗(yàn)證本文方法的可行性與準(zhǔn)確度，在實(shí)驗(yàn)過(guò)程中我們多次改變物體的位姿，對(duì)物體進(jìn)行平移、旋轉(zhuǎn)操作，考察在仿真環(huán)境和真實(shí)環(huán)境下機(jī)械臂運(yùn)動(dòng)情況。圖15為仿真環(huán)境下，機(jī)械臂靠近松鼠的運(yùn)動(dòng)過(guò)程。在此過(guò)程中人為改變了松鼠的位姿，可以看到機(jī)械臂也對(duì)應(yīng)改變了運(yùn)動(dòng)方向。圖16為真實(shí)環(huán)境下的實(shí)驗(yàn)，目標(biāo)物體為易拉罐，實(shí)現(xiàn)了機(jī)械臂邊看邊靠近的動(dòng)態(tài)反饋控制過(guò)程。

圖15 仿真環(huán)境實(shí)驗(yàn)結(jié)果Fig.15 Experimental results in the simulation environment

圖16 真實(shí)環(huán)境實(shí)驗(yàn)結(jié)果Fig.16 Experimental results in the real environment

為了評(píng)估基于位姿估計(jì)的手眼協(xié)調(diào)系統(tǒng)的性能，我們定義對(duì)接近誤差作了量化統(tǒng)計(jì)。如圖17所示，我們以目標(biāo)物體質(zhì)心作為球心定義最小外接球，半徑為r，機(jī)械臂運(yùn)動(dòng)的目標(biāo)位置為X(x0,y0,z0)。在實(shí)際情況下，我們?cè)O(shè)置機(jī)械臂運(yùn)動(dòng)的目標(biāo)位置在z軸正方向上平移10個(gè)單位，以避免機(jī)械臂誤碰撞。我們定義誤差，其中DO′X表示機(jī)械臂運(yùn)動(dòng)的目標(biāo)位置X到球心O′之間的歐式距離，當(dāng)E≤1時(shí)，表明實(shí)驗(yàn)成功。

圖17 誤差模型示意圖Fig.17 Error model demonstration

針對(duì)5種不同的物體模型，分別進(jìn)行10組實(shí)驗(yàn)，并且每組實(shí)驗(yàn)中隨機(jī)改變物體位姿5次，然后統(tǒng)計(jì)實(shí)驗(yàn)中的接近誤差以及實(shí)驗(yàn)的成功率。如表2所示，兔子的成功率最高，而犀牛和易拉罐的成功率較低。由于犀牛模型體積較小，相機(jī)距離目標(biāo)物體較遠(yuǎn)，且該模型的紋理信息欠缺，導(dǎo)致跟蹤過(guò)程相對(duì)不穩(wěn)定，成功率較低。而易拉罐模型輪廓單一，相似度較高，成功率接近90%。實(shí)驗(yàn)結(jié)果表明，我們的手眼協(xié)調(diào)系統(tǒng)對(duì)物體的平移、旋轉(zhuǎn)、光照等具有足夠的魯棒性。

表2 真實(shí)環(huán)境中不同物體的位姿跟蹤過(guò)程中機(jī)械臂動(dòng)態(tài)反饋能力驗(yàn)證Table 2 Dynamic feedback capability verification of the robotic arm during the pose tracking of different objects in the real environment

5 結(jié)束語(yǔ)

本文提出了在不需要手眼標(biāo)定的前提下實(shí)現(xiàn)機(jī)器人手眼協(xié)調(diào)的新方法，改進(jìn)了基于區(qū)域的物體位姿跟蹤算法，使其可以應(yīng)用于真實(shí)場(chǎng)景。通過(guò)跟蹤機(jī)械臂夾持器和物體的位姿，利用它們的相對(duì)位置關(guān)系以動(dòng)態(tài)反饋的方式引導(dǎo)機(jī)械臂運(yùn)動(dòng)。最后，在仿真環(huán)境和真實(shí)環(huán)境下測(cè)試，表明這種手眼協(xié)調(diào)能力使機(jī)器人能夠較好應(yīng)對(duì)動(dòng)態(tài)的任務(wù)和多變的環(huán)境，驗(yàn)證了方案的可行性。在未來(lái)，我們會(huì)構(gòu)建快速三維重建平臺(tái)，并繼續(xù)改進(jìn)物體跟蹤算法的準(zhǔn)確性以應(yīng)對(duì)更加復(fù)雜的場(chǎng)景。