基于Frenet优化轨迹的无人车动作规划方法实例

原理参考：无人驾驶汽车系统入门（二十一）——基于Frenet优化轨迹的无人车动作规划方法
规划器的C++代码（出自CppRobotics）：

/*************************************************************************
	> File Name: frenet_optimal_trajectory.cpp
	> Author: TAI Lei
	> Mail: [email protected]
	> Created Time: Wed Apr  3 09:52:17 2019
 ************************************************************************/

#include
#include
#include
#include
#include
#include
#include
#include"cubic_spline.h"
#include"frenet_path.h"
#include"quintic_polynomial.h"
#include"quartic_polynomial.h"

#define SIM_LOOP 500
#define MAX_SPEED  50.0 / 3.6  // 最大速度 [m/s]
#define MAX_ACCEL  2.0  // 最大加速度 [m/ss]
#define MAX_CURVATURE  1.0  // 最大曲率 [1/m]
#define MAX_ROAD_WIDTH  7.0  // 最大道路宽度 [m]
#define D_ROAD_W  1.0  // 道路宽度采样间隔 [m]
#define DT  0.2  // Delta T [s]
#define MAXT  5.0  // 最大预测时间 [m]
#define MINT  4.0  // 最小预测时间 [m]
#define TARGET_SPEED  30.0 / 3.6  // 目标速度（即纵向的速度保持） [m/s]
#define D_T_S  5.0 / 3.6  // 目标速度采样间隔 [m/s]
#define N_S_SAMPLE  1  // 目标速度的采样数量
#define ROBOT_RADIUS  1.5  // 机器人底盘半径 [m]

#损失函数权重
#define KJ  0.1
#define KT  0.1
#define KD  1.0
#define KLAT  1.0
#define KLON  1.0

using namespace cpprobotics;


float sum_of_power(std::vector value_list){
  float sum = 0;
  for(float item:value_list){
    sum += item*item;
  }
  return sum;
};

/**
 *使用基于Frenet坐标系的优化轨迹方法生成一系列横向和纵向的轨迹，并且计算每条轨迹对应的损失值
**/
Vec_Path calc_frenet_paths(
    float c_speed, float c_d, float c_d_d, float c_d_dd, float s0){
  std::vector fp_list;
  // 根据道路宽度进行采样
  for(float di=-1*MAX_ROAD_WIDTH; di::min();
        fp_bot.max_accel = std::numeric_limits::min();
        for(float t_:fp.t){
          // 获取采样时间内每一刻d的位置
          fp_bot.s.push_back(lon_qp.calc_point(t_));
          // 获取采样时间内每一刻s的速度
          fp_bot.s_d.push_back(lon_qp.calc_first_derivative(t_));
          // 获取采样时间内每一刻s的加速度
          fp_bot.s_dd.push_back(lon_qp.calc_second_derivative(t_));
          // 获取采样时间内每一刻s的加加速度
          fp_bot.s_ddd.push_back(lon_qp.calc_third_derivative(t_));
          if(fp_bot.s_d.back() > fp_bot.max_speed){
            fp_bot.max_speed = fp_bot.s_d.back();
          }
          if(fp_bot.s_dd.back() > fp_bot.max_accel){
            fp_bot.max_accel = fp_bot.s_dd.back();
          }
        }

        float Jp = sum_of_power(fp.d_ddd); // d方向加加速度的总和，舒适度越好，机器人速度越平滑
        float Js = sum_of_power(fp_bot.s_ddd); // s方向加加速度的总和，舒适度越好，机器人速度越平滑
        float ds = std::pow((TARGET_SPEED - fp_bot.s_d.back()),2);// 目标速度与数组内的最后一个速度差值的平方

        fp_bot.cd = KJ * Jp + KT * Ti + KD * std::pow(fp_bot.d.back(), 2);// 横向损失函数（原则上横向加加速度越小越好，速度变化时间越小越好，终点距离中心线越近越好）
        fp_bot.cv = KJ * Js + KT * Ti + KD * ds;// 纵向损失函数（原则上纵向加加速度越小越好，速度变化时间越小越好，终点距离中心线越近越好）
        fp_bot.cf = KLAT * fp_bot.cd + KLON * fp_bot.cv;// 总的损失函数为d和s方向的损失函数乘以对应的系数并相加

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

/************************计算全局路径************************/
void calc_global_paths(Vec_Path & path_list, Spline2D csp){
  for (Vec_Path::iterator path_p=path_list.begin(); path_p!=path_list.end();path_p++){
    for(unsigned int i=0; is.size(); i++){
      if (path_p->s[i] >= csp.s.back()){
        break;
      }
      std::array poi = csp.calc_postion(path_p->s[i]);
      float iyaw = csp.calc_yaw(path_p->s[i]);
      float di = path_p->d[i];
      float x = poi[0] + di * std::cos(iyaw + M_PI/2.0);
      float y = poi[1] + di * std::sin(iyaw + M_PI/2.0);
      path_p->x.push_back(x);
      path_p->y.push_back(y);
    }

    for(int i=0; ix.size()-1; i++){
      float dx = path_p->x[i + 1] - path_p->x[i];
      float dy = path_p->y[i + 1] - path_p->y[i];
      path_p->yaw.push_back(std::atan2(dy, dx));
      path_p->ds.push_back(std::sqrt(dx * dx + dy * dy));
    }

    path_p->yaw.push_back(path_p->yaw.back());
    path_p->ds.push_back(path_p->ds.back());


    path_p->max_curvature = std::numeric_limits::min();
    for(int i=0; ix.size()-1; i++){
      path_p->c.push_back((path_p->yaw[i+1]-path_p->yaw[i])/path_p->ds[i]);
      if(path_p->c.back() > path_p->max_curvature){
        path_p->max_curvature = path_p->c.back();
      }
    }
  }
};

/************************碰撞检测************************/
bool check_collision(FrenetPath path, const Vec_Poi ob){
  for(auto point:ob){
    // 计算所有路径点到障碍物的距离来推测是否发生碰撞
    for(unsigned int i=0; i::max();
  FrenetPath final_path;
  for(auto path:save_paths){
    if (min_cost >= path.cf){
      min_cost = path.cf;
      final_path = path;
    }
  }
  return final_path;
};

cv::Point2i cv_offset(
    float x, float y, int image_width=2000, int image_height=2000){
  cv::Point2i output;
  output.x = int(x * 100) + 300;
  output.y = image_height - int(y * 100) - image_height/3;
  return output;
};

int main(){
  Vec_f wx({0.0, 10.0, 20.5, 35.0, 70.5});
  Vec_f wy({0.0, -6.0, 5.0, 6.5, 0.0});
  std::vector obstcles{
    {{20.0, 10.0}},
    {{30.0, 6.0}},
    {{30.0, 8.0}},
    {{35.0, 8.0}},
    {{50.0, 3.0}}
  };

  Spline2D csp_obj(wx, wy);
  Vec_f r_x;
  Vec_f r_y;
  Vec_f ryaw;
  Vec_f rcurvature;
  Vec_f rs;

  for(float i=0; i point_ = csp_obj.calc_postion(i);
    r_x.push_back(point_[0]);
    r_y.push_back(point_[1]);
    ryaw.push_back(csp_obj.calc_yaw(i));
    rcurvature.push_back(csp_obj.calc_curvature(i));
    rs.push_back(i);
  }

  float c_speed = 10.0/3.6;
  float c_d = 2.0;
  float c_d_d = 0.0;
  float c_d_dd = 0.0;
  float s0 = 0.0;

  float area = 20.0;

  cv::namedWindow("frenet", cv::WINDOW_NORMAL);
  int count = 0;

  for(int i=0; i