基于Carla的EMplanner教程二：基于A*的全局路径规划完全解析

通过topo = amap.get_topology()可以获得OpenDRIVE 文件拓扑的最小图形的元组列表。[（w0， w1）， （w0， w2）， （w1， w3）， （w2， w3）， （w0， w4）]表示从w0指向w1的航路点。表示前一个点可以到达后一个点。代码获得的实际输出是：

 [(<carla.libcarla.Waypoint object at 0x0000028C379826F0>, <carla.libcarla.Waypoint object at 0x0000028C379828D0>), (<carla.libcarla.Waypoint object at 0x0000028C379821B0>, ....]

Networkx创建有向图

nx.DiGraph()用来创建有向图。代码如下图所示：

import networkx as nx

# 创建一个有向图
G = nx.DiGraph()

# 添加节点
G.add_node(1)
G.add_node(2)
G.add_node(3)

# 添加边
G.add_edge(1, 2)  # 从节点1到节点2添加一条有向边
G.add_edge(2, 3)  # 从节点2到节点3添加一条有向边

# 获取图的节点和边信息
print("Nodes:", G.nodes())  # 输出: [1, 2, 3]
print("Edges:", G.edges())  # 输出: [(1, 2), (2, 3)]

具体到Carla中全局路径规划的过程，如何创建这个有向图。首先通过利用下列函数来处理get_topology()获得的元组列表。将每一个元组添加节点中间的采样点：

    def _build_topology(self):
        """
        the output of carla.Map.get_topology() could look like this: [(w0, w1), (w0, w2), (w1, w3), (w2, w3), (w0, w4)].
        由于carla.Map.get_topology()只能函数获取起点和终点信息构成的边信息，这些信息不能够为全局路径规划提供细节信息，因此需要重新构建拓扑
        新拓扑用字典类型存储每个路段，具有以下结构：
        {
        entry (carla.Waypoint): waypoint of entry point of road segment，
        exit (carla.Waypoint): waypoint of exit point of road segment，
        path (list of carla.Waypoint):  list of waypoints between entry to exit, separated by the resolution
        }
        :return None
        依次处理地图生成的topology信息，对于每一对信息，比如(w1,w2)来说。w1和w2之间间距太大，所以需要在每一对数据中增加采样点。

        """
        self._topology = []
        for seg in self._map.get_topology():
            w1 = seg[0]  # type: carla.Waypoint  # 取出一对元组。
            w2 = seg[1]  # type: carla.Waypoint
            new_seg = dict()
            new_seg["entry"] = w1
            new_seg["exit"] = w2
            new_seg["path"] = []
            # 按照采样分辨率将w1和w2之间的路径点采样出来
            w1_loc = w1.transform.location  # type: carla.Location
            # 实现的功能是，当w1和w2距离大于采样分辨率时候，就从w1作为起点开始采样点，找w1的next的第一个路点。
            # 如果新的点距离w2还是大于采样分辨率，就一直采样，知道采样点和w2距离小于采样点为止。
            if w1_loc.distance(w2.transform.location) > self._sampling_resolution:
                # 如果起始路点和结束路点之间存在其他路点，则根据采样分辨率将中间点全部存储在new_seg["path"]中
                # 方法next(self, distance)是返回与当前航电近似distance处的航点列表。
                new_waypoint = w1.next(self._sampling_resolution)[0]  # 这里从起始路点的下一个开始，
                while new_waypoint.transform.location.distance(w2.transform.location) > self._sampling_resolution:
                    # 结束路点不会记录到new_seg["path"]中
                    new_seg["path"].append(new_waypoint)
                    new_waypoint = new_waypoint.next(self._sampling_resolution)[0]
            else:  # 如果起始路点和结束路点之间的距离小于或等于采样分辨率，则仍然让new_seg["path"]保持空列表
                # new_seg["path"].append(w1.next(self._sampling_resolution)[0])
                pass
            print("new_seg:新采样：", new_seg)
            self._topology.append(new_seg)

这样处理以后得到的一个元组信息输出如下：

new_seg:新采样： {'entry': <carla.libcarla.Waypoint object at 0x000001C2B2509C30>, 'exit': <carla.libcarla.Waypoint object at 0x000001C2B2509C90>, 'path': [<carla.libcarla.Waypoint object at 0x000001C2B2668510>, <carla.libcarla.Waypoint object at 0x000001C2B2668AB0>, <carla.libcarla.Waypoint object at 0x000001C2B2668B70>, <carla.libcarla.Waypoint object at 0x000001C2B2668BD0>, <carla.libcarla.Waypoint object at 0x000001C2B2668C30>, <carla.libcarla.Waypoint object at 0x000001C2B2668C90>, <carla.libcarla.Waypoint object at 0x000001C2B2668CF0>, <carla.libcarla.Waypoint object at 0x000001C2B2668D50>, <carla.libcarla.Waypoint object at 0x000001C2B2668DB0>, <carla.libcarla.Waypoint object at 0x000001C2B2668E10>, <carla.libcarla.Waypoint object at 0x000001C2B2668E70>, <carla.libcarla.Waypoint object at 0x000001C2B2668ED0>, <carla.libcarla.Waypoint object at 0x000001C2B2668F30>, <carla.libcarla.Waypoint object at 0x000001C2B2668F90>, <carla.libcarla.Waypoint object at 0x000001C2B2669030>]}

每一个new_seg为一个元组，entry表示起点节点、exti为终点节点。path表示entry和exit之间的新添加的为了满足采样分辨率补充的新节点。

构建图

从成员函数_build_topology()进行数据处理以后，就可以对self._topology中的数据进行构建图了。依次读取每一个new_seg。self._graph是一个二向图。如下图所示代码：

        for seg in self._topology:
            entry_waypoint = seg["entry"]  # type: carla.Waypoint
            exit_waypoint = seg["exit"]  # type: carla.Waypoint
            path = seg["path"]  # 不包含端点
            intersection = entry_waypoint.is_intersection  # 判断是否在交叉路口
            road_id, section_id, lane_id = entry_waypoint.road_id, entry_waypoint.section_id, entry_waypoint.lane_id
            entry_xyz = entry_waypoint.transform.location
            entry_xyz = (np.round(entry_xyz.x, 2), np.round(entry_xyz.y, 2), np.round(entry_xyz.z, 2))  # 对小数长度进行限制
            exit_xyz = exit_waypoint.transform.location
            exit_xyz = (np.round(exit_xyz.x, 2), np.round(exit_xyz.y, 2), np.round(exit_xyz.z, 2))
            for xyz in entry_xyz, exit_xyz:
                if xyz not in self._id_map:
                    New_ID = len(self._id_map)  # 建立节点位置和ID对应，ID从0开始。标记有几个节点。
                    self._id_map[xyz] = New_ID  # # 字典类型，建立节点id和位置的对应{(x, y, z): id}
                    # 将新的节点加入graph
                    self._graph.add_node(New_ID, vertex=xyz)  # 图分为节点属性和边属性。

            n1 = self._id_map[entry_xyz]
            n2 = self._id_map[exit_xyz]

            if road_id not in self._road_to_edge:
                self._road_to_edge[road_id] = dict()
            if section_id not in self._road_to_edge[road_id]:
                self._road_to_edge[road_id][section_id] = dict()
            # 会有左右车道和多车道的情况 举例 13: {0: {-1: (34, 46), 1: (47, 31)}}，
            # 即id为13的道路，包含一个section,这个section是双向单车道
            self._road_to_edge[road_id][section_id][lane_id] = (n1, n2)
            # 输出显示为如下：总共有160个
            # 0: {0: {-3: (0, 1), -2: (2, 3), -1: (4, 5), 4: (6, 7), 5: (8, 9), 6: (10, 11)}}
            # 1: {0: {-3: (12, 13), -2: (14, 15), -1: (16, 17), 4: (18, 19), 5: (20, 21), 6: (22, 23)}}

            entry_forward_vector = entry_waypoint.transform.rotation.get_forward_vector()  # 这里是入口节点的方向信息
            exit_forward_vector = exit_waypoint.transform.rotation.get_forward_vector()  # 这里是出口节点的方向信息，用于车辆规划路径时的转向

            # 将新的边加入graph
            self._graph.add_edge(u_of_edge=n1, v_of_edge=n2,
                                 length=len(path) + 1, path=path,
                                 entry_waypoint=entry_waypoint, exit_waypoint=exit_waypoint,
                                 entry_vector=entry_forward_vector, exit_vector=exit_forward_vector,
                                 net_vector=planner_utiles.Vector_fun(entry_waypoint.transform.location,
                                                                      exit_waypoint.transform.location),
                                 intersection=intersection, type=RoadOption.LANE_FOLLOW)

如何使用A*规划一条指定起点、终点的全局路径

    def _route_search(self, origin, destination):
        """
        去顶从起点到终点的最优距离
        :param origin: carla.Location 类型
        :param destination:
        :return: list类型，成员是图中节点id
        """
        start_edge = self._find_location_edge(origin)  # 获取起点所在边
        # print("起点所在边：", start_edge)   起点所在边： (20, 21)
        end_edge = self._find_location_edge(destination)  # 获取终点所在边
        # print("终点所在边：", end_edge)  终点所在边： (2, 3)
        route = self._A_star(start_edge[0], end_edge[0])
        if route is None:  # 如果不可达就报错
            raise nx.NetworkXNoPath(f"Node {start_edge[0]} not reachable from {end_edge[0]}")
        route.append(end_edge[1])  # 可达的话就将终点所在变得右端点加入路径
        return route

这里有一个关键函数_A_star()，具体实现方法下面细说。在介绍A*之前先要了解一下 Dijkstra算法 和 最佳优先搜索。这两个一个对应移动代价，一个对应着终点距离。

首先回顾一下A*算法的原理，这需要从广度优先算法说起，广度优先遍历首先是从起点开始，遍历起点周围的点，然后再遍历已经遍历的点，逐渐向外扩张，直到终点。执行算法过程中，每一个节点需要记录下其父节点，以便于找到终点的时候顺着父节点可以找到起点。

Dijkstra算法

这个算法用来寻找图像节点最短的点，这个算法种，每一次移动需要计算节点距离起点的移动代价(这个代价一般都是距离，如欧氏距离等),同时需要有一个优先队列，对于所有待遍历的节点，都需要加入到优先队列中按照代价进行排列。算法运行中，每次都从优先队列中选出来代价最小的作为下一个遍历的节点，直到到达终点。

缺点：如果每一个节点之间的代价都是相同的，那么Dijkstra算法和广度优先搜索算法就是一样的了。

最佳优先搜索

某些情况下，我们可以以节点和终点的距离作为指导，来选取下一个节点，这样可以快速找到终点。原理和Dijkstra类似，也是使用一个优先队列，但是这时候优先队列排序是以节点到终点的距离作为优先级的，每一次都选取到终点移动代价最小(离终点最近)的节点作下一个遍历点。、

缺点：如果终点和起点之间存在障碍物，那么最佳优先匹配找到的很可能就不是最优路径了。

结算完两种算法，Dijkstra和最佳优先搜索都有各自的优点和缺点，那么结合起来，就是A*算法了。

A*算法

A*算法的优先队列是通过下面的公式来计算优先级：
$$f\left(n\right)=g\left(n\right)+h\left(n\right)$$

• f(n)代表是节点的综合优先级。
• g(n)节点距离起点的移动代价
• h(n)也就是启发式函数，代表节点到终点的代价。

A*算法每次从优先队列中找f(n)最小的节点作为下一个要遍历的节点。除了待遍历节点open_set外，还有一个close_set代表已经遍历过的节点集合。

代码描述：

初始化open_set和close_set；
* 将起点加入open_set中，并设置优先级为0（优先级最高）；
* 如果open_set不为空，则从open_set中选取优先级最高的节点n：
    * 如果节点n为终点，则：
        * 从终点开始逐步追踪parent节点，一直达到起点；
        * 返回找到的结果路径，算法结束；
    * 如果节点n不是终点，则：
        * 将节点n从open_set中删除，并加入close_set中；
        * 遍历节点n所有的邻近节点：
            * 如果邻近节点m在close_set中，则：
                * 跳过，选取下一个邻近节点
            * 如果邻近节点m也不在open_set中，则：
                * 设置节点m的parent为节点n
                * 计算节点m的优先级
                * 将节点m加入open_set中

Carla中的A*全局路径规划

route = []  # 用来保存路径
# open_set代表待遍历的节点
open_set = dict()  # 字典， 记录每个节点的父节点和最短路径
# closed_set代表已遍历的节点。
closed_set = dict()
open_set[n_begin] = (0, -1)  # 每个节点对应一个元组，第一个元素是节点到起点的最短路径，第二个元素是父节点的id

# 函数用来计算节点n到终点的距离(启发式函数)
def cal_heuristic(n):
    return math.hypot(self._graph.nodes[n]['vertex'][0] - self._graph.nodes[n_end]['vertex'][0],
           self._graph.nodes[n]['vertex'][1] - self._graph.nodes[n_end]['vertex'][1])


while 1:
    # 如果待遍历节点为空了，说明走不通了。
    if len(open_set) == 0:  # 终点不可达
        return None
    # 这就是所谓的"f(n) = g(n) + h(n)"形式的启发式函数
    # 先计算所有待遍历节点的f(n)，然后依次作为优先级依据。
    # 在open_set中找到f(n)优先级最低的节点作为c_node
    c_node = min(open_set, key=lambda n: open_set[n][0] + cal_heuristic(n))
    # 找到了终点。
    if c_node == n_end:
        closed_set[c_node] = open_set[c_node]
        del open_set[c_node]  # 如果当前节点是终点，则把该节点从open_set中移除，加入到close_set.
        break
    # 遍历当前节点的所有后继节点。
    for suc in self._graph.successors(c_node):  # 处理当前所有节点的后继
        # get_edge_data(n1,n2)是获取两个节点之间的边的数据。比如长度和权重等。
        new_cost = self._graph.get_edge_data(c_node, suc)["length"]  # 获取从c_node到后继节点的长度。
        if suc in closed_set:  # 如果访问过就不再访问
               continue
        elif suc in open_set:  # 如果在即将访问的集合中，判断是否需要更新路径
           if open_set[c_node][0] + new_cost < open_set[suc][0]:
               open_set[suc] = (open_set[c_node][0] + new_cost, c_node)  # 父节点是c_node1
        else:  # 如果是新节点，直接加入open_set中
               open_set[suc] = (open_set[c_node][0] + new_cost, c_node)
    # c_node遍历过了，加入close_set         
	closed_set[c_node] = open_set[c_node]
    del open_set[c_node]  # 遍历过该节点，则把该节点从open_set中移除，加入到close_set.

# 找到终点以后，顺着父节点找回起点，保存为路径。
while 1:
    if closed_set[route[-1]][1] != -1:
        route.append(closed_set[route[-1]][1])  # 通过不断回溯找到最短路径
    else:
        break
return list(reversed(route))  # 返回路径，因为从终点开始保存，所以需要翻转一下。