路径规划算法(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* 算法算出来的启发距离, 代码采用的是简单的曼哈顿距离实现的