无人驾驶轨迹跟踪：MPC控制与运动学建模实践

1. 从自行车到无人车：轨迹跟踪的本质思考

第一次骑自行车时，父亲在身后扶着后座，我紧张地握着把手左右摇晃。当车速逐渐提升，车身反而变得稳定——这个童年记忆完美诠释了车辆运动学的核心原理：速度与转向的微妙平衡。如今在无人驾驶领域，我们正用数学模型复现这种人类与生俱来的平衡能力。

轨迹跟踪控制要解决的核心问题是：如何让车辆严格遵循预设路径行驶？传统PID控制器在低速场景表现尚可，但面对复杂路况时往往力不从心。2016年我在参与校园无人车项目时，就曾目睹PID控制器在急弯处产生持续振荡，最终导致车辆偏离赛道。这促使我们转向模型预测控制（MPC）方案，其核心优势在于：

• 前瞻性：基于当前状态预测未来多步行为
• 约束处理：显式考虑车辆物理限制
• 多目标优化：平衡跟踪精度与乘坐舒适性

以常见的阿克曼转向车辆为例，其运动学模型可简化为：

code复制ẋ = v * cos(θ)
ẏ = v * sin(θ)
θ̇ = v * tan(δ)/L

其中(x,y)为后轴中心坐标，θ是航向角，v为车速，δ为前轮转角，L为轴距。这个看似简单的模型，却蕴含着解决轨迹跟踪问题的全部密码。

2. 运动学建模：从物理定律到差分方程

2.1 车辆运动学模型的推导艺术

建立准确的运动学模型是MPC控制的基础。与动力学模型不同，运动学模型忽略轮胎滑移、质量分布等因素，专注于几何关系。在MATLAB/Simulink中搭建模型时，我习惯采用以下验证步骤：

1. 静态验证：固定输入参数，手工计算输出并与模型比对
2. 动态验证：施加阶跃输入，观察响应曲线是否符合物理直觉
3. 极限测试：输入饱和值，检查数值稳定性

以自行车模型为例，其离散化形式常写作：

code复制x(k+1) = x(k) + T*v(k)*cos(θ(k))
y(k+1) = y(k) + T*v(k)*sin(θ(k))
θ(k+1) = θ(k) + T*v(k)*tan(δ(k))/L

其中T为采样时间。这个模型在低速（<5m/s）场景误差通常在3%以内，但要注意：

实际工程中需添加转向角速率限制，否则优化可能产生不切实际的瞬态指令。我们曾在测试中因此导致EPS系统报错。

2.2 模型线性化的实用技巧

为提升MPC的求解效率，常需要对非线性模型进行线性化。泰勒展开虽然是标准方法，但在车辆模型中存在更高效的途径。通过引入路径坐标系(s,ey,eψ)，可以将跟踪问题转化为：

code复制ėy = v*sin(eψ)
ėψ = v*(κ - tan(δ)/L)

其中κ为路径曲率。这种表述天然降低了状态耦合度，我在实际项目中测得计算耗时可减少40%。

一个容易忽略的细节是：线性化点应选在预测时域内的参考轨迹点上，而非当前车辆位置。这能显著提升大曲率路径的跟踪精度。

3. MPC控制器的工程实现细节

3.1 代价函数设计的平衡之道

构建合理的代价函数是MPC的核心艺术。经过多个项目迭代，我的代价函数模板逐渐定型为：

code复制J = Σ(α*ey² + β*eψ² + γ*Δδ²) + ρ*ε

其中：

• ey,eψ为横向/航向误差
• Δδ为转向角变化率（保证舒适性）
• ε为松弛变量（保证可行性）

参数调优时发现有趣现象：α/β比值决定车辆是"激进型"还是"保守型"驾驶风格。通过实车测试，我们总结出不同场景的黄金比例：

code复制高速公路： α/β ≈ 0.3
城市道路： α/β ≈ 1.2
停车场：  α/β ≈ 2.0

3.2 约束处理的实战经验

约束条件是将理论转化为工程的关键。除常规的转向角、转向速率限制外，有几点特别值得注意：

1. 速度相关约束：转向角限值应随速度增加而减小，我们使用：

   code复制|δ| ≤ δ_max * (1 - v/v_max)
   

2. 曲率连续性：相邻预测步间的曲率变化需满足：

   code复制|κ(k+1)-κ(k)| ≤ Δκ_max
   

3. 摩擦圆约束：虽然运动学模型不考虑动力学，但添加近似约束可提升安全性：

   code复制a² + (v²κ)² ≤ (μg)²
   

在dSPACE快速原型系统中，将这些约束转化为QP标准形式时，要特别注意稀疏矩阵的存储方式。使用CSC格式相比COO格式可节省约30%内存。

4. 实车部署中的那些"坑"

4.1 传感器异步带来的挑战

理想中的状态估计假设所有传感器数据严格同步，但现实中：

• GPS更新频率：10Hz
• IMU：100Hz
• 轮速脉冲：不定时

我们开发的解决方案是：

1. 建立统一的时间基准（PTP协议同步）
2. 采用运动学模型作为预测器
3. 设计基于IEKF的异步融合算法

实测表明，这种方法可将定位抖动从±15cm降低到±5cm以内。

4.2 计算延迟的补偿技巧

MPC求解需要时间，导致控制指令总是"过时"。通过以下方法可有效补偿：

• 在预测模型中加入延迟状态：

  code复制x_delayed = x + v*cos(θ)*t_delay
  

• 使用前向补偿：将求解完成的控制量按延迟时间提前发送

在NX平台上，我们测得平均延迟为80ms，补偿后跟踪误差降低62%。一个反直觉的发现是：延迟补偿过度反而会引发系统振荡，建议补偿量不超过实际延迟的120%。

5. 进阶：考虑道路曲率的预测增强

传统MPC使用分段线性参考路径，在急弯处会产生"切弯"现象。我们创新性地将道路曲率信息融入预测模型：

code复制θ̇ = v*(κ_ref - tan(δ)/L)

其中κ_ref来自高精地图。在张家界99道弯测试路段，这种方法将最大横向误差从1.2m降至0.3m。

实现时要注意：曲率信息需要做低通滤波，否则高频噪声会被MPC放大。建议截止频率设为：

code复制f_c = 0.1 * N / T

N为预测步数，T为采样时间。

6. 从仿真到实车的验证体系

6.1 高保真仿真环境搭建

基于CarSim+Simulink的联合仿真平台是我们的标准配置，但有几点特别优化：

1. 接口采样时间匹配：CarSim默认50Hz，需与控制器同步
2. 路面摩擦系数渐变：模拟真实道路变化
3. 传感器噪声模型：采用实测数据统计特性

一个实用技巧：在Simulink中启用"External Mode"调试，可以实时修改变量观察响应。

6.2 实车测试的标准化流程

我们制定的五步测试法：

1. 静态测试：检查通信链路与紧急停止
2. 低速跟踪（<10km/h）：验证基础功能
3. 中速跟踪（30-50km/h）：调优控制参数
4. 极限场景：湿滑路面、GPS遮挡等
5. 长期耐久：连续运行8小时以上

在冬季测试中发现一个有趣现象：低温会导致轮胎有效半径缩小约2%，需在线更新轮速脉冲系数。