基于3D目標跟蹤算法的機器人手眼協調研究

段肖，馬鋼，危輝

（復旦大學 計算機科學技術學院認知算法模型實驗室，上海 200433）

機器人手眼協調[1-4]是指機器人感受器與控制器之間的協調控制策略。傳統流水線上的工業機器人，它們面對固定不變的任務，因而其操作流程和動作序列在設定后就可以保持不變，因此即使是盲操作(無需反饋)也足以完成任務。而類似于服務機器人的非工業流水線機器人所面對的環境和任務是不可預知的，這就需要實時感知這些狀態的變化，并做出正確的調整。高級哺乳動物，如人應對這種任務的能力就特別強?；贕oodale和Milner提出的視覺雙通路理論[5-6]，我們大腦是這樣工作的，視覺信息經過腹側通路產生知覺，然后背側系統進行視覺引導。信息加工能夠在短時間內完成，保證了由視覺引導的動作是實時且動態調整的。

因此，帶反饋和閉環控制的手眼協調為完成這種具有各種可變性的機器人抓取任務提供了可能，使它們變得更加智能，不再局限于一成不變的環境和任務中。

機器人手眼協調的前提是準確的三維感知以獲取目標對象的狀態。物體跟蹤[7]是機器人領域的核心技術，三維物體位姿追蹤通過輸入的圖像序列分析確定目標物體在三維空間的位姿參數，該參數包括位置分量和姿態角。針對剛性物體位姿追蹤已有多種研究思路，并取得很大進展。主要分為以下幾類方法：

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

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

基于區域[15-19]的方法，更適應于復雜場景中的對象。此類方法假設目標和背景區域之間具有不同的圖像統計信息，基于合適的統計外觀模型和3D形狀先驗，使用分割模型在當前圖像上提取目標對象輪廓，同時再對姿態預先參數化的形狀先驗渲染得到輪廓。通過不斷改變用于合成投影的3D模型的位姿參數來最小化兩個輪廓的差異，從而得到目標對象的位姿。

目前機器人手眼協調的實現大都基于標定的方法[2-4，20-25]，通過標定相機的內參數與外參數，得到機器人手眼關系，構建圖像空間與機器人操作空間的映射。這種方式下，一旦標定工作完成，相機的位置也將固定，除非進行重新標定。標定工作是繁瑣的，同時不符合人類“Sensor-Actor”的方式。

受人類手眼協調的啟發，本文利用基于區域的位姿跟蹤算法，同時跟蹤機械臂夾持器和目標物體。在不進行手眼關系標定的前提下，我們基于位姿跟蹤算法計算夾持器與目標物體的相對位置關系，根據它們的相對位姿，求解機械臂運動的目標位置，最后通過MoveIt模塊規劃機械臂運動路徑，實時引導機械臂靠近目標物體。實驗表明，本文改進的位姿跟蹤算法能夠應用于機械臂環境，并且具有足夠的魯棒性。

1 基于區域的物體位姿跟蹤

1.1 問題描述

基于區域的位姿追蹤算法基于兩個假設：1)物體和背景具有不同的圖像統計數據；2) 已知目標物體的3D模型，即形狀先驗。首先需要初始化模型的位姿，即對3D模型賦予初始姿態參數，進而得到模型在此位姿下的投影輪廓?；趨^域的位姿跟蹤算法通過定義能量函數來最小化目標物體真實輪廓和投影輪廓之間的差異，獲得目標物體在相機坐標系下的真實位姿。

物體的位姿估計實質上是求解3D模型坐標系到相機坐標系的變換，該變換用齊次坐標可以表示為矩陣T=[R|t]，其中R是描述空間旋轉的矩陣，t是描述空間位移的向量。通過相機標定可以得到相機內參矩陣：

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

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

3D目標位姿跟蹤過程如下。

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

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

3) 針對每組區域分割線，求解局部分割模型；

4) 通過高斯-牛頓法對物體進行位姿優化，得到物體優化后的位姿T；

5) 重新進行2)，直到停止跟蹤。

1.2 初始位姿估計

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

1.3 分割模型

基于區域的位姿跟蹤算法RBOT[18]對模型投影輪廓的每個點都建立半徑為r的圓域，然后計算每個圓域的前景和背景顏色直方圖，構建外觀模型。以夾持器為例，如圖1所示，在跟蹤過程將計算大量重疊的圓域，這需要維護和更新大量的局部顏色直方圖。若減小半徑r，重疊域減小，每個局部區域計算的像素量也會減少。而由較少的像素計算出的局部顏色直方圖會降低算法的穩定性，從而導致跟蹤失敗。

圖1 經典局部外觀模型Fig.1 Classic local appearance model

為了提高RBOT算法[18]的性能，在此基礎上，本文提出在圓域的分割線上提取顏色特征的方法。如圖2所示，建立以ci為圓心，半徑為r的圓域，然后以該點的法線為基礎構建4條分割線，將圓域平均分塊。以每個圓域及其相鄰的左右兩個圓域為一組，對12條分割線上的像素點分別統計前景和背景局部顏色直方圖。由圖3可以看到，相鄰區域內計算的重復像素點大大減少。

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

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

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

其中，

式中：N表示分組個數；Bn標識像素x是否在分割線上；和為所有局部顏色直方圖的平均后驗。其中，

1.4 迭代更新

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

一般而言， αf=0.1， αb=0.2。這種更新方式保證當前結果的準確性和時序的一致性。

考慮以下情況：圖像兩幀之間由于光照等原因產生較大顏色差異。若更新時仍然以上一幀圖像的統計模型為主，位姿更新時將會產生較大偏差，繼而影響后續跟蹤。針對此問題，本文提出如下的聯合更新方式：

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

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

2 3D目標跟蹤實現手眼協調

機器人手眼協調研究的是機器人感受器與控制器之間的協調控制策略，如圖4所示。

圖4 手眼協調系統結構Fig.4 Hand-eye coordination system architecture

決策控制部分根據任務和當前機器人及目標物體的狀態，進行運動規劃，控制機器人運動；視覺反饋環節通過攝像機感知外界環境、機器人狀態等的變化，進而通過視覺處理使機器人做出新的決策調整。

本文手眼協調系統由機械臂硬件系統、單目相機、夾持器和控制計算機構成。機械臂硬件系統包括控制柜、示教器和機械臂本體。在機械臂末端，我們安裝了基于伺服電機的電動夾爪。實驗環境如圖5所示，SD700E機械臂本體固定在桌面，相機固定在桌面上方，桌面上放置待抓取物體，待抓取物體為易拉罐、犀牛玩具等物體。

圖5 手眼協調系統Fig.5 Hand-eye coordination system

首先機械臂的運動根據運動學方程，得到機械臂六軸的變換關系。由于本系統在原機器人的基礎上，增加一截電動手爪，因此需要在運動學方程中添加機器人的連桿參數，使其可以被看做一個理想的笛卡爾元件。表1為SD700E機械臂D-H參數[28]。其中 θ表示關節，d表示桿件長度，a表示桿件連接位移，α表示桿件之間扭轉角度。

表1 D-H參數Table 1 D-H parameters

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

將上述變換矩陣展開為

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

手眼協調的實現通常以系統標定為前提，通過手眼關系標定，得到相機與機械臂坐標系的變換關系。如圖6所示，通過標定得到相機坐標系與機械臂基坐標系的變換矩陣，當視覺系統得到目標物體的狀態時，由獲得物體在機械臂坐標空間的位置，進而控制機械臂運動。這種方式無法應對彈性任務，同時一旦標定成功之后，攝像機(眼)將永遠被固定，除非重新進行標定。

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

利用物體位姿跟蹤算法，同時跟蹤機械臂夾持器和目標物體，利用二者相對位置關系，實現機械臂邊看邊做的動態反饋控制。

3 運動規劃

ROS 為機器人開發提供了一個統一的開發和測試平臺，MoveIt 是 ROS 中集成的一個功能強大的軟件開發包，使用它能夠方便地進行運動規劃，同時它兼備三維感知、控制、導航等功能。根據機器人的 URDF 模型，利用 MoveIt 可以在綜合考慮場景信息基礎上，完成機器人運動規劃及避障的相關計算。我們利用ROS MoveIt結合實時位姿跟蹤，完成機械臂自主導航任務。

ROS中還提供 Gazebo 物理仿真工具，它不僅能夠模擬機器人的理想運動，還可以為機器人加入質量、慣性、摩擦系數等物理屬性。因此我們針對實驗的機械臂在Gazebo 環境下搭建了仿真實驗平臺，如圖7所示。

圖7 Gazebo仿真環境Fig.7 Gazebo simulation environment

在ROS仿真環境下，利用MoveIt進行運動規劃使機械臂根據規劃的路徑運動到目標位置。仿真結果如圖8所示。

圖8 仿真環境運動規劃Fig.8 Motion planning in the simulation environment

同時，利用MoveIt并結合機械臂底層通信實現真實環境下機械臂的運動規劃與執行，主要流程和實驗效果如圖9所示。

圖9 真實環境運動規劃Fig.9 Motion planning in the real world

1) 確定機械臂運動目標；

2) MoveIt完成運動規劃得到目標關節軌跡；

3) 通過socket接口和控制器連接，將關節軌跡發送給控制器；

4) 控制器執行插補運算，將結果周期性發送給電機驅動器；

5) 驅動器完成閉環控制后，由電機跟隨輸入指令；

6) 控制器反饋實時狀態到MoveIt。

4 實驗

4.1 位姿跟蹤實驗

為了驗證改進后的跟蹤算法的準確度，我們分別錄制了機械臂運動的局部區域視頻序列、完整跟蹤測試序列以及目標物體的運動序列。視頻序列分辨率為640×480，畫面中包括物體的平移、旋轉、運動模糊以及光照不均等現象。

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

圖10 夾持器跟蹤結果對比Fig.10 Comparison of gripper tracking results

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

圖11 兔子跟蹤結果對比Fig.11 Comparison of rabbit tracking results

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

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

為了定義位姿估計的誤差Ep,我們將物體模型的頂點分別根據估計的位姿 [R,t]和真實位姿[R*,t*]進行變換，然后計算變換后坐標點的歐氏距離：

式中：xi表示模型的頂點；m表示頂點總數。

我們統計了夾持器位姿跟蹤實驗和兔子模型位姿跟蹤實驗位的姿估計誤差。如圖13所示，夾持器模型的位姿估計平均誤差為6.43 mm，兔子模型的平均誤差為5.68 mm?？梢钥闯鰥A持器模型誤差與兔子模型相比較大，且波動明顯，這主要由于夾持器支架與夾持器具有相似的顏色特征。同時二者都存在一些誤差較大的幀，主要由運動模糊或者遮擋導致。總體而言，本文算法滿足實驗精度需求。

圖13 位姿跟蹤誤差統計Fig.13 Error statistics for pose tracking

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

圖14 本文算法與RBOT算法速度對比結果Fig.14 Speed comparisons between our algorithm and RBOT algorithm

4.2 機器人手眼協調實驗

機器人手眼協調實驗的目標是基于位姿跟蹤算法完成機械臂自主導航任務，讓夾持器自動靠近目標物體，實現手眼協調系統。 為了驗證本文方法的可行性與準確度，在實驗過程中我們多次改變物體的位姿，對物體進行平移、旋轉操作，考察在仿真環境和真實環境下機械臂運動情況。圖15為仿真環境下，機械臂靠近松鼠的運動過程。在此過程中人為改變了松鼠的位姿，可以看到機械臂也對應改變了運動方向。圖16為真實環境下的實驗，目標物體為易拉罐，實現了機械臂邊看邊靠近的動態反饋控制過程。

圖15 仿真環境實驗結果Fig.15 Experimental results in the simulation environment

圖16 真實環境實驗結果Fig.16 Experimental results in the real environment

為了評估基于位姿估計的手眼協調系統的性能，我們定義對接近誤差作了量化統計。如圖17所示，我們以目標物體質心作為球心定義最小外接球，半徑為r，機械臂運動的目標位置為X(x0,y0,z0)。在實際情況下，我們設置機械臂運動的目標位置在z軸正方向上平移10個單位，以避免機械臂誤碰撞。我們定義誤差，其中DO′X表示機械臂運動的目標位置X到球心O′之間的歐式距離，當E≤1時，表明實驗成功。

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

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

表2 真實環境中不同物體的位姿跟蹤過程中機械臂動態反饋能力驗證Table 2 Dynamic feedback capability verification of the robotic arm during the pose tracking of different objects in the real environment

5 結束語

本文提出了在不需要手眼標定的前提下實現機器人手眼協調的新方法，改進了基于區域的物體位姿跟蹤算法，使其可以應用于真實場景。通過跟蹤機械臂夾持器和物體的位姿，利用它們的相對位置關系以動態反饋的方式引導機械臂運動。最后，在仿真環境和真實環境下測試，表明這種手眼協調能力使機器人能夠較好應對動態的任務和多變的環境，驗證了方案的可行性。在未來，我們會構建快速三維重建平臺，并繼續改進物體跟蹤算法的準確性以應對更加復雜的場景。