[发明专利]一种最小化Jerk指标摆动轨迹规划方法

权利要求书

1.一种最小化Jerk指标摆动轨迹规划方法，其特征在于，所述摆动轨迹规划方法包括如下步骤：

S1.已知四足机器人当前质心高度、质心速度，结合给定的期望质心速度，根据以下公式规划在全局坐标系{N}下的落足点位置：

其中，为机器人第i条腿在{N}系下的落足点位置，为机器人第i个胯关节在{N}系下的位置，p_cog,z为机器人当前质心高度，v_cog为机器人当前质心速度，v_cog,d为期望质心速度，T_s为支撑相时间，g为重力加速度；

S2.基于摆动控制坐标系描述对机器人第i条腿的足端位置进行坐标系转换

S2-1.基于机器人结构参数与单腿构型，使用关节角度反馈值，采用运动学正解计算第i条腿足端在胯关节坐标系{H}下的三维位置：

其中，为第i条腿足端在{H}系下的三维位置，f(c₁,c₂,c₃)为对应单腿构型的运动学正解计算函数，根据以下公式进行运动学正解计算，其中c₁为机器人大腿角度，c₂为小腿角度，c₃为胯关节角度，L₁为大腿长度，L₂为小腿长度，L₃为侧摆电机相对胯关节的偏差：

S2-2.基于机器人左右两侧腿胯关节间距和前后两侧腿胯关节间距，将第i条腿足端在{H}系下的三维位置转化为机体坐标系{B}下的三维位置：

其中，为第i条腿足端在{B}系下的三维位置，W为机器人左右两侧腿胯关节间距，H为机器人前后两侧腿胯关节间距；

S2-3.基于机器人机载IMU测量得到的姿态四元数，获取当前{N}系与{B}系间的转换矩阵：

其中，为{N}系与{B}系间的转换矩阵，q₀、q₁、q₂、q₃为机器人当前的姿态四元数；

根据以下公式，将第i条腿足端在{B}系下的三维位置转换为{N}系下的三维位置：

其中，为第i条腿足端在{N}系下的三维位置，为的转置；

S2-4.设定期望摆动高度H_sw，根据以下公式计算得到第i条腿在{N}系下的摆动中点位置：

其中，为第i条腿在{N}系下的摆动中点位置，为第i条腿在{N}系下的摆动起始位置；

S3.基于摆动起始位置、摆动中点位置、落足点位置，构建第i条腿的摆动轨迹位置约束：

其中，P_st为摆动轨迹起始点的位置约束，P_mid为摆动轨迹起中点的位置约束，P_end为摆动轨迹结束点的位置约束；

基于当前质心速度v_cog与期望质心速度v_cog,d，根据以下公式构建摆动轨迹速度约束；在摆动轨迹轨迹中点不设具体速度值的约束，以保证摆动轨迹中点前后两段轨迹的速度连续，即：

其中，v_st为摆动轨迹起始点的速度约束，v_end为摆动轨迹结束点的速度约束；

S4.在确定摆动轨迹起点、中点和结束点的位置约束与速度约束后，以最小轨迹Jerk求解作为轨迹曲线函数的三次多项式的最优化多项式系数，保证轨迹加速度连续，构建连续前馈力矩，减小机器人摆动控制中的电机冲击；

S5.摆动轨迹由前后两段组成，采用步骤S3中的摆动轨迹位置约束条件和速度约束条件构建如下关系式：

其中，n为轨迹函数阶次，t₀为摆动轨迹前半段时间，t₁为摆动轨迹中点时间，t₂为摆动轨迹后半段时间；

S6.求解步骤S5中关系式的未知项X，X作为轨迹对应的多项式系数，以最小化Jerk为优化目标求取最优的轨迹参数

根据以下公式，对摆动轨迹前后两段的Jerk值分别进行积分，作为优化目标：

其中，T为该段轨迹的时间，k为轨迹分段数量；

S7.基于优化目标，采用极小值原理进行参数求解，最优参数对应的轨迹与时间函数S*(t)如下：

其中，p₀、v₀、a₀为该段轨迹起始点的位置、速度与加速度；

其中，为最优轨迹多项式系数，结合已知轨迹终点的位置p_e、速度v_e和加速度a_e，则多项式系数为：

S8.基于步骤S6-S7，根据以下公式求解出摆动轨迹前后两段曲线对应的多项式系数[α_st-mid β_st-mid γ_st-mid]和[α_mid-end β_mid-end γ_mid-end]：

结合轨迹多项式函数，得到摆动轨迹前半段和后半段任意时刻t的位置、速度和加速度：

其中，[p_st v_st a_st]为摆动轨迹开始时的位置、速度和加速度，[p_mid v_mid a_mid]为摆动轨迹中点的位置、速度和加速度，[p_end v_end a_end]为摆动轨迹终点的位置、速度和加速度，T_s为轨迹总时间。