无人车轨迹规划：基于Matlab的代价函数设计与优化

1. 无人车轨迹规划的核心挑战与解决思路

在无人驾驶技术快速发展的今天，轨迹规划作为关键子系统直接影响着车辆的行驶安全性和乘坐舒适度。我从事自动驾驶算法开发多年，发现许多工程师在实现轨迹规划时容易陷入两个极端：要么过度简化问题导致规划结果不实用，要么过度复杂化导致实时性无法满足。基于代价函数的规划方法恰好在这两者间找到了平衡点。

轨迹规划本质上是在高维状态空间中寻找从起点到目标点的最优路径，同时满足车辆动力学约束、避障要求和交通规则。与传统机器人路径规划不同，无人车需要考虑更多现实因素：

• 车辆的非完整约束（Non-holonomic constraints）
• 乘客舒适度相关的加加速度（Jerk）限制
• 动态环境下的实时重规划需求
• 多目标优化间的权衡（如路径长度vs.平滑度）

Matlab凭借其强大的矩阵运算能力和丰富的优化工具箱，成为验证轨迹规划算法的理想平台。特别是其Robotics System Toolbox和Optimization Toolbox，为快速实现和测试各种规划算法提供了坚实基础。

提示：在实际工程中，Matlab通常用于算法原型验证，最终部署可能转为C++实现。但算法核心思想保持一致，只是实现方式不同。

2. 代价函数的设计哲学与实现细节

2.1 代价函数的组成要素

一个完整的轨迹规划代价函数通常包含以下核心组件（以权重系数ω_i调节重要性）：

code复制总代价 = ω₁·路径长度代价 + ω₂·平滑度代价 + ω₃·障碍物距离代价 + ω₄·交通规则代价 + ω₅·舒适度代价

具体到Matlab实现中，每个代价分量都有其独特的计算方式：

路径长度代价：

matlab复制function cost = pathLengthCost(trajectory)
    % 计算轨迹点间的欧氏距离之和
    diffs = diff(trajectory(:,1:2));  % 提取x,y坐标
    cost = sum(sqrt(sum(diffs.^2, 2)));
end

平滑度代价（基于曲率计算）：

matlab复制function cost = smoothnessCost(trajectory)
    dx = gradient(trajectory(:,1));
    dy = gradient(trajectory(:,2));
    ddx = gradient(dx);
    ddy = gradient(dy);
    curvature = (dx.*ddy - dy.*ddx) ./ ((dx.^2 + dy.^2).^(3/2) + eps);
    cost = sum(curvature.^2);
end

2.2 权重系数的动态调整技巧

通过多个项目的实践，我总结出权重调整的几个经验法则：

1. 初期调试时先设置ω₂=0，确保基础路径可达性
2. 逐步增加平滑度权重直到出现明显转弯缓冲
3. 障碍物权重ω₃应采用距离自适应策略：

   matlab复制function w_obs = dynamicObstacleWeight(dist_to_obs)
       safe_dist = 3.0; % 安全距离阈值(m)
       if dist_to_obs > safe_dist
           w_obs = 0.1;
       else
           w_obs = 5.0 * (1 - dist_to_obs/safe_dist);
       end
   end
   

注意：权重绝对值本身没有意义，重要的是各分量间的相对比例。建议保持∑ω_i=1便于参数归一化比较。

3. Matlab实现中的关键技术点

3.1 轨迹表示与采样策略

在Matlab中，我们通常采用两种轨迹表示方式：

1. 参数化曲线（适合结构化道路）：

   • 三次样条曲线：spline(x,y,xx)
   • 五次多项式：polyfit+polyval组合

   matlab复制% 五次多项式系数求解示例
   t = [0 1 2 3]'; % 时间点
   pos = [0 2 3 1]'; % 位置约束
   A = [t.^5 t.^4 t.^3 t.^2 t ones(size(t))];
   coeffs = A\pos; % 最小二乘解
   

2. 离散点列（适合复杂环境）：

   • 采用等时间间隔采样
   • 配合动态规划生成候选轨迹簇

3.2 优化求解器的选择与配置

Matlab提供多种优化求解器，针对轨迹规划推荐使用：

典型优化调用示例：

matlab复制options = optimoptions('fmincon','Display','iter',...
                      'Algorithm','sqp','MaxIterations',100);
[x_opt, fval] = fmincon(@(x)totalCost(x,obstacles),...
                       x0,[],[],[],[],lb,ub,...
                       @(x)nonlcon(x,vehicle_params),options);

3.3 可视化调试技巧

高效的调试可视化能极大提升开发效率：

matlab复制figure('Position',[100 100 1200 400])
subplot(1,3,1)
plot(trajectory(:,1), trajectory(:,2), 'b-'); 
hold on; plot(obstacles(:,1), obstacles(:,2), 'ro');
title('路径视图');

subplot(1,3,2)
plot(t, curvature); 
title('曲率变化');

subplot(1,3,3)
bar([length_cost, smooth_cost, obs_cost]); 
title('代价分量占比');

4. 工程实践中的典型问题与解决方案

4.1 局部最优陷阱现象

问题表现：规划结果陷入不合理局部最优，如急转弯或过于靠近障碍物。

解决方案：

1. 多起点初始化策略：

   matlab复制initial_guesses = linspace(-2, 2, 5); % 横向偏移量
   costs = zeros(size(initial_guesses));
   for i = 1:length(initial_guesses)
       [~, costs(i)] = fmincon(...);
   end
   [min_cost, idx] = min(costs);
   

2. 模拟退火思想：允许暂时接受较差解

   matlab复制options = optimoptions(@fmincon,'AcceptanceFcn',...
       @(optimValues,newx,newfval)exp(-(newfval-optimValues.fval)/T));
   

4.2 实时性保障措施

性能瓶颈：复杂环境下优化耗时超过100ms。

优化手段：

1. 轨迹点稀疏化：在曲率小的区域减少采样点

   matlab复制keep_idx = [true; abs(diff(curvature)) > 0.1];
   sparse_traj = trajectory(keep_idx,:);
   

2. 热启动技术：重用上一周期优化结果作为初始猜测
3. 并行计算加速：

   matlab复制parfor i = 1:num_trajectories
       [traj{i}, cost(i)] = optimizeTrajectory(init_guess{i});
   end
   

4.3 动态障碍物处理

对于移动障碍物，需要引入时间维度约束：

matlab复制function [c, ceq] = dynamicObstacleCon(x, obs_traj)
    % x: [x1,y1,t1; x2,y2,t2; ...]
    ceq = [];
    c = zeros(size(obs_traj,1),1);
    for i = 1:size(obs_traj,1)
        [min_dist, idx] = min(vecnorm(x(:,1:2) - obs_traj(i,1:2), 2, 2));
        c(i) = 1.5 - min_dist; % 安全距离1.5m
    end
end

5. 完整实现案例与参数调优指南

5.1 典型城市道路场景实现

matlab复制% 场景参数
road_width = 3.5; % 车道宽度(m)
obs_pos = [10 1.2; 25 -0.8]; % 障碍物位置

% 车辆参数
vehicle.length = 4.8; % 车长(m)
vehicle.width = 1.9; % 车宽(m)

% 代价函数权重
weights.length = 0.3;
weights.smoothness = 0.4;
weights.obstacle = 0.3;

% 生成参考线
ref_line = [linspace(0,100,100)' zeros(100,1)];

% 调用规划器
[opt_traj, cost] = trajectoryPlanner(ref_line, obs_pos, weights, vehicle);

% 可视化
plotScenario(ref_line, obs_pos, opt_traj);

5.2 参数调优经验表

在项目实际开发中，我通常会先在一个简单场景（如单障碍物直线道路）中确定基础参数，再逐步过渡到复杂场景微调。记住每次只调整一个参数，并记录变化效果。