【最新版】BaiduApollo代码详细解析——EMPlanner中的DPSpeedOpt。。。.pdf

【最新版】BaiduApollo代码详细解析—— EMPlanner中的 DPSpeedOpt。。。 EM Planner中的速度规划，是在路 规划之后，为路 点规划合适的速度。主要包括构建STGraph，计算cost_table，规划速度三 个部分。 构建STGraph主要是针对每个障碍物，⽣成StBoundary ，⽤于计算cost。boundary ，是边界、界限；StBoundary简单来说就是在 速度规划的时间范围内，⽣成障碍物的⾏驶轨迹占据的范围，即整个时间范围内障碍物轨迹s的上下界。具体的计算⽅法不在本篇博客的重 点讨论范围，⼤家可以参考⼀位博主的博客，⾥边讲解的清晰简单，⼀ ⽬了然。 本篇博客讨论的重点是计算cost_table，在Apollo的代码中，这部分内容包括在 apollo/modules/planning/tasks/optimizers/path_time_heuristic/gridded_path_time_graph.cc中的 GriddedPathTimeGraph::Search(SpeedData* const speed_data)中。 ⼀、GriddedPathTimeGraph::Search(SpeedData* const speed_data) 下⾯进⾏详细的分析 ： 函数Search()的第⼀部分是⼀个for循环，遍历的是STGraph中所有的boundary ，每⼀个障碍物都会⽣成⼀个对应的boundary ， 这⾥就是将所有障碍的boundary都判断⼀遍。 for (const auto boundary : st_grap _data_ .st_ boundaries()) { // KeepClear obstacles not considered in Dp St decision if (boundary-boundary_type() == ST Boundary ::BoundaryType::KEEP_CLEAR) { continue; } // If init point in collision wit obstacle, return speed fallback if (boundary-IsPoint InBoundary({0.0, 0.0}) || (std ::fabs(boundary-min_t()) kBounadryEpsilon std ::fabs(boundary-min_s()) kBounadryEpsilon)) { dimension_t_ = static_castuint32_t(std ::ceil( total_ lengt _t_ / static_castdouble(unit_t_))) + 1; std ::vectorSpeedPoint speed_ profile; double t = 0.0; for (uint32_t i = 0; i dimension_t_ ; ++i, t += unit_t_) { SpeedPoint speed_ point ; speed_ point.set_s(0.0) ; speed_ point.set_t(t) ; speed_ point.set_v(0.0) ; speed_ point.set_a(0.0) ; speed_ profile.emplace_ back(speed_ point) ; } *speed_data = SpeedData(speed_ profile) ; return Status::OK() ;