【Apollo源码分析】系列的第六部分【planning】

最新推荐文章于 2026-02-11 11:15:05 发布
原创 最新推荐文章于 2026-02-11 11:15:05 发布 · 置顶 · 1w 阅读 · 50 ·
CC 4.0 BY-SA版权
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。

本文详述Apollo源码中的planning模块，分析其输入输出、主要类和功能。RTKReplayPlanner从历史轨迹中选取点进行回放，但目前planning模块仅能处理历史数据，无法进行实际的未来规划。

【Apollo源码分析】系列的第六部分【planning】

planning模块 代码大概有1000行左右。 这个模块仍然处于待完善的状态。 
和其他模块一样，这里只有框架，除了 RTKReplayPlanner，没有实用的算法。

planning模块的输入：

  • Localization
  • Perception (future work)
  • Prediction (future work)
  • Decision (future work)
  • Recorded RTK trajectory

planning模块的输出：

  • trajectory point

planning/main.cc

照例先看看main.cc

 
  1. int main(int argc, char **argv) {
  2.   google::InitGoogleLogging(argv[0]);
  3.   google::ParseCommandLineFlags(&argc, &argv, true);
  5.   ros::init(argc, argv, "planning");
  7.   ::apollo::planning::PlanningNode planning_node;
  8.   planning_node.Run();
  10.   return 0;
  11. }
  12. //与#define APOLLO_MAIN(APP) 大同小异。可能后期会改为一致。

planning/common/planning_gflags.h

.h文件和.cc文件，声明和定义planning模块的变量。 
主要的变量是：

 
  1. #include "modules/planning/common/planning_gflags.h"
  3. //发布运动规划planning的频率，单位是HZ。
  4. DEFINE_int32(planning_loop_rate, 5, "Loop rate for planning node"); 
  6. //用RTK定位算法得到的最近一段时间的历史轨迹
  7. DEFINE_string(rtk_trajectory_filename, "modules/planning/data/garage.csv",
  8.               "Loop rate for planning node");
  10. //planning时，用于回退匹配的轨迹节点数量。
  11. DEFINE_uint64(rtk_trajectory_backward, 10,
  12.               "The number of points to be included in RTK trajectory "
  13.               "before the matched point");
  14. //前向匹配的轨迹节点数量
  15. DEFINE_uint64(rtk_trajectory_forward, 800,
  16.               "The number of points to be included in RTK trajectory "
  17.               "after the matched point");
  18. //重新规划planning的阈值，高于此值则重新planning
  19. DEFINE_double(replanning_threshold, 2.0,
  20.               "The threshold of position deviation "
  21.               "that triggers the planner replanning");
  22. //轨迹点之间的时间分辨率/间隔，0.01s
  23. DEFINE_double(trajectory_resolution, 0.01,
  24.               "The time resolution of "
  25.               "output trajectory.");

planning/planner/planner.h

Planner是纯虚基类。有一个公有的Plan()： 
纯虚函数。继承它的子类必须重写这个纯虚函数才能实例化。 
所有的路径规划的class都继承自这个类。 
目的：虚函数指针，运行时决断，父类指针指向子类对象，runtime时调用子类的成员函数。

具体到Planner 而言就是提供一个公有的接口 virtual bool Plan(). 
使得在运行时可以执行不同的planning算法。

 
  1. 参数1:planning起始点
  2. 参数2：路径规划结果集，由一系列离散的轨迹点组成。
  4. 目的：根据历史行驶的一系列轨迹节点，
  5. 并结合Perception模块+Prediction模块+Decision模块+
  6. Localization地图定位模块，来进行推算未来一段时间的行驶轨迹。
  7. 本质上是路径规划算法。
  8.   virtual bool Plan(
  9.       const apollo::common::TrajectoryPoint &start_point,
  10.       std::vector<apollo::common::TrajectoryPoint> *discretized_trajectory) = 0;

planning/planner/rtk_replay_planner.h

RTKReplayPlanner继承自虚基类Planner。 
这个类通过从历史轨迹的file文件中读取轨迹点， 
并且从这些历史轨迹点中选择连续的800个point组成历史轨迹的结果集合， 
输出到ptr_trajectory中。

目的：从历史信息中提取与start_point相关联的轨迹point集合， 
为下一个具体的planner类进行路径规划做数据准备。

 
  1. // 构造函数直接读取历史轨迹节点文件file中的内容.并存储到complete_rtk_trajectory_数据成员中
  2. RTKReplayPlanner();
 
  2. /*
  3. 参数1:planning起始点
  4. 参数2：路径规划的结果集，800个连续的point存储在ptr_trajectory中，由一系列离散的轨迹点组成。
  5. ptr_trajectory存储的是历史轨迹节点而不是planning好的未来的轨迹节点。
  6. 故本class 命名为ReplayPlanner.
  8. 算法的大概步骤：
  9. 【1】找到与起点start_point最近的轨迹中的点的index。即在历史数据里找到一个点，然后把它看做start_point的近似。
  10. 【2】在match至轨迹的end之间找到一个end_index，使得end_index不越界，同时又尽可能的长。
  11. 【3】将区间[first,last)的元素赋值到在【ptr_discretized】vector容器中
  12. 【4】修改在ptr_discretized_trajectory轨迹集合中的相对时间。以第一个point为0起始点。
  13. 【5】如果point没有800个点，则直接用最后的一个点填充直到满足800个点。
  14. */
  15. bool Plan(const apollo::common::TrajectoryPoint &start_point,
  16. std::vector<apollo::common::TrajectoryPoint> *ptr_trajectory) override;
 
  1. ////打开轨迹文件，并将轨迹point存储到complete_rtk_trajectory_成员函数中。
  2. void ReadTrajectoryFile(const std::string &filename);

file内容举例：

csv格式每行的header信息： 
x,y,z,speed,acceleration,curvature,curvature_change_rate,time,theta,gear,s,throttle,brake,steering

具体的值： 
586385.858607, 4140674.7357, -28.3670201628, 0.216666668653, 0.887545286694, 0.0227670812611, -0.0177396744278, 1496957374.6140, 2.83470068837, 1, 0.00216666668653, 22.0157165527, 13.6934461594, 12.6382980347

 
  1. /*
  2. 参数1：planning起始点
  3. 参数2：历史轨迹
  4. 返回值：距离start_point的以L2距离度量的最近邻轨迹点。
  5. 步骤：
  6. 【1】求轨迹的点与start_point的距离的平方距离的最小值
  7. 【2】遍历轨迹点，找到相对于起点的最短距离点。
  8. 【3】返回最短距离点的index
  9. */
  10.   std::size_t QueryPositionMatchedPoint(
  11.       const apollo::common::TrajectoryPoint &start_point,
  12.       const std::vector<apollo::common::TrajectoryPoint> &trajectory) const;

测试代码:

 
  2. TEST_F(RTKReplayPlannerTest, ComputeTrajectory) {
  3.   FLAGS_rtk_trajectory_filename = "modules/planning/testdata/garage.csv";
  4.   RTKReplayPlanner planner;
  6.   TrajectoryPoint start_point;
  7.   start_point.set_x(586385.782842);
  8.   start_point.set_y(4140674.76063);
  10.   std::vector<TrajectoryPoint> trajectory;
  11.   bool planning_succeeded = planner.Plan(start_point, &trajectory);
  13.   EXPECT_TRUE(planning_succeeded);
  14.   EXPECT_TRUE(!trajectory.empty());
  15.   //800个点。不够时用最后一个点填充。
  16.   EXPECT_EQ(trajectory.size(), (std::size_t)FLAGS_rtk_trajectory_forward);
  18.   auto first_point = trajectory.front();
  19.   EXPECT_DOUBLE_EQ(first_point.x(), 586385.782841);//第一个匹配点，.csv第20个
  20.   EXPECT_DOUBLE_EQ(first_point.y(), 4140674.76065);
  22.   auto last_point = trajectory.back();
  23.   EXPECT_DOUBLE_EQ(last_point.x(), 586355.063786);//最后一个匹配点，.csv第819个。
  24.   EXPECT_DOUBLE_EQ(last_point.y(), 4140681.98605);
  25. }
 
  1. TEST_F(RTKReplayPlannerTest, ErrorTest) {
  2.   FLAGS_rtk_trajectory_filename =
  3.       "modules/planning/testdata/garage_no_file.csv";
  4.   RTKReplayPlanner planner;
  5.   FLAGS_rtk_trajectory_filename = "modules/planning/testdata/garage_error.csv";
  6.   RTKReplayPlanner planner_with_error_csv;//file不完整，只有1行数据时，直接返回false
  7.   TrajectoryPoint start_point;
  8.   start_point.set_x(586385.782842);
  9.   start_point.set_y(4140674.76063);
  10.   std::vector<TrajectoryPoint> trajectory;
  11.   EXPECT_TRUE(!planner_with_error_csv.Plan(start_point, &trajectory));
  12. }

planning/planner_factory.h

 
  2. // PlannerType指定可选的路径规划class类型。
  3. // 目前Apollo只支持由RTK组成的plan路径规划方法，其他方法还没有实现。
  4. enum class PlannerType { RTK_PLANNER, OTHER };
 
  1. // 工厂方法模式，没有数据成员，只有一个静态成员函数CreateInstance()
  2. class PlannerFactory {
  3.  public:
  4.   PlannerFactory() = delete;
  5. // 参数：路径规划方法。目前只支持PlannerType::RTK_PLANNER类型。
  6.   static std::unique_ptr<Planner> CreateInstance(
  7.       const PlannerType &planner_type);
  8. };

CreateInstance()实现代码：

 
  1. //  参数：planning路径规划的算法类型
  2. //  返回值：planning路径规划实例对象。
  3. std::unique_ptr<Planner> PlannerFactory::CreateInstance(
  4.     const PlannerType &planner_type) {
  5.   switch (planner_type) {
  6.     case PlannerType::RTK_PLANNER:
  7.     // new 一个实例对象。
  8.       return std::unique_ptr<Planner>(new RTKReplayPlanner());
  9.     //这里可见其他类型的规划方法为空指针实现。
  10.     default:
  11.       return nullptr;
  12.   }
  13. }

planning/planning.h

class Planning 是上层的路径规划方法封装。 
数据成员：

 
  1. std::unique_ptr<Planner> ptr_planner_;//由工厂方法模式创建的路径规划实例。
  3. std::vector<common::TrajectoryPoint> last_trajectory_; //最近一段时间planning的轨迹point集合
  5. double last_header_time_ = 0.0;//最近planning时间

私有成员函数：

 
  1. /*
  2. 计算从上次【预测的】轨迹记录历史得到的起始点的车辆自身point位置
  3. 参数:当前时间
  4. 返回值：轨迹point+索引
  5. 算法步骤：
  6. 【1】二分查找，找到【start_time】小于【轨迹point的time+last_header】的第一个值。
  7. 【2】返回point+index
  9. */
  10. std::pair<common::TrajectoryPoint, std::size_t>
  11.   ComputeStartingPointFromLastTrajectory(const double curr_time) const;
 
  2. /*计算车辆启动时的地理位置所在的轨迹point。
  3. 参数1:当前车辆状态(车辆速度，加速度，行驶方向等)
  4. 参数2：planning的预测时间段。
  5. 算法步骤：
  6. 【1】获取当前车辆的位置x,y,z
  7. 【2】获取当前车辆的速度，加速度信息
  8. 【3】设置point曲率
  9. 【4】设置转弯半径
  10. 【5】返回当前轨迹point
  11. */
  12. common::TrajectoryPoint ComputeStartingPointFromVehicleState(
  13.       const common::vehicle_state::VehicleState &vehicle_state,
  14.       const double forward_time) const;
 
  1. /*
  2. 参数1：与起始点match的轨迹点
  3. 参数2：需要回退的size，正常情况下回退10个轨迹point
  4. 返回值：在match之前的10个轨迹point
  5.   */
  6. std::vector<common::TrajectoryPoint> GetOverheadTrajectory(
  7.       const std::size_t matched_index, const std::size_t buffer_size);

公有成员函数：

 
  1. /*
  2.    从历史信息+目前的车辆状态(车辆速度，加速度，行驶方向等)进行路径规划，
  3.    结果存储在discretized_trajectory中。
  4.    参数1：目前车辆状态信息
  5.    参数2:是否自动驾驶模式
  6.    参数3:路径规划发布时间
  7.    参数4:路径规划的轨迹point结果集合
  8.  */
  9.   bool Plan(const common::vehicle_state::VehicleState &vehicle_state,
  10.             const bool is_on_auto_mode, const double publish_time,
  11.             std::vector<common::TrajectoryPoint> *discretized_trajectory);
  12.  /*
  13.  算法步骤：
  14.  如果是自动驾驶模式，并且【预测】历史轨迹不为空(已有历史预测信息)
  15. 那么:
  16. 【1】从【预测的】历史轨迹中寻找与当前时间匹配的point
  17. 【2】计算匹配点与当前点的距离的平方差
  18. 【3】若小于阈值(2.0m)，则不需要重新进行路径规划,直接从匹配点开始路径规划.
  19. 【4】将last_traj修改为*planning_traj以用于下一次路径规划
  21. 若非自动驾驶模式/没有历史轨迹点/匹配点距离太远：
  22. 【6】从当前车辆状态获取point位置，然后再从当前位置进行路径规划。
  23. 【7】保存预测轨迹，然后用于下一次预测
  24.   */
 
  1. // 重置为初始状态。清空轨迹point的信息。
  2. void Reset();

看到这里读者肯定以为planning模块的路径规划算法已经完成了。有这种看法非常不错，但是我认为他读懂了这个代码的50%。

实际情况是在 Planning类的Plan()成员函数中有这么一句话(而且是2次出现)：

 
  1.     bool planning_succeeded =
  2.           ptr_planner_->Plan(matched_point, planning_trajectory);
  3.       if (!planning_succeeded) {
  4.         last_trajectory_.clear();
  5.         return false;
  6.       }
  7. 以上代码的含义对不熟悉c++的人来说是有一点困难的。
  8. 理解这段代码需要了解3个知识：
  9. 【1】面向对象设计
  10. 【2】继承，封装，多态
  11. 【3】设计模式

planning/planning_node.h

 
  1. PlanningNode是Apollo的 planning模块与ROS通信的Node封装。
  3. class PlanningNode {
  4.  public:
  5.   /*  构造函数，初始化配置文件*/
  6.   PlanningNode();
  8.   /*  析构函数，什么也不做*/
  9.   virtual ~PlanningNode();
  11.   /* 在ROS中按照设置的频率(默认5hz),定时发布路径规划结果。多次调用RunOnce() 。  */
  12.   void Run();
  14.   /*复位，清空，调用Planning类的Reset()成员函数*/
  15.   void Reset();
  17.  private:
  19. //进行一次planning路径规划算法
  20.   void RunOnce();
  22. //序列化轨迹轨迹point到文件
  23.   ADCTrajectory ToTrajectoryPb(
  24.       const double header_time,
  25.       const std::vector<common::TrajectoryPoint> &discretized_trajectory);
  27. //进行路径规划的实例对象      
  28.   Planning planning_;

总结

planning模块分析就到这里了。废话不多说了。 
回顾一下：

planning模块的输入：

  • Localization
  • Perception (future work)
  • Prediction (future work)
  • Decision (future work)
  • Recorded RTK trajectory

planning模块的输出：

  • trajectory point

一句话：

目前的planning模块除了能处理由 RTK记录的历史轨迹point 数据，啥也没干不了。【只能回顾过去】，【不能规划未来】。

而基于【过去和现在】并【规划未来】才是planning模块的应有之意。

本文首发于微信公众号slamcode。


 注释版源码：源码




Apollo平台入门：从源码编译到Demo运行
不踩坑，不知道坑有多深；不总结，踩过的坑白踩。
11-14 15万+
Apollo平台入门：从源码编译到Demo运行 ，人工智能，计算机视觉，大模型，AI，本文是Apollo平台入门指南，先介绍其作为开源自动驾驶平台的特点与应用场景，接着讲解环境准备，包括硬件要求、操作系统选择和依赖软件安装，还说明了源码获取与编译的步骤及可能遇到的问题。随后阐述了Demo运行方法、核心概念，以及开发者工具、模块调试、进阶学习方向和相关资源，涵盖从基础入门到深入学习的多方面内容，为新手提供全面指导。
Apollo9.0 PNC源码学习之Control模块（一）—— 控制模块概览
BigDavid123的博客
06-10 3570
本文主要讲解了Apollo的控制模块，看完会对控制模块有一个清晰的认识
Apollo：planning_gflags.h
OceanStar的博客
03-06 478
【代码】Apollo：planning_gflags.h。
Apollo：循迹规划器RTKReplayPlanner
OceanStar的博客
03-06 837
RTK Replay Planner 是基于录制的轨迹进行循迹的 Planner，是 Apollo 比较原始的一种 Planner。需要事先录制好设定的轨迹再来规划行车路线。录制的轨迹文件格式可以参考 modules/planning/data/garage.csv。
apollo使用的局部规划算法
最新发布
小墨迹的博客
02-11 1419
Apollo 局部轨迹规划是规则优化派的工业标杆双主干：EM Planner（Public Road）主稳定，Lattice 主机动底层范式：Frenet 坐标系 + 路径-速度解耦 + DP 粗搜 + QP 精修版本定位：3.5+ Public Road Planner 是量产绝对主力，Lattice 做机动补充行业地位：乘用车/商用车量产首选，矿区矿卡可直接复用架构，仅需动力学与场景定制Apollo EM Planner 适配矿卡的改造参数与约束清单Apollo 局部规划在矿区的。
百度Apollo5.0规划模块代码学习（一）RTK规划器
wwyklnh的博客
08-03 5305
一 前言 关于百度Apollo中的规划模块，我也是第一次看。希望通过这个平台做好笔记。其中部分内容如果有误，希望看到的人可以给我留言。 规划模块的架构及介绍可以参考以下文章，写的非常好： https://www.cnblogs.com/liuzubing/p/11058612.html https://blog.csdn.net/shixiaolu63/article/details/865047...
apollo7.0——规划代码解析
xiaohu的博客
12-15 2259
于根据当前车辆状态信息和上一条轨迹，计算出拼接后的轨迹，并返回拼接后的轨迹。
Apollo9.0 Lattice Planner算法源码学习
Fengdf666的博客
03-15 1796
【代码】Apollo9.0 Lattice Planner算法源码学习。
Apollo：目录分析， test ok
小硕算法工程师
07-30 742
同时，交通信号灯检测也依赖于其他模块提供的数据，包含定位模块数据，高精地图数据(信号灯实时检测是不可行的或者说是不合理不必要的，原因在于太过耗费计算资源，实际只需要定位与高精度地图模块提供相应的位置信息，指示感知模块在什么时间段与路面段做信号灯检测)，E.g.路口需要启动信号灯检测，中间路段只需要障碍物检测，不存在信号灯。与模块启动相关的配置，默认建图文件是dag/planning.dag，内部实现模块节点间的通讯配置，构建DAG(有向无环图)，dag中再设置了config中的相关配置文件。
Apollo9.0 PNC源码学习之Control模块（三）—— 基于双环PID的纵向控制
BigDavid123的博客
06-12 1279
本文对百度Apollo9.0PNC纵向控制的源码做了剖析和注释方便读者理解
《从0开始搭建实现apollo9.0》系列七 规划模块解读
li1886477130的博客
02-28 3950
apollo9.0的planning的代码解析。
Apollo决策规划 - EM planner
BigDavid123的博客
03-08 2783
文章详细讲解了EM planner的理论，包括规划需要的背景知识🍎：五次多项式、凸优化和非凸优化、直角坐标与自然坐标转换，参考线🍎：平滑算法、避障(规划起点，轨迹拼接)，自动驾驶的轻决策和重决策🍎，二次规划带来的问题，SL、ST如何迭代求解
Apollo学习笔记（二）：循迹实现过程
zhengkunxian的博客
11-05 5372
Apollo学习笔记（二）：循迹过程 Apollo开发者套件买来后，就可以按照官网循迹教程录制轨迹，开始循迹。在此主要对Apollo循迹的实现过程进行学习。
Apollo 10.0 Public Road Planner 详解（Planning代码学习）（五）
ring12345677的博客
01-10 2045
速度规划的参考系是st坐标系，这里的s是沿着规划好的路径方向的位移，t是时间。动态规划：通过把原问题分解为相对简单的子问题，再根据子问题的解求解原问题的方法。 有向权重图，采用递推方法逐步求解： f(P)=min⁡{f(R)+wR→P}f(P)=\min\left\{f(R)+w_{R\to P}\right\}f(P)=min{f(R)+wR→P​} Apollo速度规划中，动态规划主要用来进行速度决策的判定，使用采样搜索算法： 对st图采样为栅格图，在t的方向以固定间隔进行采样，Apollo中默认设置
解析百度Apollo自动驾驶平台
weixin_33697898的博客
06-25 3232
最近对百度的自动驾驶平台Apollo项目做了一些了解。下面将我所了解到的一些信息分享给大家。 Apollo项目介绍 阿波罗（Apollo）是百度发布的面向汽车行业及自动驾驶领域的合作伙伴提供的软件平台。发布时间是2017年4月19日，旨在向汽车行业及自动驾驶领域的合作伙伴提供一个开放、完整、安全的软件平台，帮助他们结合车辆和硬件系统，快速搭建一套属于自己...
Apollo -- Planning (二) 框架设计
qhr_1012的博客
07-18 613
框架设计
Apollo 2.0 框架及源码分析(一) | 软硬件框架
黎国溥
07-19 4032
原文地址：https://zhuanlan.zhihu.com/p/33059132 前言 如引言中介绍的，这篇软硬件框架多为现有消息的整合加一些个人的想法。关于 Apollo 介绍的文章已经有许多，很多要点大家都已经足够了解，这些点在本篇中会被略写。本篇文章如有错误、不足之处，还请大家多多指教。 Apollo 官方 GitHub 链接：Apollo Auto 在历经1.0，1.5 ...
Apollo Planning2.0决策规划算法代码详细解析 (3)：PlanningComponent框架介绍
自动驾驶Player的博客
09-28 793
PlanningComponent是自动驾驶系统中负责决策规划的组件。它基于来自其他模块（如预测、底盘和定位等）的输入信息，计算出车辆在未来一段时间内的行驶轨迹、速度、加速度等关键参数。这些规划结果将直接指导车辆的实际驾驶操作，确保车辆能够安全、高效地行驶。PlanningComponent是一个事件触发的组件，即它不会持续运行，而是等待特定的事件或条件触发时才开始工作。这些事件通常包括同时收到预测、底盘和定位信息等。这种设计有助于减少不必要的计算资源消耗，并提高系统的响应速度和效率。
百度Apollo规划决策模块学习记录-3 Apollo EM Planner算法
热门推荐
NEO's NOTEBOOK
08-09 1万+
目录Lecture 6约束的分类Apollo EM Planner优化问题的关键EM Planner的主要步骤Apollo 无人车规划模块进展 Lecture 6 约束的分类 Soft Constraints Decisions Best Trajectory Hard Constraints Traffic Regulations Apollo EM Planner 这部分在EM Planner的论文中更细致，课程里主要概括了一些核心思路但并不完整。之后有空的话把论文笔记整理出来.
评论 1
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值