(九) carla中的全局路径规划

前言

从本节开始，学习并记录 $carla$ 中自带的车辆自主导航框架及算法研究。首先先学习规划模块。
路径规划对于一辆无人驾驶车辆来说就是根据给定的环境模型，在一定的约束条件下规划出一条连接当前位置和目标位置的无碰撞路径。自动驾驶汽车的路径规划从功能上可以分为全局路径规划、行为决策规划和局部运动规划。

全局路径规划，可以理解为实现自动驾驶汽车软件系统内部的导航功能，即在宏观层面上指导自动驾驶汽车软件系统的控制规划模块大致按照什么方向的道路走，从而引导车辆从起始点到达目标点。

行为决策规划可以简单地理解为自动驾驶汽车的“大脑”。全局路径规划模块产生的路径信息，直接被下游的行为决策规划模块所使用。行为决策规划模块接收到全局路径规划的结果，同时也接收感知、预测和地图信息，综合这些输入信息，行为决策规划模块决定车辆该如何行驶，比如正常跟车或者变道、在遇见红绿灯或者行人时等待避让以及在路口和其他车辆的交互等。

局部运动规划对无人车辆的行驶起着精确导航的作用，其任务就是在寻找到的全局最佳路径和最优行为策略的前提下，根据车辆当前的几何形状和动力学模型、实时所处环境的分布情况以及一个目标状态集，找到一系列的控制输入，驱动汽车从初始状态运动到目标状态集中的某一状态，并且在运动过程中避免和障碍物发生碰撞同时满足车辆的动力学约束。

全局路径规划程序讲解

说明

参考链接: link，这篇文章写的非常好。
在 $carla$ 自带的 $example$ 文件夹下有一个文件 $automatic\_control.py$。
参考这个自主导航程序，进行 $carla$ 的全局路径规划程序、行为规划程序、局部运动规划程序学习。
$automatic\_control.py$ 整个程序完成的功能是随机在地图上生成一辆车，然后随机生成一个无碰撞的目的地坐标，让车辆顺利到达目的地。

程序详解

在 $automatic\_control.py$ 中，最重要的是 $BehaviorAgent$ 这个 $class$。它就像是大脑一样，一切指令由它下达，车辆只需要按照这个指令执行就行。

1. 初始化BehaviorAgent类

在 $main$ 函数中，先实例化 $BehaviorAgent$ 类，$BehaviorAgent$ 在构建时需要两个输入，一个是属于 $Actorclass$ 的 $vehicle$，另一个就是车辆驾驶风格( $stringtype$ ，默认为 $normal$)。

agent = BehaviorAgent(world.player, behavior=args.behavior)

在 /CARLA/PythonAPI/carla/agents/navigation 文件夹下封装了跟车辆导航有关的代码。其中 $BehaviorAgent$ 这个类在 /CARLA/PythonAPI/carla/agents/navigation/ behavior_agent.py 这个文件中。
$BehaviorAgent$类中初始化函数如下，其中定义了车辆的一些参数以及 $agent$ 的行为参数。

    def __init__(self, vehicle, ignore_traffic_light=False, behavior='normal'):
        """
        Constructor method.

            :param vehicle: actor to apply to local planner logic onto
            :param ignore_traffic_light: boolean to ignore any traffic light
            :param behavior: type of agent to apply
        """

        super(BehaviorAgent, self).__init__(vehicle)
        self.vehicle = vehicle
        self.ignore_traffic_light = ignore_traffic_light
        self._local_planner = LocalPlanner(self)
        self._grp = None
        self.look_ahead_steps = 0

        # Vehicle information
        self.speed = 0
        self.speed_limit = 0
        self.direction = None
        self.incoming_direction = None
        self.incoming_waypoint = None
        self.start_waypoint = None
        self.end_waypoint = None
        self.is_at_traffic_light = 0
        self.light_state = "Green"
        self.light_id_to_ignore = -1
        self.min_speed = 5
        self.behavior = None
        self._sampling_resolution = 4.5

        # Parameters for agent behavior
        if behavior == 'cautious':
            self.behavior = Cautious()

        elif behavior == 'normal':
            self.behavior = Normal()

        elif behavior == 'aggressive':
            self.behavior = Aggressive()

self.behavior参数定义了车辆的几种行为，详细的函数在 /CARLA/PythonAPI/carla/agents/navigation/types_behavior.py 文件中，可以看出有三种模式:缓慢、正常、激进。三种模式的主要差异在车辆的速度限制($max\_speed$)、与前车保持的安全时间($safety\_time$)、与前车的最小安全距离($braking\_distance$)等相关参数。

2.全局路径规划

首先随机生成一个位置点，如果随机选择的点不是车辆当前所在的点，则这个点就当做目标点；如果随机选择的点是车辆当前所在的点，则选择下一个点当做目标点。

#Returns a list of recommended spawning points and random choice one from it
spawn_points = world.map.get_spawn_points()
random.shuffle(spawn_points)
#如果随机选择的点不是车辆当前所在的点，则这个点就当做目标点；
#如果随机选择的点是车辆当前所在的点，则选择下一个点当做目标点
if spawn_points[0].location != agent.vehicle.get_location():
   destination = spawn_points[0].location
else:
   destination = spawn_points[1].location

然后进行全局路径规划。$automatic\_control.py$ 中只用了一行代码就完成了全局路径规划，如下面 $set\_destination$ 函数所示，需要传进去三个参数:当前位置坐标、目的地坐标、是否清除以前保存的路径点:

agent.set_destination(agent.vehicle.get_location(), destination, clean=True)

但是深挖的话，里面还是有很多东西的。
进入到 $BehaviorAgent$ 这个类中，查看 $set\_destination$ 具体函数实现：

def set_destination(self, start_location, end_location, clean=False):
    """
    This method creates a list of waypoints from agent's position to destination location
    based on the route returned by the global router.

        :param start_location: initial position
        :param end_location: final position
        :param clean: boolean to clean the waypoint queue
    """
    #建立目标点函数，如果clean为True,则清除waypoints_queue中以前存在的点坐标
    if clean:
        self._local_planner.waypoints_queue.clear()
    #建立起始点和目标点
    self.start_waypoint = self._map.get_waypoint(start_location)
    self.end_waypoint = self._map.get_waypoint(end_location)

    route_trace = self._trace_route(self.start_waypoint, self.end_waypoint)

    self._local_planner.set_global_plan(route_trace, clean)

发现先传入起始点坐标和目标点坐标，然后进入 $BehaviorAgent$ 这个类中的_trace_route(self.start_waypoint, self.end_waypoint) 函数。继续查看 _trace_route(self.start_waypoint, self.end_waypoint) 函数，具体实现如下：

def _trace_route(self, start_waypoint, end_waypoint):
    """
    This method sets up a global router and returns the
    optimal route from start_waypoint to end_waypoint.
    建立一个全局导航实例对象，并返回从start_waypoint到end_waypoint的最佳路由

        :param start_waypoint: initial position
        :param end_waypoint: final position
    """
    # Setting up global router
    if self._grp is None:
        wld = self.vehicle.get_world()
        #这个类初始化需要两个参数，一个是carla里的map_get_map(),另一个就是sampling_resolution(默认为4.5m)
        #我们知道所谓的路径不过是一个个node与link连接起来，而sampling_resolution便定义了这两个节点之间的距离是多少
        #GlobalRoutePlannerDAO的主要作用是提取carla地图的拓扑结构
        dao = GlobalRoutePlannerDAO(
            wld.get_map(), sampling_resolution=self._sampling_resolution)
        grp = GlobalRoutePlanner(dao)
        grp.setup()
        self._grp = grp

    # Obtain route plan
    route = self._grp.trace_route(
        start_waypoint.transform.location,
        end_waypoint.transform.location)

    return route

这个函数中首先利用 $GlobalRoutePlannerDAO()$ 类提取 $carla$ 地图的拓扑结构。然后利用 $GlobalRoutePlanner()$ 进行全局路径规划。

2.1 拓扑地图提取

$GlobalRoutePlannerDAO()$ 主要作用就是提取 $carla$ 地图的拓朴结构。
$GlobalRoutePlannerDAO()$ 在 /CARLA/PythonAPI/carla/agents/navigation/global_route_planner_dao.py下。

$GlobalRoutePlanner()$ 在 /CARLA/PythonAPI/carla/agents/navigation/global_route_planner.py下。

在上述 $\_trace\_route$ 函数中可以看出，实例化 $GlobalRoutePlannerDAO()$ 和 $GlobalRoutePlanner()$ 后，主要是 $.setup()$ 函数。
进入 $GlobalRoutePlanner()$ 类的 $setup$ 函数，具体如下:

def setup(self):
    """
    Performs initial server data lookup for detailed topology
    and builds graph representation of the world map.
    """
    # 获取当前map的拓扑结构
    self._topology = self._dao.get_topology()
    # _build_graph() 会提取成员变量topology返回三个变量。
    # self._graph是最终需要的netoworx.Diagraph()对象。
    self._graph, self._id_map, self._road_id_to_edge = self._build_graph()
    self._find_loose_ends()
    self._lane_change_link()

首先利用 $GlobalRoutePlannerDAO()$ 中 $get\_topology()$ 函数获取拓扑地图，具体细节如下：

def get_topology(self):
    """
    Accessor for topology.
    This function retrieves topology from the server as a list of
    road segments as pairs of waypoint objects, and processes the
    topology into a list of dictionary objects.
    此函数从服务器检索拓扑，将其作为路段列表(路点对象对)，并将拓扑处理为字典对象列表

        :return topology: list of dictionary objects with the following attributes
            entry   -   waypoint of entry point of road segment  路段入口点的路点
            entryxyz-   (x,y,z) of entry point of road segment (x,y,z)的路段入口点
            exit    -   waypoint of exit point of road segment 路段出口点的路点
            exitxyz -   (x,y,z) of exit point of road segment  (x,y,z)的路段出口点
            path    -   list of waypoints separated by 1m from entry
                        to exit 从入口到出口，间隔1米的航路点列表
    """
    topology = []
    # Retrieving waypoints to construct a detailed topology
    # 检索路径点以构造详细的拓扑结构
    # self._wmap.get_topology()是列表类型，self._wmap.get_topology()[0]是元组类型
    # print("type(self._wmap.get_topology()[0])", type(self._wmap.get_topology()[0]))
    # self._wmap.get_topology()本质是一个list of dictionary,每一个dictionary都是一段在carla中提前定义好的lane segment
    # entry和exit是一段lane segment的开始和末尾点,Path是一个list,里面装着entry和exit之间的node,每个node相距4.5m,
    # 每一个node都是一个waypoint class,他永远位于lane的中央，同时还可以记录左右lane上平行的waypoint的位置
    for segment in self._wmap.get_topology():
        wp1, wp2 = segment[0], segment[1]
        l1, l2 = wp1.transform.location, wp2.transform.location
        # Rounding off to avoid floating point imprecision
        x1, y1, z1, x2, y2, z2 = np.round([l1.x, l1.y, l1.z, l2.x, l2.y, l2.z], 0)
        wp1.transform.location, wp2.transform.location = l1, l2
        seg_dict = dict()
        seg_dict['entry'], seg_dict['exit'] = wp1, wp2
        seg_dict['entryxyz'], seg_dict['exitxyz'] = (x1, y1, z1), (x2, y2, z2)
        seg_dict['path'] = []
        endloc = wp2.transform.location
        if wp1.transform.location.distance(endloc) > self._sampling_resolution:
            w = wp1.next(self._sampling_resolution)[0]
            while w.transform.location.distance(endloc) > self._sampling_resolution:
                seg_dict['path'].append(w)
                w = w.next(self._sampling_resolution)[0]
        else:
            seg_dict['path'].append(wp1.next(self._sampling_resolution)[0])
        topology.append(seg_dict)
    return topology

可以通过上述程序，明显看出：得到的 $topology$ 实际上是一个字典列表类型，每一个字典数据都是一段在 $carla$ 中提前定义好的道路片段。$entry$ 和 $exit$ 是每一段道路片段的起始点和终止点。$path$ 是一个列表，里面存着起始点到终止点之间的道路节点，节点之间相距 ${}_{s}amplin{g}_{r}esolution$ 米(默认 $4.5$ m)。
这样就建立好了 $carla$ 整个 $map$ 的拓扑地图。

2.2 搜索 $Graph$ 建立

上面已经得到了$carla$ 整个 $map$ 的拓扑地图，这里利用得到的拓扑地图来构建道路的 $graph$。
进入 $GlobalRoutePlanner()$ 类的 $setup$ 函数，可以看到建立 $graph$ 是利用 $GlobalRoutePlanner()$ 类中的 $\_build\_graph()$ 函数。

def setup(self):
    """
    Performs initial server data lookup for detailed topology
    and builds graph representation of the world map.
    """
    # 获取当前map的拓扑结构
    self._topology = self._dao.get_topology()
    # _build_graph() 会提取成员变量topology返回三个变量。
    # self._graph是最终需要的netoworx.Diagraph()对象。
    self._graph, self._id_map, self._road_id_to_edge = self._build_graph()
    self._find_loose_ends()
    self._lane_change_link()

查看 $\_build\_graph()$ 函数，具体如下：

def _build_graph(self):
   """
   This function builds a networkx graph representation of topology.
   The topology is read from self._topology.
   graph node properties:
       vertex   -   (x,y,z) position in world map
   graph edge properties:
       entry_vector    -   unit vector along tangent at entry point 沿入口点切线的单位向量
       exit_vector     -   unit vector along tangent at exit point 沿出口点切线的单位向量
       net_vector      -   unit vector of the chord from entry to exit 从入口到出口的弦的单位向量
       intersection    -   boolean indicating if the edge belongs to an
                           intersection 布尔值，表示边是否属于交点
   return      :   graph -> networkx graph representing the world map,
                   id_map-> mapping from (x,y,z) to node id
                   road_id_to_edge-> map from road id to edge in the graph
   构建图结构：这一步会利用上面提取的topology(list格式）来构建道路的graph(networkx.Diagraph()格式）
   它是不断地遍历topology list, 然后通过add_node与add_edge来一步步构建完整的图谱。
   """
   # id_map是一个辞典，key是每一个entry/exit waypoint的坐标位置，value是它们在graph里的id。
   # road_id_to_edge，则是一个三层字典，最外层是carla里的道路id,
   # 中间一层是carla里的在该道路上的section_id,最后一层是carla里的Lane_id,里面对应着graph里的node.id。
   # 这个看起来很复杂，其实就是为了做一件事:为了把networkx graph中的每个node都和carla里的信息一一对应，方便后面做信息转换
   graph = nx.DiGraph() #无多重边有向图
   id_map = dict()  # Map with structure {(x,y,z): id, ... }
   road_id_to_edge = dict()  # Map with structure {road_id: {lane_id: edge, ... }, ... }

   for segment in self._topology:
       entry_xyz, exit_xyz = segment['entryxyz'], segment['exitxyz']
       path = segment['path']
       entry_wp, exit_wp = segment['entry'], segment['exit']
       #判断入口点是否是junction,如果是的话返回true,否则返回false
       intersection = entry_wp.is_junction
       #road_id,section_id,lane_id
       road_id, section_id, lane_id = entry_wp.road_id, entry_wp.section_id, entry_wp.lane_id
       # entry_xyz, exit_xyz为两个元组
       for vertex in entry_xyz, exit_xyz:
           # Adding unique nodes and populating id_map
           if vertex not in id_map:
               new_id = len(id_map)
               id_map[vertex] = new_id
               graph.add_node(new_id, vertex=vertex)
       #获取起点和终点的graph的id号
       n1 = id_map[entry_xyz]
       n2 = id_map[exit_xyz]
       if road_id not in road_id_to_edge:
           road_id_to_edge[road_id] = dict()
       if section_id not in road_id_to_edge[road_id]:
           road_id_to_edge[road_id][section_id] = dict()
       road_id_to_edge[road_id][section_id][lane_id] = (n1, n2)

       entry_carla_vector = entry_wp.transform.rotation.get_forward_vector()
       exit_carla_vector = exit_wp.transform.rotation.get_forward_vector()

       # Adding edge with attributes
       graph.add_edge(
           n1, n2,
           length=len(path) + 1, path=path,
           entry_waypoint=entry_wp, exit_waypoint=exit_wp,
           entry_vector=np.array(
               [entry_carla_vector.x, entry_carla_vector.y, entry_carla_vector.z]),
           exit_vector=np.array(
               [exit_carla_vector.x, exit_carla_vector.y, exit_carla_vector.z]),
           net_vector=vector(entry_wp.transform.location, exit_wp.transform.location),
           intersection=intersection, type=RoadOption.LANEFOLLOW)

   return graph, id_map, road_id_to_edge

$\_build\_graph()$ 函数最后返回三个变量，其中：
$id\_map$、$road\_id\_to\_edge$ 这两个字典数据类型将刚才建立的拓扑地图的点和边用字典类型存储起来，将 $id$ 号与对应路段对应起来。$id\_map$ 很容易看懂，$key$ 是每一个 $entry/exitwaypoint$ 的坐标位置，$value$ 是它们在 $graph$ 里的 $id$，从 $0$ 开始注意编号递增。
$road\_id\_to\_edge$ 稍微复杂一点，是一个三层字典，$key$ 最外层是 $carla$ 里的 $Road\_id$， 中间一层是 $carla$ 里的在该道路上的 $Section\_id$，最后一层是 $carla$ 里的 $Lane\_id$，$value$ 里面对应着 $graph$ 里的起始点和终止点的 $id$。
这里$Road$ 路段的确定参考了 $OpenDrive$ 地图标准格式，如下图所示。

$graph$ 数据结构中存储的数据是：通过不断遍历建立好的拓扑图 $topology$ 的 $list$，通过$graph.add\_node()$ 和 $graph.add\_edge()$ 函数来添加点和边。
$graph.add\_node()$ 函数将建立 $id\_map$ 时起始点和终止点得到的 $id$ 加进去。
$graph.add\_edge()$ 中添加了起始点和终止点得到的 $id$、起始点和终止点之间的路径点数量、起始点和终止点之间生成的路径点 $path$，起始点和终止点的坐标信息，以及$entry\_vector$，$exit\_vector$ 以向量的形式记录了车辆如果正常驶入和离开这条 $lane$ 时它的角度是怎样的。$net\_vector$ 记录了起始点和终止点之间向量与三个轴之间的倾斜情况。 $intersection$ 判断入口点是否是 $junction$ 。还有这一路段的类型：$RoadOption.LANEFOLLOW$。

经过上述分析，$\_build\_graph()$ 函数构建的图的边与边之间是以点的坐标作为连接的，这样构建的图只考虑了纵向连接，并没有考虑横向路段之间的连接。如果只是这样构建图的话，就会导致一个严重的问题：车辆无法完成换道超车等行为，全局规划出来的路线只能进行纵向行为，即沿着一条路走。
继续往下看 $setup$ 函数，$self.\_find\_loose\_ends()$ 函数，这个函数基本和 $\_build\_graph()$ 函数类似，是为了处理一些边缘路段，将这个边缘路段加入到图中。
继续往下看 $setup$ 函数，$self.\_lane\_change\_link()$ 函数则考虑横向连接。如果当前点的右/;左方车道不为空且车道类型是 $Driving$ 且右/左方车道与本 $waypoint$ 所在车道的 $road\_id$ 是同一个,则添加横向向右/左的 $edge$。

def _lane_change_link(self):
    """
    This method places zero cost links in the topology graph
    representing availability of lane changes.
    """

    for segment in self._topology:
        left_found, right_found = False, False
        print("len(segment['path']):", len(segment['path']))
        for waypoint in segment['path']:
            if not segment['entry'].is_junction:
                next_waypoint, next_road_option, next_segment = None, None, None

                if waypoint.right_lane_marking.lane_change & carla.LaneChange.Right and not right_found:
                    #如果当前点的右方车道不为空且车道类型是Driving且右方车道与本waypoint所在车道的road_id是同一个,则添加横向向右的edge。
                    next_waypoint = waypoint.get_right_lane()
                    if next_waypoint is not None and next_waypoint.lane_type == carla.LaneType.Driving and waypoint.road_id == next_waypoint.road_id:
                        next_road_option = RoadOption.CHANGELANERIGHT
                        #获取当前车道的右边车道最近点处的road_id_to_edge
                        next_segment = self._localize(next_waypoint.transform.location)
                        #如果以前不存在，则将其加入到graph中
                        if next_segment is not None:
                            self._graph.add_edge(
                                self._id_map[segment['entryxyz']], next_segment[0], entry_waypoint=waypoint,
                                exit_waypoint=next_waypoint, intersection=False, exit_vector=None,
                                path=[], length=0, type=next_road_option, change_waypoint=next_waypoint)
                            right_found = True
                # 如果当前点的左方车道不为空且车道类型是Driving且左方车道与本waypoint所在车道的road_id是同一个,则添加横向向左的edge。
                if waypoint.left_lane_marking.lane_change & carla.LaneChange.Left and not left_found:
                    next_waypoint = waypoint.get_left_lane()
                    if next_waypoint is not None and next_waypoint.lane_type == carla.LaneType.Driving and waypoint.road_id == next_waypoint.road_id:
                        next_road_option = RoadOption.CHANGELANELEFT
                        # 获取当前车道的左边车道最近点处的road_id_to_edge
                        next_segment = self._localize(next_waypoint.transform.location)
                        # 如果以前不存在，则将其加入到graph中
                        if next_segment is not None:
                            self._graph.add_edge(
                                self._id_map[segment['entryxyz']], next_segment[0], entry_waypoint=waypoint,
                                exit_waypoint=next_waypoint, intersection=False, exit_vector=None,
                                path=[], length=0, type=next_road_option, change_waypoint=next_waypoint)
                            left_found = True
            if left_found and right_found:
                break

到此， 搜索 $Graph$ 建立好了。
然后返回继续查看 $BehaviorAgent$ 这个类中的_trace_route(self.start_waypoint, self.end_waypoint) 函数，建立好 $Graph$后，执行下面函数。

# Obtain route plan
route = self._grp.trace_route(
start_waypoint.transform.location,
end_waypoint.transform.location)

trace_route(start_waypoint.transform.location, end_waypoint.transform.location) 函数在 $GlobalRoutePlanner$ 类中，这个函数传入起始点和目标点的坐标，returns list of (carla.Waypoint, RoadOption) from origin to destination，即可完成全局路径规划。

2.3 全局路径规划

进入 $trace\_route$ 函数。
首先传入起始点和终止点，利用 $\_path\_search$ 函数进行路径查找。

route = self._path_search(origin, destination)

进入 $\_path\_search$ 函数，发现这个函数就调用了 $networkx$ 里自带的 $A^{\ast }$ 算法，至于$networkx$ 里自带的 $A^{\ast }$ 算法的细节，在另一篇博客再介绍。

route = nx.astar_path(
            self._graph, source=start[0], target=end[0],
            heuristic=self._distance_heuristic, weight='length')

这样 $\_path\_search$ 函数就返回了一个 $route$ 的列表。
得到这个列表后， $trace\_route$ 函数还针对每一个 $route$ 点分析了这一点下一时刻应采取的行为，为后续的行为规划做准备。
这样就完成了全局路径规划。