算法学习——Lattice planner算法

.小墨迹

已于 2025-02-28 10:21:00 修改

阅读量1.9k

点赞数 31

分类专栏：算法学习文章标签：算法学习开发语言 c++ linux

于 2025-02-20 14:09:03 首次发布

本文链接：https://blog.csdn.net/qq1240268067/article/details/145752007

版权

算法学习专栏收录该内容

19 篇文章

订阅专栏

Lattice Planner 算法

一、算法原理

Lattice Planner 是一种基于采样的轨迹规划算法，广泛应用于自动驾驶领域。它通过在 Frenet 坐标系下生成一系列候选轨迹，并通过代价函数评估这些轨迹的优劣，最终选择最优轨迹。

1.1 Frenet 坐标系

Frenet 坐标系是一种基于道路的局部坐标系，使用两个参数表示车辆的位置：
$s$ ：沿道路的纵向距离。
$l$ ：横向偏移量。
Frenet 坐标系与笛卡尔坐标系的转换公式如下：
从 Frenet 到笛卡尔：

$x = x_r - l * sin(θ_r);$
$y = y_r + l * cos(θ_r);$
$θ_x = arctan2(l', 1 - κ_r * l) + θ_r;$
$v = sqrt(s_dot^2 * (1 - κ_r * l) + l_dot^2);$

其中， $θ_r$ 是参考点的偏航角， $κ_r$ 是参考点的曲率。

1.2 轨迹生成

Lattice Planner 通过以下步骤生成候选轨迹：
采样横向偏移 $（ l ）$ 和时间 $（ t ）$ ：生成横向轨迹（d-t 曲线）。
采样目标速度 $（ v ）$ ：生成纵向轨迹（s-t 曲线）。
组合横向和纵向轨迹：生成完整的轨迹。

1.3 代价函数

每条轨迹的代价由横向代价和纵向代价组成：
横向代价：考虑横向 jerk（加加速度）和横向偏移量。

$c d = K J * J p + K T * t + KD * p o w (d . ba c k (), 2);$
纵向代价：考虑纵向 jerk 和与目标速度的偏差。

$cv = KJ * Js + KT * t + KD * pow(target_speed - s_d.back(), 2);$
总代价：

$c f = K L A T * c d + K L ON * c v;$
其中， $K J$ 、 $K T$ 、 $KD$ 、 $K L A T$ 和 $K L ON$ 是权重参数。

二、代码实现

以下是基于 C++ 的 Lattice Planner 简化实现：

2.1 轨迹类

#include <vector>
#include <cmath>
#include <limits>
#include <algorithm>
#include <Eigen/Dense>

using Vec_f = std::vector<float>;
using Vec_Poi = std::vector<std::pair<float, float>>;

struct FrenetPath {
    float cd = 0.0;  // 横向代价
    float cv = 0.0;  // 纵向代价
    float cf = 0.0;  // 总代价

    Vec_f t;  // 时间
    Vec_f d;  // 横向偏移
    Vec_f d_d;  // 横向速度
    Vec_f d_dd;  // 横向加速度
    Vec_f s;  // 纵向位置
    Vec_f s_d;  // 纵向速度
    Vec_f s_dd;  // 纵向加速度

    Vec_f x;  // x 位置
    Vec_f y;  // y 位置
    Vec_f yaw;  // 偏航角
    Vec_f ds;  // 速度
    Vec_f c;  // 曲率

    float max_speed = 0.0;
    float max_accel = 0.0;
    float max_curvature = 0.0;
};

2.2 四阶多项式类

class QuarticPolynomial {
public:
    float xs, vxs, axs, vxe, axe;
    float a0, a1, a2, a3, a4;

    QuarticPolynomial(float xs_, float vxs_, float axs_, float vxe_, float axe_, float T)
        : xs(xs_), vxs(vxs_), axs(axs_), vxe(vxe_), axe(axe_), a0(xs_), a1(vxs_), a2(axs_ / 2.0) {
        Eigen::Matrix2f A;
        A << 3 * std::pow(T, 2), 4 * std::pow(T, 3), 6 * T, 12 * std::pow(T, 2);
        Eigen::Vector2f B;
        B << vxe - a1 - 2 * a2 * T, axe - 2 * a2;

        Eigen::Vector2f c_eigen = A.colPivHouseholderQr().solve(B);
        a3 = c_eigen[0];
        a4 = c_eigen[1];
    }

    float calc_point(float t) {
        return a0 + a1 * t + a2 * std::pow(t, 2) + a3 * std::pow(t, 3) + a4 * std::pow(t, 4);
    }

    float calc_first_derivative(float t) {
        return a1 + 2 * a2 * t + 3 * a3 * std::pow(t, 2) + 4 * a4 * std::pow(t, 3);
    }

    float calc_second_derivative(float t) {
        return 2 * a2 + 6 * a3 * t + 12 * a4 * std::pow(t, 2);
    }

    float calc_third_derivative(float t) {
        return 6 * a3 + 24 * a4 * t;
    }
};

2.3 轨迹生成函数

Vec_f calc_frenet_paths(float c_speed, float c_d, float c_d_d, float c_d_dd, float s0) {
    std::vector<FrenetPath> fp_list;
    const float D_ROAD_W = 1.0;  // 横向采样间隔
    const float MINT = 2.0;  // 最小时间
    const float MAXT = 5.0;  // 最大时间
    const float DT = 0.2;  // 时间采样间隔
    const float TARGET_SPEED = 30.0;  // 目标速度
    const float D_T_S = 5.0;  // 速度采样间隔
    const int N_S_SAMPLE = 1;  // 速度采样数量

    for (float di = 0; di <= 5; di += D_ROAD_W) {
        for (float ti = MINT; ti <= MAXT; ti += DT) {
            FrenetPath fp_without_s;
            QuarticPolynomial lateral_fp(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, ti);
            for (float _t = 0.0; _t <= ti; _t += DT) {
                fp_without_s.t.push_back(_t);
                fp_without_s.d.push_back(lateral_fp.calc_point(_t));
                fp_without_s.d_d.push_back(lateral_fp.calc_first_derivative(_t));
                fp_without_s.d_dd.push_back(lateral_fp.calc_second_derivative(_t));
            }

            for (float vi = TARGET_SPEED - D_T_S * N_S_SAMPLE; vi <= TARGET_SPEED + D_T_S * N_S_SAMPLE; vi += D_T_S) {
                FrenetPath fp_with_s = fp_without_s;
                QuarticPolynomial longitudinal_qp(s0, c_speed, 0.0, vi, 0.0, ti);
                for (float _t : fp_without_s.t) {
                    fp_with_s.s.push_back(longitudinal_qp.calc_point(_t));
                    fp_with_s.s_d.push_back(longitudinal_qp.calc_first_derivative(_t));
                    fp_with_s.s_dd.push_back(longitudinal_qp.calc_second_derivative(_t));
                }

                fp_with_s.max_speed = *std::max_element(fp_with_s.s_d.begin(), fp_with_s.s_d.end());
                fp_with_s.max_accel = *std::max_element(fp_with_s.s_dd.begin(), fp_with_s.s_dd.end());

                fp_list.push_back(fp_with_s);
            }
        }
    }
    return fp_list;
}

2.4 碰撞检测

bool check_collision(const FrenetPath& path, const Vec_Poi& ob) {
    const float ROBOT_RADIUS = 2.0;  // 机器人半径
    for (const auto& point : ob) {
        for (size_t i = 0; i < path.x.size(); ++i) {
            float dist = std::pow(path.x[i] - point.first, 2) + std::pow(path.y[i] - point.second, 2);
            if (dist <= ROBOT_RADIUS * ROBOT_RADIUS) {
                return false;
            }
        }
    }
    return true;
}

2.5 代价函数

float calc_to_target_cost(const FrenetPath& path, float target_speed) {
    const float KJ = 0.1;  // jerk 权重
    const float KT = 0.1;  // 时间权重
    const float KD = 1.0;  // 偏移量权重
    const float KLAT = 1.0;  // 横向权重
    const float KLON = 1.0;  // 纵向权重

    float cd = KJ * std::pow(path.d_dd.back(), 2) + KT * path.t.back() + KD * std::pow(path.d.back(), 2);
    float cv = KJ * std::pow(path.s_dd.back(), 2) + KT * path.t.back() + KD * std::pow(target_speed - path.s_d.back(), 2);
    return KLAT * cd + KLON * cv;
}

2.6 主函数

int main() {
    // 初始状态
    float c_speed = 10.0;  // 当前速度
    float c_d = 0.0;  // 当前横向偏移
    float c_d_d = 0.0;  // 当前横向速度
    float c_d_dd = 0.0;  // 当前横向加速度
    float s0 = 0.0;  // 当前纵向位置

    // 障碍物
    Vec_Poi obstacles = {{10.0, 2.0}, {20.0, 3.0}};

    // 生成轨迹
    auto fplist = calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0);

    // 评估轨迹
    FrenetPath best_path;
    float min_cost = std::numeric_limits<float>::max();
    for (auto& fp : fplist) {
        if (!check_collision(fp, obstacles)) {
            continue;
        }

        float cost = calc_to_target_cost(fp, TARGET_SPEED);
        if (cost < min_cost) {
            min_cost = cost;
            best_path = fp;
        }
    }

    // 输出最优轨迹
    std::cout << "Best Path: " << std::endl;
    for (size_t i = 0; i < best_path.x.size(); ++i) {
        std::cout << "x: " << best_path.x[i] << ", y: " << best_path.y[i] << std::endl;
    }

    return 0;
}

三、参数调优

Lattice Planner 的性能依赖于多个参数的设置。以下是一些关键参数及其调优方法：

3.1 横向采样参数

横向偏移范围（D_ROAD_W）：横向偏移的采样间隔。较大的间隔可以减少计算量，但可能导致轨迹不够精细。
横向偏移的最大值：根据道路宽度和车辆安全距离设置。

3.2 纵向采样参数

时间范围（MINT 和 MAXT）：轨迹的持续时间范围。较短的时间可能导致轨迹过于激进，而较长的时间可能导致轨迹不够灵活。
目标速度（TARGET_SPEED）：车辆的期望速度。根据道路限速和交通情况调整。

3.3 代价函数权重

横向权重（KLAT）：较高的权重会使轨迹更注重横向平滑性。
纵向权重（KLON）：较高的权重会使轨迹更注重速度的平稳性。

四、优化方法

4.1 预处理

道路信息预处理：将道路信息（如曲率、参考轨迹）预处理为离散点，减少实时计算量。
障碍物检测优化：使用高效的碰撞检测算法（如 KD-Tree）加速障碍物检测。

4.2 并行计算

多线程：在轨迹生成和评估阶段使用多线程，加速计算过程。
GPU 加速：对于大规模采样，可以考虑使用 GPU 加速。

4.3 采样优化

自适应采样：根据车辆状态和环境动态调整采样间隔和范围。
贝塞尔曲线优化：使用贝塞尔曲线生成平滑轨迹，减少多项式计算量。

五、总结

Lattice Planner 是一种高效且灵活的轨迹规划算法，适用于复杂环境下的自动驾驶任务。通过合理设置参数和优化算法，可以显著提升其性能。

六、问答

1、Lattice planner算法生成轨迹使用四阶或者五阶多项式，输入条件是什么，怎么生成的样条曲线？
在 Lattice Planner 算法中，四阶或五阶多项式被广泛用于生成轨迹，这些多项式可以描述车辆在横向（d-t）和纵向（s-t）的运动。：

1. 输入条件

在 Lattice Planner 中，使用四阶或五阶多项式生成轨迹时，通常需要以下输入条件：

初始状态：
- 当前时刻的横向偏移（d0）和纵向位置（s0）。
- 当前时刻的横向速度（d'_0）和纵向速度（s'_0）。
- 当前时刻的横向加速度（d''_0）和纵向加速度（s''_0）。
目标状态：
- 目标时刻的横向偏移（d_end）和纵向位置（s_end）。
- 目标时刻的横向速度（d'_end）和纵向速度（s'_end）。
- 目标时刻的横向加速度（d''_end）和纵向加速度（s''_end）。
时间范围：
- 规划的时间范围（T），即从当前时刻到目标时刻的时间。

2. 样条曲线生成方法

Lattice Planner 通常使用四阶或五阶多项式生成轨迹，这些多项式可以满足初始和目标状态的约束条件。以下是生成轨迹的具体步骤：

2.1 四阶多项式

四阶多项式通常用于描述轨迹的横向或纵向运动：

s(t) = a0 + a1 * t + a2 * t^2 + a3 * t^3 + a4 * t^4
d(t) = b0 + b1 * t + b2 * t^2 + b3 * t^3 + b4 * t^4

初始条件：
- s(0) = s0，s'(0) = s'_0，s''(0) = s''_0。
- d(0) = d0，d'(0) = d'_0，d''(0) = d''_0。
目标条件：
- s(T) = s_end，s'(T) = s'_end。
- d(T) = d_end，d'(T) = d'_end。

通过这些条件，可以求解多项式的系数（a0 到 a4，b0 到 b4）。

2.2 五阶多项式

五阶多项式可以提供更平滑的轨迹，适用于需要更高阶连续性的场景：

s(t) = a0 + a1 * t + a2 * t^2 + a3 * t^3 + a4 * t^4 + a5 * t^5
d(t) = b0 + b1 * t + b2 * t^2 + b3 * t^3 + b4 * t^4 + b5 * t^5

初始条件：
- s(0) = s0，s'(0) = s'_0，s''(0) = s''_0。
- d(0) = d0，d'(0) = d'_0，d''(0) = d''_0。
目标条件：
- s(T) = s_end，s'(T) = s'_end，s''(T) = s''_end。
- d(T) = d_end，d'(T) = d'_end，d''(T) = d''_end。

五阶多项式可以更好地满足初始和目标状态的约束条件。

3. 样条曲线生成

在 Lattice Planner 中，样条曲线通常用于生成全局参考线或局部轨迹。例如，三次样条曲线可以用于拟合全局路径点（waypoints），作为 Frenet 坐标系的参考线。

3.1 三次样条曲线

三次样条曲线用于拟合全局路径点，生成平滑的参考线：

class Spline2D {
    Spline sx;  // x = f(s)
    Spline sy;  // y = f(s)
    Vec_f s;    // 参数 s

public:
    Spline2D(Vec_f x, Vec_f y) {
        s = calc_s(x, y);  // 计算累积弧长
        sx = Spline(s, x);
        sy = Spline(s, y);
    };

    Poi_f calc_position(float s_t) {
        float x = sx.calc(s_t);
        float y = sy.calc(s_t);
        return {x, y};  // 计算全局坐标
    };

    float calc_curvature(float s_t) {
        float dx = sx.calc_d(s_t);
        float ddx = sx.calc_dd(s_t);
        float dy = sy.calc_d(s_t);
        float ddy = sy.calc_dd(s_t);
        return (ddy * dx - ddx * dy) / pow(dx * dx + dy * dy, 1.5);  // 计算曲率
    };
};

通过三次样条曲线，可以生成平滑的全局参考线，并用于后续的局部轨迹规划。

生成五阶多项式轨迹：

#include <iostream>
#include <vector>
#include <Eigen/Dense> // 需要安装 Eigen 库

using namespace Eigen;

// 五阶多项式轨迹生成
VectorXd generateQuinticTrajectory(double t, const VectorXd& coeffs) {
    VectorXd state(3); // 位置、速度、加速度
    state(0) = coeffs(0) + coeffs(1) * t + coeffs(2) * t * t + coeffs(3) * t * t * t + coeffs(4) * t * t * t * t + coeffs(5) * t * t * t * t * t;
    state(1) = coeffs(1) + 2 * coeffs(2) * t + 3 * coeffs(3) * t * t + 4 * coeffs(4) * t * t * t + 5 * coeffs(5) * t * t * t * t;
    state(2) = 2 * coeffs(2) + 6 * coeffs(3) * t + 12 * coeffs(4) * t * t + 20 * coeffs(5) * t * t * t;
    return state;
}

int main() {
    // 初始状态和目标状态
    VectorXd start_state(3);
    start_state << 0, 0, 0; // 位置、速度、加速度
    VectorXd end_state(3);
    end_state << 10, 0, 0; // 位置、速度、加速度

    // 时间
    double T = 5.0;

    // 构建边界条件矩阵
    MatrixXd A(6, 6);
    A << 1, 0, 0, 0, 0, 0,
         0, 1, 0, 0, 0, 0,
         0, 0, 2, 0, 0, 0,
         1, T, T*T, T*T*T, T*T*T*T, T*T*T*T*T,
         0, 1, 2*T, 3*T*T, 4*T*T*T, 5*T*T*T*T,
         0, 0, 2, 6*T, 12*T*T, 20*T*T*T;

    VectorXd b(6);
    b << start_state(0), start_state(1), start_state(2), end_state(0), end_state(1), end_state(2);

    // 求解多项式系数
    VectorXd coeffs = A.colPivHouseholderQr().solve(b);

    // 生成轨迹
    for (double t = 0; t <= T; t += 0.1) {
        VectorXd state = generateQuinticTrajectory(t, coeffs);
        std::cout << "t: " << t << ", x: " << state(0) << ", v: " << state(1) << ", a: " << state(2) << std::endl;
    }

    return 0;
}