仿人機器人頭部設計與目標跟蹤運動控制

宛敏紅 周維佳 劉玉旺

(1.中國科學院沈陽自動化研究所機器人學國家重點實驗室，沈陽 110016； 2.中國科學院大學，北京 100049)

仿人機器人頭部設計與目標跟蹤運動控制

宛敏紅1,2周維佳1劉玉旺1

(1.中國科學院沈陽自動化研究所機器人學國家重點實驗室，沈陽 110016； 2.中國科學院大學，北京 100049)

針對仿人機器人視覺目標跟蹤任務需求，設計了一種高精度、靈巧型仿人機器人頭部系統，構建了視覺感知模塊與3自由度機械臂運動機構。利用虛擬連桿方法對仿人機器人頭部進行運動學建模，將目標跟蹤問題轉換為機械臂的運動學逆解問題，通過計算機構可操作度與條件數，顯示機器人頭部對目標跟蹤任務具有良好的運動學靈活性。在此基礎上，基于梯度投影法建立了目標跟蹤運動學逆解與規避關節極限位置的同步優化算式，并針對視覺測量系統帶寬較窄、容易造成較大跟蹤誤差的問題，提出了在兩次視覺測量間隔內用軌跡預測的方法獲得目標位置的估計值，以提高跟蹤精度。仿真與實驗結果表明，在測量盲區內采用軌跡預測方法，可將跟蹤精度提高約80%，實現高精度目標跟蹤。所提出的仿人機器人頭部及控制方法，對動態目標具有良好的跟蹤性能。

仿人機器人； 頭部； 目標跟蹤； 虛擬連桿； 梯度投影法； 軌跡預測

引言

仿人機器人是當前機器人領域的研究熱點之一，國內外科研機構與企業都對其開展了大量的研究工作，開發出了一系列仿人機器人[1-8]。仿人機器人頭部是仿人機器人的重要組成部分，其主要功能是實現與外界的交互，機器人頭部一般會根據任務需求集成視覺、聽覺、嗅覺、表情表達等功能模塊中的一種或幾種。麻省理工學院開發的機器人頭部Kismet，共有21個自由度，可通過面部表情、聲音以及動作實現與外界的交互，并通過寬視場角低精度相機與窄視場角高精度相機的組合使用提高視覺測量的效率[9]。Wroclaw大學開發的情感表達頭部機器人EMYS能夠通過頭部、眼睛以及眼瞼的動作表達一系列情感，如高興、悲傷、驚訝等，EMYS頭部機器人共有11個自由度，利用面部動作編碼系統(FACS)描述機器人的面部表情[10]。NASA開發的航天員機器人Robonaut2，其頭部由3自由度機構與感知系統組成，使用伺服電機與諧波傳動系統實現高精度伺服控制[11]。兒童機器人iCub的頭部具有6個自由度，集成視覺、聽覺及慣性傳感器，能實現視覺目標跟蹤、身體平衡以及聲定位功能[12]。文獻[13]提出一種機器人頭部模塊，利用3個力傳感器的組合，可同時測出外界施加于頭部的力與位置。

通過視線控制仿人機器人可以實現對動態目標的跟蹤，在控制方法上與光電跟蹤系統類似[14]。文獻[15]通過采用虛擬連桿建模方法，將視線跟蹤控制問題等效轉化為機械臂末端軌跡跟蹤問題，基于梯度投影法求解出了視線跟蹤的最優解。文獻[16]將阻尼最小二乘法用于具有冗余特性的機器人頭部視線控制中，以規避奇異點與關節極限位置。文獻[17]針對目標軌跡的不確定性，運用卡爾曼濾波技術對目標位置進行預測。文獻[18]通過機器人頭頸與眼睛部位聯合運動，實現了目標跟蹤控制。

在現有的仿人機器人頭部跟蹤運動控制研究中，鮮少有提及針對特定跟蹤任務的運動學性能評估，特別是仿人機器人頭部大量采用冗余機構，控制上比較復雜，如果能得到運動學性能比較優良的構型，有利于提高跟蹤的實時性與精度。另一方面，受限于視覺測量計算的復雜性，視覺測量的頻寬一般只有10 Hz左右，而位置伺服的控制頻寬可以達到100 Hz左右，視覺測量的響應滯后往往造成跟蹤誤差較大，在運動目標速度較快時更是如此，如何提高在視覺測量頻寬較窄時的跟蹤精度，也是需要解決的問題。為實現對動態目標的視覺跟蹤，仿人機器人頭部應具有靈活運動、高精度跟蹤的能力。本文針對仿人機器人視覺跟蹤任務需求，提出一種具有視覺跟蹤能力的頭部系統，基于虛擬連桿建模的方法，將視覺跟蹤任務轉換成機械臂軌跡跟蹤任務，利用可操作度與條件數評估其運動學性能，驗證其設計的合理性，并基于軌跡預測的方法提高視覺測量速度滯后下的跟蹤精度。

1 仿人機器人頭部系統設計

本文提出的仿人機器人頭部實物如圖1所示，主要由視覺感知模塊與機械臂本體組成，其任務是實現對運動小球的視覺跟蹤。機器人頭部整體上實現了緊湊型設計，質量為5.8 kg，包絡尺寸為400 mm×230 mm×230 mm，體積與成年人的頭頸相當。選擇類似人類頭頸的串聯機構作為機械臂構型，包含下俯仰(關節1)、回轉(關節2)、上俯仰(關節3)3個關節。

圖1 仿人機器人頭部系統組成Fig.1 Humanoid robot head system configuration1.CCD相機 2.TOF相機 3.機械臂 4.基座

視覺感知模塊負責對小球進行識別與定位，并將位置信息發送給機械臂控制器，驅動機械臂運動，以使機器人頭部視線始終對準目標。視覺感知模塊包括2個CCD相機與1個激光測距(TOF)相機，CCD相機的分辨率為2 448像素×2 050像素，TOF相機的分辨率為176像素×144像素，利用兩種相機進行信息融合以得到小球的位置。小球定位方法是首先利用TOF相機進行粗定位，獲得小球在CCD相機上大致位置并計算感興趣區域，最后在感興趣區域內利用雙目視覺進行精確定位計算，這樣可以同時保證處理速度和測量精度[19]。

圖2 模塊化關節結構設計原理圖Fig.2 Schematic diagram of modular robot joint design1.關節編碼器 2.基座 3.電動機組件 4.制動器 5.諧波減速機 6.關節輸出端

機械臂關節采用模塊化設計方法，驅動元件使用無刷直流電動機與諧波減速機以實現高精度位置伺服，圖2為模塊化關節設計原理。

仿人機器人頭部控制系統采用基于EtherCAT總線的分布式控制方式，以倍福四核嵌入式計算機為控制器平臺，實時性高，易于模塊化設計，各關節之間通過總線連接實現主從關系或平行關系。主控制器作EtherCAT主站，完成圖像處理與運動規劃任務，各關節控制器作為EtherCAT從站，通過EtherCAT總線與主控制器進行通信，完成關節層控制。CCD相機、TOF相機與主控制器之間通過千兆以太網進行通信，控制系統組成框圖如圖3所示。

圖3 機器人頭部控制系統框圖Fig.3 Robot head control block diagram

2 運動學建模與分析

2.1 基于虛擬連桿的運動學建模

采用虛擬連桿方法可以將視線跟蹤問題等效轉換為運動機構末端軌跡跟蹤問題[15-16]。仿人機器人頭部視覺跟蹤的虛擬連桿建模方法如圖4所示，在視覺坐標系原點與目標之間建立一個虛擬的連桿，虛擬連桿在運動學上等效于一個移動關節，即虛擬連桿在長度方向上可伸縮，使得在目標跟蹤運動過程中，連桿的末端始終與目標連接在一起。如此，視覺目標跟蹤問題，就等價于包含虛擬連桿在內的4自由度機械臂的動態軌跡跟蹤問題。

圖4 仿人機器人頭部虛擬連桿建模示意圖Fig.4 Modeling principle of humanoid robot head based on visual link method1.跟蹤目標 2.虛擬連桿 3.仿人機器人頭部

仿人機器人頭部連桿坐標系如圖5所示，DH參數如表1所示。設連桿末端位置點為O4，目標位置點為O4T，跟蹤運動控制方法是：由視覺感知模塊測量得到兩點位置差，即笛卡爾空間中的跟蹤誤差，再經過逆運動學計算得到驅動信號，驅動機械臂向目標位置點運動。

圖5 仿人機器人頭部連桿坐標系Fig.5 Humanoid robot head link coordinates

表1 仿人機器人頭部DH參數Tab.1 Humanoid robot head DH parameters

類似于人類頭頸的運動機理與運動范圍，3個關節運動范圍分別設定為[-95°,-20°]、[90°,270°]與[-104°,-55°]，3個關節的最大運動速度均為360(°)/s。機器人頭部的幾何參數為l1=l3=0.055 m，l2=0.163 m，l4=0.064 m。

2.2 運動學特性分析

通過可操作度與條件數指標評估機器人頭部的運動性能，以驗證機構設計的合理性。可操作度表征機構離奇異位形的遠近，Yoshikawa提出的可操作度定義為

(1)

式中J——雅可比矩陣

圖6 機器人頭部可操作度Fig.6 Manipulability of robot head

機器人處于奇異位形時，雅可比矩陣J行不滿秩，det(JJT)=0，w=0。w越大，機器人的靈活性就越好。可操作度指標受機器人關節參數影響，第一個關節為旋轉關節，其取值不影響可操作度數值，為便于分析，設θ1=-90°，分別考察虛擬連桿長度d4為0.5、2、5 m時，可操作度與θ2、θ3之間的關系，如圖6所示。可見，在整個運動范圍內，可操作度均大于零并且變化平緩，即不存在奇異位形以及運動性能的突變位置。可操作度指標是指機器人各個方向上運動能力的綜合度量，隨著虛擬連桿長度d4的增加，機器人末端點距基坐標系原點的距離增大，因此θ2、θ3的變化，能產生更大的末端位移，運動能力增加，這從圖6的可操作度變化趨勢可以看出，可操作度在逐漸增加。

機構條件數定義為

C(J)=‖J‖‖J+‖

(2)

式中J+——雅可比矩陣J的廣義逆矩陣

條件數的范圍為[1,∞)，當C=1時表明機構具有最佳的運動傳遞性能。令θ1=-90°，當虛擬連桿長度d4分別為0.5、2、5 m時，條件數與θ2、θ3之間的關系如圖7所示。可見在整個運動范圍內，條件數均大于1，取值合理，并且變化比較均勻，不存在不可控點，運動性能變化均勻。條件數指標是機器人各個方向上運動能力的均一性度量，圖7中隨著虛擬連桿長度d4的增加，條件數逐漸增大，表征運動能力均一性變差，這是因為旋轉關節在末端點產生運動的能力逐漸增加，而移動關節的運動能力未變。

圖7 機器人頭部條件數Fig.7 Condition number of robot head

3 目標跟蹤運動控制

3.1 運動學逆解算法

對于冗余機構，運動學逆解有無窮多個，需要通過合理的優化準則選擇最優解。設機器人頭部關節位置為q=(q1,q2,q3,q4)T=(θ1,θ2,θ3,d4)T，末端點在笛卡爾空間坐標為x=(x,y,z)T，則可得到

(3)

其中，J為3×4階雅可比矩陣。式(3)基于廣義逆的解為

(4)

其中

(5)

式中I——單位矩陣k——優化系數

式(4)中右邊第1項為最小范數解，第2項為關節自運動的優化解。

當J行滿秩時，J+=JT(JJT)-1。

為使機械臂規避關節極限位置，設優化函數為

(6)

式中qimax、qimin——第i個關節的上、下極限位置

在跟蹤運動控制過程中，機械臂關節位置q通過編碼器實時測量，機械臂末端點位置x通過前向運動學計算得到，設動態目標位置為xd，由視覺測量得到，可按如下步驟進行迭代計算：

(1)計算跟蹤位置差Δx=xd-x。

(3)計算關節空間位置更新值q=q+Δq，返回步驟(1)。

3.2 目標軌跡預測

如果目標速度較快，而視覺測量更新速度又較慢，則在2次測量點之間的測量盲區內目標已經運動到較遠位置，導致產生較大的跟蹤誤差。對于高精度視覺測量系統，一般更新頻率只能達到10 Hz左右，即更新間隔為0.1 s，如果目標速度為3 m/s，則在測量盲區內目標運動的距離為0.3 m，這就是跟蹤誤差。

如果能利用已測的目標軌跡預測測量盲區內的未測軌跡，以得到更致密的目標位置，則能減小跟蹤誤差。在各種目標位置預測的方法中，如卡爾曼濾波、粒子濾波、α-β濾波、最小二乘法濾波等，最小二乘法具有比較快的計算速度，更適合在實時目標跟蹤中應用[20]。

利用最小二乘法，對已測點進行多項式曲線擬合，即可用擬合曲線對測量盲區內目標軌跡進行預測。擬合曲線的次數越高，位置預測精度越高[20]，本文根據目標的運動特征，即大體上比較平緩，沒有劇烈變化，選擇二次多項式曲線進行位置擬合，兼顧精度與實時性要求。設最近4次視覺測量的時刻分別為(t1,t2,t3,t4)=(0,0.1,0.2,0.3) s，位置為xd1=(x1,y1,z1)T、xd2=(x2,y2,z2)T、xd3=(x3,y3,z3)T、xd4=(x4,y4,z4)T，利用此4個點的時間與位置可擬合出一條二次曲線，如果視覺測量間隔為0.1 s，利用此曲線在當前測量盲區內等時段預測9個點，即可有效提高測量帶寬，預測原理如圖8所示。

圖8 軌跡預測方法Fig.8 Trajectory prediction method

以x方向為例說明位置預測方法，y、z方向與此類似。設擬合曲線為

d(t)=a0+a1t+a2t2

(7)

則根據最小二乘法原理，式(7)的解為

A=(PTP)-1PTX

(8)

4 仿真

設目標運動軌跡為三維空間曲線，可以表示成二次多項式形式，參數方程為

(9)

仿真時間為0～1 s。設視覺測量更新周期為0.1 s，跟蹤控制周期為0.01 s，關節最大速度為360(°)/s，優化系數k=-0.000 01。機器人頭部初始位形為(-90°,180°,-90°,0.4 m)，初始位形時末端點位置與目標初始位置有一定距離，這與實際情況中機械臂初始位形與目標的關系存在隨意性比較相符，即總是存在一定的未知距離。在0～1 s內視覺測量更新10次，跟蹤控制100次，采用軌跡預測方法的跟蹤效果及機器人頭部構型變化如圖9所示。

圖9 目標跟蹤效果Fig.9 Target tracking effect

跟蹤誤差如圖10所示，為便于比較，將采用非軌跡預測方法的跟蹤誤差也一并顯示。可見，初始位形有較大跟蹤誤差，約0.185 m，但機器人頭部在0.1 s內就能跟上目標，但跟蹤誤差仍然明顯，最大約為0.09 m，這是因為在測量盲區內目標運動距離較遠。直到視覺測量采集到4個點位置后，軌跡預測功能開始起作用，采用軌跡預測方法的跟蹤誤差顯著減小到0.018 m左右，而采用非軌跡預測方法的跟蹤誤差在整個過程中有逐漸增大的趨勢，這是因為目標一直處于加速狀態，在測量盲區內運動的距離逐漸增大，表現出來的效果就是跟蹤誤差逐漸增大。可見，軌跡預測方法對提高跟蹤精度作用明顯，能將跟蹤精度提高約80%。

圖10 目標跟蹤誤差Fig.10 Target tracking error

關節位置變化如圖11所示，關節1、關節2均離極限位置較遠，關節3的最小位置為-102.5°，與下極限位置-104°仍有安全距離，可見在跟蹤過程中，基于梯度投影的優化算法很好地保證了關節不超越極限位置。如果去除極限位置保護功能，即令k=0，則關節位置變化如圖12所示，此時關節3的最小位置為-104.5°，超越極限位置，這在實際中是不允許的。可見，利用梯度投影法可以在得到運動學逆解的同時，獲得次目標的優化解，這不僅是實際物理結構的需要，也是仿人功能的體現，人類頭頸在進行視覺目標跟蹤時，總是以最快、最省力的方式快速跟上目標，并且保證頭頸都在舒服的區間內運動，這是人類天生的同步優化能力。

圖11 跟蹤過程關節位置變化Fig.11 Joint position changes during tracking

圖12 去除極限位置規避功能的關節位置變化Fig.12 Joint position changes under no limits avoidance

5 實驗驗證

在倍福控制器中，采用VC++軟件實現上述仿人機器人頭部目標跟蹤算法，視覺測量周期為0.1 s，跟蹤控制周期為0.01 s，考察仿人機器人頭部實際的目標跟蹤能力。小球在機器人頭部前面約1 m處作空間三維曲線運動，由人手控制小球運動，因此小球運動軌跡表現出一定的隨意性。

圖13為目標跟蹤實驗拍攝視頻的連續截屏，持續時間為18 s，共截取8個姿態，均處于穩定跟蹤階段，可見仿人機器人頭部視線始終跟隨目標運動，跟蹤誤差較小，達到了高精度跟蹤的效果。因此，前文中提到的仿人機器人頭部控制方法，能對動態目標實現穩定、平滑的視覺跟蹤。

圖13 仿人機器人頭部目標跟蹤實驗Fig.13 Target tracking experiment of humanoid robot head

6 結論

(1)對仿人機器人頭部采用冗余機構方案，可以提高視覺跟蹤的靈巧性，在進行目標跟蹤的同時，可以實現次目標，如規避關節極限位置，相較于傳統的雙軸跟蹤機構，冗余機器人頭部更好地展示了“仿人”的功能。

(2)基于虛擬連桿方法對目標跟蹤任務進行運動學建模，將跟蹤問題轉換成機械臂運動學逆解問題，即可以借助于機械臂運動學性能指標，有效地評估機構對跟蹤任務的靈活性。

(3)在視覺測量的盲區內采用軌跡預測方法，可以有效提高跟蹤精度。

1 吳偉國.面向作業與人工智能的仿人機器人研究進展[J].哈爾濱工業大學學報，2015,47(7): 1-19.

WU Weiguo.Research progress of humanoid robots for mobile operation and artificial intelligence[J].Journal of Harbin Institute of Technology, 2015,47(7): 1-19.(in Chinese)

2 于秀麗, 魏世民, 廖啟征.仿人機器人發展及其技術探索[J].機械工程學報, 2009,45(3): 71-75.

YU Xiuli, WEI Shimin, LIAO Qizheng.Development and technology research of humanoid robot[J].Chinese Journal of Mechanical Engineering,2009,45(3): 71-75.(in Chinese)

3 LAW J, SHAW P, LEE M, et al.From saccades to grasping: a model of coordinated reaching through simulated development on a humanoid robot[J].IEEE Transactions on Autonomous Mental Development, 2014, 6(2):93-109.

4 TAN K C, CHEN Y J, TAN K K, et al.Task-oriented developmental learning for humanoid robots[J].IEEE Transactions on Industrial Electronics, 2005, 52(3):906-914.

5 KIM Y D, LEE B J, RYU J H, et al.Landing force control for humanoid robot by time-domain passivity approach[J].IEEE Transactions on Robotics, 2007, 23(6):1294-1301.

6 ZHAO Jing, LI Wei, MAO Xiaoqian, et al.Behavior-based SSVEP hierarchical architecture for telepresence control of humanoid robot to achieve full-body movement[J].IEEE Transactions on Cognitive and Developmental Systems, 2017, 9(2):197-209.

7 LIU H Y, WANG W J, WANG R J, et al.Image recognition and force measurement application in the humanoid robot imitation[J].IEEE Transactions on Instrumentation and Measurement, 2012, 61(1):149-161.

8 LIU Zhi, XU Shuqiong, ZHANG Yun, et al.A multiple-feature and multiple-kernel scene segmentation algorithm for humanoid Robot[J].IEEE Transactions on Cybernetics, 2014, 44(11):2232-2240.

9 BREAZEAL C, EDSINGER A, FITZPATRICK.P, et al.Active vision for sociable robots[J].IEEE Trans.Syst.Man Cybern.A, 2001,31:443-453.

10 KEDZIERSKI J, MUSZYSKI R, ZOLL C, et al.EMYS—emotive head of a social robot[J].International Journal of Social Robotics, 2013, 5: 237-249.

11 DIFTLER M A, AHLSTROM T D, JOYCE C A, et al.Robonaut 2-initial activities on-board the ISS[C]∥In Proc.IEEE Aerospace Conference, Big Sky, MT, 2012:1-12.

12 BEIRA R, LOPES M, PRAGA M, et al.Design of the robot-cub (iCub) head[C]∥IEEE International Conference on Robotics & Automation, Orlando, Florida, 2006: 94-100.

13 KIM D K, KIM J H, KWON H J, et al.Fabrication of robot head module using contact-resistance force sensor for human-robot interaction and its evaluation[J].Journal of Mechanical Science and Technology, 2012, 26(10):3269-3276.

14 官伯林, 賈建援, 朱應敏.基于自適應差分進化算法的三軸光電跟蹤策略[J].機械工程學報, 2012, 48(10): 14-21.

GUAN B L, JIA J Y, ZHU Y M.Tracking strategy of three-axis photoelectric tracking system based on adaptive differential evolution algorithm[J].Journal of Mechanical Engineering, 2012, 48(10): 14-21.(in Chinese)

16 祁若龍, 張偉, 王鐵軍,等.仿人頭頸部機器人跟蹤運動控制方法[J].吉林大學學報: 工學版, 2016, 46(5): 1595-1601.

QI Ruolong, ZHANG Wei, WANG Tiejun, et al.Tracking motion control method of hunanoid head and neck robot[J].Journal of Jilin University: Engineering and Technology Edition, 2016, 46(5): 1595-1601.(in Chinese)

17 GIULIO M, LUCA V, ALESSANDRO D L.Adaptive predictive gaze control of a redundant humanoid robot head [C]∥2011 IEEE/RSJ International Conference on Intelligent Robots and Systems, San Francisco, USA: CA, 2011: 3192-3198.

18 GU Lizhong, SU Jianbo.Gaze control on humanoid robot head[C]∥Proceedings of the 6th World Congress on Intelligent Control and Automation, Dalian, China, 2006: 21-23.

19 范曉鵬, 郝穎明, 朱楓, 等.面向機器人航天員的球形目標識別與定位方法研究[J].載人航天, 2016, 22(3): 375-380.

FAN Xiaopeng, HAO Yingming, ZHU Feng, et al.Study on spherical target identification and localization method for robonaut[J].Manned Spaceflight, 2016, 22(3): 375-380.(in Chinese)

20 鐘建冬, 蘇劍波.基于視覺預測的運動目標實時跟蹤系統[J].機器人,2010, 32(4): 516-521.

ZHONG Jiandong, SU Jianbo.A real-time moving object tracking system based on virtual prediction[J].Robot, 2010, 32(4): 516-521.(in Chinese)

DesignofHumanoidRobotHeadandMotionControlforObjectTracking

WAN Minhong1,2ZHOU Weijia1LIU Yuwang1

(1.TheStateKeyLaboratoryofRobotics,ShenyangInstituteofAutomation,ChineseAcademyofSciences,Shenyang110016,China2.UniversityofChineseAcademyofSciences,Beijing100049,China)

According to the need for visual target tracking task of the humanoid robot, a new kind of humanoid robot head with high precision and good dexterity was put forward, and the visual perception module and the three degree of freedom robot mechanism were constructed.In order to evaluate the motion ability of humanoid robot head for the target tracking task，the kinematics model of the robot head was established by using the virtual link method, which made the problem of object tracking transform to the problem of inverse kinematics.And the kinematics characteristics was studied by the calculation of condition number and manipulability, which showed that the dexterity of the humanoid robot head was good for the tracking task.Based on this, by using gradient projection method, the synchronization optimization solution was obtained for the inverse kinematics and the avoidance of joint limits.Because of the complexity of image processing, the speed of the visual measurement system was generally slow, and the bandwidth was usually only about 10 Hz.Therefore, the target position can not be updated in the measurement blind area between the two measuring points, which led to great tracking error.According to this problem, a prediction method of trajectory in the two visual measurement interval to estimate the target position was proposed.The simulation and experimental results showed that the tracking accuracy can be improved by about 80% by using the trajectory prediction method in the blind area of vision measurement, and the high precision target tracking can be realized.The humanoid robot head and motion control method proposed had good tracking performance for dynamic target.

humanoid robot; head; object tracking; virtual link; gradient projection method; trajectory prediction

10.6041/j.issn.1000-1298.2017.10.051

TP242

A

1000-1298(2017)10-0401-07

2017-07-05

2017-08-09

國家自然科學基金項目(51605474)

宛敏紅(1982—)，男，副研究員，博士生，主要從事機器人學研究，E-mail: wanminhong@sia.cn