自行车模型与MPC在车辆轨迹跟踪中的应用

1. 自行车模型与后轴基准原理

在车辆轨迹跟踪控制中，自行车模型是最基础也最实用的运动学模型。这个简化模型将四轮车辆等效为两轮自行车，只考虑前轮转向和后轮驱动。选择后轴中心作为基准点，是因为在实际车辆控制中，后轴位置更接近车辆的质量中心，且便于测量和计算。

1.1 模型参数解析

自行车模型的核心参数包括：

• L：轴距（前后轮中心距离），典型轿车约为2.7-3.0米
• β：航向角（车辆纵轴与全局坐标系X轴的夹角）
• v：车速（后轴中心点的线速度）
• δ：前轮转向角

状态量x=[x, y, β]表示车辆后轴中心在全局坐标系中的位置和航向，控制量u=[v, δ]则是驾驶员的输入指令。这种表示方法在自动驾驶领域被称为"后轴参考模型"。

1.2 运动学方程推导

自行车模型的运动学关系可以通过几何分析得到。当车辆以速度v行驶，前轮转向角为δ时：

1. 后轴中心的x方向速度分量：v_x = v·cos(β)
2. 后轴中心的y方向速度分量：v_y = v·sin(β)
3. 航向角变化率：β' = (v·tan(δ))/L

这个非线性模型揭示了车辆运动的基本规律：位置变化由速度和航向决定，而航向变化又取决于速度和转向角。在离散时间系统中，我们采用欧拉近似将这些微分方程转化为差分方程：

python复制def bicycle_model(x, u, dt):
    L = 2.9  # 轴距(m)
    x_next = np.zeros(3)
    x_next[0] = x[0] + u[0] * np.cos(x[2]) * dt  # x位置更新
    x_next[1] = x[1] + u[0] * np.sin(x[2]) * dt  # y位置更新
    x_next[2] = x[2] + u[0] * np.tan(u[1]) / L * dt  # 航向角更新
    return x_next

注意：tan(δ)的使用体现了转向几何关系，当δ较小时tan(δ)≈δ，模型可线性化；但在大转向角时，必须使用完整的非线性关系。

2. MPC控制器设计与实现

模型预测控制(MPC)是一种基于模型、考虑约束的先进控制方法，特别适合车辆轨迹跟踪这类多变量、强耦合的控制问题。

2.1 预测模型构建

MPC的核心思想是在每个控制周期，基于当前状态和模型预测未来N步的系统行为，通过优化求解得到最优控制序列。对于我们的自行车模型：

1. 初始化预测状态序列：x_pred = [x_0, x_1, ..., x_N]
2. 初始化控制序列：u_pred = [u_0, u_1, ..., u_{N-1}]
3. 通过自行车模型建立状态传递关系：

   python复制for t in range(N):
       x_pred[t+1] = bicycle_model(x_pred[t], u_pred[t], dt)
   

2.2 目标函数设计

目标函数需要平衡轨迹跟踪精度和控制平滑性，典型设计包含以下成本项：

python复制cost = 0
for t in range(N):
    # 轨迹跟踪误差
    cost += (x_pred[t,0] - ref_x[t])**2 * 100  # x坐标偏差
    cost += (x_pred[t,1] - ref_y[t])**2 * 100  # y坐标偏差
    cost += (x_pred[t,2] - ref_theta[t])**2 * 50  # 航向角偏差
    
    # 控制平滑项
    if t > 0:
        cost += (u_pred[t,0] - u_pred[t-1,0])**2 * 0.1  # 加速度平滑
        cost += (u_pred[t,1] - u_pred[t-1,1])**2 * 10  # 转向变化平滑

权重系数选择经验：

• 位置误差权重最大(100)，确保轨迹跟踪精度
• 航向误差权重次之(50)，影响跟踪稳定性
• 转向变化权重(10)比速度变化权重(0.1)大，因为方向盘突变更危险

2.3 约束条件处理

实际车辆控制必须考虑物理限制：

python复制for t in range(N):
    # 车速限制(±15m/s ≈ 54km/h)
    opti.subject_to(opti.bounded(-15, u_pred[t,0], 15))
    
    # 转向角限制(±30度转换为弧度)
    opti.subject_to(opti.bounded(-np.pi/6, u_pred[t,1], np.pi/6))

关键细节：角度单位必须统一。曾经有项目因混用弧度/度导致实车转向过度，车辆直接冲出跑道。建议全程使用弧度制，避免单位混淆。

3. 圆形轨迹跟踪实践

圆形路径是验证跟踪控制器的经典场景，能全面测试控制器的转向协调能力和抗干扰性能。

3.1 参考轨迹生成

半径为5m的圆形参考轨迹生成方法：

python复制radius = 5.0  # 圆半径(m)
speed = 2.0   # 车速(m/s)
omega = speed / radius  # 角速度(rad/s)

ref_x = radius * np.sin(omega * t_array)
ref_y = radius * (1 - np.cos(omega * t_array))
ref_theta = omega * t_array + np.pi/2  # 切线方向

3.2 MPC参数调优

预测时域配置对控制效果影响显著：

1. 采样时间dt选择：

   • 典型值0.05-0.2s
   • 过小会增加计算负担，过大会降低控制精度

2. 预测步长N选择：

   • N×dt应接近车辆动态响应时间(1-2s)
   • 示例：dt=0.1s时N=15，预测1.5秒

3. 权重调整技巧：

   • 先调位置权重确保基本跟踪
   • 再调航向权重改善入弯表现
   • 最后调控制量权重平滑指令

3.3 可视化分析

通过轨迹可视化可以直观评估控制器性能：

python复制plt.figure(figsize=(10,6))
plt.plot(ref_x, ref_y, 'r--', label='参考轨迹')
plt.plot(x_hist[:,0], x_hist[:,1], 'b-', label='实际轨迹')
plt.quiver(x_hist[::10,0], x_hist[::10,1], 
          np.cos(x_hist[::10,2]), np.sin(x_hist[::10,2]), 
          scale=15, color='g')  # 航向箭头
plt.axis('equal')
plt.legend()

典型现象分析：

• 箭头指向外侧：MPC提前转向补偿离心力
• 轨迹波动：权重设置不当或约束过紧
• 相位滞后：预测时域不足或车速过高

4. 实战经验与问题排查

4.1 常见问题及解决方案

4.2 调试心得

1. 权重调整要循序渐进，每次只改一个参数
2. 实车测试前必须进行充分仿真验证
3. 记录所有参数变更和测试结果，便于回溯
4. 注意单位一致性，特别是角度单位
5. 预测时域应覆盖车辆动态响应时间

4.3 性能优化技巧

1. 热启动：用上一周期的解作为初始猜测
2. 代码优化：使用编译语言实现关键函数
3. 模型简化：小角度时可线性化模型
4. 并行计算：多核并行求解预测步
5. 降采样：长预测时域时可间隔采样

在调试过程中发现一个有趣现象：当设置N=50、dt=0.1s时，虽然预测时域长达5秒，但计算耗时剧增而控制效果改善有限。这是因为远处的预测对当前控制影响很小，反而增加了不必要的计算负担。最终确定N=15、dt=0.1s的组合在效果和效率间取得了良好平衡。