自动驾驶轨迹规划：APF与MPC组合方案实践

1. 自动驾驶轨迹规划中的技术组合方案

在自动驾驶系统开发中，轨迹规划与控制算法的协同设计一直是核心挑战。我最近完成的一个项目尝试将人工势场法(APF)与模型预测控制(MPC)相结合，在Carsim和Simulink联合仿真环境下实现了车辆的主动避撞和换道功能。这个方案最大的特点是既保持了人工势场法的直观物理特性，又发挥了MPC在动态环境中的优化能力。

人工势场法本质上模拟了物理学中的势能场概念：将目标点设为引力源，障碍物设为斥力源，车辆在合成力场中沿着势能下降方向运动。这种方法的优势在于计算效率高、物理意义明确，特别适合教学演示和快速原型开发。但在实际应用中，传统APF存在局部极小值问题和动态障碍物应对不足的缺陷。

MPC则通过滚动时域优化策略，在每个控制周期求解有限时域内的最优控制问题。我们将APF生成的参考轨迹作为MPC的软约束，既保留了APF的路径规划特性，又通过MPC的优化框架弥补了APF的不足。这种组合方式在测试中表现出良好的跟踪性能，最大横向误差控制在0.3m以内。

2. 人工势场法的实现与调参

2.1 核心算法实现

人工势场法的核心是计算引力和斥力向量。引力使车辆向目标点运动，斥力使车辆远离障碍物。在我的实现中，引力计算采用简单的线性模型：

matlab复制function [F_att, F_rep] = APF_Calc(current_pos, goal, obstacles)
    % 引力计算
    k_att = 0.5;  % 引力增益系数
    F_att = k_att * (goal - current_pos);
    
    % 斥力计算
    F_rep = [0,0];
    for i = 1:size(obstacles,1)
        d = norm(current_pos - obstacles(i,:));
        if d < 2.5  % 影响半径
            k_rep = 10;  % 斥力增益系数
            F_rep = F_rep + k_rep*(1/d - 1/2.5)*(1/d^2)*(current_pos - obstacles(i,:))/d;
        end
    end
end

这个函数返回的合成力(F_att + F_rep)决定了车辆的期望运动方向。值得注意的是，斥力计算中包含了距离的倒数项(1/d)和平方倒数项(1/d²)，这使得障碍物附近的斥力场具有非线性特性。

2.2 参数调试经验

在参数调试过程中，有几个关键发现：

1. 引力系数k_att：控制车辆向目标点运动的"积极性"。值过大会导致车辆在接近目标点时产生振荡，过小则会使车辆响应迟缓。经过测试，0.5左右的值在大多数场景下表现良好。

2. 斥力系数k_rep：决定避障行为的激烈程度。当k_rep>15时，轨迹会出现明显震荡；k_rep<8时，车辆可能无法及时避开障碍物。最终选择k_rep=10作为平衡点。

3. 影响半径：设为2.5米是基于典型城市道路场景中车辆的安全距离考虑。这个值需要根据实际车辆尺寸和速度调整，速度越高，影响半径应该越大。

重要提示：参数调试时建议先固定k_att，单独调整k_rep观察避障效果，然后再微调k_att使车辆能平滑到达目标点。

3. MPC控制器的设计与实现

3.1 预测模型构建

MPC控制器的性能很大程度上取决于预测模型的准确性。我们采用经典的自行车模型作为预测模型：

code复制dx/dt = v * cos(θ + β)
dy/dt = v * sin(θ + β)
dθ/dt = (v / L) * sin(β)
dδ/dt = u

其中β = arctan(0.5*tan(δ))，L为轴距，δ为前轮转角，u为控制输入(转向角速度)。这个模型在中等速度下(<15m/s)能较好地近似车辆动力学行为。

3.2 代价函数设计

代价函数是MPC的核心，我们的设计包含四个关键项：

1. 轨迹跟踪误差：最小化与APF生成参考轨迹的偏差
2. 控制量惩罚：平滑转向操作，避免剧烈转向
3. 控制变化率惩罚：保证转向角速度的连续性
4. 终端代价：确保预测时域末端的稳定性

特别值得注意的是，我们将APF的参考轨迹作为软约束而非硬约束处理。这意味着当遇到突发障碍物时，MPC可以临时偏离参考轨迹，通过调整各项权重实现更灵活的避障行为。

3.3 参数选择与优化

经过多次试验，我们确定了以下MPC参数组合：

在Simulink中实现时，我们选择active-set作为QP求解器，因为它在我们的问题规模下表现出更好的实时性。

4. 联合仿真中的工程实践

4.1 Carsim与Simulink的接口设计

联合仿真的核心是建立Carsim和Simulink之间的数据交换通道。我们采用S-Function作为桥梁，关键接口包括：

1. Carsim输出：车辆状态(位置、速度、航向角等)、传感器数据
2. Simulink输出：控制命令(转向角、油门/刹车)

在实践中发现，直接从Carsim获取的方向盘转角信号含有高频噪声，会导致MPC产生抖振。解决方案是在接口中加入二阶巴特沃斯低通滤波器，截止频率设为5Hz：

matlab复制% 二阶巴特沃斯低通滤波器设计
[b,a] = butter(2, 5/(100/2), 'low');  % 采样频率100Hz
filtered_angle = filter(b, a, raw_angle);

这个简单的处理显著提高了控制信号的平滑度。

4.2 典型问题排查

在开发过程中遇到几个典型问题及解决方案：

1. 高频抖动问题：

   • 现象：MPC输出的转向指令出现不合理的快速振荡
   • 原因：Carsim输出的方向盘转角信号噪声过大
   • 解决：添加上述低通滤波器

2. 局部极小值问题：

   • 现象：当障碍物移动速度超过自车速度70%时，车辆会陷入"犹豫"状态
   • 原因：传统APF的静态势场无法适应快速变化的场景
   • 解决：在MPC预测时域内动态调整势场参数

3. 实时性问题：

   • 现象：仿真速度远低于实时
   • 原因：MPC求解器选择不当
   • 解决：将内点法求解器改为active-set方法

5. 性能评估与改进方向

5.1 跟踪性能分析

在多种测试场景下评估系统性能，典型结果如下：

特别是在弯道加移动障碍物的复合场景中，系统表现出良好的鲁棒性，跟踪误差RMS值稳定在0.2m以内。

5.2 改进方向

虽然当前方案已经满足基本需求，但仍有几个值得改进的方向：

1. 势场动态调整：根据障碍物相对速度自适应调整势场参数，更好处理高速动态场景
2. 多目标MPC：在代价函数中同时考虑舒适性、能耗等多个优化目标
3. 传感器噪声建模：在仿真中引入更真实的传感器噪声，增强算法鲁棒性
4. 硬件在环测试：将算法部署到实时硬件平台，验证实际计算负载

这套方案的一个意外收获是它非常适合用于教学演示。通过可视化势场分布和车辆轨迹，学生可以直观理解APF的工作原理和MPC的优化过程。我在课程实践中发现，这种��视化的方法能显著提高学生对自动驾驶控制算法的理解深度。