A*图搜索算法代码实现

一、项目地址

A* Graph Search Project

关于A*算法的介绍，可以参考modern robotics书中的chapter 10部分，以下是笔者编写的python代码实现，并用V-rep仿真出规划结果。

二、Python代码实现

输入:
nodes.scv: 描述图的节点
edges.scv: 描述图的边

输出:
path.csv: 最短路径计算结果

import numpy as np
import csv


class AStarPathPlanner(object):
    def __init__(self, nodes_path, edges_path):
        # initialize heuristic_cost_to_go for each node
        self.heuristic_cost_to_go = []
        with open(nodes_path, "rb") as f_obj:
            contents = csv.reader(f_obj)
            for row in contents:
                # ignore comments in file
                if(row[0][0] != '#'):
                    self.heuristic_cost_to_go.append(float(row[3]))
        self.nodes_number = len(self.heuristic_cost_to_go)

        # initialize neighbours and cost for each node
        # neighbours information is stored in a dict
        # while cost information is stored in a matrix
        self.neighbour_table = {}
        self.cost = np.zeros((self.nodes_number, self.nodes_number))
        with open(edges_path, "rb") as f_obj:
            contents = csv.reader(f_obj)
            for row in contents:
                # ignore comments in file
                if(row[0][0] != '#'):
                    node1 = int(row[0])
                    node2 = int(row[1])
                    # construct neighbour information
                    if node1 not in self.neighbour_table:
                        self.neighbour_table[node1] = []
                    self.neighbour_table[node1].append(node2)
                    if node2 not in self.neighbour_table:
                        self.neighbour_table[node2] = []
                    self.neighbour_table[node2].append(node1)

                    # construct neighbour cost information
                    cost = float(row[2])
                    self.cost[node1-1][node2-1] = cost
                    self.cost[node2-1][node1-1] = cost
       
    def search_for_path(self):
        self.OPEN = [1]
        self.CLOSED = []
        self.est_total_cost = [float("inf")] * (self.nodes_number)
        self.past_cost = [0] * (self.nodes_number)
        self.past_cost[0] = 0
        self.path = []
        # store node parent information
        self.parent = {}
        # set to infinity for node 2...N
        for i in range(1, self.nodes_number):
            self.past_cost[i] = float("inf")
        while self.OPEN:
            current = self.OPEN.pop(0)
            self.CLOSED.append(current)
            # path has been found
            if current == self.nodes_number:
                # reconstruct path by parent node
                while True:
                    self.path.append(current)
                    if current == 1:
                        break
                    current = self.parent[current]
                return True, self.path

            for nbr in self.neighbour_table[current]:
                if nbr not in self.CLOSED:
                    tentative_past_cost = self.past_cost[current-1] + self.cost[current-1, nbr-1]
                    if tentative_past_cost < self.past_cost[nbr-1]:
                        self.past_cost[nbr-1] = tentative_past_cost
                        self.parent[nbr] = current
                        self.est_total_cost[nbr-1] = self.past_cost[nbr-1] + self.heuristic_cost_to_go[nbr-1]
                        if nbr not in self.OPEN:
                            self.OPEN.append(nbr)
                        self.OPEN.sort(key=lambda x: self.est_total_cost[x-1])
        return False, []

    def save_path_to_file(self, path):
        with open(path, 'wb') as f_obj:
            writer = csv.writer(f_obj, delimiter=',')
            writer.writerow(self.path[::-1])


if __name__ == "__main__":
    planner = AStarPathPlanner("nodes.csv", "edges.csv")
    success, path = planner.search_for_path()
    if success:
        print path[::-1]
        planner.save_path_to_file("path.csv")
    else:
        print "no solution found"

输出最短路径结果：

[1,3,4,7,10,12]

与给的文件结果一致，证明算法实现正确。接下来修改一下edges.csv文件，注释掉第17行，从而删除掉节点4通往节点7的路径：

# 7,4,0.3417

再次运行程序，得到结果：

[1, 2, 5, 7, 10, 12]

三、V-rep中仿真计算结果

输入nodes.csv, edges.csv, obstacle.csv, path.csv文件所在路径，Open File ---> Play，场景中将仿真出机器人从起点到目标点的路径。

edges.csv修改前 edges.csv修改后