[发明专利]一种面向未知崎岖地形的四足机器人运动规划方法

权利要求书

1.一种面向未知崎岖地形的四足机器人运动规划方法，其特征在于：四足机器人在面向崎岖地形时采用爬行步态，爬行步态四条腿的摆腿顺序为：前左腿FL→后右腿HR→前右腿FR→后左腿HL；

将四足机器人爬行步态的运动周期按照上述的摆腿顺序划分为六个状态相：FL摆动相、四足支撑相、HR摆动相、FR摆动相、四足支撑相、HL摆动相；在单腿摆动相，摆动腿沿着规划的足端轨迹进行摆动，同时，机器人的重心通过支撑腿的运动沿着规划的路径进行前向运动；在四足支撑相，机器人的重心在沿着规划的路径进行前向运动的同时进行侧向移动；

设计状态机，在单腿摆动相，摆动腿落地触发下一状态相；在四足支撑相，以当前所规划的四足支撑相的时间触发下一状态相；状态相一旦发生变化，机器人将会通过摆动腿的落地规划和感知策略估计未知地形的参数，并结合当前自身运动状态对新状态相进行在线的运动规划；在当前状态相的整个过程中，机器人一直执行当前状态相初始时刻运动规划所生成的轨迹，直到当前状态相结束；机器人按照前述的状态机依次进入下一个状态相，直至完成一个完整的运动周期，然后再进入下一运动周期，具体包含以下步骤：

步骤一：上一状态相结束，机器人进入新状态相；更新支撑域，根据当前支撑域，基于COG稳定性判据，进行身体重心的路径规划；该步骤具体包括以下子步骤：

(1.1)上一状态相结束，机器人进入新状态相；

(1.2)若当前状态相是因为摆动腿落地而触发，根据刚落地的摆动腿落脚点位置更新当前支撑域；

(1.3)根据运动目标，设定下一摆动腿的步长，预估下一摆动腿落脚点位置，得到下一支撑域；

(1.4)计算当前支撑域和下一支撑域的内心位置，以当前支撑域内心为起点，下一支撑域内心为终点，基于COG稳定性判据，规划一条连续的路径；

步骤二：根据步骤一所规划的路径，基于ZMP稳定性判据，进行身体重心的轨迹规划；

步骤三：若当前状态相是因为摆动腿落地而触发，以摆动腿落脚点表征未知地形，更新支撑足，并根据当前支撑足的相对位置估计未知地形的参数，规划机器人躯体质心高度以及躯体姿态以自适应地形高度与坡度的变化；该步骤具体包括以下子步骤：

(3.1)若当前状态相是因为摆动腿落地而触发，以摆动腿落脚点表征未知地形；

(3.2)地形高度的变化会导致落脚点竖直位置发生变化进而影响机器人身体高度，反过来，通过计算机器人身体高度的变化量，以表征未知地形的高度变化；

(3.3)根据刚落地的摆动腿更新当前支撑足，以支撑足所在的斜面表征未知地形的坡度；在世界坐标系中，通过三个支撑足的相对位置，计算支撑足斜面的单位法向量则未知地形的坡度估计方程为：

其中，θ_Roll、θ_Pitch、θ_Yaw分别为机器人的期望滚动角、期望俯仰角、期望偏摆角，绕Z轴旋转θ角的旋转矩阵

(3.4)根据机器人身体高度的变化量，规划机器人躯体质心高度以自适应未知地形的高度变化；

(3.5)根据未知地形的坡度估计值(θ_Pitch,θ_Roll)规划机器人躯体姿态，使机器人以平行于未知地形斜面的躯体姿态自适应未知地形的坡度变化；

步骤四：若当前状态相是单腿摆动相，指定摆动腿进行足端轨迹规划；

步骤五：机器人处于当前状态相，执行上述运动规划所生成的轨迹，直到触发下一状态相，转到步骤一，重复整个过程，持续运动直至机器人到达目标位姿点。

2.根据权利要求1所述的一种面向未知崎岖地形的四足机器人运动规划方法，其特征在于，所述步骤二具体包括以下子步骤：

(2.1)采用ZMP动态稳定性判据；

(2.2)采用cart-table ZMP模型，考虑前向x方向的运动：

其中，g是重力加速度，z为竖直方向；同理可得侧向y方向的y_ZMP；

在每时每刻，都必须保证ZMP位于支撑域内；

(2.3)约束当前轨迹的初始位置、速度、加速度分别等于上一规划轨迹的末端位置、速度、加速度。