【路径规划】全局路径规划算法——A*算法（含python实现 | c++实现）

参考资料

• Introduction to the A* Algorithm
• 路径规划与轨迹跟踪系列算法
• Robotic Motion Planning Lectures
• A星算法详解
• A星寻路算法
• 路径规划之 A* 算法

1. 算法简介

• A*（A-Star)算法是一种静态路网中求解最短路径最有效的直接搜索方法，也是解决许多搜索问题的有效算法，广泛应用于室内机器人路径搜索、游戏动画路径搜索等。它是图搜索算法的一种。

• A*算法是一种启发式的搜索算法，它是基于深度优先算法(Depth First Search, DFS)和广度优先算法(Breadth First Search, BFS)的一种融合算法，按照一定原则确定如何选取下一个结点。

---

---

启发式搜索算法指的是，从起点出发，先寻找起点相邻的栅格，判断它是否是最好的位置，基于这个最好的栅格再往外向其相邻的栅格扩展，找到一个此时最好的位置，通过这样一步一步逼近目标点，减少盲目的搜索，提高了可行性和搜索效率。

深度优先搜索算法的思想是，搜索算法从起点开始进行搜索(初始状态下待搜索区域内所有结点均未被访问)，与周围所有邻点进行比较，选择其中距离终点最近的点进行存储，然后再以该邻点为基础对比其周围未被访问的所有邻点，仍然选择距离终点最近的邻点存储。若访问结点到达终点或访问完所有结点仍未到达终点，则视为搜索失败。成功搜索所存储的结点连接而成的路径即为起点到终点的路径。

广度优先搜索的原理是，从初始点出发依次访问与它相连接的邻点，访问完毕后再从这些邻点出发访问邻点的邻点,但是要保证先被访问的邻点的邻点要比后被访问的邻点的邻点先访问，直至图中所有已被访问的结点的邻点都被访问到。如果此时图中尚有未被访问的结点，则需要选取一个尚未被访问的结点作为个新的初始点,继续搜索访问，直到图中所有的结点都被访问一遍为止。

因此深度优先算法与广度优先搜索算法从过程上存在较大差异。深度优先算法优先选择离目标点最近的结点，而广度优先搜索算法优先选择离初始点最近的点。基于深度优先算法,能以最快的速度找到一条连接初始点到目标点的路径，但不能保证路径的最优性(例如以路径最短为标准)；广度优先搜索算法则必然能找到最短的路径，但由于需要遍历所有的结点，其计算复杂程度更大。基于这两种算法的优缺点，A*算法基于启发函数构建了代价函数，既考虑了新结点距离初始点的代价，又考虑了新结点与目标点距离的代价。

---

---

• A*算法使用一个路径优劣评价公式为：
  $$f(n)=g(n)+h(n)$$

  • f(n) 是从初始状态经由状态n到目标状态的代价估计，
  • g(n) 是从初始状态到状态n的实际代价，
  • h(n) 是从状态n到目标状态的最佳路径的估计代价。

• A*算法需要维护两个状态表，分别称为openList表和closeList表。openList表由待考察的节点组成， closeList表由已经考察过的节点组成。

2. 算法精讲

2.1 预处理

如图，假设我们需要从A点到目标点，这两点之间有一堵墙。

首先，我们把地图栅格化，把每一个方格的中心称为节点；这个特殊的方法把我们的搜索区域简化为了 2 维数组。数组的每一项代表一个格子，它的状态就是可走 (walkalbe) 和不可走 (unwalkable) 。通过计算出从 A 到 目标点需要走过哪些方格，就找到了路径。一旦路径找到了，便从一个方格的中心移动到另一个方格的中心，直至到达目的地。

2.2 开始搜索

一旦我们把搜寻区域简化为一组可以量化的节点后，我们下一步要做的便是查找最短路径。在 A* 中，我们从起点开始，检查其相邻的方格，然后向四周扩展，直至找到目标。

• 从起点 A 开始，定义A为父节点，并把它加入到 openList中。现在 openList 里只有起点 A ，后面会慢慢加入更多的项。
• 如上图所示，父节点A周围共有8个节点，定义为子节点。将子节点中可达的（reachable）或者可走的（walkable）放入openList中，成为待考察对象。
• 若某个节点既未在openList，也没在closeList中，则表明还未搜索到该节点。
• 初始时， 节点A离自身距离为0，路径完全确定，将其移入closeList中； closeList 中的每个方格都是现在不需要再关注的。
• 路径优劣判断依据是移动代价，单步移动代价采取Manhattan 计算方式（即$d=|x_1-x_2|+|y_1-y_2|$），即把横向和纵向移动一个节点的代价定义为10。斜向移动代价为14（即$\sqrt{2}$倍的横向或纵向移动代价）。
• 现在openList = {B,C,D,E,F,G,H,I}, closeList = {A}

下面我们需要去选择节点A相邻的子节点中移动代价$f$最小的节点，下面以节点I的计算为例。

• 移动代价评价函数为： $f(n)=g(n)+h(n)$。 $f(n)$ 是从初始状态经由状态n到目标状态的代价估计， $g(n)$ 是在状态空间中从初始状态到状态n的实际代价， $h(n)$ 是从状态n到目标状态的最佳路径的估计代价。

• 首先考察$g$，由于从A到该格子是斜向移动，单步移动距离为14，故$g$ = 14.
• 再考察估计代价$h$。估计的含义是指忽略剩下的路径是否包含有障碍物（不可走）， 完全按照Manhattan计算方式，计算只做横向或纵向移动的累积代价：横向向右移动3步，纵向向上移动1步，总共4步，故为$h$ = 40.
• 因此从A节点移动至I节点的总移动代价为： $f=g+h=54$
• 以此类推，分别计算当前openList中余下的7个子节点的移动代价$f$，从中挑选最小代价节点F，移到closeList中。
• 现在openList = {B,C,D,E,G,H,I}, closeList = {A,F}

2.3 继续搜索

• 从openList中选择 $f$值最小的 ( 方格 ) 节点I(D节点的$f$值跟I相同，任选其一即可，不过从速度上考虑，选择最后加入 openList 的方格更快。这导致了在寻路过程中，当靠近目标时，优先使用新找到的方格的偏好。 对相同数据的不同对待，只会导致两种版本的 $A^{\ast }$ 找到等长的不同路径 )，从 openList里取出，放到 closeList 中。
• 检查所有与它相邻的子节点，忽略不可走 (unwalkable) 的节点、以及忽略已经存在于closeList的节点；如果方格不在openList中，则把它们加入到 openList中，并把它们作为节点I的子节点 。
• 如果某个相邻的节点(假设为X)已经在 opeLlist 中，则检查这条路径是否更优，也就是说经由当前节点( 我们选中的节点)到达节点X是否具有更小的 $g$ 值。如果没有，不做任何操作。否则，如果 $g$ 值更小，则把X的父节点设为当前方格 ，然后重新计算X的 $f$ 值和 $g$ 值。
• 判断完所有子节点后，现在openList = {B,C,D,E,G,H,J,K,L}, closeList = {A,F,I}
• 依次类推，不断重复。一旦搜索到目标节点T，完成路径搜索，结束算法。

2.4 确定实际路径

完成路径搜索后，从终点开始，向父节点移动，这样就被带回到了起点，这就是搜索后的路径。如下图所示。从起点 A 移动到终点 T就是简单从路径上的一个方格的中心移动到另一个方格的中心，直至目标。

3. 算法总结

3.1 算法步骤

1. 首先把起点加入 openList 。

2. 重复如下过程：

   • 遍历 openList ，查找 F 值最小的节点，把它作为当前要处理的节点。

   • 把这个节点移到 closeList 。

   • 对当前方格的 8 个相邻方格：

     ◆ 如果它是不可抵达的或者它在 closeList 中，忽略它；

     ◆ 如果它不在 openList 中，则把它加入 openList ，并且把当前方格设置为它的父节点，记录该方格的$f,g,h$值。

     ◆ 如果它已经在 openList 中，检查这条路径 ( 即经由当前方格到达它那里 ) 是否更好，用 $g$ 值作参考，更小的 $g$ 值表示这是更好的路径。如果$g$值更小，把该节点的父节点设置为当前方格，并重新计算它的 $g$ 和 $h$ 值。

3. 停止搜索的情况有两种：

   • 把终点加入到了openList 中，此时路径已经找到了

   • 查找终点失败，并且 openList 是空的，此时没有路径。

4. 保存路径。使用回溯的方法，从终点开始，每个方格沿着父节点移动直至起点，这就是最终搜索到的路径。

3.2 伪代码

初始化open_list和close_list；
* 将起点加入open_list中，并设其移动代价为0；
* 如果open_list不为空，则从open_list中选取移动代价最小的节点n：
    * 如果节点n为终点，则：
        * 从终点开始逐步追踪parent节点，一直达到起点；
        * 返回找到的结果路径，算法结束；
    * 如果节点n不是终点，则：
        * 将节点n从open_list中移除，并加入close_list中；
        * 遍历节点n所有的邻近节点：
            * 如果邻近节点m在close_list中，则：
                * 跳过，选取下一个邻近节点
            * 如果邻近节点m在open_list中，则：
            		* 计算起点经n到m的g值
           		* 如果此时计算出来的g值小于原来起点经m的原父节点到m的g值：
           				* 更新m的父节点为n，重新计算m的移动代价f=g+h
          			* 否则，跳过
            * 如果邻近节点m不在open_list也不在close_list中，则：
                * 设置节点m的parent为节点n
                * 计算节点m的移动代价f=g+h
                * 将节点m加入open_list中

4. python实现

代码实现来自PythonRobotics。

"""

A* grid planning

author: Atsushi Sakai(@Atsushi_twi)
        Nikos Kanargias (nkana@tee.gr)

See Wikipedia article (https://en.wikipedia.org/wiki/A*_search_algorithm)

"""

import math

import matplotlib.pyplot as plt

show_animation = True


class AStarPlanner:

    def __init__(self, ox, oy, resolution, rr):
        """
        Initialize grid map for a star planning

        ox: x position list of Obstacles [m]
        oy: y position list of Obstacles [m]
        resolution: grid resolution [m],地图的像素
        rr: robot radius[m]
        """

        self.resolution = resolution
        self.rr = rr
        self.min_x, self.min_y = 0, 0
        self.max_x, self.max_y = 0, 0
        self.obstacle_map = None
        self.x_width, self.y_width = 0, 0
        self.motion = self.get_motion_model()
        self.calc_obstacle_map(ox, oy)

    class Node:
        """定义搜索区域节点类,每个Node都包含坐标x和y, 移动代价cost和父节点索引。
        """
        def __init__(self, x, y, cost, parent_index):
            self.x = x  # index of grid
            self.y = y  # index of grid
            self.cost = cost
            self.parent_index = parent_index

        def __str__(self):
            return str(self.x) + "," + str(self.y) + "," + str(
                self.cost) + "," + str(self.parent_index)

    def planning(self, sx, sy, gx, gy):
        """
        A star path search
        输入起始点和目标点的坐标(sx,sy)和(gx,gy)，
        最终输出的结果是路径包含的点的坐标集合rx和ry。
        input:
            s_x: start x position [m]
            s_y: start y position [m]
            gx: goal x position [m]
            gy: goal y position [m]

        output:
            rx: x position list of the final path
            ry: y position list of the final path
        """

        start_node = self.Node(self.calc_xy_index(sx, self.min_x),
                               self.calc_xy_index(sy, self.min_y), 0.0, -1)
        goal_node = self.Node(self.calc_xy_index(gx, self.min_x),
                              self.calc_xy_index(gy, self.min_y), 0.0, -1)

        open_set, closed_set = dict(), dict()
        open_set[self.calc_grid_index(start_node)] = start_node

        while 1:
            if len(open_set) == 0:
                print("Open set is empty..")
                break

            c_id = min(
                open_set,
                key=lambda o: open_set[o].cost + self.calc_heuristic(goal_node, open_set[o]))
            current = open_set[c_id]

            # show graph
            if show_animation:  # pragma: no cover
                plt.plot(self.calc_grid_position(current.x, self.min_x),
                         self.calc_grid_position(current.y, self.min_y), "xc")
                # for stopping simulation with the esc key.
                plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None])
                if len(closed_set.keys()) % 10 == 0:
                    plt.pause(0.001)

            # 通过追踪当前位置current.x和current.y来动态展示路径寻找
            if current.x == goal_node.x and current.y == goal_node.y:
                print("Find goal")
                goal_node.parent_index = current.parent_index
                goal_node.cost = current.cost
                break

            # Remove the item from the open set
            del open_set[c_id]

            # Add it to the closed set
            closed_set[c_id] = current

            # expand_grid search grid based on motion model
            for i, _ in enumerate(self.motion):
                node = self.Node(current.x + self.motion[i][0],
                                 current.y + self.motion[i][1],
                                 current.cost + self.motion[i][2], c_id)
                n_id = self.calc_grid_index(node)

                # If the node is not safe, do nothing
                if not self.verify_node(node):
                    continue

                if n_id in closed_set:
                    continue

                if n_id not in open_set:
                    open_set[n_id] = node  # discovered a new node
                else:
                    if open_set[n_id].cost > node.cost:
                        # This path is the best until now. record it
                        open_set[n_id] = node

        rx, ry = self.calc_final_path(goal_node, closed_set)

        return rx, ry

    def calc_final_path(self, goal_node, closed_set):
        # generate final course
        rx, ry = [self.calc_grid_position(goal_node.x, self.min_x)], [
            self.calc_grid_position(goal_node.y, self.min_y)]
        parent_index = goal_node.parent_index
        while parent_index != -1:
            n = closed_set[parent_index]
            rx.append(self.calc_grid_position(n.x, self.min_x))
            ry.append(self.calc_grid_position(n.y, self.min_y))
            parent_index = n.parent_index

        return rx, ry

    @staticmethod
    def calc_heuristic(n1, n2):
        """计算启发函数

        Args:
            n1 (_type_): _description_
            n2 (_type_): _description_

        Returns:
            _type_: _description_
        """
        w = 1.0  # weight of heuristic
        d = w * math.hypot(n1.x - n2.x, n1.y - n2.y)
        return d

    def calc_grid_position(self, index, min_position):
        """
        calc grid position

        :param index:
        :param min_position:
        :return:
        """
        pos = index * self.resolution + min_position
        return pos

    def calc_xy_index(self, position, min_pos):
        return round((position - min_pos) / self.resolution)

    def calc_grid_index(self, node):
        return (node.y - self.min_y) * self.x_width + (node.x - self.min_x)

    def verify_node(self, node):
        px = self.calc_grid_position(node.x, self.min_x)
        py = self.calc_grid_position(node.y, self.min_y)

        if px < self.min_x:
            return False
        elif py < self.min_y:
            return False
        elif px >= self.max_x:
            return False
        elif py >= self.max_y:
            return False

        # collision check
        if self.obstacle_map[node.x][node.y]:
            return False

        return True

    def calc_obstacle_map(self, ox, oy):

        self.min_x = round(min(ox))
        self.min_y = round(min(oy))
        self.max_x = round(max(ox))
        self.max_y = round(max(oy))
        print("min_x:", self.min_x)
        print("min_y:", self.min_y)
        print("max_x:", self.max_x)
        print("max_y:", self.max_y)

        self.x_width = round((self.max_x - self.min_x) / self.resolution)
        self.y_width = round((self.max_y - self.min_y) / self.resolution)
        print("x_width:", self.x_width)
        print("y_width:", self.y_width)

        # obstacle map generation
        self.obstacle_map = [[False for _ in range(self.y_width)]
                             for _ in range(self.x_width)]
        for ix in range(self.x_width):
            x = self.calc_grid_position(ix, self.min_x)
            for iy in range(self.y_width):
                y = self.calc_grid_position(iy, self.min_y)
                for iox, ioy in zip(ox, oy):
                    d = math.hypot(iox - x, ioy - y)
                    if d <= self.rr:
                        self.obstacle_map[ix][iy] = True
                        break

    @staticmethod
    def get_motion_model():
        # dx, dy, cost
        motion = [[1, 0, 1],
                  [0, 1, 1],
                  [-1, 0, 1],
                  [0, -1, 1],
                  [-1, -1, math.sqrt(2)],
                  [-1, 1, math.sqrt(2)],
                  [1, -1, math.sqrt(2)],
                  [1, 1, math.sqrt(2)]]

        return motion


def main():
    print(__file__ + " start!!")

    # start and goal position
    sx = 10.0  # [m]
    sy = 10.0  # [m]
    gx = 50.0  # [m]
    gy = 50.0  # [m]
    grid_size = 2.0  # [m]
    robot_radius = 1.0  # [m]

    # set obstacle positions
    ox, oy = [], []
    for i in range(-10, 60):
        ox.append(i)
        oy.append(-10.0)
    for i in range(-10, 60):
        ox.append(60.0)
        oy.append(i)
    for i in range(-10, 61):
        ox.append(i)
        oy.append(60.0)
    for i in range(-10, 61):
        ox.append(-10.0)
        oy.append(i)
    for i in range(-10, 40):
        ox.append(20.0)
        oy.append(i)
    for i in range(0, 40):
        ox.append(40.0)
        oy.append(60.0 - i)

    if show_animation:  # pragma: no cover
        plt.plot(ox, oy, ".k")
        plt.plot(sx, sy, "og")
        plt.plot(gx, gy, "xb")
        plt.grid(True)
        plt.axis("equal")

    a_star = AStarPlanner(ox, oy, grid_size, robot_radius)
    rx, ry = a_star.planning(sx, sy, gx, gy)

    if show_animation:  # pragma: no cover
        plt.plot(rx, ry, "-r")
        plt.pause(0.001)
        plt.show()


if __name__ == '__main__':
    main()

运行结果如下：

5. c++实现

由于在自动驾驶中算法实现一般使用C++，所以我也使用C++实现了相关功能，代码结构与python代码实现类似，这边就不再做相关代码解释了。完整代码详见另一个github仓库。