CN106971050B - 一种基于Kinect的Darwin机器人关节映射解析方法 - Google Patents

Abstract

本发明公开了一种基于Kinect的Darwin机器人关节映射解析方法，该解析方法包括以下步骤：1)获取人体骨骼信息；2)建立参考坐标系并构建骨架矢量；3)欧拉旋转分析；4)重心稳定分析；5)关节角度计算。该方法利用Kinect获取人体的15个关节点，通过补偿与滑动均值滤波后，在参考坐标系中构建10个单位关节矢量，并根据Darwin机器人关节级联方式进行欧拉旋转分析，得到Kinect与Darwin机器人共同关节的相应角度，最后对Darwin机器人脚部扩展关节进行重心稳定分析，得到Darwin机器人单、双脚稳定站立的脚关节角度，从而实现Kinect到Darwin机器人的动作模仿。

Description

一种基于Kinect的Darwin机器人关节映射解析方法

技术领域

本发明涉及机器人仿真技术领域，具体涉及一种基于Kinect的Darwin机器人关节映射解析方法。

背景技术

仿人机器人是机器人领域的重要研究方向之一，体态感知作为一种自然的人机交互形式，在交互过程中能够很好的识别与传递人的意图，并携带有极为丰富的信息量。当前机器人技术的研究正朝着智能化和多样化的方向发展，为符合各种复杂、恶劣环境的特殊要求，需要机器人具有更高的适应性以及具备一定的学习能力，需要机器人能自主移动到指定的地方，在工业、医疗、航空、国防领域及危险场合等进行工作。而传统机器人往往只针对特定的任务，且需要大量人为编程，费时费力，难以满足灵活性的要求。为此人们正在积极探索机器人模仿学习的行为控制，将开发人员从复杂的编程工作中解放出来。由于人体关节自由度和仿人机器人的自由度不同，以及仿人机器人的物理约束等问题，限制了仿人机器人直接模仿人体行为。而目前大部分的研究工作主要集中在上半身的动作模仿，采用的关节解析方法也主要针对上半身，目前亟待提出一种关节解析方法能实现将人体关节信息映射到Darwin机器人上，以解决机器人自由度约束等情况的影响，并能保证在模仿过程中机器人全身的重心稳定。

发明内容

本发明的目的是为了解决现有技术中的上述缺陷，提供一种基于Kinect的Darwin机器人关节映射解析方法，该解析方法首先利用Kinect采集到的人体关节骨骼数据，然后将Kinect人体关节骨骼数据通过关节解析后映射到Darwin机器人的关节，并使其在最大程度上实现了Darwin机器人模仿人体动作。

本发明的目的可以通过采取如下技术方案达到：

一种基于Kinect的Darwin机器人关节映射解析方法，所述方法包括下列步骤：

S1、利用Kinect采集人体关节点P的骨骼数据；

S2、建立基于人体的参考坐标系，并将人体关节点P的骨骼数据映射到该坐标系上，通过同一骨架的两个关节点可以得到Darwin机器人的单位关节矢量V；

S3、通过欧拉旋转分析，将关节矢量V解析得到Kinect与Darwin机器人共同关节的相应转动角度θ；

S4、对Darwin机器人脚部扩展关节进行重心稳定分析，得到Darwin机器人单、双脚稳定站立的脚关节角度，对舵机的转动角度θ进行计算和校准。

进一步地，所述人体关节点P的骨骼数据包括：头部、颈部、躯干、左肩、左肘、左手、右肩、右肘、右手、左臀、左膝、左脚、右臀、右膝、右脚，用P₁-P₁₅表示；

所述单位关节矢量V包括：头部矢量、颈部矢量、左上臂矢量、左下臂矢量、右上臂矢量、右下臂矢量、左大腿矢量、左小腿矢量、右大腿矢量、右小腿矢量，用V₁-V₁₀表示。

进一步地，定义机器人初始状态为人体的站立姿态，则所有关节矢量的初始坐标可表示[0 -1 0]^T，根据欧拉旋转关系可以得到变换阵R_e，可以得到关节矢量V＝R_e[0 -1 0]^T，其中R_e与旋转顺序、次数和旋转轴有关，有一到多个旋转算子组成，旋转算子R_X(θ)、R_Y(θ)、R_Z(θ)分别表示绕坐标系X、Y、Z轴旋转θ角的变换阵，有：

θ角的ID号为i的舵机转过的对应角度为θ_i，第i个关节矢量V_i的各轴分量分别表示为V_Xi、V_Yi、V_Zi；

Darwin机器人头部有2个舵机，ID为19和20，ID19舵机级联ID20舵机，ID19控制Y转向，ID20控制X转向；

Darwin机器人左右手各有3个舵机，ID为1-6，其中，关于右手，ID1舵机级联ID3舵机，再级联ID5舵机，ID1控制右手X转向，ID3控制右手Y转向，ID5控制右手Z转向，关于左手，ID2舵机级联ID4舵机，再级联ID6舵机，ID2控制左手X转向，ID4控制左手Y转向，ID6控制左手Z转向；

Darwin机器人左右脚各有6个舵机，ID为7-18，关于右脚，ID7舵机级联ID9舵机，再级联ID11舵机，再级联ID13舵机，再级联ID15舵机，再级联ID17舵机，ID7控制右脚Y转向，ID9控制右脚Z转向，ID11控制右脚X转向，ID13控制右脚X转向，ID15控制右脚X转向，ID17控制右脚Z转向；关于左脚，ID8舵机级联ID10舵机，再级联ID12舵机，再级联ID14舵机，再级联ID16舵机，再级联ID18舵机，ID8控制左脚Y转向，ID10控制左脚Z转向，ID12控制左脚X转向，ID14控制左脚X转向，ID16控制左脚X转向，ID18控制左脚Z转向。

进一步地，θ₁、θ₂的取值范围(-π π)，其他角度取值范围

另外，θ₃、θ₅、θ₆、θ₉、θ₁₀只能取正，θ₄、θ₁₃、θ₁₄、θ₂₀只能取负。

进一步地，所述步骤S4具体过程如下：

定义

R₁＝R_X(θ₁),R₂＝R_X(θ₂),R₃＝R₁R_Z(θ₃),R₄＝R₂R_X(θ₄),

R₅＝R₃R_Z(θ₅),R₆＝R₄R_X(θ₆),

R₇＝R_Y(θ₇),R₈＝R_Y(θ₈),R₉＝R₇R_Z(θ₉),R₁₀＝R₈R_Z(θ₁₀),

R₁₁＝R₉R_X(θ₁₁),R₁₂＝R₁₀R_X(θ₁₂),

R₁₃＝R₁₁R_X(θ₁₃),R₁₄＝R₁₂R_X(θ₁₄),R₁₅＝R₁₃R_X(θ₁₅),R₁₆＝R₁₄R_X(θ₁₆),

R₁₇＝R₁₅R_Y(θ₁₇),R₁₈＝R₁₆R_Y(θ₁₈),R₁₉＝R_Y(θ₁₉),R₂₀＝R₁₉R_X(θ₂₀)

P_O1＝P_off1,P_O2＝P_off2,P_O3＝P_O1+R₁P_off3,P_O4＝P_O2+R₂P_off4,

P_O5＝P_O3+R₃P_off5,P_O6＝P_O4+R₄P_off6,P_O7＝P_off7,P_O8＝P_off8,

P_O9＝P_O7+R₇P_off9,P_O10＝P_O8+R₈P_off10,P_O11＝P_O9+R₉P_off11,P_O12＝P_O10+R₁₀P_off12,

P_O13＝P_O11+R₁₁P_off13,P_O14＝P_O12+R₁₂P_off14,P_O15＝P_O13+R₁₃P_off15,P_O16＝P_O14+R₁₄P_off16,P_O17＝P_O15+R₁₅P_off17,P_O18＝P_O16+R₁₆P_off18,P_O19＝P_off19,P_O20＝P_O19+R₁₉P_off20

右手重心位置：

左手重心位置：

右脚重心位置：

左脚重心位置：

头部重心位置：

整体重心位置：

同上，可计算右脚平面和左脚平面中心点坐标分别为：

P_A＝P_O17+R₁₇[10 -33.5 0]^T,P_B＝P_O18+R₁₈[-10 -33.5 0]^T

根据双足站立稳定条件可以得到脚平面法向量：

此时拓展关节矢量V₁₀＝V₁₁＝F，则机器人能够稳定站立，由旋转关系有：V₁₀＝R_X(θ₁₅)R_Z(θ₁₇)V₇,V₁₁＝R_X(θ₁₆)R_Z(θ₁₈)V₉，可得结果：

若机器人为双脚站立，如果重心G投影点C不在两脚平面中心点连线上，则机器人将左右不平衡，需要修正θ₉、θ₁₀，修正方法为加入偏移量±

通过递归方法直到找到稳定点；

若机器人为单脚站立，则重心投影应落在该脚平面的中心点上，此时机器人能稳定站立，以右脚站立时，有：F＝P_B-P_G，解析过程相同，以左脚站立时，有F＝P_B-P_G。

进一步地，所述舵机的转动角度θ的解析关系如下：

θ₁＝atan 2(sinθ₅V_Y3-cosθ₃cosθ₅V_Z3,-sinθ₅V_Z3-cosθ₃cosθ₅V_Y3)，

θ₂＝atan 2(sinθ₆V_Y5-cosθ₄cosθ₆V_Z5,-sinθ₆V_Z5-cosθ₄cosθ₆V_Y5)，

θ₃＝arcsin(V_X2)，

θ₄＝arcsin(V_X4)，

θ₅＝arccos(V₂·V₃)，

θ₆＝arccos(V₄·V₅)，

θ₇＝0，

θ₈＝0，

θ₁₁＝-arcsin(V_Z6)，

θ₁₂＝-arcsin(V_Z8)，

θ₁₃＝-arccos(V₆·V₇)，

θ₁₄＝-arccos(V₈·V₉)，

θ₂₀＝-arccos(V_Y1)。

本发明相对于现有技术具有如下的优点及效果：

采用统一的欧拉旋转分析方法进行解算，其欧拉旋转顺序与达尔文机器人的关节级联顺序相同，直观易于理解，其解析角度与达尔文机器人舵机角度基本相同，相较于传统的矢量计算方法，采用统一参考坐标系的欧拉角度计算方法思路更加清晰，拓展性更强，方便重心位置与末端位置的计算，该方法适用于头部、手部和脚部的关节解析，适用性强。该方案还采用重心稳定分析的方法对机器人双足站立进行了分析，能够保证机器人的稳定站立，实现全身的基本模仿。同时，在能判别单脚站立的情况下，该重心稳定方法也同样适用于单脚站立情况。

附图说明

图1是Kinect人体关节点信息图；

图2是Darwin机器人的关节矢量图；

图3是Kinect到Darwin机器人的关节映射解析方法流程图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例

本实施例公开了一种基于Kinect的Darwin机器人关节映射解析方法，该解析方法首先利用Kinect采集人体关节骨骼数据，然后将Kinect人体关节骨骼数据通过关节解析后映射到Darwin机器人的关节，并使其在最大程度上实现了Darwin机器人模仿人体动作。该方法具体包括下列步骤：

S1、利用Kinect采集人体关节点P的骨骼数据；

该步骤中利用OpenNI能够读取到Kinect的24个关节点信息，分别为：头部、颈部、躯干、腰、左领、左肩、左肘、左腕、左手、左指尖、右领、右肩、右肘、右腕、右手、右指尖、左臀、左膝、左踝、左脚、右臀、右膝、右踝、右脚，在实际应用中只有15个关节点数据有效，分别为：头部、颈部、躯干、左肩、左肘、左手、右肩、右肘、右手、左臀、左膝、左脚、右臀、右膝、右脚，用P₁-P₁₅表示。由于Kinect的深度图与彩色图配准的精度问题，对关节点数据进行初步补偿，并采用滑动均值滤波减缓关节点数据波动程度。

S2、建立基于人体的参考坐标系，并将人体关节点P的骨骼数据映射到该坐标系上，通过同一骨架的两个关节点可以得到Darwin机器人的单位关节矢量V，分别为：头部矢量、颈部矢量、左上臂矢量、左下臂矢量、右上臂矢量、右下臂矢量、左大腿矢量、左小腿矢量、右大腿矢量、右小腿矢量，用V₁-V₁₀表示。

以关节点躯干为坐标系原点，规定关节点左肩指向关节点右肩的方向为参考坐标系的X方向，关节点左肩、右肩与躯干平面法向量指向人体背面的方向为Z方向，则根据右手坐标系规则可确定Y方向。可以得到坐标系变换阵

以头部矢量为例，其相关的两个关节点为关节点头部与颈部，则

其他矢量计算相同。

S3、通过欧拉旋转分析，将关节矢量V解析得到Kinect与Darwin机器人共同关节的相应转动角度θ；

定义机器人初始状态为人体的站立姿态，则所有关节矢量的初始坐标可表示[0 -1 0]^T，根据欧拉旋转关系可以得到变换阵R_e，可以得到关节矢量V＝R_e[0 -1 0]^T，其中R_e与旋转顺序、次数和旋转轴有关，有一到多个旋转算子组成，旋转算子R_X(θ)、R_Y(θ)、R_Z(θ)分别表示绕坐标系X、Y、Z轴旋转θ角的变换阵，有：

θ角的取值范围为(-π π)，为表示方便，ID号为i的舵机转过的对应角度为θ_i，第i个关节矢量V_i的各轴分量分别表示为V_Xi、V_Yi、V_Zi。由于机器人机械限制，θ₁、θ₂可取(-π π)，其他角度只能取

另外，θ₃、θ₅、θ₆、θ₉、θ₁₀只能取正，θ₄、θ₁₃、θ₁₄、θ₂₀只能取负。

1)Darwin机器人头部有2个舵机，ID为19和20，ID19舵机级联ID20舵机，ID19控制Y转向，ID20控制X转向，则头部矢量V₁＝R_Y(θ₁₉)R_X(θ₂₀)[0 -1 0]^T，可以得到θ₂₀＝-arccos(V_Y1)、

2)Darwin机器人左右手各有3个舵机，ID为1-6，以右手为例，ID1舵机级联ID3舵机，再级联ID5舵机，ID1控制右手X转向，ID3控制右手Y转向，ID5控制Z转向，则右上臂矢量V₂＝R_X(θ₁)R_Y(θ₃)[0 -1 0]^T，可以得到θ₃＝arcsin(V_X2)，则右下臂矢量V₃＝R_X(θ₁)R_Y(θ₃)R_X(θ₅)[0 -1 0]^T，可以得到

考虑到θ₁取值范围为(-ππ)，修改为θ₁＝atan 2(sinθ₅V_Y3-cosθ₃cosθ₅V_Z3,-sinθ₅V_Z3-cosθ₃cosθ₅V_Y3)。而θ₅为V₂和V₃的夹角，所以θ₅＝arccos(V₂·V₃)。左手情况相同。

3)Darwin机器人左右脚各有6个舵机，ID为7-18，以右脚为例，ID7舵机级联ID9舵机，再级联ID11舵机，再级联ID13舵机，再级联ID15舵机，再级联ID17舵机，ID7控制Y转向，ID9控制Z转向，ID11控制X转向，ID13控制X转向，ID15控制X转向，ID17控制Z转向。为降低解析复杂度和消除多解情况，忽略ID7舵机的控制，则右大腿矢量V₆＝R_Z(θ₉)R_X(θ₁₁)[0 -1 0]^T，可以得到θ₁₁＝-arcsin(V_Z6)，

而θ₁₃为V₆和V₇的夹角，所以θ₁₃＝-arccos(V₆·V₇)。左脚情况相同。

S4、通过重心稳定分析，对舵机的转动角度θ进行计算和校准。

以上能实现机器人的动作模仿，但机器人不能稳定站立，通过扩展两个关节矢量，通过重心稳定方法控制脚踝部分的4个舵机，可以实现机器人的稳定站立。先不考虑地面不水平和单脚站立情况，为了实现机器人双脚稳定站立，则需要满足条件：机器人的两个脚平面与地面完全贴合；机器人的重心投影落在两脚平面中心点连线上。通过查阅资料与计算测量，可得到机器人各部件的重心信息，如表1所示。其中，重心位置为该关节重心距离旋转中心的距离，偏移位置为旋转中心距离上一个关节旋转中心的位置。

表1.Darwin机器人各部件重心信息表

根据空间坐标平移与旋转变换，可得到机器人整体的重心位置。定义

R₁＝R_X(θ₁),R₂＝R_X(θ₂),R₃＝R₁R_Z(θ₃),R₄＝R₂R_X(θ₄),

R₅＝R₃R_Z(θ₅),R₆＝R₄R_X(θ₆),

R₇＝R_Y(θ₇),R₈＝R_Y(θ₈),R₉＝R₇R_Z(θ₉),R₁₀＝R₈R_Z(θ₁₀),

R₁₁＝R₉R_X(θ₁₁),R₁₂＝R₁₀R_X(θ₁₂),

R₁₃＝R₁₁R_X(θ₁₃),R₁₄＝R₁₂R_X(θ₁₄),R₁₅＝R₁₃R_X(θ₁₅),R₁₆＝R₁₄R_X(θ₁₆),

R₁₇＝R₁₅R_Y(θ₁₇),R₁₈＝R₁₆R_Y(θ₁₈),R₁₉＝R_Y(θ₁₉),R₂₀＝R₁₉R_X(θ₂₀)

P_O1＝P_off1,P_O2＝P_off2,P_O3＝P_O1+R₁P_off3,P_O4＝P_O2+R₂P_off4,

P_O5＝P_O3+R₃P_off5,P_O6＝P_O4+R₄P_off6,P_O7＝P_off7,P_O8＝P_off8,

P_O9＝P_O7+R₇P_off9,P_O10＝P_O8+R₈P_off10,P_O11＝P_O9+R₉P_off11,P_O12＝P_O10+R₁₀P_off12,

P_O13＝P_O11+R₁₁P_off13,P_O14＝P_O12+R₁₂P_off14,P_O15＝P_O13+R₁₃P_off15,P_O16＝P_O14+R₁₄P_off16,P_O17＝P_O15+R₁₅P_off17,P_O18＝P_O16+R₁₆P_off18,P_O19＝P_off19,P_O20＝P_O19+R₁₉P_off20

右手重心位置：

左手重心位置：

右脚重心位置：

左脚重心位置：

头部重心位置：

整体重心位置：

同上，可计算右脚平面和左脚平面中心点坐标分别为：

P_A＝P_O17+R₁₇[10 -33.5 0]^T,P_B＝P_O18+R₁₈[-10 -33.5 0]^T

根据双足站立稳定条件可以得到脚平面法向量：

此时拓展关节矢量V₁₀＝V₁₁＝F，则机器人能够稳定站立，由旋转关系有：V₁₀＝R_X(θ₁₅)R_Z(θ₁₇)V₇,V₁₁＝R_X(θ₁₆)R_Z(θ₁₈)V₉，可得结果：

若重心G投影点C不在两脚平面中心点连线上，则机器人将左右不平衡，需要修正θ₉、θ₁₀，修正方法为加入偏移量±

通过递归方法直到找到稳定点。

若机器人为单脚站立，则重心投影应落在该脚平面的中心点上，此时机器人能稳定站立，以右脚站立为例，有：F＝P_B-P_G，解析过程相同。以左脚站立为例，有F＝P_B-P_G。

综上，可得到关节解析表，如下：

表2.关节解析表

综上所述，本发明公开的一种基于Kinect的Darwin机器人关节映射解析方法，利用Kinect获取人体的15个关节点，通过补偿与后，在参考坐标系中构建9个单位关节矢量，并根据Darwin机器人关节级联方式进行欧拉旋转分析，得到相应关节的角度，最后利用重心稳定分析对Darwin机器人的脚部扩展关节进行解析，从而实现Kinect到Darwin机器人的动作模仿。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (4)

1.一种基于Kinect的Darwin机器人关节映射解析方法，其特征在于，所述方法包括下列步骤：

S1、利用Kinect采集人体关节点P的骨骼数据，其中，所述人体关节点P的骨骼数据包括：头部、颈部、躯干、左肩、左肘、左手、右肩、右肘、右手、左臀、左膝、左脚、右臀、右膝、右脚，用P₁-P₁₅表示；

S2、建立基于人体的参考坐标系，并将人体关节点P的骨骼数据映射到该坐标系上，通过同一骨架的两个关节点可以得到Darwin机器人的单位关节矢量V，其中，所述单位关节矢量V包括：头部矢量、颈部矢量、左上臂矢量、左下臂矢量、右上臂矢量、右下臂矢量、左大腿矢量、左小腿矢量、右大腿矢量、右小腿矢量，用V₁-V₁₀表示；

S3、通过欧拉旋转分析，将关节矢量V解析得到Kinect与Darwin机器人共同关节的相应转动角度θ；

S4、对Darwin机器人脚部扩展关节进行重心稳定分析，得到Darwin机器人单、双脚稳定站立的脚关节角度，对舵机的转动角度θ进行计算和校准；

定义机器人初始状态为人体的站立姿态，则所有关节矢量的初始坐标可表示[0 -1 0]^T，根据欧拉旋转关系可以得到变换阵R_e，可以得到关节矢量V＝R_e[0 -1 0]^T，其中R_e与旋转顺序、次数和旋转轴有关，有一到多个旋转算子组成，旋转算子R_X(θ)、R_Y(θ)、R_Z(θ)分别表示绕坐标系X、Y、Z轴旋转θ角的变换阵，有：

θ角的ID号为i的舵机转过的对应角度为θ_i，第i个关节矢量V_i的各轴分量分别表示为V_Xi、V_Yi、V_Zi；

Darwin机器人头部有2个舵机，ID为19和20，ID19舵机级联ID20舵机，ID19控制Y转向，ID20控制X转向；

Darwin机器人左右手各有3个舵机，ID为1-6，其中，关于右手，ID1舵机级联ID3舵机，再级联ID5舵机，ID1控制右手X转向，ID3控制右手Y转向，ID5控制右手Z转向，关于左手，ID2舵机级联ID4舵机，再级联ID6舵机，ID2控制左手X转向，ID4控制左手Y转向，ID6控制左手Z转向；

Darwin机器人左右脚各有6个舵机，ID为7-18，关于右脚，ID7舵机级联ID9舵机，再级联ID11舵机，再级联ID13舵机，再级联ID15舵机，再级联ID17舵机，ID7控制右脚Y转向，ID9控制右脚Z转向，ID11控制右脚X转向，ID13控制右脚X转向，ID15控制右脚X转向，ID17控制右脚Z转向；关于左脚，ID8舵机级联ID10舵机，再级联ID12舵机，再级联ID14舵机，再级联ID16舵机，再级联ID18舵机，ID8控制左脚Y转向，ID10控制左脚Z转向，ID12控制左脚X转向，ID14控制左脚X转向，ID16控制左脚X转向，ID18控制左脚Z转向。

2.根据权利要求1所述的一种基于Kinect的Darwin机器人关节映射解析方法，其特征在于，

θ₁、θ₂的取值范围(-π π)，其他角度的取值范围

另外，θ₃、θ₅、θ₆、θ₉、θ₁₀只能取正，θ₄、θ₁₃、θ₁₄、θ₂₀只能取负。

3.根据权利要求1所述的一种基于Kinect的Darwin机器人关节映射解析方法，其特征在于，所述步骤S4具体过程如下：

定义

R₁＝R_X(θ₁),R₂＝R_X(θ₂),R₃＝R₁R_Z(θ₃),R₄＝R₂R_X(θ₄),

R₅＝R₃R_Z(θ₅),R₆＝R₄R_X(θ₆),

R₇＝R_Y(θ₇),R₈＝R_Y(θ₈),R₉＝R₇R_Z(θ₉),R₁₀＝R₈R_Z(θ₁₀),

R₁₁＝R₉R_X(θ₁₁),R₁₂＝R₁₀R_X(θ₁₂),

R₁₃＝R₁₁R_X(θ₁₃),R₁₄＝R₁₂R_X(θ₁₄),R₁₅＝R₁₃R_X(θ₁₅),R₁₆＝R₁₄R_X(θ₁₆),

R₁₇＝R₁₅R_Y(θ₁₇),R₁₈＝R₁₆R_Y(θ₁₈),R₁₉＝R_Y(θ₁₉),R₂₀＝R₁₉R_X(θ₂₀)

P_O1＝P_off1,P_O2＝P_off2,P_O3＝P_O1+R₁P_off3,P_O4＝P_O2+R₂P_off4,

P_O5＝P_O3+R₃P_off5,P_O6＝P_O4+R₄P_off6,P_O7＝P_off7,P_O8＝P_off8,

P_O9＝P_O7+R₇P_off9,P_O10＝P_O8+R₈P_off10,P_O11＝P_O9+R₉P_off11,P_O12＝P_O10+R₁₀P_off12,

P_O13＝P_O11+R₁₁P_off13,P_O14＝P_O12+R₁₂P_off14,P_O15＝P_O13+R₁₃P_off15,P_O16＝P_O14+R₁₄P_off16,

P_O17＝P_O15+R₁₅P_off17,P_O18＝P_O16+R₁₆P_off18,P_O19＝P_off19,P_O20＝P_O19+R₁₉P_off20

右手重心位置：

左手重心位置：

右脚重心位置：

左脚重心位置：

头部重心位置：

整体重心位置：

同上，可计算右脚平面和左脚平面中心点坐标分别为：

P_A＝P_O17+R₁₇[10 -33.5 0]^T,P_B＝P_O18+R₁₈[-10 -33.5 0]^T

根据双足站立稳定条件可以得到脚平面法向量：

此时拓展关节矢量V₁₀＝V₁₁＝F，则机器人能够稳定站立，由旋转关系有：V₁₀＝R_X(θ₁₅)R_Z(θ₁₇)V₇,V₁₁＝R_X(θ₁₆)R_Z(θ₁₈)V₉，可得结果：

若机器人为双脚站立，如果重心G投影点C不在两脚平面中心点连线上，则机器人将左右不平衡，需要修正θ₉、θ₁₀，修正方法为加入偏移量

通过递归方法直到找到稳定点；

若机器人为单脚站立，则重心投影应落在该脚平面的中心点上，此时机器人能稳定站立，以右脚站立时，有：F＝P_B-P_G，解析过程相同，以左脚站立时，有F＝P_B-P_G。

4.根据权利要求3所述的一种基于Kinect的Darwin机器人关节映射解析方法，其特征在于，所述舵机的转动角度θ的解析关系如下：

θ₁＝atan 2(sinθ₅V_Y3-cosθ₃cosθ₅V_Z3,-sinθ₅V_Z3-cosθ₃cosθ₅V_Y3)，

θ₂＝atan 2(sinθ₆V_Y5-cosθ₄cosθ₆V_Z5,-sinθ₆V_Z5-cosθ₄cosθ₆V_Y5)，

θ₃＝arcsin(V_X2)，

θ₄＝arcsin(V_X4)，

θ₅＝arccos(V₂·V₃)，

θ₆＝arccos(V₄·V₅)，

θ₇＝0，

θ₈＝0，

θ₁₁＝-arcsin(V_Z6)，

θ₁₂＝-arcsin(V_Z8)，

θ₁₃＝-arccos(V₆·V₇)，

θ₁₄＝-arccos(V₈·V₉)，

V_Z1≠0，

θ₂₀＝-arccos(V_Y1)。

Images