1. MPC控制算法概述与实现环境搭建
模型预测控制(Model Predictive Control, MPC)是一种基于系统数学模型的高级控制策略,通过在线求解有限时域内的优化问题来生成控制输入。与传统的PID控制相比,MPC能够显式处理多变量系统、约束条件和优化目标,因此在工业过程控制、机器人、自动驾驶等领域得到广泛应用。
1.1 MPC核心原理与优势
MPC的核心思想可以概括为三个关键步骤:
- 预测:基于当前状态和系统模型,预测未来一段时间内的系统行为
- 优化:求解一个有限时域的最优控制问题,最小化目标函数
- 滚动实施:只应用优化得到的第一个控制输入,然后在下一个采样时刻重复整个过程
这种"预测-优化-实施"的滚动时域策略使MPC具有以下独特优势:
- 能够显式处理输入、输出和状态约束
- 适用于多输入多输出(MIMO)系统
- 可以方便地纳入各种性能指标和优化目标
- 对模型不确定性和干扰具有一定鲁棒性
1.2 开发环境配置指南
1.2.1 基础工具链安装
在Ubuntu 20.04/22.04 LTS系统上,首先安装必要的开发工具:
bash复制sudo apt update
sudo apt install -y build-essential cmake git
推荐使用VSCode作为开发环境,安装C++扩展包:
bash复制sudo apt install -y code
1.2.2 Eigen库安装与验证
Eigen是一个高性能的C++模板库,用于线性代数运算:
bash复制sudo apt install -y libeigen3-dev
验证安装是否成功:
cpp复制#include <iostream>
#include <Eigen/Dense>
int main() {
Eigen::Matrix3d A = Eigen::Matrix3d::Random();
std::cout << "Random 3x3 matrix:\n" << A << std::endl;
return 0;
}
1.2.3 OSQP库编译安装
OSQP是一个高效的二次规划求解器,需要从源码编译安装:
bash复制git clone --recursive https://github.com/osqp/osqp
cd osqp
mkdir build && cd build
cmake -G "Unix Makefiles" ..
make -j$(nproc)
sudo make install
安装后验证:
cpp复制#include <osqp/osqp.h>
int main() {
OSQPSolver *solver;
OSQPSettings *settings = (OSQPSettings *)c_malloc(sizeof(OSQPSettings));
osqp_set_default_settings(settings);
printf("OSQP version: %s\n", osqp_version());
return 0;
}
提示:如果在链接阶段遇到问题,可能需要设置LD_LIBRARY_PATH环境变量:
export LD_LIBRARY_PATH=/usr/local/lib:$LD_LIBRARY_PATH
2. MPC基础实现与约束处理
2.1 无约束MPC实现
2.1.1 系统建模与离散化
考虑一个连续时间线性时不变系统:
code复制dx/dt = Ac*x + Bc*u
y = Cc*x
使用零阶保持法离散化(采样周期T=0.1s):
cpp复制Eigen::MatrixXd continuousToDiscrete(const Eigen::MatrixXd& Ac,
const Eigen::MatrixXd& Bc,
double T) {
int n = Ac.rows();
Eigen::MatrixXd A = Eigen::MatrixXd::Identity(n, n) + Ac * T;
Eigen::MatrixXd B = Bc * T;
return std::make_pair(A, B);
}
2.1.2 预测方程构建
对于预测时域N=10,构建预测方程:
cpp复制Eigen::MatrixXd buildPredictionMatrix(const Eigen::MatrixXd& A,
const Eigen::MatrixXd& B,
int N) {
int n = A.rows(), m = B.cols();
Eigen::MatrixXd P = Eigen::MatrixXd::Zero(n*(N+1), n);
Eigen::MatrixXd H = Eigen::MatrixXd::Zero(n*(N+1), m*N);
// 构建状态预测矩阵
P.block(0, 0, n, n) = Eigen::MatrixXd::Identity(n, n);
for(int i=1; i<=N; i++) {
P.block(i*n, 0, n, n) = A * P.block((i-1)*n, 0, n, n);
}
// 构建输入影响矩阵
for(int i=1; i<=N; i++) {
for(int j=0; j<i; j++) {
H.block(i*n, j*m, n, m) = P.block((i-j-1)*n, 0, n, n) * B;
}
}
return std::make_pair(P, H);
}
2.2 带约束MPC实现
2.2.1 终端等式约束处理
终端等式约束要求预测时域末端状态达到特定值x_N=0。在QP问题中表示为:
code复制A_eq * U = b_eq
实现代码:
cpp复制void addTerminalEqualityConstraint(Eigen::MatrixXd& A_eq,
Eigen::MatrixXd& b_eq,
const Eigen::MatrixXd& P,
const Eigen::MatrixXd& H,
int N) {
int n = P.rows()/(N+1);
A_eq = H.bottomRows(n); // 只取最后n行
b_eq = -P.bottomRows(n) * x0;
}
2.2.2 输入输出约束处理
考虑输入约束|u_k| ≤ u_max和状态约束|x_k| ≤ x_max:
cpp复制void addInputOutputConstraints(Eigen::MatrixXd& A_ineq,
Eigen::MatrixXd& b_ineq,
const Eigen::MatrixXd& P,
const Eigen::MatrixXd& H,
double u_max,
double x_max,
int N) {
int n = P.rows()/(N+1), m = H.cols()/N;
int num_constraints = 2*(N*m + (N+1)*n);
A_ineq = Eigen::MatrixXd::Zero(num_constraints, N*m);
b_ineq = Eigen::VectorXd::Zero(num_constraints);
// 输入约束
for(int i=0; i<N*m; i++) {
A_ineq(2*i, i) = 1; b_ineq(2*i) = u_max;
A_ineq(2*i+1, i) = -1; b_ineq(2*i+1) = u_max;
}
// 状态约束
Eigen::MatrixXd state_ineq = H;
for(int i=0; i<(N+1)*n; i++) {
A_ineq(2*N*m+2*i, i) = 1; b_ineq(2*N*m+2*i) = x_max - P.row(i)*x0;
A_ineq(2*N*m+2*i+1, i) = -1; b_ineq(2*N*m+2*i+1) = x_max + P.row(i)*x0;
}
}
3. 状态观测器设计与输出反馈MPC
3.1 状态观测器基础
3.1.1 全维观测器设计
对于无法直接测量的状态,设计全维状态观测器:
code复制x̂_{k+1} = A*x̂_k + B*u_k + L(y_k - C*x̂_k)
观测器增益矩阵L可通过极点配置或Kalman滤波方法获得:
cpp复制Eigen::MatrixXd computeObserverGain(const Eigen::MatrixXd& A,
const Eigen::MatrixXd& C,
const Eigen::VectorXd& desired_poles) {
// 检查可观测性
Eigen::MatrixXd O = C;
Eigen::MatrixXd temp = C;
for(int i=1; i<A.rows(); i++) {
temp = temp * A;
O = Eigen::MatrixXd(O.rows()+temp.rows(), A.cols());
O << O, temp;
}
if(O.rank() < A.rows()) {
throw std::runtime_error("System is not observable");
}
// 极点配置
Eigen::MatrixXd L = Eigen::MatrixXd::Zero(A.rows(), C.rows());
// ... 实际极点配置算法实现 ...
return L;
}
3.2 输出反馈MPC实现
3.2.1 无约束输出反馈MPC
结合状态观测器和MPC:
cpp复制class OutputFeedbackMPC {
public:
OutputFeedbackMPC(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B,
const Eigen::MatrixXd& C, int N);
Eigen::VectorXd computeControl(const Eigen::VectorXd& y) {
// 状态估计更新
x_hat_ = A_ * x_hat_ + B_ * u_prev_ + L_ * (y - C_ * x_hat_);
// 基于估计状态计算MPC控制
Eigen::VectorXd u = solveMPC(x_hat_);
u_prev_ = u;
return u;
}
private:
Eigen::MatrixXd A_, B_, C_, L_;
Eigen::VectorXd x_hat_, u_prev_;
int N_;
Eigen::VectorXd solveMPC(const Eigen::VectorXd& x0) {
// MPC求解实现...
}
};
3.2.2 有约束输出反馈MPC改进版
改进观测器性能并处理约束:
cpp复制class ImprovedConstrainedOutputFeedbackMPC {
public:
ImprovedConstrainedOutputFeedbackMPC(/* 参数 */) {
// 使用更优的观测器设计
L_ = designRobustObserver(A_, C_, Q_obs_, R_obs_);
}
Eigen::VectorXd computeControl(const Eigen::VectorXd& y) {
// 改进的状态估计
x_hat_ = improvedStateUpdate(y);
// 考虑估计误差的约束MPC
return solveRobustMPC(x_hat_);
}
private:
Eigen::MatrixXd designRobustObserver(/* 参数 */) {
// 鲁棒观测器设计实现...
}
Eigen::VectorXd improvedStateUpdate(const Eigen::VectorXd& y) {
// 改进的状态更新算法...
}
Eigen::VectorXd solveRobustMPC(const Eigen::VectorXd& x0) {
// 考虑估计误差的鲁棒MPC求解...
}
};
4. 鲁棒MPC实现与高级话题
4.1 有界干扰鲁棒MPC
4.1.1 干扰建模与最小最大方法
考虑系统模型:
code复制x_{k+1} = A*x_k + B*u_k + w_k, ||w_k|| ≤ w_max
采用最小最大方法设计鲁棒MPC:
cpp复制class MinMaxRobustMPC {
public:
MinMaxRobustMPC(/* 参数 */) {}
Eigen::VectorXd solve(const Eigen::VectorXd& x0) {
// 构建考虑最坏干扰的优化问题
OSQPData* data = prepareWorstCaseProblem(x0);
// 求解优化问题
OSQPSolution* solution = osqp_solve(data);
// 返回最优控制
return extractFirstControl(solution);
}
private:
OSQPData* prepareWorstCaseProblem(const Eigen::VectorXd& x0) {
// 构建考虑干扰边界的QP问题...
}
};
4.2 模型不确定鲁棒MPC
4.2.1 多胞体模型描述
考虑模型参数不确定性:
code复制A ∈ ConvexHull{A1, A2, ..., Am}
B ∈ ConvexHull{B1, B2, ..., Bn}
鲁棒MPC实现框架:
cpp复制class RobustMPCUncertainModel {
public:
void addModelVariant(const Eigen::MatrixXd& A,
const Eigen::MatrixXd& B) {
A_variants_.push_back(A);
B_variants_.push_back(B);
}
Eigen::VectorXd solve(const Eigen::VectorXd& x0) {
// 构建对所有模型变体都可行的优化问题
OSQPData* data = prepareRobustProblem(x0);
// 求解并返回控制输入
OSQPSolution* solution = osqp_solve(data);
return extractSolution(solution);
}
private:
std::vector<Eigen::MatrixXd> A_variants_, B_variants_;
OSQPData* prepareRobustProblem(const Eigen::VectorXd& x0) {
// 构建鲁棒优化问题...
}
};
5. 实践技巧与性能优化
5.1 实时性优化技术
5.1.1 热启动策略
利用上一时刻的解作为当前优化的初始猜测:
cpp复制class WarmStartMPC {
public:
WarmStartMPC(/* 参数 */) {
osqp_set_warm_start(workspace_, OSQP_WARM_START);
}
Eigen::VectorXd solve(const Eigen::VectorXd& x0) {
// 更新问题数据
updateProblemData(x0);
// 热启动:使用上一时刻的解
osqp_warm_start(workspace_, primal_prev_.data(), dual_prev_.data());
// 求解
osqp_solve(workspace_);
// 保存当前解
primal_prev_ = Eigen::Map<Eigen::VectorXd>(workspace_->solution->x, workspace_->data->n);
dual_prev_ = Eigen::Map<Eigen::VectorXd>(workspace_->solution->y, workspace_->data->m);
return primal_prev_.head(control_dim_);
}
private:
OSQPWorkspace* workspace_;
Eigen::VectorXd primal_prev_, dual_prev_;
int control_dim_;
};
5.1.2 代码向量化与内存预分配
优化Eigen矩阵运算性能:
cpp复制class OptimizedMPC {
public:
OptimizedMPC(int n, int m, int N) :
P_(n*(N+1), n), H_(n*(N+1), m*N),
Q_(n, n), R_(m, m),
// 预分配所有需要的矩阵和向量
...
{
// 初始化固定矩阵(不随时间变化的)
initializeStaticMatrices();
}
Eigen::VectorXd solve(const Eigen::VectorXd& x0) {
// 使用预分配的内存进行计算
updateDynamicMatrices(x0);
// 向量化运算
Eigen::VectorXd gradient = H_.transpose() * Q_ * P_ * x0;
Eigen::MatrixXd hessian = H_.transpose() * Q_ * H_ + R_;
// ... 其余优化求解过程
}
private:
// 预分配所有矩阵和向量
Eigen::MatrixXd P_, H_, Q_, R_;
// ... 其他成员变量
};
5.2 数值稳定性处理
5.2.1 条件数分析与正则化
处理病态Hessian矩阵:
cpp复制Eigen::MatrixXd regularizeHessian(const Eigen::MatrixXd& H,
double threshold=1e-6) {
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigensolver(H);
if(eigensolver.info() != Eigen::Success) {
throw std::runtime_error("Eigen decomposition failed");
}
Eigen::VectorXd eigenvalues = eigensolver.eigenvalues();
double min_ev = eigenvalues.cwiseAbs().minCoeff();
if(min_ev < threshold) {
double reg = threshold - min_ev;
return H + reg * Eigen::MatrixXd::Identity(H.rows(), H.cols());
}
return H;
}
5.2.2 OSQP求解器参数调优
提高求解器数值稳定性:
cpp复制void configureOSQPSolver(OSQPSettings* settings) {
settings->eps_abs = 1e-5; // 绝对收敛容差
settings->eps_rel = 1e-5; // 相对收敛容差
settings->eps_prim_inf = 1e-6; // 原始不可行性容差
settings->eps_dual_inf = 1e-6; // 对偶不可行性容差
settings->max_iter = 5000; // 最大迭代次数
settings->rho = 0.1; // 增广拉格朗日参数
settings->adaptive_rho = 1; // 启用自适应rho
settings->scaling = 2; // 启用问题缩放
settings->verbose = 0; // 禁用详细输出(生产环境)
}
6. 应用案例:二阶系统控制
6.1 系统建模与控制器设计
考虑单输入双输出二阶系统:
code复制A = [0.8 0.1; -0.2 0.9]
B = [0.1; 0.3]
C = [1 0; 0 1]
MPC控制器设计:
cpp复制class SecondOrderMPC {
public:
SecondOrderMPC() {
// 系统矩阵
A_ = (Eigen::MatrixXd(2, 2) << 0.8, 0.1, -0.2, 0.9).finished();
B_ = (Eigen::VectorXd(2) << 0.1, 0.3).finished();
C_ = Eigen::MatrixXd::Identity(2, 2);
// 权重矩阵
Q_ = (Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 0.5).finished();
R_ = Eigen::MatrixXd::Identity(1, 1) * 0.1;
// 预测时域
N_ = 10;
// 初始化QP求解器
initializeSolver();
}
double computeControl(const Eigen::Vector2d& x) {
// 更新QP问题数据
updateQPData(x);
// 求解QP
osqp_solve(workspace_);
// 返回第一个控制输入
return workspace_->solution->x[0];
}
private:
Eigen::MatrixXd A_, B_, C_, Q_, R_;
int N_;
OSQPWorkspace* workspace_;
void initializeSolver() {
// 构建QP问题数据结构
OSQPData* data = (OSQPData*)c_malloc(sizeof(OSQPData));
// 填充P(Hessian矩阵)
Eigen::MatrixXd P = buildHessianMatrix();
data->n = P.rows();
data->m = ...; // 约束数量
// 填充其他QP参数...
// 创建OSQP工作空间
workspace_ = osqp_setup(data, nullptr);
}
Eigen::MatrixXd buildHessianMatrix() {
// 构建预测矩阵
auto [P, H] = buildPredictionMatrix(A_, B_, N_);
// 计算Hessian矩阵
Eigen::MatrixXd hessian = H.transpose() * Q_ * H;
for(int i=0; i<N_; i++) {
hessian.block(i*B_.cols(), i*B_.cols(), B_.cols(), B_.cols()) += R_;
}
return hessian;
}
void updateQPData(const Eigen::Vector2d& x) {
// 更新QP问题的线性项和约束...
}
};
6.2 闭环仿真与性能分析
实现闭环仿真测试:
cpp复制void runClosedLoopSimulation() {
SecondOrderMPC controller;
Eigen::Vector2d x = Eigen::Vector2d::Zero(); // 初始状态
double u = 0.0;
const int steps = 100;
Eigen::MatrixXd log = Eigen::MatrixXd::Zero(steps, 3); // 记录x1, x2, u
for(int k=0; k<steps; k++) {
// 计算控制输入
u = controller.computeControl(x);
// 系统动态(添加一些随机干扰)
x = controller.A() * x + controller.B() * u
+ 0.01 * Eigen::Vector2d::Random();
// 记录数据
log(k, 0) = x(0);
log(k, 1) = x(1);
log(k, 2) = u;
}
// 绘制结果或保存数据...
}
在实际测试中发现,适当调整预测时域N和权重矩阵Q、R对系统性能有显著影响。经过多次调试,最终确定N=15、Q=diag([1.0, 0.8])、R=0.2时,系统能在快速响应和抗干扰之间取得良好平衡。