基于改进落足点的四足机器人爬楼梯MPC控制算法研究

Research on MPC Control Algorithm for Quadruped Robot Stair-Climbing with Improved Foothold Placement

摘要: 本文针对四足机器人爬楼梯这一特定场景，在传统MPC控制的基础上，提出一种改进落足点的腿部规划方法，并将其应用到MPC控制器中。通过在Matlab中建立四足机器人与楼梯的仿真模型，实现了四足机器人爬楼梯的控制过程。并通过引入姿态角的标准差这一概念，将本文所提出的优化后MPC控制方法与传统MPC控制算法下四足机器人爬楼梯过程中的姿态角标准差作对比，验证了所提出方法的优越性。

Abstract: This paper addresses the specific scenario of a quadruped robot climbing stairs. Building upon traditional Model Predictive Control (MPC), an improved leg planning method for foothold placement is proposed and integrated into the MPC controller. By establishing a simulation model of the quadruped robot and stairs in MATLAB, the control process for the robot’s stair-climbing is realized. Furthermore, by introducing the concept of the standard deviation of posture angles, a comparison is made between the standard deviation of posture angles during stair-climbing for quadruped robots controlled by the optimized MPC method proposed in this paper and those controlled by traditional MPC algorithms. This comparison validates the superiority of the proposed method.

1. 引言

近年来，机器人技术实现了显著进步，而四足机器人因其出色的运动能力和较强的地形适应性成为应用前景非常广阔的特种机器人，常被用于探索、搜救和军事领域等，能够在各种复杂地形中实现多种步态的行走，适应多样化的工作环境[1]，并表现出良好的稳定性。与双足机器人相比，四足机器人四条腿行走更加平稳，且具有更大的负载能力。与六足机器人相比，四足机器人在机械结构设计上更为简洁，且在运动控制上更为简便。

如今有许多学者对复杂地形下的四足机器人控制问题提出解决方案。对于四足机器人在复杂地形下的地形识别和路径规划问题，张慧等提出了一种算法，能够结合多种传感器数据，如腿部里程计、视觉和惯性测量，以提高路径规划的准确性和机器人的地形适应性[2]。常青等[3]研究了四足机器人在斜坡上的运动控制问题，文章提出了一种自适应控制算法，能够根据斜坡的倾斜角度和地面条件调整机器人的姿态和步态，以保持稳定运动。赵江波等[4]探讨了四足机器人步态参数的优化问题，并提出了一种探索性行走策略，通过优化步态参数，提高了机器人在未知环境中的行走效率和稳定性。最近几年，模型预测控制(MPC)逐渐运用到了四足机器人的稳定运动控制中，许多学者对其进行深入研究，并取得了不错的控制效果，苏伊士联邦理工大学的研究团队在模型预测控制(MPC)中引入了不等式约束，以处理机器人在存在间隙、斜坡和垫脚石等复杂地形中的运动问题[5]。这一创新方法已经在ANY-mal四足机器人上进行了测试，并证明了其有效性。同时，Bledt团队[6]开发了一种新型的基于非线性优化的预测控制框架。该框架能够在预测范围内对机器人的状态、脚步位置和地面反作用力进行优化。这一算法已经在MIT的Cheetah和Mini Cheetah四足机器人上进行了验证，显示出在复杂环境中优秀的动态速度追踪能力。

本文在前人已有研究的基础上，通过使用开源的宇树科技四足机器人A1模型，结合模型预测控制算法与所提出的改进的落足点规划算法，实现了四足机器人仿真环境下爬楼梯的过程，通过与传统MPC控制算法的对比，验证了算法的有效性。

2. 四足机器人模型参数

2.1. 四足机器人结构

四足机器人三维模型是仿真实验的基础，本文所选择的机器人模型为宇树科技四足机器人A1，该型号四足机器人由机身和四条腿组成，腿部构型方式为全肘型，每条腿有三个自由度，共12个自由度，机器人模型如图1所示。而表1则描述了四足机器人的基本参数，包括机器人重量，机身大小及各个腿部关节的长度。

Figure 1. 3D model of a quadruped robot

图1. 四足机器人三维模型

Table 1. Robot physical parameters

表1. 机器人物理参数

参数名称/单位	参数值	参数名称/单位	参数值
质量m/kg	12	髋关节长度L1/m	0.045
机身长度b/m	0.1805	大腿长度L2/m	0.2
机身宽度w/m	0.1308	小腿长度L3/m	0.2

2.2. 运动学模型

定义机身质心坐标系{B}位于机器人质心，世界坐标系{W}位于机身坐标系投影与地面的垂直方向。机器人左前、右前、左后、右后腿分别用符号LF、RF、LB、RB表示。由于四足机器人腿部构型均采用肘式结构，且四条单腿的结构参数相同，故仅选取一条腿进行运动学分析，建立机器人质心与足端的运动学关系。采用D-H坐标法建立的左前腿运动学方程为：

$P_{LF}=\left[\begin{array}{c}{p}_x\\ p_y\\ p_z\end{array}\right]=\left[\begin{array}{c}b+L_2{s}_2+L_3{s}_{23}\\ w+L_1{s}_1+L_2{s}_1{c}_2+L_3{s}_1{c}_{23}\\ -L_1{c}_1-L_2{c}_1{c}_2+L_3{c}_1{c}_{23}\end{array}\right]$ (1)

其中$P_{LF}$ 为左前腿足端相对于机器人质心坐标系{B}的位置，$p_x,p_y,p_z$ 分别代表足端在质心坐标系下投影到x轴，y轴，z轴上的位移。且

$\{\begin{array}{l}{s}_i=\sin {\theta }_i,\\ c_i=\cos {\theta }_i,\\ s_{ij}=\sin \left({\theta }_i+{\theta }_j\right),\\ c_{ij}=\cos \left({\theta }_i+{\theta }_j\right)\end{array}$ (2)

$\theta {}_i$ 代表第i个关节的转动角度，$i=1,2,3$ 分别表示髋关节、大腿关节和小腿关节。

机身坐标系相对于世界坐标系的位置可以表示为：

${}_B{}^{W}P={\left[{}^{W}p{}_x,{}^{W}p{}_y,{}^{W}p{}_z\right]}^{\text{T}}$ (3)

机身坐标系相对于世界坐标系的姿态可以用旋转矩阵${}_B{}^{W}R$ 表示，那么足端在世界坐标系下的位置可以表示为：

${}^{W}P{}_{LF}={}_B{}^{W}P+{}_B{}^{W}R{P}_{LF}$ (4)

2.3. 躯干简化动力学模型

由于四足机器人腿部惯性效应相对于机身来说可以忽略不记，为了简化计算，不考虑腿部动力学，只针对躯干建立动力学模型，即单刚体动力学；对其进行受力分析可以知道，此时四足机器人仅受到自身重力和地面反作用力，根据牛顿–欧拉运动定律，此时机器人在世界坐标系下的动力学方程为：

$\{\begin{array}{l}{}^W\ddot{P}=\frac{F}{m}-g\\ I\dot{\omega }={\displaystyle \sum _{i=1}^n{P}_i}\times f_i\end{array}$ (5)

式中：${}^W\ddot{P}$ 表示四足机器人在世界坐标系下的加速度，F表示足端所受的合力。I表示机器人在世界坐标系下的惯性张量，$\dot{\omega }$ 表示机器人在世界坐标系下的角加速度。$f_i$ 表示机器人每条腿足端受到地面的作用力。

基于公式(5)的简化动力学，建立四足机器人的状态方程，可以得到如下关系：

$\underset{\dot{x}}{\underbrace{\frac{\text{d}}{\text{d}t}\left[\begin{array}{c}{p}_{com}\\ \varphi \\ v_{com}\\ w\\ g\end{array}\right]}}=\underset{A}{\underbrace{\left[\begin{array}{ccccc}0& 0& I& 0& 0\\ 0& 0& 0& {}_B{}^{W}R& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& S\end{array}\right]}}\underset{x}{\underbrace{\left[\begin{array}{c}{p}_{com}\\ \varphi \\ v_{com}\\ w\\ g\end{array}\right]}}+\underset{B}{\underbrace{\left[\begin{array}{cccc}0& 0& 0& 0\\ 0& 0& 0& 0\\ \frac{1}{m}& \frac{1}{m}& \frac{1}{m}& \frac{1}{m}\\ I^{-1}{P}_1\times & I^{-1}{P}_2\times & I^{-1}{P}_3\times & I^{-1}{P}_4\times \\ 0& 0& 0& 0\end{array}\right]}}\underset{u}{\underbrace{\left[\begin{array}{c}{f}_1\\ f_2\\ f_3\\ f_4\end{array}\right]}}$ (6)

在求解上述线性时变系统过程中，需要进行离散化求解，得到最优的u。在求解间时间内设机器人能够保持状态轨迹跟随，将线性时变系统简化成单位时间间隔内的线性时不变系统，离散化过程中使用零阶保持器，以固定时间间隔$\Delta t$ 进行离散化，可以把动力学方程变成简便的状态空间形式：

$\dot{x}\left(t\right)=Ax\left(t\right)+Bu\left(t\right)$ (7)

3. 基于模型预测控制的爬楼梯控制算法

3.1. 模型预测控制基本原理

模型预测控制(Model Predictive Control，简称MPC)是一种先进的控制算法，它结合了控制理论和优化理论，用于处理具有不确定性和约束的复杂动态系统。与传统的控制方法相比，MPC能够提供更好的性能和更高的灵活性。MPC的核心思想是通过对未来行为的预测来优化当前的控制动作。这一过程通常包括以下几个关键步骤。

在模型预测控制(MPC)的框架内，首先必须构建一个数学模型以表征被控对象的动态特性。模型的精确度对于MPC的性能具有决定性影响。随后，基于该模型，MPC通过模拟一系列预定的未来控制输入，对系统在有限的预测时域内的行为进行预测。在此预测时域中，系统的未来状态被用于评估控制策略的效能。进一步地，需要定义一个目标函数用以量化控制性能的期望值。该函数可能涵盖跟踪误差、控制能量消耗、系统稳定性等多个维度。目标函数的构建对MPC的性能具有直接影响。在每个控制周期，MPC通过求解一个优化问题来最小化该目标函数，该优化问题涵盖了当前及未来预测时域内的控制序列。此外，优化问题中还需纳入系统的各类操作性约束，包括但不限于控制输入的限制和状态变量的界限。优化问题求解完成后，仅将序列中的第一个控制动作实施于实际系统。随后，系统的实际状态通过测量或估计得到，并用于更新模型，以为下一个控制周期的预测和优化提供准确的基础数据。这一过程的迭代执行，使得MPC能够动态适应系统状态的变化和外部环境的不确定性，从而实现对复杂系统的高效控制。模型预测控制框图如图2所示。

Figure 2. Model predictive control block diagram

图2. 模型预测控制框图

3.2. 基于MPC的足端力求解

利用离散有限时域预测控制器来实现四足机器人足端期望接触力的计算。一般情况下MPC控制器在每次迭代时，从当前状态开始，基于控制输入和状态轨迹的约束条件，在有限预测时域中寻找控制输入的最优序列和对应的状态轨迹。上述过程在每次选代中都是重复进行的，只需要设定第一次计算控制输人轨迹的时间步长。考虑一个步长k的标准形式MPC问题[7]，设$x_{i+1,ref}$ 是i + 1时刻系统的参考状态，$x_{i+1}$ 是i + 1时刻系统状态，得到如下求解最优足底力的方程组

$\begin{array}{l}\underset{x,u}{\min }{\displaystyle \sum _{i=0}^{k-1}\Vert x_{i+1}-x_{i+1,ref}\Vert Q_i+\Vert u_i\Vert R_i}\\ \text{st}\text{.}{x}_{i+1}=A_i{x}_i+B_i{u}_i,i=0,\cdots ,k-1\\ c_{\min }\le u_{i,m}\le c_{\max }\end{array}$ (8)

使用MATLAB的quadprog函数进行求解，可以得到最优的控制输入量u，即最优足底力F。

4. 楼梯地形建模分析

4.1. 楼梯模型建立

在Matlab simulink中建立楼梯三维模型，使用simulink中的Brick Solid模块建立一个五级楼梯模型，每一级楼梯的长为2米，楼梯台阶宽度为20厘米，台阶高度为10厘米，楼梯模型如图3所示。

4.2. 改进的爬楼梯落足点规划

在机器人行走过程中，腿部的摆动动作主要是为了确保足部能够准确地到达预定的落脚点。通过这种方式，可以间接地调整机器人的躯干姿态和移动速度，从而维持整个机体的平衡。一个精心设计的足部轨迹有助于减少落地时对地面的冲击，增强腿部对复杂地形的适应性。在规划足部摆动路径时，需要特别关注几个关键因素，包括目标落脚位置、步幅大小和腿部抬升的高度。用户可以输入机器人期望的线速度和角速度参数，这些参数包括横向和纵向速度，它们将被转换为足部的位移量[8]。具体的转换关系可以表示为以下公式：

$x_e=\frac{1}{2}\dot{x}T-k\left({\dot{x}}_d-\dot{x}\right)$ (9)

$\dot{x}$ 是摆脚沿脚端轨迹在x方向上的速度，T为支撑相的周期，${\dot{x}}_d$ 为沿x方向的期望速度，k为速度差增益。

Figure 3. 3D model of the staircase

图3. 楼梯三维模型

在爬楼梯时，预测下一个所需的脚的位置可以按照线性倒摆模型(LIPM)确定。LIPM根据髋关节位置、前馈和后馈项确定足部位置。但与行走和跑步运动的不同之处在于，不是将脚的z位置投影到0，而是将预测的X和Y位置与楼梯台阶的“已知”位置进行比较。并且该位置存在的楼梯的相应Z值(如果有)将设置为脚的Z值。

$\{\begin{array}{l}{p}_x=x_0+\left(x_e-x_0\right)t/T_{sw}\\ p_y=0\\ p_z=\lambda z_e,\lambda =1,\cdots ,5\end{array}$ (10)

式中，$p_x,p_y,p_z$ 分别表示足端在世界坐标系下投影到坐标轴上的位置。$x_0$ 是足端当前位置在x轴方向上的投影，$x_e$ 是足端期望位置在x轴上的投影。$z_e$ 是台阶的高度，$\lambda$ 的取值与$x_e$ 的位置有关，当$x_e$ 的值在第一节台阶的宽度范围内时，$\lambda =1$ ，依次类推。

此外，在爬楼梯步态中，本文提出了在楼梯边缘附近实施了一个“死区”。如果上述方程计算出的理想足部位置“过于接近”楼梯边缘，那么理想足部位置就会被修改为与楼梯底部或顶部的预定距离，可以有效防止离楼梯底部太近可能会导致的绊倒，或者如果离楼梯顶部太近可能会导致的滑落。这种方法不依赖于其他诸如视觉摄像机之类的传感器，且易于实现。

5. 仿真实验

为了验证所提出的方法的有效性，采用Matlab/Simulink平台对其进行爬楼梯仿真验证，在Simulink中使用接触力模块将机器人模型与楼梯模型连接，爬楼梯目标设定在4.5秒内完成，仿真结果如图4所示，

Figure 4. Quadruped robot stair-climbing simulation

图4. 四足机器人爬楼梯仿真

从仿真结果可以看到，在所提出的基于改进轨迹规划的MPC控制算法下，四足机器人在预期时间内能够稳定爬上台阶高度为10 cm的楼梯。

为了更进一步分析四足机器人在爬楼梯过程中的稳定性情况，对四足机器人爬楼梯过程中的质心状态参数进行提取分析，同时将本文所提出的方法与没有设置死区规划的传统MPC控制器做对比，其中图5是本文中改进步态规划后的四足机器人爬楼梯质心状态参数，图6是传统的MPC控制方法所得到四足机器人爬楼梯的状态参数，每张图的(a)图表示机器人质心姿态角随时间的变化曲线，其中Roll角(即横滚角)描述了机器人绕其前进方向轴(通常为X轴)的旋转，其期望值为0；Pitch角(即俯仰角)描述了机器人绕其垂直于地面的轴(通常为Y轴)的旋转，其期望值为0.25 rad；Yaw角(即偏航角)描述了机器人绕其垂直轴(通常为Z轴)的旋转，其期望值为0。(b)图表示机器人质心位置随时间的变化曲线，其中X，Y，Z三条曲线分别表示质心在X轴，Y轴，Z轴坐标上的投影。

(a) COM Orientation—Time Graph (b) COM Position—Time Graph

Figure 5. The centroid state parameters of the quadruped robot under the method of this paper

图5. 本文方法下四足机器人质心状态参数

(a) COM Orientation—Time Graph (b) COM Position—Time Graph

Figure 6. The centroid state parameters of the quadruped robot under traditional MPC control

图6. 传统MPC控制下四足机器人质心状态参数

通过图5(b)与图6(b)可以看到，两种方法的机器人质心轨迹相似，表明这两种方法都能控制四足机器人爬上所设置的楼梯地形，但通过对比图5(a)和图6(a)，可以看到采用本文所提出的方法，四足机器人爬楼梯过程中的横滚角变化范围在+0.2 rad~−0.2 rad之间；而采用传统MPC控制方法的四足机器人横滚角出现明显大于0.2 rad的时间段，表明此时机器人机身的稳定幅度较差。通过对比可以发现本文所使用的方法四足机器人的俯仰角的变化剧烈程度也小于传统控制方法。

为了更进一步说明本文所采用方法的稳定性，引入标准差的概念来定量分析机器人个姿态角度的变化波动幅度。标准差的计算公式如下：

$\sigma =\sqrt{\frac{1}{n}{\displaystyle \sum _{i=1}^n{\left(x_i-\mu \right)}^2}}$ (11)

$\sigma$ 是姿态角的标准差，n是仿真时长内采样点的数量，x_i是每个采样点对应的各自姿态角的值，$\mu$ 是各个姿态角期望值，标准差越小表示角度的波动幅度越小，机身也就越平稳。通过计算可以得到本文所提出策略的标准差${\sigma }_1=\left[\begin{array}{ccc}0.045666& 0.060441& 0.025566\end{array}\right]$ ；传统MPC策略下标准差的值为 “${\sigma }_2=\left[\begin{array}{ccc}0.063319& 0.069226& 0.027156\end{array}\right]$ 。通过对比可以发现，本文所提出的控制策略下四足机器人各个姿态角的标准差结果都要优于未优化过的方法。

6. 结论

本文在MPC控制的基础上，提出了一种建立落足点“死区”的落足点优化方法，提高了MPC控制器在四足机器人爬楼梯场景下的稳定性。通过建立质心姿态角的标准差方程与评判方法，对比本文优化方法与传统MPC控制方法，发现在采用本文所提出的方案下，四足机器人在攀爬楼梯时的各个姿态角变化范围在期望姿态角范围的±0.2 rad内变化，且姿态角标准差的值要优于传统方法的姿态角标准差值，说明了该优化方法在爬楼梯这一场景下的有效性及评判方法的准确性。