一种稳定姿态行走的四足机器人虚拟力矩控制方法

doi:10.12677/mos.2024.136598

期刊菜单

一种稳定姿态行走的四足机器人虚拟力矩控制方法
A Virtual Torque Control Method for Stable Posture Walking in Quadruped Robots

DOI: 10.12677/mos.2024.136598, PDF, HTML, XML,
作者: 靳小龙, 周孟成：上海理工大学，机械工程学院，上海
关键词: 四足机器人；MATLAB工具箱；蒙特卡洛法；姿态控制；虚拟力矩控制；A Quadruped Robot； MATLAB Toolboxes； Monte Carlo Method； Posture Control； Virtual Torque Control

摘要: 文章针对四足机器人在trot步态下如何进行稳定行走这一问题，提出了一种用于四足机器人稳定姿态行走的虚拟力矩控制方法。首先，对四足机器人进行运动学分析并用MATLAB工具箱和Simulink进行仿真验证。之后，用五次多项式来规划足端轨迹并利用蒙特卡洛法和MATLAB工具箱联合选出符合四足机器人行走的步高和步长。再次，将姿态控制策略和虚拟力矩控制策略分别运用到四足机器人的整体结构中和关节角上。最后仿真表明，与开环控制策略相比，本文采用的控制策略使四足机器人在trot步态行走中的俯仰角、横滚角和航偏角的变化范围进一步缩小，稳定性提高明显。

Abstract: This study presents a virtual torque control approach for the stable gait locomotion of a quadruped robot in trotting motion. The investigation commences with a kinematic analysis of the quadruped robot, which is subsequently validated through simulations utilizing MATLAB toolboxes and the Simulink environment. Thereafter, a quintic polynomial is applied to chart the trajectory of the robot’s feet, and the step height and length appropriate for the quadruped robot’s ambulation are determined through an integration of the Monte Carlo method with MATLAB toolboxes. Subsequently, posture control algorithms and virtual torque control algorithms are integrated into the robot’s overall mechanical structure and joint angles, respectively. Ultimately, the simulation outcomes demonstrate that, in comparison with open-loop control strategies, the control strategy implemented in this research substantially diminishes the fluctuation scope of the robot’s pitch, roll, and yaw angles during trotting gait, leading to a significant improvement in stability.

文章引用：靳小龙, 周孟成. 一种稳定姿态行走的四足机器人虚拟力矩控制方法[J]. 建模与仿真, 2024, 13(6): 6552-6567. https://doi.org/10.12677/mos.2024.136598

1. 引言

移动机器人根据其末端执行机构的不同，可分为轮式、腿式和履带式等类型，而四足机器人是一种模仿动物四肢行走方式的移动机器人，这一点在自然界中动物的行为中得到了体现，例如猎犬在草原上的高速奔跑，以及雪橇犬在雪地中的轻松行进。

在四足机器人设计中，稳定行走的优化至关重要，因为它对机器人操作的可靠性和适应性具有直接影响。实现稳定步态是确保机器人在平地上行走时保持平衡，避免倾覆的核心要求。例如，波士顿动力公司的Spot机器人在适应复杂地形方面表现优异，能够有效执行消防、巡检、审查、搜索和救援等多样化任务[1] [2]。此外，国内企业如宇树科技、云深处及蔚蓝科技等也在四足机器人领域取得了显著进展，推出了多款具备自主知识产权的四足机器人产品，进一步推动了该领域的技术创新与应用。在四足机器人控制方面，电动四足机器人StarlETH采用柔顺控制，提高了它在行走时的柔顺性和能量利用率。波士顿动力公司的四足机器人Spot采用了一种类似于模型预测的控制方法，使机器人能够在复杂环境下进行稳定性和适应性更强的走路和奔跑。百度强化学习团队联合小度机器人团队，利用自适应步态生成器作为强化学习算法的引导机制，通过自主探索策略，机器人能够发展出有效的步态模式，以适应并成功穿越多样化的复杂地形环境。

本文研究了四足机器人在trot步态下稳定姿态行走策略。首先，将开源四足机器人STP5425舵机版SolidWorks模型文件进行URDF文件转换，并导入到MATLAB中的Simulink仿真平台中进行研究。之后，构建该四足机器人的正逆运动学模型，并通过MATLAB工具箱和Simulink的仿真模块进行验证。随后，根据给出的3条规则，规划五次多项式足端轨迹，并通过蒙特卡洛法选取其合适的步高及步长。接着，分析了姿态控制策略和虚拟力矩控制策略在四足机器人tort步态上的控制方法。最后通过仿真获得了四足机器人在行走过程中的机身姿态数据，并验证了本文所用的控制策略的可行性。

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

2.1. 四足机器人的结构分析

机器人图1为本文所研究的四足机器人整体构型。本文首先采用SolidWorks软件进行三维结构模型搭建，再通过SolidWorks转URDF文件方式导入MATLAB软件的进行运动仿真。为了加快仿真速度，优化仿真性能，在转换URDF文件之前，进行模型的结构简化。该四足机器人由机身、四个侧摆关节、四个髋关节和四个膝关节组成。腿部构型为全肘式构型，为保持它良好的行走稳定性，因此它的质心位于机身几何中心偏后[3] [4]。

Figure 1. Simplified overall configuration diagram of quadruped robot

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

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

本文采用精度更高、复杂度很低的改进D-H参数法来确定四足机器人足端坐标系与腿部的各个关节角之间的关系[5]。如图2所示，本文将 $θ_{1}$ ， $θ_{2}$ ， $θ_{3}$ 分别定义为侧摆关节、髋关节和膝关节的转角，将坐标系{W}、{B}定义为大地坐标系和机身质心坐标系，将坐标系{0}、{1}、{2}、{3}、{4}定义为腿部基坐标系、侧摆关节坐标系、髋关节坐标系、膝关节坐标系和足端坐标系。因为本文所研究的四足机器人的四条腿的结构参数均相同，所以仅对左前腿建立关节坐标系和相应的D-H参数表，以下为附带坐标系的四足机器人的结构简图和相应的D-H参数表。

Figure 2. Schematic diagram of quadruped robot mechanism

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

Table 1. D-H parameters

表1. D-H参数

$i$	$a_{i}$ (mm)	$α_{i}$ (˚)	$d_{i}$ (mm)	$θ_{i - 1}$ (˚)
1	0	0	0	0
2	0	-90	$L_{1}$	$θ_{1}$
3	$L_{2}$	0	0	$θ_{2}$
4	$L_{3}$	0	0	$θ_{3}$

表1中： $a_{i}$ 为杆件长度； $α_{i}$ 为杆件扭角； $d_{i}$ 为关节距离； $θ_{i - 1}$ 为关节转角。

将D-H参数表中数据及参数带入公式(1)，可得到4个关节坐标系之间的变换关系。

${}_{i}^{i - 1}T = [\begin{matrix} \cos θ_{i} & - \sin θ_{i} & 0 & a_{i - 1} \\ \sin θ_{i} \cos \partial_{i - 1} & \cos θ_{1} \cos \partial_{i - 1} & - \sin \partial_{i - 1} & - d \sin \partial_{i - 1} \\ \sin θ_{i} \sin \partial_{i - 1} & \cos θ_{i} \sin \partial_{i - 1} & \cos \partial_{i - 1} & d \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 & 0 \\ 0 & 0 & 1 & h_{1} \\ - \sin θ_{2} & - \cos θ_{2} & 0 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}]$ (3)

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

${}_{4}^{3}T = [\begin{matrix} 1 & 0 & 0 & h_{3} \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}]$ (5)

其中公式(2)为基坐标系与侧摆关节坐标系之间的变换关系，公式(3)为侧摆关节坐标系与髋关节坐标系之间的变换关系，公式(4)为髋关节坐标系与膝关节坐标系之间的变换关系，公式(5)为膝关节坐标系与足端坐标系之间的变换关系。之后将公式(2)、(3)、(4)和(5)相乘可得到足端位姿与基坐标系之间的变换关系，如公式(6)所示：

${}_{4}^{0}T =_{1}^{0} T_{2}^{1} T_{3}^{2} T_{4}^{3} T = [\begin{matrix} c_{23} c_{1} & - s_{23} c_{1} & - s_{1} & h_{3} c_{1} c_{23} + h_{2} c_{1} c_{2} - h_{1} s_{1} \\ c_{23} s_{1} & - s_{23} s_{1} & c_{1} & h_{3} c_{23} s_{1} + h_{2} c_{2} s_{1} + h_{1} c_{1} \\ - s_{23} & - c_{23} & 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_{23} = \sin (θ_{2} + θ_{3})$ ， $c_{23} = \cos (θ_{2} + θ_{3})$ 。

由此可以得到足端坐标系相对于基坐标系的变换关系。足端坐标系与基坐标系的旋转关系和位置关系如公式(7)、(8)所示：

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

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

2.3. 四足机器人正运动学仿真验证

为了验证上述得到的正运动学公式的正确性，现用MATLAB工具箱(Robotics Toolbox)建立与本文四足机器人腿部一致的模型，并在操作界面输入相应的关节转角。同时，将输入的关节转角带入公式(8)，检验数据是否一致。

这里分别带入(0˚, 30˚, −60˚)，(45˚, 60˚, −30˚)两组数据进行验证。图3和图4为MATLAB工具箱的仿真结果。

Figure 3. The first set of simulation experiments

图3. 第一组仿真实验

Figure 4. The second set of simulation experiments

图4. 第二组仿真实验

带入公式(8)得到的两组数据结果分别为：

$P_{(0 °, 30 °, - 60 °)} = [\begin{matrix} 190.5256 \\ 43.9 \\ 0 \end{matrix}]$ 、 $P_{(45 °, 60 °, - 30 °)} = [\begin{matrix} 75 .2099 \\ 137 .2938 \\ - 150 .2628 \end{matrix}]$

对比仿真数据和计算数据可得，两种数据的结果完全一致，说明文本推导的正运动学公式是正确的。

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

四足机器人的逆运动学[6]是指在已知足端位置及连杆长度的情况下，用解析法或几何法推导出各个关节的转角，从而控制四足机器人完成预期的运动。文本用几何法并以右前腿为例进行四足机器人单腿的运动学逆解。图6为右前腿的 $Y_{1} Z_{2}$ 平面视图。

Figure 5. Y₁Z₁ plan view

图5. Y₁Z₁平面视图

如图5所示，点 $A_{y z}$ 为坐标系原点， $X_{1}$ 轴方向与纸面垂直并穿入纸面。点 $Z_{y z}$ 为足端点，它相对于坐标系原点的坐标 $(P_{X}, P_{Y}, P_{Z})$ 已知。另外，关节长度 $L_{1}$ 、 $L_{2}$ 和 $L_{3}$ 均已知。连接点 $A_{y z}$ 和点 $Z_{y z}$ ，并过点 $A_{y z}$ 做线段 $L_{y z}$ 的平行线。其中， $L_{y z}$ 的长度可表示为：

$L_{y z} = \sqrt{P_{X}^{2} + P_{Y}^{2} - L_{1}^{2}}$ (9)

$θ_{y z 1}$ 和 $θ_{y z 2}$ 的角度分别为：

$θ_{y z 1} = - \arctan (P_{Y} / P_{Z})$ (10)

$θ_{y z 2} = - \arctan (L_{1} / L_{y z})$ (11)

由此可得第一个关节角度 $θ_{1}$ 为：

$θ_{1} = θ_{y z 1} - θ_{y z 2}$ (12)

朝着图5中的 $X_{1} Y_{2} Z_{2}$ 方向可得到如图6所示的右前腿的 $X_{1} Z_{2}$ 平面视图。其中点 $A_{x z}$ 为坐标系原点， $Y_{2}$ 轴方向与纸面垂直并穿入纸面。线段 $A_{x z} B_{x z}$ 、线段 $B_{x z} C_{x z}$ 分别为已知线段 $L_{2}$ 、 $L_{3}$ 。

线段 $A_{x z} C_{x z}$ 的长度为：

$A_{x z} C_{x z} = \sqrt{P_{X}^{2} + L_{y z}^{2}}$ (13)

Figure 6. X₁Z₂ plan view

图6. X₁Z₂平面视图

过点 $C_{x z}$ 作延长线 $A_{x z} B_{x z}$ 的垂线，并交于点 $R$ ，可得方程组：

${\begin{matrix} B_{x z} C_{x z}^{2} = R B_{x z}^{2} + R C_{x z}^{2} \\ A_{x z} C_{x z}^{2} = R A_{x z}^{2} + R C_{x z}^{2} \end{matrix}$ (14)

由此可得线段 $R B_{x z}$ 的长度为：

$R B_{x z} = \frac{A_{x z} C_{x z}^{2} - A_{x z} B_{x z}^{2} - B_{x z} C_{x z}^{2}}{2 A_{x z} B_{x z}} = \frac{A_{x z} C_{x z}^{2} - L_{2}^{2} - L_{3}^{2}}{2 L_{2}}$ (15)

$θ_{y z 1}$ 和 $θ_{y z 2}$ 的角度分别为：

$θ_{y z 1} = - \arctan (P_{X} / L_{y x})$ (16)

$θ_{x z 2} = \arccos (\frac{A_{x z} B_{x z} + R B_{x z}}{A_{x z} C_{x z}}) = \arccos (\frac{L_{2} + R B_{x z}}{A_{x z} C_{x z}})$ (17)

由此可得第二个关节角度 $θ_{2}$ 和第三个关节角度 $θ_{3}$ 为：

$θ_{2} = θ_{y z 1} + θ_{y z 2}$ (18)

$θ_{3} = - \arccos (R B_{x z} / B_{x z} C_{x z}) = - \arccos (R B_{x z} / L_{3})$ (19)

2.5. 四足机器人逆运动学仿真验证

逆运动学验证的准确性可以确保机器人的每个关节能够精确地达到预定的空间位置，从而实现对足端的精确控制。本文采用MATLAB中的Simulink来搭建仿真环境。首先，文本从逆运动学模块里在前进方向和高度方向上输入一段30秒的正弦波信号，在侧方向上输入一个固定参数43.9。这样就能够得到相应的关节转角，接下来再把这些参数输入到正运动学模块里，得到实际的足端位置曲线图。图7是在Simulink中搭建的逆运动学验证模块。

本文把输入到逆运动学里的曲线视为期望曲线，将它与和运动学模块里输出的实际足端位置曲线放进Scope模块中进行对比，来证明逆运动学分析方法的正确性。三个方向上期望轨迹与实际轨迹的对比图，如图8、图9和图10所示：

Figure 7. Simulink inverse kinematics verification module

图7. Simulink逆运动学验证模块

Figure 8. Comparison of trajectories in the height direction

图8. 高度方向上的轨迹对比图

Figure 9. Comparison of trajectories in the forward direction

图9. 前进方向上的轨迹对比图

Figure 10. Comparison of trajectories in the lateral direction

图10. 侧方向上的轨迹对比图

如上图所示，足端点在三个方向上的期望轨迹与实际轨迹完全重合，说明本文的逆运动学分析结果是正确的。

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

足端轨迹规划是四足机器人研究中的一项关键技术，它对于降低机器人在运动过程中足端触地的冲击、提高运动效率和稳定性[7]具有重要意义。合理的轨迹规划能够确保机器人在不同步态下实现高效、平稳的运动。

在这样的条件下，本文选取的足端轨迹应有以下优点：

1) 足端轨迹曲线在每个周期下应都是连续且闭合的；

2) 足端速度变化曲线应该是平滑的，且它的速度没有发生突变；

3) 四足机器人整体在支撑相和摆动相来回切换时不会产生较大冲击。

为保证以上条件均可以满足，本文在前进方向和高度方向上均采用5次多项式[8]进行足端规划。前进方向和高度方向的轨迹方程分别为公式(20)和公式(21)：

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

$z = b_{0} + b_{1} t^{1} + b_{2} t^{2} + b_{3} t^{3} + b_{4} t^{4} + b_{5} t^{5}$ (21)

3.1. 四足机器人足端在前进方向上的轨迹规划

本文设定支撑相和摆动相在一个周期T内的占比均为0.5，并且为了大大减小足端在支撑相和摆动相切换时带来的冲击，应使足端在碰到地面时的速度和加速度都为零。由此可以列出以下约束条件：

${\begin{cases} {x |}_{t = 0} = 0, {\frac{d x}{d t} |}_{t = 0} = 0, {\frac{d^{2} x}{d t_{2}} |}_{t = 0} = 0 \\ {x |}_{t = T / 2} = S, {\frac{d x}{d t} |}_{t = T / 2} = 0, {\frac{d^{2} x}{d t_{2}} |}_{t = T / 2} = 0 \end{cases}$ (22)

其中，S为步长，并将公式(22)带入公式(21)可求解得摆动相的公式为：

$x = \frac{6 S}{{(\frac{T}{2})}^{5}} t^{5} - \frac{15 S}{{(\frac{T}{2})}^{4}} t^{4} + \frac{10 S}{{(\frac{T}{2})}^{3}} t^{3} t \in [0, \frac{T}{2}]$ (23)

另外，支撑相在前进方向上的轨迹方程与摆动相上的轨迹方程在半个周期上对称。由此可得支撑相的轨迹方程为：

$x = \frac{6 S}{{(\frac{T}{2})}^{5}} {(T - t)}^{5} - \frac{15}{{(\frac{T}{2})}^{4}} {(T - t)}^{4} + \frac{10 S}{{(\frac{T}{2})}^{3}} {(T - t)}^{3} t \in [\frac{T}{2}, T]$ (24)

3.2. 四足机器人足端在高度方向上的轨迹规划

为了使四足机器人能够平滑抬腿和平滑放腿，则可以设置四足机器人足端在高度方向上前半个周期的约束条件为：

${\begin{cases} {z |}_{t = 0} = 0, {\frac{d z}{d t} |}_{t = 0} = 0, {\frac{d^{2} z}{d t_{2}} |}_{t = 0} = 0 \\ {z |}_{t = T / 4} = H, {\frac{d z}{d t} |}_{t = T / 4} = 0, {\frac{d^{2} z}{d t_{2}} |}_{t = T / 4} = 0 \\ {z |}_{t = T / 2} = 0, {\frac{d z}{d t} |}_{t = T / 2} = 0, {\frac{d^{2} z}{d t_{2}} |}_{t = T / 2} = 0 \end{cases}$ (25)

其中，H为步高，并将公式(25)带入公式(20)可求解得摆动相的公式(26)为：

${\begin{cases} z = \frac{6 H}{{(\frac{T}{4})}^{5}} t^{5} - \frac{15 H}{{(\frac{T}{4})}^{4}} t^{4} + \frac{10 H}{{(\frac{T}{4})}^{3}} t^{3} z \in [0, \frac{T}{4}] \\ z = \frac{6 H}{{(\frac{T}{4})}^{5}} {(\frac{T}{2} - t)}^{5} - \frac{15 H}{{(\frac{T}{4})}^{4}} {(\frac{T}{2} - t)}^{4} + \frac{10 H}{{(\frac{T}{4})}^{3}} {(\frac{T}{2} - t)}^{3} z \in [\frac{T}{4}, \frac{T}{2}] \end{cases}$ (26)

在后半个周期内，足端处于支撑相并与地面一直保持接触，则：

$z = 0 z \in [\frac{T}{2}, T]$ (27)

3.3. 四足机器人步高及步长的选取

在确定了机器人腿部的可达范围之后，才能更好的进行机器人步高、步长的选取。这里，本文采用

(a) (b)

Figure 11. Leg workspace; (a) 3D view; (b) XY plane view; (c) XZ plane view; (d) YZ plane view

图11. 腿部工作空间；(a) 三维视图；(b) XY平面视图；(c) XZ平面视图；(d) YZ平面视图

蒙特卡洛法并联合机器人工具箱(Robotics Toolbox)的方式[9]来直观的得到四足机器人腿部的工作空间。

其中，侧摆关节角的取值范围为[−45˚, 90˚]，髋关节角的取值范围为[−60˚, 120˚]，膝关节角的取值范围为[−90˚, 70˚]，由此可得到四足机器人右前腿的工作空间如图11所示。

如上图可知，离初始足端点越近，随机点越密集。在这样情况下，本文最好选取关节活动角中间值的一半，即初始足端点的附近，以避免超出工作空间的最优范围，并保证关节角活动的平滑性。

由此本文选取步长为30 mm，步高为10 mm，步态周期暂定为2 s。将选取的数值带入前文的足端轨迹公式并在MATLAB上画出该步态下的足端轨迹、位移、速度和加速度曲线图分别为图12、图13、图14和图15所示。

Figure 12. Foot end trajectory curve

图12. 足端轨迹曲线

(a) (b)

Figure 13. Foot displacement curve; (a) X-direction displacement; (b) Z-direction displacement

图13. 足端位移曲线；(a) X方向位移；(b) Z方向位移

(a) (b)

Figure 14. Foot velocity curve; (a) X-direction velocity; (b) Z-direction velocity

图14. 足端速度曲线；(a) X方向速度；(b) Z方向速度

(a) (b)

Figure 15. Foot end acceleration curve; (a) X-direction acceleration; (b) Z-direction acceleration

图15. 足端加速度曲线；(a) X方向加速度；(b) Z方向加速度

如图12到图15所示，足端轨迹曲线均符合本文所提出的要求[10]。这些曲线验证了足端轨迹方程的合理性。

4. 四足机器人控制策略

4.1. 姿态控制策略

四足机器人姿态控制涉及调整机器人机身在空间中的姿势(即其横滚角、航偏角和俯仰角)，以确保其在行走过程中的稳定性和精确度。本文运用了一种高效、简洁的控制方法[11]来提高四足机器人机身的稳定性。

当四足机器人处于站立姿态(即腿部足端点一直与地面保持接触)时，控制前文1~2中腿部坐标系{0}与足端坐标系{4}的向量长度得到预期的机身姿态。本文以左前腿为例，建立如图16所示的腿部向量分析图。在图17中，点 $O$ 为机身坐标系的原点， $O_{p}$ 为机身坐标系原点在地面上的投影，点 $A$ 为腿部基坐标系的原点，点 $B$ 为足端坐标系的原点。

Figure 16. Quadrilateral vector diagram of legs

图16. 腿部四边形向量图

首先，由向量的加减法可得公式(28)：

$\vec{O_{P} B} + \vec{B A} = \vec{O_{P} O} + \vec{O A}$ (28)

变换得公式(29)：

$\vec{B A} = \vec{O_{P} O} + \vec{O A} - \vec{O_{P} B}$ (29)

式(29)中的 $\vec{O_{P} O}$ 可以由Simulink中的传感器模块测量得到， $\vec{O_{P} B}$ 由结构参数决定的恒定量， $\vec{O A}$ 可以由机身坐标系 $O$ 下的恒量 ${\vec{O A}}_{机身}$ 左乘旋转矩阵R得到。所以，可将公式(29)改写为：

$\vec{B A} = \vec{O_{P} O} + R · {\vec{O A}}_{机身} - \vec{O_{P} B}$ (30)

由此，可推出4条腿的通用公式(31)为：

$\vec{B_{a} A_{a}} = \vec{O_{P} O} + R · {\vec{O A_{a}}}_{机身} - \vec{O_{P} B_{a}}$ (31)

其中： $a = 1, 2, 3, 4$ 。

4.2. 虚拟力矩控制策略

在机器人控制中，虚拟力矩不是一种实际存在的力矩，它是为了简化算法和最大程度的优化控制效果[12]而设计的一种控制方法。本文将它应用于腿部的在髋关节、膝关节和侧摆关节上。

首先，将逆运动学算法得到的期望关节角度与传感器反馈的实际关节角度进行比较，计算出每个关节角度的偏差值如下：

$Δ Q = Q_{d} - Q_{t}$ (32)

其中：期望关节角度 $Q_{d} = {[\begin{matrix} q_{1 d} & q_{2 d} & q_{3 d} \end{matrix}]}^{T}$ ，实际关节角度 $Q_{t} = {[\begin{matrix} q_{1 t} & q_{2 t} & q_{3 t} \end{matrix}]}^{T}$ 。

随后，将把期望关节角度进行微分处理后的期望角速度与传感器模块里反馈的实际角速度进行比较，得到三个关节角速度的偏差值如下：

$Δ \dot{Q} = {\dot{Q}}_{d} - {\dot{Q}}_{t}$ (33)

其中： ${\dot{Q}}_{d} = {[\begin{matrix} {\dot{q}}_{1 d} & {\dot{q}}_{2 d} & {\dot{q}}_{3 d} \end{matrix}]}^{T} ，$ ${\dot{Q}}_{t} = {[\begin{matrix} {\dot{q}}_{1 t} & {\dot{q}}_{2 t} & {\dot{q}}_{3 t} \end{matrix}]}^{T}$ 。

最后，三个关节的总控制律如下：

$Q = K_{p} \cdot Δ Q + K_{d} \cdot Δ \dot{Q}$ (34)

在式(32)、(33)和(34)中， $q_{1 d}$ 、 $q_{2 d}$ 、 $q_{3 d}$ 和 $q_{1 t}$ 、 $q_{2 t}$ 、 $q_{3 t}$ 分别为侧摆关节、髋关节和膝关节的期望角度和实际角度， ${\dot{q}}_{1 d}$ 、 ${\dot{q}}_{2 d}$ 、 ${\dot{q}}_{3 d}$ 和 ${\dot{q}}_{1 t}$ 、 ${\dot{q}}_{2 t}$ 、 ${\dot{q}}_{3 t}$ 分别为侧摆关节、髋关节和膝关节的期望角速度和实际角速度。 $K_{p}$ 和 $K_{d}$ 分别为弹性系数和阻尼系数。

结合3.1中的姿态控制策略，可以得到总控制策略方框图如图17所示。

Figure 17. Overall control strategy block diagram

图17. 总控制策略框图

5. 四足机器人仿真验证

为了验证所提出的控制策略在实际应用中的有效性，本研究在MATLAB的Simulink模块中构建了一个仿真平台。仿真过程的时间参数被设定为10秒，其中步态周期设定为1秒。在仿真参数中，步高设定为2.3节中所述的10毫米，步长设定为30毫米。仿真结果的前5秒被展示在图18中，该阶段的前0.5秒代表机器人的启动阶段，而随后的4.5秒则代表机器人的行走阶段。在行走阶段，机器人的机身保持稳定，没有出现显著的位置偏移。

最后，在虚拟力矩控制(Virtual Torque Control, VTC)策略下与在开环控制(Open Loop Control, OLC)策略下[13]，俯仰角、横滚角和偏航角的变化对比如图19、图20和图21所示。

Figure 18. Trot gait simulation diagram

图18. Trot步态仿真图

Figure 19. Comparison of elevation angles

图19. 俯仰角对比图

Figure 20. Comparison of roll angles

图20. 横滚角对比图

Figure 21. Comparison of yaw angle

图21. 偏航角对比图

在分析图19所示的数据后，可以观察到，在VTC策略下，俯仰角的波动被有效限制在3度以内。相较于OLC策略，VTC策略在控制精度上展现了显著的提升。进一步地，通过图20的数据可以发现，采用VTC策略后，横滚角的波动被控制在了0度至1度的范围内，与OLC策略相比，其性能提升尤为显著。最终，从图21中可以明显看出，在VTC策略的影响下，偏航角的变化幅度极小，表明VTC策略在偏航角控制方面的效果提升是最为明显的。

这些仿真结果集体证实了，在本文提出的控制策略作用下，四足机器人在行走过程中能够始终保持良好稳定性的姿态，并能够依照预定轨迹实现稳定前进，无明显偏差。

6. 结论

本研究针对四足机器人在trot步态下的稳定姿态行走，开展了一系列仿真分析工作。研究的初步阶段，对四足机器人进行了正运动学建模，并利用MATLAB软件工具箱对所建立的正运动学模型进行了验证。随后，通过几何法求解了四足机器人的逆运动学问题，得出了足端在不同位置时的各个关节角度。为了进一步验证逆运动学模型的准确性，本研究在Simulink仿真环境中构建了相应的仿真模块。

此外，本文采用五次多项式对机器人的足端轨迹进行了规划，并运用蒙特卡洛法，确定了适合该四足机器人行走的步高和步长参数。最终，将姿态控制和虚拟力矩控制策略应用于四足机器人的行走控制，并借助Simulink进行仿真分析。

结果表明，与开环控制策略相比，本文提出的控制策略显著缩小了四足机器人在俯仰角、横滚角和偏航角上的变化范围。此外，从trot步态的仿真结果中可以看出，该四足机器人在机身稳定性方面表现优异，未出现明显的偏移现象。

参考文献

[1]	朱秋国. 浅谈四足机器人的发展历史、现状与未来[J]. 杭州科技, 2017(2): 47-50.
[2]	谭民, 王硕. 机器人技术研究进展[J]. 自动化学报, 2013, 39(7): 963-972.
[3]	谭小群, 李军, 赵茜. 四足机器人仿生机构模型及腿系参数优化[J]. 组合机床与自动化加工技术, 2008(12): 21-24.
[4]	Ren, H., Li, Q., Liu, B. and Dou, Z. (2018) Design and Optimization of an Elastic Linkage Quadruped Robot Based on Workspace and Tracking Error. Proceedings of the Institution of Mechanical Engineers, Part C: Journal of Mechanical Engineering Science, 232, 4152-4166. https://doi.org/10.1177/0954406217750186
[5]	Jatsun, S.F. and Soe, Y.N. (2018) Kinematic and Jacobian Analysis Approach for the Four-Legged Robot. Proceedings of the Southwest State University, 22, 32-41. https://doi.org/10.21869/2223-1560-2018-22-4-32-41
[6]	Phunopas, A. and Hayashi, E. (2018) A Four-Legged Robot’s Soft Feet Structural Design and Walking Gait Generated from Inverse Kinematics. Journal of Robotics, Networking and Artificial Life, 5, 161. https://doi.org/10.2991/jrnal.2018.5.3.4
[7]	Singh, S. and Dutta, A. (2023) Gait Planning and Optimization of an 18 DOF Quadruped Robot with Compliant Shanks. Proceedings of the Institution of Mechanical Engineers, Part C: Journal of Mechanical Engineering Science, 238, 2294-2307. https://doi.org/10.1177/09544062231194893
[8]	陈海强. 新型仿生四足机器人结构设计及步态规划[D]: [硕士学位论文]. 福州: 福州大学, 2021.
[9]	刘志国. 四足巡检机器人步态规划及摩擦补偿控制研究[D]: [硕士学位论文]. 北京: 北京石油化工学院, 2023.
[10]	周孟成, 靳小龙. 基于模糊滑模切换增益的六自由度机械臂轨迹跟踪控制[J/OL]. 重庆工商大学学报(自然科学版), 2024: 1-9. http://kns.cnki.net/kcms/detail/50.1155.n.20240914.1602.005.html, 2024-09-18.
[11]	李龙坤, 方鸿磊. 基于Trot步态的四足机器人行走姿态控制策略[J/OL]. 重庆工商大学学报(自然科学版), 2024: 1-9. http://kns.cnki.net/kcms/detail/50.1155.N.20231103.1529.004.html, 2024-09-18.
[12]	Xu, Y. and Li, G. (2019) Optimal and Robust Interference Efficiency Maximization for Multicell Heterogeneous Networks. IEEE Access, 7, 102406-102416. https://doi.org/10.1109/access.2019.2931863
[13]	Gong, C., Fang, H., Yuan, L. and Zhang, N. (2024) A Composite Motion Control Method for Quadruped Robots in Trot Gait. Journal of Vibration and Control. https://doi.org/10.1177/10775463241272945

为你推荐

友情链接