1. MPC模型预测控制概述
模型预测控制(Model Predictive Control,MPC)是一种先进的控制策略,广泛应用于工业过程控制、机器人、自动驾驶等领域。与传统的PID控制不同,MPC通过建立被控对象的数学模型,在每个控制周期内求解一个有限时域的最优控制问题,并将第一个控制量作用于系统。
MPC的核心优势在于:
- 能够显式处理多输入多输出(MIMO)系统
- 可以直接考虑系统的约束条件(如执行器饱和、状态限制等)
- 通过滚动时域优化实现良好的控制性能
在C++实现中,我们需要重点关注以下几个关键环节:
- 系统建模与离散化
- 预测方程构建
- 优化问题表述
- 约束条件处理
- 实时求解算法
2. 开发环境配置
2.1 基础工具链搭建
对于MPC的C++实现,推荐使用以下开发环境:
- 操作系统:Ubuntu 20.04 LTS或更新版本
- 编译器:GCC 9.4.0及以上
- 构建工具:CMake 3.16+
- IDE:VSCode + C/C++插件
安装基础依赖:
bash复制sudo apt update
sudo apt install build-essential cmake git
2.2 关键库安装
2.2.1 Eigen库安装
Eigen是一个高性能的C++模板库,用于线性代数运算:
bash复制sudo apt install libeigen3-dev
验证安装:
cpp复制#include <iostream>
#include <Eigen/Dense>
int main() {
Eigen::Matrix3d m = Eigen::Matrix3d::Random();
std::cout << "Random 3x3 matrix:\n" << m << std::endl;
return 0;
}
2.2.2 OSQP库安装
OSQP是一个高效的二次规划求解器:
bash复制git clone --recursive https://github.com/osqp/osqp
cd osqp
mkdir build && cd build
cmake -G "Unix Makefiles" ..
make
sudo make install
验证安装:
cpp复制#include <osqp/osqp.h>
int main() {
OSQPSolver *solver;
OSQPSettings *settings = (OSQPSettings *)c_malloc(sizeof(OSQPSettings));
osqp_set_default_settings(settings);
return 0;
}
3. MPC核心算法实现
3.1 系统建模与预测方程
考虑离散线性时不变系统:
x_{k+1} = Ax_k + Bu_k
y_k = Cx_k
其中:
- x ∈ R^n:状态向量
- u ∈ R^m:控制输入
- y ∈ R^p:系统输出
预测方程构建:
cpp复制Eigen::MatrixXd buildPredictionMatrix(const Eigen::MatrixXd& A,
const Eigen::MatrixXd& B,
int N) {
int n = A.rows();
int m = B.cols();
Eigen::MatrixXd Psi = Eigen::MatrixXd::Zero(n*(N+1), n);
Eigen::MatrixXd Theta = Eigen::MatrixXd::Zero(n*(N+1), m*N);
// Build Psi matrix
Psi.block(0, 0, n, n) = Eigen::MatrixXd::Identity(n, n);
for(int i=1; i<=N; i++) {
Psi.block(i*n, 0, n, n) = A * Psi.block((i-1)*n, 0, n, n);
}
// Build Theta matrix
for(int j=0; j<N; j++) {
for(int i=j+1; i<=N; i++) {
Theta.block(i*n, j*m, n, m) =
Psi.block((i-j-1)*n, 0, n, n) * B;
}
}
return Theta;
}
3.2 带约束MPC实现
3.2.1 终端等式约束MPC
终端等式约束要求预测时域末端状态达到特定值x_N = x_ref:
cpp复制struct MPCProblem {
Eigen::MatrixXd H; // 二次项矩阵
Eigen::VectorXd f; // 一次项向量
Eigen::MatrixXd A; // 约束矩阵
Eigen::VectorXd lb; // 约束下界
Eigen::VectorXd ub; // 约束上界
};
MPCProblem buildMPCProblem(const Eigen::MatrixXd& A,
const Eigen::MatrixXd& B,
const Eigen::MatrixXd& Q,
const Eigen::MatrixXd& R,
const Eigen::VectorXd& x0,
const Eigen::VectorXd& x_ref,
int N) {
MPCProblem problem;
int n = A.rows();
int m = B.cols();
// 构建预测矩阵
Eigen::MatrixXd Theta = buildPredictionMatrix(A, B, N);
// 构建H矩阵
problem.H = Eigen::MatrixXd::Zero(m*N, m*N);
for(int i=0; i<N; i++) {
problem.H.block(i*m, i*m, m, m) = R;
if(i < N-1) {
problem.H.block(i*m, i*m, m, m) +=
B.transpose() * Q * B;
}
}
// 构建f向量
problem.f = Eigen::VectorXd::Zero(m*N);
Eigen::VectorXd temp = A * x0 - x_ref;
for(int i=0; i<N; i++) {
problem.f.segment(i*m, m) = B.transpose() * Q * temp;
temp = A * temp;
}
// 构建等式约束 (终端约束)
problem.A = Eigen::MatrixXd::Zero(n, m*N);
Eigen::MatrixXd AN = Eigen::MatrixXd::Identity(n, n);
for(int i=0; i<N; i++) {
AN = A * AN;
}
problem.A.block(0, 0, n, m*N) = Theta.block(N*n, 0, n, m*N);
problem.lb = x_ref - AN * x0;
problem.ub = problem.lb;
return problem;
}
3.2.2 终端不等式约束MPC
更常见的是终端不等式约束x_N ∈ X_f:
cpp复制void addTerminalInequalityConstraints(MPCProblem& problem,
const Eigen::MatrixXd& X_f_A,
const Eigen::VectorXd& X_f_b) {
int n_con = X_f_A.rows();
int m = problem.H.cols();
// 扩展约束矩阵
Eigen::MatrixXd new_A = Eigen::MatrixXd::Zero(
problem.A.rows() + n_con, m);
new_A.block(0, 0, problem.A.rows(), m) = problem.A;
new_A.block(problem.A.rows(), 0, n_con, m) = X_f_A;
// 扩展约束边界
Eigen::VectorXd new_lb = Eigen::VectorXd::Zero(
problem.lb.size() + n_con);
new_lb.head(problem.lb.size()) = problem.lb;
new_lb.tail(n_con) = Eigen::VectorXd::Constant(n_con, -OSQP_INFTY);
Eigen::VectorXd new_ub = Eigen::VectorXd::Zero(
problem.ub.size() + n_con);
new_ub.head(problem.ub.size()) = problem.ub;
new_ub.tail(n_con) = X_f_b;
problem.A = new_A;
problem.lb = new_lb;
problem.ub = new_ub;
}
3.3 状态观测器设计
3.3.1 全维状态观测器
当系统状态不可直接测量时,需要设计状态观测器:
cpp复制class StateObserver {
public:
StateObserver(const Eigen::MatrixXd& A,
const Eigen::MatrixXd& B,
const Eigen::MatrixXd& C,
const Eigen::MatrixXd& L)
: A_(A), B_(B), C_(C), L_(L),
x_hat_(Eigen::VectorXd::Zero(A.rows())) {}
void update(const Eigen::VectorXd& u, const Eigen::VectorXd& y) {
x_hat_ = A_ * x_hat_ + B_ * u + L_ * (y - C_ * x_hat_);
}
Eigen::VectorXd getState() const { return x_hat_; }
private:
Eigen::MatrixXd A_, B_, C_, L_;
Eigen::VectorXd x_hat_;
};
3.3.2 卡尔曼滤波器实现
对于噪声系统,可以使用卡尔曼滤波器:
cpp复制class KalmanFilter {
public:
KalmanFilter(const Eigen::MatrixXd& A,
const Eigen::MatrixXd& B,
const Eigen::MatrixXd& C,
const Eigen::MatrixXd& Q,
const Eigen::MatrixXd& R)
: A_(A), B_(B), C_(C),
Q_(Q), R_(R),
x_hat_(Eigen::VectorXd::Zero(A.rows())),
P_(Eigen::MatrixXd::Identity(A.rows(), A.rows())) {}
void predict(const Eigen::VectorXd& u) {
x_hat_ = A_ * x_hat_ + B_ * u;
P_ = A_ * P_ * A_.transpose() + Q_;
}
void update(const Eigen::VectorXd& y) {
Eigen::MatrixXd K = P_ * C_.transpose() *
(C_ * P_ * C_.transpose() + R_).inverse();
x_hat_ = x_hat_ + K * (y - C_ * x_hat_);
P_ = (Eigen::MatrixXd::Identity(P_.rows(), P_.cols()) - K * C_) * P_;
}
Eigen::VectorXd getState() const { return x_hat_; }
private:
Eigen::MatrixXd A_, B_, C_, Q_, R_;
Eigen::VectorXd x_hat_;
Eigen::MatrixXd P_;
};
3.4 鲁棒MPC实现
3.4.1 有界干扰鲁棒MPC
考虑系统模型:
x_{k+1} = Ax_k + Bu_k + w_k
其中w_k ∈ W是有界干扰。
cpp复制MPCProblem buildRobustMPCProblem(const Eigen::MatrixXd& A,
const Eigen::MatrixXd& B,
const Eigen::MatrixXd& Q,
const Eigen::MatrixXd& R,
const Eigen::VectorXd& x0,
const Eigen::VectorXd& x_ref,
const Eigen::MatrixXd& W_A,
const Eigen::VectorXd& W_b,
int N) {
MPCProblem nominal_problem = buildMPCProblem(A, B, Q, R, x0, x_ref, N);
// 计算干扰的累积效应
Eigen::MatrixXd M = Eigen::MatrixXd::Zero(W_A.rows()*(N+1), W_A.cols());
for(int i=0; i<=N; i++) {
M.block(i*W_A.rows(), 0, W_A.rows(), W_A.cols()) = W_A;
}
// 调整约束条件
nominal_problem.lb = nominal_problem.lb - M * W_b;
nominal_problem.ub = nominal_problem.ub + M * W_b;
return nominal_problem;
}
3.4.2 模型不确定鲁棒MPC
考虑参数不确定系统:
x_{k+1} = (A+ΔA)x_k + (B+ΔB)u_k
其中(ΔA,ΔB) ∈ Ω。
cpp复制MPCProblem buildUncertainMPCProblem(const std::vector<Eigen::MatrixXd>& A_set,
const std::vector<Eigen::MatrixXd>& B_set,
const Eigen::MatrixXd& Q,
const Eigen::MatrixXd& R,
const Eigen::VectorXd& x0,
const Eigen::VectorXd& x_ref,
int N) {
// 构建最坏情况下的约束
int n = A_set[0].rows();
int m = B_set[0].cols();
MPCProblem problem;
problem.H = Eigen::MatrixXd::Zero(m*N, m*N);
problem.f = Eigen::VectorXd::Zero(m*N);
// 需要实现多面体约束的交集
// 这里简化处理,实际需要更复杂的实现
// ...
return problem;
}
4. OSQP求解器接口封装
4.1 基本接口封装
cpp复制class OSQPInterface {
public:
OSQPInterface() : workspace_(nullptr) {
osqp_set_default_settings(&settings_);
settings_.verbose = false;
}
~OSQPInterface() {
if(workspace_) {
osqp_cleanup(workspace_);
}
}
bool solveQP(const Eigen::MatrixXd& P,
const Eigen::VectorXd& q,
const Eigen::MatrixXd& A,
const Eigen::VectorXd& l,
const Eigen::VectorXd& u,
Eigen::VectorXd& solution) {
// 转换Eigen矩阵到OSQP格式
// ... (省略转换代码)
OSQPData data;
data.n = P.rows();
data.m = A.rows();
data.P = csc_matrix(data.n, data.n, P.nonZeros(),
P.valuePtr(), P.outerIndexPtr(), P.innerIndexPtr());
data.q = const_cast<c_float*>(q.data());
data.A = csc_matrix(data.m, data.n, A.nonZeros(),
A.valuePtr(), A.outerIndexPtr(), A.innerIndexPtr());
data.l = const_cast<c_float*>(l.data());
data.u = const_cast<c_float*>(u.data());
// 设置工作空间
if(workspace_) {
osqp_cleanup(workspace_);
}
workspace_ = osqp_setup(&data, &settings_);
// 求解问题
osqp_solve(workspace_);
if(workspace_->info->status_val != OSQP_SOLVED) {
return false;
}
solution = Eigen::Map<Eigen::VectorXd>(
workspace_->solution->x, data.n);
return true;
}
private:
OSQPSettings settings_;
OSQPWorkspace* workspace_;
};
4.2 MPC专用求解接口
cpp复制class MPC_Solver {
public:
MPC_Solver(const Eigen::MatrixXd& A,
const Eigen::MatrixXd& B,
const Eigen::MatrixXd& Q,
const Eigen::MatrixXd& R,
int N)
: A_(A), B_(B), Q_(Q), R_(R), N_(N),
osqp_interface_(), n_(A.rows()), m_(B.cols()) {
// 预计算预测矩阵
Theta_ = buildPredictionMatrix(A, B, N);
// 预计算H矩阵
H_ = Eigen::MatrixXd::Zero(m_*N, m_*N);
for(int i=0; i<N; i++) {
H_.block(i*m_, i*m_, m_, m_) = R_;
if(i < N-1) {
H_.block(i*m_, i*m_, m_, m_) +=
B_.transpose() * Q_ * B_;
}
}
}
bool solve(const Eigen::VectorXd& x0,
const Eigen::VectorXd& x_ref,
const Eigen::MatrixXd& A_con,
const Eigen::VectorXd& lb_con,
const Eigen::VectorXd& ub_con,
Eigen::VectorXd& u_opt) {
// 构建f向量
Eigen::VectorXd f = Eigen::VectorXd::Zero(m_*N_);
Eigen::VectorXd temp = A_ * x0 - x_ref;
for(int i=0; i<N_; i++) {
f.segment(i*m_, m_) = B_.transpose() * Q_ * temp;
temp = A_ * temp;
}
// 调用OSQP求解
return osqp_interface_.solveQP(H_, f, A_con, lb_con, ub_con, u_opt);
}
private:
Eigen::MatrixXd A_, B_, Q_, R_, Theta_, H_;
int N_, n_, m_;
OSQPInterface osqp_interface_;
};
5. 实际应用与调试技巧
5.1 数值稳定性处理
MPC实现中常见的数值问题及解决方法:
-
矩阵条件数过大:
- 对系统进行缩放处理
- 使用QR分解代替直接矩阵求逆
cpp复制Eigen::VectorXd solveLeastSquares(const Eigen::MatrixXd& A, const Eigen::VectorXd& b) { return A.householderQr().solve(b); } -
预测时域选择:
- 一般选择N=10-20
- 可通过仿真测试不同N值下的性能
-
权重矩阵调整:
- Q矩阵对角元素通常取1/(状态量期望变化范围)^2
- R矩阵对角元素通常取1/(控制量变化范围)^2
5.2 实时性优化
-
热启动技术:
cpp复制void MPC_Solver::warmStart(const Eigen::VectorXd& prev_solution) { if(prev_solution.size() == m_*N_) { osqp_interface_.warmStart(prev_solution); } } -
代码优化技巧:
- 使用Eigen的Map类避免数据拷贝
- 预分配所有矩阵内存
- 使用固定大小矩阵当维度已知
-
多线程处理:
cpp复制#include <thread> void solveMPCAsync(MPC_Solver& solver, const Eigen::VectorXd& x0, const Eigen::VectorXd& x_ref, std::promise<Eigen::VectorXd>&& result) { Eigen::VectorXd u_opt; bool success = solver.solve(x0, x_ref, u_opt); result.set_value(u_opt); }
5.3 常见问题排查
-
OSQP求解失败:
- 检查约束是否相容
- 验证H矩阵是否正定
- 尝试调整OSQP参数(eps_abs, eps_rel等)
-
系统不稳定:
- 检查预测模型准确性
- 验证状态观测器收敛性
- 调整权重矩阵Q和R
-
实时性能不足:
- 分析代码热点(使用perf工具)
- 考虑降低预测时域N
- 尝试更简单的QP求解器
6. 扩展功能实现
6.1 非线性MPC近似
对于弱非线性系统,可通过连续线性化实现:
cpp复制class NonlinearMPC {
public:
NonlinearMPC(std::function<Eigen::VectorXd(const Eigen::VectorXd&,
const Eigen::VectorXd&)> f,
int n, int m, int N)
: f_(f), n_(n), m_(m), N_(N) {}
Eigen::VectorXd solve(const Eigen::VectorXd& x0,
const Eigen::VectorXd& x_ref,
const Eigen::VectorXd& u_prev) {
// 在当前状态线性化
Eigen::MatrixXd A = numericalJacobianX(x0, u_prev);
Eigen::MatrixXd B = numericalJacobianU(x0, u_prev);
// 构建线性MPC问题
MPC_Solver linear_solver(A, B, Q_, R_, N_);
// 求解
Eigen::VectorXd u_opt;
linear_solver.solve(x0, x_ref, u_opt);
return u_opt;
}
private:
Eigen::MatrixXd numericalJacobianX(const Eigen::VectorXd& x,
const Eigen::VectorXd& u) {
double eps = 1e-6;
Eigen::MatrixXd J(n_, n_);
for(int i=0; i<n_; i++) {
Eigen::VectorXd dx = Eigen::VectorXd::Zero(n_);
dx(i) = eps;
Eigen::VectorXd f1 = f_(x + dx, u);
Eigen::VectorXd f2 = f_(x - dx, u);
J.col(i) = (f1 - f2) / (2*eps);
}
return J;
}
// 类似实现numericalJacobianU
// ...
std::function<Eigen::VectorXd(const Eigen::VectorXd&,
const Eigen::VectorXd&)> f_;
int n_, m_, N_;
Eigen::MatrixXd Q_, R_;
};
6.2 经济MPC实现
考虑控制性能与经济性的平衡:
cpp复制MPCProblem buildEconomicMPCProblem(const Eigen::MatrixXd& A,
const Eigen::MatrixXd& B,
const Eigen::MatrixXd& Q,
const Eigen::MatrixXd& R,
const Eigen::MatrixXd& S,
const Eigen::VectorXd& x0,
const Eigen::VectorXd& x_ref,
int N) {
MPCProblem problem;
// 构建扩展的H矩阵
problem.H = Eigen::MatrixXd::Zero(2*m_*N, 2*m_*N);
for(int i=0; i<N; i++) {
problem.H.block(i*m_, i*m_, m_, m_) = R_;
problem.H.block((N+i)*m_, (N+i)*m_, m_, m_) = S_;
if(i < N-1) {
problem.H.block(i*m_, i*m_, m_, m_) +=
B.transpose() * Q * B;
}
}
// 构建f向量
// ...
return problem;
}
7. 性能评估与测试
7.1 单元测试框架
使用Google Test框架进行测试:
cpp复制#include <gtest/gtest.h>
TEST(MPCTest, PredictionMatrix) {
Eigen::Matrix2d A; A << 0.9, 0.1, 0, 0.8;
Eigen::Vector2d B; B << 0.5, 1.0;
Eigen::MatrixXd Theta = buildPredictionMatrix(A, B, 3);
// 验证预测矩阵的特定元素
ASSERT_NEAR(Theta(4,0), 0.5, 1e-6);
ASSERT_NEAR(Theta(5,1), 1.44, 1e-6);
}
TEST(OSQPTest, BasicQP) {
OSQPInterface solver;
Eigen::Matrix2d P; P << 4, 1, 1, 2;
Eigen::Vector2d q; q << 1, 1;
Eigen::MatrixXd A(3,2);
A << 1, 1, -1, 0, 0, -1;
Eigen::Vector3d l; l << 1, -OSQP_INFTY, -OSQP_INFTY;
Eigen::Vector3d u; u << 1, 0, 0;
Eigen::Vector2d solution;
bool success = solver.solveQP(P, q, A, l, u, solution);
ASSERT_TRUE(success);
ASSERT_NEAR(solution(0), 0.25, 1e-6);
ASSERT_NEAR(solution(1), 0.75, 1e-6);
}
7.2 闭环仿真测试
cpp复制void runClosedLoopSimulation(const Eigen::MatrixXd& A,
const Eigen::MatrixXd& B,
const Eigen::MatrixXd& C,
const Eigen::VectorXd& x0,
const Eigen::VectorXd& x_ref,
int steps) {
MPC_Solver mpc(A, B, Q_, R_, 10);
Eigen::VectorXd x = x0;
Eigen::VectorXd u = Eigen::VectorXd::Zero(B.cols());
for(int k=0; k<steps; k++) {
// 测量输出(添加噪声)
Eigen::VectorXd y = C * x + 0.01*Eigen::VectorXd::Random(C.rows());
// 状态估计
observer_.update(u, y);
Eigen::VectorXd x_hat = observer_.getState();
// MPC求解
bool success = mpc.solve(x_hat, x_ref, u);
if(!success) {
std::cerr << "MPC solve failed at step " << k << std::endl;
break;
}
// 应用第一个控制量
u = u.head(B.cols());
// 系统更新
x = A * x + B * u + 0.01*Eigen::VectorXd::Random(x.size());
// 记录数据
// ...
}
}
7.3 性能分析
使用chrono进行计时:
cpp复制#include <chrono>
void benchmarkMPC() {
auto start = std::chrono::high_resolution_clock::now();
// MPC求解
bool success = mpc_solver_.solve(x0_, x_ref_, u_opt_);
auto end = std::chrono::high_resolution_clock::now();
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
std::cout << "MPC solve time: " << duration.count() << " μs" << std::endl;
if(!success) {
std::cerr << "MPC solve failed!" << std::endl;
}
}
