当前位置：文江博客文章教程详情

解析百度 Apollo 之参考线与轨迹

发布于 2023-08-22 00:03:31 字数 33790 浏览 40 评论 0

详细讲解决策规划模块中是如何生成参考线和行车轨迹供车辆行驶的。参考线和轨迹直接影响了自动驾驶车辆对于方向和速度的控制，因此，说它是自动驾驶系统中最重要的数据都不为过。

本文是百度自动驾驶平台 Apollo 项目系列文章中的一篇，会讲解 Planning 模块中的参考线逻辑。ReferenceLine（参考线）与 Routing 模块和 Planning 模块都相关。

本文以 Apollo 项目 2019 年初的版本为基础进行讲解。

版本：3.5
获取代码时间：2019 年 2 月 7 日

我们先介绍一下参考线在整个 Planning 模块中所处的位置：

决策规划模块负责生成车辆的行驶轨迹。要做到这一点，决策规划模块需要从宏观到局部经过三个层次来进行决策。

第一个层次是 Routing 的搜索结果。Routing 模块的输入是若干个按顺序需要达到的途径点（也可能只有一个起点和终点）。Routing 模块根据地图的拓扑结构搜索出可达的完整路线来，这个路线的长度可能是几公里甚至几百公里。因此这个是最为宏观的数据。另外，Routing 的搜索结果是相对固定的。在没有障碍物的情况下，车辆会一直沿着原先获取到的 Routing 路线行驶。只有当车辆驶出了原先规划的路线之外（例如：为了避障），才会重新发送请求给 Routing 模块，以重新计算路线。
第二个层次就是参考线。决策规划模块会实时的根据车辆的具体位置来计算参考线。参考线的计算会以 Routing 的路线为基础。但同时，参考线会考虑车辆周边的动态信息，例如：障碍物，交通规则等。参考线是包含车辆所在位置周边一定的范围，通常是几百米的长度。相较于 Routing 结果，它是较为局部的数据。
第三个层次是轨迹。轨迹是决策规划模块的最终输出结果。它的依据是参考线。在同一时刻，参考线可能会有多条，例如：在变道的时候，自车所在车道和目标车道都会有一条参考线。而轨迹，是在所有可能的结果中，综合决策和优化的结果，最终的唯一结果。因此它是更为具体和局部的数据。轨迹不仅仅包含了车辆的路线，还包含了车辆行驶这条路线时的详细状态，例如：车辆的方向，速度，加速度等等。

在 Planning 模块一文中我们已经提到：参考线是整个决策规划算法的基础。在 Planning 模块的每个计算循环中，都会先生成参考线，然后在这个基础上进行后面的处理，例如：交通规则逻辑，障碍物投影，路径优化，速度决策等等。可以说，参考线贯穿了整个 Planning 模块的实现。

下面这幅图描述了本文所涉及的主要内容。建议读者先大致浏览一下这幅图，在阅读完本文之后再回顾一遍会比较好。

从这幅图中可以看出，这里涉及到三个模块：

routing 模块，这部分内容已经在 Routing 模块一文中讲解过，本文不再赘述。
pnc_map 模块：负责读取和处理 Routing 搜索结果。
planning 模块：根据 Routing 结果和车辆的实时状态（包括周边环境）生成参考线和轨迹。

在 Planning 模块中有以下三个数据结构将是本文关注的重点：

ReferenceLine ：原始参考线，源码位于 planning/reference_line/ 目录下。根据 Routing 的搜索结果生成。
ReferenceLineInfo ：源码位于 planning/common/ 目录下。Planning 实现中，逻辑计算的基础数据结构，很多操作都会在这个数据结构上进行（例如：交通规则逻辑，障碍物投影，路径优化，速度决策等）。本文中的“参考线”一词将不区分 ReferenceLine 和 ReferenceLineInfo 两个结构。
Trajectory ：下文中我们将看到，有好几个结构用来描述轨迹。它们在不同的场合下使用。这其中， ADCTrajectory 是 Planning 模块的输出。它是 Planning 模块一次计算循环中，处理了所有逻辑的最终结果，包含了车辆行驶需要的所有信息。因此，这个数据将直接影响到自动驾驶车辆的行车行为。

注：对于决策和优化的知识需要更多的内容来讲解，因此这部分内容本文不会涉及。这些内容会通过其他文章来说明。

下文将根据从下往上的顺序来进行讲解。

pnc 全称是 Planning And Control。这是 Planning 用来对接 Routing 搜索结果的子模块。

数据结构

接下来先看一下几个主要的数据结构，这些结构是在 Planning 模块的实现中频繁见到。

RoutingRequest 和 RoutingResponse 两个结构我们已经在 Routing 模块一文中讲解过，这里不再贴出。

PointENU ：描述了地图上的一个点。定位，感知，预测和决策模块都会使用这个数据结构。目前，Apollo 地图使用 Universal Transverse Mercator 坐标系统。在一些情况下， z 字段可能会被忽略。

# modules/common/proto/geometry.proto

message PointENU {
  optional double x = 1 [default = nan];  // East from the origin, in meters.
  optional double y = 2 [default = nan];  // North from the origin, in meters.
  optional double z = 3 [default = 0.0];  // Up from the WGS-84 ellipsoid, in meters.
}

SLPoint ：描述了 Frenet 坐标系上的一个点。 s 表示距离起点的纵向距离， l 表示距离中心线的侧向距离。

# modules/common/proto/pnc_point.proto

message SLPoint {
  optional double s = 1;
  optional double l = 2;
}

LaneWaypoint ：描述了车道上的点。

// modules/map/pnc_map/path.h
using LaneInfoConstPtr = std::shared_ptr<const LaneInfo>;

struct LaneWaypoint {
LaneWaypoint() = default;
LaneWaypoint(LaneInfoConstPtr lane, const double s)
: lane(CHECK_NOTNULL(lane)), s(s) {}
LaneWaypoint(LaneInfoConstPtr lane, const double s, const double l)
: lane(CHECK_NOTNULL(lane)), s(s), l(l) {}
LaneInfoConstPtr lane = nullptr;
double s = 0.0;
double l = 0.0;

std::string DebugString() const;
};

VehicleState ：描述车辆状态，包含了自车位置，姿态，方向，速度，加速度等信息。

# modules/common/vehicle_state/proto/vehicle_state.proto

message VehicleState {
  optional double x = 1 [default = 0.0];
  optional double y = 2 [default = 0.0];
  optional double z = 3 [default = 0.0];
  optional double timestamp = 4 [default = 0.0];
  optional double roll = 5 [default = 0.0];
  optional double pitch = 6 [default = 0.0];
  optional double yaw = 7 [default = 0.0];
  optional double heading = 8 [default = 0.0];
  optional double kappa = 9 [default = 0.0];
  optional double linear_velocity = 10 [default = 0.0];
  optional double angular_velocity = 11 [default = 0.0];
  optional double linear_acceleration = 12 [default = 0.0];
  optional apollo.canbus.Chassis.GearPosition gear = 13;
  optional apollo.canbus.Chassis.DrivingMode driving_mode = 14;
  optional apollo.localization.Pose pose = 15;
}

roll，pitch和yaw分布描述了车辆在纵向，侧向和垂直方向上的角度，具体如下图所示：

RouteSegments

我们回顾一下，Routing 的搜索结果 RoutingResponse 中包含了下面三个层次的结构：

RoadSegment ：描述道路，一条道路可能包含了并行的几条通路（ Passage ）。
Passage ：描述通路，通路是直连不含变道的可行驶区域。一个通路可能包含了前后连接的多个车道。
LaneSegment ：描述车道，车道是道路中的一段，自动驾驶车辆会尽可能沿着车道的中心线行驶。

而 pnc_map 模块中的 RouteSegments 对应了上面的 Passage 结构，它其中会包含若干个车道信息。这个类继承自 std::vector<LaneSegment> 。

RouteSegments 中有如下一些方法值得关注：

NextAction() ：车辆接下来要采取的动作。可能是直行，左变道，或者右变道。
CanExit() ：当前通路是否可以接续到 Routing 结果的另外一个通路上。
GetProjection() ：将一个点投影到当前通路上。返回 SLPoint 和 LaneWaypoint 。
Stitch() ：缝合另外一个 RouteSegments 。即：去除两个 RouteSegments 间重合的多余部分，然后连接起来。
Shrink() ：缩短到指定范围。
IsOnSegment() ：车辆是否在当前 RouteSegments 上。
IsNeighborSegment() ：当前 RouteSegments 是否是车辆的临近 RouteSegments 。

PncMap

PncMap 类负责对接 Routing 搜索结果的更新。

PncMap 会根据车辆当前位置，提供车辆周边的 RouteSegments 信息供 ReferenceLineProvider 生成 ReferenceLine 。“车辆周边”与车辆的纵向和横向相关，具体如下：

对于纵向来说， PncMap 返回的结果是前后一定范围内的。具体的范围由下面三个值决定：

// modules/map/pnc_map/pnc_map.cc
DEFINE_double(
look_backward_distance, 30,
"look backward this distance when creating reference line from routing");

DEFINE_double(look_forward_short_distance, 180,
"short look forward this distance when creating reference line "
"from routing when ADC is slow");

DEFINE_double(
look_forward_long_distance, 250,
"look forward this distance when creating reference line from routing");

即：向后是 30 米的范围。向前是根据车辆的速度返回 180 米或者 250 米的范围。

对于横向来说，如果 Routing 的搜索结果包含变道的信息。则 PncMap 提供的数据会包含自车所在车道和变道后的相关通路。

PncMap 中的下面这个方法用来接收车辆的状态更新。当然，这其中很重要的就是位置状态。

bool PncMap::UpdateVehicleState(const VehicleState &vehicle_state)

此时 PncMap 会计算车辆已经经过了哪些 Routing 的途经点，以及下一个目标点等信息。

在每一个 Planning 计算循环中， ReferenceLineProvider 都会传入车辆的 VehicleState 给 PncMap ，并从 PncMap 获取车辆周围的通路（ RouteSegments ）。

PncMap 中的下面这个方法返回 Routing 途经点还有哪些没有达到：

std::vector<routing::LaneWaypoint> PncMap::FutureRouteWaypoints() const

前面我们已经说了，参考线是根据车辆位置相对局部的一个数据，它包含了车辆前后一定范围内的路径信息。在车辆行驶过程中，Planning 会在每一个计算周期中生成 ReferenceLine 。

这里我们先看一下 ReferenceLine 中主要包含了什么。

下面是 ReferenceLine 类中的数据成员：

// modules/planning/reference_line/reference_line.h
uint32_t priority_ = 0;
struct SpeedLimit {
double start_s = 0.0;
double end_s = 0.0;
double speed_limit = 0.0; // unit m/s
};
std::vector<SpeedLimit> speed_limit_;

std::vector<ReferencePoint> reference_points_;
hdmap::Path map_path_;

priority_ 是优先级，不过在 PublicRoadPlanner 中没有用到。 speed_limit_ 是限速数据，这两个我们暂时都不用关注。

reference_points_ 其实是从 map_path_ 得到，具体见 ReferenceLine 的构造函数。所以这两个数据的作用其实是一样的。

// modules/planning/reference_line/reference_line.cc
ReferenceLine::ReferenceLine(const MapPath& hdmap_path)
: map_path_(hdmap_path) {
for (const auto& point : hdmap_path.path_points()) {
DCHECK(!point.lane_waypoints().empty());
const auto& lane_waypoint = point.lane_waypoints()[0];
reference_points_.emplace_back(
hdmap::MapPathPoint(point, point.heading(), lane_waypoint), 0.0, 0.0);
}
CHECK_EQ(map_path_.num_points(), reference_points_.size());
}

std::vector<ReferencePoint> 是一系列的点，点包含了位置的信息。因此这些点就是生成车辆行驶轨迹的基础数据。

ReferencePoint

ReferencePoint 由 MapPathPoint 继承而来。下面这幅图是 ReferencePoint 的类继承关系。

这里的几个类的说明如下：

Vec2d 描述一个二维的点，包含的数据成员如下：
- double x_ ：描述点的 x 坐标。
- double y_ ：描述点的 y 坐标。
MapPathPoint 描述了一个地图上的点，包含的数据成员如下：
- double heading_ ：描述点的朝向。
- std::vector<LaneWaypoint> lane_waypoints_ ：描述路径上的点。有些车道可能会存在重合的部分，所以地图上的一个点可能同时属于多个车道，因此这里的数据是一个 vector 结构。
ReferencePoint 描述了参考线中的点，包含的数据成员如下：
- double kappa_ ：描述曲线的曲率。
- double dkappa_ ：描述曲率的导数。

除了坐标之外， ReferencePoint 中的朝向和曲率也都是非常重要的数据。因为这些数据将直接影响车辆的方向控制。如果 ReferenceLine 中的 ReferencePoint 之间存在朝向和曲率大小震动或者数据跳变将可能导致车辆方向盘的相应变化，而这种变化对于乘车体验来说是非常糟糕的。

如果你打开 Apollo 项目中的 demo 地图文件你就会发现，地图中记录的仅仅是每个点 x 和 y 坐标，并没有记录 heading 和 kappa 数据。事实上，这些数据都是在读取地图原始点数据之后计算出来的。

对于 heading 的计算，位于 path.cc 中，相关代码如下：

// modules/map/pnc_map/path.cc#InitPoints
for (int i = 0; i < num_points_; ++i) {
accumulated_s_.push_back(s);
Vec2d heading;
if (i + 1 >= num_points_) {
heading = path_points_[i] - path_points_[i - 1];
} else {
segments_.emplace_back(path_points_[i], path_points_[i + 1]);
heading = path_points_[i + 1] - path_points_[i];
s += heading.Length();
}
heading.Normalize();
unit_directions_.push_back(heading);
}

这里遍历所有的点，通过两个点的坐标相减来计算两个点所代表向量，而一旦有了向量自然就知道其朝向了。这里的 path_points_[i] 是 MapPathPoint 类型的数据，而对于坐标点的减法和朝向的计算是在其父类 Vec2d 中完成的：

// modules/common/math/vec2d.cc
double Vec2d::Angle() const { return std::atan2(y_, x_); }

Vec2d Vec2d::operator-(const Vec2d &other) const {
return Vec2d(x_ - other.x(), y_ - other.y());
}

如果你没有用过 atan2 这个函数，请看这里。简单来说， atan2 计算了$arctan(\frac{y}{x})$的值，其返回值在$(-\Pi, +\Pi]$之间，如下图所示：

而对于曲率的计算，则是具体的 Smoother 中完成的。这里就不再赘述了。

StdPlanning 已经改名为 OnLanePlanning 。

了解了 ReferenceLine 结构之后，我们再看一下 ReferenceLine 是如何创建的。

在每一次计算循环中，Planning 模块都会通过 ReferenceLineProvider 生成 ReferenceLine。ReferenceLine 由 Routing 的搜索结果决定。Routing 是预先搜索出的全局可达路径，而 ReferenceLine 是车辆当前位置的前后一段范围。

直行的情况下，ReferenceLine 是一个。而在需要变道的时候，会有多个 ReferenceLine。

目标函数

1.1 分段寻路路径：将寻路路径划分为 n 段，每段用 2 个多项式表示：

\[x = f_i(t) = a_{i0} + a_{i1} * t + a_{i2} * t^2 + a_{i3} * t^3 + a_{i4} * t^4 + a_{i5} * t^5\] \[y = g_i(t) = b_{i0} + b_{i1} * t + b_{i2} * t^2 + b_{i3} * t^3 + b_{i4} * t^4 + b_{i5} * t^5\]

1.2 定义样条段优化目标函数

\[cost = \sum_{i=1}^{n} \Big( \int\limits_{0}^{t_i} (f_i''')^2(t) dt + \int\limits_{0}^{t_i} (g_i''')^2(t) dt \Big)\]

1.3 将开销（cost）函数转换为 QP 公式，QP 公式如下：

\[\frac{1}{2} \cdot x^T \cdot H \cdot x + f^T \cdot x \\ s.t. LB \leq x \leq UB \\ A_{eq}x = b_{eq} \\ Ax \leq b\]

约束条件

2.1 平滑节点约束：该约束的目的是使样条的节点更加平滑。假设两个段$seg_k$ 和$seg_{k+1}$互相连接，且$seg_k$的累计值 s 为$s_k$。计算约束的等式为：

\[f_k(s_k) = f_{k+1} (s_0)\]

同样地，该公式也适用于下述等式：

\[f'_k(s_k) = f'_{k+1} (s_0) \\ f''_k(s_k) = f''_{k+1} (s_0) \\ f'''_k(s_k) = f'''_{k+1} (s_0) \\ g_k(s_k) = g_{k+1} (s_0) \\ g'_k(s_k) = g'_{k+1} (s_0) \\ g''_k(s_k) = g''_{k+1} (s_0) \\ g'''_k(s_k) = g'''_{k+1} (s_0)\]

2.2 点采样边界约束：在路径上均匀的取样 m 个点并检查这些点的预定义边界。

\[f_i(t_l) - x_l \lt boundary \\ g_i (t_l) - y_l \lt boundary\]

ReferenceLineInfo 由 Frame 根据 ReferenceLine 和 RouteSegments 创建得到。

在每个 Planning 计算循环的开始，都会创建和初始化一个新的 Frame ，而 Frame 初始化的时候就会创建 ReferenceLineInfo 。当有多个 ReferenceLine 的时候，则意味着需要变道。

bool Frame::CreateReferenceLineInfo(
    const std::list<ReferenceLine> &reference_lines,
    const std::list<hdmap::RouteSegments> &segments) {
  reference_line_info_.clear();
  auto ref_line_iter = reference_lines.begin();
  auto segments_iter = segments.begin();
  while (ref_line_iter != reference_lines.end()) {
    if (segments_iter->StopForDestination()) {
      is_near_destination_ = true;
    }
    reference_line_info_.emplace_back(vehicle_state_, planning_start_point_,
                                      *ref_line_iter, *segments_iter);
    ++ref_line_iter;
    ++segments_iter;
  }
if (FLAGS_enable_change_lane_decider &&
!change_lane_decider_.Apply(&reference_line_info_)) {
AERROR << "Failed to apply change lane decider";
return false;
}
...
}

ReferenceLine 主要是静态数据（路径点和限速）的存储，而 ReferenceLineInfo 中会包含动态信息（障碍物）和更多逻辑。

主要有下面两个地方会操作 ReferenceLineInfo：

/modules/planning/traffic_rules ：该目录下是交通规则的实现。不同 Rule 会向 ReferenceLineInfo 添加不同的数据，例如：障碍物，红绿灯等等。
/modules/planning/tasks ：该目录下是许多的决策器和优化器，这是 Apollo EM Planner 算法的核心。不过前面已经说过，由于篇幅所限，本文不会涉及这些内容。

在《解析百度 Apollo 之决策规划模块》一文中我们已经提到，交通规则的应用是在 TrafficDecider::Execute 方法中执行的。

每个交通规则都会实现下面这个方法来完成其逻辑：

common::Status ApplyRule(Frame* const frame,
                         ReferenceLineInfo* const reference_line_info) = 0;

这里的指针类型的参数就意味着方法实现可能会修改参数的值。

对于 ReferenceLineInfo 的操作，主要是修改该类的以下三个字段：

PathData path_data_ ：包含了路径相关的数据，逻辑实现位于 modules/planning/common/path/ 中。
SpeedData speed_data_ ：包含了速度相关的数据，逻辑实现位于 modules/planning/common/speed/ 。路径和速度最终将组合起来使用，以生成行车轨迹（见下文）。
PathDecision path_decision_ ：这个字段中包含了障碍物的决策信息。

障碍物在 Planning 模块中通过 apollo::planning::Obstacle 描述。

障碍物分为横向障碍物和纵向障碍物。横向障碍物将可能导致车辆的 nudge 行为。而纵向障碍物可能导致车辆出现：stop，yield，follow，overtake 行为。这几个行为的优先级从左到右依次递减。

预测模块对于同一个障碍物可能会有多个预测轨迹。此时在 Planning 模块中，会多个 apollo::planning::Obstacle 对象分别对应每一个轨迹。

前面我们已经多次提到“轨迹”一次。我们知道，”轨迹“不同于“路径”，“轨迹”不仅仅包含了行驶路线，还要包含每个时刻的车辆的速度，加速度，方向转向等信息。

就和高精地图通过一系列的点来描述道路的中心线一样，车辆的行驶轨迹也是由一系列的点来描述的。具体的结构在 proto 文件中定义，如下所示：

// common/proto/pnc_point.proto
message PathPoint {
optional double x = 1;
optional double y = 2;
optional double z = 3;
optional double theta = 4;
optional double kappa = 5;
optional double s = 6;

optional double dkappa = 7;
optional double ddkappa = 8;
optional string lane_id = 9;

optional double x_derivative = 10;
optional double y_derivative = 11;
}

message TrajectoryPoint {
optional PathPoint path_point = 1;
optional double v = 2; // in [m/s]
optional double a = 3;
optional double relative_time = 4;
optional double da = 5;
optional double steer = 6;
}

轨迹中的点通过 TrajectoryPoint 这个结构来描述。它的字段说明如下：

path_point ： PathPoint 类型数据。描述了一个点的位置，曲率，朝向，所属车道等信息。
v ：描述车辆的速度。
a ：描述车辆的加速度。
relative_time ：描述车辆达到该点的相对时间（以轨迹的开始为起点）。
da ：加速度的导数，也称之为 jerk。
steer ：车辆的前轮方向。

TrajectoryPoint 仅仅是一个点。而一条轨迹一定是由许多个点构成的。因此，描述轨迹的类 DiscretizedTrajectory 继承自 std::vector<common::TrajectoryPoint> 。如下所示：

class DiscretizedTrajectory : public std::vector<common::TrajectoryPoint>

DiscretizedTrajectory 还有一个子类 PublishableTrajectory ，它们的类结构如下图所示：

PublishableTrajectory 相较于 DiscretizedTrajectory 来说，最主要的提供了下面这个方法：

explicit PublishableTrajectory(const ADCTrajectory& trajectory_pb);

这个方法是将 Planning 模块中的轨迹数据结构导出到可发布的状态。这里的 ADCTrajectory 是 Planning 最终往外发出的数据结构，它也是在 proto 文件中定义的。这个数据结构比较大，字段上已经包含了描述。因此这里就不多做说明了。

# modules/planning/proto/planning.proto

message ADCTrajectory {
  optional apollo.common.Header header = 1;

  optional double total_path_length = 2;  // in meters
  optional double total_path_time = 3;    // in seconds

  // path data + speed data
  repeated apollo.common.TrajectoryPoint trajectory_point = 12;

  optional EStop estop = 6;

  // path point without speed info
  repeated apollo.common.PathPoint path_point = 13;

  // is_replan == true mean replan triggered
  optional bool is_replan = 9 [default = false];
  optional string replan_reason = 22;

  // Specify trajectory gear
  optional apollo.canbus.Chassis.GearPosition gear = 10;

  optional apollo.planning.DecisionResult decision = 14;

  optional LatencyStats latency_stats = 15;

  // the routing used for current planning result
  optional apollo.common.Header routing_header = 16;
  optional apollo.planning_internal.Debug debug = 8;

  enum RightOfWayStatus {
    UNPROTECTED = 0;
    PROTECTED = 1;
  }
  optional RightOfWayStatus right_of_way_status = 17;

  // lane id along reference line
  repeated apollo.hdmap.Id lane_id = 18;

  // set the engage advice for based on current planning result.
  optional apollo.common.EngageAdvice engage_advice = 19;

  // the region where planning cares most
  message CriticalRegion {
    repeated apollo.common.Polygon region = 1;
  }

  // critial region will be empty when planning is NOT sure which region is
  // critical
  // critial regions may or may not overlap
  optional CriticalRegion critical_region = 20;

  enum TrajectoryType {
    UNKNOWN = 0;
    NORMAL = 1;
    PATH_FALLBACK = 2;
    SPEED_FALLBACK = 3;
  }
  optional TrajectoryType trajectory_type = 21 [default = UNKNOWN];

  // output related to RSS
  optional RSSInfo rss_info = 100;
}

从参考线到生成轨迹是由 ReferenceLineInfo::CombinePathAndSpeedProfile 方法完成的。

在 EM Planner 中，路径和速度是分开优化的，这里是在将这两个优化的结果（记录在 ReferenceLineInfo 的 path_data_ 和 speed_data_ 中）合并成最终的结果。

bool ReferenceLineInfo::CombinePathAndSpeedProfile(
    const double relative_time, const double start_s,
    DiscretizedTrajectory* ptr_discretized_trajectory) {
  ...
  
  for (double cur_rel_time = 0.0; cur_rel_time < speed_data_.TotalTime();
       cur_rel_time += (cur_rel_time < kDenseTimeSec ? kDenseTimeResoltuion
                                                     : kSparseTimeResolution)) { ①
    common::SpeedPoint speed_point;
    if (!speed_data_.EvaluateByTime(cur_rel_time, &speed_point)) {
      AERROR << "Fail to get speed point with relative time " << cur_rel_time;
      return false;
    }

    if (speed_point.s() > path_data_.discretized_path().Length()) {
      break;
    }
    common::PathPoint path_point;
    if (!path_data_.GetPathPointWithPathS(speed_point.s(), &path_point)) { ②
      AERROR << "Fail to get path data with s " << speed_point.s()
             << "path total length " << path_data_.discretized_path().Length();
      return false;
    }
    path_point.set_s(path_point.s() + start_s);

    common::TrajectoryPoint trajectory_point; ③
    trajectory_point.mutable_path_point()->CopyFrom(path_point);
    trajectory_point.set_v(speed_point.v());
    trajectory_point.set_a(speed_point.a());
    trajectory_point.set_relative_time(speed_point.t() + relative_time);
    ptr_discretized_trajectory->AppendTrajectoryPoint(trajectory_point); ④
  }
  return true;
}

这段代码中的四个点说明如下：

根据 speed_data_ 中记录的总时间和间隔以确定需要在轨迹中添加多少个点。
根据 path_data_ 中的数据获取相应的点以确定轨迹点的位置。
创建一个轨迹点 TrajectoryPoint 并设置位置和速度，加速度和相对时间等信息。
在轨迹中添加一个轨迹点。

有了轨迹之后，便可以发出供车辆行驶了。相关代码如下：

Status OnLanePlanning::Plan(
    const double current_time_stamp,
    const std::vector<TrajectoryPoint>& stitching_trajectory,
    ADCTrajectory* const trajectory_pb) {
  ...
  auto status = planner_->Plan(stitching_trajectory.back(), frame_.get());  ①

  ptr_debug->mutable_planning_data()->set_front_clear_distance(
      EgoInfo::Instance()->front_clear_distance());

  const auto* best_ref_info = frame_->FindDriveReferenceLineInfo();  ②
  ...
  trajectory_pb->mutable_latency_stats()->MergeFrom(
      best_ref_info->latency_stats());
  // set right of way status
  trajectory_pb->set_right_of_way_status(best_ref_info->GetRightOfWayStatus());
  for (const auto& id : best_ref_info->TargetLaneId()) {
    trajectory_pb->add_lane_id()->CopyFrom(id); ③
  }

  trajectory_pb->set_trajectory_type(best_ref_info->trajectory_type());

  if (FLAGS_enable_rss_info) {
    trajectory_pb->mutable_rss_info()->CopyFrom(best_ref_info->rss_info());
  }

  best_ref_info->ExportDecision(trajectory_pb->mutable_decision());

  ...
  last_publishable_trajectory_.reset(new PublishableTrajectory(
      current_time_stamp, best_ref_info->trajectory())); ④
  ...

  if (FLAGS_enable_stitch_last_trajectory) {
    last_publishable_trajectory_->PrependTrajectoryPoints(
        std::vector<TrajectoryPoint>(stitching_trajectory.begin(),
                                     stitching_trajectory.end() - 1));
  }

  last_publishable_trajectory_->PopulateTrajectoryProtobuf(trajectory_pb); ⑤

  best_ref_info->ExportEngageAdvice(trajectory_pb->mutable_engage_advice());

  return status;
}

这段代码中的几个点说明如下：

planner_->Plan 是执行每个 Planner 的主体算法逻辑。
在有多个 ReferenceLineInfo 的情况下，选取最合适的一条。
记录 ReferenceLineInfo 包含的 lane id。
将轨迹的数据格式转换成 PublishableTrajectory 类型并记录到 last_publishable_trajectory_ 中。
导出成最终格式： ADCTrajectory 。Apollo 3.5 已经改用新的框架 Cyber RT，最终轨迹的发出是在 Cyber Component 的实现 PlanningComponent 发出。相关代码如下：

ADCTrajectory adc_trajectory_pb;
planning_base_->RunOnce(local_view_, &adc_trajectory_pb);
auto start_time = adc_trajectory_pb.header().timestamp_sec();
common::util::FillHeader(node_->Name(), &adc_trajectory_pb);
// modify trajectory relative time due to the timestamp change in header
const double dt = start_time - adc_trajectory_pb.header().timestamp_sec();
for (auto& p : *adc_trajectory_pb.mutable_trajectory_point()) {
  p.set_relative_time(p.relative_time() + dt);
}
planning_writer_->Write(std::make_shared<ADCTrajectory>(adc_trajectory_pb));

至此，决策规划模块的任务就完成了。接下来就是由控制模块按照这里发出的轨迹来控制车辆的行驶。