自动驾驶弯道轨迹规划：Frenet坐标系与三圆模型实践

1. 复杂弯道轨迹规划的技术挑战

在自动驾驶领域，弯道轨迹规划堪称最具挑战性的任务之一。想象一下，当车辆以60km/h的速度进入一个半径为50米的弯道时，突然发现前方有抛锚车辆——此时规划系统需要在毫秒级时间内完成以下决策：

• 保持当前车道但需紧急制动
• 借道超车但需考虑对向车道
• 轻微偏离车道线但保持安全距离

传统笛卡尔坐标系下的规划算法在这种场景下往往捉襟见肘。我曾参与过一个山区道路测试项目，原方案在直道上表现优异，但遇到连续S弯时，轨迹会出现明显的"锯齿"现象。后来我们改用Frenet坐标系，问题迎刃而解。

Frenet坐标系的核心优势在于：

1. 将复杂的二维平面问题分解为沿参考线(s轴)和垂直参考线(d轴)的两个一维问题
2. 天然适配道路的几何特性，特别是曲率变化
3. 简化了障碍物距离计算和碰撞检测逻辑

2. 三圆车辆模型与碰撞检测

2.1 模型构建原理

车辆轮廓近似是轨迹规划中的经典问题。单圆模型计算简单但精度不足，矩形包络又会导致计算复杂度激增。我们采用的三圆模型在两者间取得了完美平衡：

code复制车辆前圆权重 = 0.3 * 轴距
车辆中圆权重 = 轴距中心
车辆后圆权重 = 0.2 * 轴距

这种布局模拟了真实车辆的动力学特性：

• 前圆侧重感知区域，覆盖保险杠到前轴范围
• 中圆覆盖乘员舱核心区域
• 后圆覆盖后备箱到后轴区域

在MATLAB实现中，我们特别加入了动态调整机制：

matlab复制function circles = adjust_circle_weights(vehicle)
    % 根据轴距动态调整圆间距
    if vehicle.L > 3.5  % 大型车辆
        front_weight = 0.35;
        rear_weight = 0.25;
    else  % 小型车辆
        front_weight = 0.3;
        rear_weight = 0.2;
    end
    circles = [
        vehicle.s - front_weight*vehicle.L, vehicle.d;
        vehicle.s, vehicle.d;
        vehicle.s + rear_weight*vehicle.L, vehicle.d];
end

2.2 碰撞检测优化技巧

实际部署中发现几个关键点：

1. 障碍物半径补偿：建议在检测时给障碍物半径增加10%安全余量
2. 预测帧补偿：对于移动障碍物，需考虑算法延迟时间内的位移
3. 多级检测策略：
   • 先快速检查s坐标范围过滤远端障碍物
   • 再进行精确的三圆检测

以下是我们总结的调参经验表：

3. 改进型人工势场构建

3.1 道路势场设计

传统线性势场在弯道处会导致两个问题：

1. 内侧势场梯度不足，车辆易偏离参考线
2. 外侧势场突变，引发轨迹震荡

我们的解决方案是引入曲率自适应系数：

matlab复制function potential = road_potential(current_d, ref_d, curvature)
    base_gain = 1.0;
    curvature_gain = 0.5 * abs(curvature);
    k_road = base_gain + curvature_gain;
    
    % 非线性势场函数
    offset = current_d - ref_d;
    potential = k_road * offset^2 / (1 + abs(offset));
end

这个设计使得：

• 直道时保持温和的势场梯度
• 弯道时自动增强控制力度
• 通过分母项避免势场无限增大

3.2 障碍物势场优化

高斯斥力场虽然经典，但在密集障碍物场景会出现"局部极小值"问题。我们做了三点改进：

1. 方向性势场：考虑车辆前进方向，减弱后方障碍物影响

matlab复制direction_factor = max(0, (obs.s - current_s)/norm([obs.s-current_s, obs.d-current_d]));
obs_potential = obs_potential * direction_factor;

2. 速度自适应σ参数：

matlab复制sigma = min(5.0, 1.2 * (ego_speed^2 / (2*max_deceleration)));

3. 势场高度限幅：

matlab复制obs_potential = min(obs_potential, max_obs_potential);

4. Frenet坐标系轨迹生成

4.1 五次多项式采样策略

横向轨迹采用五次多项式有深层次原因：

1. 满足起点/终点的位置、速度、加速度约束
2. 保证jerk连续，提高乘坐舒适性
3. 计算效率优于更高阶多项式

采样点分布策略值得特别关注：

matlab复制% 自适应横向采样
if abs(curvature) < 0.01  % 直道
    lat_samples = linspace(-1.0, 1.0, 11); % 宽范围稀疏采样
else  % 弯道
    lat_samples = linspace(-0.5, 0.5, 15); % 窄范围密集采样
end

4.2 最优轨迹评价指标

除了势场值，我们还考虑三个关键因素：

1. 曲率连续性：惩罚曲率变化率大的轨迹

matlab复制cost = cost + 0.1 * sum(diff(traj.kappa).^2);

2. 与参考速度的匹配度：

matlab复制speed_cost = abs(mean(traj.s_dot) - ref_speed) / ref_speed;

3. 方向盘转角变化率：

matlab复制steer_cost = sum(diff(atan(traj.kappa * wheelbase)).^2);

5. 实战调试经验

5.1 典型问题排查指南

5.2 关键参数调试心得

1. 时间步长T的选择：

   • 干燥铺装路面：3-5秒
   • 湿滑路面：5-7秒
   • 计算公式：T = (v/3.6) / (0.8μg) ，其中μ为摩擦系数

2. 势场权重平衡：

   • 初始建议值：k_road=1.0, obs_weight=2.0
   • 调试方法：

     matlab复制while abs(avg_d_offset) > 0.3
         k_road = k_road * 1.1;
         run_simulation();
     end
     

3. 实时性优化技巧：

   • 预计算静态障碍物势场
   • 对动态障碍物采用稀疏采样
   • 使用KD-tree加速最近邻搜索

6. 算法扩展与改进方向

当前系统在以下场景仍需优化：

1. 急弯接急直道的过渡段
2. 密集静态障碍物场景
3. 低附着系数路面

我们正在试验的改进方案包括：

• 引入学习型势场权重调整
• 融合MPC的滚动优化
• 基于语义分割的势场建模

在最近一次封闭场地测试中，改进后的算法成功处理了半径仅15米的发卡弯，同时避让了5个随机放置的锥桶。一个有趣的发现是：当设置20%的势场随机扰动时，系统反而表现出更强的鲁棒性——这或许揭示了生物神经系统中的类似机制。