基于姿态控制的四足机器人Trot步态行走仿真

doi:10.12677/MOS.2023.126459

期刊菜单

基于姿态控制的四足机器人Trot步态行走仿真
Gait Simulation of Quadruped Robot Trot Based on Attitude Control

DOI: 10.12677/MOS.2023.126459, PDF, HTML, XML, 下载: 257 浏览: 466
作者: 方鸿磊, 李龙坤：上海理工大学，机械工程学院，上海
关键词: 四足机器人；Trot步态；MATLAB机器人工具箱；MATLAB/Simulink；姿态控制；Quadruped Robot； Trot Gait； Robotic Toolbook for MATLAB； MATLAB/Simulink； Attitude Control

摘要: 本文对四足机器人姿态控制方法下的trot步态行走进行研究。首先，对机器人的运动学模型进行分析，利用改进型的D-H坐标法建立了正运动学模型，并使用MATLAB机器人工具箱对正运动学分析内容进行验证。接着利用几何法对机器人的单腿逆运动学进行求解，并在MATLAB/Simulink中搭建模型对逆运动学内容进行验证。其次，根据足端零冲击原理，利用五次多项式轨迹规划，实现了机器人的平稳运动。针对行走时机身的不稳定问题，提出了一种姿态控制方法，并创新性的将其应用于trot步态行走控制策略之中，以保证行走时姿态的稳定。最后在MATLAB/Simulink中搭建仿真平台，对该控制策略的有效性和优越性进行了仿真验证。

Abstract: This article studies the gait walking and attitude control methods of a quadruped robot trot. Firstly, the kinematic model of the robot was analyzed, and an improved D-H coordinate method was used to establish a forward kinematic model. The content of forward kinematic analysis was validated using the MATLAB robot toolbox. Then, the geometric method is used to solve the inverse kinemat-ics of the robot’s single leg, and a model is built in MATLAB/Simulink to verify the inverse kinemat-ics content. Secondly, based on the principle of zero impact at the foot end, the robot's smooth mo-tion was achieved through trajectory planning using a quintic polynomial. For attitude control, in-depth research is conducted on its principles and applied to the trot gait control strategy to en-sure stable posture during walking. Finally, a simulation platform was built in MATLAB/Simulink to verify the effectiveness and superiority of the control strategy.

文章引用：方鸿磊, 李龙坤. 基于姿态控制的四足机器人Trot步态行走仿真[J]. 建模与仿真, 2023, 12(6): 5046-5062. https://doi.org/10.12677/MOS.2023.126459

1. 引言

近年来，机器人领域取得了显著的发展，各种技术的融合赋予了机器人更多样化的任务。尤其是随着仿生学在机器人领域的广泛应用，四足机器人作为一种移动式机器人，展现出了引人注目的优势，由于其拥有较强的运动能力，可以在多种复杂地形上实现多步态行走，适应各种复杂的工作环境。相较于双足机器人，四足机器人能够运行的场合更为广泛，运行更加稳定，承载能力也更大。相对于六足机器人，四足机器人的机械结构的设计方面就更为简单，且在运动方面控制起来更为容易。

在四足步态的研究中，研究者们发现对角步态(trot)作为中低速步态，具备多重优势。不仅能够在广泛的速度范围内调整四足机器人的运动速度，还展现出较高的能量效率和可行的运动控制性能。正是这些特点，使得对角步态控制方法成为当前备受瞩目的研究方向之一。在trot步态的基础之上，波士顿动力(Boston Dynamics)公司 [1] 的四足机器人spot使用类似于模型预测控制(Model Predictive Control, MPC)的方法能够考虑机器人的动力学特性和环境变化，以实现稳定性和适应性更强的运动来实现动态行走、跑步和导航；麻省理工学院(Massachusetts Institute of Technology, MIT)的四足机器人Cheetah 3 [2] ，类似于波士顿动力的Cheetah，MIT的Cheetah 3也使用了模型预测控制等先进方法来实现高效的奔跑和跳跃动作；ETH Zurich (苏黎世联邦理工学院)机器人实验室一直在四足机器人领域进行着深入研究。他们的机器人在不同场景中表现出色，使用了虚拟模型控制等先进技术。他们开发了基于VMC的四足机器人ANYmal，并将其应用于工业、勘探和搜救等领域。

本文利用开源的四足机器人ANYmal模型对其姿态控制下的trot步态行走 [2] 进行研究，首先对该四足机器人模型进行正运动学建模，并通过MATLAB的机器人工具箱对正运动学的正确性进行验证，接着利用几何法求解了单腿逆运动学 [3] ，并搭建Simulink模型进行仿真验证，其次根据三条防冲击条件，进行了足端的五次多项式轨迹规划 [3] ，接着对四足机器人姿态控制方法进行研究分析，并将其应用于四足机器人trot步态行走之中，最后在MATLAB的Simulink中搭建了四足机器人仿真平台，进行四足机器人trot步态仿真行走试验，在对四足机器人行走时的机身姿态以及位置数据进行详细分析之后得出，基于姿态控制的方法能够很好的保证四足机器人在trot步态下的运动平稳性。

2. 四足机器人运动学分析

正确的运动学分析为四足机器人提供了在三维空间中的精确描述和数学建模，是使机器人运动起来的基础，它允许机器人控制系统理解和计算机器人的运动，从而使机器人能够执行各种任务，是接下来的轨迹跟踪和姿态控制的研究基础(轨迹规划和姿态控制方法将在后文阐述)。

2.1. 四足机器人结构分析

四足机器人整体构型如图1所示，整体结构主要由机身、侧摆关节、髋关节，膝关节以及足端组成，该四足机器人为前肘后膝的腿部构型，位于前方的两条腿部构型保持一致同为肘型，而位于后方的两条腿部构型保持一致同为膝型，单腿共具有3个自由度，机身拥有3个自由度，则四足机器人整机共有15个自由度，这些自由度的存在保证了四足机器人具有稳定行走的能力。

Figure 1. Overall configuration diagram of quadruped robot

图1. 四足机器人整体构型图

2.2. 四足机器人正运动学分析

本文采用改进型的D-H坐标系建立方法 [3] 确定了机器人机身以及腿部关节坐标系之间的关系，D-H坐标的建立便于我们求解出对应姿态下的足端位姿，为后续的足端轨迹规划做好准备。四条腿按照左前腿(Left fore, LF)–左后(Left hide, LH)–右前(Right fore, RF)–右后(Right hide, RH)的顺序进行编号，并定义主要坐标系如下：{W}为世界坐标系；{B}为躯干质心(或形心)坐标系；{0}为腿部基坐标系；{1}为腿部侧摆关节坐标系；{2}为腿部髋关节坐标系；{3}为腿部膝关节坐标系；{4}为腿部足端坐标系。因为各腿结构参数相同，这里仅对左前腿进行运动学分析和D-H坐标的建立，建立好关节坐标系的四足机器人结构简图如图2所示。

Figure 2. Structure diagram of quadruped robot

图2. 四足机器人结构简图

其中侧摆关节的转角用 $θ_{1}$ 表示、髋关节的转角用 $θ_{2}$ 表示和膝关节的转角用 $θ_{3}$ 表示，侧摆连杆的长度用L₁表示、大腿连杆的长度用L₂表示、小腿连杆的长度用L₃来表示，则可根据图2中所建立起的关节坐标系之间的连杆长度、关节转角以及关节之间的高度差的几何关系构建起如表1所示的改进型D-H参数表。

Table 1. Improved D-H parameter table

表1. LF腿改进型D-H参数表

将得到的D-H表中的各项参数值代入齐次变换矩阵公式(1)中可分别得到：第二个关节坐标系相对于第一个关节坐标系的齐次变换矩阵、第三个关节坐标系相对于第二个关节坐标系的齐次变换矩阵、第四个关节坐标系相对于第三个关节坐标系的齐次变换矩阵如下：

${}_{i}^{i - 1}T = [\begin{matrix} \cos θ_{i} & - \sin θ_{i} & 0 & a_{i - 1} \\ \sin θ_{i} \cos \partial_{i - 1} & \cos θ_{i} \cos \partial_{i - 1} & - \sin \partial_{i - 1} & - d_{i} \sin \partial_{i - 1} \\ \sin θ_{i} \sin \partial_{i - 1} & \cos θ_{i} \sin \partial_{i - 1} & \cos \partial_{i - 1} & d_{i} \cos \partial_{i - 1} \\ 0 & 0 & 0 & 1 \end{matrix}]$ (1)

${}_{1}^{0}T = [\begin{matrix} \cos θ_{1} & - \sin θ_{1} & 0 & 0 \\ \sin θ_{1} & \cos θ_{1} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}]$ (2)

${}_{2}^{1}T = [\begin{matrix} \cos θ_{2} & - \sin θ_{2} & 0 & L_{1} \\ 0 & 0 & 1 & 0 \\ - \sin θ_{2} & - \cos θ_{2} & 0 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}]$ (3)

${}_{3}^{2}T = [\begin{matrix} \cos θ_{3} & - \sin θ_{3} & 0 & L_{2} \\ \sin θ_{3} & \cos θ_{3} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}]$ (4)

${}_{4}^{3}T = [\begin{matrix} \cos θ_{4} & - \sin θ_{4} & 0 & L_{3} \\ \sin θ_{4} & \cos θ_{4} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}]$ (5)

由式(2)、(3)、(4)、(5)进行连乘可得足端位姿相对于腿部基坐标系的齐次变换矩阵，如式(6)所示：

${}_{4}^{0}T = {}_{1}^{0}T {}_{2}^{1}T {}_{3}^{2}T {}_{4}^{3}T = [\begin{matrix} c_{234} c_{1} & - s_{234} c_{1} & - s_{1} & c_{1} (L_{1} + L_{3} c_{23} + L_{2} c_{2}) \\ c_{234} s_{1} & - s_{234} s_{1} & c_{1} & s_{1} (L_{1} + L_{3} c_{23} + L_{2} c_{2}) \\ - s_{234} & - c_{234} & 0 & - L_{3} s_{23} - L_{2} s_{2} \\ 0 & 0 & 0 & 1 \end{matrix}]$ (6)

其中： $s_{i} = \sin θ_{i}$ ， $c_{i} = \cos θ_{i}$

$s_{i j k} = \sin (θ_{i} + θ_{j} + θ_{k})$ ， $c_{i j k} = \cos (θ_{i} + θ_{j} + θ_{k})$ ， $s_{i j} = \sin (θ_{i} + θ_{j})$ ， $c_{i j} = \cos (θ_{i} + θ_{j})$

式(6)的齐次变换矩阵中，前三行三列表示了足端在腿部基坐标系下的旋转矩阵，如式(7)所示，而第四列的前三行表示了足端在腿部基坐标系下的位置，如式(8)所示，如此一来便可通过运动学正解来描述足端在腿部基坐标系下的位姿。

单腿的运动学正解为：

${}_{4}^{0}R = [\begin{matrix} c_{234} c_{1} & - s_{234} c_{1} & - s_{1} \\ c_{234} s_{1} & - s_{234} s_{1} & c_{1} \\ - s_{234} & - c_{234} & 0 \end{matrix}]$ (7)

$P = [\begin{matrix} c_{1} (L_{1} + L_{3} c_{23} + L_{2} c_{2}) \\ s_{1} (L_{1} + L_{3} c_{23} + L_{2} c_{2}) \\ - L_{3} s_{23} - L_{2} s_{2} \end{matrix}]$ (8)

正运动学验证分析

使用MATLAB的机器人工具箱(Robotics Toolbox)，可以对足机器人的单腿正向运动学进行验证试验 [4] 。在该仿真环境中，建立了四足机器人的单腿模型。在示教器界面输入各个关节角度，单腿模型可以摆出相应关节角度下的姿态，并且自动计算出对应的足端位姿，将我们设定的关节角度值代入之前推导的齐次变换矩阵之中，发现推导出的结果与MATLAB的仿真结果完全一致。这就证明了我们之前分析的LF腿正运动学是正确的。以下是仿真结果的示意图，先将单腿上的侧摆关节、髋关节、膝关节分别设定为90˚、30˚、−60˚进行仿真，得到如图3所示的结果图。再将三个关节分别设置为90˚、60˚、30˚进行仿真，得到如图4所示的结果图。

Figure 3. Forward kinematic simulation verification 1

图3. 正运动学仿真验证一

Figure 4. Forward kinematic simulation verification 2

图4. 正运动学仿真验证二

结合计算得到的齐次变换矩阵，即式(6)，将相关的几何参数代入式中可得两种仿真状态的齐次变换矩阵结果如下：

${}_{4}^{0}T_{(90^{\circ},3 0^{\circ}, - 60^{\circ})} = [\begin{matrix} 0 & 0 & - 1 & 0 \\ 0.866 & 0.5 & 0 & 603.6057 \\ 0.5 & - 0.866 & 0 & 32.15 \\ 0 & 0 & 0 & 1 \end{matrix}]$ 、 ${}_{4}^{0}T_{(90^{\circ}, 6 0^{\circ}, 3 0^{\circ})} = [\begin{matrix} 0 & 0 & - 1 & 0 \\ 0 & - 1 & 0 & 211.94 \\ - 1 & 0 & 0 & - 573.1651 \\ 0 & 0 & 0 & 1 \end{matrix}]$

通过图3和图4的示教器界面的仿真结果可知两种姿态下的足端位姿和我们通过计算得到的公式求解出来的结果保持一致，即可得到该正运动学分析结果正确的结论。

2.3. 四足机器人逆运动学分析

四足机器人逆运动学分析的重要性在于它涉及到机器人在实际操作中如何根据期望的末端执行器的位置(在足式机器人中通常是指足端位置)来计算关节角度，从而实现所需的动作。这对于执行精确和复杂的任务，非常关键。而在逆运动学的诸多求解方法之中代数求解法和几何法 [4] 是常被使用到的两种方法，由于四足机器人的腿部结构较为简单，这里使用几何法对单腿逆运动学进行求解。图5为单腿简化视图。

Figure 5. Single leg simplified view

图5. 单腿简化视图

根据单腿简化视图的正视图可以使用几何法解出侧摆关节角度的表达式，如(9)式所示，图6为单腿简化视图正视图。

Figure 6. Single leg simplified front view

图6. 单腿简化正视图

由图6可求解出侧摆关节角度与足端位置之间的关系如下：

$θ_{1} = \arctan \frac{P_{y}}{P_{x}}$ (9)

根据单腿简化视图的侧视图可以使用几何法解出髋关节角度和膝关节角度的表达式，如(14)和(16)式所示，图7为单腿简化视图侧视图。

Figure 7. Single leg simplified side view

图7. 单腿简化侧视图

由图7可求解出髋关节角度和膝关节角度与足端位置之间的关系如下：

$L = \sqrt{P_{x}^{2} + P_{y}^{2}} - L_{1}$ (10)

$M = \sqrt{P_{z}^{2} + {(\sqrt{P_{x}^{2} + P_{y}^{2} - L_{1}})}^{2}}$ (11)

$\partial = \arctan \frac{P_{z}}{\sqrt{P_{x}^{2} + P_{y}^{2}} - L_{1}}$ (12)

$β = \arccos \frac{L_{2}^{2} + P_{z}^{2} + {(\sqrt{P_{x}^{2} + P_{y}^{2}} - L_{1})}^{2} - L_{3}^{2}}{2 L_{2} \sqrt{P_{z}^{2} + {(\sqrt{P_{x}^{2} + P_{y}^{2}} - L_{1})}^{2}}}$ (13)

$θ_{2} = \partial + β = \arctan \frac{P_{z}}{\sqrt{P_{x}^{2} + P_{y}^{2}} - L_{1}} + \arccos \frac{L_{2}^{2} + P_{z}^{2} + {(\sqrt{P_{x}^{2} + P_{y}^{2}} - L_{1})}^{2} - L_{3}^{2}}{2 L_{2} \sqrt{P_{z}^{2} + {(\sqrt{P_{x}^{2} + P_{y}^{2}} - L_{1})}^{2}}}$ (14)

$γ = \arccos \frac{L_{2}^{2} + L_{3}^{2} - P_{z}^{2} - {(\sqrt{P_{x}^{2} + P_{y}^{2}} - L_{1})}^{2}}{2 L_{2} L_{3}}$ (15)

$θ_{3} = π - γ = π - \arccos \frac{L_{2}^{2} + L_{3}^{2} - P_{z}^{2} - {(\sqrt{P_{x}^{2} + P_{y}^{2}} - L_{1})}^{2}}{2 L_{2} L_{3}}$ (16)

由上述的图解法求解单腿逆运动学的过程可得单腿的运动学逆解为：

${\begin{array}{l} θ_{1} = \arctan \frac{P_{y}}{P_{x}} \\ θ_{2} = \arctan \frac{P_{z}}{\sqrt{P_{x}^{2} + P_{y}^{2}} - L_{1}} + \arccos \frac{L_{2}^{2} + P_{z}^{2} + {(\sqrt{P_{x}^{2} + P_{y}^{2}} - L_{1})}^{2} - L_{3}^{2}}{2 L_{2} \sqrt{P_{z}^{2} + {(\sqrt{P_{x}^{2} + P_{y}^{2}} - L_{1})}^{2}}} \\ θ_{3} = π - \arccos \frac{L_{2}^{2} + L_{3}^{2} - P_{z}^{2} - {(\sqrt{P_{x}^{2} + P_{y}^{2}} - L_{1})}^{2}}{2 L_{2} L_{3}} \end{array}$ (17)

逆运动学验证分析

在逆运动学研究中，验证逆运动学分析的准确性至关重要。为了实现这一目标，我们可以借助MATLAB的Simulink工具来建立仿真模块，以验证逆运动学分析的正确性。通过规划足端轨迹(在后续章节中会详细分析)，也就是足端在腿部基坐标系下的位置，我们可以使用几何法求解的逆运动学模块来获取相应的腿部三个关节的转角信息。接下来，通过正运动学模块，我们可以将所得到的关节转角转化为实际足端位置的轨迹。

如果实际足端位置轨迹与规划的足端位置轨迹高度吻合，那么这将证明我们的逆运动学分析方法是正确的。这种验证方法具有重要的理论和实际意义，可以为机器人运动控制领域的研究和应用提供可靠的基础。在simulink中搭建的仿真模块如图8所示：

Figure 8. Construction of Simulink inverse kinematics verification module

图8. Simulink逆运动学验证模块搭建

通过在simulink中运行搭建得逆运动学验证模块可以得到如图9~11所示的足端期望位置曲线与实际位置曲线的对比图。

Figure 9. Comparison curve of foot end position in leg lift direction

图9. 抬腿方向足端位置对比曲线

Figure 10. Comparison curve of foot end position in side swing direction

图10. 侧摆方向足端位置对比曲线

Figure 11. Comparison curve of foot position in the forward direction

图11. 前进方向足端位置对比曲线

根据上述仿真得出的足端在腿部基坐标系下x, y, z方向的位置图像可以发现，各个方向上的实际位置曲线(红色曲线)与期望位置曲线(黄色曲线)完全重合。通过上述的仿真结果验证了逆运动学分析过程的正确性。

3. 四足机器人足端轨迹规划

足端轨迹规划可通过上述的运动学过程转化成腿部各个关节的转动，即运动学提供了足端轨迹规划 [5] 所需的重要输入参数和约束条件。四足机器人有效的与环境互动仅依赖其足端与地面的接触。因此，足端轨迹的特征在决定四足机器人的运动状态方面具有关键意义。

为确保机器人实现稳定的行走，需要在机身稳定性、步态切换的协调性以及步态平稳性等方面对足端轨迹提出如下三点要求 [6] ：

(1) 连续性要求：足端轨迹在三个运动方向上的位移、速度和加速度应保持连续，避免出现突变。这有助于确保机器人的运动过程平滑，减少不稳定因素的影响。

(2) 动作连贯性要求：在单腿从摆动相切换到支撑相或反之时，必须保持动作的连贯性。这有助于避免机器人因动作切换不协调而失去稳定性，确保机器人能够平稳地完成步态切换。

(3) 稳定速度要求：机器人在行走过程中应保持稳定的速度。这可以通过在摆动相中采用五次多项式来规划足端轨迹的竖直方向和前进方向，从而实现平稳的步态变化。而在支撑相中，为维持机器人的速度稳定，足端轨迹规划则采用一次式。

为满足以上条件,摆动相足端轨迹的竖直方向和前进方向上均采用五次多项式进行规划 [7] ，由于支撑项保持机器人速度稳定，则支撑相足端轨迹采用如下的一次多项式进行规划：

$P (t) = a_{0} + a_{1} t + a_{2} t^{2} + a_{3} t^{3} + a_{4} t^{4} + a_{5} t^{5}$

通过对足端轨迹的三点要求可以确定相应的边界条件 [8] 。根据要求(1)、(2)，可以确定方程求解的边界条件为：Z方向上，摆动相轨迹的开始和结束时的位置满足设定的步高要求，速度和加速度都应该为零；X方向上，摆动相轨迹开始和结束时的位置应满足设计的步长要求，速度应等于支撑相轨迹的速度，而加速度应该为零。

$[\begin{array}{l} p_{i} \\ p_{j} \\ v_{i} \\ v_{j} \\ a_{i} \\ a_{j} \end{array}] = [\begin{matrix} 1 & t_{i} & t_{i}^{2} & t_{i}^{3} & t_{i}^{4} & t_{i}^{5} \\ 1 & t_{j} & t_{j}^{2} & t_{j}^{3} & t_{j}^{4} & t_{j}^{5} \\ 0 & 1 & 2 t_{i} & 3 t_{i}^{2} & 4 t_{i}^{3} & 5 t_{i}^{4} \\ 0 & 1 & 2 t_{j} & 3 t_{j}^{2} & 4 t_{j}^{3} & 5 t_{j}^{4} \\ 0 & 0 & 2 & 6 t_{i} & 12 t_{i}^{2} & 20 t_{i}^{3} \\ 0 & 0 & 2 & 6 t_{j} & 12 t_{j}^{2} & 20 t_{j}^{3} \end{matrix}] [\begin{matrix} a_{0} \\ a_{1} \\ a_{2} \\ a_{3} \\ a_{4} \\ a_{5} \end{matrix}]$ (18)

式(18)中， $p_{i}$ 为轨迹的起始坐标， $p_{j}$ 为轨迹的终止位置； $v_{i}$ 为轨迹的起始速度， $v_{j}$ 为轨迹的终止速度； $a_{i}$ 为轨迹的起始加速度， $a_{j}$ 为轨迹的终止加速度； $t_{i}$ 为轨迹的时间和 $t_{j}$ 为轨迹的终止时间。

分别通过边界条件解出x方向和z方向上的轨迹方程，即可得到轨迹多项式。根据前文提到的足端轨迹应满足的三个要求，则在z方向上边界条件为 $I_{z} = {[\begin{matrix} p_{z i} & p_{z j} & 0 & 0 & 0 & 0 \end{matrix}]}^{T}$ ，得到轨迹方程为：

$p_{z} = {\begin{array}{l} p_{z i} + \frac{10 h}{{(\frac{1}{4} T_{0})}^{3}} t^{3} - \frac{15 h}{{(\frac{1}{4} T_{0})}^{4}} t^{4} + \frac{6 h}{{(\frac{1}{4} T_{0})}^{5}} t^{5} & 0 \leq t \leq \frac{1}{4} T_{0} \\ p_{z i} + \frac{10 h}{{(\frac{1}{4} T_{0})}^{3}} {(T_{0} - t)}^{3} - \frac{15 h}{{(\frac{1}{4} T_{0})}^{4}} (T_{0} - t^{4}) + \frac{6 h}{{(\frac{1}{4} T_{0})}^{5}} (T_{0} - t^{5}) & \frac{1}{4} T_{0} < t \leq \frac{1}{2} T_{0} \\ p_{z i} & \frac{1}{2} T_{0} < t \leq T_{0} \end{array}$ (19)

式(19)中， $h = p_{z j} - p_{z i}$ 为机器人步高； $T_{0}$ 为一个步态周期的时长。

为满足足端轨迹的三个要求，x方向上轨迹还需要考虑机器人机身的速度 $v_{0}$ ，则x方向的边界条件为 $p = {[\begin{matrix} p_{x i} & p_{x j} & - v_{0} & - v_{0} & 0 & 0 \end{matrix}]}^{T}$ 。同样可以得出x方向的轨迹方程为：

$P_{x} = {\begin{array}{l} p_{x i} - v_{0} t + \frac{10 (s + T_{0} v_{0})}{{(\frac{1}{2} T_{0})}^{3}} t^{3} - \frac{15 (s + T_{0} v_{0})}{{(\frac{1}{2} T_{0})}^{4}} + \frac{6 (s + T_{0} v_{0})}{{(\frac{1}{2} T_{0})}^{5}} t^{5} & 0 \leq t \leq \frac{1}{2} T_{0} \\ p_{x j} - v_{0} (t - \frac{1}{2} T_{0}) & \frac{1}{2} T_{0} < t \leq T_{0} \end{array}$ (20)

式(20)中， $v_{0}$ 为四足机器人机身的速度； $s = p_{x j} - p_{x i}$ 为四足机器人行走步长。

现定轨迹步高 $h = 40 mm$ ，步长 $s = 100 mm$ ，周期 $T_{0} = 2 s$ 的情况下，根据给定的步高、步长以及周期可以得到在x方向和z方向上的足端位置和速度曲线 [9] ，如图12所示。将两个方向足端轨迹综合可得到一个周期内完整的足端轨迹，如图13所示。

(a) z方向位移曲线(b) x方向位移曲线(c) z方向速度曲线(d) x方向速度曲线

Figure 12. Displacement and velocity curves in the x and z directions

图12. x和z方向位移以及速度曲线

Figure 13. Foot trajectory curve

图13. 足端轨迹曲线

从图13可以看出，利用5次多项式规划的轨迹，在z方向上轨迹起始点和结束点的速度加速度均为0的要求，在x方向上，摆动相轨迹的起始点和结束点加速度均为0，且速度都与支撑相同，保证了运动的连续性。

4. 四足机器人姿态控制与策略

4.1. 四足机器人姿态控制方法

在四足机器人的运动过程中，机身的姿态会不断发生变化，这种不断变化的机身姿态将严重影响四足机器人的稳定性 [10] ，而将一种有效的姿态控制方法应用于机器人的行走之中显得尤为重要。于是我们提出了一种有效的姿态控制方法：

我们发现当四足机器人的足端都与地面保持接触时(即站立姿态) [11] ，可以通过控制四条腿的足端坐标系原点到腿部基坐标系原点这个向量的长度来调控机身的姿态，如图14所示，以该四足机器人的左前腿为对象进行分析，其余三条腿同理。图中AB的长度即为需要调控的向量长度，O点为机身坐标系的原点，O'点为机身坐标系的原点向地面的投影。

图中向量AB可以依据向量的加法准则来实现求解，即：

$A B = - O^{'} O - O A + O^{'} B$ (21)

式中 $O^{'} O$ 可以通过机心处的IMU测量获得， $O^{'} B$ 由机器人的结构参数决定，即为恒定量。于是 $A B$ 的求解落在了 $O A$ 的求解上，虽然 $O A$ 会随着机身姿态的变化而发生改变，但在机身坐标系下的腿部基坐标系的位姿不会发生变化，我们可以通过左乘一个旋转矩阵将其转化为 $O A$ ，求解过程如下：

$O A = R \cdot O A_{机身}$ (22)

式中：R为旋转矩阵、 $O A_{机身}$ 为机身坐标系下腿部基坐标系的向量

则式(21)可以改写为：

$A B = - O^{'} O - R \cdot O A_{机身} + O^{'} B$ (23)

其余三条腿同理，可以依据上述计算过程得出各个足端坐标系原点到腿部基坐标系原点的向量长度。如此一来便可以控制四足机器人的机身姿态保持稳定。

Figure 14. Solution diagram of single leg foot position vector

图14. 单腿足端位置向量求解图

4.2. 稳定姿态行走控制策略

我们在研究中发现将上述的机器人姿态控制方法 [12] 应用于行走之中，可以保证行走时的姿态稳定。

行走过程中机身产生的姿态晃动会使得机身的基坐标系随之发生相应转动或移动，而世界坐标系保持固定不动，此时即可通过质心处的IMU实时测量出机身姿态的变化量 [13] ，我们将变化量经过PI控制 [14] 后，可利用上述的姿态控制方法解算出想要保证机身姿态稳定所需的四条腿足端位置的补偿量 [15] ，利用补偿量和轨迹规划出来的足端位置进行加和，从而得到一种在原本足端轨迹规划基础之上，通过实时误差补偿所得到的能够保证机器人行走姿态稳定的控制方法。

上述控制思想的控制策略框图如图15所示。

Figure 15. Stable attitude walking control block diagram

图15. 稳定姿态行走控制框图

5. 仿真分析

为了验证上述姿态控制策略在四足机器人踏步行走过程中的有效性与可行性，我们在MATLAB/Simulink平台上进行了仿真验证。我们在Simulink中构建了仿真模型，如图16所示。仿真持续时间设置为9秒，其中一个完整的踏步行走周期为2秒。在仿真时间内，前1秒用于四足机器人的初始姿态调整，将其从腿部竖直站立调整为准备行走的状态。仿真过程中，我们对四足机器人的姿态控制策略进行了验证。仿真动画结果如图17所示。从仿真动画中可以清晰地观察到，在四足机器人向前行走的过程中，其机身姿态保持稳定，没有出现明显的偏离预定轨迹的现象。

Figure 16. Simulation system model

图16. 仿真系统模型

(a) t = 1 s (b) t = 1.5 s (c) t = 2 s(d) t = 2.5 s (e) t = 3 s (f) t = 3.5 s

Figure 17. Simulation system model

图17. 仿真系统模型

在simulink仿真环境中，可以通过添加传感器模块实时检测机器人行走时的各项状态参数。从图18中可以看出在使用本文提出的控制策略之下，机身的横滚角(R角)基本保持在正负0.4˚之内，而不使用本文的姿态控制策略则会有较大横滚角的浮动，从图中可以看出前后两者的最大角度差值可达到约7.373˚。从图19中可以看出，在使用本文提出的控制策略之后，俯仰角(P角)基本保持在正负1˚之内，而在没有使用的情况下俯仰角也有较大变化，前后两者的最大角度差值可达到约3.231˚。从图20中可以看出，同样在使用该控制策略下，偏航角(Y角)无较为明显的变化，与没有使用控制手段相比，前后最大差值达到0.6731˚。从RPY角的观测中可以得出机器人在本文提出的姿态控制策略之下，可以保持较为稳定的机身姿态向前行走。

图21为机器人机身质心位移曲线，从图中可以看出机器人在行走过程中除向前移动之外在侧向无较为明显的偏移，而上下高度方向的偏移量为机身质心相对于世界坐标系的固定高度差。于是可以得出，机器人在姿态控制的控制方法之下可以保持笔直向前行走，无偏移轨迹的现象发生。

Figure 18. Roll angle comparison curve

图18. 横滚角对比曲线

Figure 19. Pitch angle comparison curve

图19. 俯仰角对比曲线

Figure 20. Yaw angle comparison curve

图20. 偏航角对比曲线

Figure 21. Centroid displacement diagram

图21. 质心位移图

这一仿真结果表明，我们所提出的姿态控制策略能够使四足机器人在踏步行走过程中有效地维持机身的稳定性，确保其沿着预定轨迹稳定行进。这为该策略在实际机器人中的应用提供了有力的支持。

6. 结语

本文对四足机器人在姿态控制下的trot步态进行仿真分析研究，对四足机器人进行了正、逆运动学分析，并利用MATLAB机器人工具箱以及在Simulink中搭建的仿真模块对正逆运动学推导过程进行验证，基于机身稳定性、步态切换的协调性以及步态平稳性三条原则下，规划了足端的五次多项式轨迹，详细分析了四足机器人姿态控制策略并应用于四足机器人的行走之中，在MATLAB/simulink中，对在姿态控制下的trot行走运动进行仿真，经过仿真分析，在姿态控制下行走的四足机器人具有较高的运动稳定性以及运动轨迹平稳性。本文在较为传统的四足机器人位置控制行走之中加入了姿态控制算法，使得机器人在行走的过程之中能够保持较为稳定的运动姿态，为四足机器人运动控制提供了新的思路。

参考文献

参考文献

[1]	Wooden, D., Malchano, M., Blankespoor, K., et al. (2010) Autonomous Navigation for BigDog. IEEE International Conference on Robotics & Automation, Anchorage, 3-7 May 2010, 4736-4741. https://doi.org/10.1109/ROBOT.2010.5509226
[2]	Katz, B., Carlo, J.D. and Kim, S. (2019) Mini Cheetah: A Platform for Pushing the Limits of Dynamic Quadruped Control. 2019 International Conference on Robotics and Automation (ICRA), Montreal, 20-24 May 2019, 6295-6301. https://doi.org/10.1109/ICRA.2019.8793865
[3]	Craig John, J. 机器人学导论[M]. 贠超, 译. 北京: 机械工业出版社, 2006.
[4]	郭建, 赵易, 徐镔滨. 基于足端轨迹规划的四足机器人运动学分析与仿真[J]. 机床与液压, 2021, 49(23): 48-53.
[5]	陈久朋. 四足机器人步态及运动控制研究[D]: [博士学位论文]. 昆明: 昆明理工大学, 2021.
[6]	王银浩, 颉潭成, 徐彦伟, 等. 四足机器人侧摆型trot步态的运动学分析及仿真研究[J]. 现代制造工程, 2022(5): 24-29.
[7]	Kalakrishnan, M., Buchli, J., Pastor, P., et al. (2011) Learning, Planning, and Control for Quadruped Locomotion over Challenging Terrain. International Journal of Robotic Research, 30, 236-258. https://doi.org/10.1177/0278364910388677
[8]	王立鹏, 王军政, 汪首坤, 等. 基于足端轨迹规划算法的液压四足机器人步态控制策略[J]. 机械工程学报, 2013, 49(1): 39-44.
[9]	谢楚政. 四足机器人轨迹规划及控制仿真研究[D]: [硕士学位论文]. 株洲: 湖南工业大学, 2022.
[10]	Hutter, M., Gehring, C., Jud, D., et al. (2016) ANYmal-a Highly Mobile and Dynamic Quadruped-al Robot. 2016. IEEE/RSJ. International Conference on Intelligent Robots and Systems (IROS), Daejeon, 9-14 October 2016, 38-44. https://doi.org/10.1109/IROS.2016.7758092
[11]	任军洲. 四足机器人设计与站立控制优化研究[D]: [硕士学位论文]. 绵阳: 西南科技大学, 2023.
[12]	Yangyang, H., Guoping, L., Zhenyu, L., et al. (2023) A Stability Locomotion-Control Strategy for Quad-ruped Robots with Center-of-Mass Dynamic Planning. Journal of Zhejiang University-SCIENCE A, 24, 516-530.
[13]	刘在阳. 电机驱动四足机器人控制系统与多步态控制方法研究[D]: [硕士学位论文]. 济南: 山东大学, 2021.
[14]	Kolathaya, S. (2020) Local Stability of PD Controlled Bipedal Walking Robots. Automatica, 114, 108841-108847. https://doi.org/10.1016/j.automatica.2020.108841
[15]	Hobbs, S.J. and Clayton, H.M. (2019) Collisional Mechanics of the Diag-onal Gaits of Horses over a Range of Speeds. Peer J, 7, 76-89. https://doi.org/10.7717/peerj.7689

为你推荐

友情链接