CN114407008A - 机器人运动控制方法、装置、设备及存储介质 - Google Patents

Abstract

本申请公开了一种机器人运动控制方法、装置、设备及存储介质。本申请提供的技术方案包括：获取机器人所在平面的法向量，并根据法向量和机器人的偏航角，确定机器人的俯仰补偿角和滚转补偿角；根据俯仰补偿角和滚转补偿角，确定机器人的第一躯干姿态；获取机器人所在平面与水平面的夹角，根据夹角确定机器人的摩擦约束条件；根据第一躯干姿态和摩擦约束条件，基于质心模型确定机器人的腿部支撑力，以根据腿部支撑力控制机器人运动。通过上述技术手段，解决了现有技术中因地形变化较大导致足型机器人失控的问题，提高控制稳定性。

Description

机器人运动控制方法、装置、设备及存储介质

技术领域

本申请涉及机器人技术领域，尤其涉及一种机器人运动控制方法、装置、设备及存储介质。

背景技术

足型机器人是一种可以根据控制算法自行运动的智能设备，其配置有多个自由度的腿部机构，以帮助足型机器人在各种复杂地形上平稳移动。

足型机器人在复杂地形上移动时，将地形变化作为干扰，通过抗干扰自平衡控制算法控制足型机器人的腿部运动。当地形变化较大时，地形变化给控制算法带来较大扰动，控制算法失衡导致足型机器人失控，控制稳定性差。

发明内容

本申请实施例提供一种机器人运动控制方法、装置、设备及存储介质，解决了现有技术中因地形变化较大导致足型机器人失控的问题，提高控制稳定性。

第一方面，本申请实施例提供了一种机器人运动控制方法，包括：

获取机器人所在平面的法向量，并根据所述法向量和所述机器人的偏航角，确定所述机器人的俯仰补偿角和滚转补偿角；

根据所述俯仰补偿角和所述滚转补偿角，确定所述机器人的第一躯干姿态；

获取所述机器人所在平面与水平面的夹角，根据所述夹角确定所述机器人的摩擦约束条件；

根据所述第一躯干姿态和所述摩擦约束条件，基于质心模型确定所述机器人的腿部支撑力，以根据所述腿部支撑力控制所述机器人运动。

第二方面，本申请实施例提供了一种机器人运动控制装置，包括：

补偿角度确定模块，被配置为获取机器人所在平面的法向量，并根据所述法向量和所述机器人的偏航角，确定所述机器人的俯仰补偿角和滚转补偿角；

躯干姿态预测模块，被配置为根据所述俯仰补偿角和所述滚转补偿角，确定所述机器人的第一躯干姿态；

摩擦约束确定模块，被配置为获取所述机器人所在平面与水平面的夹角，根据所述夹角确定所述机器人的摩擦约束条件；

运动控制模块，被配置为根据所述第一躯干姿态和所述摩擦约束条件，基于质心模型确定所述机器人的腿部支撑力，以根据所述腿部支撑力控制所述机器人运动。

第三方面，本申请实施例提供了一种机器人运动控制设备，包括：

一个或多个处理器；存储装置，存储一个或多个程序，当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如第一方面所述的机器人运动控制方法。

第四方面，本申请实施例提供了一种包含计算机可执行指令的存储介质，所述计算机可执行指令在由计算机处理器执行时用于执行如第一方面所述的机器人运动控制方法。

本申请通过获取机器人所在平面的法向量，并根据法向量和机器人的偏航角，确定机器人的俯仰补偿角和滚转补偿角；根据俯仰补偿角和滚转补偿角，确定机器人的第一躯干姿态；获取机器人所在平面与水平面的夹角，根据夹角确定机器人的摩擦约束条件；根据第一躯干姿态和摩擦约束条件，基于质心模型确定机器人的腿部支撑力，以根据腿部支撑力控制机器人运动。通过上述技术手段，将机器人的躯干调整至与机器人所在平面相平行，防止机器人在上下斜坡时因机器人躯干处于不平衡状态导致机器人侧翻。通过摩擦约束条件增加腿部支撑力以间接增大摩擦力，防止机器人在上下斜坡时因足端打滑导致机器人侧翻。根据机器人所在平面，补偿控制机器人的运动状态以适应地形变化，避免因地形变化过大导致机器人侧翻失控，提高了机器人的控制稳定性，保证机器人的稳定运动。

附图说明

图1是本申请一个实施例提供的一种机器人运动控制方法的流程图；

图2是本申请实施例提供的确定机器人所在平面的法向量的流程图；

图3是本申请实施例提供的机器人移动于斜平面时的示意图；

图4是本申请实施例提供的躯干坐标系的第一示意图；

图5是本申请实施例提供的躯干坐标系的第二示意图；

图6是本申请实施例提供的确定俯仰补偿角和滚转补偿角的流程图；

图7是本申请实施例提供的根据夹角确定机器人的摩擦约束条件的流程图；

图8是本申请实施例提供的机器人位于斜平面时的足端受力示意图；

图9是本申请实施例提供的另一种机器人运动控制方法的流程图；

图10是本申请实施例提供的第一足端移动轨迹的示意图；

图11是本申请实施例提供的对第一足端移动轨迹进行补偿的流程图；

图12是本申请实施例提供的第二足端移动轨迹的示意图；

图13是本申请一个实施例提供的一种机器人运动控制装置的结构示意图；

图14是本申请一个实施例提供的一种机器人运动控制设备的结构示意图。

具体实施方式

为了使本申请的目的、技术方案和优点更加清楚，下面结合附图对本申请具体实施例作进一步的详细描述。可以理解的是，此处所描述的具体实施例仅仅用于解释本申请，而非对本申请的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本申请相关的部分而非全部内容。在更加详细地讨论示例性实施例之前应当提到的是，一些示例性实施例被描述成作为流程图描绘的处理或方法。虽然流程图将各项操作(或步骤)描述成顺序的处理，但是其中的许多操作可以被并行地、并发地或者同时实施。此外，各项操作的顺序可以被重新安排。当其操作完成时所述处理可以被终止，但是还可以具有未包括在附图中的附加步骤。所述处理可以对应于方法、函数、规程、子例程、子程序等等。

本申请的说明书和权利要求书中的术语“第一”、“第二”等是用于区别类似的对象，而不用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便本申请的实施例能够以除了在这里图示或描述的那些以外的顺序实施，且“第一”、“第二”等所区分的对象通常为一类，并不限定对象的个数，例如第一对象可以是一个，也可以是多个。此外，说明书以及权利要求中“和/或”表示所连接对象的至少其中之一，字符“/”，一般表示前后关联对象是一种“或”的关系。

本申请实施例中提供的机器人运动控制方法可以由机器人运动控制设备执行，该机器人运动控制设备可以通过软件和/或硬件的方式实现，该机器人运动控制设备可以是两个或多个物理实体构成，也可以是一个物理实体构成。例如机器人运动控制设备可以是机器人本身，也可以是机器人的控制系统。除此之外，机器人运动控制设备可以至少按照有执行机器人运动控制方法的应用程序，因此，机器人运动控制设备也可以是应用程序本身。

为便于理解，本实施例以机器人的控制系统为执行机器人运动控制方法的主体为例，进行描述。

在一实施例中，当机器人从水平面移动到斜平面上时，机器人的质心偏离支撑面导致机器人失衡，如果不及时控制机器人的运动状态容易导致机器人侧翻。现有控制方法将机器人移动到斜平面时质心变化的扰动作为输入控制，基于质心模型输出腿部支撑力，进而调整机器人的腿部支撑力。但当机器人移动到大坡度的斜平面时，机器人的质心变化较大，质心模型输入大扰动，输出的腿部支撑力的误差较大，导致机器人失控侧翻，控制稳定性差。对此，本申请实施例提供的机器人运动控制方法，旨在解决机器人移动在大坡度的斜平面时，机器人容易失控侧翻的问题，保证机器人的运动稳定性。

图1给出了本申请一个实施例提供的一种机器人运动控制方法的流程图。

参考图1，该机器人运动控制方法具体包括：

S110、获取机器人所在平面的法向量，并根据法向量和机器人的偏航角，确定机器人的俯仰补偿角和滚转补偿角。

其中，机器人是指足型机器人，足型机器人配置有四个腿部机构，四个腿部机构分别设置在足型机器人的前后左右。腿部机构采用力控模式，控制系统通过分别控制各个腿部支撑力来控制足型机器人的运动状态。机器人所在平面可以是斜平面或水平面，当机器人移动于斜平面时质心会出现较大变化影响机器人的运动状态。对此本实施例以机器人移动于斜平面为例，进行示例性描述。

在一实施例中，图2是本申请实施例提供的确定机器人所在平面的法向量的流程图。如图2所示，确定机器人所在平面的法向量的步骤具体包括S1101-S1102：

S1101、当机器人的足端触地时，根据机器人腿部的连杆长度和关节角度，基于单腿运动学模型确定各个足端的第一位置坐标。

S1102、根据第一位置坐标确定机器人所在平面的平面方程，并根据平面方程确定法向量。

其中，机器人的足端是指机器人的腿部机构的用于触地的端部，腿部机构的触地端部通过三个关节连杆连接机器人的躯干。连杆长度即为触地端部对应的连接躯干的关节连杆的长度，关节角度为相连关节连杆之间的角度。示例性的，图3是本申请实施例提供的机器人移动于斜平面时的示意图。如图3所示，机器人11从水平面13移动至斜平面12后，机器人的四个足端均接触斜平面。机器人的足端刚接触地面时的腿部支撑力是基于上一时刻机器人所在平面预测的腿部支撑力，上一时刻机器人所在平面为水平面，当前时刻机器人所在平面为斜平面。因此当机器人的足端接触到斜平面后，基于斜平面预测机器人的腿部支撑力，以控制机器人适应斜平面。在一实施例中，当机器人的足端刚触地时，将连杆长度和关节角度代入预先构建的单腿运动学模型，基于单腿运动学模型分别计算四个足端的第一位置坐标。在该实施例中，单腿运动学模型如下公式所示：

其中，(x_l，y_l，z_l)为足端在腿部坐标系下的位置坐标，l₁、l₂和l₃为连杆长度，θ₁、θ₂和θ₃为关节角度，sign为腿部标志位，用于区分不同位置处的腿部机构。例如，位于机器人左侧的腿部的sign＝1，位于机器人右侧的腿部的足端的sign＝-1。示例性的，根据足端触地时对应腿部机构的连杆长度和关节角度，基于单腿运动学模型即可计算出足端在腿部坐标系下的第一位置坐标。

进一步的，由于单腿运动学模型计算出的第一位置坐标处于机器人的腿部坐标系下，因此需要通过腿部坐标系和世界坐标系的齐次变换矩阵将第一位置坐标转换至世界坐标系下，得到第四位置坐标。其中，第四位置坐标是指足端在世界坐标下的位置坐标。根据第四位置坐标拟合斜平面的平面方程。示例性的，斜平面的平面方程可描述为a+bx2+cy2＝z2，推得

其中，(x_w，y_w，z_w)为斜平面上任意一点的位置坐标，(a，b，c)为平面方程的常数，也即本实施例要计算的平面方程的三个未知数。由于机器人足端接触斜平面时四个足端的第四位置坐标属于斜平面对应点的位置坐标，可根据四个足端的第四位置坐标，基于最小二乘法求解该平面方程的三个未知数。示例性的，最小二乘法求解公式如下所示：

其中，(x_w1，y_w1，z_w1)、(x_w2，y_w2，z_w2)、(x_w3，y_w3，z_w3)和(x_w4，y_w4，z_w4)分别为四个足端的第四位置坐标，根据这四个足端的第四位置坐标计算最小二乘解，即可得到斜平面的平面方程，并确定斜平面的法向量为n＝[-b，-c，1]^T。

在获取到机器人所在平面的法向量后，可根据法向量确定机器人所在平面与水平面的夹角。当机器人所在平面与水平面的夹角等于零时，确定机器人在水平面上移动。当机器人所在平面与水平面的夹角不等于零时，确定机器人在斜平面上移动，此时可根据斜平面的法向量对腿部支撑力进行控制，以控制机器人的躯干与斜平面相平行，避免机器人在斜平面上移动时失控侧翻。

在一实施例中，图4是本申请实施例提供的躯干坐标系的第一示意图。如图4所示，X_w、Y_w和Z_w为世界坐标系的坐标轴，X_b、Y_b和Z_b分别为躯干坐标系的坐标轴，图4中的躯干坐标系为机器人刚移动至斜平面时的状态，图4中机器人的躯干姿态是基于世界坐标系的X_wY_w轴构成的平面预测的，图4中机器人的躯干11平行于X_wY_w轴构成的水平面，躯干坐标系的X_bY_b轴构成的平面平行于水平面。但由于机器人当前位于斜平面上，因此需要将图4中机器人的躯干调整至与斜平面平行。图5是本申请实施例提供的躯干坐标系的第二示意图。如图5所示，图5中机器人的躯干11平行于斜平面12，X_bY_b轴构成的平面平行于斜平面，斜平面的法向量n垂直于X_bY_b构成的平面。在该实施例中，俯仰补偿角是指将平行于水平面的X_bY_b轴构成的平面调整至与斜平面平行时，躯干坐标系绕Y轴旋转的角度；滚转补偿角是指将平行于水平面的X_bY_b轴构成的平面调整至与斜平面平行时，躯干坐标系绕X轴旋转的角度。根据俯仰补偿角和滚转俯仰角即可将平行于水平面的躯干调整至与斜平面平行。

在该实施例中，图4中的躯干坐标系和图5中的躯干坐标系之间存在一个旋转矩阵，将图4中的躯干坐标系左乘以该旋转矩阵即可得到图5中的躯干坐标系。该旋转矩阵的表达式中包含有俯仰补偿角和滚转补偿角。因此可通过该旋转矩阵推算俯仰补偿角和滚转补偿角。示例性的，图6是本申请实施例提供的确定俯仰补偿角和滚转补偿角的流程图。如图6所示，确定俯仰补偿角和滚转补偿角的步骤具体包括S1103-S1104：

S1103、根据偏航角计算法向量对应的第一分解矩阵，根据第一分解矩阵和法向量计算法向量对应的第二分解矩阵；其中，第一分解矩阵和第二分解矩阵根据法向量对应的列矩阵分解得到。

其中，偏航角是指机器人刚移动至斜平面时躯干坐标系的Z_b轴相对于世界坐标系的Z_w轴的偏转角。在一实施例中，将机器人起始点处的躯干坐标系设置为世界坐标系，可根据机器人内部设置的陀螺仪或磁力计测量机器人当前时刻的躯干坐标系与初始时刻的躯干坐标系的偏转角，即获取到机器人的偏航角。

示例性的，图5中的躯干坐标系与世界坐标系之间的旋转矩阵可以描述为R＝[e_x，e_y，e_z]，斜平面的法向量对应的列矩阵为该旋转矩阵的最后一列，按照zyx的顺序计算该旋转矩阵后，可推得法向量的列矩阵满足下述表达式：

其中，n_e为法向量的单位向量，θ_z为图5中的躯干坐标系的Z_b轴相对于世界坐标系的Z_w轴的偏转角，θ_x为图5中的躯干坐标系的X_b轴相对于世界坐标系的X_w轴的偏转角，θ_y为图5中的躯干坐标系的Y_b轴相对于世界坐标系的Y_w轴的偏转角。由于图4中的躯干坐标系的X_bY_b轴构成的平面平行于世界坐标系的X_wY_w轴构成的水平面，因此图5中的躯干坐标系的X_b轴相对于世界坐标系的X_w轴的偏转角等于滚转补偿角，图5中的躯干坐标系的Y_b轴相对于世界坐标系的Y_w轴的偏转角等于俯仰补偿角。而将图4中的躯干坐标系调整至图5中的躯干坐标系时，不会绕Z_b轴调整，因此图5中的躯干坐标系的Z_b轴相对于世界坐标系的Z_w轴的偏转角等于偏航角。

进一步的，将该法向量的列矩阵分解成第一分解矩阵和第二分解矩阵，得到

其中，q为第一分解矩阵，p为第二分解矩阵。由第一分解矩阵的表达式可知，将偏航角代入第一分解矩阵的表达式，可计算得到第一分解矩阵。将法向量的单位向量左乘以第一分解矩阵的逆矩阵，即可算得第二分解矩阵。示例性的，第二分解矩阵的计算公式为

S1104、根据第二分解矩阵计算俯仰补偿角和滚转补偿角。

由上述内容可知，第二分解矩阵满足下述表达式：

根据该表达式可推得俯仰补偿角的计算公式为θ_y＝arccos(p1/p3)，滚转补偿角的计算公式为θ_x＝arcsin(p2)。因此在计算出第二分解矩阵的各个元素后，将元素代入俯仰补偿角和滚转补偿角的计算公式中，即可求得俯仰补偿角和滚转补偿角。

S120、根据俯仰补偿角和滚转补偿角，确定机器人的第一躯干姿态。

其中，第一躯干姿态是指机器人根据斜平面的法向量调整腿部支撑力后的躯干姿态，即处于第一躯干姿态下的机器人躯干与斜平面相平行。示例性的，机器人的躯干姿态包括躯干的俯仰角、偏航角和滚转角，将俯仰补偿角和滚转补偿角与机器人当前的躯干姿态相加，可得到机器人的第一躯干姿态。

在一实施例中，对俯仰补偿角和滚转补偿角进行滤波处理，将滤波处理后的俯仰补偿角和滚转补偿角与机器人的第二躯干姿态相加，得到第一躯干姿态。其中，第二躯干姿态是指机器人足端刚接触斜平面时的躯干姿态。示例性的，当机器人足端踩到浅坑或者小凸起时，控制系统计算会出较小的俯仰补偿角和滚转补偿角，如果机器人的躯干姿态因浅坑或小凸起等地形频繁调整，会影响机器人的控制稳定性。因此通过一阶滤波对俯仰补偿角和滚转补偿角进行滤波处理，以过滤掉因地面不平造成的波动。除此之外，一阶滤波还可对俯仰补偿角和滚转补偿角进行平滑处理，以防止躯干姿态发生突变，导致机器人侧翻。

S130、获取机器人所在平面与水平面的夹角，根据夹角确定机器人的摩擦约束条件。

其中，摩擦约束条件是指当机器人足端触地时，机器人足端不滑动时足端受到的摩擦力所满足的不等式。在一实施例中，图7是本申请实施例提供的根据夹角确定机器人的摩擦约束条件的流程图。如图7所示，根据夹角确定机器人的摩擦约束条件的步骤具体包括S1301-S1302：

S1301、根据夹角确定机器人所受重力的第一分力，第一分力为重力相对机器人所在平面的水平分力。

示例性的，图8是本申请实施例提供的机器人位于斜平面时的足端受力示意图。如图8所示，G为斜平面受到的机器人的重力，G_l为重力G相对机器人所在平面的水平分力。f为机器人足端受到的腿部支撑力，f_z为足端受到的腿部支撑力在该足端对应的腿部坐标系的Z_l轴方向上的分力，f_x为足端受到的腿部支撑力在该足端对应的腿部坐标系的X_l轴方向上的分力，f_y为足端受到的腿部支撑力在该足端对应的腿部坐标系的Y_l轴方向上的分力。其中，虽然f_x、f_y和G_l为平行于斜平面的力，但f_x、f_y和G_l的方向不一致，f_x、f_y和G_l之间存在夹角。当斜平面与水平面之间的夹角为α时，G_l＝G sinα，质心模型预测腿部支撑力f时，会同时输出腿部支撑力在腿部坐标轴各个方向上的分力f_x、f_y和f_z。

S1302、根据第一分力确定机器人的摩擦约束条件。

参考图7，当机器人位于斜平面时，足端受到斜平面在水平方向上的作用力包括f_x、f_y、G_l和摩擦力F_f，f_x、f_y和G_l使得机器人足端有相对于斜平面滑动的趋势。当摩擦力F_f大于足端在X_l轴方向上的G_l和f_x的合力F_x，以及摩擦力F_f大于足端在Y_l轴方向上的G_l和f_y的合力F_y时，可以防止足端打滑，避免机器人足端打滑导致机器人侧翻。此时可推得摩擦约束条件为|F_f|≥|F_x|，|F_f|≥|F_y|。

由于足端在相对于支持面的水平方向受到的分力f_x、f_y和G_l之间存在夹角，可按照最大合力约束足端在X_l轴方向上的G_l和f_x的合力F_x和在Y_l轴方向上的G_l和f_y的合力f_y，此时足端在X_l轴方向上的G_l和f_x的合力F_x模长满足|F_x|≤|f_x|+|G_l|，足端在Y_l轴方向上的总受力的模长满足|F_y|≤|f_y|+|G_l|。足端摩擦力F_f的大小与摩擦系数以及足端受到的斜面支撑力有关，而由图8可知，足端受到的斜面支撑力等于重力相对于斜平面的垂直方向的分力G_v与f_z的合力。为避免机器在相对于斜平面的垂直方向的加速度不为0，质心模型考虑了G_v与f_z之间的平衡约束条件，因此本实施例中只考虑f_z对足端摩擦力F_f的影响。此时可推得摩擦约束条件为：

|f_x|+|G_l|≤|μf_z|

|f_y|+|G_l|≤|μf_z|

其中，μ为摩擦系数。

S140、根据第一躯干姿态和摩擦约束条件，基于质心模型确定机器人的腿部支撑力，以根据腿部支撑力控制机器人运动。

示例性的，质心模型的状态方程包括机器人的躯干姿态Θ、位置p、角速度w，线速度p和重力加速度g等状态量。质心模型的状态方程如下所示：

其中，A和B为质心模型的状态方程线性化后的矩阵，f1、f2、f3和f4分别为四个足端受到的腿部支撑力，f1＝(f_x1，f_y1，f_z1)，f2、f3和f4同理。示例性的，将第一躯干姿态代入该质心模型，得到该质心模型输出的腿部支撑力在三个方向上的分力。当质心模型输出的腿部支撑力在三个方向上的分力满足机器人所在平面的摩擦约束条件时，根据该腿部支撑力，基于逆运动学确定各关节力矩，以根据各关节力矩控制机器人平稳站立于该斜平面上。

综上，本申请实施例提供的机器人运动控制方法，通过将机器人的躯干调整至与机器人所在平面相平行，防止机器人在上下斜坡时因机器人躯干处于不平衡状态导致机器人侧翻。通过摩擦约束条件增加腿部支撑力以间接增大摩擦力，防止机器人在上下斜坡时因足端打滑导致机器人侧翻。根据机器人所在平面，补偿控制机器人的运动状态以适应地形变化，避免因地形变化过大导致机器人侧翻失控，提高了机器人的控制稳定性，保证机器人的稳定运动。

在上述实施例的基础上，图9是本申请实施例提供的另一种机器人运动控制方法的流程图。如图9所示，该机器人运动控制方法包括：

S210、获取机器人所在平面的法向量，并根据法向量和机器人的偏航角，确定机器人的俯仰补偿角和滚转补偿角。

S220、根据俯仰补偿角和滚转补偿角，确定机器人的第一躯干姿态。

S230、获取机器人所在平面与水平面的夹角，根据夹角确定机器人的摩擦约束条件。

S240、根据第一躯干姿态和摩擦约束条件，基于质心模型确定机器人的腿部支撑力，以根据腿部支撑力控制机器人运动。

示例性的，步骤S210-S240可参考步骤S110-S140。

S250、获取机器人下一次移动的第一足端移动轨迹，根据俯仰补偿角和滚转补偿角对第一足端移动轨迹进行补偿，得到机器人的第二足端移动轨迹，以根据第二足端移动轨迹控制机器人移动。

其中，第一足端移动轨迹是当机器人在水平面上移动时规划出下一次移动的足端移动轨迹。图9是本申请实施例提供的第一足端移动轨迹的示意图。如图9所示，C点、D点和F点分别为第一足端移动轨迹14的起始点、最高点和落点。但由于机器人当前所在平面为下坡的斜平面，机器人按照第一足端移动轨迹运动时实际落点为G点，使得机器人抬腿过高并过迟触地，机器人容易侧翻。同理的，当机器人所在平面为上坡的斜平面时，机器人按照第一足端移动轨迹运动，实际落点比F点高，使得机器人抬腿过低并过早触地，机器人容易侧翻。因此根据机器人的俯仰补偿角和滚转补偿角，对机器人的第一足端移动轨迹进行补偿，以得到适应于机器人所在平面的第二足端移动轨迹，避免机器人出现侧翻。

在一实施例中，图10是本申请实施例提供的对第一足端移动轨迹进行补偿的流程图。如图10所示，确定第二足端移动轨迹的步骤具体包括S2501-S2504：

S2501、根据预设足端移动距离和第一位置坐标，确定对应足端下一次移动时的落点坐标。

其中，足端移动距离是指机器人足端移动时前一落地位置和后一落地位置之间的距离。根据机器人的前进方向和足端移动距离，确定足端下一次移动时的落点坐标。参考图9，F点为机器人移动于水平面时下一移动的足端落点。示例性的，由于第一位置坐标是位于腿部坐标系下的足端位置坐标，而足端移动距离位于世界坐标系下，因此需要将腿部坐标系和世界坐标系的齐次变换矩阵，将第一位置坐标转换至世界坐标系下。然后基于落点坐标计算公式确定世界坐标系下的落点坐标。具体的，落点坐标计算公式如下所示：

其中，

为在世界坐标系下F点的位置坐标，

为在世界坐标系下的C点的位置坐标，

通过第一位置坐标左乘腿部坐标系和世界坐标系的齐次变换矩阵T_wl得到，

为足端移动距离。

S2502、根据预设足端移动高度、第一位置坐标和落点坐标，确定对应足端下一次移动时的最高点坐标。

其中，足端移动高度是预先设置的机器人足端移动时的足端抬腿高度。参考图9，D点为机器人移动于水平面时下一移动的足端最高点。示例性的，最高点坐标计算公式如下所示：

其中，

为D点在世界坐标系下的位置坐标，h为足端移动高度，足端移动高度也位于世界坐标系下。

S2503、根据俯仰补偿角和滚转补偿角确定齐次变换矩阵。

示例性的，齐次变换矩阵的计算公式如下所示：

其中，θ_y和θ_x分别为俯仰补偿角和滚转补偿角，T_xy为齐次变换矩阵。将基于法向量和偏航角计算得到的俯仰补偿角和滚转补偿角代入上述计算公式中，即可得到齐次变换矩阵T_xy。

S2504、通过齐次变换矩阵对转换至躯干坐标系下的落点坐标和最高点坐标进行旋转变换，并将旋转变换后的落点坐标和最高点坐标转换至世界坐标系下，得到第二位置坐标和第三位置坐标。

示例性的，落点坐标和最高点坐标均在世界坐标系下，而齐次变换矩阵T_xy是躯干坐标系下的矩阵。因此先根据预先设置的世界坐标系和躯干坐标系之间的齐次变换矩阵T_bw将位于世界坐标系下的落点坐标和最高点坐标转换至躯干坐标系下。坐标转换的第一计算公式如下所示：

其中，

和

分别为躯干坐标系下F点和D点的位置坐标，T_bw为世界坐标系和躯干坐标系之间的齐次变换矩阵。

进一步的，将躯干坐标系下F点和D点的位置坐标左乘齐次变换矩阵T_xy，以对躯干坐标系下F点和D进行俯仰角补偿和滚转角补偿，得到位于躯干坐标系下的F′点和D′点的位置坐标。其中，F′点和D′点分别为角度补偿后的足端落点和足端最高点。补偿计算公式如下所示：

其中，

和

分别为躯干坐标系下的F′点和D′点的位置坐标。

进一步的，通过躯干坐标系和世界坐标系之间的齐次变换矩阵，将躯干坐标系下的F′点和D′点的位置坐标转换至世界坐标系下，得到世界坐标系下F′点和D′点的位置坐标。其中，第二位置坐标和第三位置坐标分别为世界坐标系下补偿后的落点坐标和最高点坐标。坐标转换的第二计算公式如下所示：

其中，

为第二位置坐标，

为第三位置坐标。

示例性的，根据第一位置坐标、第二位置坐标和第三位置坐标，基于贝塞尔曲线拟合得到第二足端移动轨迹。图11是本申请实施例提供的第二足端移动轨迹的示意图。如图11所示，第二足端移动轨迹15的落点F′在斜平面上，第二足端移动轨迹15的最高点D′与斜平面的距离不会过高。根据第二足端移动轨迹控制足端移动，可以避免了机器人因抬脚过高以及落点较晚导致侧翻。

综上，本申请实施例提供的机器人运动控制方法，通过将机器人的躯干调整至与机器人所在平面相平行，防止机器人在上下斜坡时因机器人躯干处于不平衡状态导致机器人侧翻。通过摩擦约束条件增加腿部支撑力以间接增大摩擦力，防止机器人在上下斜坡时因足端打滑导致机器人侧翻。通过机器人所在平面的坡度对机器人移动在水平面时的足端移动轨迹进行补偿，以使机器人的足端移动轨迹的落点刚好落在机器人所在平面上，避免机器人因抬脚过高以及落点较晚或者因抬脚过高以及落点较早导致机器人侧翻。根据机器人所在平面，补偿控制机器人的运动状态以适应地形变化，避免因地形变化过大导致机器人侧翻失控，提高了机器人的控制稳定性，保证机器人的稳定运动。

图13为本申请一个实施例提供的一种机器人运动控制装置的结构示意图。参考图13，本实施例提供的机器人运动控制装置具体包括：补偿角度确定模块21、躯干姿态预测模块22、摩擦约束确定模块23和运动控制模块24。

其中，补偿角度确定模块，被配置为获取机器人所在平面的法向量，并根据法向量和机器人的偏航角，确定机器人的俯仰补偿角和滚转补偿角；

躯干姿态预测模块，被配置为根据俯仰补偿角和滚转补偿角，确定机器人的第一躯干姿态；

摩擦约束确定模块，被配置为获取机器人所在平面与水平面的夹角，根据夹角确定机器人的摩擦约束条件；

运动控制模块，被配置为根据第一躯干姿态和摩擦约束条件，基于质心模型确定机器人的腿部支撑力，以根据腿部支撑力控制机器人运动。

在上述实施例的基础上，机器人运动控制装置还包括：移动轨迹补偿模块，被配置为获取机器人下一次移动的第一足端移动轨迹，根据俯仰补偿角和滚转补偿角对第一足端移动轨迹进行补偿，得到机器人的第二足端移动轨迹，以根据第二足端移动轨迹控制机器人移动。

在上述实施例的基础上，补偿角度确定模块包括：足端坐标确定单元，被配置为当机器人的足端触地时，根据机器人腿部的连杆长度和关节角度，基于单腿运动学模型确定各个足端的第一位置坐标；平面法向量确定单元，被配置为根据第一位置坐标确定机器人所在平面的平面方程，并根据平面方程确定法向量。

在上述实施例的基础上，补偿角度确定模块包括：第一矩阵确定单元，被配置为根据偏航角计算法向量对应的第一分解矩阵，根据第一分解矩阵和法向量计算法向量对应的第二分解矩阵；其中，第一分解矩阵和第二分解矩阵根据法向量对应的列矩阵分解得到；补偿角确定单元，被配置为根据第二分解矩阵计算俯仰补偿角和滚转补偿角。

在上述实施例的基础上，第一分解矩阵的计算公式为：

第二分解矩阵的计算公式为：

俯仰补偿角的计算公式为：θ_y＝arccos(p1/p3)

滚转补偿角的计算公式为：θ_x＝arcsin(p2)

其中，q为第一分解矩阵，p为第二分解矩阵，n_e为法向量的单位向量，θ_z为偏航角，θ_x为滚转补偿角，θ_y为俯仰补偿角。

在上述实施例的基础上，躯干姿态预测模块包括：滤波单元，被配置为对俯仰补偿角和滚转补偿角进行滤波处理，将滤波处理后的俯仰补偿角和滚转补偿角与机器人的第二躯干姿态相加，得到第一躯干姿态。

在上述实施例的基础上，摩擦约束确定模块包括：重力分力确定单元，被配置为根据所述夹角确定所述机器人所受重力的第一分力，所述第一分力为所述重力相对所述机器人所在平面的水平分力；摩擦约束条件确定单元，被配置为根据所述第一分力，确定所述机器人的摩擦约束条件。

在上述实施例的基础上，移动轨迹补偿模块包括：第二位置确定单元，被配置为根据预设足端移动距离和第一位置坐标，确定对应足端下一次移动时的落点坐标；第三位置确定单元，被配置为根据预设足端移动高度、第一位置坐标和落点坐标，确定对应足端下一次移动时的最高点坐标。

在上述实施例的基础上，移动轨迹补偿模块包括：第二矩阵确定单元，被配置为根据俯仰补偿角和滚转补偿角确定齐次变换矩阵；第四位置确定单元，被配置为通过齐次变换矩阵对转换至躯干坐标系下的落点坐标和最高点坐标进行旋转变换，并将旋转变换后的落点坐标和最高点坐标转换至世界坐标系下，得到第二位置坐标和第三位置坐标。

综上，本申请实施例提供的机器人运动控制装置，通过将机器人的躯干调整至与机器人所在平面相平行，防止机器人在上下斜坡时因机器人躯干处于不平衡状态导致机器人侧翻。通过摩擦约束条件增加腿部支撑力以间接增大摩擦力，防止机器人在上下斜坡时因足端打滑导致机器人侧翻。通过机器人所在平面的坡度对机器人移动在水平面时的足端移动轨迹进行补偿，以使机器人的足端移动轨迹的落点刚好落在机器人所在平面上，避免机器人因抬脚过高以及落点较晚或者因抬脚过高以及落点较早导致机器人侧翻。根据机器人所在平面，补偿控制机器人的运动状态以适应地形变化，避免因地形变化过大导致机器人侧翻失控，提高了机器人的控制稳定性，保证机器人的稳定运动。

本申请实施例提供的机器人运动控制装置可以用于执行上述实施例提供的机器人运动控制方法，具备相应的功能和有益效果。

图14是本申请一个实施例提供的一种机器人运动控制设备的结构示意图，参考图14，该机器人运动控制设备包括：处理器31、存储器32、通信装置33、输入装置34及输出装置35。该机器人运动控制设备中处理器31的数量可以是一个或者多个，该机器人运动控制设备中的存储器32的数量可以是一个或者多个。该机器人运动控制设备的处理器31、存储器32、通信装置33、输入装置34及输出装置35可以通过总线或者其他方式连接。

存储器32作为一种计算机可读存储介质，可用于存储软件程序、计算机可执行程序以及模块，如本申请任意实施例所述的机器人运动控制方法对应的程序指令/模块(例如，机器人运动控制装置中的补偿角度确定模块21、躯干姿态预测模块22、摩擦约束确定模块23和运动控制模块24)。存储器32可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序；存储数据区可存储根据设备的使用所创建的数据等。此外，存储器32可以包括高速随机存取存储器，还可以包括非易失性存储器，例如至少一个磁盘存储器件、闪存器件、或其他非易失性固态存储器件。在一些实例中，存储器可进一步包括相对于处理器远程设置的存储器，这些远程存储器可以通过网络连接至设备。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。

通信装置33用于进行数据传输。

处理器31通过运行存储在存储器32中的软件程序、指令以及模块，从而执行设备的各种功能应用以及数据处理，即实现上述的机器人运动控制方法。

输入装置34可用于接收输入的数字或字符信息，以及产生与设备的用户设置以及功能控制有关的键信号输入。输出装置35可包括显示屏等显示设备。

上述提供的机器人运动控制设备可用于执行上述实施例提供的机器人运动控制方法，具备相应的功能和有益效果。

本申请实施例还提供一种包含计算机可执行指令的存储介质，所述计算机可执行指令在由计算机处理器执行时用于执行一种机器人运动控制方法，该机器人运动控制方法包括：获取机器人所在平面的法向量，并根据法向量和机器人的偏航角，确定机器人的俯仰补偿角和滚转补偿角；根据俯仰补偿角和滚转补偿角，确定机器人的第一躯干姿态；获取机器人所在平面与水平面的夹角，根据夹角确定机器人的摩擦约束条件；根据第一躯干姿态和摩擦约束条件，基于质心模型确定机器人的腿部支撑力，以根据腿部支撑力控制机器人运动。

存储介质——任何的各种类型的存储器设备或存储设备。术语“存储介质”旨在包括：安装介质，例如CD-ROM、软盘或磁带装置；计算机系统存储器或随机存取存储器，诸如DRAM、DDR RAM、SRAM、EDO RAM，兰巴斯(Rambus)RAM等；非易失性存储器，诸如闪存、磁介质(例如硬盘或光存储)；寄存器或其它相似类型的存储器元件等。存储介质可以还包括其它类型的存储器或其组合。另外，存储介质可以位于程序在其中被执行的第一计算机系统中，或者可以位于不同的第二计算机系统中，第二计算机系统通过网络(诸如因特网)连接到第一计算机系统。第二计算机系统可以提供程序指令给第一计算机用于执行。术语“存储介质”可以包括驻留在不同位置中(例如在通过网络连接的不同计算机系统中)的两个或更多存储介质。存储介质可以存储可由一个或多个处理器执行的程序指令(例如具体实现为计算机程序)。

当然，本申请实施例所提供的一种包含计算机可执行指令的存储介质，其计算机可执行指令不限于如上所述的机器人运动控制方法，还可以执行本申请任意实施例所提供的机器人运动控制方法中的相关操作。

上述实施例中提供的机器人运动控制装置、存储介质及机器人运动控制设备可执行本申请任意实施例所提供的机器人运动控制方法，未在上述实施例中详尽描述的技术细节，可参见本申请任意实施例所提供的机器人运动控制方法。

上述仅为本申请的较佳实施例及所运用的技术原理。本申请不限于这里所述的特定实施例，对本领域技术人员来说能够进行的各种明显变化、重新调整及替代均不会脱离本申请的保护范围。因此，虽然通过以上实施例对本申请进行了较为详细的说明，但是本申请不仅仅限于以上实施例，在不脱离本申请构思的情况下，还可以包括更多其他等效实施例，而本申请的范围由权利要求的范围决定。

Claims (12)

1.一种机器人运动控制方法，其特征在于，包括：

获取机器人所在平面的法向量，并根据所述法向量和所述机器人的偏航角，确定所述机器人的俯仰补偿角和滚转补偿角；

根据所述俯仰补偿角和所述滚转补偿角，确定所述机器人的第一躯干姿态；

获取所述机器人所在平面与水平面的夹角，根据所述夹角确定所述机器人的摩擦约束条件；

根据所述第一躯干姿态和所述摩擦约束条件，基于质心模型确定所述机器人的腿部支撑力，以根据所述腿部支撑力控制所述机器人运动。

2.根据权利要求1所述的机器人运动控制方法，其特征在于，所述方法还包括：

获取所述机器人下一次移动的第一足端移动轨迹，根据所述俯仰补偿角和所述滚转补偿角对所述第一足端移动轨迹进行补偿，得到所述机器人的第二足端移动轨迹，以根据所述第二足端移动轨迹控制所述机器人移动。

3.根据权利要求1所述的机器人运动控制方法，其特征在于，所述获取机器人所在平面的法向量包括：

当机器人的足端触地时，根据机器人腿部的连杆长度和关节角度，基于单腿运动学模型确定各个足端的第一位置坐标；

根据所述第一位置坐标确定所述机器人所在平面的平面方程，并根据所述平面方程确定所述法向量。

4.根据权利要求1所述的机器人运动控制方法，其特征在于，所述根据所述法向量和所述机器人的偏航角，确定所述机器人的俯仰补偿角和滚转补偿角包括：

根据所述偏航角计算所述法向量对应的第一分解矩阵，根据所述第一分解矩阵和所述法向量计算所述法向量对应的第二分解矩阵；其中，所述第一分解矩阵和所述第二分解矩阵根据所述法向量对应的列矩阵分解得到；

根据所述第二分解矩阵计算所述俯仰补偿角和所述滚转补偿角。

5.根据权利要求4所述的机器人运动控制方法，其特征在于，所述第一分解矩阵的计算公式为：

所述第二分解矩阵的计算公式为：

所述俯仰补偿角的计算公式为：θ_y＝arccos(p1/p3)

所述滚转补偿角的计算公式为：θ_x＝arcsin(p2)

其中，q为所述第一分解矩阵，p为所述第二分解矩阵，n_e为所述法向量的单位向量，θ_z为所述偏航角，θ_x为所述滚转补偿角，θ_y为所述俯仰补偿角。

6.根据权利要求1所述的机器人运动控制方法，其特征在于，所述根据所述俯仰补偿角和所述滚转补偿角，确定所述机器人的第一躯干姿态包括：

对所述俯仰补偿角和所述滚转补偿角进行滤波处理，将滤波处理后的俯仰补偿角和滚转补偿角与所述机器人的第二躯干姿态相加，得到所述第一躯干姿态。

7.根据权利要求1所述的机器人运动控制方法，其特征在于，所述根据所述夹角确定所述机器人的摩擦约束条件包括：

根据所述夹角确定所述机器人所受重力的第一分力，所述第一分力为所述重力相对所述机器人所在平面的水平分力；

根据所述第一分力，确定所述机器人的摩擦约束条件。

8.根据权利要求2所述的机器人运动控制方法，其特征在于，所述获取所述机器人下一次移动的第一足端移动轨迹包括：

根据预设足端移动距离和第一位置坐标，确定对应足端下一次移动时的落点坐标；

根据预设足端移动高度、所述第一位置坐标和所述落点坐标，确定对应足端下一次移动时的最高点坐标。

9.根据权利要求8所述的机器人运动控制方法，其特征在于，所述根据所述俯仰补偿角和所述滚转补偿角对所述第一足端移动轨迹进行补偿，确定所述机器人的第二足端移动轨迹包括：

根据所述俯仰补偿角和所述滚转补偿角确定齐次变换矩阵；

通过所述齐次变换矩阵对转换至所述躯干坐标系下的落点坐标和最高点坐标进行旋转变换，并将旋转变换后的落点坐标和最高点坐标转换至世界坐标系下，得到第二位置坐标和第三位置坐标。

10.一种机器人运动控制装置，其特征在于，包括：

补偿角度确定模块，被配置为获取机器人所在平面的法向量，并根据所述法向量和所述机器人的偏航角，确定所述机器人的俯仰补偿角和滚转补偿角；

躯干姿态预测模块，被配置为根据所述俯仰补偿角和所述滚转补偿角，确定所述机器人的第一躯干姿态；

摩擦约束确定模块，被配置为获取所述机器人所在平面与水平面的夹角，根据所述夹角确定所述机器人的摩擦约束条件；

运动控制模块，被配置为根据所述第一躯干姿态和所述摩擦约束条件，基于质心模型确定所述机器人的腿部支撑力，以根据所述腿部支撑力控制所述机器人运动。

11.一种机器人运动控制设备，其特征在于，包括：一个或多个处理器；存储装置，存储一个或多个程序，当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如权利要求1-9任一所述的机器人运动控制方法。

12.一种包含计算机可执行指令的存储介质，其特征在于，所述计算机可执行指令在由计算机处理器执行时用于执行如权利要求1-9任一所述的机器人运动控制方法。

Images