自动驾驶控制与规划——Project 5: Lattice Planner

零、任务介绍

补全lattice_planner.cpp中的TODO部分，实现横纵向解耦的Lattice Planner。

一、算法原理

1.1 Frenet坐标系与笛卡尔坐标系的转换

给定横向偏置$l$，参考点在全局坐标系下的位置$(x_r,y_r)$，以及参考点的偏航角${\theta }_r$、参考点处曲率${\kappa }_r$，按照如下公式计算Frenet到笛卡尔坐标系的变换（$[s,\dot{s},l,l^{\prime }]$到$[x,y,{\theta }_x,\kappa ,v]$的变换）。
$$\begin{array}{rl}x& =x_r-l\sin {\theta }_r\\ y& =y_r+l\cos {\theta }_r\\ {\theta }_x& =\text{arctan2}(l^{\prime },1-{\kappa }_{r}l)+{\theta }_r\\ v& =\sqrt{{\dot{s}}^2(1-{\kappa }_{r}l)+{\dot{l}}^2}\end{array}$$
其中 $\dot{l}$ 是 $l$ 对时间 $t$ 的导数，$l^{\prime }$ 是 $l$ 对 $s$ 的导数。
本项目中，使用三次样条曲线拟合全局路点作为Frenet的参考线，参考线的曲率、偏航角等参数的计算方法在Spline2D中给出。

1.2 Lattice Planner

此处的Lattice Planner参考Werling M, Ziegler J, Kammel S, et al. Optimal trajectory generation for dynamic street scenarios in a frenet frame[C]//2010 IEEE international conference on robotics and automation. IEEE, 2010: 987-993.，详细的论文阅读笔记可以参考【论文笔记】Optimal trajectory generation for dynamic street scenarios in a Frenét Frame

本项目中使用高速情况下的Lattice Planner，高速巡航时，车辆的横纵向轨迹规划可以解耦，可以用如下的四阶多项式描述横纵向轨迹：
$$\begin{array}{rl}s(t)& =a_1{t}^4+a_2{t}^3+a_3{t}^2+a_{4}t+a_5\\ d(t)& =b_1{t}^4+b_2{t}^3+b_3{t}^2+b_{4}t+b_5\end{array}$$
约束如下，需要注意由于此场景是巡航场景，因此$s(t_1)$无约束：
$$\begin{array}{rl}s(t_0)& =a_5=s_0\\ v(t_0)& =a_4=v_0\\ a(t_0)& =2a_3=a_0\\ v(t_1)& =4a_1{t}_1^3+3a_2{t}_1^2+2a_3{t}_1+a_4\\ a(t_1)& =12a_1{t}_1^2+6a_2{t}_1+2a_3\\ d(t_0)& =b_5=d_0\\ d_v(t_0)& =b_4=d_{v0}\\ d_a(t_0)& =2b_3=d_{a0}\\ d(t_1)& =b_1{t}_1^4+b_2{t}_1^3+b_3{t}_1^2+b_4{t}_1+b_5\\ d_v(t_1)& =4b_1{t}_1^3+3b_2{t}_1^2+2b_3{t}_1+b_4\\ d_a(t_1)& =12b_1{t}_1^2+6b_2{t}_1+2b_3\end{array}$$
规划流程如下：

1. 将道路横向离散化，按时间步长采样，计算四阶多项式（d-t曲线）
2. 对上述计算的每一条d-t曲线，将纵向速度离散化，计算四阶多项式（s-t曲线）
3. 计算每个轨迹的代价函数，代价包含jerk，时间，速度误差等
4. 检查轨迹的可行性，例如是否超过最大速度、是否发生碰撞等
5. 选择代价函数最优的可行轨迹

二、代码实现

2.1 轨迹类

FrenetPath类，存储Frenet坐标系下的轨迹点横纵向位置及其导数，以及对应的笛卡尔坐标系下的位置、偏航角、速度、曲率。

class FrenetPath {
   public:
    float cd = 0.0;
    float cv = 0.0;
    float cf = 0.0;
    // Vec_f = std::vector<float>
    Vec_f t;        // time
    Vec_f d;        // lateral offset
    Vec_f d_d;      // lateral speed
    Vec_f d_dd;     // lateral acceleration
    Vec_f d_ddd;    // lateral jerk
    Vec_f s;        // s position along spline
    Vec_f s_d;      // s speed
    Vec_f s_dd;     // s acceleration
    Vec_f s_ddd;    // s jerk

    Vec_f x;      // x position
    Vec_f y;      // y position
    Vec_f yaw;    // yaw in rad
    Vec_f ds;     // speed
    Vec_f c;      // curvature

    float max_speed;
    float max_accel;
    float max_curvature;
};

三次样条曲线Spline2D类，世界坐标系下的横纵坐标表示为以s为参数的三次样条sx和sy，计算给定s_t下的全局坐标、曲率、偏航角等参数。本项目中使用Spline2D拟合全局waypoint作为Frenet坐标系的参考线。

class Spline2D {
   public:
    Spline sx;
    Spline sy;
    Vec_f s;

    Spline2D(Vec_f x, Vec_f y) {
        s = calc_s(x, y);   
        sx = Spline(s, x);
        sy = Spline(s, y);
    };

    Poi_f calc_postion(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);
        // 曲率计算公式是 kappa = (x'*y'' - x''*y') / (x'^2 + y'^2)^(3/2)
        return (ddy * dx - ddx * dy) / (dx * dx + dy * dy);
    };

    float calc_yaw(float s_t) { // 计算yaw
        float dx = sx.calc_d(s_t);
        float dy = sy.calc_d(s_t);
        return std::atan2(dy, dx);  // 全局坐标系下的v与x轴的夹角
    };

   private:
    Vec_f calc_s(Vec_f x, Vec_f y) {
        Vec_f ds;
        Vec_f out_s{0};
        Vec_f dx = vec_diff(x);
        Vec_f dy = vec_diff(y);

        for (unsigned int i = 0; i < dx.size(); i++) {
            ds.push_back(std::sqrt(dx[i] * dx[i] + dy[i] * dy[i]));
        }

        Vec_f cum_ds = cum_sum(ds);
        out_s.insert(out_s.end(), cum_ds.begin(), cum_ds.end());
        return out_s;
    };
};

四阶多项式类QuarticPolynomial，本项目中使用四阶多项式描述规划轨迹中的s-t轨迹和d-t轨迹。

class QuarticPolynomial {
   public:
    // current parameter at t=0
    float xs;
    float vxs;
    float axs;

    // parameters at target t=t_j
    float vxe;
    float axe;

    // function parameters
    float a0, a1, a2, a3, a4;

    QuarticPolynomial(){};

    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.2 轨迹规划相关函数

calc_frenet_paths获取采样轨迹，此处将横向和纵向解耦，算法步骤如下：

1. 按D_ROAD_W将道路横向离散化，按时间步长DT从MINT到MAXT采样，计算四阶多项式lateral_fp（d-t曲线），离散化为fp_without_s
2. 对上述计算的每一条d-t曲线，按D_T_S将纵向速度从TARGET_SPEED - D_T_S * N_S_SAMPLE到TARGET_SPEED + D_T_S * N_S_SAMPLE离散化，计算四阶多项式longitudinal_fp（s-t曲线），与d-t曲线合成并离散化为fp_with_s
3. 计算每个fp_with_s的代价函数，代价包含jerk，时间，速度误差等

// 01 获取采样轨迹
Vec_Path FrenetOptimalTrajectory::calc_frenet_paths(float c_speed, float c_d, float c_d_d, float c_d_dd, float s0) {
    std::vector<FrenetPath> fp_list;
    // 根据 当前车辆的速度，当前车辆在frenet坐标系中的s坐标，l坐标，l坐标的一阶导数和二阶导数来采样生成备选轨迹
    // 先遍历 d 方向，再遍历 t 方向，这样可以生成 d-t 曲线，每个d-t曲线下，再遍历备选速度，生成 s-t 曲线，这种方法其实只适用终点 s 自由的方式
    // 完成轨迹采样
    for (float di = 0; di <= 5; di += D_ROAD_W) {
    	// 起始右车道中心线是0，向左侧遍历
        for (float ti = MINT; ti <= MAXT; ti += DT) {
            FrenetPath fp_without_s;
            // std::cout << "采样过程中的c_d: " << c_d << std::endl;
            QuinticPolynomial lateral_fp(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, ti);    // 四阶多项式
            // 这个 for 不涉及采样，是计算已经采样得到的d-t曲线在每个时刻点状态值，这里只计算一根轨迹
            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));
                fp_without_s.d_ddd.push_back(lateral_fp.calc_third_derivative(_t));
            }
            // 当前遍历下，每一条s-t曲线复用上面刚刚生成的d-t曲线（只有一根），for循环每运行一次，生成一条完整的备选轨迹
            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.s_ddd.push_back(longitudinal_qp.calc_third_derivative(_t));
                }
                fp_with_s.max_speed = *max_element(fp_with_s.s_d.begin(), fp_with_s.s_d.end());
                fp_with_s.max_accel = *max_element(fp_with_s.s_dd.begin(), fp_with_s.s_dd.end());

                float Jp = sum_of_power(fp_with_s.d_ddd);    // 横向jerk平方和
                float Js = sum_of_power(fp_with_s.s_ddd);    // 纵向jerk平方和

                // 计算每条备选轨迹的代价，参考的是论文 “Optimal trajectory generation for dynamic street scenarios in a Frenét Frame“ 里面的巡航控制的代价函数计算方式
                fp_with_s.cd = KJ * Jp + KT * ti + KD * pow(fp_with_s.d.back(), 2);    // 横向代价
                // # square of diff from target speed
                float ds = pow(TARGET_SPEED - fp_with_s.s_d.back(), 2);
                fp_with_s.cv = KJ * Js + KT * ti + KD * ds;                  // 纵向代价
                fp_with_s.cf = KLAT * fp_with_s.cd + KLON * fp_with_s.cv;    // 总代价函数

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

check_paths检查路径是否超过最大速度、最大加速度、最大曲率，是否发生碰撞

// 03
// 检查路径，通过限制最大速度，最大加速度，最大曲率与避障，选取可使用的轨迹数组
Vec_Path FrenetOptimalTrajectory::check_paths(Vec_Path path_list, const Vec_Poi ob) {
    Vec_Path output_fp_list;
    // TODO: 补全代码
    // std::cout << "before check: " << path_list.size() << std::endl;
    for (auto path : path_list) {
        // 检查最大速度、最大加速度、最大曲率
        if (path.max_speed > MAX_SPEED) {
            // std::cout << "exceed max speed" << std::endl;
            continue;
        } else if (path.max_accel > MAX_ACCEL) {
            // std::cout << "exceed max acceleration" << std::endl;
            continue;
        } else if (path.max_curvature > MAX_CURVATURE) {
            // std::cout << "exceed max curvature" << std::endl;
            continue;
        } else if (!check_collision(path, ob)) {
            // 检查是否与障碍物发生碰撞
            // std::cout << "collision" << std::endl;
            continue;
        } else {
            output_fp_list.emplace_back(path);
        }
    }
    // std::cout << "after check: " << output_fp_list.size() << std::endl;
    return output_fp_list;
};

calc_global_paths根据参考轨迹和多条采样轨迹，计算轨迹在笛卡尔坐标系中的坐标、偏航角等参数

// 02
// 根据参考轨迹与采样的轨迹数组，计算frenet中的其他曲线参数，如航向角，曲率，ds等参数
void FrenetOptimalTrajectory::calc_global_paths(Vec_Path& path_list, Spline2D csp) {
    int iiii = 0;
    // 计算采样轨迹的其他参数
    for (Vec_Path::iterator _fpp = path_list.begin(); _fpp != path_list.end(); _fpp++) {
        iiii++;
        // # calc global positions
        for (int i = 0; i < _fpp->s.size(); i++) {
            // 重规划局部轨迹的 s 坐标超过参考路径的最大 s 以后，超过的部分不需要计算其他参数了。
            if (_fpp->s[i] >= csp.s.back()) {
                break;
            }
            Poi_f location_cartesian = csp.calc_postion(_fpp->s[i]);
            float i_yaw_angle = csp.calc_yaw(_fpp->s[i]);
            float di = _fpp->d[i];
            float fx = location_cartesian[0] + di * cos(i_yaw_angle + M_PI / 2.0);
            float fy = location_cartesian[1] + di * sin(i_yaw_angle + M_PI / 2.0);
            _fpp->x.push_back(fx);
            _fpp->y.push_back(fy);
        }

        // # calc yaw and ds
        for (int i = 0; i < _fpp->s.size() - 1; i++) {
            // 重规划局部轨迹的 s 坐标超过参考路径的最大 s 以后，超过的部分不需要计算其他参数了。
            if (_fpp->s[i] >= csp.s.back()) {
                break;
            }
            float dx = _fpp->x[i + 1] - _fpp->x[i];
            float dy = _fpp->y[i + 1] - _fpp->y[i];
            _fpp->yaw.push_back(atan2(dy, dx));
            _fpp->ds.push_back(sqrt(pow(dx, 2) + pow(dy, 2)));
        }
        _fpp->yaw.push_back(_fpp->yaw.back());
        _fpp->ds.push_back(_fpp->ds.back());

        // # calc curvature
        for (int i = 0; i < _fpp->s.size() - 1; i++) {
            // 重规划局部轨迹的 s 坐标超过参考路径的最大 s 以后，超过的部分不需要计算其他参数了。
            if (_fpp->s[i] >= csp.s.back()) {
                break;
            }
            _fpp->c.push_back((_fpp->yaw[i + 1] - _fpp->yaw[i]) / _fpp->ds[i]);
        }
        _fpp->max_curvature = *max_element(_fpp->c.begin(), _fpp->c.end());
    }
};

check_collision碰撞检测，此处将车辆简化为一个圆形的机器人，检查轨迹上每个点到障碍物的距离是否小于机器人半径，若无碰撞返回true，发生碰撞返回false。

bool FrenetOptimalTrajectory::check_collision(FrenetPath path, const Vec_Poi ob) {
    // 碰撞返回false，无碰撞返回true
    for (auto point : ob) {
        for (unsigned int i = 0; i < path.x.size(); i++) {
            float dist = std::pow((path.x[i] - point[0]), 2) + std::pow((path.y[i] - point[1]), 2);
            if (dist <= ROBOT_RADIUS * ROBOT_RADIUS) {
                return false;
            }
        }
    }
    return true;
};

frenet_optimal_planning使用上述函数获得路径，选择最小代价的路径

// TODO: step 1 finish frenet_optimal_planning
FrenetPath FrenetOptimalTrajectory::frenet_optimal_planning(Spline2D csp, float s0, float c_speed, float c_d, float c_d_d, float c_d_dd, Vec_Poi ob) {
    // 01 获取采样轨迹
    Vec_Path fp_list = calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0);
    // 02 根据参考轨迹与采样的轨迹数组，计算frenet中的其他曲线参数，如航向角，曲率，ds等参数
    // 此处的csp是用三次样条拟合路点得到的，作用是提供frenet坐标系的参考点
    calc_global_paths(fp_list, csp);
    // 03 检查路径，通过限制最大速度，最大加速度，最大曲率与避障，选取可使用的轨迹数组
    Vec_Path save_paths = check_paths(fp_list, ob);
    float min_cost = std::numeric_limits<float>::max();
    FrenetPath final_path;
    for (auto path : save_paths) {
        if (min_cost >= path.cf) {
            min_cost = path.cf;
            final_path = path;
        }
    }
    return final_path;
};

三、效果展示