基于支撑线运动分解的四足机器人trot步态本体位姿控制方法

摘要

一种基于支撑线运动分解的四足机器人trot步态本体位姿控制方法，其步骤为：S1：将机器人分别投影到径向平面和法向平面上；所述径向平面为通过支撑腿足端点连线并且与水平面垂直的平面；所述法向平面为通过本体质心并且垂直于径向平面的平面；S2：将机器人本体的期望速度使用与步骤S1相同的方法投影到径向平面和法向平面上，得到两个投影平面上的本体期望速度；S3：分别建立简化模型的运动学和动力学方程，通过采用控制器使得两个投影平面上的本体速度跟踪各自的期望轨迹，再进行速度合成，完成对机器人本体的速度和姿态的控制。本发明具有通用性强、控制效果好、控制精度高等优点。

说明书

技术领域

本发明主要涉及到机器人运动控制技术领域，特指一种基于支撑线运动分解的四足机器 人trot步态本体位姿控制方法。

背景技术

四足机器人是一种模仿自然界中四足哺乳动物的骨骼结构和行走步态的机器人，充分结 合了仿生学和机器人学的先进技术，其最大的优点是对山地、陡坡、沙漠等非结构化的地形 环境有极强的适应能力，可以用于在危险环境下执行巡逻，物资运输等一些复杂任务，拥有 广阔的应用前景。

四足机器人一般由一个本体和四条仿生腿构成，每条仿生腿由2～3个前向关节和一个侧 向关节组成，以保证足端点的运动有三个自由度。目前比较成熟的四足机器人的关节驱动器 分为两种：液压驱动器和电机驱动器。通过对四足哺乳动物的运动方式的学习、分析、模仿， 四足机器人的移动主要采用以下三种典型步态：WALK步态(爬行步态)、TROT步态(对角 小跑步态)和BOUNDING步态(奔跑步态)。

在四足机器人以TROT步态行进过程中，对角两条腿的运动相同，机器人依靠两组对角 腿按照一定的规律在支撑腿模式和摆动腿模式之间周期性切换，完成对本体速度和姿态的控 制。目前，四足机器人运动控制普遍采用以下方法：

1、CPG控制方法；即中枢模式发生器，借助神经元振荡器模型产生周期性信号，控制 腿部运动，这种方法的优点是实现简单，缺点是参数多，而且缺乏明确的物理含义，难以准 确调节。

2、机器学习；借助机器学习的理论和方法，使机器人能够学习四足动物稳定的行走步态， 但是需要大量的训练，很难在短时间适应新的非结构化环境。

3、在线规划；根据机器人当前的传感器信息，计算下一个或者几个步行周期足端点期望 运动轨迹，通过调整关节驱动力矩使得足端点跟踪期望运动轨迹。但是由于非结构化环境的 未知性，可能出现摆动腿与环境提前接触的情况，产生非预期冲击力，影响本体的速度和姿 态。

4、足端接触力控制；通过力传感器反馈足端与环境的接触力信息，设计足端接触力控制 算法，进而控制机器人本体的速度和姿态。这种控制方法的缺点在于在两条支撑腿之间可能 产生内力，在支撑腿切换到摆动腿模式的时刻，内力瞬间释放，对摆动腿和本体的姿态产生 非预期冲击。因此，需要进一步设计控制策略，既能够控制机器人本体的速度和姿态，又能 够有效消除支撑腿之间的内力。

发明内容

本发明要解决的技术问题就在于：针对现有技术存在的技术问题，本发明提供一种通用 性强、控制效果好、控制精度高的基于支撑线运动分解的四足机器人trot步态本体位姿控制 方法。

为解决上述技术问题，本发明采用以下技术方案：

一种基于支撑线运动分解的四足机器人trot步态本体位姿控制方法，其步骤为：

S1：将机器人分别投影到径向平面和法向平面上；所述径向平面为通过支撑腿足端点连 线并且与水平面垂直的平面；所述法向平面为通过本体质心并且垂直于径向平面的平面；

S2：将机器人本体的期望速度使用与步骤S1相同的方法投影到径向平面和法向平面上， 得到两个投影平面上的本体期望速度；

S3：分别建立简化模型的运动学和动力学方程，通过采用控制器使得两个投影平面上的 本体速度跟踪各自的期望轨迹，再进行速度合成，完成对机器人本体的速度和姿态的控制。

作为本发明的进一步改进：所述步骤S1和S2中，在所述径向平面上的投影简化为平面 七连杆模型；通过调整关节驱动力矩使得平面七连杆的质心位置、姿态跟踪期望轨迹。

作为本发明的进一步改进：所述步骤S1和S2中，在所述法向平面上的投影简化为一维 线形倒立摆模型。

作为本发明的进一步改进：所述步骤S3中，通过计算、选择法向平面内摆动腿落足点的 位置，调整每个支撑期开始时刻机器人本体的速度、姿态，控制本体在支撑期结束时刻法向 平面内的速度，与径向平面内机器人本体的速度进行合成，完成对机器人速度的控制。

作为本发明的进一步改进：采用基于足端接触力控制的双环PID控制器，外环利用本体 质心和姿态误差计算期望的足端接触力，且加入支撑腿足端之间内力为零的约束条件，计算 出唯一的期望接触力；内环通过测量实际足端接触力和期望接触力之间的误差计算关节驱动 力矩。

与现有技术相比，本发明的优点在于：

1、本发明的基于支撑线运动分解的四足机器人trot步态本体位姿控制方法，在四足机 器人以TROT步态行进过程中，采用基于速度分解和合成的控制策略可以实现本体的速度姿态 准确跟踪期望轨迹。

2、本发明的基于支撑线运动分解的四足机器人trot步态本体位姿控制方法，所提出的 控制策略能够有效减小四足机器人在行走过程中支撑腿足端之间的内力。

3、本发明的基于支撑线运动分解的四足机器人trot步态本体位姿控制方法，能够提高 四足机器人对非结构化环境的适应能力，实现机器人在不平整地面上的行走。

4、本发明的基于支撑线运动分解的四足机器人trot步态本体位姿控制方法，具有较强 的通用性。控制策略不依赖具体的四足机器人系统，只要构建合适的传感器系统，结合机器 人运动学、动力学模型相关参数，适当调整控制器参数，即可实现本发明所提出的方法。

5、本发明的基于支撑线运动分解的四足机器人trot步态本体位姿控制方法，结构清晰、 层次分明，具有较好的理论和应用价值。

附图说明

图1是四足机器人的系统结构示意图。

图2是本发明在具体应用时四足机器人的径向平面投影示意图。

图3是本发明在具体应用时四足机器人的法向平面投影示意图。

图4是本发明在具体应用实例中的控制流程示意图。

图5是本发明在具体应用实例中的具体计算过程示意图。

图6是本发明方法的流程示意图。

具体实施方式

以下将结合说明书附图和具体实施例对本发明做进一步详细说明。

本发明的方法主要适用于四足机器人，四足机器人的系统结构如图1所示，它包括一个 本体和四条仿生腿，即包括左前腿、右前腿、左后腿、右后腿。四条仿生腿的机械结构、尺 寸相同，左前腿与左后腿，右前腿与右后腿的安装方式相对于本体互为镜像。每条仿生腿包 含四个主动关节(踝关节、膝关节、髋关节、髋部侧向关节)，各主动关节均采用液压作动器 驱动，足端与地面为点接触。每个主动关节装有位移传感器和力传感器，用于检测作动器长 度信息和关节力信息，在足端点通过三维力传感器反馈足端与地面的接触力信息，机器人本 体在惯性空间中的位置、姿态可通过安装在上体的IMU姿态传感器实时检测。

在本发明中，行走步态默认为TROT步态。如图6所示，本发明的基于支撑线运动分解 的四足机器人trot步态本体位姿控制方法，其步骤为：

S1：将机器人分别投影到径向平面和法向平面上；

所述径向平面为通过支撑腿足端点连线并且与水平面垂直的平面；所述法向平面为通过 本体质心并且垂直于径向平面的平面；

在所述径向平面上的投影简化为平面七连杆模型(如图2所示)，在所述法向平面上的投 影简化为一维线形倒立摆模型(如图3所示)；

S2：将机器人本体的期望速度使用与步骤S1相同的方法投影到径向平面和法向平面上， 得到两个投影平面上的本体期望速度；

基于本体的期望前向速度V_d(本体的侧向期望速度为0)，计算两个投影平面内本体的期望 速度。

V_sd＝V_dcosα

V_fd＝V_dsinα

其中V_d为本体的前向期望速度，V_sd为本体在径向平面内的期望速度，V_fd为本体在法向平 面内的期望速度，α为机器人前向速度与径向平面之间的夹角。

S3：分别建立简化模型的运动学和动力学方程，通过采用控制器使得两个投影平面上的 本体速度跟踪各自的期望轨迹，再进行速度合成，以完成对机器人本体的速度和姿态的控制。

以左前腿和右后腿处于支撑期，右前腿和左后腿处于摆动期为例：

TROT步态四足机器人的动力学模型为：

径向平面：

${\tilde{M}}_1{\stackrel{··}{q}}_1+{\tilde{C}}_1{\stackrel{·}{q}}_1+{\tilde{N}}_1={\tilde{J}}_1{F}_e$

$M_2{\stackrel{··}{q}}_2+C_2{\stackrel{·}{q}}_2+N_2=\tau +J_2{F}_e$

其中，q₁＝[X,Y,β]^T表示本体在径向平面内的水平位置、高度和俯仰角，

q₂＝[X_C,Y_C,θ_d1,X_D,Y_D,θ_d2]^T，[X_C,Y_C]^T([X_D,Y_D]^T)为右后腿(左前腿)足端相对于髋关节安装点的位 置，θ_d1(θ_d2)为右后腿(左前腿)前向关节角线性约束，F_e＝[F_xe1,F_ye1,F_xe2,F_ye2]^T为支撑腿足端接 触力向量，τ为支撑腿关节驱动力矩，为惯性矩阵，为哥氏力矩阵,为重力 项，为雅克比矩阵。

法向平面：

$l\left(t\right)=l\left(0\right)\cosh (t/T_c)+T_c\stackrel{·}{l}\left(0\right)\sinh (t/T_c)$

$\stackrel{·}{l}\left(t\right)=l\left(0\right)\sinh (t/T_c)/T_c+\stackrel{·}{l}\left(0\right)\cosh (t/T_c)$

$T_c=\sqrt{Y/g}$

其中，l(t)为本体在法向平面内的水平位置，l(0)为支撑期初始时刻本体在法向平面内的水平 位置，为支撑期初始时刻本体在法向平面内的水平速度。g为重力加速度。

控制器分为内环和外环两部分：

外环控制器：

$F_{ed}={\left({\tilde{J}}_1\right)}^{-1}\left({\tilde{M}}_1\left({\stackrel{··}{q}}_{1d}+{\mathrm{Kp}}_1\left(q_{1d}-q_1\right)+{\mathrm{Kd}}_1\left({\stackrel{·}{q}}_{1d}-{\stackrel{·}{q}}_1\right)+{\mathrm{Ki}}_1{\int }_0^T\left(q_{1d}-q_1\right)dt\right)+{\tilde{C}}_1{\stackrel{·}{q}}_1+{\tilde{N}}_1\right)$

其中q_1d为q₁的期望轨迹，F_ed为支撑腿足端期望接触力，Kp₁，Kd₁，Ki₁为外环控制参数。

内环控制器：

$\tau =-(M_2{\stackrel{··}{q}}_2+C_2{\stackrel{·}{q}}_2+N_2)+J_2\left({\mathrm{Kp}}_2\right(F_{ed}-F_e\left)\right)$

其中，Kp₂为内环控制参数。

t₁时刻线性倒立摆初始位置的计算公式为：

$l\left(t_1^{+}\right)=\left(V_{fd}^{-}\left(t_1+T\right)-V\left(t_1\right)\sin \alpha \cosh \left(T/T_c\right)\right)T_c/\sinh (T/T_c)$

其中，T为单个支撑期时间。

在上述过程中，将机器人投影到径向平面上，简化为一个平面七连杆后，通过调整关节 驱动力矩使得平面七连杆的质心位置(2维)、姿态(1维)跟踪期望轨迹。

在上述步骤S3中，本发明采用基于足端接触力控制的双环PID控制器，外环利用本体 质心和姿态误差计算期望的足端接触力，由于控制量为4维，控制目标为3维，所以加入支 撑腿足端之间内力为零的约束条件，计算出唯一的期望接触力。内环通过测量实际足端接触 力和期望接触力之间的误差计算关节驱动力矩。进一步，如果前向关节个数大于2，也加入 适当约束条件，以保证关节驱动力矩的唯一性。

由于本体的高度在径向平面内可控，所以机器人在法向平面内的投影可以简化为一个一 维线性倒立摆(见图3)。当步态周期固定，机器人本体侧向期望速度为0的情况下，根据一 维倒立摆的运动学和动力学模型，每个支撑期结束时刻机器人的前向速度只与此支撑期开始 时刻机器人本体的初始速度、姿态相关。

为此，本发明通过计算、选择法向平面内摆动腿落足点的位置，在保证机器人稳定的前 提下，调整每个支撑期开始时刻机器人本体的速度、姿态，进而控制本体在支撑期结束时刻 法向平面内的速度，与径向平面内机器人本体的速度进行合成，完成对机器人速度的控制。

在上述步骤S1和步骤S2中，于径向平面内进行建模与控制时，包括以下过程：

四足机器人在径向平面内的投影可以简化为一个七连杆结构，如图2所示(假设左前腿 和右后腿作为支撑腿)。(X,Y)为本体质心在惯性坐标系下的位置，L₀为机器人本体长度，d 为本体的宽度，L₁,L₂,L₃分别为腿部踝关节、膝关节、髋关节的长度，θ₁,θ₂,θ₃为左前腿踝关节 角、膝关节角、髋关节角，θ₄,θ₅,θ₆为右后腿踝关节角、膝关节角、髋关节角，β为本体俯仰 角，O、A、B、C、D分别为本体坐标系原点(即本体质心)、右后腿髋部与本体连接点、左 前腿髋部与本体连接点、右后腿足端点、左前腿足端点，F_xe1,F_ye1,F_xe2,F_ye2分别为C点和D点 受到的接触力。C点和D点的位置坐标为：

X_C＝(X-(L₀/2)cosβ)-L₃cos(θ₆-β)+L₂cos(θ₅-θ₆+β)-L₁cos(θ₄-θ₅+θ₆-β)

(1)

Y_C＝(Y-(L₀/2)sinβ)-L₃sin(θ₆-β)-L₂sin(θ₅-θ₆+β)-L₁sin(θ₄-θ₅+θ₆-β)

X_D＝(X+(L₀/2)cosβ)+L₃cos(θ₃-β)-L₂cos(θ₂-θ₃+β)+L₁cos(θ₁-θ₂+θ₃-β)

(2)

Y_D＝(Y+(L₀/2)sinβ)-L₃sin(θ₃-β)-L₂sin(θ₂-θ₃+β)-L₁sin(θ₁-θ₂+θ₃-β)

基于拉格朗日方程，七连杆结构的动力学模型为：

$M_1{\stackrel{··}{q}}_1+C_1{\stackrel{·}{q}}_1+N_1=J_1{F}_e$

(3)

$M_2{\stackrel{··}{q}}_2+C_2{\stackrel{·}{q}}_2+N_2=\tau +J_2{F}_e$

其中，q₁＝[X,Y,β]^T，q₂＝[X_C,Y_C,θ_d1,X_D,Y_D,θ_d2]^T，F_e＝[F_xe1,F_ye1,F_xe2,F_ye2]^T，τ为对应于θ₁...θ₆的 关节驱动力矩，M₁,M₂为惯性矩阵，C₁,C₂为哥氏力矩阵,N₁,N₂为重力项，J₁,J₂为雅克比矩 阵。

公式(3)中J₁为3*4雅克比矩阵，不能直接求取逆矩阵，本发明出于柔顺性的考虑，加 入支撑腿足端点之间内力为0的约束条件，即：

$\left(\begin{array}{c}{F}_{xe1}-F_{xe2}\\ F_{ye1}-F_{ye2}\end{array}\right)·\left(\begin{array}{c}{X}_D-X_C\\ Y_D-Y_{\partial C}\end{array}\right)=0---\left(4\right)$

根据式(3)和(4)，七连杆系统的动力学模型变为：

${\tilde{M}}_1{\stackrel{··}{q}}_1+{\tilde{C}}_1{\stackrel{·}{q}}_1+{\tilde{N}}_1={\tilde{J}}_1{F}_e$

(5)

$M_2{\stackrel{··}{q}}_2+C_2{\stackrel{·}{q}}_2+N_2=\tau +J_2{F}_e$

其中，为4*4雅克比矩阵。

根据七连杆系统的动力学模型，本发明所采用的基于力内环的双环PID控制器：

外环控制器：

$F_{ed}={\left({\tilde{J}}_1\right)}^{-1}\left({\tilde{M}}_1\right({\stackrel{··}{q}}_{1d}+{\mathrm{Kp}}_1(q_{1d}-q_1)+{\mathrm{Kd}}_1({\stackrel{·}{q}}_{1d}-{\stackrel{·}{q}}_1)+{\mathrm{Ki}}_1{\int }_0^T(q_{1d}-q_1)dt)+{\tilde{C}}_1{\stackrel{·}{q}}_1+{\tilde{N}}_1)---\left(6\right)$

其中q_1d为q₁的期望轨迹，F_ed为支撑腿足端期望接触力。

内环控制器：

$\tau =-(M_2{\stackrel{··}{q}}_2+C_2{\stackrel{·}{q}}_2+N_2)+J_2\left({\mathrm{Kp}}_2\right(F_{ed}-F_e\left)\right)---\left(7\right)$

在上述步骤S1和步骤S2中，于法向平面内建模与控制时，包括以下过程：

由于机器人本体的高度在径向平面内可控，所以四足机器人在法向平面内的投影可以简 化为一个线性倒立摆(如图3所示)。线性倒立摆的运动模型如下：

$l\left(t\right)=l\left(0\right)\cosh (t/T_c)+T_c\stackrel{·}{l}\left(0\right)\sinh (t/T_c)$

$\stackrel{·}{l}\left(t\right)=l\left(0\right)\sinh (t/T_c)/T_c+\stackrel{·}{l}\left(0\right)\cosh (t/T_c)---\left(8\right)$

$T_c=\sqrt{Y/g}$

从公式(8)中可以看出，一个支撑期结束时刻机器人本体的速度在法向平面上的投影只是 和此支撑期开始时刻本体的位置和速度相关。本发明中，在法向平面内的控制策略是通过调 整摆动腿落足点的位置，从而改变下一个支撑期开始时刻本体的位置和速度，进而控制支撑 期结束时刻本体的速度。在一个步态周期内的具体计算过程如下(假设当前时刻为t₀，下一 个支撑期开始时刻为t₁，支撑期时间为T，支撑腿足端点连线与机器人前进方向的夹角为α， 当前支撑腿为左前腿和右后腿，t₁-t₁+T时间内支撑腿为右前腿和左后腿，t₁+T-t₁+2T时间内 支撑腿为左前腿和右后腿，流程图如图4所示，具体解算过程如图5所示)：

第一步：计算t₁+T时刻本体在径向平面和法向平面内的期望速度

$V_{sd}^{-}(t_1+T)=V_d(t_1+T)cos\alpha$

(9)

$V_{fd}^{-}(t_1+T)=V_d(t_1+T)sin\alpha$

V_d(t)、V(t)为t时刻本体前向期望速度、实际速度，上标^-(⁺)表示按照落足前(后)支撑腿 足端连线角度分解径向平面和法向平面。

第二步：基于公式(8)，根据计算时刻线性倒立摆初始位置：

$l\left(t_1^{+}\right)=\left(V_{fd}^{-}\right(t_1+T)-V_f^{+}(t_1\left)\cosh \right(T/T_c\left)\right)T_c/\sin h(T/T_c)---\left(10\right)$

其中，$V_f^{+}\left(t_1\right)=V\left(t_1\right)sin\alpha .$

第三步：计算本体x方向速度轨迹、径向平面内速度轨迹、法向平面内速度轨迹：

$V_f\left(t\right)=l\left(t_1^{+}\right)\sinh \left(\right(t-t_1)/T_c)/T_c+V_f^{+}\left(t_1\right)\cosh \left(\right(t-t_1)/T_c),t\in \left[t_1^{+},t_1^{-}+T\right]---\left(11\right)$

$V\left(t\right)=V_f\left(t\right)/sin\alpha ,t\in \left[t_1^{+},t_1^{-}+T\right]---\left(12\right)$

$V_s\left(t\right)=V\left(t\right)cos\alpha ,t\in \left[t_1^{+},t_1^{-}+T\right]---\left(13\right)$

V_s(t)的控制方法在上述径向平面内建模与控制过程中已有阐述。

第四步：计算时刻摆动腿操作空间落足位置：

右前腿落足点操作空间位置：

$x_{eRF}\left(t_1^{-}\right)=\frac{{\int }_{t_1^{+}}^{t_1^{-}+T}V\left(t\right)dt}{2}---\left(14\right)$

$z_{eRF}\left(t_1^{-}\right)=tan\alpha (\frac{L_0}{2}+x_{eRF}(t_1^{-})-l\left(t_1^{+}\right)/sin\alpha )-d---\left(15\right)$

左后腿落足点操作空间位置：

$x_{eLR}\left(t_1^{-}\right)=\frac{{\int }_{t_1^{+}}^{t_1^{-}+T}V\left(t\right)dt}{2}---\left(16\right)$

$z_{eLR}\left(t_1^{-}\right)=d-l\left(t_1^{+}\right)/cos\alpha -\left(\right(L_0/2-x_{eLR}\left(t_1^{-}\right)\left)tan\alpha \right)---\left(17\right)$

第五步：计算t₁+2T时刻本体在径向平面和法向平面内的期望速度

$V_{sd}^{-}(t_1+2T)=V_d(t_1+2T)cos\alpha$

(18)

$V_{fd}^{-}(t_1+2T)=V_d(t_1+2T)sin\alpha$

第六步：基于公式(8)，根据计算时刻线性倒立摆初始位置：

$l\left(t_1^{+}+T\right)=\left(V_{fd}^{-}\right(t_1+2T)-V_f^{+}(t_1+T\left)\cosh \right(T/T_c\left)\right)T_c/\sinh (T/T_c)---\left(19\right)$

其中，$V_f^{+}(t_1+T)=V(t_1+T)sin\alpha .$

第七步：计算本体x方向速度轨迹、径向平面内速度轨迹、法向平面内速度轨迹：

$V_f\left(t\right)=l\left(t_1^{+}\right)\sinh \left(\right(t-t_1-T)/T_c)/T_c+V_f^{+}\left(t_1\right)\cosh \left(\right(t-t_1-T)/T_c),t\in \left[t_1^{+}+T,t_1^{-}+2T\right]---\left(20\right)$

$V\left(t\right)=V_f\left(t\right)/sin\alpha ,t\in \left[t_1^{+}+T,t_1^{-}+2T\right]---\left(21\right)$

$V_s\left(t\right)=V\left(t\right)cos\alpha ,t\in \left[t_1^{+}+T,t_1^{-}+2T\right]---\left(22\right)$

V_s(t)的控制方法与第三步相同。

第八步：计算时刻摆动腿操作空间落足位置：

左前腿落足点操作空间位置：

$x_{eLF}(t_1^{-}+T)=\frac{{\int }_{t_1^{+}+T}^{t_1^{-}+2T}V\left(t\right)dt}{2}---\left(23\right)$

$z_{eLF}(t_1^{-}+T)=d-tan\alpha (\frac{L_0}{2}+x_{eLF}(t_1^{-}+T)-l\left(t_1^{+}+T\right)/sin\alpha )---\left(24\right)$

右后腿落足点操作空间位置：

$x_{eRR}(t_1^{-}+T)=\frac{{\int }_{t_1^{+}+T}^{t_1^{-}+2T}V\left(t\right)dt}{2}---\left(25\right)$

$z_{eLR}(t_1^{-}+T)=-d+l\left(t_1^{+}\right)/cos\alpha +\left(\right(L_0/2-x_{eLR}\left(t_1^{-}+T\right)\left)tan\alpha \right)---\left(26\right)$

以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于 本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术 人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。