模型预测控制下四足机器人的地形适应性优化

Optimization of Terrain Adaptability for Quadruped Robots under Model Predictive Control

摘要: 本研究旨在解决四足机器人在复杂工业地形上进行巡检时遇到的稳定性问题，通过开发一种基于模型预测控制(MPC)的位姿闭环调整策略，提高机器人在不规则地面上的自适应行走能力。利用IMU数据，本研究构建了机器人的运动学模型和姿态矩阵，从而精确控制和映射机器人的位姿。此外，利用基于MPC的躯干姿态调整算法，根据IMU测量的运动姿态参数，使机器人能够在动态环境中进行自我调整。经过仿真测试和实验室验证，证明了该算法的可行性与高效性。结果表明，该闭环控制策略有效增强了机器人在不规则地面上的稳定性，成功实现了姿态的自适应调整。

Abstract: This study aims to address the stability issues encountered by quadruped robots during inspections on complex industrial terrains. A pose closed-loop adjustment strategy based on Model Predictive Control (MPC) has been developed to enhance the robots’ adaptive walking capabilities on irregular surfaces. Utilizing IMU data, a kinematic model and an attitude matrix for the robot were constructed, allowing for precise control and mapping of the robot’s pose. Additionally, a trunk posture adjustment algorithm based on MPC was employed, which, informed by IMU-measured motion parameters, enables the robot to self-adjust in dynamic environments. The practicality and effectiveness of this algorithm were validated through simulation and laboratory tests. The results demonstrate that this closed-loop control strategy significantly enhances the stability of robots on uneven terrain and successfully achieves adaptive posture adjustment.

1. 引言

随着全球技术的自动化和智能化进程不断加快，越来越多的行业开始依赖机器人技术来提升生产效率和确保作业安全性。特别是在复杂的工业环境和具有挑战性的作业场景中，四足机器人凭借其优异的越障能力和卓越的运动性能，已成为应对复杂地形巡检任务的理想选择[1]-[3]。四足机器人能够在崎岖不平的地面上行走，并适应多变的环境。然而，这也意味着机器人在执行任务时面临着巨大的稳定性挑战。不同的地形条件以及不断变化的环境因素，如不规则地面、斜坡、障碍物及外界干扰，都会对其姿态控制产生负面影响，进而影响任务的可靠性与安全性。因此，为确保机器人能够在这些复杂条件下稳定运行，制定有效的姿态控制策略至关重要。

为应对这些挑战，近年来在四足机器人的研究中开发并采用了多种闭环控制系统，包括PID控制、LQR控制、卡尔曼滤波算法、基于模型预测的控制策略(MPC)、自适应控制系统，以及通过BP神经网络实现的智能控制方法等[4]-[6]。每种控制方法都有其优势，但在复杂环境下的效果各不相同。PID控制作为最传统的控制方法，虽然在简单场景中效果较好，但当环境变化较大时，往往表现出较高的滞后性和低效的响应。尤其是在频繁调整参数以适应环境变化时，PID控制容易引发振动问题，进而影响系统的稳定性[7]。由于四足机器人具有高度非线性的系统特性[8]，采用能够应对非线性的控制方法至关重要。在这一方面，MPC逐渐成为更为合适的选择。

模型预测控制(MPC)在四足机器人的运动控制领域中日益受到重视，其优势在于能够预测系统未来状态并进行预调节，从而显著提升机器人的运动稳定性。MPC的基本原理是通过优化算法不断预测系统的未来状态，并根据预测结果实时调整控制策略，确保系统在外界干扰下仍能保持稳定运行。近年来，MPC在四足机器人稳定运动控制中的应用得到了广泛研究，并展示了出色的控制效果。例如，苏黎世联邦理工学院(ETH)将落足点不等式约束引入MPC控制算法中，成功优化了四足机器人在复杂地形(如间隙、斜坡和垫脚石等)上的运动轨迹。这项技术已在ANY-mal四足机器人平台上得到验证，并取得了良好的实验效果[9]。

此外，MIT的Bledt [10]开发了基于非线性优化的预测模型框架(RPC)，该框架通过优化机器人状态和步态，提升了四足机器人在复杂地形中的适应性，并在Cheetah和Mini Cheetah机器人平台上得到了广泛验证[11]。同时，Dini Navid提出了一种基于MPC的步态调整策略，模拟测试表明该策略能够有效抵抗突发外力干扰，进一步验证了MPC在复杂环境中的应用优势[12]。Michael Neunert设计了一种简化的轨迹优化策略，通过简化机器人控制与估计框架的整合，提高了控制效率，并已成功应用于实际设备，展现出其在实际环境中的强大适应性和稳定性[13]。

本研究的主要目标是提升四足机器人在复杂地形中的自适应能力。为此，开发了一种基于模型预测控制(MPC)的新型闭环姿态调整策略，以替代传统的PID控制。与PID控制不同，MPC能够在复杂且动态的环境中，通过预测未来状态来调整控制策略，确保机器人在环境变化下依然能够保持姿态稳定性。通过构建运动学模型和姿态矩阵，设计了一个全面的坐标系统，从而更加精确地实现了机器人位姿控制和坐标转换。借助惯性测量单元(IMU)收集的实时数据，闭环控制策略能够根据外界干扰自动调整机器人的躯干姿态，进而实现动态自适应调整。

经过一系列仿真测试和现场实验，验证了该方法的有效性和实际应用价值。仿真结果表明，MPC控制器在面对各种复杂外界干扰时，能够保持机器人姿态的稳定，调节过程迅速且精确，系统响应速度显著优于传统控制方法。在实际测试中，四足机器人能够在斜坡、障碍物和不规则地面等环境中保持平稳运行，进一步证明了该闭环控制策略的优越性。总体而言，基于模型预测控制的闭环姿态调整策略不仅提高了四足机器人在复杂地形中的稳定性，还显著增强了其在实际应用中的可靠性和灵活性。

2. 机器人物理模型搭建

基于宇树科技的Alien-go四足机器人，构建了简化的结构模型，旨在研究其在不同地形条件下的适应能力。如图1所示，Alien-go由一个刚性机身和四条机械腿组成，每条腿包含三个关节，从而赋予机器人在三维空间中的运动自由度。每个关节的运动范围以及腿部的具体尺寸已在表1中详细列出，进一步完善了其结构和运动性能的描述。值得注意的是，即使该模型基于Alien-go机器人构建，其结构和结论对于其他类型的四足机器人也是通用的，适用于不同平台的适应性研究。

关节位置控制是四足机器人运动控制的核心步骤。要实现精确控制并适应多样化地形，首先必须建立单腿及整体机器人的运动学模型，以确保各关节在运动过程中的精确调控。

Figure 1. Physical model of the quadruped robot

图1. 四足机器人实体模型

Table 1. Structural parameters of a single leg

表1. 单腿结构参数表

参数名称	参数值	参数名称	参数值
髋部长度$l_1\text{/m}$	0.0838	Hip关节角${\theta }_1\text{/m}$	−0.85~0.85
大腿长度$l_2\text{/m}$	0.25	Thigh关节角${\theta }_2\text{/m}$	−3.14~3.14
小腿长度$l_3\text{/m}$	0.27	Calf关节角${\theta }_3\text{/m}$	−2.77~-0.65

2.1. 单腿运动学模型构建

为了深入研究机器人运动学，本研究以四足机器人右前腿(FR)为重点，采用改进的D-H方法构建了该腿的坐标系，如图2所示。通过这一方法，建立了机器人腿部的运动学模型，并推导出了相应的正向和逆向运动学方程。

Figure 2. Coordinate system distribution of the front right leg of the robot

图2. 机器人前右腿的坐标系分布

依据表1中的结构参数和几何配置，推导出了机器人腿部的D-H参数，并详细列出了坐标系的具体信息，见表2。

通过使用D-H方法，为单腿关节赋予了旋转和平移参数，并构建了齐次变换矩阵(见公式1)，该矩阵详细描述了关节的运动过程。在此基础上，进一步计算得出了单腿的正向运动学传递矩阵(见公式2)，并由此推导出腿部末端的三维位置(见公式3)。这些计算为理解和控制机器人动态运动提供了坚实的数学基础。

Table 2. D-H parameters

表2. D-H参数表

i	${\alpha }_{i-1}$	$a_{i-1}$	$d_i$	${\theta }_i$
1	0	0	${\theta }_1$	0
2	$\pi /2$	$l_1$	${\theta }_2$	$l_1$
3	0	$l_2$
4	0	$l_3$	${\theta }_3$	0

$i-1_{T_i}=\left[\begin{array}{cccc}c{\theta }_i& -s{\theta }_i& 0& a_{i-1}\\ s{\theta }_i& c{\theta }_{i}c{\alpha }_{i-1}& -s{\alpha }_{i-1}& -s{\alpha }_{i-1}{d}_i\\ s{\theta }_{i}s{\alpha }_{i-1}& c{\theta }_{i}s{\alpha }_{i-1}& c{\alpha }_{i-1}& c{\alpha }_{i-1}{d}_i\\ 0& 0& 0& 1\end{array}\right]$ (1)

$\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]=T_2^1{T}_3^2{T}_4^{3}T$ (2)

$\begin{array}{l}=l_1{c}_1+l_2{c}_1{c}_2+l_3{c}_1{c}_{23}+s_1{d}_2\\ =l_1{s}_1+l_2{s}_1{c}_2+l_3{s}_1{c}_{23}-c_1{d}_2\\ =l_2{s}_2+l_3{s}_{23}\end{array}$ (3)

方程式(1)至(3)描述了单腿正向运动学的传递关系，其中，$s_1$ 表示$\text{sin}{\theta }_1$ ，$c_1$ 表示$\text{cos}{\theta }_1$ ₁，其他符号遵循相同的规则定义。通过这些方程，可以利用关节的角度信息，结合各个关节的几何特性，精确地计算足端相对于腿部基座在三维空间中的具体坐标位置。

通过将方程扩展到复数域，并结合欧拉公式的求解过程，逆运动学可以确定每个关节的角度。在已知足端位置$P_f=\left[\begin{array}{ccc}{P}_x& P_y& P_z\end{array}\right]$ 的情况下，结合几何参数及运动学模型，能够推导出关节旋转角度${\theta }_1,{\theta }_2,{\theta }_3$ 。

$e^{ix}=\cos x+i\sin x$ (4)

$\{\begin{array}{l}=\arcsin \frac{P_z{d}_2\pm P_y\sqrt{P_z^2+P_y^2-d_2^2}}{P_z^2+P_y^2}\\ =\arccos \left(\frac{A^2+P_x^2-l_2^2-l_3^2}{2l_2{l}_3}\right)\\ =\arcsin \left(\frac{AB\pm P_x\sqrt{P_x^2+A^2-B}}{A^2+P_x^2}\right)\end{array}$ (5)

2.2. 整体运动学模型构建

在机器人攀登斜坡的过程中，协调腿部与机身姿态的联动是关键。机器人的姿态调整依赖于四足间的协同运动，而不是独立的姿态驱动单元。这种协调是通过基于单腿的D-H坐标系实现的，其中应用了标准的坐标系转换和平移原理来构建以机器人质心为核心的整体坐标系。

在该坐标系中，用{W}表示固定的世界坐标系，{B}标记为随机器人机身中心而变化的体坐标系，而$\left\{o_0\right\}$ 则是体坐标系中的基坐标系。通过这种方法，可以精确描述机器人在斜坡上的动态行为，确保其在复杂地形中的稳定性和效率。此外，机器人的尺寸参数，包括宽度2w，长度2b和高度2h，在设计时也需充分考虑，以适应不同的操作环境。这种系统的建立不仅提高了机器人的操作性能，也增强了其适应不同环境的能力。

Figure 3. Unified global coordinate system of the robot

图3. 机器人统一整体坐标系

在图3中，基坐标系$\left\{o_i\right\}$ 向体坐标系{B}的转换过程，不仅包括沿着Y轴的平移，还涉及围绕X轴和Y轴进行的旋转操作，这些操作协同作用，形成了最终的坐标系变换矩阵${ }^B{T}_i$ 。通过这种平移和旋转的组合，确保了坐标系之间的精确转换。具体来说，该变换首先通过Y轴的平移调整位置，随后绕X轴旋转以适应倾斜或斜坡的角度，最后绕Y轴旋转以完成方向的微调。

$\begin{array}{c}{ }^B{T}_i=\text{Trans}\left(0,k_{1}w,0\right)\text{Trans}\left(0,k_{2}b,0\right)\text{Rot}\left(Y_i,\frac{\pi }{2}\right)\\ =\left[\begin{array}{cccc}0& 0& 1& k_{2}b\\ 0& 1& 0& k_{1}w\\ -1& 0& 0& 0\\ 0& 0& 0& 1\end{array}\right]\end{array}$ (6)

参数$k_1,k_2$ 的取值决定了各腿的基坐标$q_2$ 系相对于体坐标系的位置，具体定义如下：

$\{\begin{array}{ll}{k}_1=1\hfill & 右前腿\left(\text{RF}\right)、左后腿\left(\text{LR}\right)\hfill \\ k_1=-1\hfill & 左前腿\left(\text{LF}\right)、右后腿\left(\text{RR}\right)\hfill \end{array}$

$\{\begin{array}{ll}{k}_2=1\hfill & 右前腿\left(\text{RF}\right)、左前腿\left(\text{LF}\right)\hfill \\ k_2=-1\hfill & 右后腿\left(\text{RR}\right)、左后腿\left(\text{LR}\right)\hfill \end{array}$

结合公式(6)的左侧和公式(2)的右侧，可以得到足端在体坐标系中的位姿矩阵$T_{if}$ ：

$T_{if}= {}^{B}T{}_{i4}^{0}T=\left[\begin{array}{cccc}{s}_{23}& c_{23}& 0& l_2{s}_2+l_3{s}_{23}+k_{2}b\\ s_1{c}_{23}& -s_1{s}_{23}& -c_1& l_1{s}_1+l_2{s}_1{c}_2+l_3{s}_1{c}_{23}-c_1{d}_2+k_{1}w\\ -c_1{c}_{23}& c_1{s}_{23}& -s_1& -l_1{c}_1-l_2{c}_1{c}_2-l_3{c}_1{c}_{23}-s_1{d}_2\\ 0& 0& 0& 1\end{array}\right]$ (7)

通过这一推导过程，可以将各腿的足端坐标统一映射到同一个参考坐标系中，这样不仅简化了位姿控制的处理，还能够大幅提升后续控制步骤的效率。

当四足机器人处于运动状态时，姿态必然会发生动态变化。为了准确刻画这种变化，本文使用了欧拉角来表示姿态的不同角度，具体用于描述体坐标系{B}相对于世界坐标系{W}的三维姿态转换。这其中涵盖了三个主要旋转角度：横滚角(绕x轴的旋转)、俯仰角(围绕y轴的旋转)和偏航角(绕z轴的旋转)。通过$q_1$ 、和$q_3$ 这三个变量来分别表示对应的旋转角度[14] [15]。在姿态变化过程中，按照$Z{q}_3-Y{q}_2-X{q}_1$ 的顺序依次进行旋转，如图4所示，可以通过这些旋转操作推导出从体坐标系{B}到世界坐标系{W}的旋转变换矩阵R，从而实现精准的姿态转换。

Figure 4. Rotation structure diagram

图4. 旋转结构图

$R=\left[\begin{array}{cccc}c{q}_{2}c{q}_3& -c{q}_{1}s{q}_3+s{q}_{1}s{q}_{2}c{q}_3& s{q}_{1}s{q}_3+c{q}_{1}s{q}_{2}c{q}_3& 0\\ c{q}_{2}s{q}_3& c{q}_{1}c{q}_3+s{q}_{1}s{q}_{2}s{q}_3& -s{q}_{1}c{q}_3+c{q}_{1}s{q}_{2}s{q}_3& 0\\ -s{q}_2& s{q}_{1}c{q}_2& c{q}_{1}c{q}_2& 0\\ 0& 0& 0& 1\end{array}\right]$ (8)

在已知足端坐标$P_f={\left[\begin{array}{cccc}{P}_x& P_y& P_z& 1\end{array}\right]}^{\text{T}}$ 的条件下，通过将旋转变换矩阵R与$T_{if}$ 相乘，可以将足端的初始坐标数据成功转换至惯性坐标系，完成坐标系间的映射和转换过程。这一矩阵乘法操作不仅精确，还能够确保各个坐标值的无缝衔接。具体的计算公式如下所示：

$P_e=R{T}_{if}$ (9)

在式(9)的两侧同时进行操作，将旋转变换矩阵R及其逆矩阵$T_{if}$ 分别相乘处理，这样可以确保整个转换过程保持一致性，完成所需的坐标转换和校正工作。

${ }_4^{0}T=T_{if}^{-1}{R}^{-1}{P}_e$ (10)

通过这种计算方式，可以精准获取足端在设定姿态下的位置信息。然后，利用逆运动学方法推导出各个关节的精确旋转角度，以便系统进行姿态调整。这种方法不仅确保了姿态的精确控制，还能够在动态环境中维持系统的稳定性，提升整体控制精度。

3. 姿态稳定控制的模型预测方法研究

模型预测控制(MPC)，有时也称为滚动优化控制，通过将当前系统状态作为初始输入，生成未来一段时间内系统的动态行为预测。基于这些预测，MPC通过解决一个优化问题(通常是二次规划问题)，来在约束条件下实现最优控制效果[16] [17]。图5展示了基于MPC方法的位姿闭环控制流程。

Figure 5. Pose closed-loop control flow

图5. 位姿闭环控制流程

MPC的结构可以分为三个主要模块：预测模型、滚动优化以及反馈修正[18]。其中，预测模型的主要作用是对系统的未来状态进行估算，这是MPC的基础部分；滚动优化则在整个预测时域内持续工作，是MPC的核心；馈校正则根据实际值与预测值的差异来调整模型，以实现更好的控制效果[19] [20]。

1) 模型构建

在使用MPC进行姿态控制时，四足机器人只需控制${\theta }_{\text{roll}},{\theta }_{\text{pitch}}$ 。根据MPC理论，对于系统的离散方程，其离散模型为[11]：

$\{\begin{array}{l}x\left(k+1\right)=A_{p}x\left(k\right)+B_{p}u\left(k\right)\\ y\left(k\right)=C_{p}x\left(k\right)\end{array}$ (11)

其中，$x\left(k\right)$ 、$u\left(k\right)$ 、$y\left(k\right)$ 分别表示系统的状态、控制输入和输出；而$x\left(k+1\right)$ 表示$k+1$ 时刻的系统状态，$A_p,B_p,C_p$ 为离散化后的系统矩阵。

在构建离散模型后，正式进入控制器设计阶段。假设MPC的预测时域长度为$N_p$ ，控制时域长度为$N_c$ 。当前时间点设为k，在预测时域内，系统的状态可以表示为：$x\left(k+1|k\right),x(k+2|k),\cdots ,x\left(k+N_p|k\right)$ ，控制量为$u\left(k\right),u(k+1),\cdots ,u\left(k+N_c-1\right)$ ，输出量为$y\left(k+1|k\right),y(k+2|k),\cdots ,y\left(k+N_p|k\right)$ 。由于$N_c<N_p$ ，控制量只能达到$u_e\left(k+N_c-1\right)$ ，无法达到$u_e\left(k+N_p-1\right)$ 。基于模型(11)，我们可以推导出预测状态量的状态空间方程如下：

$\begin{array}{l}\left[\begin{array}{c}y\left(k+1|k\right)\\ y\left(k+2|k\right)\\ y\left(k+3|k\right)\\ ⋮\\ y\left(k+N_p|k\right)\end{array}\right]=\left[\begin{array}{c}{C}_p{A}_p\\ C_p{A}_p^2\\ C_p{A}_p^3\\ ⋮\\ C_p{A}_p^{N_p}\end{array}\right]x\left(k\right)\\ +\left[\begin{array}{ccccc}{C}_p{B}_p& 0_{6\times 6}& 0_{6\times 6}& \cdots & 0_{6\times 6}\\ C_p{A}_p{B}_p& C_p{B}_p& 0_{6\times 6}& \cdots & 0_{6\times 6}\\ C_p{A}_p^2{B}_p& C_p{A}_p{B}_p& C_p{B}_p& \cdots & 0_{6\times 6}\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ C_p{A}_p^{N_p-1}{B}_p& C_p{A}_p^{N_p-2}{B}_p{C}_p{A}_p^{N_p-3}{B}_p& \cdots & C_p{A}_p^{N_p-N_p}{B}_p& \end{array}\right]\times \left[\begin{array}{c}u\left(k\right)\\ u\left(k+1\right)\\ u\left(k+2\right)\\ ⋮\\ u\left(k+N_c-1\right)\end{array}\right]\end{array}$ (12)

令

$Y={\left[y\left(k+1|k\right),y\left(k+2|k\right),y\left(k+3|k\right),\cdots ,y\left(k+N_p|k\right)\right]}^1,$

$F={\left[\begin{array}{ccccc}{C}_p{A}_p& C_p{A}_p^2& C_p{A}_p^3& \cdots & C_p{A}_p^{N_p}\end{array}\right]}^{\text{T}},$

$\Delta U={\left[\begin{array}{ccccc}u\left(k\right)& u\left(k+1\right)& u\left(k+2\right)& \cdots & u\left(k+N_c-1\right)\end{array}\right]}^{\text{T}},$

$\Phi =\left[\begin{array}{ccccc}{C}_p{B}_p& 0_{6\times 6}& 0_{6\times 6}& \cdots & 0_{6\times 6}\\ C_p{A}_p{B}_p& C_p{B}_p& 0_{6\times 6}& \cdots & 0_{6\times 6}\\ C_p{A}_p^2{B}_p& C_p{A}_p{B}_p& C_p{B}_p& \cdots & 0_{6\times 6}\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ C_p{A}_p^{N_p-1}{B}_p& C_p{A}_p^{N_p-2}{B}_p& C_p{A}_p^{N_p-3}{B}_p& \cdots & C_p{A}_p^{N_p-N_p}{B}_p\end{array}\right],$

因此，预测模型的方程式(12)可以表示为：

$Y=Fx\left(k\right)+\Phi \text{Δ}U$ (13)

2) 滚动优化和反馈调整

基于式(13)，可以得出输出Y的维度为$6N_p\times 1$ 。在四足机器人行走的控制中，仅需要调整两个姿态参数：${\theta }_{\text{roll}}$ 和${\theta }_{\text{pitch}}$ 。因此，引入一个新的预测输出$Y_d$ ，维度根据系统控制需求灵活设定，与期望输出$y_d$ 的维度保持一致。由此，我们可以推导出${\theta }_{\text{roll}}$ 和${\theta }_{\text{pitch}}$ 的预测输出$Y_d$ 的具体公式，形式如下展示。

$Y_d=F\left[\begin{array}{c}{0}_{12\times 6}\\ I_{6\times 6}\end{array}\right]y_d=FR{y}_{ϵ}$ (14)

基于式(13)和式(14)的结合，可以导出控制变量的变化对性能指标的具体影响，推导后的公式如下给出，展示了各因素之间的关联。

$J={\left(Y_d-Y\right)}^{\text{T}}\left(Y_d-Y\right)+\Delta U^{\text{T}}{R}_w\Delta U$ (15)

其中，$R_w=r_{w}I6N_c\times 6N_c$ ，$r_w\ge 0$ ，且$r_w\ge 0$ 为可调节的权重值，用于评估控制变量对性能指标的影响。将公式(13)与公式(14)替换至性能指标方程中，并进一步展开为控制量变化的具体形式如下：

$\begin{array}{c}J={\left(FR{y}_d-F{x}_e\left(k\right)-\Phi \Delta U\right)}^{\prime }\times \left(FR{y}_d-F{x}_e\left(k\right)-\Phi \Delta U\right)+\Delta U^{\text{T}}{R}_e\Delta U\\ ={\left(R{y}_d-x_e\left(k\right)\right)}^{\text{T}}{F}^{\text{T}}F\left(R{y}_d-x_e\left(k\right)\right)-2\Delta U^{\text{T}}{\Phi }^{\text{T}}F\left(R{y}_d-x_e\left(k\right)\right)+\Delta U^{\text{T}}\left({\Phi }^{\text{T}}\Phi +R_e\right)\Delta U\end{array}$ (16)

从优化设计的角度来看，减少系统输出误差是实现姿态控制稳定性的关键所在。该目标的达成只能通过在运动过程中精确调整控制变量来进行。因此，控制性能与$\text{Δ}U$ 密切相连。接下来，对性能指标函数(16)进行关于$\text{Δ}U$ 的导数计算，结果如下所示：

$\frac{\partial J}{\partial \Delta U}=-2{\Phi }^{\text{T}}F\left(R{y}_d-x_e\left(k\right)\right)+2\left({\Phi }^{\text{T}}\Phi +R_e\right)\Delta U$ (17)

为了优化性能指标，其偏导数应等于0，即公式(17)满足为零的条件：

$-2{\Phi }^{\text{T}}F\left(R_d-x_e\left(k\right)\right)+2\left({\Phi }^{\text{T}}\Phi +R_e\right)\text{Δ}U=0$ (18)

对公式(18)进行整理后，控制量的变化$\text{Δ}U$ 表达如下：

$\Delta U={\left({\Phi }^{\text{T}}\Phi +R_w\right)}^{-1}{\Phi }^{T}F\left(R{y}_d-x_e\left(k\right)\right)$ (19)

将${\theta }_{\text{roll}}$ 和${\theta }_{\text{pitch}}$ 视为离散模型中的控制量变化量，则下一步的控制量为：

$u\left(k+1\right)=u\left(k\right)+{\left[\Delta U\left(1\right)\Delta U\left(2\right)\Delta U\left(3\right)\Delta U\left(4\right)\Delta U\left(5\right)\right]}^{\prime }$ (20)

将控制量公式(20)代入离散模型公式(11)后，可以计算出机器人在时刻k + 1的状态值。在这个步骤中，状态变量的变化被准确描述，并为后续的姿态调整提供了基础。然后，重复该过程，直到姿态稳定控制的任务完全完成，确保整个系统在操作过程中保持平稳且一致的运行状态。

3) MATLAB验证

在MATLAB的仿真实验中，针对三种不同的条件进行了测试，具体的参数设置已在表3中列出。在实验中，使用黑色线条表示期望输出，而MPC控制器计算的实际响应则用红色线条标出。仿真结果展示在图6至图8中，每张图的(a)部分展示了系统在没有外界干扰时的响应情况，而(b)部分则反映了当引入

Table 3. Matlab validation parameters

表3. Matlab验证参数表

实验序号	曲线类型	幅值/rad	初始值/rad
1	直线	0.35	0.1
2	正弦曲线	0.35	0.1
3	直线 + 斜线正弦曲线	0.35/0.7/0.7	0.1
4	0	4	$l_3$

Figure 6. Linear trajectory tracking performance

图6. 直线轨迹跟踪效果

Figure 7. Sine wave trajectory tracking performance

图7. 正弦曲线轨迹跟踪效果

Figure 8. Combined trajectory tracking performance

图8. 组合轨迹跟踪效果

[−0.1, 0.1]范围内的干扰时，系统的调整表现。

根据图6至图8的仿真结果，MPC控制器在多种期望轨迹下展现了出色的跟踪能力。图6(a)显示，系统在短短0.15秒内达到了目标值，反应速度相当迅速；而在图8(a)中，系统在0.12秒时就完成了目标跟踪，进一步表明该控制器具有快速响应的优势，并且超调现象非常轻微。图6(b)到图8(b)展示了在环境干扰的情况下，MPC控制器依然能够保持高效的目标跟踪，系统的误差与滞后都保持在较小的范围内。通过MATLAB的仿真结果可以确认，MPC控制器在提升四足机器人稳定性控制方面表现优异，不仅在响应时间上有优势，其跟踪精度也明显优于开环控制策略。

4. 实验分析

4.1. 仿真实验

在此次模拟实验中，利用了Gazebo平台进行环境搭建，使用的实验装置是具备双自由度(即Roll和Pitch)的振动平台。为了模拟现实中的干扰情况，平台通过输入幅度为0.2、频率为100 Hz的正弦波信号进行摆动测试。该实验的核心目的是评估四足机器人在遭遇这种外界摆动干扰时，是否能够维持其机身的水平稳定状态，并且测试MPC控制器在提升机器人姿态稳定性方面的控制效果。为充分分析其抗干扰性能，实验设置了四种不同的测试情境。在相同的摆动幅度和频率下，分别测试四足机器人在单自由度(仅Pitch轴)与双自由度(Roll轴与Pitch轴组合)下，静态姿态与动态步态两种情况下的抗干扰能力。

图9展示了实验中记录的结果，呈现出在不同工况下的四足机器人姿态表现。通过这些实验，证明了MPC控制器在静止和运动两种状态下，都能够有效保持机器人的稳定性。

在虚拟实验中，数据通过虚拟IMU传感器采集，图10展示了各类测试条件下的姿态变化信息。图10的各子图(a)至(d)分别与图9的实验数据相对应。尽管实验平台的角度波动范围较大(从−0.25弧度到0.25弧度)，MPC控制器依然通过闭环反馈控制有效限制了四足机器人的姿态变化，将其机身的摆动保持在−0.05弧度到0.05弧度的较小范围内。该控制器展现出良好的反应性能，包括低滞后、高精度以及有限的波动范围。仿真结果表明，MPC控制器能够在仿真环境中有效提高四足机器人的稳定性，特别是在复杂摆动条件下，控制效果尤为显著。

4.2. 实物设备测试与验证

为了验证机器狗在波动环境下算法的表现，研究设计并搭建了一个单自由度摆动实验平台(见图11)，该平台的摆动幅度变化在0.15至0.3弧度之间。通过整合里程计提供的姿态信息，以及相机IMU输出的数据，来实时跟踪和监控实验平台及机器狗在摆动过程中的姿态调整情况。本次实验设定了两种不同的测试场景：一是评估机器狗在静态站立时的姿态稳定性，二是分析其在Trot步态下的平衡响应和调节能力，以全面考察其在不同姿态模式下应对波动环境的效率。

1) 评估系统在静态条件下的稳定性测试

在实验过程中，由于人为施加的周期性摆动，平台出现了误差影响。图12呈现了整个摆动周期的细节，展示了当四足机器人遭遇大幅度摆动时，如何通过腿部的调节动作保持其机身的稳定水平。此外，机器人能够通过精准的姿态控制，确保在这些动态环境下保持平衡。图13则记录了实验平台与机器狗的俯仰角变化情况，显示出机器人在运动过程中与平台摆动的高度同步。在平台达到最大倾斜角时，机器狗的俯仰角调整到了大约0.05弧度，未能完全恢复到零度。这一现象主要是由于静态条件下电机反应的滞后性导致姿态调整不够迅速，从而影响了机身的稳定性。尽管如此，机身的平衡状态整体上仍保持良好，0.05弧度的偏差对稳定性的影响较小。

2) 评估系统在动态条件下的稳定性测试

动态稳定性测试的目的是评估机器狗在不同状态下的稳定控制表现，包括静止站立、Trot步态以及受到外界环境干扰时的反应能力。从图14可以看出，在Trot步态下，相比于静止状态，机器狗的机身更加容易保持水平。这主要归因于在动态运动中，电机能够更高效地进行姿态调整，以应对身体的摆动。

Figure 9. Pose control performance in different scenarios

图9. 不同情境下位姿控制效果

Figure 10. Test results

图10. 测试结果

Figure 11. Experimental test platform

图11. 实物实验测试平台

此外，图15(a)显示，在平台进行周期性摆动的过程中，机器狗的机身俯仰角趋近于零，这表明Trot步态有助于维持机身的平衡。然而，图15(b)揭示了由于Trot步态的对角线扭转特性，机器狗的横滚角较大，导致了一定程度的姿态不稳定。

Figure 12. Process of a cyclic back-and-forth swing

图12. 一个周期性往返摆动的过程

Figure 13. Variation in angular fluctuation of the robot dog on the Pitch axis

图13. 机器狗在Pitch轴上的角度波动值变化

Figure 14. Process of a cyclic back-and-forth swing

图14. 一个周期性往返摆动的过程

Figure 15. Recorded measurement data of various parameters during dynamic balance testing

图15. 动态平衡测试过程中各项参数的测量数据记录

5. 结束语

本研究成功开发了适用于复杂且不稳定工业环境的四足机器人姿态闭环控制系统，基于优化设计的模型预测控制算法，实现了稳定的闭环控制结构。该系统通过机载IMU实时收集姿态数据，确保反馈和动态调整，从而显著提升了姿态控制的精度和环境适应能力。机器人通过误差反馈自动调整姿态，确保在动态环境中的稳定性。此外，关节位置控制方法的引入有效减少了计算需求，简化了程序在不同平台间的迁移。广泛的实验验证表明，该四足机器人在模拟与真实测试中的极端摇摆环境下表现出优异的稳定性和灵活性。响应曲线分析显示，机器人在动态环境中的目标追踪效率较高；在工业环境下的测试进一步证明了该控制系统的高容错性和稳定性。本文提出的控制策略增强了机器人在复杂环境中的操作稳定性，显著提升了其在多变场景中的应用潜力。未来研究将着重优化四足机器人在复杂地形中的适应能力和在自然条件下的自主交互性能。