一种类人型机器人平衡的控制方法与流程

本发明涉及一种类人型机器人平衡的控制方法，特别是利用类人型机器人身体重心及下肢关节点位置计算并预测机器人运动时髋关节和踝关节应弯曲的角度以维持平衡的一种控制方法。

背景技术：

在当今高速发展的机器人领域，类人型机器人是一大代表。类人型机器人与人体外形相近，可以做出与人类相似的动作。由于具有双足，这类机器人可以适应更多的环境，完成更多的任务，例如面对包含上台阶相关动作的任务时，类人形机器人可以轻易完成，而靠轮子前进的机器人就很难胜任。

而在操作类人形机器人的过程中，不可避免地会遇到机器人的平衡问题。现在主流的平衡控制方法是根据负反馈的思想，在机器人身体移动并使重心移动时，通过机器人全身各部件的联动来反方向移动身体重心，使重心到地面的投影始终在支撑脚的脚面到地面投影的面内，以此维持平衡。

但是这种方法很难让机器人做出指定的动作，比如在指定机器人全身大部分关节的角度后，这种方法就不太适用了。而且由于需动用机器人全身部件，这一方法计算复杂度较大，又由于是通过负反馈实现的控制，此方法需防范震荡现象。

技术实现要素：

为了解决类人型机器人在双脚站立、单脚站立、行走时的平衡控制问题，本发明提供一种类人型机器人平衡的控制方法，技术方案如下：

一种类人型机器人平衡的控制方法，包括以下步骤：根据传感器获取的所述机器人全身关节弯曲的角度和已知的所述机器人部件尺寸，计算所述机器人的重心及下肢关节位置；根据所述机器人的状态计算踝关节和/或髋关节维持平衡的理想角度；用所述踝关节和/或髋关节维持平衡的理想角度替换步骤1中传感器获取的踝关节和/或髋关节弯曲的角度。

所述根据所述机器人的状态计算踝关节和/或髋关节维持平衡的理想角度包括，所述机器人双脚站立时，计算踝关节在前后方向、左右方向应弯曲的角度，或所述机器人单脚站立时，计算踝关节在前后方向、左右方向应弯曲的角度和髋关节在左右方向上应弯曲的角度，或所述机器人在行走中抬起的脚刚落地时，计算双脚的踝关节在左右、前后方向上应弯曲的角度。

所述机器人在行走中抬起的脚刚落地时踝关节应弯曲的角度，是在所述机器人抬起的脚的底面到地面的距离为0.5～1.5个机器人踝关节到脚的底面的距离时进行预测的。

本发明的有益效果在于：在指定机器人大部分身体关节角度后仍能较好地控制平衡。通过本发明，机器人能够动态地、实时地维持平衡，计算复杂度较小。

附图说明

图1是基于关节角度对类人形机器人平衡控制方法的示例的示意图；

图2是类人型机器人下肢各关节点及重心位置的示意图；

图3是类人型机器人双脚站立时，下肢关节及身体重心相对位置经抽象后的示意图；

图4是类人型机器人双脚站立且右腿在前时，下肢的左视图的示意图；

图5是类人型机器人双脚站立且右腿在前时，右腿部分左视图经抽象后的示意图；

图6是类人型机器人双脚站立且右腿在后时，下肢的左视图的示意图；

图7是类人型机器人双脚站立且右腿在后时，右腿部分左视图经抽象后的示意图；

图8是类人型机器人在双脚站立时，下肢的主视图的示意图；

图9是类人型机器人在双脚站立时，右腿部分的主视图经抽象后的示意图；

图10是类人型机器人右脚单脚站立时，下肢关节及身体重心相对位置的示意图；

图11是类人型机器人左脚单脚站立时，下肢的左视图的示意图；

图12是类人型机器人左脚单脚站立时，左腿部分左视图经抽象后的示意图；

图13是类人型机器人左脚单脚站立时，下肢的示意图；

图14是类人型机器人左脚单脚站立时，在左右方向上髋关节、踝关节经抽象后的示意图；

图15是类人型机器人行走时，落脚前对落脚后关节点的预测示意图；

图16是类人型机器人行走时，模式切换的示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

图1为基于关节角度对类人形机器人平衡控制方法的示例的示意图，本发明主要是“点坐标投影变换与位置预测”与“三角形中求解关键关节角度”部分的内容。本方法是先通过传感器获取机器人的关节角度，结合已知的机器人部件尺寸，根据正向运动学原理，计算出所需关节点坐标及重心坐标。再根据双脚站立时，重心在地面的投影要在机器人两踝关节点在地面投影的连线上这一原则，利用各关节点及重心的坐标，计算踝关节的角度；根据在单脚站立时，重心到地面的投影与踝关节点到地面的投影接近重合这一原则，利用各关节点及重心的坐标，计算踝关节的角度和髋关节在左右方向的角度；根据行走时，抬起的脚要落地时，身体的各关节点可以近似看作是绕支撑脚的踝关节点作旋转这一思路，预测脚落地的瞬间，身体各关节点的坐标，从而利用双脚平衡时的计算方法去计算脚落地时踝关节应弯曲的角度。然后改变这些关节角的角度，使机器人的重心落在维持平衡应在的位置，机器人就表现出了一个能维持平衡的姿势。本方法由于是根据机器人的质量分布来计算机器人关键的关节角度，故可在指定了机器人大部分身体关节角度后仍能较好地控制平衡，且计算复杂度较小。

首先说明一种数学方法，用于在空间中由已知坐标的三点构成的三角形中，求解任意内角的方法。此举只是为了说明由空间中已知坐标的三点构成的三角形，可以求其内角，但方法不限于这一种。

已知三点坐标分别为：(x1，y1，z1)、(x2，y2，z2)、(x3，y3，z3)，由两点间距离公式：

其中l为两点间距离，a1、a2、b1、b2、c1、c2分别为空间中两点的x、y、z轴坐标，可求得任意两点间距离，分别记为l1、l2、l3。再由：

即可求得三角形内角的角度。

如图3，本发明涉及的机器人在下肢中，右髋关节点2、右踝关节点4、左髋关节点6、左踝关节点8应至少有两个自由度，可实现前后方向和左右方向的运动；右膝关节点3、左膝关节点7应至少有一个自由度，可实现前后方向的运动。

图2为类人型机器人在双脚站立时，下肢各关节以及重心相对位置的示意图，经抽象后如图3。图3中节点5为机器人重心节点1到地面的投影。为保证机器人维持平衡不摔倒，重心在地面的投影必须在两脚及其连线上。为简化计算、保证稳定性，本发明将其简化为：重心在地面的投影要在机器人两踝关节点在地面投影的连线上。

根据正向运动学，即通过已知的机器人各关节角度和各部件的尺寸可计算出各关节的坐标。若各部件的重量及重心也为已知，利用：

可以计算出机器人整体重心的坐标位置。其中，x轴的正方向可规定为机器人的正前方向，y轴的正方向可规定为机器人的正左方向。至此，可以得到机器人各关节的角度、各关节的坐标、整体重心的坐标。

图4是类人型机器人双脚站立且右腿在前时，下肢的左视图的示意图。经抽象后，右腿部分左视图的示意图如图5。图5中节点10为重心投影节点5在过右脚踝节点4的前后方向的直线上的投影。右脚在前，故投影节点10在右脚踝节点4的正后方。所以投影节点10的x轴坐标与重心节点1的x轴坐标相同，投影节点10的y轴坐标与右踝关节点4的y轴坐标相同，投影节点10的z轴坐标为0。

右膝盖关节点3到右髋关节点2的距离、右髋关节点2到右踝关节点4的距离为已知，图5中12的角度可由无线传感器获取，故在以右髋关节点2、右膝盖关节点3、右踝关节点4为三个端点的三角形中，可求得图5中14的角度。在以右髋关节点2、投影节点10、右踝关节点4为端点的三角形中，三端点坐标已知，可求得图5中13的角度。计算：

π-(∠13+∠14)(4)

可得右脚踝在前后方向上应弯曲的角度15。

图6是类人型机器人双脚站立且右腿在后时，下肢的左视图的示意图。经抽象后，右腿部分左视图的示意图如图7。图7中关节点10为重心投影节点5在过右脚踝节点4的前后方向的直线上的投影。右脚在后，故投影节点10在右脚踝节点4的正前方。故投影节点10的x轴坐标与重心节点1的x轴坐标相同，投影节点10的y轴坐标与右脚踝节点4的y轴坐标相同，投影节点10的z轴坐标为0。

与图5情况下的方法相似。右膝盖关节点3到右髋关节点2的距离、右髋关节点2到右踝关节点4的距离为已知，图7中12的角度可由无线传感器获取，故在以右髋关节点2、右膝盖关节点3、右踝关节点4为三个端点的三角形中，可求得图7中14的角度。在以右髋关节点2、投影节点10、右踝关节点4为端点的三角形中，三端点坐标已知，可求得图7中13的角度。将13与14做差，即可求得右脚踝在前后方向上应弯曲的角度15。

当右脚踝节点4与投影点10重合时，则在前后方向上达到单脚站立的条件，可参照下面图12的方法计算右脚踝在前后方向上应弯曲的角度。

图8是类人型机器人在双脚站立时，下肢的主视图的示意图。经抽象后，可得右腿部分的主视图的示意图9。图9中关节点11为重心投影节点5在过右脚踝节点4的左右方向的直线上的投影。投影节点11的y轴坐标与重心投影节点5的y轴坐标相同，投影节点11的x轴坐标与右脚踝节点4的x轴坐标相同，投影节点11的z轴坐标为0。在以右髋关节点2、右脚踝节点4、投影节点11为端点的三角形中，三端点坐标已知，可求得图9中16的角度，再由：

π/2-∠16(5)

即得到右脚踝在左右方向上应弯曲的角度。此处严格的来说，应将关节点2替换为点2到过点4与点11且与地面垂直的面的投影点，来计算角16的角度，但为了简单，做了近似处理。

在右踝关节点4与投影节点11重合时，则在左右方向上达到单脚站立的条件，可参照下面图14的方法。

在双脚站立时，左脚踝在前后和左右方向上弯曲角度的计算方法与右脚相似，此处不再赘述。

当机器人单脚站立时，如图10，为保证机器人的平衡，机器人整体重心的到地面的投影点应在承重的那只脚到地面投影的面内。为简化计算、增加精度，本发明将其简化为：重心到地面的投影与踝关节点到地面的投影接近重合。

图11是类人型机器人左脚单脚站立时，下肢的左视图的示意图。经抽象后，左腿部分左视图的示意图如图12。图12中，由左髋关节6、左膝关节7、左踝关节8构成的三角形中，三个端点坐标已知，故可求出18的角度。在由重心节点1、左髋节点6、左踝关节点8构成的三角形中，三个端点已知，可求得19的角度。由

π/2-(∠18-∠19)(6)

可计算得，机器人在单脚站立时，脚踝在前后方向上应弯曲的角度20。

图13是类人型机器人左脚单脚站立时，下肢的示意图。经抽象后，在左右方向上髋关节、踝关节的示意图如图14。

在计算左右方向上的角度时，分别从髋关节6、踝关节8做垂线，因此图14中两个21角呈内错角。参照图14，可得图中两个21角相等，而可由：

π/2+∠21(7)

轻易得到机器人的髋关节和踝关节在左右方向上应弯曲的角度22。而在以重心节点1、髋关节6、左踝关节8为端点的三角形中，三端点坐标已知，可求得角21的角度。

在类人型机器人以右脚单脚站立时，髋关节、踝关节弯曲角度的计算方法与以左脚单脚站立相似，不再赘述。

至此，已经介绍完机器人在双脚、单脚情况下，计算关键关节角度的方法。下面，介绍有关机器人行走时，计算关节角度以维持平衡的方法。

机器人在行走时，与人行走类似，首先机器人将处于双脚站立的状态，此时利用双脚站立时的平衡维持方法。然后，机器人将抬起一只脚，此时将处于单脚站立状态，利用单脚站立时维持平衡的方法可维持这一阶段的平衡。接着，机器人的身体将会逐渐前倾，同时机器人将抬起的脚逐渐放向地面，但是由于处在单脚模式下，机器人仍旧能处于平衡状态。如图15，当机器人抬起的脚到地面的距离，即图中距离23，为0.5～1.5个机器人踝关节到脚底面的距离，即图中距离25的0.5～1.5倍时，本方法将对脚落到地面时各关节点位置进行预测。预测是通过设定机器人支撑脚的脚踝为转轴，如15中的踝关节8，计算机器人的下肢各关节点绕转轴旋转后，机器人的脚刚好能够落到地面时各关节点的坐标。旋转过程中各关节点的相对位置将近似不变。利用预测出的关节点坐标，带入维持双脚平衡时的角度计算方法，即可预测出机器人抬起的脚落到地面时，两踝关节应弯曲的角度。预测完成后，机器人调整踝关节，表现出预测的弯曲角度。这时，机器人将会绕支撑脚的脚踝旋转，重心向前，抬起的脚将会落地，并维持平衡。本方法通过预测落脚时各关节的位置，实现状态的切换，从而使机器人在向前行走时保持平衡。

本方法涉及的机器人在脚踝部分会有前后和左右两个方向的自由度，故机器人在落脚时，绕着旋转的轴是与y轴平行的。以图15的情况为例，当机器人右脚即将落下时，旋转轴过关节点8。记关节点8的坐标为(x4，y4，z4)，记关节点4的坐标为(x5，y5，z5)。则旋转轴可表示为关节点4到旋转轴的距离可通过公式：

计算得关节点4到旋转轴的距离。

点26为预测到的关节点4的落点，设点26的坐标为(x6，y6，z6)，故点26到旋转轴距离也为l4。图中点4和点26间的距离可近似为机器人脚底到地面的距离，即距离23。故，在由点4和点26的连线以及这两点分别到旋转轴的垂线，在这三条线构成的三角形中，三边长分别为l4、l4和距离23，可由公式(2)计算得旋转角度，记为ω。

已知旋转轴、旋转角度、旋转的对象即点4，即可求得旋转后的点26。具体求解方法为数学问题，此处列举一种。可利用旋转矩阵求解，各关节点坐标关系如下：

求解其他关节点旋转后的坐标，例如图15中点3、点2、点7旋转后的坐标，也可用这种方法。求解完成后，即可预测脚落下后各关节点的位置，利用双脚平衡时的计算方法，即可计算出机器人右脚落地时身体各关节应弯曲的角度，以此维持机器人右脚刚落地时的平衡。当机器人是左脚即将落下时，计算方法与右脚落下的方法相似，此处不再赘述。当机器人的脚落下后，即可按照图16所示的循环结构继续向前走动。