一、项目地址
关于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修改后
