Apollo开源代码链接：https://github.com/ApolloAuto/apollo
本文主要讲解Apollo/modules/planning中的路径规划的架构和算法。
第一章 架构设计与实现
一、架构设计与实现
引自：https://github.com/ApolloAuto/apollo/blob/master/modules/planning/README_cn.md
Apollo FSM（finite state machine）:一个有限状态机，与高清地图确定车辆状态给定其位置和路线。
----------------------------------------------------------------------------------------------------------------------------------------
Planning模块整体框架：
1. 可执行程序层： Planning Dispatcher根据车辆的状态和其他相关信息，调用合适的Planner。Planner实现获取所需的上下文数据和其他信息，确定相应的车辆意图，执行该意图所需的规划任务并生成规划轨迹。它还将更新未来作业的上下文。根据工作模式不同将规划任务分成不同的Planner模式（LatticePlanner\NaviPlanner\PublicRoadPlanner\RTKReplayPlanner）。
2.app层：每种Planner分成多个场景Scenario（BareIntersectionUnprotectedScenario、EmergencyPullOverScenario、EmergencyStopScenario、LaneFollowScenario、TestLearningModelScenario、NarrowStreetUTurnScenario、PullOverScenario、ValetParkingScenario、ParkAndGoScenario、StopSignUnprotectedScenario、TrafficLightProtectedScenario、TrafficLightUnprotectedLeftTurnScenario、TrafficLightUnprotectedRightTurnScenario、YieldSignScenario）-》每个场景又分解成多个Stage(例如ValetParkingScenario包含StageApproachingParkingSpot和StageParking两个Stage)
3.lib层：每个Stage又分解成可执行的Task(包括不同的Deciders & Optimizers库)，Task是Deciders & Optimizers :一组实现决策任务和各种优化的无状态库。优化器特别优化车辆的轨迹和速度。决策者是基于规则的分类决策者，他们建议何时换车道、何时停车、何时爬行(慢速行进)或爬行何时完成。
-------------------------------------------------------------------------------------------------------------------------------------------
Planning Context: 作业的上下文。
这种 可执行程序层-》app层 -》lib层 的三层分级架构实质与视觉感知模块类似。
本文路径规划模块架构与视觉感知模块架构（ https://blog.csdn.net/Cxiazaiyu/article/details/106256330 ）区别：
视觉感知模块中更像是自下而上地设计出这种三层结构的，因此，我们也自下而上地讲解了这个结构；
路径规划模块更像是自上而下设计出这种架构的，因此，我们也自上而下地讲解这个结构。
视觉感知模块命名更直白。
路径规划模块把接口分散放入各个层的模块中了，没有集中放在一个文件夹。
1. 可执行程序层--planning/planner
定义抽象基类PlannerDispatcher，包含了Planner类型的数据成员；
class PlannerDispatcher {
public:
virtual std::unique_ptr<Planner> DispatchPlanner() = 0;
protected:
common::util::Factory<PlannerType, Planner> planner_factory_;
};NaviPlannerDispatcher和OnLanePlannerDispatcher继承自PlannerDispatcher，覆盖了抽象基类中的virtual std::unique_ptr<Planner> DispatchPlanner()方法，实现通过工厂模式创建配置文件中指定的Planner对象。
以 NaviPlannerDispatcher为例：
std::unique_ptr<Planner> NaviPlannerDispatcher::DispatchPlanner() {
PlanningConfig planning_config;
if (!apollo::cyber::common::GetProtoFromFile(FLAGS_planning_config_file,
&planning_config)) {
return nullptr;
}
auto planner_type = PlannerType::NAVI;
if (planning_config.has_navigation_planning_config()) {
planner_type = planning_config.navigation_planning_config().planner_type(0);
}
return planner_factory_.CreateObject(planner_type);
}Planner为定义的基类接口，Planner中包含了scenario。
class Planner {
protected:
scenario::ScenarioManager scenario_manager_;
scenario::Scenario* scenario_ = nullptr;
};PlannerWithReferenceLine继承自 Planner，做了一层抽象：
class PlannerWithReferenceLine : public Planner {};根据不同的工作模式定义了4种Planner：
class LatticePlanner : public PlannerWithReferenceLine {};
/**
* @class NaviPlanner
* @brief NaviPlanner is a planner based on real-time relative maps. It uses the
* vehicle's FLU (Front-Left-Up) coordinate system to accomplish tasks such as
* cruising, following, overtaking, nudging, changing lanes and stopping.
* Note that NaviPlanner is only used in navigation mode (turn on navigation
* mode by setting "FLAGS_use_navigation_mode" to "true") and do not use it in
* standard mode.
*/
class NaviPlanner : public PlannerWithReferenceLine {};
/**
* @class PublicRoadPlanner
* @brief PublicRoadPlanner is an expectation maximization planner.
*/
class PublicRoadPlanner : public PlannerWithReferenceLine{};
/**
* @class RTKReplayPlanner
* @brief RTKReplayPlanner is a derived class of Planner.
* It reads a recorded trajectory from a trajectory file and
* outputs proper segment of the trajectory according to vehicle
* position.
*/
class RTKReplayPlanner : public PlannerWithReferenceLine{};2. app层--planning/scenarios
前面讲到Planner中包含scenario::ScenarioManager scenario_manager_和scenario::Scenario* scenario_ = nullptr数据成员，
class Scenario {
public:
/**
* Each scenario should define its own stages object's creation
* scenario will call stage's Stage::Process function following a configured
* order, The return value of Stage::Process function determines the
* transition from one stage to another.
*/
virtual std::unique_ptr<Stage> CreateStage(
const ScenarioConfig::StageConfig& stage_config) = 0;
// Each scenario should define its own transfer condition, i.e., when it
// should allow to transfer from other scenario to itself.
virtual bool IsTransferable(const Scenario& other_scenario,
const Frame& frame) {
return true;
}
protected:
std::unique_ptr<Stage> current_stage_;
};基于scenario派生出BareIntersectionUnprotectedScenario、EmergencyPullOverScenario、EmergencyStopScenario、LaneFollowScenario、TestLearningModelScenario、NarrowStreetUTurnScenario、PullOverScenario、ValetParkingScenario、ParkAndGoScenario、StopSignUnprotectedScenario、TrafficLightProtectedScenario、TrafficLightUnprotectedLeftTurnScenario、TrafficLightUnprotectedRightTurnScenario、YieldSignScenario；
Scenario类型中又包含Stage类型的数据成员。一个Scenario分解成多个Stage，不同Stage可能是基于不同规划算法实现的。例如：泊车场景中（Apollo/modules/planning/scenarios/park/valet_parking），划分成了StageApproachingParkingSpot和StageParking两个Stage。
Stage中包含Task类型的数据成员，依次执行Tasklilst上的Task，相当于将多个lib串联起来实现一定功能的app：
class Stage {
public:
/**
* @brief Each stage does its business logic inside Process function.
* If the stage want to transit to a different stage after finish,
* it should set the type of 'next_stage_'.
*/
virtual StageStatus Process(
const common::TrajectoryPoint& planning_init_point, Frame* frame) = 0;
/**
* @brief The sequence of tasks inside the stage. These tasks usually will be
* executed in order.
*/
const std::vector<Task*>& TaskList() const { return task_list_; }
protected:
std::map<TaskConfig::TaskType, std::unique_ptr<Task>> tasks_;
std::vector<Task*> task_list_;
};3. lib层--planning/tasks
决策任务deciders和各种优化optimizers的无状态库，都是基于Task基类派生。
决策者是基于规则的分类决策者，他们建议何时换车道、何时停车、何时爬行(慢速行进)或爬行何时完成。
class Decider : public Task {};Decider又派生出：CreepDecider、LaneChangeDecider、OpenSpaceFallbackDecider、OpenSpacePreStopDecider、OpenSpaceRoiDecider、PathAssessmentDecider、PathBoundsDecider、PathDecider、PathLaneBorrowDecider、PathReuseDecider、RssDecider、RuleBasedStopDecider、SpeedBoundsDecider、SpeedLimitDecider、SpeedDecider、STBoundsDecider
优化器特别优化车辆的轨迹和速度。
class PathOptimizer : public Task {};
class SpeedOptimizer : public Task {};
class TrajectoryOptimizer : public Task{};具体包括：
class OpenSpaceTrajectoryProvider : public TrajectoryOptimizer{};
class OpenSpaceTrajectoryPartition : public TrajectoryOptimizer{};
class PathTimeHeuristicOptimizer : public SpeedOptimizer {};
class PiecewiseJerkPathOptimizer : public PathOptimizer{};
class PiecewiseJerkSpeedNonlinearOptimizer : public SpeedOptimizer{};
class PiecewiseJerkSpeedOptimizer : public SpeedOptimizer{};第二章 设计模式
一、简单工厂模式
Apollo/modules/planning/scenarios/scenario_manager.cc
std::unique_ptr<Scenario> ScenarioManager::CreateScenario(
ScenarioConfig::ScenarioType scenario_type) {
std::unique_ptr<Scenario> ptr;
switch (scenario_type) {
case ScenarioConfig::BARE_INTERSECTION_UNPROTECTED:
ptr.reset(
new scenario::bare_intersection::BareIntersectionUnprotectedScenario(
config_map_[scenario_type], &scenario_context_));
break;
case ScenarioConfig::EMERGENCY_PULL_OVER:
ptr.reset(new emergency_pull_over::EmergencyPullOverScenario(
config_map_[scenario_type], &scenario_context_));
break;
case ScenarioConfig::EMERGENCY_STOP:
ptr.reset(new emergency_stop::EmergencyStopScenario(
config_map_[scenario_type], &scenario_context_));
break;
case ScenarioConfig::LANE_FOLLOW:
ptr.reset(new lane_follow::LaneFollowScenario(config_map_[scenario_type],
&scenario_context_));
break;
case ScenarioConfig::PARK_AND_GO:
ptr.reset(new scenario::park_and_go::ParkAndGoScenario(
config_map_[scenario_type], &scenario_context_));
break;
case ScenarioConfig::PULL_OVER:
ptr.reset(new scenario::pull_over::PullOverScenario(
config_map_[scenario_type], &scenario_context_));
break;
case ScenarioConfig::STOP_SIGN_UNPROTECTED:
ptr.reset(new scenario::stop_sign::StopSignUnprotectedScenario(
config_map_[scenario_type], &scenario_context_));
break;
case ScenarioConfig::TEST_LEARNING_MODEL:
ptr.reset(new scenario::TestLearningModelScenario(
config_map_[scenario_type], &scenario_context_));
break;
case ScenarioConfig::TRAFFIC_LIGHT_PROTECTED:
ptr.reset(new scenario::traffic_light::TrafficLightProtectedScenario(
config_map_[scenario_type], &scenario_context_));
break;
case ScenarioConfig::TRAFFIC_LIGHT_UNPROTECTED_LEFT_TURN:
ptr.reset(
new scenario::traffic_light::TrafficLightUnprotectedLeftTurnScenario(
config_map_[scenario_type], &scenario_context_));
break;
case ScenarioConfig::TRAFFIC_LIGHT_UNPROTECTED_RIGHT_TURN:
ptr.reset(
new scenario::traffic_light::TrafficLightUnprotectedRightTurnScenario(
config_map_[scenario_type], &scenario_context_));
break;
case ScenarioConfig::VALET_PARKING:
ptr.reset(new scenario::valet_parking::ValetParkingScenario(
config_map_[scenario_type], &scenario_context_));
break;
case ScenarioConfig::YIELD_SIGN:
ptr.reset(new scenario::yield_sign::YieldSignScenario(
config_map_[scenario_type], &scenario_context_));
break;
default:
return nullptr;
}
if (ptr != nullptr) {
ptr->Init();
}
return ptr;与视觉感知模块（https://blog.csdn.net/Cxiazaiyu/article/details/106256330）中使用的简单工厂模式类似，这里不再赘述。
二、工厂方法模式（基于类模板）
工厂方法模式：调用抽象工厂的接口，返回抽象产品的指针；根据输入配置参数不同，在抽象工厂接口内部调用相应的具体工厂，生产相应的具体产品，并将具体产品的指针绑定在抽象产品的指针上作为返回值。Register解决的就是实现“根据输入配置参数不同，在抽象工厂接口内部调用相应的具体工厂”，方法是建立一个map，根据配置参数名称映射到相应的具体工厂。
路径规划模块（Apollo/modules/planning）主要使用基于类模板实现工厂方法模式创建对象。包括在planner_dispatcher中创建planner对象（ Apollo/modules/planning/planner/planner_dispatcher.h中定义 common::util::Factory<PlannerType, Planner> planner_factory_;）,在scenarios中创建stage对象（Apollo/modules/planning/scenarios/stop_sign/unprotected/stop_sign_unprotected_scenario.cc中定义的 static apollo::common::util::Factory<ScenarioConfig::StageType, Stage, Stage* (*)(const ScenarioConfig::StageConfig& stage_config)> s_stage_factory_;），在stage中创建task(Apollo/modules/planning/scenarios/stage.cc中调用auto ptr = TaskFactory::CreateTask(*config_map[task_type]);)。
2.1 UML结构
参与者如下（这里以task_factory_一条应用链路为例，planner_factory_的链路与之类似）：
| 参与者 | 作用 | 在Apollo中的示例 |
| 抽象工厂 | 类模板，创建对象调用的接口和Register的接口 | 由Factory（factory.h中定义）实例化出的task_factory_ |
| 具体工厂 | 实例化抽象工厂的类模板，负责生产具体产品。将抽象工厂的指针绑定在具体工厂指针上，根据需要调用相应的具体工厂以生产相应的具体产品 | task_factory_中输入的creator（这里是匿名函数的形式） |
| 抽象产品 | 抽象工厂返回类型为抽象产品的指针 | Task |
| 具体产品 | 绑定在基类（抽象产品）指针上的具体派生类指针 | LaneChangeDecider、SpeedBoundsDecider、OpenSpaceRoiDecider （Decider和PathOptimizer是根据逻辑需要在抽象产品和具体产品之间抽象的一层） |
注意：相比于一般只能创建一种抽象产品的工厂方法模式，这里设计的工厂方法模式还支持创建不同的抽象产品（基于类模板，把抽象产品类型作为参数）。
2.2 实现方法
2.2.1 抽象工厂 & 抽象产品 、 map & 创建对象的接口
- 类模板的定义
Apollo/modules/common/util/factory.h 中Factory类模板提供了注册和创建对象的接口。
template <typename IdentifierType, class AbstractProduct, //IdentifierType为派生类名的类型（一般可设为string），AbstractProduct为基类
class ProductCreator = AbstractProduct *(*)(),
class MapContainer = std::map<IdentifierType, ProductCreator>>
class Factory {
public:
/**
* @brief Registers the class given by the creator function, linking it to id.
* Registration must happen prior to calling CreateObject.
* @param id Identifier of the class being registered
* @param creator Function returning a pointer to an instance of
* the registered class
* @return True if the key id is still available
*/
bool Register(const IdentifierType &id, ProductCreator creator) { //注册的接口
return producers_.insert(std::make_pair(id, creator)).second;
}
/**
* @brief Creates and transfers membership of an object of type matching id.
* Need to register id before CreateObject is called. May return nullptr
* silently.
* @param id The identifier of the class we which to instantiate
* @param args the object construction arguments
*/
template <typename... Args>
std::unique_ptr<AbstractProduct> CreateObjectOrNull(const IdentifierType &id, //创建对象的接口，返回的是抽象产品（基类）的指针
Args &&... args) {
auto id_iter = producers_.find(id);
if (id_iter != producers_.end()) {
return std::unique_ptr<AbstractProduct>(
(id_iter->second)(std::forward<Args>(args)...));
}
return nullptr;
}
/**
* @brief Creates and transfers membership of an object of type matching id.
* Need to register id before CreateObject is called.
* @param id The identifier of the class we which to instantiate
* @param args the object construction arguments
*/
template <typename... Args>
std::unique_ptr<AbstractProduct> CreateObject(const IdentifierType &id, //创建对象的接口
Args &&... args) {
auto result = CreateObjectOrNull(id, std::forward<Args>(args)...);
AERROR_IF(!result) << "Factory could not create Object of type : " << id;
return result;
}
private:
MapContainer producers_;
};- map定义：
std::map<IdentifierType, ProductCreator>> producers_;
注册就是将实际需要生产的具体产品的名称和具体工厂插入producers_，在需要创建对象时根据具体产品的名称索引到具体工厂，调用具体工厂生产具体产品，并将具体产品的指针绑定在抽象产品的指针上作为具体工厂的返回值。
- 基于抽象工厂类模板+抽象产品作为模板实参=》实例化出抽象工厂
在Apollo/modules/planning/tasks/task_factory.h中针对task基类对工厂模板类进行了实例化，产生task_factory_抽象工厂，并使用TaskFactory类对task_factory_抽象工厂做了一层封装。
class TaskFactory {
public:
...
private:
static apollo::common::util::Factory<TaskConfig::TaskType, Task, Task *(*)(const TaskConfig &config),std::unordered_map<TaskConfig::TaskType, Task *(*)(const TaskConfig &config), std::hash<int>>> task_factory_;
static std::unordered_map<TaskConfig::TaskType, TaskConfig, std::hash<int>>
default_task_configs_;
};上文已经讲过Task为基类（抽象产品），派生出各种Decider和Optimizer的具体产品。
2.2.2 具体工厂 & 具体产品、 Register(向map中添加元素)
//Register
void TaskFactory::Init(const PlanningConfig& config) {
task_factory_.Register(TaskConfig::LANE_CHANGE_DECIDER,
[](const TaskConfig& config) -> Task* {
return new LaneChangeDecider(config); //使用匿名函数定义具体工厂，具体工厂生产具体产品
});
task_factory_.Register(TaskConfig::SPEED_BOUNDS_PRIORI_DECIDER,
[](const TaskConfig& config) -> Task* {
return new SpeedBoundsDecider(config);
});
task_factory_.Register(TaskConfig::OPEN_SPACE_ROI_DECIDER,
[](const TaskConfig& config) -> Task* {
return new OpenSpaceRoiDecider(config);
});
}
std::unique_ptr<Task> TaskFactory::CreateTask(const TaskConfig& task_config) {
TaskConfig merged_config;
if (default_task_configs_.find(task_config.task_type()) !=
default_task_configs_.end()) {
merged_config = default_task_configs_[task_config.task_type()];
}
merged_config.MergeFrom(task_config);
return task_factory_.CreateObject(task_config.task_type(), merged_config);
}特点：在Apollo/modules/planning/tasks/task_factory.cc定义中对task的派生类LaneChangeDecider、PATH_LANE_BORROW_DECIDER等集中进行注册。
2.2.3 Client使用示例：
Apollo/modules/planning/scenarios/stage.cc中读入配置参数，调用抽象工厂的接口，依次生产具体产品：
for (int i = 0; i < config_.task_type_size(); ++i) {
auto task_type = config_.task_type(i);
ACHECK(config_map.find(task_type) != config_map.end())
<< "Task: " << TaskConfig::TaskType_Name(task_type)
<< " used but not configured";
auto iter = tasks_.find(task_type);
if (iter == tasks_.end()) {
auto ptr = TaskFactory::CreateTask(*config_map[task_type]); //根据配置文件的参数动态地创建具体产品
task_list_.push_back(ptr.get());
tasks_[task_type] = std::move(ptr);
} else {
task_list_.push_back(iter->second.get());
}
}可参考：https://blog.csdn.net/davidhopper/article/details/79197075
下表总结对比基于类模板实现工厂方法模式和视觉感知模块（https://blog.csdn.net/Cxiazaiyu/article/details/106256330）使用的基于宏定义的工厂方法模式的异同：
基于类模板的工厂方法实现 基于宏定义的工厂方法实现 可适用不同基类类型的抽象工厂机理
工厂的类模板只有一个，利用类模板机理，将抽象产品类型（基类）作为模板参数，根据抽象产品类型实例化出不同的抽象工厂 只有一个抽象工厂原型类，使用返回创建对象的接口使用Any类型机理（支持代表任意类型）保证可以返回任意抽象产品 实现方法的区别 直接将Register和CreateObject作为工厂类模板的两个方法，使用类模板Factory实例化出的对象调用Register和CreateObject方法
CreateObject: 为每一个基类宏展开一个伴随的Register类（实际作用是CreateObject），该Register类中给出创建对象的接口；
Register: 为每一个派生类宏展开一个伴随的Register函数，该Register函数建立配置参数名称到具体工厂的map。
Register的map的区别
（相同点：都基于Register添加一个map，建立根据输入配置文件参数映射到对应的具体工厂）
无论是基于宏定义还是类模板，在抽象工厂和具体工厂之间需要实现怎么基于抽象工厂的接口调用具体工厂（Register机理建立map后，使用map索引到真实绑定的具体工厂）。
每一个基类维护一个Local的map
(std::map<IdentifierType, ProductCreator>> producers_;)，通过producers_[派生类名]可以调用对应的具体工厂；
不同基类维护不同的Local map。
维护一个Global的map, (static std::map<string, std::map<std::string, ObjectFactory *>> factory_map;)，通过factory_map[基类名][派生类名]可以调用对应的具体工厂 具体工厂的实现机制 直接使用匿名函数 在派生类中使用宏定义展开成对应的具体工厂类，继承自抽象工厂类 Register的方式区别 在factory的Register方法中集中Register，将派生类名-具体工厂的对应关系插入到map中；
在每个派生类定义中分布式地Register，将[基类名][派生类名]-具体工厂的对应关系插入到map中；
优\缺点 优点：类模板工厂定义简单
缺点：如果需要添加新的基类，需要重新实例化factory；如果需要添加新的派生类，需要在实例化的factory中集中Register。
缺点：定义相比类模板实现略显复杂
优点：如果需要增加新的基类和派生类，不需要修改原来的工厂相关的代码，仅需要在新添加的基类和派生类后调用宏定义。