葫芦岛市网站建设_网站建设公司_服务器维护_seo优化

从零学自动驾驶lattice规划(上)--参考线，frenet标架，多项式拟合 算法详细文档说明。 2种代码实现:matlab和c++

刚摸自动驾驶那会儿总被轨迹规划绕得头疼，直到遇见Lattice算法——这货居然能把复杂的路径拆成纵向横向两个维度来思考。今天咱们先搞明白前置知识：参考线怎么生成、Frenet坐标系究竟香在哪、多项式凭什么能拟合轨迹。手撸Matlab和C++两版代码，带你在坐标系转换的坑里爬出来。

一、参考线：车道线的灵魂骨架

没有参考线的规划就像没画线的马路，车子根本不知道往哪靠。实际项目中参考线通常由高精地图的车道中心线点序列生成，但咱们自己玩可以用三次样条插值：

Matlab暴力美学版：

% 随便扔几个离散点 raw_points = [0,0; 2,1; 5,2; 7,0]; x = raw_points(:,1); y = raw_points(:,2); % 三次样条直接出平滑曲线 t = 1:0.1:length(x); ref_line = spline(x, y, t);

spline函数自动生成C2连续（加速度连续）的曲线，这对规划模块的舒适性至关重要。

C++实战版：

#include <tk_spline.h> // 第三方库 vector<Point2d> raw_points = {{0,0}, {2,1}, {5,2}, {7,0}}; tk::spline s; s.set_points(raw_points.x(), raw_points.y()); // 采样时直接s(x_coord)获得y值

注意这里用到了开源的样条库，比手写矩阵求解省事得多。生成的参考线在x=3.5处的曲率变化率必须小于0.1，否则乘客会晕车——这是车企的硬指标。

二、Frenet坐标系：老司机的分割视角

笛卡尔坐标系下同时处理横向偏移和纵向距离简直要命，Frenet把这两个维度拆开处理：

!Frenet坐标系示意图

坐标系转换核心代码：

function [s, d] = cart2frenet(x, y, ref_line) % 找最近参考点 [~, idx] = min(sum((ref_line - [x,y]).^2, 2)); s = cum_dist(ref_line(1:idx,:)); % 累计距离 vec_t = ref_line.tangent_at(idx); % 参考线切向 d = cross([vec_t,0], [x-ref_line.x(idx), y-ref_line.y(idx),0]); d = d(3); % 取z分量 end

这里有个魔鬼细节：计算最近点时直接用欧式距离会翻车，应该用投影到参考线的近似距离。C++版用Eigen库实现更高效：

struct FrenetConverter { const ReferenceLine& ref_line; FrenetPoint convert(const CartesianPoint& p) { auto proj = ref_line.FindProjection(p); return {proj.s, proj.l}; // l即横向偏移d } };

实际工程中要考虑参考线的方向，当车辆逆行驶方向时s坐标会减小，这时候需要做方向判断。

三、五次多项式：让轨迹丝滑的秘诀

横向规划常用五次多项式，因为它能满足位置、速度、加速度三阶约束：

d(s) = a0 + a1s + a2s² + a3s³ + a4s⁴ + a5*s⁵

Matlab拟合演示：

% 起点终点约束 s0 = 0; d0 = 2; d0_dot = 0; d0_ddot = 0; s1 = 10; d1 = 0; d1_dot = 0; d1_ddot = 0; A = [s0^0 s0^1 s0^2 s0^3 s0^4 s0^5; 0 1 2*s0 3*s0^2 4*s0^3 5*s0^4; 0 0 2 6*s0 12*s0^2 20*s0^3; ... ]; % 构造6x6矩阵 b = [d0; d0_dot; d0_ddot; d1; d1_dot; d1_ddot]; coeffs = A\b; % 解线性方程组

C++暴力解方程：

Eigen::MatrixXd A(6,6); Eigen::VectorXd b(6); // 填充约束条件... Eigen::VectorXd coeffs = A.colPivHouseholderQr().solve(b);

为什么不用三次多项式？因为车辆运动学模型中加速度变化率（jerk）影响舒适性，五次多项式可保证jerk连续。

下篇预告：如何把上面这些零件组装成完整的Lattice规划器，并解决轨迹冲突检测的玄学问题。代码仓库已准备好Matlab和CMake两套环境配置指南，需要的老铁评论区打个卡。