[人工智能](九) carla中的全局路径规划

(九) 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()$ 类中的 ${}_{b}uil{d}_{g}raph()$ 函数。

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$、起始点和终止点之间的路径点数量、起始点和终止点的向量。
剩下的后续再写。