[发明专利]一种基于双阈值法ZMP的六足机器人失稳调节算法

权利要求书

1.一种基于双阈值法ZMP的六足机器人失稳调节算法，其特征在于，包括以下步骤：

S1，设定足端压力阈值N1和机身姿态角的阈值N2，当六足机器人足端压力的输出值大于N1且机身姿态角大于N2时，则判定为机器人失稳状态，否则为稳定状态，稳定状态下的六足机器人以三足行走步态运动；

S2，当六足机器人被判定为失稳状态后，将三足行走步态转换为五足行走步态，六足机器人的六条腿单个依次循环运动，无论何时都至少有五条腿支撑地面；

S3，同时利用ZMP法构建六足机器人稳定判据，通过得到的六足机器人机身的姿态信息，经过计算得到六足机器人稳定判据，作为对六足机器人机身姿态调整的依据；

S4，再对ZMP稳定裕量所对应边涉及到的两条腿进行调整，使六足机器人机身抬高或降低，从而使稳定裕量恢复到安全地范围内，再重复步骤S1-S4，使六足机器人保持稳定的行走步态。

2.根据权利要求1所述的一种基于双阈值法ZMP的六足机器人失稳调节算法，其特征在于，步骤S1中，机身姿态角包括翻滚角θ和俯仰角φ，与之对应的阈值N2包括翻滚角阈值θ_N2与俯仰角阈值φ_N2。

3.根据权利要求2所述的一种基于双阈值法ZMP的六足机器人失稳调节算法，其特征在于，所述翻滚角θ和俯仰角φ为：

式中，gx,gy,gz为六足机器人机身三轴加速度计的输出值。

4.根据权利要求3所述的一种基于双阈值法ZMP的六足机器人失稳调节算法，其特征在于，翻滚角Ф与俯仰角φ任意一个超出所设定的阈值时，六足机器人均为失稳状态。

5.根据权利要求1或2或3或4所述的一种基于双阈值法ZMP的六足机器人失稳调节算法，其特征在于，步骤S3中，阈值N1和阈值N2判定顺序为：当足端压力在阈值N1的范围内时，不对三轴加速度计Z轴输出量进行判断，判定为六足机器人为稳定状态；当足端压力超出阈值N1时，才对三轴加速度计Z轴输出量进行判断，若三轴加速度计Z轴输出量在阈值N2的范围内，则判定为六足机器人为稳定状态，否则判定为六足机器人为失稳状态。

6.根据权利要求1所述的一种基于双阈值法ZMP的六足机器人失稳调节算法，其特征在于，所述ZMP稳定裕量的获得方法包括以下步骤：

(1)根据当前姿态下六足机器人与地面所接触点形成的支撑多边形各顶点坐标，求得ZMP零力矩点的坐标；

(2)根据ZMP零力矩点的坐标求得ZMP零力矩点至支撑多边形各边的距离；

(3)通过比较得到的最小值即为六足机器人的ZMP稳定裕量。

7.根据权利要求6所述的一种基于双阈值法ZMP的六足机器人失稳调节算法，其特征在于，所述六足机器人机身姿态调整的依据为S_m＞0，S_m为ZMP零力矩点到支撑多边形的最短距离。