Apollo 6.0 参考线 ReferenceLine 生成

相关文章：

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

《apollo介绍之参考线（二十七）》

《Apollo 6.0 参考线平滑算法解析》

《Apollo 6.0 pnc_map解析》

《pnc_map模块（规划与控制地图）》

简介

参考线在apollo中起到了关键的承上启下的作用。

它收集了上游路由，高精地图，定位等模块的信息，整合后生成一段基于车道中心线的平滑的路径，输送给下游决策规划控制模块使用。

在阅读了几遍源码和网上的介绍后，我想要整理出一版参考线生成的简化版本。

主要参考的还是下面这个链接。

pnc_map模块（规划与控制地图）_lzw0107的博客-CSDN博客blog.csdn.net/lzw0107/article/details/107814610#t1正在上传…重新上传取消https://link.zhihu.com/?target=https%3A//blog.csdn.net/lzw0107/article/details/107814610%23t1

我补充说明了一些上面的链接中部分未解释到的细节。

为了让思路更清晰，本文章未对解释如何实现某功能，而更注重于该模块实现了什么，以及每个模块的输入和输出，对实现细节感兴趣的同学可以去自行查阅源码。

参考线生成流程

从Apollo中Planning模块的入口说起，调用流程大致为：

其中，箭头代表函数间的调用关系。例如，Planning模块入口为PlanningComponent::Init(...)，它调用了OnLanePlanning::Init(...)，而后调用ReferenceLineProvider中的函数开启新的线程来生成ReferenceLine，其刷新的频率为20Hz。

本文章主要为了说明生成参考线的这一过程，即ReferenceLineProvider::CreateReferenceLine(...)函数的说明。该函数根据调用顺序可以分为以下三部分，也是本文章的大体结构。

第一部分：路由响应的处理

RoutingResponse数据结构

首先，我们需要了解路由输出的结果的数据结构，在apollo中，它们被定义在modules/routing/proto/routing.proto文件中，我挑选了比较重要的数据展示在下面。

其中我们最为关注的为RoutingResponse，它为参考线接收到的数据。

// 为路由模块的输入，车辆在全局地图中必须经过的点
// PointENU为东北天坐标系，apollo中被简化为二维横轴墨卡托坐标系
message LaneWaypoint {optional string id = 1;optional double s = 2;optional apollo.common.PointENU pose = 3;// When the developer selects a point on the dreamview route editing// the direction can be specified by dragging the mouse// dreamview calculates the heading based on this to support construct lane way point with headingoptional double heading = 4;
}// 其中id存储的为全局唯一的车道id，即定义在map_lane.proto中的Lane的id
message LaneSegment {optional string id = 1;optional double start_s = 2;optional double end_s = 3;
}message RoutingRequest {optional apollo.common.Header header = 1;// at least two points. The first is start point, the end is final point.// The routing must go through each point in waypoint.repeated LaneWaypoint waypoint = 2;repeated LaneSegment blacklisted_lane = 3;repeated string blacklisted_road = 4;optional bool broadcast = 5 [default = true];optional apollo.hdmap.ParkingSpace parking_space = 6 [deprecated = true];optional ParkingInfo parking_info = 7;optional DeadEndInfo dead_end_info = 8;
}enum ChangeLaneType {FORWARD = 0;LEFT = 1;RIGHT = 2;
};message Passage {repeated LaneSegment segment = 1;optional bool can_exit = 2;optional ChangeLaneType change_lane_type = 3 [default = FORWARD];
}message RoadSegment {optional string id = 1;repeated Passage passage = 2;
}message RoutingResponse {optional apollo.common.Header header = 1;repeated RoadSegment road = 2;optional Measurement measurement = 3;optional RoutingRequest routing_request = 4;// the map version which is used to build road graphoptional bytes map_version = 5;optional apollo.common.StatusPb status = 6;
}

而其中最重要的为RoadSegment，路由模块将道路结构分解为多个RoadSegment，每个RoadSegment又包括多个通道：Passage，每个Passage又由多个LaneSegment组成。

如图所示，为双车道中的一个例子。红色点为Routing起点，橙色为终点。

则RoutingResponse中给我们的结果就是RoadSegment1, 2, 3。

RoadSegment1中包括两个Passage（即每个车道为一个通道），每个Passage又包括三个LaneSegment。

这样做有什么好处呢？

当我们为RoadSegment，Passage，LaneSegment编号时，就可以将Routing模块输出的地图道路信息中每个车道中的每个LaneSegment快速的索引出来。如图中（1, 1, 1）代表左下角红圈所在的LaneSegment。

另外需要注意的是Passage中的两个属性:

can_exit : 代表能否从当前通道去下一个通道。用于判断将来是否需要变道。例如Passage1中can_exit为false， Passage2中为true。

change_lane_type : 代表当前通道是直行（FORWARD），左转（LEFT）还是右转（RIGHT）。图中Passage1为RIGHT，因为需要右转才能到终点。Passage2为FORWARD。

RoadSegment，Passage，LaneSegment图解：

更新路由信息

主要函数为：

PncMap::UpdateRoutingResponse(const routing::RoutingResponse &routing)

可以看到，其输入为RoutingResponse成员，即routing模块的输出。

其主要功能为初始化下面加粗的三个pnc_map类成员：

在得到RoutingResponse后，该函数为所有的routing::LaneSegment构建了索引列表route_indices_ ，其类型为vector<RouteIndex>。并将索引相对应的LaneSegment类型转换为hdmap::LaneSegment存进RouteIndex。

  struct RouteIndex {//此处类型为hdmap::LaneSegmentLaneSegment segment;//{road_index, passage_index, lane_index}std::array<int, 3> index;};

2. 将LaneSegment中的车道的lane_id取出，存入all_lane_ids_，和route_indices_ 中的LaneSegment一一对应。

3. 查询起始点和终点在哪段LaneSegment中，存入routing_waypoint_index_。

第二部分：根据车辆当前位置生成短期通道

这一部分主要涉及到的函数为ReferenceLineProvider::CreateRouteSegments（...）调用的PncMap::GetRouteSegments（...）

输入为车辆当前位置，前向查找距离，后向查找距离，

输出为std::list<RouteSegments>，list中每一个RouteSegments代表一条参考线经过的RouteSegments。在apollo中，该list的元素被限制为了最多为两个，原因后面会说明。

PncMap::GetRouteSegments(const VehicleState &vehicle_state,const double backward_length,const double forward_length,std::list<RouteSegments> *const route_segments)

RouteSegments类型为公有继承的std::vector<LaneSegment>，我们来看apollo中对它的说明，简单来说，该类从Passage扩展而来，保留了Passage的某些特征。该类包含了生成hdmap::Path类的原始数据，而Path类又可以用于初始化参考线。

也就是说我们在这一步主要是生成连接起来的可通行的Passage，从而为生成参考线做准备。

/*** @brief class RouteSegments** This class is a representation of the Passage type in routing.proto.* It is extended from a passage region, but keeps some properties of the* passage, such as the last end LaneWaypoint of the original passage region* (route_end_waypoint), whether the passage can lead to another passage in* routing (can_exit_).* This class contains the original data that can be used to generate* hdmap::Path.**/
class RouteSegments : public std::vector<LaneSegment>

该函数大致分为三步：

更新pnc map中无人车状态

主要函数为PncMap::UpdateVehicleState(const VehicleState &vehicle_state) ，输入为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;optional double steering_percentage = 16;
}

VehicleState在这里主要用到了其全局坐标x，y，以及自车航向角heading。

这一部分就是根据自车的全局坐标系以及航向角，找到自车在哪一个LaneSegment中，或者说投影后距离那个LaneSegment最近，输出自车所在LaneSegment对应的index，并计算离自车最近的下一个必经点LaneWaypoint所在的LaneSegment对应的index。

计算临近通道

主要函数为GetNeighborPassages。

输入为自车所在的RoadSegment和通道Passage的id，如果当前Passage的change_lane_type为RIGHT，则输出为当前RoadSegment中自车右手边全部通道Passage的id，包括自车所在通道。若为LEFT，则输出左边。

举例来说，蓝色车为自车，则该函数输出Passage1, 2。

若不为双车道，例如右边有Passage3，Passage4，则输出为Passage1，2，3，4。

生成list<RouteSegments>

遍历第二步中输出的全部通道，

若为自车所在通道，根据前向查找距离，后向查找距离生成一条短期通道RouteSegments，存入list<RouteSegments>。

若不为自车所在通道，调用CanDriveFrom查看能否从自车位置驶入该通道，若能则将自车投影到该通道并根据前向查找距离，后向查找距离生成一条短期通道RouteSegments，存入list<RouteSegments>。

CanDriveFrom函数判定依据为：

自车能投影到该Passage上，自车距离该Passage横向距离不能超过一条车道的宽度，自车heading应该和该Passage所在车道方向一致等。

上面标黑的条件导致了最终生成的list最多只有两个RouteSegments！

因此，对于上例来说，最终生成的list<RouteSegments>可能长这样，图中灰色区域：

至于为什么RouteSegments1会延长超出当前RoadSegment2, 感兴趣的同学可以去查看ExtendSegments中的算法。

至此，ReferenceLineProvider::CreateRouteSegments函数运行完毕。

第三部分：初始化Path

这一部分涉及到ReferenceLineProvider::SmoothRouteSegment函数，该函数输入为RouteSegments（上图中灰色区域），输出为ReferenceLine。

首先，会根据输入的RouteSegments初始化一条Path。

Path的构造函数重载了很多版本，而当输入只有vector<LaneSegment>（RouteSegments为其子类）时，构造函数会调用MapPathPoint::GetPointsFromLane(...)来从LaneSegment中取出车道中心线的构成点来初始化Path。

Path::Path(const std::vector<LaneSegment>& segments): lane_segments_(segments) {for (const auto& segment : lane_segments_) {const auto points = MapPathPoint::GetPointsFromLane(segment.lane, segment.start_s, segment.end_s);path_points_.insert(path_points_.end(), points.begin(), points.end());}MapPathPoint::RemoveDuplicates(&path_points_);CHECK_GE(path_points_.size(), 2U);Init();
}

MapPathPoint::GetPointsFromLane(...)的输入为当前LaneSegment所在的车道，起点s和终点s。这里segment.lane为指向LaneInfo类的指针。

apollo中车道的数据结构主要写在modules/map/proto/map_lane.proto中，然后又用LaneInfo对proto数据做了一层封装。感兴趣的同学可以去查阅。

根据输入，该函数取出车道数据中的中线构成点的世界坐标和Frenet坐标，然后根据这些信息初始化MapPathPoint点，也就是构成Path的基本元素，输出vector<MapPathPoint>。

调用Init()为Path中的点赋予属性

void Path::Init() {InitPoints();InitLaneSegments();InitPointIndex();InitWidth();InitOverlaps();
}

初始化函数为Path点初始化一些Path类易维护的属性以及行车过程中所需要的必要信息。

例如InitWidth为初始化该路径点距离两边的车道边缘距离，距离两边道路边缘距离。

InitOverlaps为初始化该路径经过的一些和交通元素等的一些重叠，细节部分可以重开一帖再写。

平滑参考线

在得到最终Path后，用它来构造一条粗糙的ReferenceLine，然后通过函数ReferenceLineProvider::SmoothReferenceLine调用smoother平滑该参考线得到最终的参考线。

平滑参考线见《Apollo6.0_ReferenceLine_Smoother解析与子方法对比》