自动驾驶轨迹跟踪：MPC控制MATLAB实现与优化

1. 项目背景与核心问题

轨迹跟踪是自动驾驶和智能车辆控制领域的经典问题。传统PID控制方法在车辆动力学非线性特性面前往往表现不佳，特别是在高速过弯或紧急避障等复杂场景下。模型预测控制（MPC）因其能够显式处理多变量约束和系统非线性的优势，成为解决这一问题的理想选择。

我在实际车辆控制项目中发现，MPC的核心挑战在于三个方面：首先是车辆动力学模型的准确性，过于简单的模型会导致控制效果失真；其次是实时性要求，必须在有限采样周期内完成优化计算；最后是参数整定，需要平衡跟踪精度与执行器负荷。这些痛点正是本项目要攻克的关键。

2. 模型预测控制基础架构

2.1 MPC工作原理图解

MPC采用滚动时域优化策略，每个控制周期执行以下流程：

1. 获取当前系统状态（车辆位置、速度等）
2. 基于模型预测未来N步的系统行为
3. 求解最优控制序列（通常是最小化跟踪误差）
4. 仅执行第一步控制量，下一周期重新优化

关键参数选择：预测时域N=10~20，控制时域M=3~5（需根据车辆速度调整）

2.2 车辆动力学建模

采用自行车模型（Bicycle Model）作为基础：

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

其中l_f/l_r为前后轴距，δ为前轮转角。这个简化模型在速度<10m/s时误差<5%，适合实时控制。

3. MATLAB实现详解

3.1 仿真环境搭建

matlab复制% 参考轨迹生成（Clothoid曲线）
s = 0:0.1:100;
kappa = 0.01*s; % 曲率线性变化
refTraj = cumtrapz(s, [cos(kappa.*s); sin(kappa.*s)]);

% 车辆参数
param.mass = 1500; % kg
param.lf = 1.2;    % m
param.lr = 1.6;    % m

3.2 MPC控制器核心代码

matlab复制function [u, opt_info] = mpc_controller(x0, refTraj, param)
    % 构建优化问题
    prob = optimproblem;
    u = optimvar('u', 2, N, 'LowerBound',[-5;-0.5], 'UpperBound',[5;0.5]);
    
    % 目标函数：跟踪误差+控制量平滑
    cost = 0;
    x_pred = x0;
    for k = 1:N
        x_pred = vehicle_model(x_pred, u(:,k), param);
        cost = cost + (x_pred(1:2)-refTraj(:,k))'*Q*(x_pred(1:2)-refTraj(:,k));
        if k>1
            cost = cost + (u(:,k)-u(:,k-1))'*R*(u(:,k)-u(:,k-1));
        end
    end
    
    prob.Objective = cost;
    [sol,~,exitflag] = solve(prob);
    
    if exitflag <= 0
        warning('MPC求解失败！采用备用控制');
        u = [0;0]; 
    else
        u = sol.u(:,1); % 仅取第一步控制量
    end
end

3.3 权重矩阵整定技巧

Q/R矩阵的选择直接影响控制效果：

• 增大Q(1,1)强化横向位置跟踪
• 增大Q(2,2)强化纵向位置跟踪
• 增大R(1,1)限制加速度突变
• 增大R(2,2)限制转向角速度

实测建议初始值：

matlab复制Q = diag([10, 5]);  % 位置误差权重
R = diag([0.1, 1]); % 控制量变化权重

4. 典型问题与调优方案

4.1 求解器超时问题

当采样周期为50ms时，可能遇到：

• 现象：fmincon无法在时限内收敛
• 解决方案：
  1. 采用热启动(warm-start)，用上一周期解作为初始猜测
  2. 减少预测时域N（牺牲长远预测保实时性）
  3. 改用QP求解器（如OSQP）处理凸优化问题

4.2 轨迹振荡分析

常见于高速场景：

• 根本原因：模型失配或延迟补偿不足
• 调试步骤：
  1. 检查模型与实际车辆响应的一致性
  2. 在成本函数中加入转向角速率惩罚项
  3. 增加状态估计器的预测补偿

4.3 硬件在环测试要点

转入实车测试阶段需注意：

1. 执行器延迟补偿（通常200-500ms）
2. CAN通信时序同步（时间戳对齐）
3. 安全监控策略（如转向角速率限制）

5. 进阶优化方向

5.1 非线性MPC实现

当车辆处于极限工况（如低附着路面），需采用：

matlab复制% 使用fmincon直接处理非线性模型
options = optimoptions('fmincon', 'Algorithm','sqp',...
                      'MaxIterations',100);
nlprob = optimproblem;
...
sol = solve(nlprob, 'Options',options);

5.2 机器学习辅助参数整定

通过强化学习自动调整Q/R权重：

1. 定义奖励函数：r = -（跟踪误差 + 控制代价）
2. 构建DDPG智能体
3. 在多种场景下训练权重调整策略

5.3 多模MPC架构

针对不同驾驶场景切换控制器模式：

• 巡航模式：侧重舒适性（R矩阵权重增大）
• 紧急避障：侧重响应速度（Q矩阵权重增大）
• 泊车模式：缩短预测时域提高频率

6. 实测效果对比

在Carsim联合仿真中，与PID控制器对比：

虽然MPC计算负荷较高，但在急弯路段（曲率>0.1m⁻¹）的跟踪精度提升显著。实际部署时建议采用Intel i7以上处理器或嵌入式GPU加速。