最近在看百度阿波罗的动态规划算法,当前版本主要使用的是lattice plan,2.0版本使用的是EM plan,本篇文章主要记录Lattice plan算法:
Lattice planner程序入口:
Status LatticePlanner::PlanOnReferenceLine(
const TrajectoryPoint& planning_init_point, Frame* frame,
ReferenceLineInfo* reference_line_info)
在lattice_planner.cc文件中,通过for循环进入规划,执行函数为PlanOnReferenceLine.该函数具体执行流程如下:
1.获得当前参考线(车辆未来轨迹),并通过TodiscretizedReferenceLine进行参考线的离散化.
auto ptr_reference_line =
std::make_shared<std::vector<PathPoint>>(ToDiscretizedReferenceLine(
reference_line_info->reference_line().reference_points()));
2.使用规划原点匹配第一步离散化后的轨迹,找出匹配点matched_point。
PathPoint matched_point = PathMatcher::MatchToPath(
*ptr_reference_line, planning_init_point.path_point().x(),
planning_init_point.path_point().y());
3.使用ComputeInitFrenetState计算当前状态下,Frenet坐标系下的点,S和D值,横向速度,纵向速度等。
ComputeInitFrenetState(matched_point, planning_init_point, &init_s, &init_d);
4.加入感知信息,将之前感知模块内容加入到path_time_grapth,也就是所谓的ST-graph
auto ptr_predictio