MPC模型预测控制系列之C++实现

MPC模型预测控制系列， C++实现 前请仔细阅读如下说明: 带约束的MPC 终端等式约束MPC 终端不等式约束MPC 带有状态观测器的无约束输出反馈MPC 带有最优状态观测器的无约束输出反馈MPC 带有状态观测器的有约束输出反馈MPC 改进版带有状态观测器的有约束输出反馈MPC 有界干扰鲁棒MPC 模型不确定鲁棒MPC 有界干扰+模型不确定鲁棒MPC 上述例程仅有cpp版对应联系即可 Linux环境vscode +cmake编译， 自编MPC增益矩阵求解.cpp文件 使用OSQP Eigen库求解二次规划。 注意: 1. 需自行配置eigen和OSQP 2. 默认为单个例程，非所有例程打包 3. 该程序为学习例程旨在学习mpc系列算法思想以及OSQP的实现方式，数值算例为单入多出的二阶系统(注意:不是车辆模型) 不在特殊应用场景下做改动 前请认真阅读商品简介后再做咨询 4.与ROS无关、与Autoware无关

一、引言

MPC（Model Predictive Control，模型预测控制）在控制领域应用广泛，它基于系统模型进行预测，并通过优化求解得到控制输入。本文将介绍如何在Linux环境下，使用C++实现多种MPC控制策略，包括带约束和无约束的情况，以及考虑状态观测器和鲁棒性的版本。

二、实现环境

我们将在Linux环境中，借助VSCode编辑器与CMake构建系统来编译代码。同时，需要自行配置Eigen和OSQP库，用于矩阵运算与二次规划求解。

（一）Eigen库

Eigen是一个C++的线性代数库，提供了高效的矩阵和向量运算。例如，我们定义一个简单的矩阵：

#include <Eigen/Dense> int main() { Eigen::MatrixXd matrix(2, 2); matrix << 1, 2, 3, 4; std::cout << matrix << std::endl; return 0; }

这里我们使用Eigen::MatrixXd定义了一个2x2的矩阵，并使用<<操作符填充矩阵元素。Eigen库极大地简化了矩阵运算的代码编写。

（二）OSQP库

OSQP（Operator Splitting QP solver）用于求解二次规划问题，在MPC中用于计算最优控制输入。在使用前需正确配置该库，配置好后，代码中引入头文件就可以使用其功能。

三、MPC控制策略实现

（一）带约束的MPC

带约束的MPC考虑系统的输入输出限制，使控制输入在合理范围内。终端等式约束MPC和终端不等式约束MPC是常见的带约束形式。

MPC模型预测控制系列， C++实现 前请仔细阅读如下说明: 带约束的MPC 终端等式约束MPC 终端不等式约束MPC 带有状态观测器的无约束输出反馈MPC 带有最优状态观测器的无约束输出反馈MPC 带有状态观测器的有约束输出反馈MPC 改进版带有状态观测器的有约束输出反馈MPC 有界干扰鲁棒MPC 模型不确定鲁棒MPC 有界干扰+模型不确定鲁棒MPC 上述例程仅有cpp版对应联系即可 Linux环境vscode +cmake编译， 自编MPC增益矩阵求解.cpp文件 使用OSQP Eigen库求解二次规划。 注意: 1. 需自行配置eigen和OSQP 2. 默认为单个例程，非所有例程打包 3. 该程序为学习例程旨在学习mpc系列算法思想以及OSQP的实现方式，数值算例为单入多出的二阶系统(注意:不是车辆模型) 不在特殊应用场景下做改动 前请认真阅读商品简介后再做咨询 4.与ROS无关、与Autoware无关

以终端等式约束MPC为例，假设我们有系统状态空间模型$x{k + 1} = Axk + Bu_k$，终端等式约束通常要求在预测时域的末端状态达到特定值。在代码中，我们会在二次规划的约束条件中体现这一点。

// 假设已经定义好A, B矩阵，x0为初始状态，N为预测时域 Eigen::MatrixXd A, B; Eigen::VectorXd x0; int N; // 构建二次规划问题 // 这里简化代码示例，实际需要更完整的构建 osqp::OSQPData data; data.A = // 根据A, B和约束构建的矩阵 data.l = // 约束下限 data.u = // 约束上限 data.P = // 二次项系数矩阵 data.q = // 一次项系数向量 osqp::OSQPWorkspace work; work.setup(data); work.solve(); // 获取最优控制输入 Eigen::VectorXd u_opt = work.getSolution();

上述代码通过OSQP库求解二次规划问题，得到最优控制输入u_opt。其中data.A,data.l,data.u等的构建需要根据具体的约束条件和系统模型来确定。

（二）带有状态观测器的MPC

1. 无约束输出反馈MPC

在实际系统中，并非所有状态都可直接测量，此时需要状态观测器。无约束输出反馈MPC通过状态观测器估计状态，并基于估计状态进行控制。

假设我们有一个简单的状态观测器方程：$\hat{x}{k + 1} = A\hat{x}k + Buk + L(yk - C\hat{x}_k)$，其中$\hat{x}$是估计状态，$L$是观测器增益矩阵。

// 假设已经定义好A, B, C矩阵，L为观测器增益矩阵，x_hat为估计状态，y为测量输出 Eigen::MatrixXd A, B, C, L; Eigen::VectorXd x_hat, y; x_hat = A * x_hat + B * u + L * (y - C * x_hat); // 基于估计状态x_hat进行MPC控制计算

这段代码展示了状态观测器的更新过程，之后就可以基于估计状态x_hat进行MPC控制计算。

1. 有约束输出反馈MPC

结合状态观测器和约束条件，有约束输出反馈MPC在实际应用中更为常见。改进版带有状态观测器的有约束输出反馈MPC则可能在观测器设计或约束处理上有进一步优化。

（三）鲁棒MPC

1. 有界干扰鲁棒MPC

考虑系统存在有界干扰时，有界干扰鲁棒MPC通过调整控制策略，使系统在干扰存在的情况下仍能保持稳定。例如，在系统模型$x{k + 1} = Axk + Buk + wk$中，$w_k$为有界干扰。在二次规划求解时，需要将干扰的影响考虑进约束条件。

1. 模型不确定鲁棒MPC

当系统模型存在不确定性时，模型不确定鲁棒MPC能够处理这种情况。可能的实现方式是通过对模型不确定性进行建模，并在优化过程中考虑这种不确定性。

1. 有界干扰 + 模型不确定鲁棒MPC

结合有界干扰和模型不确定两种情况，这种MPC策略更加复杂，但能应对更实际的系统情况。

四、自编MPC增益矩阵求解

我们通过自编的MPC增益矩阵求解.cpp文件来计算MPC中的一些关键增益矩阵，比如反馈增益矩阵等。这些矩阵的计算基于系统模型和MPC的优化目标，其计算过程通常涉及到矩阵运算和求解黎卡提方程等操作。

// 假设这里有计算反馈增益矩阵K的函数 Eigen::MatrixXd calculateK(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B, const Eigen::MatrixXd& Q, const Eigen::MatrixXd& R) { // 这里简化计算过程，实际需要完整的黎卡提方程求解等 Eigen::MatrixXd K; // 假设通过一些矩阵运算得到K return K; }

上述代码展示了一个简单的计算反馈增益矩阵K的函数，实际中会根据具体的MPC算法进行更复杂的计算。

五、总结

本文介绍了在Linux环境下使用C++实现多种MPC控制策略的方法，包括带约束、带状态观测器以及鲁棒MPC等。通过使用Eigen和OSQP库，结合自编的MPC增益矩阵求解文件，能够有效地实现这些MPC算法。这些实现主要用于学习MPC系列算法思想以及OSQP的实现方式，基于单入多出的二阶系统数值算例，为MPC的学习和研究提供了一个良好的起点。