模型预测控制(MPC)在轨迹跟踪中的Matlab实现与优化-代码聚汇网

模型预测控制(MPC)在轨迹跟踪中的Matlab实现与优化

RED韵

1. 项目背景与核心价值

第一次接触模型预测控制（MPC）做轨迹跟踪时，我被它那种"预判走位"的能力惊艳到了。传统PID控制就像蒙着眼睛走路，走一步算一步；而MPC则是提前计算未来N步的走法，选出最优解后只执行第一步，然后重新计算——这种滚动优化的思路在自动驾驶、机器人控制等领域简直是降维打击。

纯Matlab实现意味着不依赖ROS、CarSim等专业平台，从运动学建模到求解器编写全部自己撸代码。这种"裸奔"方式虽然工程性不强，但对理解MPC底层原理特别有帮助。我曾用这个方案在大学生方程式赛车上实现过循迹控制，实测在20km/h速度下横向误差能控制在5cm以内。

2. 运动学模型构建

2.1 自行车模型简化

车辆运动学模型选用了经典的自行车模型（Bicycle Model），这是大多数入门级MPC教程的首选。虽然忽略了轮胎滑移等动力学因素，但对低速场景（<30km/h）已经足够：

code复制dx/dt = v * cos(θ + β)
dy/dt = v * sin(θ + β)
dθ/dt = v * sin(β) / l_r
β = arctan(l_r / (l_f + l_r) * tan(δ))

其中β是滑移角，l_f和l_r分别是前后轴到质心的距离。在Matlab里我习惯用符号变量先声明这些参数：

matlab复制syms v theta delta lf lr
beta = atan(lr/(lf+lr) * tan(delta));

2.2 线性化处理技巧

MPC需要离散化的状态空间模型，直接处理非线性模型计算量太大。我的经验是在每个控制周期做一阶泰勒展开：

matlab复制% 在参考点(x_ref, u_ref)处线性化
A = jacobian(f, x);
B = jacobian(f, u);
C = eye(4);  
D = zeros(4,1);
sys = ss(A,B,C,D);

这里有个坑：当参考转向角δ_ref较大时，线性化误差会显著增加。解决方法要么是减小预测时域，要么在QP求解器里增加约束条件。

3. MPC控制器设计

3.1 预测模型离散化

采用零阶保持法离散化，采样时间Δt的选择很有讲究：

matlab复制dt = 0.1; % 100ms控制周期
sys_d = c2d(sys, dt);

实测发现：Δt>0.2s时控制器开始出现明显振荡，<0.05s则QP求解时间可能超过控制周期。我在FSAE赛车上最终选用0.08s作为平衡点。

3.2 代价函数设计

代价函数J包含三个关键项：

matlab复制Q = diag([10, 10, 5, 0]); % 状态误差权重
R = 0.1;                  % 控制量权重
S = 2;                    % 控制变化率权重

调参心得：

横向误差(y)权重通常要大于纵向(x)
航向角θ的权重过大会导致"蛇形走位"
控制变化率权重S能抑制方向盘抖动

3.3 约束条件处理

在Matlab中直接用quadprog求解时，约束设置要特别注意单位统一：

matlab复制Aineq = []; bineq = [];
Aeq = []; beq = [];
lb = [-0.5; -5];  % 前轮转角限幅±30度，加速度限幅±5m/s²
ub = [0.5; 5];

遇到过的一个坑：没考虑转向执行器速率限制，导致仿真可行但实车无法跟踪。后来增加了相邻控制量差值的约束：

matlab复制delta_u_max = 0.2; % 转向角变化率限制
Aineq = [1 0 -1 0; -1 0 1 0];
bineq = [delta_u_max; delta_u_max];

4. Matlab实现细节

4.1 滚动优化实现

主循环包含三个关键步骤：

matlab复制while t < t_final
    % 1. 获取当前状态
    x = measure_state();
    
    % 2. 求解QP问题
    u = solve_mpc(x, ref_traj);
    
    % 3. 执行控制量
    apply_control(u(1));
    
    % 4. 更新状态
    t = t + dt;
end

实测发现90%的计算时间消耗在solve_mpc上，其中矩阵构造就占了60%。通过预分配内存可提升30%效率：

matlab复制persistent H f A_con b_con;
if isempty(H)
    H = zeros(N*p, N*p); % 预分配Hessian矩阵
end

4.2 参考轨迹处理

参考轨迹的存储方式影响很大。我推荐用循环缓冲区而非直接截取：

matlab复制function ref = get_ref_traj(current_pos, horizon)
    global full_traj;
    [~, idx] = min(vecnorm(full_traj(:,1:2) - current_pos, 2, 2));
    ref = full_traj(idx:min(idx+horizon,end), :);
end

处理闭环轨迹时有个技巧：在终点和起点之间插入10%的重叠段，可以避免突变。

5. 调试与性能优化

5.1 典型问题排查

控制器发散：
- 检查线性化点是否偏离实际状态太远
- 尝试增大预测时域(N>20)或减小控制时域(M<5)
高频振荡：
- 降低Q矩阵中位置误差权重
- 增加控制变化率权重S
求解失败：
- 检查quadprog选项：options = optimset('Display','off','Algorithm','interior-point-convex');
- 放宽约束条件再逐步收紧

5.2 实时性优化

热启动：用上一周期的解作为初始猜测

matlab复制x0 = [u_prev; zeros(N*p-1,1)];
[u, fval] = quadprog(H,f,Aineq,bineq,Aeq,beq,lb,ub,x0,options);

降维处理：当N>30时，可尝试用Krylov子空间方法近似求解

代码生成：把核心QP求解部分转为C代码

matlab复制coder.config('mex');
codegen solve_mpc -args {zeros(4,1), zeros(N,4)}

6. 扩展应用与改进方向

6.1 参数自适应

在实践中发现固定权重参数难以适应不同曲率路径。后来改进为速度相关的动态调整：

matlab复制Q(1,1) = 10 + v^2/2;  % 速度越高，横向误差惩罚越大
R = 0.2 - v/100;      % 高速时放松转向控制

6.2 考虑执行器延迟

真实系统存在约100ms的转向延迟，可通过状态预测补偿：

matlab复制x_actual = x + f(x,u)*delay_time;

6.3 结合曲率前馈

单纯反馈控制会存在原理性跟踪误差。我的解决方案是增加前馈项：

matlab复制delta_ff = atan(wheelbase * curvature);
u_total = u_fb + 0.7*delta_ff;  % 前馈占比70%

这个方案在2023年大学生电动方程式大赛中，帮助车队在高速弯道（45km/h）将最大横向误差从22cm降到了8cm。后来移植到实验室的AGV小车上时，发现需要把自行车模型改成全向轮模型才能准确描述其运动特性——这提醒我模型选择必须与实际被控对象匹配。