- Admissible heuristic h(currentNode,goal)
- We will not accidentally miss an optimal solution.
- In the case of optimal path planning: An admissible heuristic is any "Lower Bound" on the remaining distance to the goal.
- L1-norm is NOT an admissible heuristic, because it gives an overestimation of the distance.
- Priority queue sorting order:
- A* uses 'cost-to-start + admissible heuristic'
- Dijkstra uses 'cost-to-start'
:::warning
Function AStarAlgorithm(G,start,goal,Q)
UNVISITED ← V\{start}
while Q.top()≠Ø
v ← Q.pop()
while v.neighbor()≠Ø
u ← v.nextneighbor()
if u ∈ UNVISITED or u.costToStart > v.costToStart + cost(v,u)
UNVISITED ← UNVISITED\{u}
u.parent ← v
u.costToStart ← v.costToStart + cost(v,u)
Q.update(u,u.costToStart+h(u,goal))
if v = goal
return SUCCESS
return FAILURE
:::
:::warning Construct a nxn 8-connected Grid Graph
import networkx as nx
n = 4
G = nx.grid_2d_graph(n,n)
Node Initialization: Assign index from 1, set costs as zero, and label parent and 'visited' property.
idx = 1
for node in G.nodes:
G.nodes[node]['idx'] = idx
G.nodes[node]['g'] = 0
G.nodes[node]['h'] = 0
G.nodes[node]['f'] = 0
G.nodes[node]['parent'] = None
G.nodes[node]['visited']= False
if idx == start_idx:
start_node = node
idx = idx+1
Note that the default setting of grid graph in networkX is 4-connected, so we need to add diagonal edges manually to have graph 8-connected and set weights of the diagonal edges as 1.414 to ensure triangle inequality.
# Set all weights to 1
for edge in G.edges:
G.edges[edge]['weight'] = 1
# Add diagonal edges to make 8-connected grid
G.add_edges_from([
((x, y), (x+1, y+1))
for x in range(n-1)
for y in range(n-1)
] + [
((x+1, y), (x, y+1))
for x in range(n-1)
for y in range(n-1)
], weight=1.414)
A* starts here
# Initialize priority queue and visited list
Q = []
VISITED = [] # visited list
path = []
Q.append(start_node)
if len(Q) > 0:
current_node = Q.pop(0)
G.nodes[current_node]['visited'] = True
VISITED.append(current_node)
if current_node == goal_node:
print('Goal!')
path = backTrace(G, VISITED)
reachGoal = True
neighbors = G.neighbors(current_node)
for neighbor in neighbors:
if neighbor not in obs_list:
current_g = G.nodes[neighbor]['g']
new_g = G.nodes[current_node]['g']+ G.get_edge_data(current_node,neighbor)['weight']
if not G.nodes[neighbor]['visited'] or current_g > new_g:
g = new_g
y_dis2goal = int(neighbor[1])-int(goal_pos[0])
x_dis2goal = int(neighbor[0])-int(goal_pos[1])
h = np.linalg.norm((x_dis2goal,y_dis2goal), 2)
f = h+g
# Update node data and priority queue
G.nodes[neighbor]['visited'] = True
G.nodes[neighbor]['parent'] = current_node
G.nodes[neighbor]['g'] = g
G.nodes[neighbor]['f'] = f
G.nodes[neighbor]['h'] = h
Q.append(neighbor)
Q = sorted(Q, key=lambda n: G.nodes[n]['f'])
:::