用C++和Eigen库从零实现一个简易MPC控制器（附完整代码和调参心得）-开发者社区

从零构建MPC控制器：C++与Eigen库实战指南

1. 理解MPC的核心思想

模型预测控制（MPC）本质上是一种基于模型的闭环优化控制策略。想象你正在驾驶一辆汽车，不仅要考虑当前的方向盘角度和油门位置，还要预测未来几秒内车辆的轨迹，并据此调整控制输入。这就是MPC的核心思想——在每个控制周期，基于当前状态和系统模型，预测未来一段时间内的系统行为，并通过优化计算得到最优控制序列。

MPC与传统控制方法相比有三个显著特点：

预测能力：利用系统模型预测未来状态
滚动优化：在每个采样时刻重新求解优化问题
反馈校正：根据实际测量值修正预测误差

在C++中实现MPC需要处理几个关键组件：

系统建模：用状态空间方程描述系统动态
预测方程：构建未来状态与当前控制量之间的关系
代价函数：定义控制目标（如跟踪误差、控制量大小等）
优化求解：计算最优控制序列

2. 搭建开发环境与Eigen库基础

2.1 环境配置

首先确保你的开发环境已准备好：

# 安装必要的开发工具 sudo apt-get install build-essential cmake

Eigen是一个纯头文件的C++模板库，安装非常简单：

# 安装Eigen库 sudo apt-get install libeigen3-dev

2.2 Eigen库核心操作

Eigen库是MPC实现中处理矩阵运算的利器。以下是几个关键操作：

#include <Eigen/Dense> // 创建矩阵 Eigen::MatrixXd A(2, 2); // 2x2动态大小矩阵 A << 1, 0.1, 0, 2; // 初始化矩阵元素 // 矩阵运算 Eigen::MatrixXd B = A.transpose(); // 转置 Eigen::MatrixXd C = A.inverse(); // 逆矩阵 Eigen::MatrixXd D = A * B; // 矩阵乘法 // 分块操作 Eigen::MatrixXd block = A.block(0, 0, 1, 2); // 提取左上1x2子矩阵

3. 系统建模与预测方程构建

3.1 离散系统表示

我们考虑一个简单的线性离散系统：

xₖ₊₁ = A xₖ + B uₖ

其中：

xₖ ∈ ℝⁿ 是k时刻的状态向量
uₖ ∈ ℝᵐ 是k时刻的控制输入
A ∈ ℝⁿˣⁿ 是状态转移矩阵
B ∈ ℝⁿˣᵐ 是控制输入矩阵

3.2 预测方程推导

预测未来N步的状态序列可以表示为：

X_k = M x_k + C U_k

其中：

X_k = [xₖ₊₁, xₖ₊₂, ..., xₖ₊N]ᵀ
U_k = [uₖ, uₖ₊₁, ..., uₖ₊N₋₁]ᵀ
M 和 C 是由A、B构成的块矩阵

在代码中构建这些矩阵：

Eigen::MatrixXd buildM(const Eigen::MatrixXd& A, int N) { int n = A.rows(); Eigen::MatrixXd M((N+1)*n, n); M.setZero(); Eigen::MatrixXd temp = Eigen::MatrixXd::Identity(n, n); M.block(0, 0, n, n) = temp; for(int i=1; i<=N; ++i) { temp = A * temp; M.block(i*n, 0, n, n) = temp; } return M; }

4. 代价函数设计与优化求解

4.1 代价函数组成

MPC的代价函数通常包含三部分：

状态误差代价：使系统状态跟踪参考轨迹
控制输入代价：限制控制量大小
终端代价：保证稳定性

数学表达式为：

J = xₖ₊Nᵀ F xₖ₊N + Σ [xₖ₊iᵀ Q xₖ₊i + uₖ₊iᵀ R uₖ₊i]

4.2 二次规划求解

将代价函数转化为标准二次型：

J = xₖᵀ G xₖ + Uₖᵀ H Uₖ + 2 Uₖᵀ E xₖ

最优解可直接通过求导得到：

U* = H⁻¹ (-E xₖ)

代码实现：

Eigen::VectorXd solveMPC(const Eigen::VectorXd& x_k, const Eigen::MatrixXd& A, const Eigen::MatrixXd& B, const Eigen::MatrixXd& Q, const Eigen::MatrixXd& R, const Eigen::MatrixXd& F, int N) { int n = A.rows(); int m = B.cols(); // 构建M和C矩阵 Eigen::MatrixXd M = buildM(A, N); Eigen::MatrixXd C = buildC(A, B, N); // 构建Q_bar和R_bar Eigen::MatrixXd Q_bar = buildQBar(Q, F, N); Eigen::MatrixXd R_bar = buildRBar(R, N); // 计算G, E, H Eigen::MatrixXd G = M.transpose() * Q_bar * M; Eigen::MatrixXd E = C.transpose() * Q_bar * M; Eigen::MatrixXd H = C.transpose() * Q_bar * C + R_bar; // 求解最优控制序列 return H.inverse() * (-E * x_k); }

5. 参数调优与性能分析

5.1 权重矩阵影响

不同权重矩阵对系统性能的影响：

参数	增大效果	减小效果
Q	状态跟踪更精确	允许更大跟踪误差
R	控制量更平滑	允许更大控制输入
F	终端状态更精确	不强调终端状态

5.2 预测时域选择

预测时域N的选择需要考虑：

计算复杂度：O(N³)的增长速度
控制性能：N太小可能导致短视，N太大增加计算负担
系统动态：快速系统需要较小N，慢速系统需要较大N

经验法则：N应覆盖系统主要动态响应时间

6. 完整实现与测试案例

6.1 系统定义

考虑一个二阶系统：

// 系统矩阵 Eigen::Matrix2d A; A << 1, 0.1, 0, 2; Eigen::Vector2d B(0, 0.5); // 权重矩阵 Eigen::Matrix2d Q = Eigen::Matrix2d::Identity(); Eigen::Matrix2d F = 2 * Eigen::Matrix2d::Identity(); Eigen::MatrixXd R(1,1); R << 0.1; // 初始状态 Eigen::Vector2d x_init(5, 5);

6.2 控制循环实现

int N = 3; // 预测时域 int steps = 50; // 仿真步数 Eigen::Vector2d x = x_init; for(int k=0; k<steps; ++k) { // 求解MPC Eigen::VectorXd U = solveMPC(x, A, B, Q, R, F, N); // 取第一个控制量 double u = U(0); // 应用控制量并更新状态 x = A * x + B * u; // 记录状态和控制量 // ... }

7. 高级话题与扩展方向

7.1 约束处理

实际系统中常需要处理约束：

控制量约束：u_min ≤ u ≤ u_max
状态约束：x_min ≤ x ≤ x_max
速率约束：Δu_min ≤ Δu ≤ Δu_max

这些约束可以转化为二次规划问题的线性约束条件。

7.2 非线性系统MPC

对于非线性系统，主要有两种方法：

线性化方法：在工作点附近线性化
直接非线性优化：使用更复杂的优化算法

7.3 实时性优化

提高MPC实时性能的技术：

热启动：重用上一周期的解作为初始猜测
代码生成：离线生成优化求解代码
并行计算：利用多核处理器加速

8. 常见问题与调试技巧

8.1 数值不稳定问题

条件数检查：H矩阵的条件数过大可能导致求解不稳定
正则化：添加小量单位矩阵改善数值稳定性
精度问题：使用更高精度浮点数（如double而非float）

8.2 性能不佳排查

检查系统模型：确认A,B矩阵是否正确
验证预测方程：人工计算几步预测结果
调整权重矩阵：从简单配置开始（如单位矩阵）
检查求解结果：确认优化求解得到合理控制量

8.3 Eigen使用技巧

内存预分配：对于固定维数问题，使用Eigen::Matrix3d等固定大小矩阵
避免临时对象：使用.noalias()避免不必要的临时矩阵
利用SIMD：确保编译器启用向量化优化（如-march=native）

9. 实际应用案例

9.1 倒立摆控制

倒立摆是经典的MPC应用案例。系统状态包括：

小车位置x
小车速度ẋ
摆杆角度θ
摆杆角速度θ̇

控制目标是通过左右移动小车保持摆杆直立。

9.2 无人机轨迹跟踪

无人机MPC控制器需要考虑：

位置和姿态动力学
环境干扰（如风）
避障约束
能量效率优化

9.3 工业过程控制

在化工过程中，MPC用于：

温度控制
流量调节
压力维持
多变量协调控制

10. 性能优化与进阶技巧

10.1 稀疏矩阵利用

对于大规模系统，预测方程中的矩阵往往是稀疏的。Eigen提供了稀疏矩阵支持：

#include <Eigen/Sparse> Eigen::SparseMatrix<double> sparseMat(100,100); sparseMat.insert(0,0) = 1; // 填充非零元素

10.2 实时性能分析

使用Chrome tracing工具分析MPC计算时间分布：

#include <chrono> auto start = std::chrono::high_resolution_clock::now(); // MPC计算代码 auto end = std::chrono::high_resolution_clock::now(); std::chrono::duration<double> elapsed = end - start;

10.3 自动微分应用

对于非线性MPC，可以使用自动微分库（如CppAD）计算雅可比矩阵：

#include <cppad/cppad.hpp> CppAD::AD<double> x = 1.0; // 定义自动微分变量 CppAD::AD<double> y = sin(x); // 计算函数值

11. 工程实践建议

模块化设计：将MPC分解为独立模块（预测、优化、应用等）
单元测试：为每个组件编写测试用例
日志记录：保存每次优化的输入输出用于分析
安全机制：检测异常情况并采取保守策略
可视化调试：绘制状态轨迹和控制量曲线

12. 资源与延伸阅读

12.1 推荐书籍

《Model Predictive Control》by Eduardo F. Camacho
《Predictive Control for Linear and Hybrid Systems》by Borrelli et al.
《Applied Nonlinear Control》by Slotine and Li

12.2 开源项目参考

ACADO Toolkit：MPC代码生成框架
CasADi：非线性优化与自动微分
OSQP：二次规划求解器

12.3 在线资源

MATLAB MPC工具箱文档
Eigen官方文档
IEEE Control Systems Society资源