仿人机器人头部设计与目标跟踪运动控制

宛敏红 周维佳 刘玉旺

(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