0.前置知识

1.涉及主要函数

1) 函数名: PathTimeGraph( const std::vector<const Obstacle*>& obstacles, const std::vector< PathPoint >& discretized_ref_points, const ReferenceLineInfo* ptr_reference_line_info, const double s_start, const double s_end, const double t_start, const double t_end, const std::array<double, 3>& init_d)
            函数位置: modules/planning/lattice/behavior/path_time_graph.cc
            函数作用: ST/SL图类的构造函数
        2) 函数名: SetupObstacles( const std::vector<const Obstacle*>& obstacles, const std::vector< PathPoint >& discretized_ref_points)
            函数位置: modules/planning/lattice/behavior/path_time_graph.cc
            函数作用: 构建障碍物的ST/SL图
        3) 函数名: SetStaticObstacle( const Obstacle* obstacle, const std::vector< PathPoint >& discretized_ref_points)
            函数位置: modules/planning/lattice/behavior/path_time_graph.cc
            函数作用: 构建静态障碍物的SL图
        4) 函数名: ComputeObstacleBoundary( const std::vector< common::math::Vec2d >& vertices, const std::vector< PathPoint >& discretized_ref_points)
            函数位置: modules/planning/lattice/behavior/path_time_graph.cc
            函数作用: 计算障碍物SL边框
        5) 函数名: GetPathFrenetCoordinate( const std::vector& reference_line, const double x, const double y)
            函数位置: modules/common/math/path_matcher.cc
            函数作用: 计算障碍物当前点在sl坐标系中的sl_point
        6) 函数名: SetDynamicObstacle( const Obstacle* obstacle, const std::vector< PathPoint >& discretized_ref_points)
            函数位置: modules/common/math/path_matcher.cc
            函数作用: 构建动态障碍物的ST图
        7) 函数名: GetPointAtTime( const double relative_time)
            函数位置: modules/planning/common/obstacle.cc
            函数作用: 获得障碍物在当前时刻的TrajectoryPoint
        8) 函数名: GetBoundingBox( const common::TrajectoryPoint& point)
            函数位置: modules/planning/common/obstacle.cc
            函数作用: 获得障碍物在当前时刻的TrajectoryPoint
        9) 函数名: Box2d(const Vec2d &center, const double heading, const double length, const double width)
            函数位置: modules/common/math/box2d.cc
            函数作用: Box2d构造函数
        10) 函数名: InitCorners()
            函数位置: modules/common/math/box2d.cc
            函数作用: 计算Box2d四个角点的笛卡尔坐标
        11) 函数名: GetSpeedLimitFromS(const double s)
            函数位置: modules/planning/reference_line/reference_line.cc
            函数作用: 跟据当前s值返回速度限制值
        12) 函数名: GetReferencePoint(const double s)
            函数位置: modules/planning/reference_line/reference_line.cc
            函数作用: 跟据当前s值返回参考线上的ReferencePoint
        13) 函数名: InterpolateWithMatchedIndex( const ReferencePoint& p0, const double s0, const ReferencePoint& p1, const double s1, const InterpolatedIndex& index)
            函数位置: modules/planning/reference_line/reference_line.cc
            函数作用: 线性插值找ReferencePoint

2.函数关系

3.部分函数代码详解

3.1 lattice_planner.cc中代码部分

  // 通过预测得到障碍物listauto ptr_prediction_querier = std::make_shared<PredictionQuerier>(frame->obstacles(), ptr_reference_line);// 解析决策,计算障碍物的s-t图// 4. parse the decision and get the planning target.// 传入obs,参考线,参考线信息,自车当前点的s值,sl坐标系下规划终点的s值,轨迹的t,自车当前点的l相关值;// 传入这么多构建一个PathTimeGraph对象,会跟据障碍物类型生成对应的ST图auto ptr_path_time_graph = std::make_shared<PathTimeGraph>(ptr_prediction_querier->GetObstacles(), *ptr_reference_line,reference_line_info, init_s[0],init_s[0] + FLAGS_speed_lon_decision_horizon, 0.0,  // 200mFLAGS_trajectory_time_length, init_d);  // 8s// 跟据init_s[0]寻找规划时的速度限制double speed_limit =reference_line_info->reference_line().GetSpeedLimitFromS(init_s[0]);// 设定lattice算法的巡航速度reference_line_info->SetLatticeCruiseSpeed(speed_limit);// planning_target来自高精地图PlanningTarget planning_target = reference_line_info->planning_target();if (planning_target.has_stop_point()) {ADEBUG << "Planning target stop s: " << planning_target.stop_point().s()<< "Current ego s: " << init_s[0];}ADEBUG << "Decision_Time = "<< (Clock::NowInSeconds() - current_time) * 1000;current_time = Clock::NowInSeconds();

3.2 函数-GetReferencePoint

代码部分:

ReferencePoint ReferenceLine::GetReferencePoint(const double s) const {const auto& accumulated_s = map_path_.accumulated_s();// 如果s在参考线的后面，就返回参考线的第一个点if (s < accumulated_s.front() - 1e-2) {AWARN << "The requested s: " << s << " < 0.";return reference_points_.front();}// 如果s在参考线的前面，就返回参考线的最后一个点if (s > accumulated_s.back() + 1e-2) {AWARN << "The requested s: " << s<< " > reference line length: " << accumulated_s.back();return reference_points_.back();}// 根据当前s值找到参考线上最近的点auto interpolate_index = map_path_.GetIndexFromS(s);size_t index = interpolate_index.id;size_t next_index = index + 1;if (next_index >= reference_points_.size()) {next_index = reference_points_.size() - 1;}// 下面就是用线性插值找到更精确的点const auto& p0 = reference_points_[index];const auto& p1 = reference_points_[next_index];const double s0 = accumulated_s[index];const double s1 = accumulated_s[next_index];return InterpolateWithMatchedIndex(p0, s0, p1, s1, interpolate_index);
}

3.3 函数-GetSpeedLimitFromS

代码部分:

double ReferenceLine::GetSpeedLimitFromS(const double s) const {// 有速度限制段直接返回速度限制，该段的速度限制貌似是从高精地图获取的 for (const auto& speed_limit : speed_limit_) {if (s >= speed_limit.start_s && s <= speed_limit.end_s) {return speed_limit.speed_limit;}}const auto& map_path_point = GetReferencePoint(s);double speed_limit = FLAGS_planning_upper_speed_limit;  // 31.3bool speed_limit_found = false;// 循环更新speed_limit的最小值for (const auto& lane_waypoint : map_path_point.lane_waypoints()) {if (lane_waypoint.lane == nullptr) {AWARN << "lane_waypoint.lane is nullptr.";continue;}speed_limit_found = true;speed_limit =std::fmin(lane_waypoint.lane->lane().speed_limit(), speed_limit);}// 如果速度限制没有找到，则通过判断当前道路的类型，赋值不同道路类型默认的速度限制值if (!speed_limit_found) {// use default speed limit based on road_typespeed_limit = FLAGS_default_city_road_speed_limit;  // 15.67hdmap::Road::Type road_type = GetRoadType(s);if (road_type == hdmap::Road::HIGHWAY) {speed_limit = FLAGS_default_highway_speed_limit;  // 29.06}}return speed_limit;
}

3.4 函数-InitCorners

图解:

            代码部分:

void Box2d::InitCorners() {const double dx1 = cos_heading_ * half_length_;const double dy1 = sin_heading_ * half_length_;const double dx2 = sin_heading_ * half_width_;const double dy2 = -cos_heading_ * half_width_;corners_.clear();// 插入障碍物box四个角点的坐标corners_.emplace_back(center_.x() + dx1 + dx2, center_.y() + dy1 + dy2);corners_.emplace_back(center_.x() + dx1 - dx2, center_.y() + dy1 - dy2);corners_.emplace_back(center_.x() - dx1 - dx2, center_.y() - dy1 - dy2);corners_.emplace_back(center_.x() - dx1 + dx2, center_.y() - dy1 + dy2);for (auto &corner : corners_) {max_x_ = std::fmax(corner.x(), max_x_);min_x_ = std::fmin(corner.x(), min_x_);max_y_ = std::fmax(corner.y(), max_y_);min_y_ = std::fmin(corner.y(), min_y_);}
}

3.5 函数-Box2d

图解:

            代码部分:

Box2d::Box2d(const Vec2d &center, const double heading, const double length,const double width): center_(center),length_(length),width_(width),half_length_(length / 2.0),half_width_(width / 2.0),heading_(heading),cos_heading_(cos(heading)),sin_heading_(sin(heading)) {CHECK_GT(length_, -kMathEpsilon);CHECK_GT(width_, -kMathEpsilon);InitCorners();
}

3.6 函数-GetPointAtTime

代码部分:

common::TrajectoryPoint Obstacle::GetPointAtTime(const double relative_time) const {const auto& points = trajectory_.trajectory_point();if (points.size() < 2) {  // 认为是静态障碍物common::TrajectoryPoint point;point.mutable_path_point()->set_x(perception_obstacle_.position().x());point.mutable_path_point()->set_y(perception_obstacle_.position().y());point.mutable_path_point()->set_z(perception_obstacle_.position().z());point.mutable_path_point()->set_theta(perception_obstacle_.theta());point.mutable_path_point()->set_s(0.0);point.mutable_path_point()->set_kappa(0.0);point.mutable_path_point()->set_dkappa(0.0);point.mutable_path_point()->set_ddkappa(0.0);point.set_v(0.0);point.set_a(0.0);point.set_relative_time(0.0);return point;} else {  // 认为是一个运动的障碍物auto comp = [](const common::TrajectoryPoint p, const double time) {return p.relative_time() < time;};auto it_lower =std::lower_bound(points.begin(), points.end(), relative_time, comp);if (it_lower == points.begin()) {return *points.begin();} else if (it_lower == points.end()) {return *points.rbegin();}return common::math::InterpolateUsingLinearApproximation(*(it_lower - 1), *it_lower, relative_time);}
}

3.7 函数-GetPathFrenetCoordinate

代码部分:

std::pair<double, double> PathMatcher::GetPathFrenetCoordinate(const std::vector<PathPoint>& reference_line, const double x,const double y) {// 找到障碍物x,y最近的参考线上的点auto matched_path_point = MatchToPath(reference_line, x, y);// 参考线点的headingdouble rtheta = matched_path_point.theta();// 参考线点的xdouble rx = matched_path_point.x();// 参考线点的ydouble ry = matched_path_point.y();double delta_x = x - rx;double delta_y = y - ry;// Tr X Nrdouble side = std::cos(rtheta) * delta_y - std::sin(rtheta) * delta_x;std::pair<double, double> relative_coordinate;// 参考线点在参考线上的总长,即当前障碍物点在sl坐标系的s值relative_coordinate.first = matched_path_point.s();// 当前障碍物点在sl坐标系的l值(带正负)relative_coordinate.second =std::copysign(std::hypot(delta_x, delta_y), side);return relative_coordinate;
}

3.8 函数-ComputeObstacleBoundary

代码部分:

// 计算障碍物边框
SLBoundary PathTimeGraph::ComputeObstacleBoundary(const std::vector<common::math::Vec2d>& vertices,const std::vector<PathPoint>& discretized_ref_points) const {// 初始化操作double start_s(std::numeric_limits<double>::max());double end_s(std::numeric_limits<double>::lowest());double start_l(std::numeric_limits<double>::max());double end_l(std::numeric_limits<double>::lowest());for (const auto& point : vertices) {// 获得障碍物当前点在sl坐标系中的sl_pointauto sl_point = PathMatcher::GetPathFrenetCoordinate(discretized_ref_points,point.x(), point.y());// 通过下面的操作，可以使得最后获得sl的start和endstart_s = std::fmin(start_s, sl_point.first);end_s = std::fmax(end_s, sl_point.first);start_l = std::fmin(start_l, sl_point.second);end_l = std::fmax(end_l, sl_point.second);}/// The start_s and end_s are longitudinal values.// start_s <= end_s.////              end_s//                ^//                |//          S  direction//                |//            start_s//// The start_l and end_l are lateral values.// start_l <= end_l. Left side of the reference line is positive,// and right side of the reference line is negative.//  end_l  <-----L direction---- start_l/// 得到障碍物的在SL坐标系中的boundarySLBoundary sl_boundary;sl_boundary.set_start_s(start_s);sl_boundary.set_end_s(end_s);sl_boundary.set_start_l(start_l);sl_boundary.set_end_l(end_l);return sl_boundary;
}

3.9 函数-SetStaticObstacle

图解:

            代码部分:

void PathTimeGraph::SetStaticObstacle(const Obstacle* obstacle,const std::vector<PathPoint>& discretized_ref_points) {// 获得障碍物边框const Polygon2d& polygon = obstacle->PerceptionPolygon();// 障碍物IDstd::string obstacle_id = obstacle->Id();// 得到障碍物的在SL坐标系中的boundarySLBoundary sl_boundary =ComputeObstacleBoundary(polygon.GetAllVertices(), discretized_ref_points);// 宽度 = 参考线宽度的一半double left_width = FLAGS_default_reference_line_width * 0.5;  // 4mdouble right_width = FLAGS_default_reference_line_width * 0.5;// ptr_reference_line_info_->reference_line().GetLaneWidth(sl_boundary.start_s(), &left_width, &right_width);// 忽略一些车道之外和参考线范围之外的障碍物if (sl_boundary.start_s() > path_range_.second ||  // 即障碍物所在位置超出参考线结束端sl_boundary.end_s() < path_range_.first ||  // 即障碍物所在位置还没进入参考线起始端sl_boundary.start_l() > left_width ||  // 即障碍物所在位置在左侧车道之外sl_boundary.end_l() < -right_width) {  // 即障碍物所在位置在右侧车道之外ADEBUG << "Obstacle [" << obstacle_id << "] is out of range.";return;}// 障碍物IDpath_time_obstacle_map_[obstacle_id].set_id(obstacle_id);// 障碍物SL图的左下角点（start_s,0）path_time_obstacle_map_[obstacle_id].set_bottom_left_point(SetPathTimePoint(obstacle_id, sl_boundary.start_s(), 0.0));// 障碍物SL图的右下角点（start_s,8）path_time_obstacle_map_[obstacle_id].set_bottom_right_point(SetPathTimePoint(obstacle_id, sl_boundary.start_s(), FLAGS_trajectory_time_length));// 障碍物SL图的左上角点（end_s,0）path_time_obstacle_map_[obstacle_id].set_upper_left_point(SetPathTimePoint(obstacle_id, sl_boundary.end_s(), 0.0));// 障碍物SL图的右上角点（end_s,8）path_time_obstacle_map_[obstacle_id].set_upper_right_point(SetPathTimePoint(obstacle_id, sl_boundary.end_s(), FLAGS_trajectory_time_length));static_obs_sl_boundaries_.push_back(std::move(sl_boundary));ADEBUG << "ST-Graph mapping static obstacle: " << obstacle_id<< ", start_s : " << sl_boundary.start_s()<< ", end_s : " << sl_boundary.end_s()<< ", start_l : " << sl_boundary.start_l()<< ", end_l : " << sl_boundary.end_l();
}

3.10 函数-SetDynamicObstacle

图解:

            代码部分:

void PathTimeGraph::SetDynamicObstacle(const Obstacle* obstacle,const std::vector<PathPoint>& discretized_ref_points) {double relative_time = time_range_.first;  // 0s// 循环遍历轨迹中的每一个点，每个点画一个ST图，最后把所有ST图组合起来，形成动态障碍物的ST图while (relative_time < time_range_.second) {  // 8s// 获得障碍物轨迹点TrajectoryPoint point = obstacle->GetPointAtTime(relative_time);Box2d box = obstacle->GetBoundingBox(point);SLBoundary sl_boundary =ComputeObstacleBoundary(box.GetAllCorners(), discretized_ref_points);double left_width = FLAGS_default_reference_line_width * 0.5;double right_width = FLAGS_default_reference_line_width * 0.5;ptr_reference_line_info_->reference_line().GetLaneWidth(sl_boundary.start_s(), &left_width, &right_width);// The obstacle is not shown on the region to be considered.if (sl_boundary.start_s() > path_range_.second ||sl_boundary.end_s() < path_range_.first ||sl_boundary.start_l() > left_width ||sl_boundary.end_l() < -right_width) {// 如果该障碍物的ID在障碍物的map里，说明该障碍物已经处理过了，直接退出当前while循环if (path_time_obstacle_map_.find(obstacle->Id()) !=path_time_obstacle_map_.end()) {break;}relative_time += FLAGS_trajectory_time_resolution;continue;}// 如果该障碍物的ID不在障碍物的map里，添加相关信息if (path_time_obstacle_map_.find(obstacle->Id()) ==path_time_obstacle_map_.end()) {// 记录idpath_time_obstacle_map_[obstacle->Id()].set_id(obstacle->Id());// 这个if里记录的是ST图的左边边框,如果是第一次注册的话,因为第一次注册需要知道是从哪里开始的// 记录左下path_time_obstacle_map_[obstacle->Id()].set_bottom_left_point(SetPathTimePoint(obstacle->Id(), sl_boundary.start_s(),relative_time));// 记录左上path_time_obstacle_map_[obstacle->Id()].set_upper_left_point(SetPathTimePoint(obstacle->Id(), sl_boundary.end_s(), relative_time));}// 这里会每次都更新障碍物ST图的右侧边界,等于说每次都更新障碍物在ST图上的终点,即结束// 右下path_time_obstacle_map_[obstacle->Id()].set_bottom_right_point(SetPathTimePoint(obstacle->Id(), sl_boundary.start_s(), relative_time));// 右上path_time_obstacle_map_[obstacle->Id()].set_upper_right_point(SetPathTimePoint(obstacle->Id(), sl_boundary.end_s(), relative_time));relative_time += FLAGS_trajectory_time_resolution;}
}

3.11 函数-SetupObstacles

代码部分:

// 实际上是在做障碍物的ST/SL图
void PathTimeGraph::SetupObstacles(const std::vector<const Obstacle*>& obstacles,const std::vector<PathPoint>& discretized_ref_points) {// 遍历所有的障碍物，去生成对应的ST图for (const Obstacle* obstacle : obstacles) {// 判断是不是虚拟的obsif (obstacle->IsVirtual()) {continue;}// 通过判断障碍物是否有轨迹来判断是 静态障碍物 还是 动态障碍物if (!obstacle->HasTrajectory()) {SetStaticObstacle(obstacle, discretized_ref_points);} else {SetDynamicObstacle(obstacle, discretized_ref_points);}}// 静态障碍物按start_s从小到大排列，并没有动态目标的信息（时刻在变没有存储意义）std::sort(static_obs_sl_boundaries_.begin(), static_obs_sl_boundaries_.end(),[](const SLBoundary& sl0, const SLBoundary& sl1) {return sl0.start_s() < sl1.start_s();});// 所有障碍物的ST信息则全部压入path_time_obstacles_中实现存储。for (auto& path_time_obstacle : path_time_obstacle_map_) {path_time_obstacles_.push_back(path_time_obstacle.second);}
}

4.参考资料

1.参考视频：Lattice.
         2.参考资料：Lattice Planner从学习到放弃（一）.额不…到实践.

Apollo6.0代码Lattice算法详解——Part4:计算障碍物ST/SL图相关推荐

1. Apollo6.0代码Lattice算法详解——Part5: 生成横纵向轨迹

   Apollo6.0代码Lattice算法详解--Part5: 生成横纵向轨迹 0.前置知识 1.涉及主要函数 2.函数关系 3.部分函数代码详解 3.1 lattice_planner.cc中代码部分 ...

2. Apollo6.0代码Lattice算法详解——Part6:轨迹评估及碰撞检测对象构建

   Apollo6.0代码Lattice算法详解--Part6:轨迹评估及碰撞检测对象构建 0.前置知识 1.涉及主要函数 2.函数关系 3.部分函数代码详解 3.1 lattice_planner.cc ...

3. 数据结构与算法详解（含算法分析、动图图解、Java代码实现、注释解析）

   数据结构和算法的重要性 算法是程序的灵魂,优秀的程序可以在海量数据计算时,依然保持高速计算 数据结构和算法的关系: 程序 = 数据结构 + 算法 数据结构是算法的基础, 换言之,想要学好算法,需要把数 ...

4. KMP算法详解及代码

   KMP算法详解及代码 KMP算法详解及代码 定义及应用 理论 基本概念 next 数组 总结 注意 代码 KMP算法详解及代码 最近正好在看字符串相关的算法内容,就顺便把KMP算法回顾了一下.相应的代 ...

5. 数学建模——主成分分析算法详解Python代码

   数学建模--主成分分析算法详解Python代码 import matplotlib.pyplot as plt #加载matplotlib用于数据的可视化 from sklearn.decomposi ...

6. C语言实现扫雷完整算法详解~（附完整代码~）

   扫雷是一个常见小游戏,那么如何用C语言实现扫雷呢?学习了二维数组之后,我们可将扫雷的网格区域存储为二维数组,从而使用C语言实现扫雷. 目录 1.算法基本思路 2.算法详解 1.初始化数组与打印数组 2 ...

7. 基于多相滤波器的数字信道化算法详解(Python, Verilog代码已开源)

   基于多相滤波器的数字信道化算法详解 推导过程 总结 仿真 本文详细介绍了基于多相滤波器的数字信道化算法的推导过程, 如果您在阅读的过程中发现算法推导过程中有任何错误, 请不吝指出. 此外, 进入我的G ...

8. Matlab中的FCM算法代码及中文详解

   Matlab中的FCM算法代码及中文详解 转自:http://xiaozu.renren.com/xiaozu/106512/336681453 function [center, U, obj_fc ...

9. 相位 unwrap 与 wrap 算法详解（附代码）

   相位 unwrap 与 wrap 算法详解(附代码) 最近接手了一个项目,光通信方面的,我负责编写初测结果的数据处理算法,其中有一个算法叫做 unwrap 与 wrap,之前没有听说过.通过询问同事与 ...

最新文章

1. 用机器学习还原《隐秘的角落》那些被修改的台词
2. jq 实现发送验证码倒计时功能
3. drawContours函数
4. leetcode算法题--删除链表的节点
5. 【USACO】奶牛抗议 树状数组+dp
6. 结束python服务器进程_服务器端后台持续执行python程序小demo
7. pat 乙级 1032 挖掘机技术哪家强（C++）
8. 《Spring3.0就这么简单》
9. java验证xml格式是否正确的是_spring源码附录（1）java实现对XML格式的验证
10. PHP接收到的$_POST数据里的邮箱符号@变成了%40
11. Postman 设置token为全局变量
12. 台式机dp接口_2K至4K显示器性价比线材选购攻略，毕亚兹 DP高清线入手
13. ESP8266 wifi模块开发汇总
14. [实战] 图片转素描图
15. 8.线性回归之非线性回归
16. 相干接收机matlab仿真,基于DP-16QAM的400Gbit/s集成相干接收机关键技术研究
17. css中表格内容从顶部开始,CSS粘性定位固定表格thead部分元素小方法
18. glm/glm.hpp_从GLM到GBM（第2部分）
19. matlab 中trace,trace命令和tracert命令的用途 matlab中的trace 怎么用的？
20. 大智慧、飞狐、分析家、通达信、钱龙、麒麟公式编辑教程大全

热门文章

1. 阿里云Apsara Clouder专项技能认证-实现调用API接口-学习笔记
2. 套接字socket选项TCP_NODELAY、TCP_CORK与TCP_QUICKACK
3. 英语的动态句型与静态句型
4. Android-圆形头像
5. A1,A2,A3,A4纸的尺寸大小|A4纸与分辨率的关系|像素换算|ABC号纸尺寸|纸张幅面规格
6. A session ended very soon after starting. Check that the command in profile解决方法
7. 蓄电池01——充电电流及时间计算
8. c++_-nan(ind) NAN
9. 卡尔曼滤波的原理与思想
10. 最全的PDF转换成Word方法，搞定市面上99%的PDF