[发明专利]一种仿人机器人摔倒检测和稳定控制方法

权利要求书

1.一种仿人机器人摔倒检测和稳定控制方法，包括：

步骤1，计算仿人机器人运动过程中的ZMP期望轨迹，测量ZMP实际轨迹，并计算二者之间的误差；

步骤2，计算仿人机器人运动过程中期望的身体姿态，测量仿人机器人运动过程中实际的身体姿态，并计算两者的误差；

步骤3，设定仿人机器人ZMP轨迹误差阈值D₁，仿人机器人身体姿态向前倾倒时的阈值D₂，以及仿人机器人身体姿态向后倾倒时的阈值D₃；通过设定的阈值范对机器人当前的稳定状态进行判定。

2.根据权利要求1所述的一种仿人机器人摔倒检测和稳定控制方法，其特征在于，所述步骤1中的仿人机器人运动过程中的期望ZMP轨迹[p_xr p_yr]由所期望的关节运动轨迹，以及全身所有组成元件的动力学参数计算，公式如下：

其中[x_i y_i z_i]为仿人机器人第i个连杆的质心位置，p_z为地面的高度，当仿人机器人在地面上时，p_z＝0。

3.根据权利要求1所述的一种仿人机器人摔倒检测和稳定控制方法，其特征在于，所述步骤1中仿人机器人运行过程中的实际ZMP位置[p_x p_y]通过固定在双足的六维力/力矩传感器获得，计算公式如下：

其中[p_Rx p_Ry]和[f_Rx f_Rx f_Rx]为安装在右脚的六维力/力矩传感器测量的ZMP位置和三个方向的受力，[p_Lx p_Ly]和[f_Lx f_Lx f_Lx]为安装在左脚的六维力/力矩传感器测量的ZMP位置和三个方向的受力。

4.根据权利要求1所述的一种仿人机器人摔倒检测和稳定控制方法，其特征在于，所述步骤2中仿人机器人期望的身体姿态根据仿人机器人当前的关节角度和正运动学计算获得：

θ_r＝Forward_k ine(q(q₁,q₂,...,q_n))

其中q_n为仿人机器人关节电子码盘读取的关节角度。

5.根据权利要求1所述的一种仿人机器人摔倒检测和稳定控制方法，其特征在于，所述步骤2中仿人机器人运动过程中实际的身体姿态通过安装在仿人机器人胸腔的IMU传感器实时监测躯干关于世界坐标系的绝对姿态θ获得。

6.根据权利要求1所述的一种仿人机器人摔倒检测和稳定控制方法，其特征在于，所述步骤3中，当仿人机器人实际的ZMP轨迹处于仿人机器人的脚底，并且与参考ZMP轨迹的距离小于D₁，认为仿人机器人处于平衡状态，不需要调整仿人机器人的状态。

7.根据权利要求1所述的一种仿人机器人摔倒检测和稳定控制方法，其特征在于，所述步骤3中，当仿人机器人绕着脚尖转动时，如果身体的实际姿态与期望姿态的差值小于D₂时，判定机器人可以通过抗扰动控制来恢复平衡，当差值大于D₂时，判定机器人将会发生向前摔倒。

8.根据权利要求1所述的一种仿人机器人摔倒检测和稳定控制方法，其特征在于，所述步骤3中，当仿人机器人绕着脚跟转动时，如果身体的实际姿态与期望姿态的差值小于D₃时，判定机器人可以通过抗扰动控制来恢复平衡，当差值大于D₃时，判定机器人将会发生向后摔倒。