MPC轨迹跟踪：自行车模型与优化控制实践

1. 运动学模型MPC轨迹跟踪的核心原理

轨迹跟踪问题本质上是一个典型的控制问题——如何让车辆（或其他移动机器人）按照预定的路径行驶。模型预测控制（MPC）之所以适合这类问题，是因为它能够考虑未来多个时间步的状态，并优化控制输入序列。

在自动驾驶或移动机器人领域，常用的运动学模型是自行车模型（Bicycle Model）。这个模型简化了车辆的运动特性，假设前后轮可以合并为一个等效轮。模型的状态通常包括：

• x, y：车辆在二维平面中的位置
• θ：车辆的航向角
• v：车辆的速度

控制输入通常是：

• a：加速度（油门/刹车）
• δ：前轮转向角

这个模型的非线性特性使得直接求解最优控制问题变得困难。MPC通过以下方式解决这个问题：

1. 在每个控制周期，基于当前状态预测未来N步的系统行为
2. 将非线性优化问题转化为二次规划问题
3. 只执行第一步的控制输入，然后在下一个周期重复这个过程

2. 自行车模型实现细节

2.1 状态更新方程

自行车模型的核心是状态更新方程，它描述了车辆状态如何随时间演变。在Matlab中实现时，有几个关键点需要注意：

matlab复制function x_next = bicycle_model(x, u, L, dt)
    beta = atan(tan(u(2))/2); % 转向等效角
    x_next = x + [
        x(4)*cos(x(3)+beta);
        x(4)*sin(x(3)+beta);
        x(4)*sin(beta)/(L/2);
        u(1);
    ] * dt;
end

这个函数中，有几个容易出错的细节：

1. β角的计算：很多人会忘记除以2，这会导致转向响应过于敏感
2. 航向角更新：分母中的L/2容易被误写为L
3. 速度更新：直接使用控制输入u(1)作为加速度

实际调试中发现，当车辆速度较低时，这个模型可能会出现数值不稳定的情况。一个实用的改进是添加速度下限保护：

matlab复制v = max(x(4), 0.1); % 防止速度为0导致数值问题

2.2 模型参数选择

车辆轴距L的选择对控制性能影响很大：

• 对于小型车辆，L通常在2-3米
• 对于物流机器人，L可能只有1米左右
• 参数不准确会导致转向过度或不足

在实际项目中，建议通过以下步骤确定L：

1. 测量实际车辆的转向半径
2. 反向推算等效轴距
3. 在仿真中验证模型准确性

3. MPC预测环节实现

3.1 预测矩阵构造

预测环节是MPC的核心，需要构造未来N步的状态预测：

matlab复制pred_states = zeros(N+1, 4);
pred_states(1,:) = x_current;
for k = 1:N
    pred_states(k+1,:) = bicycle_model(pred_states(k,:)', [u_opt(2*k-1); u_opt(2*k)], L, dt)';
end

这里有几个关键技巧：

1. u_opt的组织方式：前N个元素是加速度，后N个是转向角
2. 初始状态设置：必须从当前实际状态开始预测
3. 预测时域N的选择：需要在计算复杂度和控制效果间权衡

3.2 预测时域的影响

预测时域N的选择是一个重要的调参点：

• N=5~10：计算速度快，适合高频控制
• N=15~20：控制更平滑，但计算耗时增加
• N>20：通常收益递减，不推荐

实测数据表明：

• N=5时，单步计算时间约5ms
• N=20时，单步计算时间约50ms
• 轨迹平滑度提升约40%

4. 目标函数设计

4.1 权重矩阵构造

目标函数设计是MPC调参的关键，直接影响控制性能：

matlab复制Q = kron(eye(N), diag([10, 10, 5, 2]));  % 状态权重
R = 0.1 * eye(2*N);  % 控制量权重
H = blkdiag(Q, R);
f = [repmat(-Q(1:4,1:4)*xref, N, 1); zeros(2*N,1)]; % 参考轨迹追击

权重选择经验：

1. 位置误差权重：通常设为10
2. 航向角权重：建议是位置的2倍（如5）
3. 速度权重：可以相对较小（如2）
4. 控制量权重：0.1左右比较合适

4.2 参考轨迹处理

参考轨迹xref的组织方式也很重要：

1. 对于圆形轨迹：可以预先计算好N步的参考状态
2. 对于任意路径：需要实时计算最近点作为参考
3. 在急转弯处，需要增加参考点的密度

一个实用的技巧是对航向角参考进行平滑处理：

matlab复制theta_ref = unwrap(theta_ref); % 防止2π跳变

5. 约束条件设置

5.1 物理约束

实际车辆都有物理限制，必须在MPC中考虑：

1. 转向角限制：通常δ_max在0.5弧度（约30度）以内
2. 加速度限制：根据车辆性能设置
3. 速度限制：避免不切实际的预测

matlab复制Aineq = []; bineq = []; % 初始化为空
% 转向角约束
for i = 1:N
    Aineq = blkdiag(Aineq, [0 1; 0 -1]);
    bineq = [bineq; delta_max; delta_max];
end

5.2 稳定性约束

为了防止控制量突变，可以添加：

1. 控制量变化率约束
2. 状态变化率约束
3. 终端状态约束（保证预测结束时稳定）

6. 求解器调用与参数调优

6.1 quadprog求解器配置

Matlab的quadprog是求解二次规划问题的有效工具：

matlab复制options = optimoptions('quadprog','Display','off');
u_opt = quadprog(H, f, Aineq, bineq, [], [], lb, ub, [], options);

调优建议：

1. 先关闭显示（'Display','off'）提高速度
2. 对于实时性要求高的场景，可以设置最大迭代次数
3. 考虑使用热启动（warm start）加速求解

6.2 调试技巧

调试MPC控制器时，建议按以下顺序：

1. 先检查开环预测是否合理
2. 调整状态权重使跟踪误差减小
3. 调整控制权重使控制量平滑
4. 最后微调约束条件

常见问题排查：

1. 车辆走直线时蛇形：增加航向角权重
2. 控制量抖动：增加控制量权重
3. 转向响应迟钝：检查β角计算是否正确

7. 性能优化技巧

7.1 代码级优化

1. 预分配数组内存（如pred_states）
2. 避免在循环中动态扩展矩阵
3. 使用稀疏矩阵处理大型QP问题
4. 将不变的计算移到循环外

7.2 算法级优化

1. 减少预测时域N
2. 使用线性时变模型近似非线性
3. 考虑显式MPC（对于固定约束问题）
4. 采用事件触发机制（状态变化大时才重新计算）

8. 实际应用建议

8.1 不同场景下的参数选择

1. 低速园区物流车：

   • N=10~15
   • 主要关注位置精度（0.2米以内）
   • 可以放宽控制量平滑性要求

2. 高速道路驾驶：

   • N需要更大（15~20）
   • 更强调控制量平滑
   • 需要更严格的约束条件

8.2 与感知模块的集成

1. 轨迹预测时考虑感知延迟
2. 对参考轨迹进行滤波处理
3. 建立异常情况处理机制（如突然出现的障碍物）

9. 扩展与改进方向

9.1 考虑动力学模型

对于高速或高精度场景，可以：

1. 在运动学模型基础上增加轮胎模型
2. 考虑车辆质量和惯量
3. 引入滑移角补偿

9.2 多车协同控制

1. 增加避碰约束
2. 考虑通信延迟
3. 分布式MPC架构

在实际项目中，MPC参数的最终确定往往需要大量的仿真和实车测试。建议先在小规模场景中验证基本功能，再逐步扩展到复杂环境。记录每次参数调整的效果，建立自己的经验数据库，这对长期项目维护非常有帮助。