自动驾驶面经总结（二）
赞
踩
如何设计一个十字路口左转的算法，包括路径规划与决策两个部分
本文将以apollo为例设计一个十字路口左转的算法，包含从规划模块的输入、输出，具体的几个主要代码模块的讲述，还有细节的十字路口左转的算法。
1、十字路口左转算法在apollo框架的位置
上面的图片是apollo 的planning模块的整体框架，十字路口左转算法是众多scenario的一种。
1、planning 的输入输出
在Apollo CyberRT框架下，输出输出由Reader和Writer构成。CyberRT框架定义了两种模式，分别为消息触发和时间触发，而planning 中采用的消息触发，因此要接到特定的上游消息后，才会进入内部主逻辑，消息触发的上游消息，localview定义在common文件里.
planning接受的Reader输入包括traffic_light，routing,pad,relative_map, story-telling;接受的Process（）入参为Prediction,Localization,Chassis等。
planning的输出就比较简单，主要是给ADCTrajectory数据，包含里一条带时间。速度的轨迹点集。
apollo的主框架分为两个线程，子线程ReferenceLinePrivider 以20HZ的频率运行，用于计算planning中最重要的数据结构reference_line;主线程则是基于场景划分的思路，多数场景下是采用基于ReferenceLine的规划算法，对于泊车相关场景，则利用openspace算法。
2、planning中的场景管理
apollo中的规划里的一个重要思想就是基于场景划分来调用不同的task处理，而其中如何进行场景分配便是实现该思想的核心。下面的图就是scenario_manager的具体流程逻辑，可以看到scenario_manager会从Observe(frame)获取reference_line所遇到的第一个overlap的类型，从SenarioDispatch(frame)获取对有场景的判断（这里的场景会是依次判断的：ParkandGo,intersection,Pullover,ValetParking,Deadend等）。然后scenario_manager_Update()会开始scenario、stage、task这三个。
（具体来说，scenario代表当前所处的场景，包括正常的路上行驶场景、停车场景、红绿灯路口场景等。stage则是在当前场景下的阶段，例如在红绿灯路口场景下，stage可以分为等待绿灯、行驶中等。task则是在当前阶段下的任务，例如在等待绿灯阶段下，task可以分为等待时间到达、检测绿灯等）
这里重点介绍十字路口场景
首先，根据先前计算的地图第一个遇到的overlap来确定大类型是包含交通标识的交叉口，还是其他交叉口。其次，若是包含交通标识的交叉口，还细分为stop_sign、traffic_light以及yield_sign.
3、规划模块里对scenario,stage,task的理解
apollo在每个panning 周期首先决策当前处于哪个场景scenario下面的哪个状态stage当中，当确认好stage之后，便会调用这个stage的Process（）函数来执行具体的规划逻辑。
stage::Process();主要逻辑是根据配置文件，来依次执行每一个注册的task的Execute（）函数，从而把具体的规划任务分散到每个task当中。
stage不属于某个特定的scenario，task也不属于某个特定的stage，这些任务通过配置文件进行配置，这样便保证了整个规划模块的解耦和可扩展性
每个stage都要做速度决策，速度优化，路径决策，路径优化等task
4、intersection的具体执行
因为intersection有很多种情况，比如在trafiic_sign的scenario下就有protected,unprotected_left_turn,unprotected_right_turn三种情况；还有bare_intersection/unprotected,stop_sign/unprotcted这两种情况
以traffic_sign下protected场景为例，分析apollo的代码
在protected场景下可以看到有几个代码文件，一个是stage_approach,一个是stage_intersection_cruise,一个是traffic_light_protected_scenario;
首先我们看stage_approach
namespace apollo { namespace planning { namespace scenario { namespace traffic_light { struct TrafficLightProtectedContext; class TrafficLightProtectedStageApproach : public Stage { public: TrafficLightProtectedStageApproach( const ScenarioConfig::StageConfig& config, const std::shared_ptr<DependencyInjector>& injector) : Stage(config, injector) {} private: Stage::StageStatus Process(const common::TrajectoryPoint& planning_init_point, Frame* frame) override; TrafficLightProtectedContext* GetContext() { return GetContextAs<TrafficLightProtectedContext>(); } private: Stage::StageStatus FinishStage(); private: ScenarioTrafficLightProtectedConfig scenario_config_; }; } // namespace traffic_light } // namespace scenario } // namespace planning } // namespace apollo
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
这段代码是Apollo Auto中的一个交通灯保护场景的实现。TrafficLightProtectedStageApproach类是该场景中的一个阶段，用于处理车辆接近交通灯的情况。Process函数是TrafficLightProtectedStageApproach类的一个成员函数，用于处理车辆接近交通灯的情况。FinishStage函数是TrafficLightProtectedStageApproach类的另一个成员函数，用于完成该阶段。TrafficLightProtectedContext结构体是TrafficLightProtectedStageApproach类中使用的一个结构体。
然后看stage_intersection_cruise
namespace apollo { namespace planning { namespace scenario { namespace traffic_light { struct TrafficLightProtectedContext; class TrafficLightProtectedStageIntersectionCruise : public Stage { public: TrafficLightProtectedStageIntersectionCruise( const ScenarioConfig::StageConfig& config, const std::shared_ptr<DependencyInjector>& injector) : Stage(config, injector) {} private: Stage::StageStatus Process(const common::TrajectoryPoint& planning_init_point, Frame* frame) override; TrafficLightProtectedContext* GetContext() { return GetContextAs<TrafficLightProtectedContext>(); } Stage::StageStatus FinishStage(); private: ScenarioTrafficLightProtectedConfig scenario_config_; StageIntersectionCruiseImpl stage_impl_; }; } // namespace traffic_light } // namespace scenario } // namespace planning } // namespace apollo
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
这段代码是Apollo Auto中的一个交通灯保护场景的实现。TrafficLightProtectedStageIntersectionCruise类是该场景中的一个阶段，用于处理车辆在交通灯保护下通过路口的情况。Process函数是TrafficLightProtectedStageIntersectionCruise类的一个成员函数，用于处理车辆在交通灯保护下通过路口的情况。FinishStage函数是TrafficLightProtectedStageIntersectionCruise类的另一个成员函数，用于完成该阶段。TrafficLightProtectedContext结构体是TrafficLightProtectedStageIntersectionCruise类中使用的一个结构体。
最后看traffic_light_protected_scenario;
namespace apollo { namespace planning { namespace scenario { namespace traffic_light { // stage context struct TrafficLightProtectedContext { ScenarioTrafficLightProtectedConfig scenario_config; std::vector<std::string> current_traffic_light_overlap_ids; }; class TrafficLightProtectedScenario : public Scenario { public: TrafficLightProtectedScenario( const ScenarioConfig& config, const ScenarioContext* context, const std::shared_ptr<DependencyInjector>& injector) : Scenario(config, context, injector) {} void Init() override; std::unique_ptr<Stage> CreateStage( const ScenarioConfig::StageConfig& stage_config, const std::shared_ptr<DependencyInjector>& injector); TrafficLightProtectedContext* GetContext() { return &context_; } private: static void RegisterStages(); bool GetScenarioConfig(); private: static apollo::common::util::Factory< StageType, Stage, Stage* (*)(const ScenarioConfig::StageConfig& stage_config, const std::shared_ptr<DependencyInjector>& injector)> s_stage_factory_; bool init_ = false; TrafficLightProtectedContext context_; }; } // namespace traffic_light } // namespace scenario } // namespace planning } // namespace apollo
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
这段代码是Apollo Auto中的一个交通灯保护场景的实现。TrafficLightProtectedScenario类是该场景的一个实现，用于处理车辆在交通灯保护下通过路口的情况。TrafficLightProtectedContext结构体是TrafficLightProtectedScenario类中使用的一个结构体，用于存储场景中的一些信息。CreateStage函数是TrafficLightProtectedScenario类的一个成员函数，用于创建场景中的阶段。Init函数是TrafficLightProtectedScenario类的另一个成员函数，用于初始化场景。
这段代码是TrafficLightProtectedScenario类的实现，它包括了初始化函数Init()和RegisterStages()函数。Init()函数初始化了场景，并且获取了当前交通灯的信息。RegisterStages()函数注册了两个阶段：TRAFFIC_LIGHT_PROTECTED_APPROACH和TRAFFIC_LIGHT_PROTECTED_INTERSECTION_CRUISE。
这个代码片段中，traffic_light_status是一个planning_status结构体中的traffic_light成员变量，它包含了当前交通灯的信息。如果当前交通灯的id为空，那么就会输出错误信息并返回。如果当前交通灯的id不为空，那么就会将当前交通灯的id存储到context_.current_traffic_light_overlap_ids中。
这个代码片段中还使用了HDMapUtil::BaseMap().GetSignalById()函数来获取交通灯的信息。HDMapUtil是一个工具类，它提供了一些与HDMap相关的函数。GetSignalById()函数可以根据交通灯的id获取交通灯的信息。
5以一个stage为例分析stge如何注册task，以及一个场景如何工作
task的注册发生在的注册发生在stage对象的简历阶段，stage的构造函数继承了stage类的构造函数，在建立对象时读取配置文件，并对当前stage包含的task进行注册；
stage的构造函数对stage的构造函数进行继承；
stage的构造函数读取配置文件，并且注册每个task，并保存在全局变量task_list_中；
Process()函数执行主要的规划逻辑，另外一个重要的函数是PlanOnReferenceLine(),在这个函数中实现了对每个task的依次调用
下面的图是task的被调用的逻辑
6、交规决策的运行逻辑
