CN112859901B - 一种仿人机器人连续动态稳定跳跃控制方法 - Google Patents

Abstract

本发明提供了一种仿人机器人连续动态稳定跳跃控制方法，属于仿人机器人技术领域。本发明根据仿人机器人全身动力学方程，获取仿人机器人下肢各关节驱动力矩，对仿人机器人下肢施加驱动力矩，仿人机器人进入起跳阶段，当仿人机器人上身位置到达起跳高度，进入空中阶段，控制仿人机器人各关节达到落地时的期望姿态，当仿人机器人从空中下落并与地面接触时，进入落地阶段，完成一个跳跃循环，并且返回机器人跳跃的初始状态。本发明保证仿人机器人的连续动态稳定跳跃控制，显著提升仿人机器人的运动能力，进一步增强仿人机器人的环境适应能力。

Description

一种仿人机器人连续动态稳定跳跃控制方法

技术领域

本发明属于仿人机器人技术领域，具体涉及一种仿人机器人连续动态稳定跳跃控制方法。

背景技术

仿人机器人具有人类的外形特征，通过双腿适应各种复杂地形，能辅助或替代人类完成作业任务。跳跃运动能使仿人机器人具有更强的运动能力，增强仿人机器人的环境适应能力和实际应用能力。现有关于仿人机器人跳跃的方法，大多为单次跳跃的控制方法，还未有针对仿人机器人连续跳跃控制方法。

发明内容

针对现有技术中存在不足，本发明提供了一种仿人机器人连续动态稳定跳跃控制方法，使仿人机器人跳跃过程中连续动态稳定。

本发明是通过以下技术手段实现上述技术目的的。

一种仿人机器人连续动态稳定跳跃控制方法，其特征在于：

建立基于浮动基的仿人机器人全身动力学方程；

规划仿人机器人跳跃轨迹，包括起跳阶段、空中阶段和稳定落地阶段；起跳阶段中，仿人机器人的期望外力为：

f_x＝k_s*v_d；稳定落地阶段中，仿人机器人的期望外力为f_x＝0、

建立仿人机器人跳跃控制方法：对浮动基的广义自由度进行分解，将仿人机器人全身动力学方程表示为

通过仿人机器人期望的状态，得到仿人机器人所期望的脚底受力F₁与F₂，进一步获取仿人机器人下肢各关节驱动力矩τ，将所述驱动力矩τ施加至仿人机器人下肢，根据规划仿人机器人跳跃轨迹，实现仿人机器人连续动态稳定跳跃控制；

其中，k_{p_jump}与k_{v_jump}分别为仿人机器人竖直方向期望外力的PD控制参数，q₂为仿人机器人起跳时刻的膝关节角度，q_{2_jump}为仿人机器人起跳时刻的期望膝关节角度，

为仿人机器人起跳时刻的膝关节角速度，f_z为仿人机器人上身施加的竖直方向力，f_x为仿人机器人水平方向期望受力，k_s为仿人机器人水平方向期望外力与上身期望力矩的PD控制参数，v_d为仿人机器人水平方向期望的运动速度，z为仿人机器人在竖直方向的位置，

为仿人机器人在竖直方向的速度，z_init为仿人机器人初始跳跃高度，k_p-landing和k_vz分别为仿人机器人落地时竖直方向期望外力的PD控制参数，M_ij为仿人机器人的质量矩阵，q_f为浮动基的6个广义自由度，q_l为仿人机器人双腿的关节角度向量，h_i为仿人机器人重力、科里奥利力矩阵，J_i1与J_i2为从仿人机器人脚底转换到仿人机器人关节空间的雅克比转换矩阵，i、j＝1，2。

进一步的技术方案，在跳跃的过程中，引入仿人机器人稳定的控制条件：

p_x、p_y分别为浮动基∑_f在世界坐标系∑_w中沿着x、y方向的位置，f_Lz和f_Rz分别为所述F₁和F₂在z方向的分量，[p_Rx p_Ry]和[p_Lx p_Ly]分别为根据仿人机器人左右脚的期望接触力和力矩计算的ZMP位置。

更进一步的技术方案，在跳跃的过程中，仿人机器人的ZMP需满足如下约束：Lx_min+δ_rx＜p_x＜Lx_max-δ_ax和Ly_min+δ_ry＜p_y＜Ly_max-δ_ay，其中Lx_min、Lx_max为仿人机器人脚底与地面形成的支撑多边形沿x方向的最小值与最大值，δ_rx与δ_ax分别为沿x方向的补偿量，Ly_min、Lx_max为仿人机器人脚底与地面形成的支撑多边形沿y方向的最小值与最大值，δ_ry与δ_ay分别为沿y方向的补偿量。

进一步的技术方案，所述仿人机器人连续动态稳定跳跃控制具体为：对仿人机器人下肢施加驱动力矩τ，仿人机器人进入起跳阶段，当仿人机器人上身位置到达起跳高度，进入空中阶段，控制仿人机器人各关节达到落地时的期望姿态，当仿人机器人从空中下落并与地面接触时，进入落地阶段，完成一个跳跃循环，并且返回机器人跳跃的初始状态。

进一步的技术方案，所述仿人机器人全身动力学方程为：

其中F_i为仿人机器人双脚与地面产生的接触力和力矩。

进一步的技术方案，所述仿人机器人跳跃轨迹包括躯干的运动轨迹与脚踝的运动轨迹，具体为：

其中H₀为仿人机器人空中阶段质心能达到的最高高度与起跳时的质心高度的高度差，v₀为仿人机器人起跳时刻的质心速度，g为重力加速度，

为仿人机器人的质心加速度，M为仿人机器人整体质量。

进一步的技术方案，所述对浮动基的广义自由度进行分解，具体为：q_f＝[p_x p_y p_zθ_x θ_y θ]^T，其中p_x、p_y、p_z分别为浮动基∑_f在世界坐标系∑_w中沿着x、y、z三个方向的位置，θ_x、θ_y、θ_z分别为浮动基∑_f在世界坐标系∑_w中沿着x、y、z三个方向的姿态。

本发明的有益效果为：

(1)本发明根据期望的跳跃高度确定仿人机器人躯干的运动轨迹与脚踝的运动轨迹，包括起跳阶段、空中阶段和稳定落地阶段，根据起跳阶段和稳定落地阶段的特点，控制仿人机器人的期望外力，保证仿人机器人的连续动态稳定跳跃控制，本发明显著提升仿人机器人的运动能力，进一步增强仿人机器人的环境适应能力。

(2)本发明在仿人机器人跳跃的过程中，引入仿人机器人稳定的控制条件，同时设置仿人机器人的ZMP约束，使仿人机器人在跳跃过程中保持稳定，避免仿人机器人脚部翻转而引起摔倒情况的发生。

附图说明

图1为本发明所述仿人机器人简化模型图；

图2为本发明所述仿人机器人单次跳跃过程中质心与脚踝的运动轨迹示意图；

图3为本发明所述仿人机器人单次跳跃的状态示意图；

图4为本发明所述实现仿人仿人机器人连续跳跃流程图。

具体实施方式

下面结合附图以及具体实施例对本发明作进一步的说明，但本发明的保护范围并不限于此。

一种仿人机器人连续动态稳定跳跃控制方法，具体包括如下步骤：

步骤(1)，建立基于浮动基的仿人机器人全身动力学方程

将仿人机器人简化为如图1所示的五个连杆组成的模型，其中m₀为仿人机器人上身质量，m₁为仿人机器人右腿大腿质量，m₂为仿人机器人右腿小腿质量，m₃为仿人机器人左腿大腿质量，m₄为仿人机器人左腿小腿质量，机器人足部的质量忽略不计。仿人机器人的运动自由度简化为具有踝关节q₁、膝关节q₂、髋关节q₃的三个关节所组成的平面机器人。

建立仿人机器人质心在以浮动基∑_f为参考坐标系的动力学方程：

其中，M_ij(i，j＝1，2)为仿人机器人的质量矩阵；q_f为浮动基的6个广义自由度；q_l为仿人机器人双腿的关节角度向量，包括仿人机器人下肢所有关节自由度；h_i为仿人机器人重力、科里奥利力矩阵(i＝1，2)，τ为仿人机器人下肢各个关节的驱动力矩，F_i为仿人机器人双脚与地面产生的接触力和力矩(包括图1中n_r、f_r、n_l和f_l，n_r表示右脚与地面产生的力矩，f_r表示右脚与地面产生的接触力，n_l表示左脚与地面产生的力矩，f_l表示左脚与地面产生的接触力)，J_i1与J_i2为从仿人机器人脚底转换到仿人机器人关节空间的雅克比转换矩阵，其中i＝1时代表右脚，i＝2时代表左脚。

步骤(2)，规划仿人机器人跳跃轨迹

如图2所示为设计仿人机器人跳跃过程的运动轨迹，其中t₀为仿人机器人开始跳跃的时刻，t₁为仿人机器人跳跃离地的时刻，t₂为仿人机器人落地的时刻，t₃为仿人机器人跳跃结束的时刻；根据期望的跳跃高度确定躯干的运动轨迹与脚踝的运动轨迹，计算方法如下：

其中，在本实施例中，定义仿人机器人空中阶段质心能达到的最高高度与起跳时的质心高度的高度差H₀为起跳的期望高度，由于仿人机器人在空中只受重力作用，空中的状态由仿人机器人离开地面的状态决定，式(2)中v₀为仿人机器人起跳时刻的质心速度，g为重力加速度，且起跳时刻的质心速度v₀由仿人机器人的质心加速度

积分获得，而

由仿人机器人期望的地面受力及仿人机器人所受重力决定；M为仿人机器人整体质量，F₁、F₂表示仿人机器人所期望的脚底受力。

如图3所示，仿人机器人单次跳跃主要由三个状态组成，分别是：

1)起跳阶段：仿人机器人站立在地面上，通过下肢的运动，推动仿人机器人上身向上运动，使全身具有向上的运动速度，为仿人机器人提供跳跃的动力。在起跳阶段中，仿人机器人的期望外力可以由以下公式计算得到：

f_x＝k_s*v_d (6)

其中，k_{p_jump}与k_{v_jump}分别为仿人机器人竖直方向期望外力的PD控制参数(位置和速度)，q₂为仿人机器人起跳时刻的膝关节角度，q_{2_jump}为仿人机器人起跳时刻的期望膝关节角度，

为仿人机器人起跳时刻的膝关节角速度；f_z为仿人机器人上身施加的竖直方向力，且f_z＝f_Lz+f_Rz，可以通过调节k_{p_jump}的大小来调节仿人机器人跳跃的高度；f_x为仿人机器人水平方向期望受力，且f_x＝f_Lx+f_Rx，k_s为仿人机器人水平方向期望外力与上身期望力矩的PD控制参数，v_d为仿人机器人水平方向期望的运动速度，可以通过调节v_d的大小来调节仿人机器人跳跃过程中水平方向期望受力。

2)空中阶段：仿人机器人在到达期望的离地姿态之后，进入跳跃的空中阶段，仿人机器人在空中只受到竖直方向的重力，完成由起跳阶段到落地调整阶段的过渡，需要将仿人机器人起跳结束姿态调整至落地姿态(为现有技术)。

3)稳定落地阶段：本阶段的主要目的是使仿人机器人在落地的过程中保持稳定，并且返回机器人跳跃的初始状态，从而进入下一个跳跃循环。本阶段仿人机器人各个关节的控制如下：

f_x＝0 (7)

为使仿人机器人保持稳定性，将仿人机器人水平方向期望受力设为0，竖直方向期望受力由仿人机器人初始跳跃高度z_init决定，k_p-landing和k_vz分别为仿人机器人落地时竖直方向期望外力的PD控制参数，z为仿人机器人在竖直方向的位置，

为仿人机器人在竖直方向的速度；经过仿人机器人落地阶段，仿人机器人的状态将回到起跳阶段。

步骤(3)，建立仿人机器人跳跃控制方法

对浮动基的广义自由度进行分解：

q_f＝[p_x p_y p_z θ_x θ_y θ_z]^T (9)

其中p_x、p_y、p_z分别为浮动基∑_f在世界坐标系∑_w中沿着x、y、z三个方向的位置，θ_x、θ_y、θ_z分别为浮动基∑_f在世界坐标系∑_w中沿着x、y、z三个方向的姿态；令p＝[p_x p_y p_z]^T、θ＝[θ_x θ_y θ_z]^T，则式(1)可变形为：

通过分析公式(10)可知，与浮动基固连的仿人机器人躯干运动状态(包括运动位置和姿态)只与仿人机器人脚底受到的外力相关，而且，可以通过仿人机器人期望的状态(见图

2)，得到

和

通过公式(3)(公式(3)中的质量矩阵、雅克比转换矩阵和重力、科里奥利力矩阵可以直接求出)计算得到仿人机器人所期望的脚底受力，即F₁与F₂。在获得仿人机器人期望外力与仿人机器人运动状态的基础之上，通过公式(11)的关系，求得仿人机器人下肢各关节驱动力矩τ，从而获得了使仿人机器人产生跳跃运动的控制量。

在仿人机器人跳跃过程中，需要使仿人机器人在跳跃过程中保持稳定，避免仿人机器人脚部翻转而引起摔倒情况的发生。因此，在跳跃的过程中，引入仿人机器人稳定的控制条件：

其中，f_Lz和f_Rz分别为仿人机器人左脚和右脚受到的期望外力F₁与F₂在z方向的分量，[p_Rx p_Ry]和[p_Lx p_Ly]分别为根据仿人机器人左右脚的期望接触力和力矩计算的ZMP(Zero Moment Point，零力矩点，为仿人机器人脚底受到沿水平方向合力矩为零的位置，若该位置位于仿人机器人脚底与地面接触点所组成的支撑多边形内，则仿人机器人能够保持稳定，不发生翻到)位置，其计算方法如下式：

其中，p_Rx0、p_Ry0、p_Lx0、p_Ly0分别为仿人机器人右脚和左脚的在世界坐标系∑_w中的位置；在跳跃的过程中，仿人机器人的ZMP需满足如下约束：

Lx_min+δ_rx＜p_x＜Lx_max-δ_ax (16)

Ly_min+δ_ry＜p_y＜Ly_max-δ_ay (17)

其中，Lx_min、Lx_max为仿人机器人脚底与地面形成的支撑多边形沿x方向的最小值与最大值，δ_rx与δ_ax分别是沿着x方向的补偿量，使仿人机器人在x方向具有更大的稳定性；Ly_min、Lx_max为仿人机器人脚底与地面形成的支撑多边形沿y方向的最小值与最大值，δ_ry与δ_ay分别是沿着y方向的补偿量，使仿人机器人在y方向具有更大的稳定性。

图4所示为实现仿人仿人机器人连续跳跃的流程图，主要过程为：

首先仿人机器人站立于地面之上，进入起跳阶段，对仿人机器人下肢施加驱动力矩τ，使仿人机器人具有向上的运动；当仿人机器人上身位置到达起跳高度，进入空中阶段，空中阶段控制仿人机器人各关节达到落地时的期望姿态；当仿人机器人从空中下落，并与地面接触之后，仿人机器人进入跳跃的落地阶段；通过上述三个阶段，仿人机器人完成了一个跳跃循环，如果未接收到跳跃截止命令，仿人机器人将重新进入到起跳阶段，完成下一个跳跃循环，直到结束跳跃。

所述实施例为本发明的优选的实施方式，但本发明并不限于上述实施方式，在不背离本发明的实质内容的情况下，本领域技术人员能够做出的任何显而易见的改进、替换或变型均属于本发明的保护范围。

Claims (7)

1.一种仿人机器人连续动态稳定跳跃控制方法，其特征在于：

建立基于浮动基的仿人机器人全身动力学方程；

规划仿人机器人跳跃轨迹，包括起跳阶段、空中阶段和稳定落地阶段；起跳阶段中，仿人机器人的期望外力为：

f_x＝k_s*v_d；稳定落地阶段中，仿人机器人的期望外力为f_x＝0、

建立仿人机器人跳跃控制方法：对浮动基的广义自由度进行分解，将仿人机器人全身动力学方程表示为

通过仿人机器人期望的状态，得到仿人机器人所期望的脚底受力F₁与F₂，进一步获取仿人机器人下肢各关节驱动力矩τ，将所述驱动力矩τ施加至仿人机器人下肢，根据规划仿人机器人跳跃轨迹，实现仿人机器人连续动态稳定跳跃控制；

其中，k_{p_jump}与k_{v_jump}分别为仿人机器人竖直方向期望外力的PD控制参数，q₂为仿人机器人起跳时刻的膝关节角度，q_{2_jump}为仿人机器人起跳时刻的期望膝关节角度，

为仿人机器人起跳时刻的膝关节角速度，f_z为仿人机器人上身施加的竖直方向力，f_x为仿人机器人水平方向期望受力，k_s为仿人机器人水平方向期望外力与上身期望力矩的PD控制参数，v_d为仿人机器人水平方向期望的运动速度，z为仿人机器人在竖直方向的位置，

为仿人机器人在竖直方向的速度，z_init为仿人机器人初始跳跃高度，k_p-landing和k_vz分别为仿人机器人落地时竖直方向期望外力的PD控制参数，M_ij为仿人机器人的质量矩阵，q_f为浮动基的6个广义自由度，q_l为仿人机器人双腿的关节角度向量，h_i为仿人机器人重力、科里奥利力矩阵，J_i1与J_i2为从仿人机器人脚底转换到仿人机器人关节空间的雅克比转换矩阵，i、j＝1，2。

2.根据权利要求1所述的仿人机器人连续动态稳定跳跃控制方法，其特征在于，在跳跃的过程中，引入仿人机器人稳定的控制条件：

p_x、p_y分别为浮动基∑_f在世界坐标系∑_w中沿着x、y方向的位置，f_Lz和f_Rz分别为所述F₁和F₂在z方向的分量，[p_Rx p_Ry]和[p_Lx p_Ly]分别为根据仿人机器人左右脚的期望接触力和力矩计算的ZMP位置。

3.根据权利要求2所述的仿人机器人连续动态稳定跳跃控制方法，其特征在于，在跳跃的过程中，仿人机器人的ZMP需满足如下约束：Lx_min+δ_rx＜p_x＜Lx_max-δ_ax和Ly_min+δ_ry＜p_y＜Ly_max-δ_ay，其中Lx_min、Lx_max为仿人机器人脚底与地面形成的支撑多边形沿x方向的最小值与最大值，δ_rx与δ_ax分别为沿x方向的补偿量，Ly_min、Lx_max为仿人机器人脚底与地面形成的支撑多边形沿y方向的最小值与最大值，δ_ry与δ_ay分别为沿y方向的补偿量。

4.根据权利要求1所述的仿人机器人连续动态稳定跳跃控制方法，其特征在于，所述仿人机器人连续动态稳定跳跃控制具体为：对仿人机器人下肢施加驱动力矩τ，仿人机器人进入起跳阶段，当仿人机器人上身位置到达起跳高度，进入空中阶段，控制仿人机器人各关节达到落地时的期望姿态，当仿人机器人从空中下落并与地面接触时，进入落地阶段，完成一个跳跃循环，并且返回机器人跳跃的初始状态。

5.根据权利要求1所述的仿人机器人连续动态稳定跳跃控制方法，其特征在于，所述仿人机器人全身动力学方程为：

其中F_i为仿人机器人双脚与地面产生的接触力和力矩。

6.根据权利要求1所述的仿人机器人连续动态稳定跳跃控制方法，其特征在于，所述仿人机器人跳跃轨迹包括躯干的运动轨迹与脚踝的运动轨迹，具体为：

其中H₀为仿人机器人空中阶段质心能达到的最高高度与起跳时的质心高度的高度差，v₀为仿人机器人起跳时刻的质心速度，g为重力加速度，

为仿人机器人的质心加速度，M为仿人机器人整体质量。

7.根据权利要求1所述的仿人机器人连续动态稳定跳跃控制方法，其特征在于，所述对浮动基的广义自由度进行分解，具体为：q_f＝[p_x p_y p_z θ_x θ_y θ_z]^T，其中p_x、p_y、p_z分别为浮动基∑_f在世界坐标系∑_w中沿着x、y、z三个方向的位置，θ_x、θ_y、θ_z分别为浮动基∑_f在世界坐标系∑_w中沿着x、y、z三个方向的姿态。

Images