CN106607910A - 一种机器人实时模仿方法 - Google Patents

Abstract

本发明涉及一种机器人实时模仿方法，包括：获得人体动作的各个关节位置，利用所述人体动作的各关节位置计算出人体相邻关节间的方向向量；将所述人体相邻关节间的方向向量与机器人对应的相邻关节间的方向向量进行匹配，获得机器人需移动到的目标角。本技术方案通过应用运动学反解和LP优化方法，并用贝塞尔曲线拟合轨迹，将传感器检测的人体信息映射到机器人上并使其执行相似动作。同时还会考虑平衡问题以使得机器人能够模拟其他高难度动作。该方法操作简单、成本相对较低且稳定性较高。

Description

一种机器人实时模仿方法

技术领域

本发明涉及智能机器人技术领域，特别涉及一种机器人实时模仿方法。

背景技术

目前，人形机器人凭借其拟人的外形以及强大的可编程能力正越来越受到大众的关注，法国Aldebaran-Robotics公司出产的NAO机器人正是其中的一种。

NAO机器人有着与人相似的外形，全身总共有25个自由度。其中，NAO机器人的每个手臂有六个自由度，两条腿共有十一个自由度，头部有两个自由度。这种机器人由于其可爱的外形，正越来越受到大众的欢迎。

目前，基于人形机器人的模仿系统不是特别健全，大都停留在给机器人硬编码的程度。现有方法为：首先设定一些机器人运动的姿态，然后将相应的关节角保存起来，每当机器人需要展示的时候调用机器人执行这些关节角。这种方法使得机器人比较呆板，没有办法适应实时使用的需求。

基于上述需求，相关技术人员努力开发一种能够使得机器人在线模仿人运动系统，该系统能够使得机器人通过Kinect捕捉到人体的运动，并通过软件将这些运动所获取的信息以角度计算映射到NAO机器人的关节角上。这种模仿方法能够提高机器人的适应性，对于某些特定的情况，机器人可以直接模仿人类的运动，与硬编码方法相比，这种方法提高了人机交互的能力，减少程序编码的复杂程度。因此模仿学习不论是在工业领域还是在日常生活中都有广泛的应用场景。

机器人在线模仿人运动系统的学习机制要么运用摄像技术，通过摄像头捕捉人体的运动。这种系统实时性差，采集的图像往往需要复杂的程序处理才能够得到机器人的关节角。还有就是利用动作捕捉设备，这样精确地获得人体运动的数据来实现模仿。这种系统一般比较昂贵，而且需要示教者穿戴。

发明内容

为解决现有技术的问题，本发明提出一种机器人实时模仿方法，本技术方案采用价格相对低廉的传感器作为动作捕捉设备捕捉并分析处理人类的运动，在设备的程序接口中能够利用人体的深度图像获得人体的骨架信息，利用该骨架信息，计算出的关节角使得人形机器人能够模仿人类的运动，从而提高模仿学习的效率以及准确率。

为实现上述目的，本发明提供了一种机器人实时模仿方法，包括：

获得人体动作的各个关节位置，利用所述人体动作的各关节位置计算出人体相邻关节间的方向向量；

将所述人体相邻关节间的方向向量与机器人对应的相邻关节间的方向向量进行匹配，获得机器人需移动到的目标角。

优选地，所述匹配的方法为：

对所述人体相邻关节间的方向向量进行归一化，对机器人相邻关节间的方向向量进行归一化；

利用归一化的人体相邻关节间的方向向量和归一化的机器人相邻关节间的方向向量获得相对于机器人坐标系的误差函数；

对所述误差函数作最小值优化求解，获得机器人需移动到的关节角。

优选地，所述误差函数为：

其中，^BH_i表示为在机器人坐标系中人体相邻关节间的方向向量；^BV_i(θ)表示为在机器人坐标系中机器人相邻关节间的方向向量，^BV_i(θ)是关于关节角θ的函数表达式；n表示肢体数，i表示某个肢体，^BS表示误差值，ω_i表示第i个肢体的权值。

优选地，还包括：

利用机器人的质心对机器人作单脚平衡处理和双脚平衡处理。

优选地，所述单脚平衡处理的步骤包括：

获得在机器人坐标系中的机器人质心的坐标；

利用所述机器人质心的坐标确定支撑踝关节至质心的方向向量；

利用支撑踝关节至质心的方向向量确定最优踝关节角度。

优选地，所述机器人质心的坐标表达式为：

其中，^BX_C表示在机器人坐标系中机器人质心的坐标；n表示肢体数，i表示某个肢体，M表示机器人的质量；T_i表示在机器人坐标系中第i个肢体的质心坐标；m_i表示第i个肢体的质量。

优选地，所述支撑踝关节至质心的方向向量的表达式为：

^WX_C＝^WR_B(^BX_C-^BX_Ankle)

其中，^BX_C表示在机器人坐标系中机器人质心的坐标；^WX_C表示在世界坐标系中支撑踝关节至质心的方向向量；^WR_B表示从机器人坐标系到世界坐标系的旋转矩阵；^BX_Ankle表示在机器人坐标系中踝关节的位置坐标。

优选地，所述利用支撑踝关节至质心的方向向量确定最优踝关节角度的步骤包括：

利用世界坐标系中支撑踝关节至质心的方向向量^WX_C构建^WS表达式；其中，^WS表示归一化后的方向向量^WX_C相对于世界坐标系中z轴方向向量的偏移向量的模；

对^WS表达式进行最小值优化，获得最优踝关节角。

优选地，所述^WS表达式为：

其中，^WX_C表示在世界坐标系中支撑踝关节至质心的方向向量；z表示在世界坐标系中z轴方向的单位向量z＝(0,0,1)^T。

优选地，所述双脚平衡处理的步骤包括：

获得在世界坐标系中机器人质心；

确定在世界坐标系中机器人质心至机器人两脚踝关节之间的中点的向量、机器人理想质心至机器人两脚踝关节之间的中点的向量；获得两向量之间的夹角；其中，所述机器人理想质心为机器人质心位于机器人两脚踝关节连线的垂线上。

利用所述夹角确定机器人髋关节轴线旋转角度。

优选地，所述机器人髋关节轴线旋转角度的表达式为：

φ＝k_φ·J⁺·Δ^WX_C

其中，φ表示机器人髋关节轴线旋转角度；k_φ取值为0.1～0.5，以保证机器人髋关节正常移动； 表示在世界坐标系中机器人质心至机器人两脚踝关节之间的中点的向量，^WX_cdesired表示在世界坐标系中机器人理想质心至机器人两脚踝关节之间的中点的向量，Δ^WX_C表示两者向量的差向量，J⁺表示支撑踝关节至质心的方向向量^WX_C关于关节角θ的雅克比矩阵的伪逆。

优选地，还包括：

根据机器人两肢体间的最短距离是否小于两肢体对应的两肢体模型的圆柱体半径之和来判断机器人在运动过程中预测是否发生自碰撞。

上述技术方案具有如下有益效果：本技术方案通过应用运动学反解和LP优化方法，并用贝塞尔曲线拟合轨迹，将传感器检测的人体信息映射到机器人上并使其执行相似动作。同时还会考虑平衡问题以使得机器人能够模拟其他高难度动作。该方法操作简单、成本相对较低且稳定性较高。并且，本案将对后续NAO机器人平台上的更高层次智能的研究具有重要作用，比如基于模仿人类行为的跟随音乐节奏的舞蹈，双足行走，物品抓取，乃至机器人能够理解行为并根据学习模仿的行为自主地行动等等。从产业上来说，该系统具有较好的趣味性，对玩具产业将比较有吸引力。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本技术方案的工作原理示意图；

图2为本发明提出一种机器人实时模仿方法流程图；

图3为本实施例采用的视觉平台Kinect示意图；

图4为人体骨架示意图之一；

图5为人体骨架示意图之二；

图6为机器人坐标系示意图；

图7为世界坐标系示意图；

图8为本实施例机器人单脚站立时左脚踝调整以保持平衡示意图；

图9为本实施例机器人双脚平衡时髋关节旋转示意图；

图10为本实施例机器人双脚平衡原理示意图；

图11为本实施例实时模仿对比图。

具体实施方式

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

本技术方案的工作原理为：如图1所示，为本技术方案的工作原理示意图。本技术方案使用传感器，通过电脑调用API获得人体运动的骨架，利用骨架信息得到人类的运动数据。利用这些运动数据获得关节角数据，通过wifi或者有线传输至机器人上，机器人利用这些关节角进行运动，从而能够执行与人类相似的动作，实现机器人实时模仿人的动作，

基于上述工作原理，本发明提出一种机器人实时模仿方法，如图2所示。包括：

步骤201)：获得人体动作的各个关节位置，利用所述人体动作的各关节位置计算出人体相邻关节间的方向向量；

步骤202)：将所述人体相邻关节间的方向向量与机器人对应的相邻关节间的方向向量进行匹配，获得机器人需移动到的目标角。

对于本实施例来说，采用Kinect作为动作捕捉设备，如图3所示。但是这个不仅仅局限于Kinect，我们还使用过穿戴设备来捕捉动作。不过为了便于说明，我们只详细介绍使用Kinect的系统。微软生产的Kinect的程序接口中能够将人体的深度信息转换为人体的骨架，从而知道各个关节的三维位置坐标信息。如图4所示，为人体骨架示意图之一；如图5所示，为人体骨架示意图之二。在图4中，示意出人体各关节点。在图5中，示意出人体各部分。

对于本技术方案来说，涉及两个坐标系。分别为机器人坐标系(如图6所示)和世界坐标系(如图7所示)。

由于Kinect仅获得了人体各关节坐标数据及肢体间的方向向量，而不是肢体的所有参数。例如，如果沿着机器人坐标轴旋转，不会影响坐标数据，但是影响与其相连的其他肢体的参数。基于此，本案提出了对于整个肢体控制角度的优化。

通过把Kinect获得的数据归一化，即将连接两相邻关节点的方向向量除以两关节点的距离，这样就能得到各个人体关节相对于上一个关节的单位向量。

由于大多人形机器人，比如NAO，与人体有完全不同的尺寸，对机器人相邻关节间的方向向量进行归一化；利用归一化的人体相邻关节间的方向向量和归一化的机器人相邻关节间的方向向量获得相对于机器人坐标系的误差函数；通过优化误差函数的方法获得机器人需移动到的关节角。其中，所述误差函数为：

其中，^BH_i表示为在机器人坐标系中人体相邻关节间的方向向量；^BV_i(θ)表示为在机器人坐标系中机器人相邻关节间的方向向量，^BV_i(θ)是关于关节角θ的函数表达式；n表示肢体数，i表示某个肢体，^BS表示误差值，ω_i表示第i个肢体的权值。

对式(1)的误差函数采用反向运动学法进行优化，在给定条件下的误差最优问题转变成为即如下述方程：

其中，在给定关节角θ的限制条件下使^BS最小。

针对上述优化问题，采用Levenberg-Marquardt算法(以下简称LM算法)来解决此问题。LM算法是梯度下降算法和高斯-牛顿算法的结合，其迭代的过程可有下述方程表示：

其中，k表示帧数，θ_k表示第k帧所求得的机器人所有关节角向量，j表示LM算法的迭代次数；^BS_k为第k帧机器人各肢体向量与Kinect获取的对应人体向量之间的误差，θ的维数是由特定肢体的关节数决定的；J表示误差函数关于θ的雅克比矩阵；λ为适应系数以改变收敛的速度及算法的稳定性，当λ较小时，收敛速度较高。若满足以下条件：

||θ_k+1,j+1-θ_k+1,j||＜ε₁or^BS_k＜ε₂

即第k+1帧，LM算法前后两次迭代的所获取的机器人关节角向量差的模，小于特定阈值ε₁或第k帧机器人各肢体向量与Kinect获取的人体向量的偏差小于特定阈值ε₂，迭代过程将会终止，同时为了避免发散，我们会设置一个最大迭代次数值来强制停止。

综合上述实施方案，可总结关节角的具体优化流程：Kinect捕捉人体动作的各个关节角，计算出相邻关节间的方向向量，将此方向向量与NAO机器人自身对应各关节间方向向量进行匹配(即使第7条中的误差函数最小)得到NAO机器人应移动到的目标角。随后，通过NAO机器人的设备通信模块发送控制角，使机器人各关节移动至目标角已实现机器人跟随人体动作。

在通过上述方法获得机器人需移动到的目标角后，我们需要对机器人关节电机提出控制方法：通过NAO机器人自身软件开发环境的设备通信管理模块进行控制。

设备通信管理模块是NAO机器人软件开发环境的一部分，其负责大部分机器人电子设备间的通信。设备通信管理模块有独立的实时线程，每10s执行一次。为了连通制动器，动作模块通常会定时向设备通信管理模块发出更新请求。接着设备通信管理模块计算该请求的一个线性近似值并将其发送给适当的装置。通过在每个实时线程环中设置目标角度来使用设备通信管理模块。为了获得下一个实时线程环的控制角来驱动关节至目标角，我们建立每个自由度的轨迹。

通过上述描述，我们可以通过设备通信管理模块对机器人的动作进行控制，但是我们仍需要考虑机器人的平衡性问题。对于平衡性问题，我们首先要计算质心。

机器人通常为了保持平衡，系统的零力矩点应该位于脚的支撑多边形中。另外，对于两足支撑状态也有限制，即两个脚要与地面平行。因此，在模仿人动作的同时保持平衡是一个很复杂的问题。

对于本技术方案来说，若假设机器人移动缓慢，零力矩点将会粗略的接近质心的在地面的投影。这样平衡问题可以极大的简化为肢体坐标系质心的控制。

对于质心的位置计算，我们提出了一个“加权平均定位方法”来获得机器人的质心：整个机器人可以分解为不同的部分，每部分的质心及其位置已由机器人公司给出。对每个部分我们构建一个前向运动链来获得相对于肢体坐标系的局部质心位置，这样在局部坐标系中对每部分乘以旋转矩阵和平移向量，我们可以获得在机器人坐标系中每个肢体的转换向量T。接着用向量T乘以相应肢体的质心，最后将获得的所有乘积值相加求平均，即为机器人的总质心。

具体质心计算如下：

其中，^BX_C表示在机器人坐标系中机器人质心的坐标；n表示肢体数，i表示某个肢体，M表示机器人的质量；T_i表示在机器人坐标系中第i个肢体的质心坐标；m_i表示第i个肢体的质量。

在求得质心后，首先考虑单脚平衡问题。由于从Kinect直接获取脚的信息是极其不准确的，故踝关节角度是任意确定的。对于单脚平衡，我们应用直接踝关节角度适应方法来调整踝关节以保持平衡。

对于机器人单脚站立来说，如图8所示。我们需要将质心的投影置于支撑脚，换句话说，我们的目标即为找到相对于世界坐标系的向量I，I表示机器人质心相对于踝关节的方向。连接质心与支撑踝关节的向量^WX_c可表示如下：

^WX_C＝^WR_B(^BX_C-^BX_Ankle)

其中，^BX_C表示在机器人坐标系中机器人质心的坐标；^WX_C表示在世界坐标系中支撑踝关节至质心的方向向量；^WR_B表示从机器人坐标系到世界坐标系的旋转矩阵；^BX_Ankle表示在机器人坐标系中踝关节的位置坐标。

利用世界坐标系中支撑踝关节至质心的方向向量^WX_C构建^WS表达式，应用前面所提出的LM算法对^WS表达式进行优化，获得最优的踝关节角度。LM算法的目标方程可以表示如下：

其中，^WX_C表示在世界坐标系中支撑踝关节至质心的方向向量；z表示在世界坐标系中z轴方向的单位向量z＝(0,0,1)^T。最小化上述目标方程，我们可以找到期望的踝关节角，同时调整质心投影至支撑脚这样便可保持平衡。

解决了单脚平衡之后，我们将考虑双脚平衡的问题。双脚平衡的区域包括两脚及其中间的所有区域。当质心投影下两脚踝的连线上时其最稳定。换句话说，如图10所示，当质心位于穿过脚踝轴线的垂直平面上时，脚的法线应当垂直于踝关节的轴线。这里，定义脚的法线为：穿过质心，垂直于两踝关节连线的向量。

然而，仅仅调节踝关节角度有许多限制：机械的限制可能会使踝关节的策略失效；由于机械误差踝关节的控制也会增加动态不稳定性；事实上在踝关节调节过程中，零力矩点也是极易变化的。因此，为了减少踝关节修正产生的影响，我们在髋关节增加调节以使质心移动至希望的位置。相对的世界坐标系的质心可以表示如下：

^WX_C＝^WR_B(^BX_C-^BX_O)

其中：^WR_B表示从机器人坐标系到世界坐标系的旋转矩阵，^BX_C表示在机器人坐标系中机器人质心的坐标，^BX_O为从机器人坐标系到世界坐标系的转移矩阵。由于^BX_O转移矩阵的不确定性，我们对问题增加新的限制：对髋关节的调整仅有基于髋关节轴线的旋转，同时保持两踝关节间的距离不变，如图9所示。定义Φ为增加的关于髋关节轴线旋转角度，在重新定义^WR_B和^BX_O之后将其应用于雅克比矩阵可得到计算增加旋转角度的公式为：

φ＝k_φ·J⁺·Δ^WX_C

其中，φ表示机器人髋关节轴线旋转角度；k_φ取值为0.1～0.5，以保证机器人髋关节正常移动； 表示在世界坐标系中机器人质心至机器人两脚踝关节之间的中点的向量，^WX_cdesired表示在世界坐标系中机器人理想质心至机器人两脚踝关节之间的中点的向量，Δ^WX_C表示两者向量的差向量，J⁺表示支撑踝关节至质心的方向向量^WX_C关于关节角θ的雅克比矩阵的伪逆。

综合上述实施方案的平衡性优化：单脚平衡的目标即为使机器人质心到踝关节的向量与机器人世界坐标系Z轴平行。双脚站立的目标为使机器人的质心在世界坐标系XY平面的投影位于机器人两脚踝连线的上。

关节角的优化与机器人平衡性优化是相互结合的过程，即在关节角优化得到机器人控制角的基础上，使机器人模仿人行动的同时要进行机器人平衡性优化，从而使得机器人在模仿人行动的过程中保持平衡。

解决了机器人控制角度优化以及自平衡问题后，我们考虑机器人避免自碰撞的问题：自碰撞即为类人形机器人肢体被身体其他部分阻挡，若不立马停止，会损坏机器人电机。

为了用一个合理的计算代价来检测碰撞，机器人的每个肢体都模型化为一个末端为球面的圆柱体。这样检测两个肢体碰撞的尺度为：检测两个肢体间的最短距离是否小于圆柱体半径和。我们定义了两个肢体a，b的碰撞强度：

f_ab＝r_a+r_b-min{d_ab}

r_a和r_b分别表示圆柱体半径，d_ab表示a，b之间的距离。对于所有可能的碰撞肢体间都有碰撞强度检测，若f_ab大于零，则表示发生碰撞。我们比较当前步骤的碰撞强度，同时预测下一步骤；若预测值大于0且增加，则控制相应肢体的电机将停止执行下一个设备通信模块命令，否则动作会被允许。

综上，根据上述的实施方案，实验机器人实时模仿人的动作情况，如图11所示。从图11可以看出，本技术方案可实现机器人实时模仿人类动作，并且将机器人动作和人的动作相比较可知，本技术方案提高机器人模仿学习的效率和准确率。进一步地，本技术方案操作简单、成本相对较低且系统稳定性较高。

以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (12)

1.一种机器人实时模仿方法，其特征在于，包括：

获得人体动作的各个关节位置，利用所述人体动作的各关节位置计算出人体相邻关节间的方向向量；

将所述人体相邻关节间的方向向量与机器人对应的相邻关节间的方向向量进行匹配，获得机器人需移动到的目标角。

2.如权利要求1所述的方法，其特征在于，所述匹配的方法为：

对所述人体相邻关节间的方向向量进行归一化，对机器人相邻关节间的方向向量进行归一化；

利用归一化的人体相邻关节间的方向向量和归一化的机器人相邻关节间的方向向量获得相对于机器人坐标系的误差函数；

对所述误差函数作最小值优化求解，获得机器人需移动到的关节角。

3.如权利要求2所述的方法，其特征在于，所述误差函数为：

$S_B=\underset{i=1}{\overset{n}{\Σ}}{\mathrm{\ω}}_i\left|\right|\frac{1}{\left|\right|{H_B}_i\left|\right|}{H_B}_i-\frac{1}{\left|\right|{V_B}_i\left(\mathrm{\θ}\right)\left|\right|}{V_B}_i\left(\mathrm{\θ}\right)|{|}^2$

其中，^BH_i表示为在机器人坐标系中人体相邻关节间的方向向量；^BV_i(θ)表示为在机器人坐标系中机器人相邻关节间的方向向量，^BV_i(θ)是关于关节角θ的函数表达式；n表示肢体数，i表示某个肢体，^BS表示误差值，ω_i表示第i个肢体的权值。

4.如权利要求1所述的方法，其特征在于，还包括：

利用机器人的质心对机器人作单脚平衡处理和双脚平衡处理。

5.如权利要求4所述的方法，其特征在于，所述单脚平衡处理的步骤包括：

获得在机器人坐标系中的机器人质心的坐标；

利用所述机器人质心的坐标确定支撑踝关节至质心的方向向量；

利用支撑踝关节至质心的方向向量确定最优踝关节角度。

6.如权利要求5所述的方法，其特征在于，所述机器人质心的坐标表达式为：

${X_B}_C=\frac{1}{M}\underset{i=1}{\overset{n}{\Σ}}{T}_i\×m_i$

其中，^BX_C表示在机器人坐标系中机器人质心的坐标；n表示肢体数，i表示某个肢体，M表示机器人的质量；T_i表示在机器人坐标系中第i个肢体的质心坐标；m_i表示第i个肢体的质量。

7.如权利要求6所述的方法，其特征在于，所述支撑踝关节至质心的方向向量的表达式为：

^WX_C＝^WR_B(^BX_C-^BX_Ankle)

其中，^BX_C表示在机器人坐标系中机器人质心的坐标；^WX_C表示在世界坐标系中支撑踝关节至质心的方向向量；^WR_B表示从机器人坐标系到世界坐标系的旋转矩阵；^BX_Ankle表示在机器人坐标系中踝关节的位置坐标。

8.如权利要求7所述的方法，其特征在于，所述利用支撑踝关节至质心的方向向量确定最优踝关节角度的步骤包括：

利用世界坐标系中支撑踝关节至质心的方向向量^WX_C构建^WS表达式；其中，^WS表示归一化后的方向向量^WX_C相对于世界坐标系中z轴方向向量的偏移向量的模；

对^WS表达式进行最小值优化，获得最优踝关节角。

9.如权利要求8所述的方法，其特征在于，所述^WS表达式为：

$S_W=\left|\right|\frac{1}{\left|\right|{X_W}_C\left|\right|}{X_W}_C-z|{|}^2$

其中，^WX_C表示在世界坐标系中支撑踝关节至质心的方向向量；z表示在世界坐标系中z轴方向的单位向量z＝(0,0,1)^T。

10.如权利要求4所述的方法，其特征在于，所述双脚平衡处理的步骤包括：

获得在世界坐标系中机器人质心；

确定在世界坐标系中机器人质心至机器人两脚踝关节之间的中点的向量、机器人理想质心至机器人两脚踝关节之间的中点的向量；获得两向量之间的夹角；其中，所述机器人理想质心为机器人质心位于机器人两脚踝关节连线的垂线上；

利用所述夹角确定机器人髋关节轴线旋转角度。

11.如权利要求10所述的方法，其特征在于，所述机器人髋关节轴线旋转角度的表达式为：

φ＝k_φ·J⁺·Δ^WX_C

其中，φ表示机器人髋关节轴线旋转角度；k_φ取值为0.1～0.5，以保证机器人髋关节正常移动； 表示在世界坐标系中机器人质心至机器人两脚踝关节之间的中点的向量，^WX_cdesired表示在世界坐标系中机器人理想质心至机器人两脚踝关节之间的中点的向量，Δ^WX_C表示两者向量的差向量，J⁺表示支撑踝关节至质心的方向向量^WX_C关于关节角θ的雅克比矩阵的伪逆。

12.如权利要求1或4所述的方法，其特征在于，还包括：

根据机器人两肢体间的最短距离是否小于两肢体对应的两肢体模型的圆柱体半径之和来判断机器人在运动过程中预测是否发生自碰撞。