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

最新推荐文章于 2023-09-17 15:05:28 发布

原创

最新推荐文章于 2023-09-17 15:05:28 发布 · 2.4k 阅读

31 ·

CC 4.0 BY-SA版权

本文介绍了一种基于Frenet坐标系的无人车动作规划方法，通过优化横向和纵向轨迹，确保车辆行驶的安全性和舒适度。代码实现中考虑了速度、加速度、曲率等约束条件，同时进行了碰撞检测。

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

/*************************************************************************
	> File Name: frenet_optimal_trajectory.cpp
	> Author: TAI Lei
	> Mail: ltai@ust.hk
	> Created Time: Wed Apr  3 09:52:17 2019
 ************************************************************************/

#include<iostream>
#include<limits>
#include<vector>
#include<opencv2/opencv.hpp>
#include<opencv2/core/core.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<sys/time.h>
#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<float> 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<FrenetPath> fp_list;
  // 根据道路宽度进行采样
  for(float di=-1*MAX_ROAD_WIDTH; di<MAX_ROAD_WIDTH; di+=D_ROAD_W){
    // 横向动作规划
    for(float Ti=MINT; Ti<MAXT; Ti+=DT){
      FrenetPath fp;
      // 计算出关于目标配置d