Apollo学习(1)—预测

前言

本文将对Apollo预测模块进行梳理，了解Apollo中进行预测的具体流程和工作原理

• 如上述架构图所示，预测处于感知与规划之间，它会对感知到的障碍物信息进行分析，并对障碍物未来一段时间内的行为及轨迹进行预测，并将信息反馈到规划模块，使规划模块能够生成更加合理、安全的轨迹

我会根据每个不同的文件，来对Apollo预测模块进行解读

1. ReadMe

预测模块中的ReadMe出了该模块的输入、输出和功能

• 输入: 障碍物信息、定位信息
• 输出：具有预测轨迹的障碍物
• 功能: 预测模块实现预测轨迹输出的一些组件,由容器、评估器和预测器构成。
  

2.prdiction_component

在predictiond模块中，可以看到prdiction_component，其会对该模块进行相关初始化操作

// 如果需要，则对该模块下相关数据进行初始化
bool Init() override;
//init中主要对相关Manager对象以及向cyber中的reader和writer进行对应初始化
// 接收感知模块传来的障碍信息进行处理
bool Proc(const std::shared_ptr<perception::PerceptionObstacles>&) override;
//头文件中同时设置了四个管理器对象
  std::shared_ptr<ContainerManager> container_manager_;//容器

  std::unique_ptr<EvaluatorManager> evaluator_manager_;//评估

  std::unique_ptr<PredictorManager> predictor_manager_;//预测

  std::unique_ptr<ScenarioManager> scenario_manager_;//场景

//...还有cyber相关的reader和writer...
 std::shared_ptr<cyber::Reader<planning::ADCTrajectory>> planning_reader_;//读取规划信息（作用未知）

  std::shared_ptr<cyber::Reader<localization::LocalizationEstimate>>
      localization_reader_;//读取位置信息（作用未知）

  std::shared_ptr<cyber::Reader<storytelling::Stories>> storytelling_reader_;//storytelling？

2.1 Proc

• 该函数会对接收到的信息进行处理，对信息处理这里使用了两种情况：ContainerSubmoduleProcess和PredictionEndToEndProc
  这里主要对PredictionEndToEndProc进行解读

//1.对位置信息进行获取（本车位置信息？）
localization_reader_->Observe();//observer()调用的node中的数据
auto ptr_localization_msg = localization_reader_->GetLatestObserved();//获取最新的位置信息（获取message对列中前面的数据块）
  MessageProcess::OnLocalization(container_manager_.get(),
                                 *ptr_localization_msg);

//2. 读取storytelling模块信息（stroytell作用？）
storytelling_reader_->Observe();
  auto ptr_storytelling_msg = storytelling_reader_->GetLatestObserved();//同理
//读取最后一帧规划信息（但规划需要先有预测？）
  if (ptr_storytelling_msg != nullptr) {
    MessageProcess::OnStoryTelling(container_manager_.get(),*ptr_storytelling_msg);
  }
planning_reader_->Observe();
  auto ptr_trajectory_msg = planning_reader_->GetLatestObserved();
if (ptr_trajectory_msg != nullptr) {
    MessageProcess::OnPlanning(container_manager_.get(), *ptr_trajectory_msg);
  }
//3. 读取感知的障碍物信息
auto perception_msg = *perception_obstacles;
PredictionObstacles prediction_obstacles;
 MessageProcess::OnPerception(
      perception_msg, container_manager_, evaluator_manager_.get(),
      predictor_manager_.get(), scenario_manager_.get(), &prediction_obstacles);
//在得到prediction_obstacles后，会有一些后续处理

• 在对信息进行处理过程中，可以发现都是从localization_reader、storytelling_reader_、planning_reader_中读取信息，并且交由MessageProcess类进行处理，MessageProcess处理就是将这些信息存放到对应的容器中去，最后调用MessageProcess::OnPerception，其中会使用评估器、预测器来对获取的信息进行处理
  

3.MessageProcess

• PredictionEndToEndProc对数据进行获取，其中各个reader读取最新的消息，这些消息读取后存储至对应的容器中去。MessageProcess会对不同容器进行管理，同时调用OnPerception进行处理。下面则是MessageProcess的头文件

  //这里会对容器、评估器、预测器进行初始化
  static bool Init(ContainerManager *container_manager,
                   EvaluatorManager *evaluator_manager,
                   PredictorManager *predictor_manager,
                   const PredictionConf &prediction_conf);
  static bool InitContainers(ContainerManager *container_manager);

  static bool InitEvaluators(EvaluatorManager *evaluator_manager,
                             const PredictionConf &prediction_conf);

  static bool InitPredictors(PredictorManager *predictor_manager,
                             const PredictionConf &prediction_conf);
  //通过获取的容器、评估器、预测器、场景对障碍物信息进行处理（主要）
  static void OnPerception(
      const perception::PerceptionObstacles &perception_obstacles,
      const std::shared_ptr<ContainerManager> &container_manager,
      EvaluatorManager *evaluator_manager, PredictorManager *predictor_manager,
      ScenarioManager *scenario_manager,
      PredictionObstacles *const prediction_obstacles);

  //将获取的相关信息，存储到对应容器中去，例如localization信息，存到PoseContainer中（在Prediction中存储了多个容器，来存储不同对象的信息）
  static void OnLocalization(
      ContainerManager *container_manager,
      const localization::LocalizationEstimate &localization);

  static void OnPlanning(ContainerManager *container_manager,
                         const planning::ADCTrajectory &adc_trajectory);

  static void OnStoryTelling(ContainerManager *container_manager,
                             const storytelling::Stories &story);

• MessageProcess中OnPerception是对获取的感知数据进行处理，并进行相关的评估、预测

//对容器信息进行处理
//这里会将container_manager中的相关信息进行处理，其中一个就是将scenario_manager中的信息提取，每个障碍物中会存储其对应的场景信息
ContainerProcess(container_manager, perception_obstacles, scenario_manager);
//将障碍物信息获取
  auto ptr_obstacles_container =
      container_manager->GetContainer<ObstaclesContainer>(
          AdapterConfig::PERCEPTION_OBSTACLES);
  CHECK_NOTNULL(ptr_obstacles_container);

//获取规划曲线容器(规划怎么结合呢？)
  auto ptr_ego_trajectory_container =
      container_manager->GetContainer<ADCTrajectoryContainer>(
          AdapterConfig::PLANNING_TRAJECTORY);
  CHECK_NOTNULL(ptr_ego_trajectory_container);

//评估器运行（obstacles信息和规划信息）
evaluator_manager->Run(ptr_ego_trajectory_container,ptr_obstacles_container);
//预测器运行
predictor_manager->Run(perception_obstacles, ptr_ego_trajectory_container,ptr_obstacles_container);
//将结果返回，prediction_obstacles传进来的就是指针，相当于返回了
*prediction_obstacles = predictor_manager->prediction_obstacles();

在OnPerception将容器的中的相关信息再做一个处理，然后通过获取obstacles、planing的相关信息进行评估和预测

4.Evaluator

• 评估器中会将不同障碍物处理进行处理，并根据障碍物类型、所处场景来返回对应的评估器对其进行评估。同样我们通过代码进行了解

//这里选择三个函数进行了解
//根据配置文件对评估器进行相关评估器
void Init(const PredictionConf& config);
//评估器接收信息，进行调用的接口，messageprocess调用的该接口
void Run(const ADCTrajectoryContainer* adc_trajectory_container,
           ObstaclesContainer* obstacles_container){
  if (FLAGS_enable_multi_thread) {
    IdObstacleListMap id_obstacle_map;//获得障碍物的列表？
    GroupObstaclesByObstacleIds(obstacles_container, &id_obstacle_map);
    //下面开辟预测线程，来对障碍物进行评估
    PredictionThreadPool::ForEach(
        id_obstacle_map.begin(), id_obstacle_map.end(),
        [&](IdObstacleListMap::iterator::value_type& obstacles_iter) {
          for (auto obstacle_ptr : obstacles_iter.second) {
            EvaluateObstacle(adc_trajectory_container, obstacle_ptr,
                             obstacles_container, dynamic_env);//这里感觉就在评估，获取相对应评估器
            //这里进行评估的时候，用了轨迹规划容器
          }
        });
  }
}
//这里是对每个障碍物进行检测的地方
//这里在进行评估时，就是一个switch了，函数对障碍物的类型、状态和所处场景
void EvaluateObstacle(const ADCTrajectoryContainer* adc_trajectory_container,
                        Obstacle* obstacle,
                        ObstaclesContainer* obstacles_container,
                        std::vector<Obstacle*> dynamic_env){
//直接进行判定
switch (obstacle->type()){
//Apollo中对障碍物分为了三类：轿车、自行车和行人，以及一个默认类型
//其中对Vehicle类型判定是最多的
    case PerceptionObstacle::VEHICLE:{
//对车辆的状态评估，在对其所处环境进行评估，然后获取对应的评估器进行评估
      if (obstacle->IsCaution() && !obstacle->IsSlow()) {
        if (obstacle->IsInteractiveObstacle()) {
          evaluator = GetEvaluator(interaction_evaluator_);//获取对应类型的评估
        } else if (obstacle->IsNearJunction()) {
          evaluator = GetEvaluator(vehicle_in_junction_caution_evaluator_);
        } else if (obstacle->IsOnLane()) {
          evaluator = GetEvaluator(vehicle_on_lane_caution_evaluator_);
        } else {
          evaluator = GetEvaluator(vehicle_default_caution_evaluator_);
        }
//...
if (evaluator->Evaluate(obstacle, obstacles_container)) {
            break;//进行相关条件检测后则进行评估
          } else {
            AERROR << "Obstacle: " << obstacle->id()
                  << " caution evaluator failed, downgrade to normal level!";
          }
    }
    }
    case PerceptionObstacle::BICYCLE: {
    }
    case PerceptionObstacle::PEDESTRIAN:{
    }
    default: {
    }
}

• evaluator->Evaluate(obstacle, obstacles_container))
  Evaluate是一个虚函数，在EvaluatorManager中存在多个不同的评估器，这些评估器都会继承Evaluate类并重写Evaluate,所以这里就是调用对应评估函数进行评估。
• EvaluatorManager头文件中定义了多个评估器，如下

    //评估器类型：vehicle_on_lane
  ObstacleConf::EvaluatorType vehicle_on_lane_evaluator_ =
      ObstacleConf::CRUISE_MLP_EVALUATOR;//MLP

    // 评估器：vehicle_on_lane_caution
  ObstacleConf::EvaluatorType vehicle_on_lane_caution_evaluator_ =
      ObstacleConf::CRUISE_MLP_EVALUATOR;//MLP

    // 评估器：vehicle_in_junction
  ObstacleConf::EvaluatorType vehicle_in_junction_evaluator_ =
      ObstacleConf::JUNCTION_MLP_EVALUATOR;//MLP

    // 评估器：vehicle_on_junction_caution
  ObstacleConf::EvaluatorType vehicle_in_junction_caution_evaluator_ =
      ObstacleConf::JUNCTION_MAP_EVALUATOR;//MAP

    // 评估器：vehicle_defaut_caution评估器
  ObstacleConf::EvaluatorType vehicle_default_caution_evaluator_ =
      ObstacleConf::SEMANTIC_LSTM_EVALUATOR;//LSTM

    // 自行车_on_lanep 评估器
  ObstacleConf::EvaluatorType cyclist_on_lane_evaluator_ =
      ObstacleConf::CYCLIST_KEEP_LANE_EVALUATOR;//keep_lane保持车道

    // 行人评估器
  ObstacleConf::EvaluatorType pedestrian_evaluator_ =
      ObstacleConf::SEMANTIC_LSTM_EVALUATOR;//LSTM

    // vectornet 评估器
  ObstacleConf::EvaluatorType vectornet_evaluator_ =
      ObstacleConf::VECTORNET_EVALUATOR;//vectornet
    // on_lane 评估器
  ObstacleConf::EvaluatorType default_on_lane_evaluator_ =
      ObstacleConf::MLP_EVALUATOR;

    // 交互评估器
  ObstacleConf::EvaluatorType interaction_evaluator_ =
      ObstacleConf::JOINTLY_PREDICTION_PLANNING_EVALUATOR;

Apollo中对评估器做出过解释：评估器对任何给定的障碍分别预测路径和速度。评估器通过使用存储在prediction/data/模型中的给定模型输出路径的概率来评估路径（车道序列）。

• 将提供三种类型的评估器，包括：
  • 成本评估器：概率是由一组成本函数计算的。
  • MLP评估器：用MLP模型计算概率
  • RNN评估器：用RNN模型计算概率
  • 预测规划交互评估器:vectornet（论文：https://arxiv.org/pdf/2005.04259v1.pdf）

预测器

预测器中得到评估器计算得到的概率，生成该障碍物的轨迹，在Apollo中有所总结：

• 预测器生成障碍物的预测轨迹。当前支持的预测器包括：
  • 空：障碍物没有预测的轨迹
  • 单行道：在公路导航模式下障碍物沿着单条车道移动。不在车道上的障碍物将被忽略。
  • 车道顺序：障碍物沿车道移动
  • 移动序列：障碍物沿其运动模式沿车道移动
  • 自由运动：障碍物自由移动
  • 区域运动：障碍物在可能的区域中移动