路径规划器的实现
1. 伪代码
function A*(start, goal)
//初始化已经探索过的节点为空
closedSet := {}
// 初始化未探索的节点，开始时仅包含起点
openSet := {start}
//cameFrom是一个字典，键是当前节点，值是当前节点的父节点
cameFrom := the empty map
// gScore为一个字典，存放该点的gsocre，起点为0，其余点初始化为无穷
gScore := map with default value of Infinity
gScore[start] := 0
// fScore为一个字典，存放该点的fsocre F = G + H，
//因此起点为起点到终点的直线距离 ，其余点初始化为无穷
fScore := map with default value of Infinity
fScore[start] := heuristic_cost_estimate(start, goal)
while openSet is not empty
//取出F值最小的节点设为当前点
current := the node in openSet having the lowest fScore[] value //当前点为目标点，跳出循环返回路径，并通过camefrom来生成路径（逆序）
if current = goal
return reconstruct_path(cameFrom, current)
//将该点从待探索集中移除，放入已探索集
openSet.Remove(current)
closedSet.Add(current)
//遍历当前节点的所有邻居节点（需要在未探索集中）
for each neighbor of current
if neighbor in closedSet
continue
//Discover a new node
if not neighbor in openSet:
self.openSet.add(neighbor)
//计算tentative_gScore作为新路径的gScore
//tentative_gScore可以理解为假设移动到临接点之后点gscore
tentative_gScore := gScore[current] + dist_between(current, neighbor)
//类似冒泡排序，首先存一个最小耗散的点（初始时可能是任意的，然后如果发现比它小的就替换掉，否则使用原来的路径）
//新gScore>=原gScore，则按照原路径（移动后好散增加了）
if tentative_gScore >= gScore[neighbor]
continue
// 否则选择gScore较小的新路径，并更新G值与F值。同时更新节点的父子关系。
cameFrom[neighbor] := current
gScore[neighbor] := tentative_gScore
fScore[neighbor] := gScore[neighbor] + heuristic_cost_estimate(neighbor, goal)
return failure
//从caomeFrom中从goal点追溯到start点，取得路径节点。
2. 基于函数实现
这里直接引用 jluiz20/udacity_python_a_start_route_planner的实现
import math
def shortest_path(M,start,goal):
print("shortest path called, Start", start, "Goal", goal)
#the set already explored
closedSet = set()
#only the start node is known
openSet = {start}
#the dictionary with the parent with the best way until the node
cameFrom = dict()
#cost of getting from the start to the node
gScore = dict()
gScore[start] = 0
fScore = dict()
#the straight line distance between start and goal
fScore[start] = distance(M.intersections[start], M.intersections[goal])
while openSet:
current = node_lowest_value(openSet, fScore)
# If reached the goal, YEAH!
if current == goal:
print("Goal Reached!")
return reconstruct_path(cameFrom, current)
openSet.remove(current) #remove from open
closedSet.add(current) #mark as processed
for neighbor in M.roads[current]:
if neighbor in closedSet:
continue
#add the neighbor to the frontier
openSet.add(neighbor)
#The distance from start to a neighbor
tentative_gScore = gScore[current] + distance(M.intersections[current], M.intersections[neighbor])
if neighbor in gScore:
if tentative_gScore >= gScore[neighbor]:
continue # This is not a better path.
# This path is the best until now. Record it!
cameFrom[neighbor] = current
gScore[neighbor] = tentative_gScore
fScore[neighbor] = gScore[neighbor] + distance(M.intersections[neighbor], M.intersections[goal])
return
def reconstruct_path(cameFrom, current):
total_path = list()
total_path.insert(0,current)
while current in cameFrom:
current = cameFrom[current]
total_path.insert(0 , current)
return total_path
def node_lowest_value(openSet, fScore):
lowest_node = 35767 #large number
lowest_value = float('inf')
for node in fScore:
if fScore[node] < lowest_value:
if node in openSet:
lowest_node = node
lowest_value = fScore[node]
return lowest_node
def distance(p1,p2):
distance = math.sqrt( ((p1[0]-p2[0])**2)+((p1[1]-p2[1])**2) )
return distance
3. 基于类的实现
class PathPlanner():
"""Construct a PathPlanner Object"""
def __init__(self, M, start=None, goal=None):
""" """
self.map = M
self.start= start
self.goal = goal
self.closedSet = self.create_closedSet() if goal != None and start != None else None
self.openSet = self.create_openSet() if goal != None and start != None else None
self.cameFrom = self.create_cameFrom() if goal != None and start != None else None
self.gScore = self.create_gScore() if goal != None and start != None else None
self.fScore = self.create_fScore() if goal != None and start != None else None
self.path = self.run_search() if self.map and self.start != None and self.goal != None else None
def create_closedSet(self):
return set()
def create_openSet(self):
if self.start != None:
return {self.start}
raise(ValueError, "Must create start node before creating an open set. Try running PathPlanner.set_start(start_node)")
def create_cameFrom(self):
return dict()
def create_gScore(self):
gScore = dict().fromkeys(self.map.intersections,float('inf'))
gScore[self.start] = 0
return gScore
def create_fScore(self):
fScore=dict().fromkeys(self.map.intersections,float('inf'))
fScore[self.start] = self.distance(self.map.intersections[self.start],self.map.intersections[self.goal])
return fScore
def _reset(self):
self.closedSet = None
self.openSet = None
self.cameFrom = None
self.gScore = None
self.fScore = None
self.path = self.run_search() if self.map and self.start and self.goal else None
def get_path(self):
""" Reconstructs path after search """
if self.path:
return self.path
else :
self.run_search()
return self.path
def set_map(self, M):
self._reset(self)
self.start = None
self.goal = None
self.map = M
def set_start(self, start):
self._reset(self)
self.start = start
def set_goal(self, goal):
self._reset(self)
self.goal = goal
def get_current_node(self):
lowest_node = 9999
lowest_value = float('inf')
for node in self.openSet:
if node in self.fScore and self.fScore[node] < lowest_value:
lowest_node = node
lowest_value = self.fScore[node]
return lowest_node
def get_neighbors(self, node):
return self.map.roads[node]
def get_gScore(self, node):
return self.gScore[node]
def get_tenative_gScore(self, current, neighbor):
return self.get_gScore(current) + self.distance(self.map.intersections[current], self.map.intersections[neighbor])
def is_open_empty(self):
return len(self.openSet)==0
def distance(self, node_1, node_2):
distance = math.sqrt( ((node_1[0]-node_2[0])**2)+((node_1[1]-node_2[1])**2) )
return distance
def heuristic_cost_estimate(self, node):
return self.distance(self.map.intersections[node], self.map.intersections[self.goal])
def calculate_fscore(self, node):
return self.get_gScore(node) + self.heuristic_cost_estimate(node)
def record_best_path_to(self, current, neighbor):
self.cameFrom[neighbor] = current
self.gScore[neighbor] = self.get_tenative_gScore(current,neighbor)
self.fScore[neighbor] = self.calculate_fscore(current)
def reconstruct_path(self, current):
""" Reconstructs path after search """
total_path = [current]
while current in self.cameFrom.keys():
current = self.cameFrom[current]
total_path.append(current)
return total_path
def run_search(self):
""" """
if self.map == None:
raise(ValueError, "Must create map before running search. Try running PathPlanner.set_map(start_node)")
if self.goal == None:
raise(ValueError, "Must create goal node before running search. Try running PathPlanner.set_goal(start_node)")
if self.start == None:
raise(ValueError, "Must create start node before running search. Try running PathPlanner.set_start(start_node)")
self.closedSet = self.closedSet if self.closedSet != None else self.create_closedSet()
self.openSet = self.openSet if self.openSet != None else self.create_openSet()
self.cameFrom = self.cameFrom if self.cameFrom != None else self.create_cameFrom()
self.gScore = self.gScore if self.gScore != None else self.create_gScore()
self.fScore = self.fScore if self.fScore != None else self.create_fScore()
while not self.is_open_empty():
current = self.get_current_node()
if current == self.goal:
self.path = [x for x in reversed(self.reconstruct_path(current))]
return self.path
else:
self.openSet.remove(current)
self.closedSet.add(current)
for neighbor in self.get_neighbors(current):
if neighbor in self.closedSet:
continue # Ignore the neighbor which is already evaluated.
if not neighbor in self.openSet: # Discover a new node
self.openSet.add(neighbor)
# The distance from start to a neighbor
#the "dist_between" function may vary as per the solution requirements.
if self.get_tenative_gScore(current, neighbor) >= self.get_gScore(neighbor):
continue # This is not a better path.
# This path is the best until now. Record it!
self.record_best_path_to(current, neighbor)
print("No Path Found")
self.path = None
return False
if __name__ == '__main__':
planner = PathPlanner(map_40, 5, 34)
path = planner.path