基于X86六轮差速移动机器人运动控制器设计与实现（二）规划控制算法
引言
在移动机器人领域，路径跟踪是实现自主导航的关键环节。针对 X86 六轮差速移动机器人（SWDMR），本文在前文介绍的运动学模型基础上，进一步探讨 带输入约束的模型预测控制（MPC） 在路径跟踪中的实现细节。读者将学习到 MPC 的基本原理、如何构建预测模型、约束的设定方式以及线性时变 MPC 的求解过程，帮助在实际项目中快速落地高性能的轨迹跟踪控制器。
1. MPC 基础回顾
模型预测控制（Model Predictive Control，简称 MPC）是一类基于系统模型的优化控制方法。其核心思想是：
- 预测：利用系统动力学模型在当前时刻对未来若干个采样步长（预测时域）进行状态预测。
- 优化：在每个采样周期内，依据设定的性能指标（如误差最小、控制增量最小）以及系统约束（输入、状态约束），求解一个有限时域的优化问题，得到一系列控制序列。
- 执行：仅取优化序列的第一个控制量作为当前时刻的实际输入，随后滚动时域继续预测与优化。
MPC 的优势在于 能够显式考虑多种物理约束（如电机转速、加速度限制），并且 通过模型与实时反馈 预估未来位姿，从而缓解传统 PID 控制在高动态、强耦合场景下的迟滞与抖动问题。
2. MPC 路径跟踪控制器框架
基于第 2 章的运动学模型，本文构建了 SWDMR 的 MPC 控制框架。整体流程如下：
- 运动学建模：利用机器人在大地坐标系下的横向位移 $x$、纵向位移 $y$ 与横摆角 $\psi$ 组成状态向量 $\mathbf{x} = [x, y, \psi]^{\top}$，推导出离散化的预测方程。
- 损失函数构建：以 控制增量最小 为主要性能指标，构造目标函数（亦称代价函数），即在每个采样周期内最小化 $\Delta \mathbf{u}$ 的二范数。
- 约束融合：将参考路径信息（期望轨迹）与预测方程的输出代入目标函数，并加入 输入约束（如电机转速上下限）以及 状态约束（如横向误差容限），形成约束优化问题。
- 求解最优解：使用合适的数值求解器（如 QP 求解器）得到当前采样周期的最优控制增量 $\Delta \mathbf{u}^{\ast}$。
- 控制量映射：将 $\Delta \mathbf{u}^{\ast}$ 与上一时刻的控制输入 $\mathbf{u}{k-1}$ 叠加得到当前时刻的控制指令 $\mathbf{u}{k}$，再通过机器人的 映射矩阵 将 $\mathbf{u}_{k}$ 转换为各电机的转速指令。
- 执行与反馈：电机驱动执行转速指令，机器人实际运动后通过里程计、IMU 等传感器获取反馈，进入下一轮预测-优化循环。
图 4.1 为上述框架的结构示意图：
3. 基于线性时变 MPC 的路径跟踪控制
3.1 线性时变模型的优势
相较于完整的 非线性 MPC，线性时变（Linear Time-Varying, LTV）MPC 在计算量、求解速度以及实现复杂度上更具优势。文献 [57] 已指出 LTV-MPC 能在保持足够控制精度的前提下，实现 更快的响应 与 更低的实时计算负担，这对于资源受限的嵌入式平台尤为重要。
3.2 运动学方程
在本节中，依据上节得到的 SWDMR 运动学模型，选取机器人在大地坐标系下的 横向位移 $x$、纵向位移 $y$ 与横摆角 $\psi$ 作为状态量，记为 $\mathbf{x} = [x, y, \psi]^{\top}$。对应的离散时间运动学方程可写成：
该方程在每个采样时刻会根据当前的线速度 $v$ 与转向角速度 $\omega$ 进行线性化，得到 时变的系统矩阵 $A_k$ 与输入矩阵 $B_k$，从而形成 LTV 系统模型。
3.3 优化求解
3.3.1 目标函数的设计
目标函数的构建旨在 确保机器人能够紧跟期望路径 的同时 响应更迅速、调节更平稳。因此，除了传统的轨迹误差项（如 $(\mathbf{x}_k - \mathbf{x}^{\text{ref}}_k)^{\top} Q (\mathbf{x}_k - \mathbf{x}^{\text{ref}}_k)$），还需加入 控制量变化趋势 的惩罚项：
[ J = \sum_{k=0}^{N-1} \Big[ (\mathbf{x}_k - \mathbf{x}^{\text{ref}}_k)^{\top} Q (\mathbf{x}_k - \mathbf{x}^{\text{ref}}_k) + \Delta \mathbf{u}_k^{\top} R \Delta \mathbf{u}_k \Big] ]
其中，$\Delta \mathbf{u}_k = \mathbf{u}k - \mathbf{u}{k-1}$ 表示相邻控制输入的差分，$R$ 为权重矩阵，用于调节 控制平滑度 与 响应速度 的权衡。
3.3.2 输入约束的必要性
在实际机械结构中，差速转向的机身结构对突变的控制量极为敏感。若控制量在相邻采样周期之间变化过大，会导致：
- 路径抖动：机器人在跟踪期望轨迹时出现明显的振荡。
- 结构冲击：电机与驱动机构受到冲击，长期运行可能导致机械磨损或失效。
为此，在优化问题中加入 输入变化率约束（即 $\Delta \mathbf{u}_{\min} \le \Delta \mathbf{u}k \le \Delta \mathbf{u}{\max}$），限制每个采样周期内控制量的最大增量，从而避免 猛然抖动。
3.3.3 求解流程
- 构造预测矩阵：基于线性时变模型 $A_k, B_k$，构建从当前时刻到预测终点的状态转移矩阵 $\Phi$ 与控制影响矩阵 $\Gamma$。
- 形成二次规划（QP）：将目标函数与约束统一写成标准 QP 形式 $\min \frac{1}{2}\mathbf{z}^{\top} H \mathbf{z} + f^{\top}\mathbf{z}$，其中 $\mathbf{z}$ 为待求的控制增量序列。
- 调用求解器：使用实时 QP 求解器（如 OSQP、qpOASES）在每个采样周期内求解最优 $\Delta \mathbf{u}^{\ast}$。
- 滚动时域：仅取 $\Delta \mathbf{u}^{\ast}_0$ 作为本周期的实际控制增量，随后将时域向前滚动，重复步骤 1~3。
通过上述步骤，MPC 控制器能够在 满足输入约束 的前提下，实现 平滑且快速的路径跟踪。
4. 实践要点与调试建议
| 项目 | 关键点 | 常见问题 | 解决思路 |
|---|---|---|---|
| 模型线性化 | 采样周期 $T_s$ 选取合适，确保线性化误差可接受 | 线性化误差导致预测偏差 | 适当减小 $T_s$ 或在关键转弯处使用分段线性模型 |
| 约束设置 | 输入上下限、变化率上下限需基于电机规格与机械结构 | 约束过紧导致求解 infeasible | 先放宽约束，逐步收紧；检查是否存在冲突约束 |
| 权重调节 | $Q$ 与 $R$ 权重决定误差抑制与控制平滑的平衡 | 抖动或响应迟缓 | 增大 $R$ 抑制控制变化；增大 $Q$ 提高轨迹跟踪精度 |
| 求解器选择 | 实时性要求高，推荐使用稀疏 QP 求解器 | 求解时间超过采样周期 | 调整预测步长 $N$，或采用提前离线计算的参考控制序列 |
| 传感器噪声 | 里程计、IMU 误差会影响状态反馈 | 预测误差累积 | 引入卡尔曼滤波或 EKF 对状态进行融合估计 |
5. 小结
本文围绕 带输入约束的 MPC 路径跟踪控制，从框架搭建、线性时变模型推导到约束优化求解，系统阐述了在 X86 六轮差速移动机器人 上实现高效轨迹跟踪的关键技术点。通过合理的 模型预测 与 约束设计，能够显著提升机器人在复杂路径上的跟踪精度，同时抑制因控制量突变带来的机械冲击与抖动。
后续工作可进一步探索 非线性 MPC 与 学习型预测模型 的结合，以期在更高动态范围内实现更优的控制性能。欢迎读者在实践中提出问题或分享改进经验，共同推动工业移动机器人技术的进步。