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

从零构建MPC控制器C与Eigen库实战指南1. 理解MPC的核心思想模型预测控制MPC本质上是一种基于模型的闭环优化控制策略。想象你正在驾驶一辆汽车不仅要考虑当前的方向盘角度和油门位置还要预测未来几秒内车辆的轨迹并据此调整控制输入。这就是MPC的核心思想——在每个控制周期基于当前状态和系统模型预测未来一段时间内的系统行为并通过优化计算得到最优控制序列。MPC与传统控制方法相比有三个显著特点预测能力利用系统模型预测未来状态滚动优化在每个采样时刻重新求解优化问题反馈校正根据实际测量值修正预测误差在C中实现MPC需要处理几个关键组件系统建模用状态空间方程描述系统动态预测方程构建未来状态与当前控制量之间的关系代价函数定义控制目标如跟踪误差、控制量大小等优化求解计算最优控制序列2. 搭建开发环境与Eigen库基础2.1 环境配置首先确保你的开发环境已准备好# 安装必要的开发工具 sudo apt-get install build-essential cmakeEigen是一个纯头文件的C模板库安装非常简单# 安装Eigen库 sudo apt-get install libeigen3-dev2.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((N1)*n, n); M.setZero(); Eigen::MatrixXd temp Eigen::MatrixXd::Identity(n, n); M.block(0, 0, n, n) temp; for(int i1; iN; 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 k0; ksteps; 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而非float8.2 性能不佳排查检查系统模型确认A,B矩阵是否正确验证预测方程人工计算几步预测结果调整权重矩阵从简单配置开始如单位矩阵检查求解结果确认优化求解得到合理控制量8.3 Eigen使用技巧内存预分配对于固定维数问题使用Eigen::Matrix3d等固定大小矩阵避免临时对象使用.noalias()避免不必要的临时矩阵利用SIMD确保编译器启用向量化优化如-marchnative9. 实际应用案例9.1 倒立摆控制倒立摆是经典的MPC应用案例。系统状态包括小车位置x小车速度ẋ摆杆角度θ摆杆角速度θ̇控制目标是通过左右移动小车保持摆杆直立。9.2 无人机轨迹跟踪无人机MPC控制器需要考虑位置和姿态动力学环境干扰如风避障约束能量效率优化9.3 工业过程控制在化工过程中MPC用于温度控制流量调节压力维持多变量协调控制10. 性能优化与进阶技巧10.1 稀疏矩阵利用对于大规模系统预测方程中的矩阵往往是稀疏的。Eigen提供了稀疏矩阵支持#include Eigen/Sparse Eigen::SparseMatrixdouble 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::durationdouble elapsed end - start;10.3 自动微分应用对于非线性MPC可以使用自动微分库如CppAD计算雅可比矩阵#include cppad/cppad.hpp CppAD::ADdouble x 1.0; // 定义自动微分变量 CppAD::ADdouble 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 Li12.2 开源项目参考ACADO ToolkitMPC代码生成框架CasADi非线性优化与自动微分OSQP二次规划求解器12.3 在线资源MATLAB MPC工具箱文档Eigen官方文档IEEE Control Systems Society资源