CN113771046B - 一种最小化Jerk指标摆动轨迹规划方法 - Google Patents

Abstract

本发明提出一种最小化Jerk指标摆动轨迹规划方法，使用当前机器人估计质心速度与期望速度规划摆动落足点，摆动轨迹规划以最小化轨迹Jerk为目标，同时使用当前质心速度与期望速度作为轨迹始末的速度约束，从而规划出满足机器人本身速度状态且加速度平滑连续的摆动轨迹，基于最优Jerk目标保证轨迹总体冲击积分最小。

Description

一种最小化Jerk指标摆动轨迹规划方法

技术领域

本发明属于机器人运动控制技术领域，具体涉及一种适用于机器人的最小化Jerk指标摆动轨迹规划方法。

背景技术

四足机器人摆动相控制决定机器人是否能够按期望速度进行移动，足端位置轨迹需要实时进行规划并能够基于机器人本体速度与期望速度进行在线调节。现有摆动轨迹规划方法仅能够保证轨迹始末点位置和摆动高度，以目前四足机器人常用摆线轨迹为例，其速度和加速度往往不连续，存在大角度拐点。以力控四足机器人为例，如果加速度存在拐点会对执行器带来较大冲击；以电驱动四足机器人为例，其加速度方向切换时电机也需要换向，高频换向往往会对减速与传动机构带来损坏。因此，需要提出一种新的摆动轨迹规划方法，以在保证加速度平滑时满足位置约束。

发明内容

(一)要解决的技术问题

本发明的目的是提出一种最小化Jerk指标摆动轨迹规划方法，以解决如何在保证加速度平滑时满足位置约束的技术问题。

(二)技术方案

为解决上述技术问题，本发明提出一种最小化Jerk指标摆动轨迹规划方法，其特征在于，该摆动轨迹规划方法包括如下步骤：

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

i＝1…4

其中，

为机器人第i条腿在{N}系下的落足点位置，/>

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

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

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

i＝1…4

其中，

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

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

i＝1…4

其中，

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

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

其中，

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

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

i＝1…4

其中，

为第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为轨迹总时间。

(三)技术效果

本发明提出一种最小化Jerk指标摆动轨迹规划方法，使用当前机器人估计质心速度与期望速度规划摆动落足点，摆动轨迹规划以最小化轨迹Jerk为目标，同时使用当前质心速度与期望速度作为轨迹始末的速度约束，从而规划出满足机器人本身速度状态且加速度平滑连续的摆动轨迹，基于最优Jerk目标保证轨迹总体冲击积分最小。

附图说明

图1为本发明实施例中摆动轨迹规划方法流程图；

图2为本发明实施例中机器人坐标系描述：(a)机器人右视图，(b)机器人俯视图({H}为胯关节坐标系，{B}为机体坐标系)；

图3(a)为本发明实施例中规划的摆动轨迹结果，图3(b)为摆线轨迹与规划的X加速度曲线，图3(c)为Z加速度曲线。

具体实施方式

为使本发明的目的、内容和优点更加清楚，下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。

本实施例提出一种最小化Jerk指标摆动轨迹规划方法，主要流程如图1所示，具体包括如下步骤：

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

i＝1…4

其中，

为机器人第i条腿在{N}系下的落足点位置，/>

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

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

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

i＝1…4

其中，

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

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

i＝1…4

其中，

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

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

其中，

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

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

i＝1…4

其中，

为第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为轨迹总时间。

基于本发明进行摆动轨迹规划的结果如图3所示。其中，图3(a)给出本发明与传统摆线轨迹规划的三维曲线结果，本发明和传统方法均能满足起点、中点、终点的轨迹位置约束；图3(b)和3(c)分别为X轴和Z轴轨迹加速度规划结果，可以看出本发明在轨迹中点时加速度能实现连续过度，而传统摆线轨迹会出现加速度突变。

上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

Claims (1)

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为轨迹总时间。

Images