# EM Planner 

**Repository Path**: AlexL002/em-planner

## Basic Information

- **Project Name**: EM Planner 
- **Description**: 使用C++复现EM Planner算法，并使用Prescan进行仿真。
- **Primary Language**: C++
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 2
- **Created**: 2025-03-10
- **Last Updated**: 2025-03-10

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

### 介绍

Prescan自动驾驶仿真，用于验证规划控制算法。<br>
横向控制:LQR、模糊PID(弃用)<br>
纵向控制:PID<br>
全局规划:静态全局轨迹点，文件reference.csv<br>
局部规划:EM Planner  DP部分（待完善）<br>

### 平台要求

python、simulink、matlab2018b、prescan2021.1.0 64 bits<br>
规划控制模块使用c++搭建<br>
环境搭建来自于prescan<br>
动力学仿真来自于simulink<br>
程序和prescan使用UDP通信<br>

### 使用教程

1. prescan打开road文件夹中road.pex文件<br>
2. 点击Build,点击Invoke Simulink run mode<br>
   ![img.png](D:\4_APP Files\PrescanProject\prescan_simulator-develop\img.png)
3. matlab在Road目录下，启动Road_cs.slx文件<br>
   ![img_1.png](D:\4_APP Files\PrescanProject\prescan_simulator-develop\img_1.png)
4. 点击Regenerate，然后点击Run<br>
   ![img_2.png](D:\4_APP Files\PrescanProject\prescan_simulator-develop\img_2.png)
5. 运行c++ 主程序<br>

### 程序结构

#### 功能

main：读取本地导航路径到Path类，配置通信接口，配置预测、规划、控制模块

config：定于全局配置参数

common：定义一些基本的结构体和类

math：定义数学运算常用的点、线段、矩形框类

chassis：通信设置

map：定义地图信息，主要是导航路径Path类

perception：预测模块，主要用于传输障碍物信息

planning_：规划模块，输入导航路径Path类，输出轨迹Trajectory类，数据流通过Frame结构体传输

control_：控制模块，定义纵向PID和横向LQR两个控制器，输入Frame结构体，输出steer、throttle、brake

#### 规划模块

规划模块从planning_referenceLine开始，到planning结束

1. planning_referenceLine：无（由于导航路径是光滑的，因此参考线视为导航路径直接截取不做其他处理的一段）

2. planning_trajectory：定义规划的轨迹类Trajectory和Frenet坐标系的点FrenetPoint类

3. planning_coordinate：定义坐标系转化的若干函数

4. planning_obstacle：定义规划中障碍物在Frenet坐标系中的信息，包括S-L和S-T信息

5. planning_generator：定义轨迹生成的类TrajectoryGenerator，用于计算两点连接的五次多项式

6. planning_base：定义Frame类，用于程序运行中的数据流，方便数据取用

7. planning：规划模块的主要部分，定义一些函数和Planner类

   ```c++
   // 142 将导航路径path截断获取参考线，并放入到frame中
   GetReferenceLine(frame) ;
   // 192 计算自车SL坐标，将投影点ego_proj、frenet信息ego_s、ego_l放入frame
   EgoFrenetPoint(frame);
   // 209 计算起始点和匹配点
   TrajectoryPoint initPoint = FindPlanningInitPoint(frame);
       // 218 Replan成立，起始点通过运动学推断
       predictEgoState(frame, planning_cycle_time, egoPredic);
       // 233 Replan不成立，起始点通过匹配规划时间间隔和上次规划轨迹trajectory的时间戳获取
   	egoPredic = frame.trajectory.trajectory()[initIndex];
   	// 238 将规划起始点egoPredic匹配到当前规划周期的参考线中，获取匹配点，存入frame
   	matchPoint = pathMatcher.MatchToPath(frame.referenceLine.path(),
               egoPredic.path_point().x(), egoPredic.path_point().y());
   // 244 计算规划起始点的Frenet信息
    void Planner::FindMatchPoint(Frame& frame, TrajectoryPoint& initPoint, PathPoint& matchPoint, std::array<double, 3>& init_s, std::array<double, 3>& init_d)
   // 252 将预测模块获取的障碍物信息转化为规划模块需要的障碍物信息
   std::list<PlanningObstacle> Planner::GetFrenetObstacle(Frame& frame);
   // 269 生成DP路径
   std::vector<FrenetPoint> Planner::GeneratePathDP(Frame& frame,
           std::array<double, 3>& init_s, std::array<double, 3>& init_d, 
           std::list <PlanningObstacle> planningObsList)
   	// 287 获取撒点点集，前方纵向撒4列，每列间距8*v米，横向撒八个点，左右边界为2倍道路宽度（应严格定义为道路边界）
       samplePoint.emplace_back(sl_vector_tmp);
       // 313 获取撒点点集各点连接的cost，
   	// 328 每次选择相邻列的两个点进行计算，第一个点为上次循环中计算的cost最小的点，第二个点为遍历当前列的所有点
   	slPoint_select = { {samplePoint[i - 1][index_minCost[i - 1]].s, samplePoint[i - 1][index_minCost[i - 1]].l}, {samplePoint[i][j].s, samplePoint[i][j].l} };
       // 340 计算选择两个点之间连接的五次多项式系数
   	tg.CoeQuinticPolynomial(A, X);	
   	// 343 两个点之间进行插值，插值的越多，计算量越大，cost的计算越精确
       std::vector<FrenetPoint> interPoints = 			tg.InterQuinticPolynomial(X,interpolation_num);
   	// 345 计算所有插值点的cost = 光滑代价+障碍物距离代价+参考线距离代价
   	// 353 更新最小cost的插值点到smoothPath中(后续改为系数矩阵，方便二次插值)
   // 399 将获取的S-L坐标系下的规划轨迹frenetPath转为笛卡尔坐标系轨迹trajectory
   Trajectory Planner::FrenetToCartesian(Frame& frame, std::vector<FrenetPoint> frenetPath) 
   ```