数据流向
可以看到规划(planning)模块的上游是Localization, Prediction, Routing模块,而下游是Control模块。Routing模块先规划出一条导航线路,然后Planning模块根据这条线路做局部优化,如果Planning模块发现短期规划的线路行不通(比如前面修路,或者错过了路口),会触发Routing模块重新规划线路,因此这两个模块的数据流是双向的。
Planning模块的输入在"planning_component.h"中,接口如下:
bool Proc(const std::shared_ptr<prediction::PredictionObstacles>&
prediction_obstacles,
const std::shared_ptr<canbus::Chassis>& chassis,
const std::shared_ptr<localization::LocalizationEstimate>&
localization_estimate) override;
输入参数为:
- 预测的障碍物信息(prediction_obstacles)
- 车辆底盘(chassis)信息(车辆的速度,加速度,航向角等信息)
- 车辆当前位置(localization_estimate)
- 实际上还有高精度地图信息,不在参数中传入,而是在函数中直接读取的。
Planning模块的输出结果在"PlanningComponent::Proc()"中,为规划好的线路,发布到Control模块订阅的Topic中。
输出结果为:规划好的路径。
planning_writer_->Write(std::make_shared<ADCTrajectory>(adc_trajectory_pb));
模块注册
Planning模块的入口为"planning_component.h"和"planning_component.cc"两个文件,实现的功能如下:
// 订阅和发布消息
std::shared_ptr<cyber::Reader<perception::TrafficLightDetection>> traffic_light_reader_;
std::shared_ptr<cyber::Reader<routing::RoutingResponse>> routing_reader_;
std::shared_ptr<cyber::Reader<planning::PadMessage>> pad_message_reader_;
std::shared_ptr<cyber::Reader<relative_map::MapMsg>> relative_map_reader_;
std::shared_ptr<cyber::Writer<ADCTrajectory>> planning_writer_;
std::shared_ptr<cyber::Writer<routing::RoutingRequest>> rerouting_writer_;
// 在Cyber中注册模块
CYBER_REGISTER_COMPONENT(PlanningComponent)
这些消息的订和发布是在PlanningComponent::Init初始化的,如下:
// 发布规划好的线路
planning_writer_ = node_->CreateWriter<ADCTrajectory>(
config_.topic_config().planning_trajectory_topic());
// 发布重新规划请求
rerouting_writer_ = node_->CreateWriter<RoutingRequest>(
config_.topic_config().routing_request_topic());
planning_learning_data_writer_ = node_->CreateWriter<PlanningLearningData>(
config_.topic_config().planning_learning_data_topic());
接下来我们来看下PlanningComponent是个什么东西
是什么?
- Cyber RT以组件的方式来管理各个模块,组件的实现会基于该框架提供的基类:apollo::cyber::Component。
- Planning模块自然也不例外:planning组件,即PlanningComponent类、对象,其继承自cyber::Component<M0, M1, M2, NullType>类,可在CyberRT中完成注册工作,实现消息响应机制的托管,是工程实现和算法实现之间的桥梁。其实现的类如下:
- 从上面可以看出planning组件是一个支持三个channel输入的消息回调型组件,支持的消息类型分别如下:
- 预测的障碍物信息:prediction::PredictionObstacles
- 车辆底盘信息:canbus::Chassis,即车辆的速度,加速度,航向角等信息
- 车辆当前位置:localization::LocalizationEstimate
- cyberRT系统收到上面任何一个消息后都会调用planning组件的proc函数进行消息响应(与回调型组件对应的是定时器型组件,即cyberRT中的定时器会按照一定频率调用proc函数)
组件工作流:
模块初始化: PlanningComponent::Init
模块初始化调用了PlanningComponent::Init,初始化工作由CyberRT系统的launch命令触发.
planning_base_
- planning_base_是PlanningComponent中最重要的数据成员,实现具体的规划功能。它是planning的入口
-
PlanningBase只是一个抽象类,该类如下子类:
- NaviPlanning,车道规划
- OnLanePlanning,导航规划,主要的应用场景是开放道路的自动驾驶。
-
在PlanningComponent的实现中,会根据具体的配置选择Planning的入口。其入口是由planning_base_描述的。默认配置是OnLanePlanning
模块初始化实现2种Planning的注册:
在PlanningBase类中,下面这个方法是及其重要的:
/**
* @brief main logic of the planning module,
* runs periodically triggered by timer.
*/
void RunOnce(const LocalView& local_view,
ADCTrajectory* const trajectory_pb) override;
方法的注释已经说明得很清楚了:这是Planning模块的主体逻辑,会被timer以固定的间隔调用。每次调用就是一个规划周期。很显然,我们需要重点要关注的就是OnLanePlanning::RunOnce方法的逻辑。
重要数据成员
在PlanningComponent中,除了planning_base_之外,还有一些数据成员需要注意
LocalView
从注释中我们可以看出,LocalView包含了所有planning输入所需要的信息,它是所有输入数据的打包,包括planning_component的输入和通过reader获取的输入:
- 障碍物的预测信息
- 车辆底盘信息
- 大致定位信息
- 交通灯信息
- 导航路由信息
- 相对地图信息
组件数据缓存 DependencyInjector
- DependencyInjector:依赖注入器,这是一个过于专业的名词,来自软件设计模式的依赖倒置原则的一种具体实现方式,起到模块解耦作用。
- “依赖倒置原则(Dependence Inversion Principle)是程序要依赖于抽象接口,不要依赖于具体实现。简单的说就是要求对抽象进行编程,不要对实现进行编程,这样就降低了客户与实现模块间的耦合。”
- DependencyInjector本质上是一个数据缓存中心,叫做DataCacheCenter更为贴切,可以叫做数据缓存器
- DependencyInjector对象内部管理了planning模块工程过程中的实时数据和几乎全部历史数据,以便于规划任务的前后帧之间的承接,以及异常处理的回溯。
- DependencyInjector是以空对象的形式引入到planning组件中,进而引入到planning模块中用来承载中间数据。
DependencyInjector类结构如下:
依赖注入结构中主要有6类成员变量,分别如下:
- PlanningContext planning_context_:负责planning上下文的缓存,比如是否触发重新路由的ReroutingStatus信息
- History history_: 负责障碍物状态的缓存,包括运动状态,决策结果。该数据与routing结果绑定,routing变更后会清理掉历史数据
- FrameHistory frame_history_:是一个可索引队列,负责planning的输入,输出等主要信息的缓存,以Frame类进行组织,内部包含LocalView结构体(负责输入数据的融合管理)。与上述的History是不同的是,该缓数据自模块启动后就开始缓存所有的Frame对象,不受routing变动的影响。
- EgoInfo ego_info_:提供车辆动、静信息,即车辆运动状态参数(轨迹、速度、加速度等)和车辆结构参数(长宽高等)
- apollo::common::VehicleStateProvider vehicle_state_:车辆状态提供器,用于获取车辆实时信息
- LearningBasedData learning_based_data_:基于学习的数据,用于学习建模等
https://blog.csdn.net/weixin_39199083/article/details/124641311
小结
这里完成了planner的注册。
大致分为2步,核心代码整理截图如下
- 1、根据配置选择planning实现方式,并调用init初始化
- Planning_base_是PlanningBase的实例化对象,用来描述planning的执行过程,其中比较重要的两个函数是RunOnce和Plan,OnLanePlanning和NaviPlanning都是PlanningBase的继承类
- 默认选择的规划模式是OnLanePlanning
- 2、初始化输入输出通道
模块响应:PlanningComponent::Proc
消息响应由CyberRT在接收到关联消息到达后触发。Proc的主要是检查数据,并且执行注册好的Planning,生成路线并且发布。
bool PlanningComponent::Proc(...) {
// 1. 检查是否需要重新规划线路。
CheckRerouting();
// 2. 数据放入local_view_中,并且检查输入数据。
...
// 3. 执行注册好的Planning,生成线路。
planning_base_->RunOnce(local_view_, &adc_trajectory_pb);
// 4. 发布消息
planning_writer_->Write(std::make_shared<ADCTrajectory>(adc_trajectory_pb));
}
更进一步,核心代码整理截图如下
数据检查及更新
- (图中3-1)检查是否需要重新规划路线,Routing模块先规划出一条导航线路,然后Planning模块根据这条线路做局部优化,如果Planning模块发现局部规划的路径行不通(比如前面修路,或者错过了路口),会发送rerouting消息触发Routing模块重新规划线路。
-
(图中3-2)数据放入local_view中,数据来源包括外部传参和内部缓存数据。并对输入数据local_view进行检查,因为planning是事件触发,只有收集完所有的TOPIC的信息,才能触发执行。(最终local_view会保存到Frame对象中(在RunOnce()))
-
配置强化学习模块,进行数据预处理和数据发布
-
执行规划任务(图中4),此处会调用PlanningBase子类的RunOnce函数进行一次规划任务,生成规划后的轨迹
-
发布规划结果(图中5),利用已经创建的planning通道发送规划结果
-
更新历史缓存(图中6),将最终轨迹更新到history中(其他缓存在PlanningBase子类中完成的)以备不时之需
通过调用planning_base_->RunOnce()函数来开启决策规划之旅,目前含有两个大的规划器:navi_planning和on_lane_planning,至于选取那个,在程序初始化的时候被配置了
Planning搞起:RunOnce 结构
在这里,主要做了下图中的事:
RunOnce中使用的planner
在RunOnce执行中,调用了planner_->Plan,whose内部包含了:lattice、navi、public_road、rtk_replay四种规划器
lattice::plan
主要七个步骤:
1.将参考线转变为离散地图点
2.计算参考线上初始规划点的匹配点
3.根据匹配点计算Frenet帧的初始状态
4.解析决策,得到规划目标
5.分别生成纵向和横向一维轨迹束
6.评价:首先,根据动态约束条件对一维轨迹的可行性进行评价;其次,评估可行的纵向和横向轨迹对,并根据成本进行排序。
7.返回无碰撞的、符合条件的轨迹
如下图所示:
- 在采用Frenet坐标系后,ST和SL图极为便捷,位于第4部分实现
- 横纵向轨迹束的生成,位于第5部分,分为纵向轨迹、横向轨迹两个实现
纵向轨迹的生成
主要使用了四次和五次多项式,五次多项式可保证一阶导v、二阶导a、三阶导jerk等连续性
横向轨迹的生成
横向轨迹生成用了五次多项式和二次规划两套方法,就像图中的一样…二次规划求解我没看懂,等闲下来搞定它,只知道大致原理不能算懂~额额
轨迹的评价
对于lattice第6部分,看到很多介绍的材料里都有解释,集火在cost函数上,cost函数形式在很多论文里可以直接看到了
轨迹的挑选
在对轨迹进行筛选时,一开始没有看到对轨迹的剔除,后来才看到while循环时,不满足直接continue了…直接保证凡是执行到reference_line_info->SetTrajectory时,就意味着是成功符合的轨迹了,是在下村儿了
小结
planning组件主要完成两项任务:初始化工作Init,消息响应工作Proc
该模块的输入输出如下:
这里再重复一下,planning的输入分为Reader和Process入参的原因在于,planning依赖于Process的三个上游输入,只有同时接到这三个输入,才会触发planning的主逻辑,即是planning正常启动的必要条件。而Reader则不是,其中部分上游还依赖于配置参数是否打开,具体可以查看Apollo的源码。
planning_component.cc代码框架
https://www.codenong.com/cs106106522/