基于模糊MPC算法的双轮足机器人运动控制研究

Research on Motion Control of Bipedal Wheel-Legged Robots Based on the Fuzzy MPC Algorithm

摘要: 针对双轮足机器人在外部干扰下易姿态发散、对精确模型敏感且在线求解压力大的问题，本文提出一种模糊调度——模型预测控制(Fuzzy-MPC)一体化方案。方法以倒立摆模型近似为预测模型，在不改变凸QP结构的前提下：1) 依据IMU的姿态误差与变化率构造风险度量，将其连续映射到“采样步长/预测域”以实现自适应预测时域；2) 设计两路Mamdani模糊器(位移–速度、俯仰角–角速度)生成误差补偿系数，按指数衰减注入到滚动预测轨迹，抵消模型失配与数值滞后的影响。以MATLAB平台进行对比显示：在15˚与30˚初始倾角两种场景下，相较于PD与标准MPC，Fuzzy-MPC可在更短稳定时间与更小位移漂移的同时，显著降低约束违背与执行器持续饱和的风险，体现出更好的鲁棒性与能耗友好性。

Abstract: To address the issues that bipedal wheel-legged robots are prone to posture divergence under external disturbances, are sensitive to accurate models, and face heavy online computation demands, this paper proposes an integrated Fuzzy-MPC (Fuzzy Scheduling-Model Predictive Control) scheme. The method uses an inverted pendulum model as an approximate prediction model, and without changing the convex QP structure: 1) it constructs a risk metric based on the IMU’s attitude error and its rate of change, continuously mapping it to the “sampling step/prediction horizon” to achieve an adaptive prediction horizon; 2) it designs two Mamdani fuzzy controllers (displacement-velocity and pitch angle-angular velocity) to generate error compensation coefficients, which are injected into the rolling prediction trajectory with exponential decay to counteract model mismatch and numerical lag. Comparative results on the MATLAB platform show that in scenarios with initial tilt angles of 15˚ and 30˚, compared with PD and standard MPC, Fuzzy-MPC achieves shorter stabilization times and smaller displacement drift while significantly reducing the risks of constraint violations and continuous actuator saturation, demonstrating better robustness and energy efficiency.

1. 前言

双轮足机器人平衡系统在矢状面内运动需要兼顾俯仰稳定与位移抑制，传统PD控制实现简单，但对大幅扰动与约束处理能力有限；常规MPC擅长处理输入/状态约束与平滑惩罚，却对模型精度与在线求解时间较敏感[1]。为在不牺牲MPC约束优势的同时提升非线性工况下的稳定性，本文将模糊经验规则与风险感知的时域自适应嵌入到MPC框架，形成Fuzzy-MPC：由模糊器在线调整补偿强度，由风险度量来调节预测域与采样周期，二者协同面向大角度、快扰动情形实现更稳健的瞬态与稳态表现。本文给出统一的模型、指标与评测流程，便于与标准MPC和PD控制进行实验对比。

2. 系统模型

MPC要实现预测功能，前提是具备能够准确描述系统运动的数学模型。相比仅基于几何关系的运动学描述，动力学模型能够把关节/车轮驱动扭矩与姿态、位移等输出之间的因果关系明确化，使规划与控制在设计阶段就纳入实际约束，从而获得更平滑的力矩分配，避免运动学前馈在加速度突变时引发的扭矩冲击。对于双轮足机器人，常使用的模型可以近似为倒立摆模型[2]；为便于在线优化与稳定性分析，通常在直立平衡点附近对其线性化，来得到适合MPC求解的离散线性化的模型(图1和图2)。

Figure 1. Simplified model of the bipedal wheel-legged robot

图1. 双轮足机器人简化模型

Figure 2. Dynamics model of inverted pendulum

图2. 倒立摆动力学模型

基于上述动力学模型，在平衡点处可将轮关节驱动矩M表示为当前状态量的函数，关系如下：

$\ddot{\eta }:\left(\frac{J_w}{r^2}+m_w\right)\cdot L\ddot{\eta }-\left(\frac{J_w}{r^2}+m_w+m\right)\cdot g\eta -\left(\frac{mL}{r}+\frac{J_w}{r^2}+m_w+m\right)\cdot \frac{M}{mL}=0$ (1)

$\ddot{\delta }:\left(\frac{J_w}{r^2}+m_w\right)\cdot \ddot{\delta }+mg\eta +M\cdot \left(\frac{1}{L}+\frac{1}{R}\right)=0$ (2)

用状态空间方程表示为：

$\dot{X}=\left(\begin{array}{cccc}0& 1& 0& 0\\ A_a& 0& 0& 0\\ 0& 0& 0& 1\\ A_b& 0& 0& 0\end{array}\right)X+\left(\begin{array}{c}0\\ B_c\\ 0\\ B_d\end{array}\right)$ (3)

式中：$A_a=\frac{\left(\frac{J_W}{r^2}+m_W+m\right)g}{\left(\frac{J_W}{r^2}+m_W\right)L}$ ，$A_b=\frac{-mg}{\frac{J_W}{r^2}+m_W}$ ，$B_c=\frac{\frac{J_W}{r^2}+\frac{mL}{r}{m}_W+m}{\left(\frac{J_W}{r^2}+m_W\right)m{L}^2}$ ，$B_d=\frac{-\left(\frac{1}{L}+\frac{1}{R}\right)}{\frac{J_W}{r^2}+m_W}$ 。

3. MPC控制方法设计

3.1. MPC原理分析

MPC策略主要包含三个核心环节：

预测模型：基于被控对象的动态描述(如状态空间、传递函数、阶跃/脉冲响应等)，在预测时域P内生成未来输出的轨迹预报。

滚动优化：每个采样时刻利用参考轨迹与预测输出，在线求解控制时域M (M ≤ P)上的最优控制序列，通常将跟踪误差与控制增量写入目标函数J并以QP等方法求解。

反馈校正：虽然计算出了M个未来的控制动作，但只取第一个控制动作作为Δu (k)。然后在下一个采样时刻k + 1，重新测量当前的输出值，利用这个更新的测量值来对模型预测进行校正，再重新生成对未来M步的预测，重新计算优化出的控制序列。

总而言之，MPC的工作流程就是在每个采样时刻，基于当前测量，预测未来输出，在线优化未来控制序列，实施第一个控制动作，然后滚动进行。

3.2. 问题描述分析

经典MPC严重依赖一个预先设定的、通常是线性化的动力学模型。当预测模型与真实系统差异巨大时，MPC基于错误模型做出的最优预测就失去了意义，会导致控制性能急剧下降，甚至造成机器人失稳[3]。另外，当系统模型状态维数高，控制输入维数也多时，MPC需要在极短的控制周期内(通常1~5毫秒)求解一个高维、带约束的优化问题，这对于硬件来说有极高的要求。为了改善这一问题，笔者将从预测模型中的预测和误差补偿方面进行研究。

3.3. 基于能量与非线性度量的自适应预测机制

为了解决固定步长MPC在快速动态响应与计算效率之间的矛盾，本文设计了一种基于系统“风险度”的时域自适应机制。

3.3.1. 风险度量函数的构建与物理意义

风险度量$r_k$ 的本质是对系统当前状态偏离稳态平衡点(Upright Equilibrium)程度的归一化评估。考虑到倒立摆系统的势能与姿态角$\eta$ 相关，动能与角速度$\dot{\eta }$ 相关，本文构建了如下基于加权二次型的风险评价函数：

$r_k=sat\left(\sqrt{{\left(\frac{{\eta }_k}{{\eta }_{\max }}\right)}^2+\lambda {\left(\frac{{\dot{\eta }}_k}{{\omega }_{\max }}\right)}^2}\right)\in \left[0,1\right],$ (4)

式中，${\eta }_{\max }$ 和${\omega }_{\max }$ 分别为机器人能够维持自恢复的极限倾角与极限角速度，$\lambda$ 为权重系数。该度量函数的物理合理性在于：

1) 能量近似：该形式近似于系统的哈密顿量(总能量)，反映了系统当前“危险程度”。

2) 线性化误差的代理：MPC常用的线性化模型仅在工作点附近($\eta \approx 0$ )精确。当$r_k$ 增大时，系统远离线性区，线性预测模型的截断误差显著增加。此时需要通过缩短预测步长来抑制模型失配的影响。

3.3.2. 变参数对稳定性的影响分析

基于计算出的$r_k$ ，系统动态调整采样步长$\text{Δ}{t}_k$ 和预测时域$N_k$ 分别为：

$\Delta t_k=\Delta t_{\min }+{\left(1-r_k\right)}^p\left(\Delta t_{\max }-\Delta t_{\min }\right)$ (5)

$N_k=clip\left(round\left(T_{p,k}/\Delta t_k\right),N_{\min },N_{\max }\right)$ (6)

这种时变参数机制对稳定性的贡献体现在两个方面：

高风险区的高频响应：当机器人处于大角度倾斜($r_k\to 1$ )时，$\text{Δ}{t}_k$ 趋向于$\Delta t_{\min }$ 。更小的采样时间意味着控制器的带宽增加，能够更及时地响应高频的非线性动态，有效抑制发散。

低风险区的计算减负：当系统趋于平稳($r_k\to 0$ )时，$\text{Δ}{t}_k$ 增大，延长了单步预测的物理时长，有助于MPC规划更长远的轨迹，提升稳态性能并降低计算负载。尽管离散时间$\text{Δ}{t}_k$ 是时变的，但只要保证$\text{Δ}{t}_k$ 在有界范围内$\left[\Delta t_{\min },\Delta t_{\max }\right]$ 且优化问题在每一步均可行，系统的闭环稳定性即可通过终端约束集或足够长的预测域得到保障。

值得注意的是，由于采样步长$\text{Δ}{t}_k$ 是时变的，离散化后的系统状态矩阵$A_d\left(\text{Δ}{t}_k\right)$ 与输入矩阵$B_d\left(\text{Δ}{t}_k\right)$ 不再是常数矩阵。因此，在MPC的每一步滚动优化开始前，必须根据当前计算出的$\text{Δ}{t}_k$ 重新计算A_d与B_d，以消除由采样时间变化引起的模型离散化误差，确保预测模型与实际物理过程的动态匹配。

3.4. 基于相平面能量分析的自适应权重调度

为了解决传统MPC在固定权重下难以兼顾快速动态响应与稳态平滑性的矛盾，本文设计了一种基于模糊逻辑的权重自适应机制。该机制依据系统在相平面(Phase Plane)上的能量演化趋势，在线动态调整MPC代价函数中的状态权重矩阵Q与输入权重矩阵R。

3.4.1. 模糊规则设计的稳定性依据

首先，我们定义系统的误差状态向量$x_e={\left[e,\dot{e}\right]}^{\text{T}}$。为了分析系统的稳定性趋势，构建关于误差能量的评价函数$V=\frac{1}{2}{e}^2$ 。其时间导数为：

$\dot{V}=e\cdot \dot{e}$ (7)

为了保证系统的渐进稳定性，需满足其导数负定，即$\dot{V}=e\cdot \dot{e}<0$ 。基于相平面(Phase Plane)分析，我们将系统状态($e,\dot{e}$ )划分为两类区域，以此指导模糊规则及参数的设计(如图3和图4所示)。

Figure 3. Graph of input variable membership function

图3. 输入变量隶属度函数图

Figure 4. Graph of output variable membership function

图4. 输出变量隶属度函数图

发散区(相平面第I、III象限)：

当$e$ 与$\dot{e}$ 同号时(即$e\cdot \dot{e}>0$ )，意味着误差绝对值正在增大，系统正在远离平衡点。例如，机器人前倾($\eta >0$ )且角速度向前($\dot{\eta }>0$ )。此时系统处于“能量积聚”的不稳定状态。

控制策略：必须施加“强校正”。在MPC优化层面，应显著增大俯仰角误差权重Q，迫使优化器将消除误差作为首要目标；同时减小输入惩罚权重R，允许电机输出更大的峰值力矩以遏制发散趋势。

收敛区(相平面第II、IV象限)：

当$e$ 与$\dot{e}$ 异号时(即$e\cdot \dot{e}<0$ )，意味着误差绝对值正在减小，系统正在向平衡点回归。此时系统自然满足$\dot{V}<0$ ，处于“能量耗散”过程。

控制策略：此时应施加“弱阻尼”。为了防止因过度控制导致越过平衡点产生超调(Overshoot)，应适当减小Q并增大R。增加对控制增量的惩罚有助于平滑输出，抑制高频抖动，提升稳态性能。

3.4.2. 模糊控制器设计与参数整定

基于前述相平面能量分析及双轮足机器人的控制特性，本文设计了包含“姿态镇定”与“位移抑制”两个子系统的双通道Mamdani型模糊控制器，分别为权重调节与误差补偿机制提供非线性增益系数。

1) 模糊变量定义与隶属度函数

控制器包含两组独立的输入输出映射：

姿态环：选取俯仰角跟踪误差$e_{\eta }$ 及其变化率${\dot{e}}_{\eta }$ 作为输入变量，选取状态权重缩放系数${\alpha }_{\eta }$ (用于调节Q矩阵)和输入惩罚缩放系数${\alpha }_R$ (用于调节R矩阵)作为输出变量。

位移环：选取位移误差$e_p$ 及其变化率${\dot{e}}_p$ 为输入。输出变量为位移补偿系数${\alpha }_p$ 。

基于模糊控制的特点，论域划分的越多，系统的控制精度就会越高，但实现难度加大[4]。本文将输入变量(误差量)划分为7个模糊子集{NB, NM, NS, ZO, PS, PM, PB}，分别代表负大、负中、负小、零、正小、正中、正大；输出变量(缩放系数)划分为5个等级{VL, L, M, H, VH}，代表极小、小、中、大、极大。输入采用三角型隶属函数以提高计算敏感度，输出采用双高斯型隶属函数以保证权重切换的平滑性。

2) 模糊规则库构建

姿态环规则(权重逆向调节)：旨在解决快速响应与稳态平滑的矛盾。当系统处于“发散区”时，输出较大的${\alpha }_{\eta }$ 并减小${\alpha }_R$ ，强化姿态纠正；当系统处于“收敛区”时，减小${\alpha }_{\eta }$ 并增大${\alpha }_R$ ，以柔化输出。

位移环规则(正向补偿增强)：旨在消除大扰动下的位置漂移。其核心逻辑是“大误差强补偿”。当位移误差$e_p$ 较大或发散趋势明显时，输出较大的补偿系数${\alpha }_p$ (对应等级VH或H)，使预测模型产生更强的主动回正参考信号；当位移接近目标值时，${\alpha }_p$ 迅速衰减为VL，避免在平衡点附近产生震荡。

姿态环模糊控制规则表如图5所示，模糊控制的模糊规则曲面如图6所示。位移环隶属度函数与规则设计逻辑与姿态环类似，篇幅所限不再赘述，仅以姿态环为例展示规则曲面。

Figure 5. Fuzzy control rule table

图5. 姿态环模糊控制规则表

Figure 6. Fuzzy control fuzzy rule surface

图6. 模糊控制的模糊规则曲面

3) 解模糊与权重更新

采用重心法对模糊输出进行解模糊，在每个MPC控制周期k，控制器并行输出三个标量系数${\alpha }_{\eta }$ ，${\alpha }_R$ ，${\alpha }_p$ 。其中${\alpha }_{\eta }$ 和${\alpha }_R$ 直接用于更新MPC权重矩阵：

$Q_k={\alpha }_{\eta }\left(e_k,{\dot{e}}_k\right)\cdot Q_{nom}$ (8)

$R_k={\alpha }_R\left(e_k,{\dot{e}}_k\right)\cdot R_{nom}$ (9)

式中$Q_{nom}$ 和$R_{nom}$ 为标称权重矩阵。通过上述机制，控制器在保持凸优化求解结构不变的前提下，获得了针对非线性扰动的自适应调节能力。

3.4.3. 引入基于模糊规则的误差补偿系数

为了进一步克服线性化预测模型在非线性区域(大角度倾斜)的失配问题，本文在模糊权重调节的基础上，引入了基于模糊规则的参考轨迹动态补偿机制。该机制利用2.4.2节中模糊推理器输出的非线性度量系数，构造前馈补偿项并注入预测模型，使其在感知到高风险时提前产生更强的控制作用。

首先，统一定义各状态量的跟踪误差：

$\begin{array}{l}{e}_p\left(k\right)=x\left(k\right)-x_{ref}\left(k\right)\\ e_{\eta }\left(k\right)=\eta \left(k\right)-{\eta }_{ref}\left(k\right)\end{array}$ (10)

式中$e_p$ 为位移误差，$e_{\eta }$ 为俯仰角误差。

为了实现多维度的精确补偿，我们利用前述模糊控制器输出的系数${\alpha }_{\eta }$ (来自俯仰角模糊规则)以及${\alpha }_p$ (来自位移模糊规则，对应x与$\dot{x}$ 的偏差程度)，构建动态补偿增益。这些系数反映了当前系统偏离平衡点的程度，也间接反映了线性模型预测失效的风险。

传统的MPC基于线性模型生成预测轨迹Y，但在大扰动下该轨迹往往滞后于真实动态。为此，我们引入补偿项H(e)对预测输出进行修正：

${\widehat{y}}_j\left(t+i|t\right)=y_j\left(t+i|t\right)+h_j\left(e_t\right),j=1,2,\cdots ,N_p$ (11)

式中，$y_j\left(t+i|t\right)$ 为原始模型的第j步预测输出，${\widehat{y}}_j\left(t+i|t\right)$ 为补偿后的预测输出。补偿项$h_j\left(e_t\right)$ 构造如下：

$h_j\left(e_t\right)=\text{Γ}{\kappa }^{i-1}\left[\begin{array}{c}{\alpha }_p{e}_p\left(k\right)\\ 0\\ {\alpha }_{\eta }{e}_{\eta }\left(k\right)\\ 0\end{array}\right]$ (12)

式中：$\text{Γ}=\text{diag}\left(k_p^F,k_{\eta }^F\right)$ 为模糊增益幅值矩阵，用于调整补偿的物理量级；$\kappa \in \left[0,1\right]$ 为沿预测时域的指数衰减因子。引入$\kappa$ 的目的是强化前几步(对当前时刻影响最大)的补偿，从而减弱远端预测步的补偿，防止因模型线性化误差累积导致的过补偿。

在状态空间形式下，设离散模型$x_{k+1}=A_d{x}_k+B_d{u}_k$ ，输出$y=Cx$ 。将补偿项按预测时域堆叠，得到修正后的预测方程：

$Y_k^{comp}={\widehat{Y}}_k+H_k\left(e\right)=C{X}_k+H_k\left(e\right)=C\left(A_{qp}{x}_k+B_{qp}{U}_k\right)+H_k\left(e\right)$ (13)

式中：补偿向量$H_k\left(e\right)={\left[h_1^{\text{T}},\cdots ,h_{N_p}^{\text{T}}\right]}^{\text{T}}$ 。

相应地，MPC的二次型代价函数修正为：

$\underset{U_k}{\min }J={\displaystyle \sum }_{i=1}^{N_p}{\Vert {\widehat{y}}_{k+i|k}^{comp}-Y_{k+i}^{ref}\Vert }_Q^2+{\displaystyle \sum }_{i=0}^{N_c-1}{\Vert u_{k+i}\Vert }_R^2+\rho {\Vert \Delta u_{k+i}\Vert }^2$ (14)

为了保持标准二次规划(QP)求解器的形式不变，可将补偿项移至参考轨迹项中，定义等效参考轨迹${\tilde{Y}}_{ref}=Y_{ref}-H_k\left(e\right)$ ，则优化目标重写为：

$\underset{U_k}{\min }{\displaystyle \sum }_{i=1}^{N_p}{\Vert C{x}_{k+i}-{\tilde{Y}}_{k+i}^{ref}\Vert }_Q^2+{\displaystyle \sum }_{i=0}^{N_c-1}\left({\Vert u_{k+i}\Vert }_R^2+\rho {\Vert \Delta u_{k+i}\Vert }^2\right)$ (15)

同理，对于输出硬约束$Y_{\text{min}}\le Y\le Y_{\text{max}}$ ，引入补偿后的约束条件变为：

$Y_{\min }-H_k\left(e\right)\le C{X}_k\le Y_{\max }-H_k\left(e\right)$ (16)

通过这种方式，模糊系统感知到的非线性风险被转化为参考轨迹的动态偏移和约束边界的收缩，从而在不改变底层QP求解结构的前提下，显著增强了线性MPC对非线性扰动的鲁棒性。

4. 实验验证

为验证加入了模糊控制的MPC方法的有效性，笔者在Matlab 2024统一平台下，设定双轮足机器人在静止初态受俯仰角阶跃扰动，初始倾角分别取15˚与30˚。以俯仰角回零过程与质心位移漂移为核心指标分别对PD控制[5]、标准MPC控制算法[6]和本文提出的加入了模糊控制的MPC控制算法进行了仿真实验。如图7与图8所示(表1和表2)。

Table 1. Parameter setting table

表1. 参数设定表

倒立摆等效质量M (kg)	地面摩擦系数	轮子质量m (kg)	质心高度L (m)	轮子半径(m)
9	0.3	0.52	0.4	0.15

Table 2. Comparison table of data in the same scenario

表2. 同场景下数据对比表

控制方法	15度$\theta$ 反向峰值(˚)	30度$\theta$ 反向峰值(˚)	15度最大位移(m)	30度最大位移(m)
文献5	−3.19	−6.38	0.58	1.16
文献6	−2.87	−5.74	0.49	0.98
本文	−2.39	−4.96	0.43	0.9

Figure 7. Comparison of motion control among various control methods under an initial inclination angle of 15 degrees

图7. 15度初始倾角下各控制方法运动控制对比

Figure 8. Comparison of motion control among various control methods under an initial inclination angle of 30 degrees

图8. 30度初始倾角下各控制方法运动控制对比

结果表明：

1) 在15˚初始倾角下，三种方法均能实现稳定回零，但本文方法的T最短，俯仰角回零效果较好，同时X漂移最小，低于其他两种方法。

2) 在30˚初始倾角这一更强扰动条件下，对比方法的T明显增长且出现较大位移偏移；本文方法仍保持最快的俯仰回零，并显著抑制质心位移，未出现约束违规与执行器持续饱和。

5. 结论

本研究以双轮足机器人为实验对象，进行了基于模糊MPC算法的运动控制方法研究。该方法使用IMU风险判据使采样步长自适应收敛、预测域收短，首步控制更果断。同时模糊误差补偿系数在前几步指数衰减注入QP，抵消模型失配带来的姿态偏差，从而实现俯仰角回零更快、位移X更小。本文在MATLAB中对该算法做了对比试验，结果表明本文所提出的基于模糊MPC算法的运动控制方法俯仰角回零更快，位移更小，具有更好的控制效果和对环境的适应性。

NOTES

^*通讯作者。