1. MPC模型预测控制基础原理
模型预测控制(Model Predictive Control, MPC)是一种先进的控制策略,它通过在线求解有限时域内的最优控制问题来实现对动态系统的控制。与传统的PID控制相比,MPC具有处理多变量、约束条件和非线性系统的天然优势。
1.1 MPC核心思想解析
MPC的核心思想可以用"三步走"来概括:
- 预测:基于当前系统状态和模型,预测未来一段时间内系统的行为
- 优化:求解一个有限时域的最优控制问题,得到最优控制序列
- 执行:只实施控制序列中的第一步控制量,下一时刻重复整个过程
这种滚动时域的策略使MPC能够实时响应系统变化,同时考虑未来的控制效果。在实际工程中,这种"走一步看多步"的方法特别适合处理具有明显滞后特性的系统。
注意:MPC的计算复杂度与预测时域长度呈指数关系,实际应用中需要在控制精度和计算负担之间权衡。
1.2 数学建模与优化问题
MPC的数学基础可以表述为如下优化问题:
min J = ∑(xᵀQx + uᵀRu) + xₙᵀPxₙ
s.t.
xₖ₊₁ = Axₖ + Buₖ (系统动力学约束)
x ∈ 𝕏, u ∈ 𝕌 (状态和控制量约束)
其中:
- Q、R、P分别为状态、控制量和终端状态的权重矩阵
- 𝕏和𝕌定义了状态和控制的可行域
- 预测时域N决定了优化问题的规模
在车辆控制应用中,状态x通常包含位置、速度、航向角等,控制量u则包括加速度、转向角等。
1.3 车辆模型的选择与建立
车辆控制中常用的两种模型具有不同的特点和适用场景:
1.3.1 运动学模型
运动学模型基于几何关系建立,不考虑力的作用,其状态方程相对简单:
ẋ = v cosθ
ẏ = v sinθ
θ̇ = v tanδ / L
其中:
- (x,y)为车辆位置
- θ为航向角
- v为速度
- δ为前轮转向角
- L为轴距
运动学模型计算量小,适合低速场景(如园区车辆、自动泊车),但在高速时精度不足。
1.3.2 动力学模型
动力学模型考虑轮胎力、惯性等物理效应,更精确但计算复杂。常用的二自由度模型:
v̇ = (F₁ cosδ - F₂ sinδ)/m
ω̇ = (F₁ l₁ sinδ + F₁ l₂ cosδ + F₂ l₂)/I
ψ̇ = ω
ẋ = v cosψ - v sinψ
ẏ = v sinψ + v cosψ
其中:
- F₁、F₂为前后轮受力
- m为质量
- I为转动惯量
- l₁、l₂为前后轴到质心距离
动力学模型适合高速、高精度要求的场景,如高速公路自动驾驶。
2. MPC控制器设计与实现
2.1 控制器整体架构
一个完整的MPC控制系统通常包含以下模块:
- 状态估计器:获取/估计当前系统状态
- 参考轨迹生成:提供期望的跟踪轨迹
- 优化求解器:求解MPC优化问题
- 执行机构:实施控制命令
- 反馈回路:闭环控制的基础
在代码实现中,我们采用面向对象的设计模式,将各功能模块封装为独立类:
cpp复制class MPCController {
public:
void Init(const MPCConfig& config); // 初始化参数
ControlCommand ComputeControlCommand(
const VehicleState& state,
const Trajectory& ref_traj); // 计算控制量
private:
Eigen::MatrixXd SolveQPProblem(); // 求解QP问题
MPCConfig config_;
VehicleModel model_;
};
2.2 预测模型离散化
连续系统模型需要离散化后才能用于MPC。常用的离散化方法有:
-
前向欧拉法:简单但精度较低
xₖ₊₁ = xₖ + Tₛ(Axₖ + Buₖ) -
零阶保持法:更精确但计算复杂
xₖ₊₁ = eᴬᵀˢxₖ + (∫eᴬᵗdt)Buₖ
在车辆动力学控制中,我们采用零阶保持法保证精度:
cpp复制MatrixXd DiscretizeMatrix(const MatrixXd& A, const MatrixXd& B, double dt) {
int n = A.rows();
MatrixXd M = MatrixXd::Zero(n + A.cols(), n + A.cols());
M.block(0, 0, n, n) = A;
M.block(0, n, n, B.cols()) = B;
M.block(n, 0, B.cols(), n + B.cols()) = MatrixXd::Zero(B.cols(), n + B.cols());
MatrixXd Md = (M * dt).exp();
MatrixXd Ad = Md.block(0, 0, n, n);
MatrixXd Bd = Md.block(0, n, n, B.cols());
return Ad, Bd;
}
2.3 约束处理技术
MPC的强大之处在于能显式处理各种约束。常见的约束处理方法包括:
-
硬约束:直接作为优化问题的约束条件
cpp复制// 设置输入约束 for (int i = 0; i < horizon_; ++i) { qp_problem_.AddConstraint(u_min_ <= u.segment(i * nu, nu) <= u_max_); } -
软约束:通过惩罚函数处理可能违反的约束
cpp复制// 在代价函数中添加松弛变量惩罚项 cost += rho_ * s.squaredNorm(); // s为松弛变量 -
终端约束:保证稳定性
cpp复制// 添加终端状态约束 qp_problem_.AddConstraint(x_N == x_ref);
在车辆控制中,我们需要考虑转向角限制、加速度限制等物理约束。
3. 代码实现详解
3.1 C++实现核心模块
3.1.1 车辆动力学控制器
动力学控制器处理高精度控制场景,主要流程如下:
- 初始化车辆状态和控制器参数
- 在每个控制周期:
- 获取当前状态
- 生成参考轨迹
- 求解MPC问题
- 应用第一个控制量
- 更新车辆状态
关键代码片段:
cpp复制void VehicleDynControl::Run() {
// 初始化
VehicleState state = Initialize();
MPCController controller;
controller.Init(config_);
// 主循环
for (int i = 0; i < max_iter_; ++i) {
// 获取参考轨迹
Trajectory ref_traj = GenerateReferenceTrajectory(state, i);
// 计算控制命令
ControlCommand cmd = controller.ComputeControlCommand(state, ref_traj);
// 应用控制并更新状态
ApplyControl(cmd);
state = vehicle_model_.Update(state, cmd, dt_);
// 记录数据
LogData(state, cmd);
}
}
3.1.2 运动学控制器实现
运动学控制器更轻量,适合实时性要求高的场景。与动力学控制器的主要区别在于模型部分:
cpp复制VehicleState VehicleKinematicModel::Update(const VehicleState& state,
const ControlCommand& cmd,
double dt) {
VehicleState new_state = state;
// 更新航向角
new_state.yaw += state.v * tan(cmd.steer) / wheelbase_ * dt;
// 更新位置
new_state.x += state.v * cos(new_state.yaw) * dt;
new_state.y += state.v * sin(new_state.yaw) * dt;
// 更新速度
new_state.v += cmd.acc * dt;
return new_state;
}
3.2 MATLAB实现对比
MATLAB版本更适合快速原型验证,利用其强大的控制系统工具箱可以简化开发:
matlab复制function [u, x_opt] = mpc_controller(x0, ref_traj, model)
% 创建MPC控制器对象
mpcobj = mpc(model.A, model.B, model.C, model.D, ...
'Ts', model.Ts, 'PredictionHorizon', 10, ...
'ControlHorizon', 2);
% 设置约束
mpcobj.MV(1).Min = -0.5; % 最小加速度
mpcobj.MV(1).Max = 0.5; % 最大加速度
mpcobj.MV(2).Min = -0.6; % 最小转向角
mpcobj.MV(2).Max = 0.6; % 最大转向角
% 设置权重
mpcobj.Weights.OutputVariables = [1 1 0.5]; % 位置x,y和航向角权重
mpcobj.Weights.ManipulatedVariables = [0.1 0.1]; % 控制量权重
% 求解MPC问题
[u, ~, ~, x_opt] = mpcmove(mpcobj, x0, ref_traj);
end
MATLAB实现的优势在于:
- 内置QP求解器,无需手动实现
- 丰富的系统辨识和控制设计工具
- 便捷的可视化功能
但生产环境通常需要C++实现以获得更好的性能。
4. 典型应用案例实现
4.1 倒立摆控制
倒立摆是经典的MPC测试平台,其动力学方程为:
θ̈ = (mgl sinθ - bθ̇ + mlẍ cosθ)/(ml²)
MPC控制器设计要点:
- 状态变量: [θ, θ̇, x, ẋ]
- 控制量: 小车加速度ẍ
- 约束: 小车位移限制、加速度限制
- 目标: 保持倒立摆直立,同时小车跟踪参考轨迹
实现代码片段:
cpp复制VectorXd InvertedPendulumMPC::ComputeControl(const VectorXd& state,
const VectorXd& ref) {
// 设置QP问题
QPProblem qp;
qp.SetHessian(CalculateHessian());
qp.SetGradient(CalculateGradient(state, ref));
qp.SetConstraints(GetConstraints(state));
// 求解
VectorXd solution = qp.Solve();
// 返回第一个控制量
return solution.head(1);
}
4.2 车辆轨迹跟踪
车辆轨迹跟踪是自动驾驶的核心问题之一。我们实现了两种跟踪场景:
4.2.1 高速变道场景(动力学模型)
关键参数设置:
- 预测时域:20步(2秒)
- 控制时域:5步
- 采样时间:0.1秒
- 状态权重:Q = diag([10, 10, 5, 1, 1])
- 控制权重:R = diag([0.1, 0.5])
性能指标:
- 横向误差:<0.2m
- 航向角误差:<3°
- 计算时间:<50ms/步
4.2.2 低速泊车场景(运动学模型)
关键参数差异:
- 预测时域:30步(3秒)
- 采样时间:0.05秒
- 状态权重:Q = diag([5, 5, 3])
- 更严格的转向角约束:±30°
5. 工程实践中的关键问题
5.1 实时性保障技术
确保MPC在实时系统中的稳定运行需要多方面的优化:
-
代码级优化:
- 使用Eigen库的矩阵运算
- 开启编译器优化(-O3)
- 使用内存池避免动态分配
-
算法级优化:
- 热启动:用上一周期的解作为初始猜测
cpp复制void MPCController::Solve() { if (first_run_) { solver_.SetInitialGuess(zeros()); } else { solver_.SetInitialGuess(last_solution_.tail(N_*nu_)); } // ...求解过程 }- 提前终止:设置最大迭代次数和容忍度
-
硬件加速:
- 使用多线程并行计算
- 考虑GPU加速(QP求解)
5.2 数值稳定性处理
MPC实现中常见的数值问题及解决方案:
-
病态Hessian矩阵:
- 添加正则化项:H += δI
- 使用更好的数值库(Eigen的LLT或LDLT分解)
-
约束冲突:
- 引入松弛变量
- 优先级约束处理
-
采样时间敏感:
- 自适应MPC:根据计算时间调整预测时域
- 鲁棒MPC:考虑不确定性
5.3 参数整定经验
MPC参数整定是一个迭代过程,建议顺序:
- 先调整状态权重Q,确保基本跟踪性能
- 然后调整控制权重R,平滑控制量
- 最后调整预测时域N,平衡性能和计算量
经验法则:
- 预测时域应覆盖系统主要动态(如车辆至少1-2秒)
- 控制时域通常为预测时域的1/3到1/2
- 终端权重P可以取无限时域LQR的解
6. 可视化与调试技巧
6.1 基于ImGui的实时监控
我们开发了基于ImGui的可视化工具,主要功能包括:
-
轨迹对比视图:
- 同时显示规划轨迹和实际轨迹
- 支持缩放和平移
- 误差统计和显示
-
信号监控视图:
- 状态变量随时间变化曲线
- 控制量变化曲线
- 约束边界显示
-
参数调节界面:
- 实时调整MPC权重
- 修改预测时域
- 保存/加载参数配置
关键实现代码:
cpp复制void MPCVis::Update() {
ImGui::Begin("MPC Monitor");
// 轨迹视图
if (ImPlot::BeginPlot("Trajectory")) {
ImPlot::PlotLine("Reference", ref_x_.data(), ref_y_.data(), ref_x_.size());
ImPlot::PlotLine("Actual", act_x_.data(), act_y_.data(), act_x_.size());
ImPlot::EndPlot();
}
// 参数调节
ImGui::SliderFloat("Q Position", &q_pos_, 0.1f, 10.0f);
ImGui::SliderFloat("R Steering", &r_steer_, 0.01f, 1.0f);
ImGui::End();
}
6.2 常见问题诊断指南
实际调试中遇到的典型问题及解决方法:
| 问题现象 | 可能原因 | 解决方案 |
|---|---|---|
| 轨迹跟踪发散 | 预测时域太短 | 增加预测步数 |
| 控制量振荡 | 控制权重太小 | 增大R矩阵对应值 |
| 求解失败 | 约束冲突 | 检查约束合理性,添加松弛变量 |
| 计算超时 | QP问题太复杂 | 减少预测时域,简化模型 |
| 稳态误差 | 模型不准确 | 重新辨识模型参数 |
6.3 性能优化记录
我们对关键代码段进行了多次优化,效果对比:
-
QP求解器选择:
- qpOASES:平均2.1ms/步
- OSQP:平均1.3ms/步
- 自定义求解器:平均0.8ms/步
-
矩阵运算优化:
- 原始实现:5.2ms/步
- 使用Eigen:3.7ms/步
- 手动SIMD优化:2.9ms/步
-
内存管理:
- 动态分配:1.8ms/步
- 预分配内存:1.2ms/步
这些优化使得我们的MPC控制器能在10ms内完成一次计算,满足实时性要求。
