路徑規劃算法(2) - A*尋路算法 python實現及解析

代碼

#coding=utf-8
import math

# 啓發距離, 當前點和目標點的啓發距離; -- 就是簡單的曼哈頓距離
def heuristic_distace(Neighbour_node,target_node):
    H = abs(Neighbour_node[0] - target_node[0]) + abs(Neighbour_node[1] - target_node[1])
    return H

def go_around(direction):
    box_length = 1
    diagonal_line = box_length * 1
    if (direction==0 or direction==2 or direction==6 or direction==8):
        return diagonal_line
    elif (direction==1 or direction==3 or direction==4 or direction==5 or direction==7):
        return diagonal_line

def find_coordinate(map,symble):
    #store coordinate
    result=[]
    for index1,value1 in enumerate(map):
        if symble in value1:
            row = index1
            for index2, value2 in enumerate(map[index1]):
                if symble==value2:
                   column = index2
                   result.append([row, column])
    return result

def show_map(map):
    for idx in map:
        print idx

map =[[".", ".", ".", "#", ".", "#", ".", ".", ".", "."],
      [".", ".", "#", ".", ".", "#", ".", "#", ".", "#"],
      ["s", ".", "#", ".", "#", ".", "#", ".", ".", "."],
      [".", "#", "#", ".", ".", ".", ".", ".", "#", "."],
      [".", ".", ".", ".", "#", "#", ".", ".", "#", "."],
      [".", "#", ".", ".", ".", ".", "#", ".", ".", "."],
      [".", "#", ".", ".", ".", "#", "#", ".", "#", "."],
      [".", ".", ".", ".", ".", ".", ".", ".", "#", "."],
      [".", "#", "#", ".", ".", ".", "#", ".", ".", "."],
      [".", ".", ".", "#", "#", "#", ".", ".", "#", "f"],
      ["#", "#", ".", ".", "#", "#", "#", ".", "#", "."],
      [".", "#", "#", ".", ".", ".", "#", ".", ".", "."],
      [".", ".", ".", ".", "#", "#", ".", ".", "#", "."]]

#these datas are store in the form of list in a singal list
# 記錄所有的障礙物點 座標
obstacle = find_coordinate(map,"#")
start_node = find_coordinate(map,"s")[0]
target_node = find_coordinate(map,"f")[0]
current_node = start_node

# 設置路徑起點爲當前節點
path_vertices = [start_node]

#visited_vertices should be stored in the form of a singal list
Neighbour_vertices = []


# 進入查找
while current_node != target_node:

    x_coordinate = current_node[0]
    y_coordinate = current_node[1]
    F = []  # 節點權值 F = g + h
    Neighbour_vertices =   [[x_coordinate - 1, y_coordinate - 1],
                            [x_coordinate - 1, y_coordinate    ],
                            [x_coordinate - 1, y_coordinate + 1],
                            [x_coordinate,     y_coordinate - 1],
                            [x_coordinate    , y_coordinate    ],
                            [x_coordinate,     y_coordinate + 1],
                            [x_coordinate + 1, y_coordinate - 1],
                            [x_coordinate + 1, y_coordinate    ],
                            [x_coordinate + 1, y_coordinate + 1]]
    # 遍歷相鄰座標
    for index, value in enumerate(Neighbour_vertices):
        if value[0] in range(len(map)):
            if value[1] in range(len(map)):
               if value not in obstacle+path_vertices:
                    # 如果滿足節點 1, 在地圖邊界內 2, 不在障礙物點和已經經過的點, 計算權重
                    F.append(heuristic_distace(value, target_node) + go_around(index))
                    map[value[0]][value[1]] = str(F[-1])
               else:
                    F.append(10000)
            else:
                    F.append(10000)
        else:
                    F.append(10000)
               #a very large number

    # print(F) # 打印出遍歷的 節點的權重
    #將當前點設置爲 權重最小的點
    current_node=Neighbour_vertices[F.index(min(total_distance for total_distance in F))]
    map[current_node[0]][current_node[1]] = str(min(F))
    show_map(map)
    print(len(path_vertices))
    path_vertices.append(current_node)
      # if current_node not in visited_vertices:
      #     visited_vertices.append(current_node)
      # else:
      #     print("there is no route between")
      #     break

print(path_vertices)

運行完成

• 數字表示 A* 算法算出來的啓發距離, 代碼採用的是簡單的曼哈頓距離實現的