CN105252532A - 动作捕捉机器人协同柔性姿态控制的方法 - Google Patents

Abstract

一种动作捕捉机器人协同柔性姿态控制的方法，利用体感设备Kinect实时捕获不同时刻目标人体对应目标关节位移数据；将目标关节位移数据发送到PC端；PC端实时绘制出人体骨骼框架；PC端将人体骨骼框架动态显示在屏幕上，并提供报错反馈；得到关节角度数据；对人形机器人NAO的舵机进行控制，使人形机器人NAO实时跟踪目标人体动作。本发明利用Kinect捕获目标人体的位移关节数据，识别人体的各种姿态，实现机器人实时模仿人体动作。本发明人体动作识别速度快、精度高，机器人追踪动作流畅，且可以实时反映出目标人体的姿态，在不同环境复杂条件下具有较好的鲁棒性。

Description

动作捕捉机器人协同柔性姿态控制的方法

技术领域

本发明涉及一种动作捕捉机器人协同柔性姿态控制的方法，属于人机交互领域和人工智能领域。

背景技术

人机交互(HCI，Human-ComputerInteraction)是一门关于设计、评价和实现供人们使用的交互式计算机***，以及研究由此而发生的相关现象的学科。人机交互技术主要是研究人与计算机之间的信息交换，体现在人到计算机和计算机到人的信息交互两部分，前者是指人们可以借助键盘、鼠标、操作杆、位置***、数据手套等设备，用手、脚、声音、姿态和身体的动作、视线甚至脑电波等向计算机传递信息；后者是计算机通过打印机、绘图仪、显示器、头盔式显示器、音箱等输出设备或显示设备给人提供信息。

Kinect是微软在2009年6月2日的E3大展上，正式公布的XBOX360体感周边外设。Kinect彻底颠覆了游戏的单一操作，使人机互动的理念更加彻底的展现出来。它是一种3D体感摄影机，同时它导入了即时动态捕捉、影像辨识、麦克风输入、语音辨识、社群互动等功能。体感，即躯体感觉，是触觉、压觉、温觉、痛觉和本体感觉(关于肌肉和关节位置和运动、躯体姿势和运动以及面部表情的感觉)的总称。采用LightCoding和光源标定技术的Kinect利用红外线发射器发出雷射光，通过红外线CMOS摄像机记录空间中的每个散斑，结合原始散斑图案，通过晶片计算出具有3D深度的图像，进而转换到骨架追踪***，使得Kinect可以应用到许多领域。

NAO是由AldebaranRobotics公司研制的一个身高58厘米的可编程仿人机器人。其身体具有25个自由度，主要元件为电机和电动致动器。其运动模块基于广义逆运动学，可处理笛卡尔坐标系、关节控制、平衡、冗余性和任务优先性。NAO头部内嵌英特尔ATOM1.6GHz处理器，运行LINUX内核，拥有NAOqi操作***，是一个开源的软件平台，可用C++或Python语言进行编程。基于以上特点，机器人NAO可以最逼真地实现动作跟踪，是本发明的重要组成部分。

发明内容

针对现有技术的不足，本发明涉及一种动作捕捉机器人协同柔性姿态控制的方法。本发明的目的在于为人机交互提出一种新模式。

本发明的技术方案如下：

一种动作捕捉机器人协同柔性姿态控制的方法，包括步骤如下：

(1)利用体感设备Kinect实时捕获不同时刻目标人体对应目标关节位移数据；目标人体站在体感设备Kinect视距范围内，为保证最佳识别效果，目标人体应处在体感设备Kinect的镜头前1.2-3.5m，水平视角±57°以内；

(2)将体感设备Kinect捕获到的目标关节位移数据发送到PC端；

(3)PC端接收所有目标关节位移数据，并根据其实时绘制出人体骨骼框架；

(4)PC端将人体骨骼框架动态显示在屏幕上，并提供报错反馈；

(5)PC端对接收到的所有目标关节位移数据进行处理：包括相对阈值距离比较滤波计算、空间向量计算，得到人形机器人NAO的姿态控制数据，即关节角度数据；本发明由于环境因素和传感器自身抖动等原因，导致得到的原始数据里存在干扰数据，因此有必要对原始数据进行相对阈值距离比较滤波计算，使得机器人动作追踪更加准确、可靠；

(6)调用人形机器人NAO的NAOqi操作***中的JointControl函数，根据传送来的关节角度数据，对人形机器人NAO的舵机进行控制，使人形机器人NAO实时跟踪目标人体动作。

根据本发明优选的，所述步骤(4)还包括，所述PC端动态显示人体骨骼框架：如果PC端显示的人体骨骼框架与目标人体的动作一致，则将关节角度数据发送至人形机器人NAO；如果PC端显示的人体骨骼框架与目标人体的动作不匹配，则重新初始化程序，以保证人形机器人NAO收到可靠姿态控制数据。

根据本发明优选的，所述步骤(5)中，所述相对阈值距离比较滤波计算包括：

通过观察位移关节点的空间坐标变化，计算固定时间段(例如：0.1s-0.5s)内同一关节在开始时间和结束时间组成的波动向量，观察该波动向量的模以及在空间坐标系各个方向的波动，通过设定波动阈值筛选关节波动值。可以看出关节点识别位置的抖动主要是沿坐标轴方向的快速抖动，且出现抖动时波动向量的模会大幅增加。因此，应对波动较大的关节点做相应处理，对小幅度波动的关节点保持上一状态不变，对于不同的关节处使用不同的阈值比较，保证每次滤波后的结果都是最优解，以保证机器人姿态变换的连续性。

根据本发明优选的，所述步骤(5)中，所述空间向量计算包括：

Kinect所使用的空间坐标系不同于常见的空间坐标系，其x轴与y轴的零点与传统空间坐标系相同，但其z轴坐标零点为Kinect传感器，正方向为Kinect指向的正前方。Kinect空间坐标系如图6所示：

由Kinect空间坐标系中，得出Kinect坐标系中任意两个不重合的坐标点P1(x1,y1,z1)，P2(x2,y2,z2)，对其组成的向量P1P2：

若存在另一点P3且该点不在P1P2所在的直线上，则存在以下关系式：

根据上述性质，将人体关节角度计算简化为对空间向量夹角的计算，下面将分别说明上肢关节角度的计算方法：

1)关节角度：LeftElbowRoll

如图7所示：计算LeftElbowRoll角度只需构造该空间夹角两边所在的一组向量，

并根据上文中提到的关节角度计算公式得：

$LeftElbowRoll=\∠ABC=arccos\frac{\stackrel{\→}{BA}\·\stackrel{\→}{BC}}{\left|\stackrel{\→}{BA}\right|\left|\stackrel{\→}{BC}\right|}---(3-3)$

2)关节角度：LeftElbowYaw

如图8所示：

LeftElbowYaw关节的角度为肘部绕上臂旋转时产生的角度，此角度通常情况下为ABC和BCD两相交平面的夹角，即图中平面S1和S2的夹角；根据空间平面夹角计算公式得出角度LeftElbowRoll的计算方法如下。

首先需要明确计算两非共线向量所在平面的法向量的公式：

$\stackrel{\→}{m}=\stackrel{\→}{a}\×\stackrel{\→}{b}=\det \left|\begin{array}{ccc}i& j& k\\ a_x& a_y& a_z\\ b_x& b_y& b_z\end{array}\right|---(3-4)$

因此S1、S2平面的法向量表示为：

$\begin{array}{c}\stackrel{\→}{M_1}=\stackrel{\→}{BA}\×\stackrel{\→}{BC}=\det \left|\begin{array}{ccc}\stackrel{\→}{i}& \stackrel{\→}{j}& \stackrel{\→}{k}\\ {\mathrm{BA}}_x& {\mathrm{BA}}_y& {\mathrm{BA}}_z\\ {\mathrm{BC}}_x& {\mathrm{BC}}_y& {\mathrm{BC}}_z\end{array}\right|\\ =({\mathrm{BA}}_y{\mathrm{BC}}_z-{\mathrm{BA}}_z{\mathrm{BC}}_y)\stackrel{\→}{i}+({\mathrm{BA}}_z{\mathrm{BC}}_x-{\mathrm{BA}}_x{\mathrm{BC}}_z)\stackrel{\→}{j}\\ +({\mathrm{BA}}_x{\mathrm{BC}}_y-{\mathrm{BA}}_y{\mathrm{BC}}_x)\stackrel{\→}{k}\end{array}---(3-5)$

$\begin{array}{c}\stackrel{\→}{M_2}=\stackrel{\→}{CB}\×\stackrel{\→}{CD}=\det \left|\begin{array}{ccc}\stackrel{\→}{i}& \stackrel{\→}{j}& \stackrel{\→}{k}\\ {\mathrm{CB}}_x& {\mathrm{CB}}_y& {\mathrm{CB}}_z\\ {\mathrm{CD}}_x& {\mathrm{CD}}_y& {\mathrm{CD}}_z\end{array}\right|\\ =({\mathrm{CB}}_y{\mathrm{CD}}_z-{\mathrm{CB}}_z{\mathrm{CD}}_y)\stackrel{\→}{i}+({\mathrm{CB}}_z{\mathrm{CD}}_x-{\mathrm{CB}}_x{\mathrm{CD}}_z)\stackrel{\→}{j}\\ +({\mathrm{CB}}_x{\mathrm{CD}}_y-{\mathrm{CB}}_y{\mathrm{CD}}_x)\stackrel{\→}{k}\end{array}$

LeftElbowYaw关节的角度即等于M1、M2两法向量的夹角：

$LeftElbowYaw=arccos\frac{\stackrel{\→}{M1}\·\stackrel{\→}{M2}}{\left|\stackrel{\→}{M_1}\right|\left|\stackrel{\→}{M_2}\right|}---(3-7)$

3)关节角度：LeftShoulder.Roll

如图9所示：

$LeftShoulderRoll=arccos\frac{\stackrel{\→}{BA}\·\stackrel{\→}{BC}}{\left|\stackrel{\→}{BA}\right|\left|\stackrel{\→}{BC}\right|}---(3-8)$

4)关节角度：LeftShoulderPitch

如图10所示：

LeftShoulderPitch关节的角度为上臂与双肩轴线组成的平面与双肩轴线与脊柱点组成的平面之间的夹角，此角度通常情况下为ABC和BCD两相交平面的夹角，类比LeftElbowYaw关节角的计算方法，根据空间平面夹角计算公式可得出角度LeftShoulderPitch的计算方法如下：

平面ABC的法向量：

平面BCD的法向量：

关节角LeftShoulderPitch计算：

5)手部张合角度：LeftHand

由于KINECT对于手部信息的读取无法精确到手指的状态，因此对于手部张合角度计算，通过计算Lefthand和LeftWrist之间的距离来估算手部张合的角度，计算方法如下：

如图11所示：

$LeftHaud=\left|\stackrel{\→}{AB}\right|=\sqrt{{(A_x-B_x)}^2+{(A_y-B_y)}^2+{(A_z-B_z)}^2}---(3-12)$

至此，左臂所有关节的角度全部计算完毕，同理可以计算右臂关节角度；

6)关节角度：HeadPitch

头部两个关节中，与低头和抬头有关的角度名称HeadPitch

如图12所示：

头部关节HeadPitch的角度选取肩中指向头部的向量与两肩与脊柱组成的平面的法向量之间的夹角，具体计算公式如下：

7)关节角：HeadYaw

如图13所示：

头部另外一个重要的关节角HeadYaw负责实现头部左右转动，由于KINECT骨骼识别结果中无法给出面部朝向的数据信息，因此HeadYaw角度无法进行直接计算，然而通过观察骨骼绘制动画可以看出，头部的空间位置明显位于肩中的前方(即正面面对KINECT时，Head点和Shouldercenter点在z轴上存在一定距离)，因此关于此关节角本文将转化为平面夹角的计算：

平面ABC的法向量

平面BCD法向量

关节角度

在机器人的运动过程中下肢关节活动将直接影响到机器人整体的平稳性，为了简化控制难度，本文对于下肢控制采用相对位置法，通过计算下肢末端在相对坐标系中的位置结合人体质心的高度，实现对机器人动作的控制；

如图14所示：

将骨骼识别结果的Hipcenter点垂直投影到地面，作为新的坐标系原点，取左右脚踝点Rightankle和Leftankle在新坐标系中坐标作为机器人的控制数据。

B、C两点在O坐标系中坐标如下，

LeftFoot＝(A_z-B_z,A_x-B_x)(3-17)

RightFoot＝(A_z-C_z,A_x-C_x)(3-18)

为了由于不同的人身高差异造成的绝对距离误差，此处将坐标数值除以人体胯宽，其中胯宽计算公式如下：

$Hipwidth=\left|\stackrel{\→}{BC}\right|=\sqrt{{(B_x-C_x)}^2+{(B_y-C_y)}^2+{(B_z-C_z)}^2}---(3-19)$

因此下肢末端在新坐标系中的位置如下所示:

$LeftFoot=(\frac{A_z-B_z}{Hipwidth},\frac{A_x-B_x}{Hipwidth})---(3-20)$

$RightFoot=(\frac{A_z-C_z}{Hipwidth},\frac{A_x-C_x}{Hipwidth})---(3-21).$

本发明的特点是：

1、本发明采用先进体感设备和前沿机器人技术，设计了一种新的人机交互模式***。

2、本发明实现控制人形机器人实时模仿人体动作，具有很高的可靠性，很强的灵活性，在不同环境复杂条件下具有较好的鲁棒性。

3、本发明采用相对阈值距离滤波方法，降低了环境因素和传感器自身抖动造成的影响，有效地提高了数据的可靠性，***运行的稳定性。

4、本发明将骨骼追踪算法应用于人体姿态识别，针对NAO是拥有25个自由度的可编程仿人机器人的特点，实现控制人形机器人实时模仿人体动作，具有很高的可靠性，在不同环境复杂条件下具有较好的鲁棒性。本发明以Kinect骨骼框架追踪和机器人NAO硬件平台为基础。Kinect端，首选进行程序初始化，该过程包括硬件连接驱动检查、实例化传感器对象、获得深度权限、注册事件***。然后开始骨骼框架识别，该过程包括深度图像的获取，通过骨骼识别算法库深度图像识别出人体关节点并提取空间坐标，经过滤波、空间向量计算，为机器人NAO提供姿态控制数据。

附图说明

图1是本发明所述方法的流程图；

图2是如果PC端显示的人体骨骼框架与目标人体的动作不匹配，则重新初始化程序的流程图；

图3是人体骨骼框架识别流程图；

图4是人体骨骼框架动态显示在屏幕的示意图；

图5是本发明所述相对阈值距离比较滤波算法的流程图；

图6是Kinect空间坐标系；

图7为关节角度：LeftElbowRoll的计算示意图；

图8为关节角度：LeftElbowYaw的计算示意图；

图9为关节角度：LeftShoulder.Roll的计算示意图；

图10为关节角度：LeftShoulderPitch的计算示意图；

图11为手部张合角度：LeftHand的示意图；

图12为关节角度：HeadPitch的计算示意图；

图13为关节角：HeadYaw的计算示意图；

图14为将骨骼识别结果的Hipcenter点垂直投影到地面，作为新的坐标系原点，取左右脚踝点Rightankle和Leftankle在新坐标系中坐标作为机器人的控制数据的计算示意图。

具体实施方式

下面结合实施例和说明书附图对本发明做详细的说明，但不限于此。

如图1-5所示。

实施例、

一种动作捕捉机器人协同柔性姿态控制的方法，包括步骤如下：

(1)利用体感设备Kinect实时捕获不同时刻目标人体对应目标关节位移数据；目标人体站在体感设备Kinect视距范围内，为保证最佳识别效果，目标人体应处在体感设备Kinect的镜头前1.2-3.5m，水平视角±57°以内；所述体感设备Kinect具备彩色和深度感应镜头，水平视野：57度，垂直视野：43度，深度感应器范围1.2m-3.5m。运行PC端的服务程序，见图2，首先对体感设备Kinect进行初始化，该过程包括硬件连接驱动检查、实例化传感器对象、获得深度权限、注册事件等等；

(2)将体感设备Kinect捕获到的目标关节位移数据发送到PC端；目标人体站在体感设备Kinect深度感应镜头前，可以改变头部、手臂、手指等姿态，由体感设备Kinect捕获所有目标关节位移数据；

(3)PC端接收所有目标关节位移数据，并根据其实时绘制出人体骨骼框架；绘制人体骨骼框架的流程如图3；

(4)PC端将人体骨骼框架动态显示在屏幕上，并提供报错反馈；如图4，给出了四张具有代表性的服务程序运行状态图：标记为1的部分是目标人体站在视距范围内，程序初始化结束后显示的由体感设备Kinect首次捕获的数据构建的人体骨骼框架，代表程序启动正常，开始实时捕获目标人体的姿态变化；标记为2的部分是程序开始后某一时刻由体感设备Kinect捕获的数据构建的人体骨骼框架，该部分反映了这一时刻目标人体的姿态；标记为3的部分代表了由于程序运行报错、目标人体紧贴体感设备Kinect的镜头的原因，造成人体骨骼框架失真的情况；标记为4的部分是由于目标人体远离视距范围，导致体感设备Kinect无法捕获。标记为3、4的部分是程序给使用者的一个报错反馈，这时候需要重新初始化程序，以保证机器人收到可靠姿态控制数据；

(5)PC端对接收到的所有目标关节位移数据进行处理：包括相对阈值距离比较滤波计算、空间向量计算，得到人形机器人NAO的姿态控制数据，即关节角度数据；本发明由于环境因素和传感器自身抖动等原因，导致得到的原始数据里存在干扰数据，因此有必要对原始数据进行相对阈值距离比较滤波计算，使得机器人动作追踪更加准确、可靠；

(6)调用人形机器人NAO的NAOqi操作***中的JointControl函数，根据传送来的关节角度数据，对人形机器人NAO的舵机进行控制，使人形机器人NAO实时跟踪目标人体动作。

根据本发明优选的，所述步骤(4)还包括，所述PC端动态显示人体骨骼框架：如果PC端显示的人体骨骼框架与目标人体的动作一致，则将关节角度数据发送至人形机器人NAO；如果PC端显示的人体骨骼框架与目标人体的动作不匹配，则重新初始化程序，以保证人形机器人NAO收到可靠姿态控制数据。

根据本发明优选的，所述步骤(5)中，所述相对阈值距离比较滤波计算包括：

通过观察位移关节点的空间坐标变化，计算固定时间段(例如：0.1s-0.5s)内同一关节在开始时间和结束时间组成的波动向量，观察该波动向量的模以及在空间坐标系各个方向的波动，通过设定波动阈值筛选关节波动值。可以看出关节点识别位置的抖动主要是沿坐标轴方向的快速抖动，且出现抖动时波动向量的模会大幅增加。因此，应对波动较大的关节点做相应处理，对小幅度波动的关节点保持上一状态不变，对于不同的关节处使用不同的阈值比较，保证每次滤波后的结果都是最优解，以保证机器人姿态变换的连续性。

根据本发明优选的，所述步骤(5)中，所述空间向量计算包括：

根据本发明优选的，所述步骤(5)中，所述空间向量计算包括：

Kinect所使用的空间坐标系不同于常见的空间坐标系，其x轴与y轴的零点与传统空间坐标系相同，但其z轴坐标零点为Kinect传感器，正方向为Kinect指向的正前方。Kinect空间坐标系如图6所示：

由Kinect空间坐标系中，得出Kinect坐标系中任意两个不重合的坐标点P1(x1,y1,z1)，P2(x2,y2,z2)，对其组成的向量P1P2：

若存在另一点P3且该点不在P1P2所在的直线上，则存在以下关系式：

根据上述性质，将人体关节角度计算简化为对空间向量夹角的计算，下面将分别说明上肢关节角度的计算方法：

1)关节角度：LeftElbowRoll

如图7所示：计算LeftElbowRoll角度只需构造该空间夹角两边所在的一组向量，

并根据上文中提到的关节角度计算公式得：

$LeftElbowRoll=\∠ABC=arccos\frac{\stackrel{\→}{BA}\·\stackrel{\→}{BC}}{\left|\stackrel{\→}{BA}\right|\left|\stackrel{\→}{BC}\right|}---(3-3)$

2)关节角度：LeftElbowYaw

如图8所示：

LeftElbowYaw关节的角度为肘部绕上臂旋转时产生的角度，此角度通常情况下为ABC和BCD两相交平面的夹角，即图中平面S1和S2的夹角；根据空间平面夹角计算公式得出角度LeftElbowRoll的计算方法如下。

首先需要明确计算两非共线向量所在平面的法向量的公式：

$\stackrel{\→}{m}=\stackrel{\→}{a}\×\stackrel{\→}{b}=\det \left|\begin{array}{ccc}i& j& k\\ a_x& a_y& a_z\\ b_x& b_y& b_z\end{array}\right|---(3-4)$

因此S1、S2平面的法向量表示为：

$\begin{array}{c}\stackrel{\→}{M_1}=\stackrel{\→}{BA}\×\stackrel{\→}{BC}=\det \left|\begin{array}{ccc}\stackrel{\→}{i}& \stackrel{\→}{j}& \stackrel{\→}{k}\\ {\mathrm{BA}}_x& {\mathrm{BA}}_y& {\mathrm{BA}}_z\\ {\mathrm{BC}}_x& {\mathrm{BC}}_y& {\mathrm{BC}}_z\end{array}\right|\\ =({\mathrm{BA}}_y{\mathrm{BC}}_z-{\mathrm{BA}}_z{\mathrm{BC}}_y)\stackrel{\→}{i}+({\mathrm{BA}}_z{\mathrm{BC}}_x-{\mathrm{BA}}_x{\mathrm{BC}}_z)\stackrel{\→}{j}\\ +({\mathrm{BA}}_x{\mathrm{BC}}_y-{\mathrm{BA}}_y{\mathrm{BC}}_x)\stackrel{\→}{k}\end{array}---(3-5)$

$\begin{array}{c}\stackrel{\→}{M_2}=\stackrel{\→}{CB}\×\stackrel{\→}{CD}=\det \left|\begin{array}{ccc}\stackrel{\→}{i}& \stackrel{\→}{j}& \stackrel{\→}{k}\\ {\mathrm{CB}}_x& {\mathrm{CB}}_y& {\mathrm{CB}}_z\\ {\mathrm{CD}}_x& {\mathrm{CD}}_y& {\mathrm{CD}}_z\end{array}\right|\\ =({\mathrm{CB}}_y{\mathrm{CD}}_z-{\mathrm{CB}}_z{\mathrm{CD}}_y)\stackrel{\→}{i}+({\mathrm{CB}}_z{\mathrm{CD}}_x-{\mathrm{CB}}_x{\mathrm{CD}}_z)\stackrel{\→}{j}\\ +({\mathrm{CB}}_x{\mathrm{CD}}_y-{\mathrm{CB}}_y{\mathrm{CD}}_x)\stackrel{\→}{k}\end{array}$

LeftElbowYaw关节的角度即等于M1、M2两法向量的夹角：

$LeftElbowYaw=arccos\frac{\stackrel{\→}{M1}\·\stackrel{\→}{M2}}{\left|\stackrel{\→}{M_1}\right|\left|\stackrel{\→}{M_2}\right|}---(3-7)$

3)关节角度：LeftShoulder.Roll

如图9所示：

$LeftShoulderRoll=arccos\frac{\stackrel{\→}{BA}\·\stackrel{\→}{BC}}{\left|\stackrel{\→}{BA}\right|\left|\stackrel{\→}{BC}\right|}---(3-8)$

4)关节角度：LeftShoulderPitch

如图10所示：

LeftShoulderPitch关节的角度为上臂与双肩轴线组成的平面与双肩轴线与脊柱点组成的平面之间的夹角，此角度通常情况下为ABC和BCD两相交平面的夹角，类比LeftElbowYaw关节角的计算方法，根据空间平面夹角计算公式可得出角度LeftShoulderPitch的计算方法如下：

平面ABC的法向量：

平面BCD的法向量：

关节角LeftShoulderPitch计算：

5)手部张合角度：LeftHand

由于KINECT对于手部信息的读取无法精确到手指的状态，因此对于手部张合角度计算，通过计算Lefthand和LeftWrist之间的距离来估算手部张合的角度，计算方法如下：

如图11所示：

$LeftHaud=\left|\stackrel{\→}{AB}\right|=\sqrt{{(A_x-B_x)}^2+{(A_y-B_y)}^2+{(A_z-B_z)}^2}---(3-12)$

至此，左臂所有关节的角度全部计算完毕，同理可以计算右臂关节角度；

6)关节角度：HeadPitch

头部两个关节中，与低头和抬头有关的角度名称HeadPitch

如图12所示：

头部关节HeadPitch的角度选取肩中指向头部的向量与两肩与脊柱组成的平面的法向量之间的夹角，具体计算公式如下：

7)关节角：HeadYaw

如图13所示：

头部另外一个重要的关节角HeadYaw负责实现头部左右转动，由于KINECT骨骼识别结果中无法给出面部朝向的数据信息，因此HeadYaw角度无法进行直接计算，然而通过观察骨骼绘制动画可以看出，头部的空间位置明显位于肩中的前方(即正面面对KINECT时，Head点和Shouldercenter点在z轴上存在一定距离)，因此关于此关节角本文将转化为平面夹角的计算：

平面ABC的法向量

平面BCD法向量

关节角度

在机器人的运动过程中下肢关节活动将直接影响到机器人整体的平稳性，为了简化控制难度，本文对于下肢控制采用相对位置法，通过计算下肢末端在相对坐标系中的位置结合人体质心的高度，实现对机器人动作的控制；

如图14所示：

将骨骼识别结果的Hipcenter点垂直投影到地面，作为新的坐标系原点，取左右脚踝点Rightankle和Leftankle在新坐标系中坐标作为机器人的控制数据。

B、C两点在O坐标系中坐标如下，

LeftFoot＝(A_z-B_z,A_x-B_x)(3-17)

RightFoot＝(A_z-C_z,A_x-C_x)(3-18)

为了由于不同的人身高差异造成的绝对距离误差，此处将坐标数值除以人体胯宽，其中胯宽计算公式如下：

$Hipwidth=\left|\stackrel{\→}{BC}\right|=\sqrt{{(A_x-C_x)}^2+{(B_y-C_y)}^2+{(B_z-C_z)}^2}---(3-19)$

因此下肢末端在新坐标系中的位置如下所示:

$LeftFoot=(\frac{A_z-B_z}{Hipwidth},\frac{A_x-B_x}{Hipwidth})---(3-20)$

$RightFoot=(\frac{A_z-C_z}{Hipwidth},\frac{A_x-C_x}{Hipwidth})---(3-21).$

Claims (4)

1.一种动作捕捉机器人协同柔性姿态控制的方法，其特征在于该方法包括步骤如下：

(1)利用体感设备Kinect实时捕获不同时刻目标人体对应目标关节位移数据；

(2)将体感设备Kinect捕获到的目标关节位移数据发送到PC端；

(3)PC端接收所有目标关节位移数据，并根据其实时绘制出人体骨骼框架；

(4)PC端将人体骨骼框架动态显示在屏幕上，并提供报错反馈；

(5)PC端对接收到的所有目标关节位移数据进行处理：包括相对阈值距离比较滤波计算、空间向量计算，得到人形机器人NAO的姿态控制数据，即关节角度数据；

(6)调用人形机器人NAO的NAOqi操作***中的JointControl函数，根据传送来的关节角度数据，对人形机器人NAO的舵机进行控制，使人形机器人NAO实时跟踪目标人体动作。

2.根据权利要求1所述的一种动作捕捉机器人协同柔性姿态控制的方法，其特征在于，所述步骤(4)还包括，所述PC端动态显示人体骨骼框架：如果PC端显示的人体骨骼框架与目标人体的动作一致，则将关节角度数据发送至人形机器人NAO；如果PC端显示的人体骨骼框架与目标人体的动作不匹配，则重新初始化程序。

3.根据权利要求1所述的一种动作捕捉机器人协同柔性姿态控制的方法，其特征在于，所述步骤(5)中，所述相对阈值距离比较滤波计算包括：

通过观察位移关节点的空间坐标变化，计算固定时间段内同一关节在开始时间和结束时间组成的波动向量，观察该波动向量的模以及在空间坐标系各个方向的波动，通过设定波动阈值筛选关节波动值。

4.根据权利要求1所述的一种动作捕捉机器人协同柔性姿态控制的方法，其特征在于，所述步骤(5)中，所述空间向量计算包括：

由Kinect空间坐标系中，得出Kinect坐标系中任意两个不重合的坐标点P1(x1,y1,z1)，P2(x2,y2,z2)，对其组成的向量P1P2：

若存在另一点P3且该点不在P1P2所在的直线上，则存在以下关系式：

根据上述性质，将人体关节角度计算简化为对空间向量夹角的计算，下面将分别说明上肢关节角度的计算方法：

1)关节角度：LeftElbowRoll

计算LeftElbowRoll角度只需构造该空间夹角两边所在的一组向量，

并根据上文中提到的关节角度计算公式得：

$\mathrm{leftElbowRoll}=\∠\mathrm{ABC}=\arccos \frac{\stackrel{\→}{\mathrm{BA}}\·\stackrel{\→}{\mathrm{BC}}}{\left|\stackrel{\→}{\mathrm{BA}}\right|\left|\stackrel{\→}{\mathrm{BC}}\right|}---(3-3)$

2)关节角度：LeftElbowYaw

LeftElbowYaw关节的角度为肘部绕上臂旋转时产生的角度，此角度通常情况下为ABC和BCD两相交平面的夹角；根据空间平面夹角计算公式得出角度LeftElbowRoll的计算方法如下：

首先需要明确计算两非共线向量所在平面的法向量的公式：

$\stackrel{\→}{m}=\stackrel{\→}{a}\×\stackrel{\→}{b}=\det \left|\begin{array}{ccc}i& j& k\\ a_x& a_y& a_z\\ b_x& b_y& b_z\end{array}\right|---\left(3-4\right)$

因此S1、S2平面的法向量表示为：

$\begin{array}{c}\stackrel{\→}{M_1}=\stackrel{\→}{BA}\×\stackrel{\→}{BC}=\det \left|\begin{array}{ccc}\stackrel{\→}{i}& \stackrel{\→}{j}& \stackrel{\→}{k}\\ {\mathrm{BA}}_x& {\mathrm{BA}}_y& {\mathrm{BA}}_z\\ {\mathrm{BC}}_x& {\mathrm{BC}}_y& {\mathrm{BC}}_z\end{array}\right|\\ =\left({\mathrm{BA}}_y{\mathrm{BC}}_z-{\mathrm{BA}}_z{\mathrm{BC}}_y\right)\stackrel{\→}{i}+\left({\mathrm{BA}}_z{\mathrm{BC}}_x-{\mathrm{BA}}_x{\mathrm{BC}}_z\right)\stackrel{\→}{j}\\ +\left({\mathrm{BA}}_x{\mathrm{BC}}_y-{\mathrm{BA}}_y{\mathrm{BC}}_x\right)\stackrel{\→}{k}\end{array}---\left(3-5\right)$

$\begin{array}{c}\stackrel{\→}{M_2}=\stackrel{\→}{CB}\×\stackrel{\→}{CD}=\det \left|\begin{array}{ccc}\stackrel{\→}{i}& \stackrel{\→}{j}& \stackrel{\→}{k}\\ {\mathrm{CB}}_x& {\mathrm{CB}}_y& {\mathrm{CB}}_z\\ {\mathrm{CD}}_x& {\mathrm{CD}}_y& {\mathrm{CD}}_z\end{array}\right|\\ =\left({\mathrm{CB}}_y{\mathrm{CD}}_z-{\mathrm{CB}}_z{\mathrm{CD}}_y\right)\stackrel{\→}{i}+\left({\mathrm{CB}}_z{\mathrm{CD}}_x-{\mathrm{CB}}_x{\mathrm{CD}}_z\right)\stackrel{\→}{j}\\ +\left({\mathrm{CB}}_x{\mathrm{CD}}_y-{\mathrm{CB}}_y{\mathrm{CD}}_x\right)\stackrel{\→}{k}\end{array}$

LeftElbowYaw关节的角度即等于M1、M2两法向量的夹角：

$LeftElbowYaw=arccos\frac{\stackrel{\→}{M1}\·\stackrel{\→}{M2}}{\left|\stackrel{\→}{M_1}\right|\left|\stackrel{\→}{M_2}\right|}---\left(3-7\right)$

3)关节角度：LeftShoulder.Roll

$LeftShoulderRoll=arccos\frac{\stackrel{\→}{BA}\·\stackrel{\→}{BC}}{\left|\stackrel{\→}{BA}\right|\left|\stackrel{\→}{BC}\right|}---\left(3-8\right)$

4)关节角度：LeftShoulderPitch

LeftShoulderPitch关节的角度为上臂与双肩轴线组成的平面与双肩轴线与脊柱点组成的平面之间的夹角，此角度通常情况下为ABC和BCD两相交平面的夹角，类比LeftElbowYaw关节角的计算方法，根据空间平面夹角计算公式可得出角度LeftShoulderPitch的计算方法如下：

平面ABC的法向量：

平面BCD的法向量：

关节角LeftShoulderPitch计算：

5)手部张合角度：LeftHand

由于KINECT对于手部信息的读取无法精确到手指的状态，因此对于手部张合角度计算，通过计算Lefthand和LeftWrist之间的距离来估算手部张合的角度，计算方法如下：

$LeftHaud=\left|\stackrel{\→}{AB}\right|=\sqrt{{(A_x-B_x)}^2+{(A_y-B_y)}^2+{(A_z-B_z)}^2}---\left(3-12\right)$

至此，左臂所有关节的角度全部计算完毕，同理可以计算右臂关节角度；

6)关节角度：HeadPitch

头部两个关节中，与低头和抬头有关的角度名称HeadPitch

头部关节HeadPitch的角度选取肩中指向头部的向量与两肩与脊柱组成的平面的法向量之间的夹角，具体计算公式如下：

7)关节角：HeadYaw

头部另外一个重要的关节角HeadYaw负责实现头部左右转动，因此关于此关节角本文将转化为平面夹角的计算：

平面ABC的法向量

平面BCD法向量

关节角度

在机器人的运动过程中下肢关节活动将直接影响到机器人整体的平稳性，为了简化控制难度，本文对于下肢控制采用相对位置法，通过计算下肢末端在相对坐标系中的位置结合人体质心的高度，实现对机器人动作的控制；

将骨骼识别结果的Hipcenter点垂直投影到地面，作为新的坐标系原点，取左右脚踝点Rightankle和Leftankle在新坐标系中坐标作为机器人的控制数据；

B、C两点在O坐标系中坐标如下，

LeftFoot＝(A_z-B_z,A_x-B_x)(3-17)

RightFoot＝(A_z-C_z,A_x-C_x)(3-18)

为了由于不同的人身高差异造成的绝对距离误差，此处将坐标数值除以人体胯宽，其中胯宽计算公式如下：

$Hipwidth=\left|\stackrel{\→}{BC}\right|=\sqrt{{(B_x-C_x)}^2+{(B_y-C_y)}^2+{(B_z-C_z)}^2}---\left(3-19\right)$

因此下肢末端在新坐标系中的位置如下所示:

$LeftFoot=\left(\frac{A_z-B_z}{Hipwidth},\frac{A_x-B_x}{Hipwidth}\right)---\left(3-20\right)$

$RightFoot=\left(\frac{A_z-C_z}{Hipwidth},\frac{A_x-C_x}{Hipwidth}\right)---\left(3-21\right).$