做了ppp之后 ospf ppp loan

决策规划模块

我们来看下Apollo中的planning。本文以Apollo项目2019年初的版本为基础进行讲解。版本：3.5

Apollo系统中的Planning模块实际上是整合了决策和规划两个功能，该模块是自动驾驶系统中最核心的模块之一（另外三个核心模块是：定位，感知和控制）。

作用

决策规划模块一般也叫Planning模块，任务是计算出一个无碰撞可执行的轨迹（包含路径和速度信息），保证车辆从起点安全的驾驶到目的地，并尽可能高效。主要职责是：

• 根据导航信息以及车辆的当前状态，在有限的时间范围内，计算出一条合适的轨迹供车辆行驶。

• 车辆的行驶路线通常由Routing模块提供，Routing模块会根据目的地以及地图搜索出一条代价尽可能小的路线。
• 车辆的当前状态包含了很多因素，例如：车辆自身的状态（包括姿态，速度，角速度等等），当前所处的位置，周边物理世界的静态环境以及交通状态（行人/车辆、红绿灯、车道线）等等。
• Planning模块的响应速度必须是稳定可靠的（当然，其他模块也是一样）。正常人类的反应速度是300ms，而自动驾驶车辆想要做到安全可靠，其反应时间必须短于100ms。所以，Planning模块通常以10Hz的频率运行着。如果其中某一个算法的时间耗费了太长时间，就可能造成其他模块的处理延迟，最终可能造成严重的后果。例如：没有即时刹车，或者转弯。
• ”合适的轨迹“有多个层次的含义。

• 首先，车辆会做出一定的决策，比如变道超车或者沿当前车道保持
• 然后，会规划一条新的轨迹。注意，”轨迹“不同于“路径”，“轨迹”不仅仅包含了行驶路线，还要包含每个时刻的车辆的速度，加速度，方向转向等信息。
• 另外，这条轨迹必须是底层控制可以执行的。因为车辆在运动过程中，具有一定的惯性，车辆的转弯角度也是有限的。在计算行驶轨迹时，这些因素都要考虑。最后，从人类的体验上来说，猛加速，急刹车或者急转弯都会造成非常不好的乘坐体验，因此这些也需要考虑。

apollo::planning，其作用在于构建无人车从起点到终的局部行驶路径

• Planning模块负责整个车辆的驾驶决策，而驾驶决策需要根据当前所处的地理位置，周边道路和交通情况来决定。
• Planning不直接控制车辆硬件，而是借助于控制模块来完成。

Planning在学界不同的领域有着不同的视角，比如：

• 在机器人学领域，Planning问题可以定义为：在机器人的位形空间内寻找一个满足约束条件的，从起点到终点的一条最优路径；
• 在人工智能领域，Planning问题又可以定义为：给定系统的state action寻找最优的policy；
• 在自动驾驶领域，规划和控制问题，相当于一个人的脑和身体，在给定感知和定位的情况下，自动驾驶车辆需要知道如何地实现安全和舒适的自主运动。

Planning其问题的本质是一个多目标的数学优化问题。

主要的优化目标包括：

• 安全性：避免与场景中的障碍物发生碰撞；针对动态障碍物，由于其未来运动的不确定性，降低其未来的碰撞风险；
• 稳定性：由于车辆的惯性较大，灵活性差，期望轨迹需要保证车辆的物理可行性和控制器的稳定性；
• 舒适性：考虑到乘员的舒适性，需要在满足安全性和稳定性的同时保证车辆的驾驶舒适度，包括加减速以及转向等过程；
• 驾驶效率：在满足安全性和稳定性的同时，保证车辆以更快的速度驾驶，从而更短的时间到达目的地。

在实际场景中，规划过程需要考虑各种物理约束，有且不限于：

• 加减速度约束：受到动力系统和制动系统的性能极限，及驾驶员的安全性和舒适性的制约；
• 非完整性约束：车辆具有三个运动自由度，但是只有两个控制自由度，其非完整性约束决定了轨迹的物理可行性；
• 动力学约束：考虑到车辆的动力学特性和车身稳定性，其驾驶过程中的曲率和横摆角速度具有一定的约束；

其他模块和planning的关系

下图描述了其他模块和planning的关系，这其中的黄线代表了数据流，黑线代表了控制流。

在整个自动驾驶系统中，Planning模块相当于人类驾驶员的大脑。从这幅图中可以看出，对于Planning模块来说：

• 它的上游模块是：定位，地图，导航，感知和预测模块。
• 它的下游模块是控制模块。

规划模块的组成

在Planning模块中，一般分为决策模块decision与运动规划motion planning两部分。

• decision模块又分为场景选择即scenario Manager、当前状态选择stage Process、以及行为决策behavior decider；
• motion planning一般又分为路径规划、速度规划以及轨迹生成。

apollo的Planning模块就是首先确定当前自车所处在的场景scenario，然后判断在当前场景下处在什么状态stage下，最后调用一系列task进行behavior的判断从而完成决策过程。　在decision结束之后，便开始motion planning的过程。

Planning模块的架构

Apollo 3.5中Planning模块的架构如下图所示：

这其中主要的组件包括：

• Apollo FSM：一个有限状态机，与高清地图确定车辆状态给定其位置和路线。
• Planning Dispatcher：根据车辆的状态和其他相关信息，调用合适的Planner。
• Planner：获取所需的上下文数据和其他信息，确定相应的车辆意图，执行该意图所需的规划任务并生成规划轨迹。它还将更新未来作业的上下文。
• Deciders和Optimizers：一组实现决策任务和各种优化的无状态库。优化器特别优化车辆的轨迹和速度。决策者是基于规则的分类决策者，他们建议何时换车道、何时停车、何时爬行（慢速行进）或爬行何时完成。
• 黄色框：这些框被包含在未来的场景和/或开发人员中，以便基于现实世界的驱动用例贡献他们自己的场景。

整体Pipeline

• PncMap：全称是Planning and Control Map。这个部分的实现并不在Planning内部，而是位于/modules/map/pnc_map/目录下。但是由于该实现与Planning模块紧密相关，因此这里放在一起讨论。该模块的主要作用是：根据Routing提供的数据，生成Planning模块需要的路径信息。
• Frame：Frame中包含了Planning一次计算循环中需要的所有数据。例如：地图，车辆状态，参考线，障碍物信息等等。ReferenceLine是车辆行驶的参考线，TrafficDecider与交通规则相关，这两个都是Planning中比较重要的子模块
• EM Planner：Apollo系统中内置了好几个Planner，但目前默认使用的是EM Planner，这也是专门为开放道路设计的。该模块的实现可以说是整个Planning模块的灵魂所在。官方论文：Baidu Apollo EM Motion Planner。

层次结构

规划模块内部的主要类成员如下图所示：

基础数据结构

Planning模块是一个比较大的模块，因此这其中有很多的数据结构需要在内部实现中流转。

这些数据结构集中定义在两个地方：

• proto目录：该目录下都是通过Protocol Buffers格式定义的结构。这些结构会在编译时生成C++需要的文件。这些结构没有业务逻辑，就是专门用来存储数据的。（实际上不只是Planning，几乎每个大的模块都会有自己的proto文件夹。）
• common目录：这里是C++定义的数据结构。很显然，通过C++定义数据结构的好处是这些类的实现中可以包含一定的处理逻辑。

proto目录下的文件如下所示：

apollo/modules/planning/proto/
├── auto_tuning_model_input.proto
├── auto_tuning_raw_feature.proto
├── decider_config.proto
├── decision.proto
├── dp_poly_path_config.proto
├── dp_st_speed_config.proto
├── lattice_sampling_config.proto
├── lattice_structure.proto
├── navi_obstacle_decider_config.proto
├── navi_path_decider_config.proto
├── navi_speed_decider_config.proto
├── pad_msg.proto
├── planner_open_space_config.proto
├── planning.proto
├── planning_config.proto
├── planning_internal.proto
├── planning_stats.proto
├── planning_status.proto
├── poly_st_speed_config.proto
├── poly_vt_speed_config.proto
├── proceed_with_caution_speed_config.proto
├── qp_piecewise_jerk_path_config.proto
├── qp_problem.proto
├── qp_spline_path_config.proto
├── qp_st_speed_config.proto
├── reference_line_smoother_config.proto
├── side_pass_path_decider_config.proto
├── sl_boundary.proto
├── spiral_curve_config.proto
├── st_boundary_config.proto
├── traffic_rule_config.proto
└── waypoint_sampler_config.proto

common目录下的头文件如下：

apollo/modules/planning/common/
├── change_lane_decider.h
├── decision_data.h
├── distance_estimator.h
├── ego_info.h
├── frame.h
├── frame_manager.h
├── indexed_list.h
├── indexed_queue.h
├── lag_prediction.h
├── local_view.h
├── obstacle.h
├── obstacle_blocking_analyzer.h
├── path
│   ├── discretized_path.h
│   ├── frenet_frame_path.h
│   └── path_data.h
├── path_decision.h
├── planning_context.h
├── planning_gflags.h
├── reference_line_info.h
├── speed
│   ├── speed_data.h
│   ├── st_boundary.h
│   └── st_point.h
├── speed_limit.h
├── speed_profile_generator.h
├── threshold.h
├── trajectory
│   ├── discretized_trajectory.h
│   ├── publishable_trajectory.h
│   └── trajectory_stitcher.h
└── trajectory_info.h

这里有如下一些结构值得我们注意：

名称	说明
EgoInfo类	包含了自车信息，例如：当前位置点，车辆状态，外围Box等。
Frame类	包含了一次Planning计算循环中的所有信息。
FrameManager类	Frame的管理器，每个Frame会有一个整数型id。
LocalView类	Planning计算需要的输入，下文将看到其定义。
Obstacle类	描述一个特定的障碍物。障碍物会有一个唯一的id来区分。
PlanningContext类	Planning全局相关的信息，例如：是否正在变道。这是一个单例。
ReferenceLineInfo类	车辆行驶的参考线，下文会专门讲解。
path文件夹	描述车辆路线信息。包含：PathData，DiscretizedPath，FrenetFramePath三个类。
speed文件夹	描述车辆速度信息。包含SpeedData，STPoint，StBoundary三个类。
trajectory文件夹	描述车辆轨迹信息。包含DiscretizedTrajectory，PublishableTrajectory，TrajectoryStitcher三个类。
planning_gflags.h	定义了模块需要的许多常量，例如各个配置文件的路径。

模块配置

Planning模块中有很多处的逻辑是通过配置文件控制的。通过将这部分内容从代码中剥离，可以方便的直接对配置文件进行调整，而不用编译源代码。这对于系统调试和测试来说，是非常方便的。

Apollo系统中，很多模块都是类似的设计。因此每个模块都会将配置文件集中放在一起，也就是每个模块下的conf目录。

Planning模块的配置文件如下所示：

apollo/modules/planning/conf/
├── adapter.conf
├── cosTheta_smoother_config.pb.txt
├── navi_traffic_rule_config.pb.txt
├── planner_open_space_config.pb.txt
├── planning.conf
├── planning_config.pb.txt
├── planning_config_navi.pb.txt
├── planning_navi.conf
├── qp_spline_smoother_config.pb.txt
├── scenario
│   ├── lane_follow_config.pb.txt
│   ├── side_pass_config.pb.txt
│   ├── stop_sign_unprotected_config.pb.txt
│   ├── traffic_light_protected_config.pb.txt
│   └── traffic_light_unprotected_right_turn_config.pb.txt
├── spiral_smoother_config.pb.txt
└── traffic_rule_config.pb.txt

这里的绝大部分文件都是.pb.txt后缀的。因为这些文件是和上面提到的proto结构相对应的。因此可以直接被proto文件生成的数据结构读取。对于不熟悉的读者可以阅读Protocal Buffer的文档：google.protobuf.text_format。

暂时不用太在意这些文件的内容。随着对于Planning模块实现的熟悉，再回过来看这些配置文件，就很容易理解每个配置文件的作用了。

Planner

TrafficRule

行驶在城市道路上的自动驾驶车辆必定受到各种交通规则的限制。在正常情况下，车辆不应当违反交通规则。

另外，交通规则通常是多种条例，不同城市和国家地区的交通规则可能是不一样的。

如果处理好这些交通规则就是模块实现需要考虑的了。目前Planning模块的实现中，有如下这些交通规则的实现：

TrafficRule配置

交通条例的生效并非是一成不变的，因此自然就需要有一个配置文件来进行配置。交通规则的配置文件是：modules/planning/conf/traffic_rule_config.pb.txt。

下面是其中的一个代码片段：

// modules/planning/conf/traffic_rule_config.pb.txt
...

config: {
  rule_id: SIGNAL_LIGHT
  enabled: true
  signal_light {
    stop_distance: 1.0
    max_stop_deceleration: 6.0
    min_pass_s_distance: 4.0
    max_stop_deacceleration_yellow_light: 3.0
    signal_expire_time_sec: 5.0
    max_monitor_forward_distance: 135.0
    righ_turn_creep {
      enabled: false
      min_boundary_t: 6.0
      stop_distance: 0.5
      speed_limit: 1.0
    }
  }
}

TrafficDecider

TrafficDecider是交通规则处理的入口，它负责读取上面这个配置文件，并执行交通规则的检查。在上文中我们已经看到，交通规则的执行是在StdPlanning::RunOnce中完成的。具体执行的逻辑如下：

Status TrafficDecider::Execute(Frame *frame,
                               ReferenceLineInfo *reference_line_info) {
  for (const auto &rule_config : rule_configs_.config()) {
    if (!rule_config.enabled()) { ①
      continue;
    }
    auto rule = s_rule_factory.CreateObject(rule_config.rule_id(), rule_config); ②
    if (!rule) {
      continue;
    }
    rule->ApplyRule(frame, reference_line_info); ③
  }

  BuildPlanningTarget(reference_line_info); ④
  return Status::OK();
}

这段代码说明如下：

• 遍历配置文件中的每一条交通规则，判断是否enable。
• 创建具体的交通规则对象。
• 执行该条交通规则逻辑。
• 在ReferenceLineInfo上合并处理所有交通规则最后的结果。

Task

一直到目前最新的Apollo 3.5版本为止，Planning模块最核心的算法就是其EM Planner（实现类是PublicRoadPlanner），而EM Planner最核心的就是其决策器和优化器。

这里我们仅仅粗略的了解一下其实现结构。

Planning中这部分逻辑实现位于tasks目录下，无论是决策器还是优化器都是从apollo::planning::Task继承的。该类具有下面这些子类：

Task类提供了Execute方法供子类实现，实现依赖的数据结构就是Frame和ReferenceLineInfo。

Status Task::Execute(Frame* frame, ReferenceLineInfo* reference_line_info) {
  frame_ = frame;
  reference_line_info_ = reference_line_info;
  return Status::OK();
}

有兴趣的读者可以通过阅读子类的Execute方法来了解算法实现。

参考

• 10种自动驾驶规划控制算法技术解析
• 自动驾驶重卡在规划和控制中的技术难点与挑战
• 自动驾驶之轨迹规划1——算法综述
• 解析百度Apollo之决策规划模块

Planning模块的组成

轨迹规划的目标是生成一系列路径点所定义的轨迹。我们为每个路径点分配了一个时间戳和速度。由于移动的障碍物可能会暂时阻挡部分路段，轨迹中的每个路径点都有时间戳。我们可以将 时间戳与预测模块的输出相结合，以确保我们计划通过时，轨迹上的每个路径点均未被占用。这 些时间戳和空间上的两个维度(2D position)共同创建了一个三维轨迹(3DTrajectory)。我们还为每个路径点指定了一个速度，用于确保车辆按时到达每个路径点。

现实世界中的规划面临多种约束。首先轨迹应能免于碰撞，这意味着必须没有障碍物。其次，要让乘客感到舒适，所以路径点之间的过渡以及速度的任何变化都必须平滑。再者，路径点对车辆应实际可行，例如高速行驶的汽车不能立即做180度转弯。我们不能构建包含这种不满足控制算法的轨迹。最后，轨迹应合法。我们需要了解每个路径点的交通法律，并确保轨迹遵守这些法律法规。

在道路的任何两点，可能会有多个不会发生碰撞、行驶舒适、可行且合法的轨迹。我们如何选择最佳轨迹呢?答案是使用“cost function”。cost function每个轨迹分配了一个“成本”，我们选择成 本最低的轨迹。轨迹“成本”由各种犯规处罚组成，例如:偏离道路中心，有可能产生碰撞，速度限制，轨迹的曲率和加速度让乘客感到不舒服等。

轨迹成本将所有这些缺陷聚合为单个数值，这使我们能对不同的轨迹按数字大小进行排名。车辆 甚至可能在不同的环境中使用不同的成本函数。例如，高速路的成本函数可能与停车场的不同。