自动驾驶轨迹规划：基于代价函数的MATLAB实现

1. 无人车轨迹规划的核心挑战

在自动驾驶领域，轨迹规划是决定车辆如何从A点安全、高效到达B点的关键技术。与简单的路径规划不同，轨迹规划需要考虑时间维度，即车辆在每个时刻的位置、速度和加速度状态。这就像人类驾驶员不仅要选择走哪条路，还要决定何时加速、何时刹车、如何平滑地变道。

传统轨迹规划方法面临三大核心挑战：

• 动态障碍物避让：需要实时应对突然出现的行人、车辆等移动物体
• 运动约束处理：必须符合车辆动力学限制（如最大加速度、转向角速度）
• 多目标优化：同时满足安全性、舒适性、效率等多个相互冲突的目标

2. 基于代价函数的解决方案框架

代价函数法通过数学建模将复杂规划问题转化为优化问题。其核心思想是：为每条候选轨迹计算一个"代价分数"，选择总分最低的轨迹作为最优解。这就像考试评分，从多个答案中选出综合表现最好的那个。

典型代价函数包含以下组成部分：

matlab复制total_cost = safety_cost * w1 + comfort_cost * w2 + 
            efficiency_cost * w3 + feasibility_cost * w4

其中w1-w4是权重系数，反映不同指标的重视程度。

2.1 安全代价计算

安全代价主要评估与障碍物的碰撞风险。常用方法包括：

• 欧氏距离场：计算轨迹点与最近障碍物的距离
• 侵入惩罚：对进入障碍物膨胀区的轨迹点施加指数增长惩罚
• 时间冲突检测：预测动态障碍物运动轨迹，检查时空冲突

Matlab实现示例：

matlab复制function safety_cost = calcSafetyCost(traj, obstacles)
    min_dist = inf;
    for i = 1:length(traj.points)
        for j = 1:size(obstacles,1)
            dist = norm(traj.points(i,:) - obstacles(j,:));
            if dist < min_dist
                min_dist = dist;
            end
        end
    end
    safety_cost = exp(-min_dist/2); % 距离越近代价越高
end

2.2 舒适度代价建模

舒适度主要考量加速度变化率（jerk）和向心加速度：

matlab复制function comfort_cost = calcComfortCost(traj)
    jerk = diff(traj.acceleration);
    centripetal_acc = traj.velocity.^2 .* abs(traj.curvature);
    comfort_cost = sum(jerk.^2) + 0.5*sum(centripetal_acc.^2);
end

3. 最优轨迹生成实践

3.1 轨迹采样策略

常用采样方法包括：

1. Frenet坐标系采样：在道路参考线横向/纵向离散采样
2. 状态格栅采样：对位置、速度、加速度等状态量离散化
3. 随机采样：在可行状态空间内随机生成候选轨迹

Matlab实现Frenet采样示例：

matlab复制function trajectories = sampleInFrenet(ref_path, num_samples)
    trajectories = [];
    for s = linspace(0, ref_path.length, 5) % 纵向采样
        for d = linspace(-2, 2, 5) % 横向采样
            traj = generateQuinticPolynomial(ref_path, s, d);
            trajectories = [trajectories; traj];
        end
    end
end

3.2 轨迹评估与选择

完整的评估流程包括：

1. 可行性过滤：剔除不符合车辆动力学约束的轨迹
2. 代价计算：对剩余轨迹进行多维度评分
3. 最优选择：选取综合代价最低的轨迹

Matlab实现：

matlab复制function optimal_traj = selectOptimalTrajectory(candidates, obstacles)
    min_cost = inf;
    optimal_traj = [];
    for traj = candidates
        if checkFeasibility(traj) == false
            continue;
        end
        cost = 0.4*calcSafetyCost(traj,obstacles) + ...
               0.3*calcComfortCost(traj) + ...
               0.3*calcEfficiencyCost(traj);
        if cost < min_cost
            min_cost = cost;
            optimal_traj = traj;
        end
    end
end

4. 实际应用中的关键技巧

4.1 代价权重调参经验

通过大量实测总结的权重配置原则：

• 城市道路：安全(0.5) > 舒适(0.3) > 效率(0.2)
• 高速公路：效率(0.4) ≈ 安全(0.4) > 舒适(0.2)
• 泊车场景：安全(0.6) > 舒适(0.3) > 效率(0.1)

4.2 实时性优化方案

为保证规划器能在100ms内完成计算：

1. 并行计算：使用parfor并行评估多条轨迹
2. 分层规划：首先生成粗糙轨迹，再局部优化
3. 轨迹缓存：复用上一周期的高分轨迹作为基准

matlab复制% 并行计算示例
parfor i = 1:num_trajectories
    costs(i) = evaluateTrajectory(trajectories(i));
end

4.3 典型问题排查指南

常见问题及解决方法：

1. 轨迹震荡：

   • 检查jerk代价权重是否过小
   • 增加轨迹采样密度
   • 添加历史轨迹平滑项

2. 避障失败：

   • 确认障碍物膨胀半径足够
   • 检查距离场计算是否正确
   • 验证传感器数据时间同步

3. 计算超时：

   • 减少采样点数量
   • 采用更高效的距离计算方法
   • 预生成轨迹库离线计算

5. Matlab实现完整案例

以高速公路换道场景为例：

matlab复制%% 初始化
ref_path = generateReferencePath(); % 生成参考路径
obstacles = detectObstacles(); % 获取障碍物信息
vehicle_state = getVehicleState(); % 获取当前状态

%% 轨迹生成
candidate_trajs = sampleTrajectories(ref_path, vehicle_state);

%% 轨迹评估
for i = 1:length(candidate_trajs)
    traj = candidate_trajs(i);
    costs(i) = 0.4*calcSafetyCost(traj,obstacles) + ...
               0.3*calcComfortCost(traj) + ...
               0.3*calcEfficiencyCost(traj);
end

%% 最优选择
[~, idx] = min(costs);
optimal_traj = candidate_trajs(idx);

%% 可视化
plotTrajectory(ref_path, obstacles, optimal_traj);

这个实现展示了完整的规划流程，实际应用中还需要考虑：

• 传感器数据的不确定性处理
• 多模态交通参与者的交互预测
• 紧急情况的应急轨迹生成

轨迹规划算法的性能很大程度上取决于代价函数的设计和参数调优。在实际项目中，我们通常会收集大量驾驶数据，通过反向强化学习等方法自动优化代价函数的权重参数，这比手动调参能获得更好的人机共驾体验。