A星算法代码及示例

  • A*算法代码及示例——python实现

A*算法是一种特殊的单源最短路径算法,通常用于在加权图中确定从一个特定节点(起点 S S S)到一个指定节点(终点 E E E)的最短路。

  • A* 算法通过结合Dijkstra算法的精确性和启发式方法的速度优势优化路径搜索过程;

    • “实际成本”g(n):从 S S S到当前节点的实际路径成本
    • “预估成本”h(n):用于评估从当前节点到 E E E估计成本,常见的启发式方法包括曼哈顿距离、欧几里德距离等;
    • “综合成本”f(n):即 f ( n ) = g ( n ) + h ( n ) f(n)=g(n)+h(n) f(n)=g(n)+h(n),启发式函数估计的成本和实际成本之和,用于决定搜索顺序
  • 启发式函数h(n),通常称为估计成本函数,来估算从任一顶点 n n n到目标顶点的成本,是A*算法与其他单源最短路径算法最主要的区别;

  • 在实际应用中,确保启发式函数是可行的(即它不会高估到达目标的实际成本),这样才能保证算法的效率和结果的正确性。

一、算法基本步骤

  1. 初始化
    • 将起点 S S S加入开放列表open list),并将其设置为当前节点,该节点的g(n)h(n)f(n)均设为0;
    • 将网格中的障碍物(无法通过的节点)加入关闭列表closed list),表示已经处理过;
  2. 寻找下一个节点
    • 从开放列表中选择 f(n) 最小的节点作为当前节点
      • 若多个节点f(n)相等则选择h(n)较小的节点,因为h(n)越小,表示当前格子越接近目标(不考虑障碍物的情况下);
      • 相关链接:A-Star(A*)寻路算法原理与实现 - 王江荣的文章 - 知乎https://zhuanlan.zhihu.com/p/385733813
    • 如果当前节点是终点 E E E,则算法结束,路径已找到;
    • 否则,将当前节点从open list中移至closed list,表示已处理过。
  3. 考虑当前节点的相邻节点
    • 遍历当前节点 T T T的相邻节点(以某个相邻节点 A A A为例进行说明):
      • A A Aclosed list中,则忽略;
      • A A Aopen list中,检查从起点 S S S通过当前节点 T T T到达节点 A A A的路径是否更优(即通过当前节点能否使节点 A A Ag(n) 更小),
        • A g ( n ) < T g ( n ) + Δ L A_{g(n)}<T_{g(n)}+\Delta L Ag(n)<Tg(n)+ΔL,其中 Δ L \Delta L ΔL表示节点单步可移动的距离,则更新节点 A A A A g ( n ) = T g ( n ) + Δ L A_{g(n)}=T_{g(n)}+\Delta L Ag(n)=Tg(n)+ΔL(同步更新f(n)),同时节点 A A A的“父节点”(上游节点)更新为 T T T
        • 否则,不更新节点 A A Ag(n)f(n)
      • 若不在open list中,则将其加入open list,并计算g(n)、h(n)、f(n),同时记录节点 A A A的“父节点”为 T T T
        • g(n):从 S S S A A A的实际代价,由 S S S T T Tg(n)与节点单步可移动的距离决定;
        • h(n):到终点 E E E的启发式估计代价,例如根据节点 A 、 E A、E AE的坐标计算曼哈顿距离;
        • f(n):总代价,即 f(n) = g(n) + h(n)
  4. 确定最短路径
    • 重复步骤2、3,直到找到终点 E E E或者open list为空
      • open list为空且没有找到终点 E E E,则表示无解
      • 若找到了终点 E E E,则从终点 E E E出发通过节点记录的“父节点”,反向回溯找到起点 S S S,最终确定最短路径上的各个节点。

二、节点距离与启发函数

2.1、计算节点距离

网络中两个节点之间的距离有三类常用的表示方式:欧几里得距离、曼哈顿距离、对直距离(对角线+直线)

  • 欧几里得距离(Euclidean distance)

    • 对于节点 S 、 E S、E SE,坐标分别为 ( s i , s j ) 、 ( e i , e j ) (s_i,s_j)、(e_i,e_j) (si,sj)(ei,ej),则两点之间的欧几里得距离为: E D = ( s i − e i ) 2 + ( s j − e j ) 2 ED=\sqrt{(s_i-e_i)^2+(s_j-e_j)^2} ED=(siei)2+(sjej)2
  • 曼哈顿距离(Manhattan distance)

    • 对于节点 S 、 E S、E SE,坐标分别为 ( s i , s j ) 、 ( e i , e j ) (s_i,s_j)、(e_i,e_j) (si,sj)(ei,ej),则两点之间的曼哈顿距离为: M D = ∣ s i − e i ∣ + ∣ s j − e j ∣ MD=|s_i-e_i|+|s_j-e_j| MD=siei+sjej
  • 对直距离(Diagonal and linear distance)

    • 对于节点 S 、 E S、E SE,坐标分别为 ( s i , s j ) 、 ( e i , e j ) (s_i,s_j)、(e_i,e_j) (si,sj)(ei,ej)​,则两点之间的对直距离为:

    • D L D = D D + L D D D = 2 × min ⁡ ( ∣ s i − e i ∣ , ∣ s j − e j ∣ ) ≈ 1.4 × min ⁡ ( ∣ s i − e i ∣ , ∣ s j − e j ∣ ) L D = m a x ( ∣ s i − e i ∣ , ∣ s j − e j ∣ ) − m i n ( ∣ s i − e i ∣ , ∣ s j − e j ∣ ) DLD=DD+LD\\ DD=\sqrt{2}\times\min(|s_i-e_i|,|s_j-e_j|)\approx1.4\times\min(|s_i-e_i|,|s_j-e_j|)\\ LD=max(|s_i-e_i|,|s_j-e_j|)-min(|s_i-e_i|,|s_j-e_j|) DLD=DD+LDDD=2 ×min(siei,sjej)1.4×min(siei,sjej)LD=max(siei,sjej)min(siei,sjej)

  • 距离计算实例(图中节点 S ( 1 , 1 ) 、 E ( 6 , 3 ) S(1,1)、E(6,3) S(1,1)E(6,3)之间距离)

    • E D = ( 1 − 6 ) 2 + ( 1 − 3 ) 2 ≈ 5.4 ED=\sqrt{(1-6)^2+(1-3)^2}\approx5.4 ED=(16)2+(13)2 5.4

    • M D = ∣ 1 − 6 ∣ + ∣ 1 − 3 ∣ = 7 MD=|1-6|+|1-3|=7 MD=∣16∣+∣13∣=7​;

    • D D = 1.4 × m i n ( ∣ 1 − 6 ∣ , ∣ 1 − 3 ∣ ) = 2.8 L D = m a x ( ∣ 1 − 6 ∣ , ∣ 1 − 3 ∣ ) − m i n ( ∣ 1 − 6 ∣ , ∣ 1 − 3 ∣ ) = 5 − 2 = 3 D L D = D D + L D = 2.8 + 3 = 5.8 DD=1.4\times{min(|1-6|,|1-3|)}=2.8\\ LD=max(|1-6|,|1-3|)-min(|1-6|,|1-3|)=5-2=3\\ DLD=DD+LD=2.8+3=5.8 DD=1.4×min(∣16∣,∣13∣)=2.8LD=max(∣16∣,∣13∣)min(∣16∣,∣13∣)=52=3DLD=DD+LD=2.8+3=5.8

    • 说明: M D ≥ D L D ≥ E D MD\ge{DLD}\ge{ED} MDDLDED

在这里插入图片描述

2.2、启发函数设计及有效性证明

A*算法的关键是合理的启发式函数 h(n),它要能够提供一个对从当前节点到目标节点最短路径代价的合理估计,以保证算法能够高效地搜索到最优路径。

启发函数设计及算法有效性(可采纳性、一致性、最优性)证明详见该文章:https://blog.csdn.net/weixin_42639395/article/details/139715043

三、算法实例

3.1、实例基本信息

如图网格,找到起点 S S S终点 E E E​之间的最短路径(图中深灰色部分为障碍物,需绕行)

在这里插入图片描述

说明:

  • 右图中虚线为使用曼哈顿距离作为启发函数求解的最短路径,实线为使用对直距离作为启发函数求解的最短路径(本实例未将欧几里得距离作为启发函数,可自行实现);
  • 对于节点 S 、 E S、E SE,坐标分别为 ( s i , s j ) 、 ( e i , e j ) (s_i,s_j)、(e_i,e_j) (si,sj)(ei,ej),函数中所用的坐标为字符串形式 s i − s j 、 e i − e j s_i-s_j、e_i-e_j sisjeiej
  • 实例完整代码:

所求解的最短路径信息如下:

  • 使用曼哈顿距离

    • 最短路径长度为14;
    • 最短路径为['1-1', '2-1', '3-1', '4-1', '5-1', '6-1', '7-1', '8-1', '8-2', '8-3', '8-4', '8-5', '8-6', '8-7', '8-8']
  • 使用对直距离

    • 最短路径长度为12.8;
    • 最短路径为['1-1', '2-2', '3-3', '4-3', '5-3', '6-3', '7-4', '8-5', '8-6', '8-7', '8-8']

3.2、实例代码

3.2.1、网格构建以及路径求解
import numpy as np


if __name__ == '__main__':
    maze = np.zeros((10, 10))
    # 设置起点、终点
    start = '1-1'
    end = '8-8'

    # 添加障碍
    maze[2:7, 5] = 1
    barrier_list = ['2-5', '3-5', '4-5', '5-5', '6-5', '7-5']
    # 使用曼哈顿距离作为启发函数
    # detect_shortest_path_process_using_manhattan_distance(maze, start, end, barrier_list)
    # 使用对直距离作为启发函数
    # detect_shortest_path_process_using_diagonal_distance(maze, start, end, barrier_list)
3.2.2、以曼哈顿距离作为启发函数
  • 对于节点 S 、 E S、E SE,坐标分别为 ( s i , s j ) 、 ( e i , e j ) (s_i,s_j)、(e_i,e_j) (si,sj)(ei,ej),函数中所用的坐标为字符串形式 s i − s j 、 e i − e j s_i-s_j、e_i-e_j sisjeiej
def get_manhattan_distance(end, node):
    """
    计算曼哈顿距离
    :param end: 终点
    :param node: 节点
    :return: 节点与终点的曼哈顿距离
    """
    distance = 0
    for end_position, node_position in zip(end.split(sep='-'), node.split(sep='-')):
        distance += abs(int(end_position) - int(node_position))

    return distance
def detect_shortest_path_process_using_manhattan_distance(maze, start, end, barrier_list):
    """
    确定起终点之间的最短路径
    只能上下左右移动(↑、↓、←、→),不能走斜线,h(n)为曼哈顿距离
    不仅能上下左右移动(↑、↓、←、→),而且能走斜线(↖、↙、↗、↘),h(n)为对角线距离
    :param maze: 网格
    :param start: 起点
    :param end: 终点
    :param barrier_list: 障碍物信息
    :return: 起终点之间的最短路径
    """
    
    row, column = maze.shape

    # 使用字典记录temp节点(临时性记录)
    # {节点:[f(n), g(n), h(n)]}
    temp_dict = {}
    # 使用列表记录已确定最短路的节点(start到这些节点的距离已是最短)
    confirmed_list = []
    confirmed_list.extend(barrier_list)
    # 添加start
    temp_dict[start] = [0, 0, 0]
    # 是否到达终点
    end_flag = False
    # 记录各个节点的“父节点”,确定起点到终点的路径
    maze_path = [[0] * 10 for i in range(10)]

    while temp_dict and not end_flag:
        next_node = get_next_node(temp_dict)
        distance = temp_dict.pop(next_node)

        # 记录该节点
        confirmed_list.append(next_node)

        # 遍历相邻节点(假设节点只能上下左右移动,不能走对角线)
        i = int(next_node.split(sep='-')[0])
        j = int(next_node.split(sep='-')[1])

        # 若相邻节点在范围内且未“确认”
        adjacent_node_list = [[i - 1, j], [i + 1, j], [i, j - 1], [i, j + 1]]
        for adjacent_node in adjacent_node_list:
            node_str = f'{adjacent_node[0]}-{adjacent_node[1]}'
            # 若为终点,则停止while循环
            if end == node_str:
                print(f"起点{start}到终点{end}的距离为{distance[0]}")
                end_flag = True
                # 记录“父节点”
                maze_path[adjacent_node[0]][adjacent_node[1]] = next_node
                break

            if in_area(adjacent_node[0], adjacent_node[1], row, column) and node_str not in confirmed_list:
                # 若已在temp_dict中,判断是否需要更新
                if node_str in temp_dict:
                    if distance[1] + 1 < temp_dict[node_str][1]:
                        temp_dict[node_str][1] = distance[1] + 1
                        temp_dict[node_str][0] = temp_dict[node_str][1] + temp_dict[node_str][2]

                        # 更新“父节点”
                        maze_path[adjacent_node[0]][adjacent_node[1]] = next_node
                else:
                    # 放入temp_dict中
                    temp_dict[node_str] = []
                    # 计算实际距离g(n)
                    temp_dict[node_str].append(distance[1] + 1)
                    # 计算估计距离h(n),使用曼哈顿距离
                    # 只能上下左右移动,不能走斜线,此时h(n) <= 实际距离
                    temp_dict[node_str].append(get_manhattan_distance(end, node_str))
                    # 计算f(n)
                    temp_dict[node_str].insert(0, sum(temp_dict[node_str]))

                    # 记录“父节点”
                    maze_path[adjacent_node[0]][adjacent_node[1]] = next_node
    # print(maze_path)
    shortest_path = get_path_info(start, end, maze_path)
    print(shortest_path)
3.2.3、以对直距离作为启发函数
def get_diagonal_distance(end, node):
    """
    计算对直距离
    :param end: 终点
    :param node: 节点
    :return: 节点与终点的对角线距离
    """
    distance_list = []
    for end_position, node_position in zip(end.split(sep='-'), node.split(sep='-')):
        distance_list.append(abs(int(end_position) - int(node_position)))
    # 按照对角线 + 直行,计算距离
    distance = min(distance_list) * 1.4 + max(distance_list) - min(distance_list)
    return distance
def detect_shortest_path_process_using_diagonal_distance(maze, start, end, barrier_list):
    """
    确定起终点之间的最短路径
    不仅能上下左右移动(↑、↓、←、→),而且能走斜线(↖、↙、↗、↘),h(n)为对角线距离
    :param maze: 网格
    :param start: 起点
    :param end: 终点
    :param barrier_list: 障碍物信息
    :return: 起终点之间的最短路径
    """
    
    row, column = maze.shape

    # 使用字典记录temp节点(临时性记录)
    # {节点:[f(n), g(n), h(n)]}
    temp_dict = {}
    # 使用列表记录已确定最短路的节点(start到这些节点的距离已是最短)
    confirmed_list = []
    confirmed_list.extend(barrier_list)
    # 添加start
    temp_dict[start] = [0, 0, 0]
    # 是否到达终点
    end_flag = False
    # 记录各个节点的“父节点”,确定起点到终点的路径
    maze_path = [[0] * 10 for i in range(10)]

    while temp_dict and not end_flag:
        next_node = get_next_node(temp_dict)
        distance = temp_dict.pop(next_node)

        # 记录该节点
        confirmed_list.append(next_node)

        # 遍历相邻节点(假设节点只能上下左右移动,不能走对角线)
        i = int(next_node.split(sep='-')[0])
        j = int(next_node.split(sep='-')[1])

        # 若相邻节点在范围内且未“确认”
        adjacent_node_list = [[i - 1, j], [i + 1, j], [i, j - 1], [i, j + 1],
                              [i - 1, j - 1], [i - 1, j + 1], [i + 1, j - 1], [i + 1, j + 1]]
        for node_i, adjacent_node in enumerate(adjacent_node_list):
            node_str = f'{adjacent_node[0]}-{adjacent_node[1]}'
            # 若为终点,则停止while循环
            if end == node_str:
                print(f"起点{start}到终点{end}的距离为{distance[0]}")
                end_flag = True
                # 记录“父节点”
                maze_path[adjacent_node[0]][adjacent_node[1]] = next_node
                break

            if in_area(adjacent_node[0], adjacent_node[1], row, column) and node_str not in confirmed_list:
                # 若已在temp_dict中,判断是否需要更新
                if node_str in temp_dict:
                    # 对于对角线上的相邻节点,对角线距离直接取1.4
                    if node_i >= 4:
                        if distance[1] + 1.4 < temp_dict[node_str][1]:
                            temp_dict[node_str][1] = distance[1] + 1.4
                            temp_dict[node_str][0] = temp_dict[node_str][1] + temp_dict[node_str][2]

                            # 更新“父节点”
                            maze_path[adjacent_node[0]][adjacent_node[1]] = next_node
                    else:
                        if distance[1] + 1 < temp_dict[node_str][1]:
                            temp_dict[node_str][1] = distance[1] + 1
                            temp_dict[node_str][0] = temp_dict[node_str][1] + temp_dict[node_str][2]

                            # 更新“父节点”
                            maze_path[adjacent_node[0]][adjacent_node[1]] = next_node
                else:
                    if node_i >= 4:
                        # 放入temp_dict中
                        temp_dict[node_str] = []
                        # 计算实际距离g(n)
                        temp_dict[node_str].append(distance[1] + 1.4)
                        # 计算估计距离h(n),使用对角线距离
                        # 能够上下左右移动,也能走斜线,此时h(n) <= 实际距离
                        temp_dict[node_str].append(get_diagonal_distance(end, node_str))
                        # 计算f(n)
                        temp_dict[node_str].insert(0, sum(temp_dict[node_str]))

                        # 记录“父节点”
                        maze_path[adjacent_node[0]][adjacent_node[1]] = next_node
                    else:
                        # 放入temp_dict中
                        temp_dict[node_str] = []
                        # 计算实际距离g(n)
                        temp_dict[node_str].append(distance[1] + 1.4)
                        # 计算估计距离h(n),使用曼哈顿距离
                        # 只能上下左右移动,不能走斜线,此时h(n) <= 实际距离
                        temp_dict[node_str].append(get_diagonal_distance(end, node_str))
                        # 计算f(n)
                        temp_dict[node_str].insert(0, sum(temp_dict[node_str]))

                        # 记录“父节点”
                        maze_path[adjacent_node[0]][adjacent_node[1]] = next_node
    # print(maze_path)
    shortest_path = get_path_info(start, end, maze_path)
    print(shortest_path)
3.2.4、公用函数
def in_area(i, j, row, column):
    """
    判断是否在范围内
    :param i: 横坐标
    :param j: 纵坐标
    :param row: 行
    :param column: 列
    :return:
    """
    if 0 <= i < row and 0 <= j < column:
        return True
    return False


def get_next_node(temp_dict):
    """
    根据距离确定下一个节点
    :param temp_dict: 可选择的节点
    :return: 下一个节点
    """
    # 若多个节点f(n)相等,则选择h(n)最小的节点,减少没必要的探索
    smallest = 10000
    smallest_h = 10000
    next_node = None
    for node, distance in temp_dict.items():
        if distance[0] < smallest:
            smallest = distance[0]
            smallest_h = distance[2]
            next_node = node
        if distance[0] == smallest:
            if distance[2] < smallest_h:
                smallest_h = distance[2]
                next_node = node

    return next_node


def get_path_info(start, end, maze_path):
    """
    确定起终点间的中间节点(最短路径信息)
    :param start: 起点
    :param end: 终点
    :param maze_path: 各个节点的“父节点”信息
    :return: 起终点之间的最短路径
    """
    path = []
    # 从end开始,根据各个节点的“父节点”往前递推,直至start
    path.append(end)
    while start not in path:
        tem = path[0]
        i = int(tem.split(sep='-')[0])
        j = int(tem.split(sep='-')[1])
        path.insert(0, maze_path[i][j])
    return path

四、相关链接

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值