1. MPC控制基础与实现环境搭建
模型预测控制(MPC)是一种基于系统数学模型的高级控制策略,它通过在每个控制周期求解有限时域内的优化问题来生成控制指令。与传统的PID控制相比,MPC能够显式处理多变量系统、约束条件和优化目标,这使得它在工业过程控制、机器人、自动驾驶等领域得到广泛应用。
1.1 MPC核心原理
MPC的核心思想可以概括为三个关键步骤:
- 预测:基于当前系统状态和模型,预测未来一段时间内的系统行为
- 优化:求解使性能指标最优的控制序列,同时满足各种约束条件
- 执行:应用优化得到的第一个控制输入,然后在下一个采样周期重复整个过程
这种滚动时域优化的特点使MPC能够适应系统变化和不确定性。在数学上,MPC问题通常可以表述为一个二次规划(QP)问题:
minimize 1/2 * u^T * H * u + f^T * u
subject to A * u ≤ b
其中u是优化变量,H和f定义了目标函数,不等式约束描述了系统动力学和操作限制。
1.2 开发环境配置
1.2.1 工具链安装
在Linux环境下,我们推荐使用以下工具链进行MPC开发:
- VSCode:轻量级但功能强大的代码编辑器
- CMake:跨平台的构建系统
- GCC/G++:C++编译器
安装命令示例(Ubuntu系统):
bash复制sudo apt update
sudo apt install -y build-essential cmake code
1.2.2 依赖库安装
MPC实现需要两个核心数学库:
- Eigen3:高性能线性代数库
bash复制sudo apt install -y libeigen3-dev
- 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 <Eigen/Dense>
#include <osqp/osqp.h>
int main() {
Eigen::MatrixXd test(2,2);
test << 1, 0, 0, 1;
OSQPSettings settings;
osqp_set_default_settings(&settings);
return 0;
}
提示:如果在链接阶段遇到问题,可能需要手动设置库路径:
bash复制export LD_LIBRARY_PATH=/usr/local/lib:$LD_LIBRARY_PATH
2. MPC基础实现框架
2.1 系统建模与离散化
考虑一个典型的线性时不变系统:
dx/dt = A_c * x + B_c * u
y = C * x
首先需要将其离散化为:
x_{k+1} = A * x_k + B * u_k
y_k = C * x_k
离散化方法(零阶保持):
cpp复制Eigen::MatrixXd discretize(const Eigen::MatrixXd& Ac,
const Eigen::MatrixXd& Bc,
double dt) {
int n = Ac.rows();
Eigen::MatrixXd A = Eigen::MatrixXd::Identity(n, n) + Ac * dt;
Eigen::MatrixXd B = Bc * dt;
return std::make_pair(A, B);
}
2.2 预测模型构建
MPC的核心是预测模型,我们需要构建预测时域内的状态和控制序列。对于预测时域N,定义:
X = [x_0, x_1, ..., x_N]^T
U = [u_0, u_1, ..., u_{N-1}]^T
预测方程可以表示为:
X = Sx * x0 + Su * U
其中Sx和Su是适当的转换矩阵:
cpp复制void buildPredictionMatrices(const Eigen::MatrixXd& A,
const Eigen::MatrixXd& B,
int N,
Eigen::MatrixXd& Sx,
Eigen::MatrixXd& Su) {
int n = A.rows(), m = B.cols();
Sx = Eigen::MatrixXd::Zero(N*n, n);
Su = Eigen::MatrixXd::Zero(N*n, N*m);
// 构建Sx
Eigen::MatrixXd An = Eigen::MatrixXd::Identity(n, n);
for(int i=0; i<N; i++) {
Sx.block(i*n, 0, n, n) = An;
An = An * A;
}
// 构建Su
for(int i=0; i<N; i++) {
Eigen::MatrixXd An = Eigen::MatrixXd::Identity(n, n);
for(int j=0; j<=i; j++) {
Su.block(i*n, j*m, n, m) = An * B;
An = A * An;
}
}
}
3. 约束MPC实现
3.1 终端等式约束MPC
终端等式约束要求预测时域末端状态达到特定值x_N = x_ref。这在跟踪问题中很常见。
QP问题构建:
cpp复制OSQPData buildEqTerminalMPC(const Eigen::MatrixXd& Sx,
const Eigen::MatrixXd& Su,
const Eigen::MatrixXd& Q,
const Eigen::MatrixXd& R,
const Eigen::VectorXd& x0,
const Eigen::VectorXd& x_ref,
int N) {
OSQPData data;
int n = Q.rows(), m = R.rows();
// 目标函数:H = Su'*Qbar*Su + Rbar, f = x0'*Sx'*Qbar*Su
Eigen::MatrixXd Qbar = Eigen::kroneckerProduct(Eigen::MatrixXd::Identity(N, N), Q);
Eigen::MatrixXd Rbar = Eigen::kroneckerProduct(Eigen::MatrixXd::Identity(N, N), R);
data.P = Su.transpose() * Qbar * Su + Rbar;
data.q = (x0.transpose() * Sx.transpose() * Qbar * Su).transpose();
// 等式约束:Sx_N * x0 + Su_N * U = x_ref
Eigen::MatrixXd Su_N = Su.block((N-1)*n, 0, n, N*m);
Eigen::MatrixXd A_eq = Su_N;
Eigen::VectorXd l_eq = x_ref - Sx.block((N-1)*n, 0, n, n) * x0;
Eigen::VectorXd u_eq = l_eq;
// 合并约束
data.A = A_eq;
data.l = l_eq;
data.u = u_eq;
return data;
}
3.2 终端不等式约束MPC
更一般的情况是终端状态需要满足不等式约束:
x_min ≤ x_N ≤ x_max
只需修改约束构建部分:
cpp复制// 替换等式约束部分
Eigen::MatrixXd A_ineq = Su_N;
Eigen::VectorXd l_ineq = x_min - Sx.block((N-1)*n, 0, n, n) * x0;
Eigen::VectorXd u_ineq = x_max - Sx.block((N-1)*n, 0, n, n) * x0;
data.A = A_ineq;
data.l = l_ineq;
data.u = u_ineq;
4. 状态观测器设计
4.1 全状态观测器
当系统状态不可直接测量时,需要设计状态观测器。Luenberger观测器形式为:
x̂_{k+1} = A * x̂_k + B * u_k + L * (y_k - C * x̂_k)
观测器增益矩阵L的设计可以通过极点配置:
cpp复制Eigen::MatrixXd placeObserverPoles(const Eigen::MatrixXd& A,
const Eigen::MatrixXd& C,
const Eigen::VectorXd& poles) {
// 检查能观性
Eigen::MatrixXd Ob = C;
Eigen::MatrixXd An = A;
for(int i=1; i<A.rows(); i++) {
Ob = (Ob.transpose() * An).transpose();
An = An * A;
}
if(Ob.rank() < A.rows()) {
throw std::runtime_error("System is not observable");
}
// 极点配置
Eigen::MatrixXd K = Eigen::MatrixXd::Zero(C.cols(), A.rows());
Eigen::MatrixXd F = A.transpose();
Eigen::MatrixXd G = C.transpose();
K = acker(F, G, poles);
return K.transpose();
}
4.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),
P(Eigen::MatrixXd::Identity(A.rows(), A.rows())),
x_hat(Eigen::VectorXd::Zero(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 += K * (y - C * x_hat);
P = (Eigen::MatrixXd::Identity(P.rows(), P.cols()) - K * C) * P;
}
Eigen::VectorXd state() const { return x_hat; }
private:
Eigen::MatrixXd A, B, C, Q, R, P;
Eigen::VectorXd x_hat;
};
5. 鲁棒MPC实现
5.1 有界干扰鲁棒MPC
考虑系统模型:
x_{k+1} = A * x_k + B * u_k + w_k
其中 ||w_k|| ≤ δ
鲁棒MPC需要保证在最坏干扰下仍满足约束。采用min-max方法:
cpp复制OSQPData buildRobustMPC(const Eigen::MatrixXd& Sx,
const Eigen::MatrixXd& Su,
const Eigen::MatrixXd& Q,
const Eigen::MatrixXd& R,
const Eigen::VectorXd& x0,
double delta,
int N) {
// 构建标称问题
OSQPData data = buildEqTerminalMPC(Sx, Su, Q, R, x0, x_ref, N);
// 添加干扰影响项
Eigen::MatrixXd W = Eigen::kroneckerProduct(Eigen::MatrixXd::Identity(N, N),
Eigen::MatrixXd::Identity(n, n));
data.P += W.transpose() * Q * W * delta * delta;
// 收紧约束
for(int i=0; i<data.l.size(); i++) {
data.l(i) += delta;
data.u(i) -= delta;
}
return data;
}
5.2 模型不确定鲁棒MPC
当系统矩阵(A,B)存在不确定性时,可以采用多模型方法:
cpp复制struct UncertainModel {
Eigen::MatrixXd A, B;
double probability;
};
OSQPData buildMultiModelMPC(const std::vector<UncertainModel>& models,
const Eigen::MatrixXd& Q,
const Eigen::MatrixXd& R,
const Eigen::VectorXd& x0,
int N) {
// 为每个模型构建预测矩阵
std::vector<Eigen::MatrixXd> Sxs, Sus;
for(const auto& model : models) {
Eigen::MatrixXd Sx, Su;
buildPredictionMatrices(model.A, model.B, N, Sx, Su);
Sxs.push_back(Sx);
Sus.push_back(Su);
}
// 构建联合优化问题
OSQPData data;
int n = Q.rows(), m = R.cols();
// 目标函数是各模型代价的加权和
Eigen::MatrixXd P = Eigen::MatrixXd::Zero(N*m, N*m);
Eigen::VectorXd q = Eigen::VectorXd::Zero(N*m);
for(size_t i=0; i<models.size(); i++) {
Eigen::MatrixXd Qbar = Eigen::kroneckerProduct(Eigen::MatrixXd::Identity(N, N), Q);
P += models[i].probability * (Sus[i].transpose() * Qbar * Sus[i] +
Eigen::kroneckerProduct(Eigen::MatrixXd::Identity(N, N), R));
q += models[i].probability * (x0.transpose() * Sxs[i].transpose() * Qbar * Sus[i]).transpose();
}
data.P = P;
data.q = q;
// 约束需要满足所有模型
// 这里简化处理,实际需要更复杂的约束合并
data.A = Sus[0];
data.l = Eigen::VectorXd::Constant(N*n, -OSQP_INFTY);
data.u = Eigen::VectorXd::Constant(N*n, OSQP_INFTY);
return data;
}
6. 实现技巧与调试建议
6.1 数值稳定性处理
MPC实现中常见的数值问题及解决方案:
-
矩阵条件数过大:
- 对系统进行缩放,使状态变量量纲相近
- 使用QR分解代替直接矩阵求逆
-
QP问题不可行:
- 检查约束是否自相矛盾
- 引入松弛变量处理可能的约束冲突
cpp复制// 在目标函数中添加松弛变量惩罚项 data.P += Eigen::kroneckerProduct(Eigen::MatrixXd::Identity(N, N), rho * Eigen::MatrixXd::Identity(n, n)); -
预测时域选择:
- 一般选择N使得N*Δt覆盖系统主要动态
- 可以通过仿真试验确定合适的N值
6.2 性能优化技巧
-
稀疏矩阵利用:
cpp复制// 使用Eigen的稀疏矩阵特性 typedef Eigen::SparseMatrix<double> SpMat; SpMat P_sparse = P.sparseView(); OSQPCscMatrix* P_csc = (OSQPCscMatrix*)csc_spalloc(P_sparse.cols(), P_sparse.rows(), P_sparse.nonZeros(), 1, 1); // 填充P_csc... data.P = P_csc; -
热启动:
cpp复制// 重用上一次的解作为初始猜测 if(work) { osqp_warm_start(work, u_prev.data(), nullptr); } -
代码剖析:
- 使用perf或gprof分析计算热点
- 通常QP求解和矩阵乘法是主要耗时部分
6.3 调试与验证
-
开环测试:
- 固定控制序列检查预测是否正确
- 验证QP求解器输出是否符合预期
-
闭环测试:
- 使用已知稳定系统验证控制器性能
- 逐步增加复杂度:先无约束,再加约束
-
可视化工具:
cpp复制// 使用matplotlib-cpp进行结果可视化 #include <matplotlibcpp.h> namespace plt = matplotlibcpp; void plotResults(const std::vector<double>& t, const std::vector<double>& x) { plt::plot(t, x); plt::title("State Trajectory"); plt::xlabel("Time"); plt::ylabel("State"); plt::show(); }
7. 扩展与进阶方向
7.1 非线性MPC
对于非线性系统,可以考虑:
- 连续线性化:在每个采样点进行线性化
- 微分动态规划:基于动态规划的优化方法
- 基于模型的强化学习:结合学习与优化
7.2 分布式MPC
对于大规模系统,可采用分布式架构:
- 分解协调法:将大系统分解为子系统
- ADMM算法:分布式优化框架
7.3 硬件加速
实时性要求高的应用可以考虑:
- GPU加速:使用CUDA实现矩阵运算
- FPGA实现:定制硬件加速QP求解
- 代码生成:使用CVXGEN等工具生成定制求解器
在实际工程应用中,我发现MPC参数调节需要平衡多个因素:预测时域长度影响计算负担和控制性能,约束收紧可以提高鲁棒性但可能降低可行性。一个实用的技巧是从简单配置开始,先让控制器能稳定运行,再逐步添加复杂特性。对于实时性要求高的应用,可以预先计算好不同工况下的控制策略,运行时根据当前状态选择最接近的策略作为热启动点,这能显著减少在线计算时间。