基于Kinect的七自由度空间机械臂体感控制方法

张博文，黄攀峰，刘正雄

(1. 西北工业大学航天学院智能机器人研究中心，西安 710072； 2. 西北工业大学航天飞行动力学技术重点实验室，西安 710072)

1 引言

随着机器人技术的发展，机械臂的应用范围越来越广，在某些特殊的环境下，由机械臂代替人体完成任务将极大的降低危险性，例如空间、深海、辐射环境等[1]。使用传统的机械臂控制方式控制难度较大，学习成本较高。因此，机器人的体感控制逐渐发展起来。与机械臂的传统控制方式相比，体感控制具有许多优点，如直观、便捷、易用等。

Kinect是微软发布的深度信息传感器[2]，由于其能够采集多个用户信息，配置和使用简单，精度高等优点，近年来被逐渐使用在机械臂的体感控制上。使用Kienct传感器对机械臂进行体感控制可以充分发挥人手臂的灵活特点，操作者只需对自身手臂进行控制，无需附加其他设备。

使用Kinect对机械臂进行体感控制，关键问题是在操作者的手臂运动与机械臂的运动之间建立运动映射。人体与机械臂的运动映射方法主要有关节空间映射和末端笛卡尔空间映射两种[3]。Nguyen等[4]针对三自由度机械臂，采用关节空间映射方法将人臂自由度映射到机械臂相应关节上，并使用空间向量法计算机械臂关节角度，实现运动复现；但使用的机械臂自由度较低。卫沅等[5]针对特定构型七自由度机械臂，使用四个关节点求取逆运动学解，对机械臂进行实时控制。王朝阳等[6]针对SRS(Spherical-Roll-Spherical)构型的七自由度机械臂，提出了一种基于肘部关节约束的逆运动学解法，根据关节点位置直接解算出机械臂角度。上述两种方法均针对特定结构机械臂，而对肩、肘均存在偏置的冗余机械臂则无法求解。

为了解决现有方法无法使用人臂对肩、肘均存在偏置的七自由度空间机械臂进行体感控制的问题，本文提出一种基于操作周期和运动周期的增量式关节角映射方法，在操作周期内使用Kinect传感器采集操作者手臂关节点坐标，经滤波处理后使用空间向量法计算关节角，并将关节角增量映射给机械臂关节角，作为机械臂控制指令实现体感控制。

2 人臂与机械臂建模

手臂关节由肩关节、肘关节、腕关节组成，肩关节和腕关节能够绕三轴运动，具有三个自由度；肘关节可以视为一个转动关节，具有一个自由度。在将人臂自由度向机械臂自由度映射时，可将球关节等效为三个轴线相交的串联轴，以人体右臂为研究对象，七个自由度的划分如图1所示。

本文以如图2所示七自由度空间机械臂作为被控对象。机械臂末端在上方，基座在下方并固定于服务航天器上，其结构类似于人臂。基座附近和末端附近均具有三个自由度，可以视为肩关节和腕关节，但三轴不交于一点；中间有一个自由度，可以视为肘关节。

图1 手臂关节自由度Fig.1 DOF of the human arm

图2 使用的七自由度空间机械臂Fig.2 The 7-DOF space manipulator used

将人手臂垂直放下，手心向内作为初始状态，将机械臂展开，末端向内作为初始状态。该空间机械臂的结构与人手臂是十分相似的，因此可以使用关节角映射的方法来进行空间机械臂的体感控制。

3 体感控制方案

3.1 坐标转换

本文使用Kinect V2传感器，能够采集每个用户25个关节，能够采集的关节已由Kinect传感器直接给定，能够采集的部分关节角如图3所示[7]。

图3 部分人体骨骼关键点[7]Fig.3 Part of skeleton joint positions of the human[7]

本文使用空间向量法(该方法详见文献[8])求取操作者手臂关节角度，需处理的关节包括：脊柱O、右肩A、右肘B、右腕C、右手掌D、右手掌尖E、右拇指尖F，左肩G。

由于Kinect提供的人体关节点坐标是基于Kinect相机坐标系的，而Kinect相机坐标系固连于Kinect传感器。在计算关节角向量时需要关节点相对于固连于人体的基座标系的坐标，故定义人体右肩关节点作为人体基座标系原点，将关节点坐标转换到与人体固连的基坐标系中。

定义Kinect相机坐标系为Ok，固连于人体的基坐标系为Oh，定义人体基座标系的Z轴方向为人体左肩关节指向右肩关节。坐标转换分两步进行，包括平移和旋转：首先，记右肩关节A在Ok中的坐标为A(xA,yA,zA)，平移变换即将待转换关节点的坐标减去右肩关节A在Ok中的坐标A(xA,yA,zA)，易得转换完成后关节点A的坐标为A(0,0,0)。第二步为旋转变换，记任意关节点V进行平移变换之后，旋转变换前的坐标为V(xk,yk,zk)，进行旋转变换后的坐标为Vh(xh,yh,zh)，两者的关系如式(1)：

(1)

其中RT是正交旋转矩阵，可以用式(2)表示[6]：

(2)

ix=[1,0,0]T,jy=[0,1,0]T,kz=[0,0,1]T

(3)

3.2 操作周期与运动周期定义

(4)

(5)

Kinect传感器可直接输出操作者手势信息，因此可以方便地使用Kinect传感器采集操作者手势变化判断操作周期的开始与结束：操作者由握拳到手张开且五指并拢为操作周期开始，操作者由手张开且五指并拢到握拳为操作周期结束。为避免手势对关节角的影响，使用左臂作为手势采集的目标。操作周期开始后，操作者进行手臂动作，机械臂得到关节角信号，开始运动，运动周期开始；操作周期结束后，机械臂运动停止，运动周期结束。即每个操作周期对应一个运动周期，连续循环直至操作任务完成。这样，操作者操纵机械臂完成任务的过程即可划分为若干个操作周期，机械臂的动作过程依据操作周期划分为若干个运动周期。在分析操作者操纵机械臂的过程时，具体分析每一个操作周期即可，在研究机械臂完成整个运动的过程时，具体分析每一个运动周期即可。

3.3 基于周期循环的控制方法

(6)

在第k个运动周期内，机械臂关节角计算如式(7)：

(7)

图4 操作周期与运动周期Fig.4 The action period and the movement period

经过多个操作周期与运动周期，即可完成相应任务。

4 体感控制策略设计

4.1 滤波器设计

Kinect传感器的输出数据存在一定的噪声，某些帧甚至会出现数据的剧烈波动，使用带有噪声或这种不稳定值的数据直接计算机械臂关节角会造成机械臂运动的不稳定。因此，必须对Kinect传感器的输出数据进行滤波处理，以得到平滑、稳定的关节角信号。

王朝阳等[6]设计了一种基于限幅的实时消抖滤波算法，通过定义抖动阈值与计数器容量来消除七个关节角的抖动，但是滤波器导致了数据延迟，降低了操作的实时性。范勇涛[9]使用卡尔曼滤波对计算得到的关节角进行平滑处理，消除了噪声，但是只对数据进行了平滑处理，没有考虑数据跳变的情况。但是，上述方法均是直接对关节角数据进行滤波处理，而忽略了对计算关节角的数据来源，即关节点坐标的分析。因此，本文首先对关节点坐标进行识别精度分析，然后根据分析结果设计滤波器。

4.1.1 关节点坐标识别精度分析

为了有针对性的设计识别算法，必须了解Kinect对人体关节点识别精度的影响因素，了解噪声和数据跳变产生的原因。

在操作者的运动过程中，靠近身体的关节点，如肩关节，与远离身体的关节，如肘关节、腕关节等运动范围与运动的剧烈情况显然不同，关节点的识别精度也不相同。选取右肩关节点A，右肘关节点B及右拇指关节点F，对比其在一段时间内，操作者进行手臂操作运动时采集到的Z轴上坐标数据如图5，其中横坐标单位选取为帧，Kinect传感器采集数据的频率为30 FPS。

图5 A、B、F三个关节点在Z轴上的坐标数据Fig.5 Coordinate datain Z axis of three joint point

由图5可见，在操作者运动过程中，右肩关节是最稳定的，而肘部关节具有一定波动，在第220和360帧出现了较为严重的数据波动，而拇指关节的数据失真现象是最严重的，数据波动也最多。从图中可以发现，越靠近手部的关节点，在操作时其数据波动越大，不可信数据越多。因此在滤波时，既需要平滑处理，也需要剔除失真的数据。

本文在计算手臂腕关节的三个关节角时，使用了腕部的关节点坐标进行计算。因此取右手部位三个关节点——右手掌关节点D、右手中指尖关节点E、右手拇指关节点F——的Z轴坐标数据进行对比，如图6所示。

图6 D、E、F三个关节点在Z轴上的坐标数据Fig.6 Coordinate datain Z axis of three joint points

由图可见，手掌部分三个关节的坐标极为接近，且某些帧均存在较大的数据波动。在计算关节角时，使用这些关节点数据会造成误差的累计和放大。

综上，Kinect传感器对手掌部位的三个关节点识别精度较低。这一方面是由于手部关节易于产生遮挡等情况，造成某些帧数据波动；另一方面由于手掌附近的三个关节点坐标值差距很小，经过计算导致误差的放大。这三种原因会导致计算得到的腕关节三个关节角波动较大，难以反映真实情况。

4.1.2 滤波器设计

针对上文对Kienct采集关节角数据精度的分析，本文提出一种限幅平滑滤波算法，滤波过程分两部进行：

1)限幅滤波，主要目的是消除某些特殊帧产生的大幅度数据波动。定义两次采样之间允许的大偏差值T，每次采样后进行判断。如果本次值与上次值之差小于最大偏差值T，则本次值有效；如果本次值与上次值之差大于最大偏差值T，则本次值无效，认定本次采样值为数据波动，放弃本次值，用上次值代替本次值。

2)五点三次平滑滤波，主要目的是消除采样值在真实值附近微小范围内的抖动，使计算出的机械臂关节角更加稳定、平滑。取五次连续采样值进行拟合，得到一条三次函数曲线，使用三次函数曲线上相应点的值作为平滑结果，每次采样进行一次平滑处理。

设连续的五次采样值为yi-2,yi-1,yi,yi+1,yi+2，设有式(8)所示三次多项式：

y=a0+a1x+a2x2+a3x3

(8)

(9)

综上，限幅平滑滤波算法伪代码如下：

/**********************/

Algorithm

input:realtimeValue[i-2],realtime Value[i-1],realtime Value[i],realtime Value[i+1],realtime Value[i+2],max Difference

output: steady Value[i]

/**********************/

If i<3Then

steady Value[i]=realtime Value[i]

Else

if|realtime Value[i]-realtime Value[i-1]| >max Difference

realtime Value[i]=realtime Value[i-1]

End

SteadyValue[i]=(-3*realtimeValue[i-2]+

12*realtimeValue[i-1]+17*realtimeValue[i]+12*realtimeValue[i+1]-3*realtimeValue[i+2])/35

End

return realtime Value[i]

/**********************/

4.1.3 滤波器效果验证

针对右肩关节点G、右肘关节点B及右拇指关节点F对比其在一段时间内，操作者进行手臂操作运动时采集到的Z轴上坐标的滤波前后数据，如图7所示。

图7 三个关节点在Z轴上滤波后的坐标数据Fig.7 Coordinate data in Z axis of three joint points after filtering

与图5进行对比可以发现，关节坐标数据的微小抖动现象抑制明显，数据相比于滤波前更加稳定流畅，且相对于未滤波前的数据延迟仅两帧。

4.2 基于分步操作周期的体感控制策略

基于上述结果，本文提出一种将操作周期分两步进行，仅使用操作者手臂的前四个自由度完成体感操作的改进策略。即将操作周期和运动周期都分为如下两步：

1)当操作者做出操作周期开始手势(左手张开，五指并拢)时，采集操作者肩关节和肘关节的四个关节坐标，经滤波后计算得到机械臂关节角，映射给机械臂前四个关节，此时机械臂运动周期第一步开始，此时后三个关节角保持固定，当操作者做出停止手势(左手握拳)时，操作周期第一步结束，开始操作周期第二步；

2)采集操作者肩关节三个关节角，实时映射给机械臂末端三个关节，此时机械臂前四个关节停止运动，保持静止。

图8 分步操作周期Fig.8 The operation period by step

5 试验验证

针对上文提出的基于操作周期与运动周期的体感控制算法与滤波算法，设计仿真试验进行验证，首先搭建仿真机械臂控制系统，然后在虚拟环境中构建机械臂工作环境，实现对七自由度空间机械臂的体感控制。

5.1 平台搭建

本文搭建的试验平台结构如图9所示，包括操作者、Kinect传感器、数据处理与仿真系统三部分。

图9 试验平台结构Fig.9 The structure of experiment platform

在Unity3D物理引擎中搭建机械臂仿真试验场景，作为仿真系统，通过调用Kinect SDK实时获取Kinect传感器读取的数据。仿真试验场景如图10所示。

图10 仿真试验场景Fig.10 The scene of simulation experiment

5.2 空间机械臂体感控制仿真试验

试验通过使用上述基于分步操作周期的体感控制策略来实时控制仿真环境中的机械臂末端到达其工作空间内的某一空间点。试验过程分为两步：

1) 操作者做出初始姿势，开始第一个操作周期，空间机械臂从图11所示的初始状态开始第一个运动周期。

图11 操作者初始姿势Fig.11 The initial gesture of the operator

2) 操作者进行动作，实时控制机械臂运动；同时通过左手手势控制操作周期的开始与结束；操作周期与运动周期循环进行，制导机械臂到达目标状态。

试验结果如图12所示，可见试验经历了两个操作周期。操作周期第一步时，操作者左臂处于固定姿势，使用手势进行操作周期开始和结束的操作。第一个操作周期略过了第二步，因为此时机械臂末端还没有到达目标点附近，因此并未对机械臂的腕关节三个自由度进行调整，直至第二个操作周期的第二步，机械臂腕关节到达指定位置之后，开始对机械臂后三个自由度进行控制，以达到更加精细的末端位姿。

图12 空间七自由度机械臂体感控制Fig.12 Somatosensory control of 7-DOF space manipulator

6 结论

1)提出的基于操作周期和运动周期的机械臂体感控制方法，使用关节角增量将人体手臂关节角映射给机械臂关节角，经仿真验证，可实现机械臂关节角稳定的体感控制。

2)提出的针对关节点坐标的限幅平滑滤波算法，经仿真验证，能有效剔除某些帧的较大失真值，并削减采样值在真实值附近的微小抖动。