Dijkstra、A*算法python实现及对比分析

Dijkstra、A*算法

本篇文章为在栅格地图下的Dijkstra算法、A算法python实现,以及A算法在采用不同的距离计算函数的表现效果。
最后附上搜索范围的对比。

#coding=utf-8

import os
import math
import time
import numpy as np
from PIL import Image
import matplotlib.pyplot as plt

class Node():
    def __init__(self, x, y, cost):
        self.x = x
        self.y = y
        self.cost = cost
        self.parent = None

def read_image(filename):
    cur_dir = os.getcwd()
    im = Image.open(cur_dir + '/' + filename)
    height = im.height
    width = im.width
    # 205是未知区域,0是障碍物,255是空闲区域
    return np.array(im), height, width

def generate_points(height, width):
    diff = 3
    start_x = diff
    start_y = diff
    goal_x = width - diff
    goal_y = height - diff
    node_start = Node(start_x, start_y, 10000)
    node_goal = Node(goal_x, goal_y, 0)

    return node_start, node_goal

def Dijkstra(map, height, width, node_start, node_goal, method):
    openlist = [node_start]
    closelist = [node_start]
    x_offset = [-1, 0, 1]
    y_offset = [-1, 0, 1]
    add = []
    for i in x_offset:
        for j in y_offset:
            if (i == 0) and (j == 0):
                continue
            else:
                add.append([i, j])
    costrecord = [10000]
    idx = 0
    start_time = time.time()
    while 1:
        s_point = [openlist[idx].x, openlist[idx].y]
        G_cost = openlist[idx].cost
        node_current = openlist[idx]
        if s_point == [node_goal.x, node_goal.y]:
            print('path planning over!')
            break
        openlist.pop(idx)
        costrecord.pop(idx)
        for i in range(len(add)):
            neighbor = []
            x = s_point[0] + add[i][0]
            if x < 0 or x >= width:
                continue
            y = s_point[1] + add[i][1]
            if y < 0 or y >= height:
                continue
            if map[y][x] >= 200:
                neighbor.append([x, y])

            if len(neighbor) == 0:
                continue

            flagcontinue = 0
            for j in closelist:
                if [j.x, j.y] == neighbor[0]:
                    flagcontinue = 1
                    break

            if flagcontinue:
                continue

            G = G_cost + math.sqrt(add[i][0]**2 + add[i][1]**2)
            if method == 0:
                # H_Dijkstra = 0
                H = 0
            elif method == 1:
                # H_Astar_Euclidean_distance = math.sqrt((neighbor[0][0] - node_goal.x) ** 2 + (neighbor[0][1] - node_goal.y) ** 2)
                H = math.sqrt((neighbor[0][0] - node_goal.x) ** 2 + (neighbor[0][1] - node_goal.y) ** 2)
            else:
                # H_Astar_Manhattan_Distance = abs(neighbor[0][0] - node_goal.x) + abs(neighbor[0][1] - node_goal.y)
                H = abs(neighbor[0][0] - node_goal.x) + abs(neighbor[0][1] - node_goal.y)
            F = G + H

            flag_append = 1
            for k in range(len(openlist)):
                if [openlist[k].x, openlist[k].y] == neighbor[0]:
                    flag_append = 0
                    break

            if flag_append:
                node_new = Node(x, y, F)
                node_new.parent = node_current
                costrecord.append(F)
                openlist.append(node_new)
            else:
                if openlist[k].cost > F:
                    openlist[k].cost = F
                    openlist[k].parent = node_current
                    costrecord[k] = F

        idx = costrecord.index(min(costrecord))
        closelist.append(openlist[idx])
    end_time = time.time()
    print('planning time', end_time - start_time)

    path = []
    path.append(closelist[-1])
    point = path[-1]
    while point != node_start:
        path.append(point.parent)
        point = point.parent

    grid_value = []
    for i in range(len(path)):
        grid_value.append([path[i].x, path[i].y])

    access_value = []
    for i in range(len(closelist)):
        access_value.append([closelist[i].x, closelist[i].y])

    return grid_value, access_value

def Dijkstra_v2(map, height, width, node_start, node_goal, method):
    openlist = [node_start]
    closelist = [node_start]
    x_offset = [-1, 0, 1]
    y_offset = [-1, 0, 1]
    add = []
    for i in x_offset:
        for j in y_offset:
            if (i == 0) and (j == 0):
                continue
            else:
                add.append([i, j])
    costrecord = [10000]
    idx = 0
    start_time = time.time()
    while 1:
        s_point = openlist[idx]['p']
        G_point = openlist[idx]['G']
        if s_point == node_goal['p']:
            print('path planning over!')
            break
        openlist.pop(idx)
        costrecord.pop(idx)
        for i in range(len(add)):
            neighbor = []
            x = s_point[0] + add[i][0]
            if x < 0 or x >= width:
                continue
            y = s_point[1] + add[i][1]
            if y < 0 or y >= height:
                continue
            if map[y][x] >= 200:
                neighbor.append({'p': (x, y)})

            if len(neighbor) == 0:
                continue

            flagcontinue = 0
            for j in closelist:
                if j['p'] == neighbor[0]['p']:
                    flagcontinue = 1
                    break

            if flagcontinue:
                continue

            G = G_point + math.sqrt(add[i][0]**2 + add[i][1]**2)
            if method == 0:
                # H_Dijkstra = 0
                H = 0
            elif method == 1:
                # H_Astar_Euclidean_distance = math.sqrt((neighbor[0][0] - node_goal.x) ** 2 + (neighbor[0][1] - node_goal.y) ** 2)
                H = math.sqrt((neighbor[0]['p'][0] - node_goal['p'][0]) ** 2 + (neighbor[0]['p'][1] - node_goal['p'][1]) ** 2)
            else:
                # H_Astar_Manhattan_Distance = abs(neighbor[0][0] - node_goal.x) + abs(neighbor[0][1] - node_goal.y)
                H = abs(neighbor[0]['p'][0] - node_goal['p'][0]) + abs(neighbor[0]['p'][1] - node_goal['p'][1])
            F = G + H

            flag_append = 1
            for k in range(len(openlist)):
                if openlist[k]['p'] == neighbor[0]['p']:
                    flag_append = 0
                    break

            if flag_append:
                costrecord.append(F)
                openlist.append({'p': (neighbor[0]['p'][0], neighbor[0]['p'][1]), 'cost': F, 'G': G, 'parent': (s_point[0], s_point[1])})
            else:
                if openlist[k]['cost'] > F:
                    openlist[k] = {'p': (neighbor[0]['p'][0], neighbor[0]['p'][1]), 'cost': F, 'G': G, 'parent': (s_point[0], s_point[1])}
                    costrecord[k] = F

        idx = costrecord.index(min(costrecord))
        closelist.append({'p': (openlist[idx]['p'][0], openlist[idx]['p'][1]), 'cost': openlist[idx]['cost'], 'G': openlist[idx]['G'], 'parent': (openlist[idx]['parent'][0], openlist[idx]['parent'][1])})
    end_time = time.time()
    print('planning time', end_time - start_time)

    path = []
    path.append(closelist[-1]['p'])
    point = path[-1]
    closelist.reverse()
    position = []
    for i in closelist:
        position.append(i['p'])
    while point != node_start['p']:
        idx = position.index(point)
        point = closelist[idx]['parent']
        path.append(point)
        if point == node_start['p']:
            print('path search complete!')
            break

    access_value = []
    for i in range(len(closelist)):
        access_value.append(closelist[i]['p'])

    return path, access_value

def main():
    filename = "box_map3.pgm"
    map, height, width = read_image(filename)
    node_start, node_goal = generate_points(height, width)

    # plt.scatter(node_start.x, node_start.y)
    # plt.scatter(node_goal.x, node_goal.y)

    # v1,用类,比v2费时
    # path_dijkstra, access_dijkstra = Dijkstra(map, height, width, node_start, node_goal, 0)
    # path_astar1, access_astar1 = Dijkstra(map, height, width, node_start, node_goal, 1)
    # path_astar2, access_astar2 = Dijkstra(map, height, width, node_start, node_goal, 2)

    # v2,不用类,比v1省时
    node_start = {'p': (node_start.x, node_start.y), 'cost': 10000, 'G': 0, 'parent': (node_start.x, node_start.y)}
    node_goal = {'p': (node_goal.x, node_goal.y), 'cost': 0, 'G': 10000, 'parent': (node_goal.x, node_goal.y)}
    path_dijkstrav2, access_dijkstrav2 = Dijkstra_v2(map, height, width, node_start, node_goal, 0)
    path_astar1, access_astar1 = Dijkstra_v2(map, height, width, node_start, node_goal, 1)
    path_astar2, access_astar2 = Dijkstra_v2(map, height, width, node_start, node_goal, 2)

    # plt.imshow(map, cmap='gray')
    #
    # plt.plot(np.array(path_dijkstra).transpose()[0], np.array(path_dijkstra).transpose()[1])
    # plt.scatter(np.array(access_dijkstra).transpose()[0], np.array(access_dijkstra).transpose()[1], c='m', alpha=0.2)
    # plt.title('Dijkstra')
    # plt.plot(np.array(path_astar1).transpose()[0], np.array(path_astar1).transpose()[1], label='Astar1')
    # plt.scatter(np.array(access_astar1).transpose()[0], np.array(access_astar1).transpose()[1], c='b', label='Astar1', alpha=0.2)
    # plt.plot(np.array(path_astar2).transpose()[0], np.array(path_astar2).transpose()[1], label='Astar2')
    # plt.scatter(np.array(access_astar2).transpose()[0], np.array(access_astar2).transpose()[1], c='g', label='Astar2', alpha=0.2)

    plt.figure()
    plt.imshow(map, cmap='gray')
    plt.plot(np.array(path_dijkstrav2).transpose()[0], np.array(path_dijkstrav2).transpose()[1], label='Dijkstra_v2')
    plt.scatter(np.array(access_dijkstrav2).transpose()[0], np.array(access_dijkstrav2).transpose()[1], c='r', label='Dijkstra_v2', alpha=0.2)
    plt.title('Dijkstra_v2')
    plt.legend()

    plt.figure()
    plt.imshow(map, cmap='gray')
    plt.plot(np.array(path_astar1).transpose()[0], np.array(path_astar1).transpose()[1], label='Astar_v1')
    plt.scatter(np.array(access_astar1).transpose()[0], np.array(access_astar1).transpose()[1], c='b', label='Astar_v1', alpha=0.2)
    plt.title('Astar_v1')
    plt.legend()

    plt.figure()
    plt.imshow(map, cmap='gray')
    plt.plot(np.array(path_astar2).transpose()[0], np.array(path_astar2).transpose()[1], label='Astar_v2')
    plt.scatter(np.array(access_astar2).transpose()[0], np.array(access_astar2).transpose()[1], c='g', label='Astar_v2', alpha=0.2)
    plt.title('Astar_v2')
    plt.legend()

    plt.show()

if __name__ == "__main__":
    main()

个人比较懒,这里直接附上代码以及一些地方作出说明:

几个函数

1.read_image ,很明显,这个函数是用于读取栅格地图以及计算长宽的,但我并没有附上栅格地图的文件,你们可以直接用numpy数组自己写一个栅格地图出来(可以参考后面华为笔试题博客栅格地图的写法),不过要注意自己对应修改第165行的可通行区域栅格值;
2.generate_points,这个函数用于生成起点和终点,这边我分别测试了用类去存储起点终点相关信息以及直接存储相关信息的时长,从运行结果来看第二个版本速度更快,也就是Dijkstra_v2函数;
3.Dijkstra函数和Dijkstra_v2函数,这两个函数实际上也是A函数,不同的是Dijkstra函数的节点信息存储采用的是类Node,第10行的类,而Dijkstra_v2函数采用的节点存储方式是第246行的那种;这两个函数有一个传入参数是method,method有三个选择,0/1/2;A算法的启发函数是F=G+H,如果method=0,说明H=0,也就是Dijkstra算法,method=1,H则采用欧式距离计算,为第一种A算法,method=2,H采用曼哈顿距离计算,为第二种A算法。
最后就是一些绘图函数,不多做说明了,只有一点需要提的,在Dijkstra函数返回的时候有两个参数,一个是path_dijkstra,另一个是access_dijkstra,path_dijkstra返回的是最终的路径,而access_dijkstra返回的是遍历过的节点,也就是最后附图中比较A*和Dijkstra的遍历节点数量用的。

结果图

在这里插入图片描述
可以看到,在采用类存储节点的Dijkstra算法耗时要比Dijkstra_v2长7秒左右,然而两种版本的Dijkstra算法生成路径长度是一致的,且访问节点的数量也是一样的。

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
这里从上到下三个运行时间依次是Dijkstra_v2,Astar_v1(采用欧式距离),Astar_v2(采用曼哈顿距离),可以看出采用曼哈顿距离的运行时长明显比采用欧式距离的快,一个原因是采用曼哈顿距离访问的节点数量更少,还有一个就是曼哈顿距离不需要开方,开方对于计算机来说是很吃计算资源的,这个其实很好验证,我们可以直接粗略计算斜向距离为1.4,然后在上述的代码中如果add[i][0] == add[i][1],直接令G=G_point + 1.4,然后看看时长有没有减少。
在这里插入图片描述

在这里插入图片描述
最后附上两种A*算法的路径图。

  • 1
    点赞
  • 11
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
A*算法是一种常用的寻路算法,可以用于计算机游戏等应用中。它在Dijkstra算法的基础上增加了一些启发式函数,以提高搜索效率。以下是一个简单的Python代码实现: ```python import heapq class Node: def __init__(self, x, y, reachable): self.x = x self.y = y self.reachable = reachable self.parent = None self.g = 0 self.h = 0 self.f = 0 def __lt__(self, other): return self.f < other.f class AStar: def __init__(self, grid): self.grid = grid self.open = [] self.close = [] def get_path(self, start, end): start_node = Node(start, start, True) end_node = Node(end, end, True) if not self.grid.is_reachable(end_node.x, end_node.y): return None heapq.heappush(self.open, start_node) while len(self.open) > 0: current_node = heapq.heappop(self.open) self.close.append(current_node) if current_node.x == end_node.x and current_node.y == end_node.y: path = [] while current_node.parent is not None: path.append((current_node.x, current_node.y)) current_node = current_node.parent path.append((start_node.x, start_node.y)) return path[::-1] neighbors = self.grid.get_neighbors(current_node.x, current_node.y) for neighbor in neighbors: if neighbor.reachable and neighbor not in self.close: g = current_node.g + 1 h = ((neighbor.x - end_node.x) ** 2) + ((neighbor.y - end_node.y) ** 2) f = g + h if neighbor in self.open: if neighbor.g > g: neighbor.g = g neighbor.h = h neighbor.f = f neighbor.parent = current_node heapq.heapify(self.open) else: neighbor.g = g neighbor.h = h neighbor.f = f neighbor.parent = current_node heapq.heappush(self.open, neighbor) return None class Grid: def __init__(self, width, height): self.width = width self.height = height self.nodes = [[Node(x, y, True) for y in range(height)] for x in range(width)] def get_neighbors(self, x, y): neighbors = [] if x > 0: neighbors.append(self.nodes[x-1][y]) if x < self.width - 1: neighbors.append(self.nodes[x+1][y]) if y > 0: neighbors.append(self.nodes[x][y-1]) if y < self.height - 1: neighbors.append(self.nodes[x][y+1]) return neighbors def is_reachable(self, x, y): return self.nodes[x][y].reachable ``` 以上代码是一个基于堆优化的A*算法实现,其中Node表示一个节点,Grid表示一个网格,AStar表示A*算法实现类。get_path方法接受起点和终点作为参数,返回一条从起点到终点的路径。

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值