0137]
[0138] 4)关节角度：LeftShoulderPitch
[0139] 如图10所示：
[0140] LeftShoulderPitch关节的角度为上臂与双肩轴线组成的平面与双肩轴线与脊 柱点组成的平面之间的夹角，此角度通常情况下为ABC和BCD两相交平面的夹角，类比Left ElbowYaw关节角的计算方法，根据空间平面夹角计算公式可得出角度LeftShoulder Pitch的计算方法如下：
[0141] 平面ABC的法向量：
[0142]
[0143] 平面B⑶的法向量：
[0144]
[0145]关节角LeftShoulderPitch计算：
[0146]
[0147] 5)手部张合角度：LeftHand
[0148] 由于KINECT对于手部信息的读取无法精确到手指的状态，因此对于手部张合角 度计算，通过计算Lefthand和LeftWrist之间的距离来估算手部张合的角度，计算方法 如下：
[0149] 如图11所示：
[0150]
[0151] 至此，左臂所有关节的角度全部计算完毕，同理可以计算右臂关节角度；
[0152] 6)关节角度：HeadPitch
[0153] 头部两个关节中，与低头和抬头有关的角度名称HeadPitch
[0154] 如图12所示：
[0155] 头部关节HeadPitch的角度选取肩中指向头部的向量与两肩与脊柱组成的平面 的法向量之间的夹角，具体计算公式如下：
[0156]
[0157]
[0158] 7)关节角：HeadYaw
[0159] 如图13所示：
[0160] 头部另外一个重要的关节角HeadYaw负责实现头部左右转动，由于KINECT骨骼 识别结果中无法给出面部朝向的数据信息，因此HeadYaw角度无法进行直接计算，然而通 过观察骨骼绘制动画可以看出，头部的空间位置明显位于肩中的前方（即正面面对KINECT 时，Head点和Shouldercenter点在z轴上存在一定距离），因此关于此关节角本文将转化 为平面夹角的计算：
[0161] 平面ABC的法向量
[0162] ' '-r '
i
[0163] 平面B⑶法向量
[0164]
[0165] 关节角度
[0166]
[0167] 在机器人的运动过程中下肢关节活动将直接影响到机器人整体的平稳性，为了简 化控制难度，本文对于下肢控制采用相对位置法，通过计算下肢末端在相对坐标系中的位 置结合人体质心的高度，实现对机器人动作的控制；
[0168] 如图14所示：
[0169] 将骨骼识别结果的Hipcenter点垂直投影到地面，作为新的坐标系原点，取左右 脚踝点Rightankle和Leftankle在新坐标系中坐标作为机器人的控制数据。
[0170] B、C两点在0坐标系中坐标如下，
[0171] LeftFoot= (Az-Bz,Ax-Bx) (3-17)
[0172] RightFoot= (AZ_CZ,AX_CX) (3-18)
[0173] 为了由于不同的人身高差异造成的绝对距离误差，此处将坐标数值除以人体胯 宽，其中胯宽计算公式如下：
[0174]
[0175] 因此下肢末端在新坐标系中的位置如下所示：
[0176]
[0177]
【主权项】
1. 一种动作捕捉机器人协同柔性姿态控制的方法，其特征在于该方法包括步骤如下： (1) 利用体感设备Kinect实时捕获不同时刻目标人体对应目标关节位移数据； (2) 将体感设备Kinect捕获到的目标关节位移数据发送到PC端； (3) PC端接收所有目标关节位移数据，并根据其实时绘制出人体骨骼框架； (4) PC端将人体骨骼框架动态显示在屏幕上，并提供报错反馈； (5)PC端对接收到的所有目标关节位移数据进行处理：包括相对阈值距离比较滤波计 算、空间向量计算，得到人形机器人ΝΑΟ的姿态控制数据，即关节角度数据； (6) 调用人形机器人ΝΑΟ的NAOqi操作系统中的JointControl函数，根据传送来的 关节角度数据，对人形机器人ΝΑΟ的舵机进行控制，使人形机器人ΝΑΟ实时跟踪目标人体动 作。2. 根据权利要求1所述的一种动作捕捉机器人协同柔性姿态控制的方法，其特征在 于，所述步骤（4)还包括，所述PC端动态显示人体骨骼框架：如果PC端显示的人体骨骼框 架与目标人体的动作一致，则将关节角度数据发送至人形机器人ΝΑΟ;如果PC端显示的人 体骨骼框架与目标人体的动作不匹配，则重新初始化程序。3. 根据权利要求1所述的一种动作捕捉机器人协同柔性姿态控制的方法，其特征在 于，所述步骤（5)中，所述相对阈值距离比较滤波计算包括： 通过观察位移关节点的空间坐标变化，计算固定时间段内同一关节在开始时间和结束 时间组成的波动向量，观察该波动向量的模以及在空间坐标系各个方向的波动，通过设定 波动阈值筛选关节波动值。4. 根据权利要求1所述的一种动作捕捉机器人协同柔性姿态控制的方法，其特征在 于，所述步骤（5)中，所述空间向量计算包括： 由Kinect空间坐标系中，得出Kinect坐标系中任意两个不重合的坐标点PI(xl，yl，zl)，P2 (x2,y2,z2)，对其组成的向量P1P2 : 户1芎=(无一私为一H一及）（3-1): 若存在另一点P3且该点不在P1P2所在的直线上，则存在以下关系式：根据上述性质，将人体关节角度计算简化为对空间向量夹角的计算，下面将分别说明 上肢关节角度的计算方法： 1) 关节角度：LeftElbowRoll 计算LeftElbowRoll角度只需构造该空间夹角两边所在的一组向量， 并根据上文中提到的关节角度计算公式得：2) 关节角度：LeftElbowYaw LeftElbowYaw关节的角度为肘部绕上臂旋转时产生的角度，此角度通常情况下为ABC和BCD两相交平面的夹角；根据空间平面夹角计算公式得出角度LeftElbowRoll的 计算方法如下： 首先需要明确计算两非共线向量所在平面的法向量的公式：因此Sl、S2平面的法向量表示为：LeftElbowYaw关节的角度即等于Ml、M2两法向量的夹角：3) 关节角度：LeftShoulder.Roll4) 关节角度：LeftShoulderPitch LeftShoulderPitch关节的角度为上臂与双肩轴线组成的平面与双肩轴线与脊柱 点组成的平面之间的夹角，此角度通常情况下为ABC和BCD两相交平面的夹角，类比Left ElbowYaw关节角的计算方法，根据空间平面夹角计算公式可得出角度LeftShoulder Pitch的计算方法如下： 平面ABC的法向量： 平面B⑶的法向量：关节角Left Shoulder Pitch计算：5)手部张合角度：Left Hand 由于KINECT对于手部信息的读取无法精确到手指的状态，因此对于手部张合角度计 算，通过计算Left hand和Left Wrist之间的距离来估算手部张合的角度，计算方法如下：至此，左臂所有关节的角度全部计算完毕，同理可以计算右臂关节角度； 6)关节角度：Head Pitch 头部两个关节中，与低头和抬头有关的角度名称Head Pitch 头部关节HeadPitch的角度选取肩中指向头部的向量与两肩与脊柱组成的平面的法 向量之间的夹角，具体计算公式如下：7)关节角：Head Yaw 头部另外一个重要的关节角Head Yaw负责实现头部左右转动，因此关于此关节角本文 将转化为平面夹角的计算： 平面ABC的法向量 平面BCD法向量关节角度在机器人的运动过程中下肢关节活动将直接影响到机器人整体的平稳性，为了简化控 制难度，本文对于下肢控制采用相对位置法，通过计算下肢末端在相对坐标系中的位置结 合人体质心的高度，实现对机器人动作的控制； 将骨骼识别结果的Hipcenter点垂直投影到地面，作为新的坐标系原点，取左右脚踝 点Rightankle和Leftankle在新坐标系中坐标作为机器人的控制数据； B、C两点在0坐标系中坐标如下， LeftFoot= (Az-Bz,Ax-Bx) (3-17) RightFoot= (Az-Cz,Ax-Cx) (3-18) 为了由于不同的人身高差异造成的绝对距离误差，此处将坐标数值除以人体胯宽，其 中胯宽计算公式如下：因此下肢末端在新坐标系中的位置如下所示：
【专利摘要】一种动作捕捉机器人协同柔性姿态控制的方法，利用体感设备Kinect实时捕获不同时刻目标人体对应目标关节位移数据；将目标关节位移数据发送到PC端；PC端实时绘制出人体骨骼框架；PC端将人体骨骼框架动态显示在屏幕上，并提供报错反馈；得到关节角度数据；对人形机器人NAO的舵机进行控制，使人形机器人NAO实时跟踪目标人体动作。本发明利用Kinect捕获目标人体的位移关节数据，识别人体的各种姿态，实现机器人实时模仿人体动作。本发明人体动作识别速度快、精度高，机器人追踪动作流畅，且可以实时反映出目标人体的姿态，在不同环境复杂条件下具有较好的鲁棒性。
【IPC分类】B25J9/16
【公开号】CN105252532
【申请号】CN201510824988
【发明人】邢建平, 孟宪昊, 王康, 孟宪鹏
【申请人】山东大学
【公开日】2016年1月20日
【申请日】2015年11月24日