自动驾驶轨迹规划：Lattice算法与Frenet坐标系实践

1. 自动驾驶轨迹规划基础概念

在自动驾驶系统中，轨迹规划是决定车辆如何从当前位置安全、舒适地行驶到目标位置的核心模块。Lattice规划算法因其结构清晰、计算高效而成为业界主流方案之一。理解Lattice规划需要掌握三个关键基础：参考线生成、Frenet坐标系转换和多项式轨迹拟合。

轨迹规划的本质是在复杂环境中为车辆寻找一条既符合交通规则又满足乘坐舒适性的可行路径。传统方法直接在笛卡尔坐标系下进行规划，需要同时考虑横向和纵向运动约束，计算复杂度高且难以满足实时性要求。Lattice算法的创新之处在于将轨迹规划问题解耦为独立的纵向和横向运动规划，大大简化了问题复杂度。

2. 参考线生成技术详解

2.1 参考线的作用与要求

参考线是轨迹规划的基础框架，相当于车辆行驶的"理想路径"。在实际自动驾驶系统中，参考线通常由高精地图提供的车道中心线生成。一条合格的参考线需要满足以下条件：

1. 几何连续性：至少C2连续（位置、速度、加速度连续）
2. 曲率连续性：曲率变化平缓，最大值通常限制在0.1-0.2m⁻¹
3. 采样密度：相邻点间距一般控制在0.1-0.5米

提示：参考线的质量直接影响后续规划效果。曲率不连续的参考线会导致规划出的轨迹出现急转弯，影响乘坐舒适性。

2.2 三次样条插值实现

三次样条插值是生成参考线的常用方法，它能保证生成的曲线二阶导数连续。Matlab中的spline函数可以直接实现这一功能：

matlab复制% 输入离散点序列
raw_points = [0,0; 2,1; 5,2; 7,0];
x = raw_points(:,1);
y = raw_points(:,2);

% 生成插值参数
t = linspace(0, 1, length(x)); % 归一化参数
pp = spline(t, [x'; y']); % 生成分段多项式

% 采样参考线
sample_t = linspace(0, 1, 100);
ref_points = ppval(pp, sample_t)';

在C++中，我们可以使用开源库如tk::spline来实现类似功能：

cpp复制#include <tk_spline.h>
#include <vector>

struct Point2D {
    double x, y;
};

std::vector<Point2D> generateRefLine(const std::vector<Point2D>& raw_points) {
    tk::spline sx, sy;
    std::vector<double> t(raw_points.size());
    
    // 计算累计距离作为参数
    t[0] = 0;
    for(size_t i=1; i<raw_points.size(); ++i) {
        double dx = raw_points[i].x - raw_points[i-1].x;
        double dy = raw_points[i].y - raw_points[i-1].y;
        t[i] = t[i-1] + std::sqrt(dx*dx + dy*dy);
    }
    
    // 归一化参数
    for(auto& val : t) val /= t.back();
    
    // 设置样条
    std::vector<double> px, py;
    for(const auto& p : raw_points) {
        px.push_back(p.x);
        py.push_back(p.y);
    }
    sx.set_points(t, px);
    sy.set_points(t, py);
    
    // 采样参考线
    std::vector<Point2D> ref_line;
    for(double ti = 0; ti <= 1.0; ti += 0.01) {
        ref_line.push_back({sx(ti), sy(ti)});
    }
    return ref_line;
}

2.3 参考线优化技巧

实际工程中，直接使用样条插值生成的参考线可能不满足所有要求，需要进行后处理：

1. 曲率平滑：对生成的参考线进行低通滤波，消除高频曲率变化
2. 等弧长重采样：确保采样点间距均匀，便于后续处理
3. 切线/法线预计算：提前计算每个参考点的切向和法向向量

cpp复制// 曲率计算示例
double computeCurvature(const Point2D& p1, const Point2D& p2, const Point2D& p3) {
    double dx1 = p2.x - p1.x, dy1 = p2.y - p1.y;
    double dx2 = p3.x - p2.x, dy2 = p3.y - p2.y;
    
    double cross = dx1*dy2 - dy1*dx2;
    double dot = dx1*dx2 + dy1*dy2;
    double ds1 = std::sqrt(dx1*dx1 + dy1*dy1);
    double ds2 = std::sqrt(dx2*dx2 + dy2*dy2);
    
    return 2.0 * cross / (ds1 * ds2 * (ds1 + ds2));
}

3. Frenet坐标系原理与实现

3.1 Frenet坐标系优势分析

Frenet坐标系将复杂的二维轨迹规划问题解耦为独立的纵向(s)和横向(d)运动规划，具有以下优势：

1. 解耦性：纵向和横向规划可以独立进行
2. 直观性：横向偏差直接反映车辆偏离参考线的程度
3. 计算简化：许多约束条件在Frenet系下表达更简单

3.2 笛卡尔到Frenet坐标转换

坐标转换的核心是找到给定点在参考线上的投影点。以下是详细的转换步骤：

1. 寻找参考线上最近点（投影点）
2. 计算沿参考线的累计距离s
3. 计算横向偏移d（有符号距离）

matlab复制function [s, d] = cart2frenet(x, y, ref_line)
    % 第一步：寻找最近点
    distances = sum((ref_line - [x, y]).^2, 2);
    [~, idx] = min(distances);
    
    % 第二步：计算累计距离s
    s = 0;
    if idx > 1
        diff = diff(ref_line(1:idx,:), 1, 1);
        s = sum(sqrt(sum(diff.^2, 2)));
    end
    
    % 第三步：计算横向偏移d
    tangent = ref_line(min(idx+1,size(ref_line,1)),:) - ref_line(max(idx-1,1),:);
    tangent = tangent / norm(tangent);
    normal = [-tangent(2), tangent(1)];
    
    vec = [x, y] - ref_line(idx,:);
    d = dot(vec, normal);
end

C++实现版本效率更高，适合实时系统：

cpp复制struct FrenetPoint {
    double s; // 纵向距离
    double d; // 横向偏移
};

FrenetPoint cartesianToFrenet(const Point2D& cart, 
                             const std::vector<Point2D>& ref_line) {
    // 寻找最近点
    size_t closest_idx = 0;
    double min_dist = std::numeric_limits<double>::max();
    
    for(size_t i = 0; i < ref_line.size(); ++i) {
        double dx = cart.x - ref_line[i].x;
        double dy = cart.y - ref_line[i].y;
        double dist = dx*dx + dy*dy;
        
        if(dist < min_dist) {
            min_dist = dist;
            closest_idx = i;
        }
    }
    
    // 计算累计距离s
    double s = 0.0;
    for(size_t i = 1; i <= closest_idx; ++i) {
        double dx = ref_line[i].x - ref_line[i-1].x;
        double dy = ref_line[i].y - ref_line[i-1].y;
        s += std::sqrt(dx*dx + dy*dy);
    }
    
    // 计算横向偏移d
    size_t prev_idx = closest_idx > 0 ? closest_idx - 1 : 0;
    size_t next_idx = closest_idx < ref_line.size()-1 ? closest_idx + 1 : closest_idx;
    
    Point2D tangent = {
        ref_line[next_idx].x - ref_line[prev_idx].x,
        ref_line[next_idx].y - ref_line[prev_idx].y
    };
    
    double tangent_norm = std::sqrt(tangent.x*tangent.x + tangent.y*tangent.y);
    Point2D unit_tangent = {tangent.x/tangent_norm, tangent.y/tangent_norm};
    Point2D unit_normal = {-unit_tangent.y, unit_tangent.x};
    
    Point2D vec = {cart.x - ref_line[closest_idx].x, 
                  cart.y - ref_line[closest_idx].y};
    
    double d = vec.x * unit_normal.x + vec.y * unit_normal.y;
    
    return {s, d};
}

3.3 实现注意事项

1. 投影点搜索优化：直接遍历所有参考点效率低，可采用KD-tree或滑动窗口优化
2. 方向处理：需要考虑参考线方向与车辆行驶方向的关系
3. 边界情况处理：起点/终点附近的特殊处理

cpp复制// 使用KD-tree优化最近点搜索
#include <nanoflann.hpp>

struct PointCloud {
    std::vector<Point2D> pts;
    
    inline size_t kdtree_get_point_count() const { return pts.size(); }
    
    inline double kdtree_distance(const double* p1, const size_t idx_p2, size_t) const {
        const double d0 = p1[0] - pts[idx_p2].x;
        const double d1 = p1[1] - pts[idx_p2].y;
        return d0*d0 + d1*d1;
    }
    
    inline double kdtree_get_pt(const size_t idx, int dim) const {
        if(dim == 0) return pts[idx].x;
        else return pts[idx].y;
    }
};

size_t findClosestPoint(const Point2D& p, const std::vector<Point2D>& ref_line) {
    PointCloud cloud;
    cloud.pts = ref_line;
    
    nanoflann::KDTreeSingleIndexAdaptor<
        nanoflann::L2_Simple_Adaptor<double, PointCloud>,
        PointCloud, 2> index(2, cloud);
    
    index.buildIndex();
    
    double query_pt[2] = {p.x, p.y};
    size_t ret_index;
    double out_dist_sqr;
    
    nanoflann::KNNResultSet<double> resultSet(1);
    resultSet.init(&ret_index, &out_dist_sqr);
    index.findNeighbors(resultSet, query_pt);
    
    return ret_index;
}

4. 多项式轨迹拟合方法

4.1 五次多项式拟合原理

在Frenet坐标系中，横向规划通常使用五次多项式表示：

d(s) = a₀ + a₁s + a₂s² + a₃s³ + a₄s⁴ + a₅s⁵

选择五次多项式的原因在于它可以满足边界条件的位置、速度和加速度约束：

1. 起点条件：d(0), d'(0), d''(0)
2. 终点条件：d(s₁), d'(s₁), d''(s₁)

这六个条件正好可以唯一确定一个五次多项式的六个系数。

4.2 多项式系数求解

建立线性方程组求解多项式系数：

matlab复制function coeffs = quintic_poly_fit(s0, d0, d0_dot, d0_ddot, s1, d1, d1_dot, d1_ddot)
    % 构造系数矩阵
    A = [s0^0,    s0^1,     s0^2,      s0^3,       s0^4,        s0^5;
         0,       s0^0,    2*s0^1,   3*s0^2,    4*s0^3,     5*s0^4;
         0,       0,       2*s0^0,   6*s0^1,   12*s0^2,    20*s0^3;
         s1^0,    s1^1,     s1^2,      s1^3,       s1^4,        s1^5;
         0,       s1^0,    2*s1^1,   3*s1^2,    4*s1^3,     5*s1^4;
         0,       0,       2*s1^0,   6*s1^1,   12*s1^2,    20*s1^3];
     
    % 构造右侧向量
    b = [d0; d0_dot; d0_ddot; d1; d1_dot; d1_ddot];
    
    % 解线性方程组
    coeffs = A \ b;
end

C++版本使用Eigen库实现：

cpp复制#include <Eigen/Dense>

Eigen::VectorXd solveQuinticCoeffs(double s0, double d0, double d0_dot, double d0_ddot,
                                  double s1, double d1, double d1_dot, double d1_ddot) {
    Eigen::MatrixXd A(6,6);
    Eigen::VectorXd b(6);
    
    // 填充系数矩阵
    A << 1, s0, s0*s0, s0*s0*s0, s0*s0*s0*s0, s0*s0*s0*s0*s0,
         0, 1, 2*s0, 3*s0*s0, 4*s0*s0*s0, 5*s0*s0*s0*s0,
         0, 0, 2, 6*s0, 12*s0*s0, 20*s0*s0*s0,
         1, s1, s1*s1, s1*s1*s1, s1*s1*s1*s1, s1*s1*s1*s1*s1,
         0, 1, 2*s1, 3*s1*s1, 4*s1*s1*s1, 5*s1*s1*s1*s1,
         0, 0, 2, 6*s1, 12*s1*s1, 20*s1*s1*s1;
    
    // 填充右侧向量
    b << d0, d0_dot, d0_ddot, d1, d1_dot, d1_ddot;
    
    // 解方程
    return A.colPivHouseholderQr().solve(b);
}

4.3 多项式轨迹评估

得到多项式系数后，可以评估任意s位置处的横向位置及其导数：

cpp复制struct QuinticPoly {
    Eigen::VectorXd coeffs; // a0-a5
    
    double evaluate(double s) const {
        return coeffs[0] + coeffs[1]*s + coeffs[2]*s*s + 
               coeffs[3]*s*s*s + coeffs[4]*s*s*s*s + coeffs[5]*s*s*s*s*s;
    }
    
    double derivative(double s) const {
        return coeffs[1] + 2*coeffs[2]*s + 3*coeffs[3]*s*s + 
               4*coeffs[4]*s*s*s + 5*coeffs[5]*s*s*s*s;
    }
    
    double secondDerivative(double s) const {
        return 2*coeffs[2] + 6*coeffs[3]*s + 12*coeffs[4]*s*s + 20*coeffs[5]*s*s*s;
    }
};

5. 实际工程中的问题与解决方案

5.1 参考线生成常见问题

1. 曲率不连续：表现为车辆在特定点出现突然转向

   • 解决方案：增加样条插值控制点，或进行后处理平滑

2. 采样点不均匀：导致后续规划计算不稳定

   • 解决方案：等弧长重采样，确保相邻点间距一致

3. 方向突变：参考线出现锐角转折

   • 解决方案：检查原始输入点，必要时添加中间过渡点

5.2 Frenet转换精度问题

1. 投影点搜索不准确：在急弯处可能找到错误的最近点
   • 解决方案：采用牛顿迭代法进行精确投影点搜索

cpp复制size_t findProjectionNewton(const Point2D& p, 
                          const std::vector<Point2D>& ref_line,
                          size_t init_idx = 0) {
    const size_t max_iter = 10;
    const double tolerance = 1e-3;
    
    size_t idx = init_idx;
    if(idx >= ref_line.size()) idx = ref_line.size() / 2;
    
    for(size_t iter = 0; iter < max_iter; ++iter) {
        if(idx == 0 || idx == ref_line.size()-1) break;
        
        Point2D prev = ref_line[idx-1];
        Point2D curr = ref_line[idx];
        Point2D next = ref_line[idx+1];
        
        Point2D tangent = {next.x - prev.x, next.y - prev.y};
        Point2D vec = {p.x - curr.x, p.y - curr.y};
        
        double dot = tangent.x * vec.x + tangent.y * vec.y;
        if(std::abs(dot) < tolerance) break;
        
        idx += (dot > 0) ? 1 : -1;
    }
    
    return idx;
}

2. 横向偏移计算误差：在曲率较大区域误差明显
   • 解决方案：考虑参考线曲率修正横向偏移计算

5.3 多项式拟合的数值稳定性

1. 矩阵病态问题：当s0和s1接近时，系数矩阵接近奇异
   • 解决方案：对s进行归一化处理，s' = (s - s0)/(s1 - s0)

matlab复制function coeffs = normalized_quintic_fit(s0, s1, ...)
    % 归一化处理
    scale = s1 - s0;
    A = [1, 0, 0, 0, 0, 0;
         0, 1/scale, 0, 0, 0, 0;
         0, 0, 2/scale^2, 0, 0, 0;
         1, 1, 1, 1, 1, 1;
         0, 1/scale, 2/scale, 3/scale, 4/scale, 5/scale;
         0, 0, 2/scale^2, 6/scale^2, 12/scale^2, 20/scale^2];
     
    % 右侧向量不变
    b = [d0; d0_dot; d0_ddot; d1; d1_dot; d1_ddot];
    
    % 解方程后需要调整系数
    coeffs_scaled = A \ b;
    coeffs = [coeffs_scaled(1);
              coeffs_scaled(2)/scale;
              coeffs_scaled(3)/scale^2;
              coeffs_scaled(4)/scale^3;
              coeffs_scaled(5)/scale^4;
              coeffs_scaled(6)/scale^5];
end

2. 约束冲突问题：边界条件相互矛盾导致无解
   • 解决方案：放松部分约束或增加多项式阶数

6. 性能优化技巧

6.1 参考线预处理

1. 预计算切线/法线：避免实时计算消耗
2. 建立曲率查找表：提前计算并存储参考线各点曲率
3. 分段缓存：对长参考线进行分段处理

6.2 Frenet转换加速

1. 滑动窗口搜索：利用车辆运动的连续性，缩小搜索范围
2. 并行计算：对多个点同时进行坐标转换
3. 近似投影：在非关键区域使用线性近似

6.3 多项式计算优化

1. 霍纳法则：优化多项式求值过程

   cpp复制double hornerEval(double s, const double* coeffs, int order) {
       double result = coeffs[order];
       for(int i = order-1; i >= 0; --i) {
           result = result * s + coeffs[i];
       }
       return result;
   }
   

2. 查表法：对固定多项式预计算采样点

3. SIMD指令：使用向量指令并行计算多个多项式

7. 不同场景下的参数调整

7.1 城市道路场景

1. 参考线曲率限制：0.1-0.15 m⁻¹
2. 多项式拟合长度：30-50米
3. 横向偏移范围：±1.5米（考虑车道宽度）

7.2 高速公路场景

1. 参考线曲率限制：0.05 m⁻¹以下
2. 多项式拟合长度：100-150米
3. 横向偏移范围：±0.5米（车道保持更严格）

7.3 泊车场景

1. 参考线曲率限制：可放宽至0.3 m⁻¹
2. 多项式拟合长度：5-10米
3. 横向偏移范围：±2.5米（需要大范围移动）

8. 测试与验证方法

8.1 单元测试要点

1. 参考线测试：

   • 连续性检查（位置、切线、曲率）
   • 曲率最大值验证
   • 采样密度检查

2. Frenet转换测试：

   • 已知点往返测试（Cartesian→Frenet→Cartesian）
   • 特殊位置测试（起点、终点、曲率最大点）
   • 方向一致性测试

3. 多项式拟合测试：

   • 边界条件验证
   • 中间点合理性检查
   • 导数连续性验证

8.2 集成测试方法

1. 可视化检查：绘制参考线、Frenet坐标系和拟合轨迹

   matlab复制% 绘制参考线和Frenet坐标系
   figure;
   plot(ref_line(:,1), ref_line(:,2), 'b-'); hold on;
   
   % 绘制Frenet坐标轴
   sample_s = 0:5:max_s;
   for s = sample_s
       [x,y] = frenet2cart(s, 0, ref_line);
       tangent = getTangent(s, ref_line);
       normal = [-tangent(2), tangent(1)];
       
       quiver(x,y,tangent(1),tangent(2),0.5,'r');
       quiver(x,y,normal(1),normal(2),0.5,'g');
   end
   axis equal;
   

2. 动态测试：模拟车辆沿生成轨迹运动的舒适性

3. 实时性测试：统计各模块计算时间，确保满足实时要求

8.3 实际道路测试注意事项

1. 分段验证：先在封闭场地测试，再逐步扩展到简单道路
2. 安全监控：实时监测轨迹曲率和加速度，超过阈值时触发安全措施
3. 数据记录：记录测试过程中的所有中间结果，便于问题分析

9. 进阶话题与扩展方向

9.1 参考线动态更新

在实际驾驶中，参考线可能需要根据环境变化动态调整：

1. 换道情况：平滑过渡到相邻车道参考线
2. 避障场景：局部调整参考线绕过障碍物
3. 路口处理：根据转向意图选择合适的分支

9.2 非结构化道路处理

对于没有明确车道线的场景，参考线生成需要考虑：

1. 可行区域识别：基于感知结果确定可行驶区域
2. 参考线生成：沿可行驶区域中心生成参考路径
3. 边界约束：将道路边界转化为横向偏移约束

9.3 运动约束集成

将车辆运动学约束直接融入规划过程：

1. 最小转弯半径：转化为最大曲率约束
2. 加速度限制：约束轨迹的二阶导数
3. jerk限制：约束轨迹的三阶导数变化

cpp复制bool checkTrajectoryConstraints(const QuinticPoly& poly, double s_start, double s_end) {
    const double max_curvature = 0.2; // 最大曲率
    const double max_lat_acc = 2.0;   // 最大横向加速度 m/s^2
    const double max_jerk = 5.0;      // 最大jerk m/s^3
    
    const int n_samples = 10;
    bool valid = true;
    
    for(int i = 0; i <= n_samples; ++i) {
        double s = s_start + (s_end - s_start) * i / n_samples;
        
        double d = poly.evaluate(s);
        double d_dot = poly.derivative(s);
        double d_ddot = poly.secondDerivative(s);
        
        // 曲率近似计算
        double curvature = std::abs(d_ddot) / std::pow(1 + d_dot*d_dot, 1.5);
        if(curvature > max_curvature) {
            valid = false;
            break;
        }
        
        // 横向加速度检查 (假设纵向速度为v)
        double v = 10.0; // 示例速度
        double lat_acc = v * v * curvature;
        if(lat_acc > max_lat_acc) {
            valid = false;
            break;
        }
    }
    
    return valid;
}

10. 工程实践建议

10.1 代码组织建议

1. 模块化设计：

   • 参考线模块(ReferenceLine)
   • 坐标系转换模块(FrenetConverter)
   • 轨迹生成模块(TrajectoryGenerator)

2. 接口设计原则：

   cpp复制class ReferenceLine {
   public:
       virtual Point2D getPoint(double s) const = 0;
       virtual Point2D getTangent(double s) const = 0;
       virtual double getCurvature(double s) const = 0;
       virtual double length() const = 0;
   };
   
   class FrenetConverter {
   public:
       FrenetPoint toFrenet(const CartesianPoint& p) const;
       CartesianPoint toCartesian(const FrenetPoint& fp) const;
   };
   

10.2 参数配置建议

1. 参数文件化：将所有可调参数放入配置文件

   yaml复制reference_line:
     max_curvature: 0.15
     sample_interval: 0.2
   
   frenet:
     search_window: 10
     projection_tolerance: 0.01
   
   trajectory:
     poly_order: 5
     max_lat_acc: 2.5
   

2. 参数自动化测试：建立参数组合自动测试框架

10.3 调试工具建议

1. 可视化工具：

   • 参考线显示
   • 坐标系可视化
   • 轨迹动画演示

2. 数据回放工具：记录和回放测试场景

3. 性能分析工具：实时监控各模块计算耗时

11. 常见问题解答

Q1: 为什么选择五次多项式而不是三次或更高次？

五次多项式能够在起点和终点同时满足位置、速度和加速度约束，这是保证轨迹舒适性的最低要求。三次多项式只能满足位置和速度约束，无法保证加速度连续性；而更高次多项式虽然可以提供更多自由度，但会增加计算复杂度且容易产生不必要的振荡。

Q2: 如何处理参考线的分支情况（如路口）？

对于有分支的参考线，通常采用以下策略：

1. 提前生成各分支的参考线
2. 根据车辆意图选择当前激活的分支
3. 在分支点附近进行平滑过渡

Q3: Frenet坐标系在急弯处的精度问题如何解决？

在急弯处可以采取以下措施：

1. 增加参考线采样密度
2. 使用更精确的投影算法（如牛顿迭代法）
3. 考虑曲率修正的横向偏移计算
4. 必要时切换到笛卡尔坐标系处理

Q4: 如何保证生成的轨迹动力学可行？

需要从以下几个方面进行验证：

1. 曲率检查：确保不超过车辆最大转向能力
2. 加速度检查：包括纵向和横向加速度
3. jerk检查：保证舒适性
4. 与车辆模型的兼容性检查

Q5: 实时性要求下如何优化计算效率？

典型的优化手段包括：

1. 预计算和缓存（参考线信息、多项式系数等）
2. 简化算法（在非关键区域使用近似计算）
3. 并行计算（如同时计算多条候选轨迹）
4. 热点代码优化（使用SIMD指令等）

12. 总结与个人实践心得

在实际开发自动驾驶轨迹规划模块的过程中，我总结了以下几点关键经验：

1. 参考线质量至关重要：一条好的参考线可以大大简化后续规划问题。在项目初期，我们曾因参考线曲率不连续导致规划轨迹出现急转，通过引入C2连续的样条插值和后处理平滑解决了这一问题。

2. Frenet转换的细节决定成败：最初我们使用简单的最近点搜索，在急弯处经常出现投影点跳动问题。后来实现了基于牛顿迭代的精确投影算法，并结合曲率修正横向偏移计算，显著提高了转换精度。

3. 多项式阶数不是越高越好：我们曾尝试使用七次多项式以获得更多自由度，但发现容易产生不自然的振荡。最终回归五次多项式，并通过合理设置边界条件获得满意结果。

4. 实时性需要系统级优化：单独优化某个模块效果有限，需要从参考线预处理、Frenet转换加速到轨迹验证全流程优化。我们最终实现了在10ms内完成完整规划循环的性能目标。

5. 测试验证要全面：除了常规的场景测试，我们特别关注边界条件测试，如零曲率路段、大曲率急弯、参考线不连续点等，这些地方最容易暴露问题。