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*算法的路径图。