路径规划算法 Matlab 代码 基于 Frenet坐标系下的局部路径规划 matlab脚本编写，在学习frenet坐标系时具有一定的参考价值。

最近在折腾自动驾驶的局部路径规划，发现Frenet坐标系真是个好东西。特别是在处理车辆跟随参考线行驶的场景时，比笛卡尔坐标系方便太多了。今天咱们就边撸代码边聊聊怎么用Matlab实现这个算法。

先说说为什么用Frenet坐标系。想象你在高速上开车，导航路线就是参考线。这时候判断车辆位置用纵向距离（开了多远）和横向偏移（离中线多近）比用经纬度直观多了。这种坐标系下做轨迹生成和碰撞检测都特别方便。

上硬菜！先定义参考线。这里我们用三次样条曲线模拟一段弯道：

% 生成参考线
ref_points = [0,0; 20,3; 40,-2; 60,0];
ref_spline = cscvn(ref_points');
ref_curve = fnplt(ref_spline);

这里用了Matlab自带的样条曲线工具包，关键是要保证参考线的曲率连续，不然规划出来的路径会抽风。

接下来是坐标系转换的核心函数。把笛卡尔坐标转成Frenet坐标时，需要找到参考线上最近的点：

function [s, d] = cart2frenet(x, y, ref_curve)
    % 找最近点
    distances = sum((ref_curve - [x;y]).^2);
    [~, idx] = min(distances);
    
    % 计算横向偏移
    tangent = ref_curve(:,idx+1) - ref_curve(:,idx);
    normal = [-tangent(2); tangent(1)];
    normal = normal/norm(normal);
    d = dot([x;y]-ref_curve(:,idx), normal);
    
    % 纵向距离近似处理
    s = idx * step_size; % 实际项目要用累积距离
end

这个简化版忽略了曲率影响，真实项目得用牛顿迭代找投影点。但作为demo够用了，重点看横向偏移d的计算——通过参考线切向量的法向量来做投影，这个几何操作很巧妙。

生成候选轨迹时采用分层采样策略。横向做五次多项式插值，纵向用恒定速度假设：

% 横向轨迹采样
lat_samples = linspace(-1.5, 1.5, 5); % 横向偏移量
for d = lat_samples
    % 五次多项式保证加速度连续
    coeff = poly5_coeff(0, current_d, 0, 0, d, 0, 0, T);
    lat_traj = polyval(coeff, t);
end

% 纵向速度采样
vel_samples = [8, 10, 12]; % m/s
for v = vel_samples
    s_traj = current_s + v * t;
end

这里poly5_coeff函数需要解方程组得到五次多项式系数。横向用五次多项式是为了让横向加速度连续，坐起来不会晕车。

路径规划算法 Matlab 代码 基于 Frenet坐标系下的局部路径规划 matlab脚本编写，在学习frenet坐标系时具有一定的参考价值。

代价函数设计是算法的灵魂。这里简单实现三个考量因素：

function cost = calc_cost(traj, ref_curve)
    % 舒适性代价：横向加速度
    max_lat_acc = max(abs(traj.ay));
    
    % 效率代价：与期望速度偏差
    speed_cost = abs(traj.v - desired_speed);
    
    % 参考线贴合度
    ref_deviation = mean(abs(traj.d));
    
    cost = 0.4*max_lat_acc + 0.4*speed_cost + 0.2*ref_deviation;
end

权重的设置需要实际调试，比如在高速场景下可以加大速度权重，城区道路侧重舒适性。建议用网格搜索找最佳参数组合。

最后看看规划效果（配图说明更直观）：

!生成的候选轨迹与最优路径

红色是参考线，蓝色候选轨迹，绿色为最终选择。可以看到算法在弯道自动选择了相对保守的横向偏移。

几个调试小技巧：

1. 横向采样范围要根据车速动态调整，速度越快采样范围越小
2. 时间跨度T取3-5秒为宜，太短容易短视，太长计算量大
3. 实时性优化可以从减少采样点入手，比如先粗筛再细选

完整代码里还缺碰撞检测模块，实际应用中需要叠加高精度地图和障碍物预测。不过核心框架已经搭起来了，拿去改成Python版也就是换个语法的事儿。

下次可以试试加入动态障碍物避让，或者结合MPC做滚动优化。路径规划这玩意儿就像搭乐高，基础模块拼好了，往上加功能才有意思。