In realen 3-dimensionalen Umgebungen, muss die Pfadberechnung für eine Bewegung natürlich auch dreidimensional erfolgen. Der A*-Algorithmus eignet sich dafür sehr gut. Er muss lediglich um die 3. Dimension (Höhe) erweitert werden:
Implementierung 3D A* Pathplanning in Python
[python]
from math import *
import random
import time
#
# Based on the great Course CS373 from Udacity taught by Sebastian Thrun
# https://www.udacity.com/course/cs373
#
# 3D Erweiterung von Paul Balzer
# CC-BY2.0 Lizenz
# Generate Grid
xgrid = 10
ygrid = 10
zgrid = 6
grid = [[[round(random.random()-.4) for x in range(xgrid+1)] for y in range(ygrid+1)] for z in range(zgrid+1)]
init = [0, 0, 0]
goal = [len(grid[0][0])-1, len(grid[0])-1, len(grid)-1]
heuristic = [[[0 for x in range(len(grid[0][0]))] for y in range(len(grid[0]))] for z in range(len(grid))]
delta = [[-1, 0, 0], # zurück
[ 0,-1, 0], # links
[ 1, 0, 0], # vor
[ 0, 1, 0], # rechts
[ 0, 0,-1], # unten
[ 0, 0, 1]] # oben
cost = 1
# A* Algorithm
def search():
closed = [[[0 for x in range(len(grid[0][0]))] for y in range(len(grid[0]))] for z in range(len(grid))]
closed[init[0]][init[1]][init[2]] = 1
expand = [[[-1 for x in range(len(grid[0][0]))] for y in range(len(grid[0]))] for z in range(len(grid))]
action = [[[-1 for x in range(len(grid[0][0]))] for y in range(len(grid[0]))] for z in range(len(grid))]
x = init[0]
y = init[1]
z = init[2]
g = 0
h = heuristic[z][y][x]
f = g+h
open = [[f, g, x, y, z]]
found = False # flag that is set when search is complete
resign = False # flag set if we can’t find expand
count = 0
while not found and not resign:
if len(open) == 0:
resign = True
return "Fail"
else:
open.sort()
open.reverse()
next = open.pop()
x = next[2]
y = next[3]
z = next[4]
g = next[1]
f = next[0]
expand[z][y][x] = count
count += 1
if x == goal[0] and y == goal[1] and z == goal[2]:
found = True
else:
for i in range(len(delta)):
x2 = x + delta[i][0]
y2 = y + delta[i][1]
z2 = z + delta[i][2]
if z2 >= 0 and z2 < len(grid) and \
y2 >=0 and y2 < len(grid[0]) and \
x2 >=0 and x2 < len(grid[0][0]):
if closed[z2][y2][x2] == 0 and grid[z2][y2][x2] == 0:
g2 = g + cost
f2 = g2 + heuristic[z2][y2][x2]
open.append([f2, g2, x2, y2, z2])
closed[z2][y2][x2] = 1
# Memorize the sucessfull action for path planning
action[z2][y2][x2] = i
print(‚\nA* Result:‘)
#for i in range(len(expand)):
# print(expand[i])
# Policy
“‘
policy = [[‚ ‚ for row in range(len(grid[0]))] for col in range(len(grid))]
x = goal[0]
y = goal[1]
policy[x][y]=’*‘ # Goal
“‘
path=[]
path.append([goal[0], goal[1], goal[2]])
while x != init[0] or y != init[1] or z != init[2]:
x2 = x-delta[action[z][y][x]][0]
y2 = y-delta[action[z][y][x]][1]
z2 = z-delta[action[z][y][x]][2]
#policy[x2][y2][z2]=delta_name[action[x][y][z]]
x = x2
y = y2
z = z2
# Path
path.append([x2, y2, z2])
“‘
print(‚\nActions:‘)
for i in range(len(action)):
print(action[i])
print(‚\nPolicy (Path):‘)
for i in range(len(policy)):
print(policy[i])
“‘
print(‚\nCoordinates for Path smoothing=‘)
path.reverse()
“‘
for i in range(len(path)):
print(path[i])
“‘
return path
# Heuristic berechnen
def calcheuristic(grid,goal):
for z in range(len(grid)):
for y in range(len(grid[0])):
for x in range(len(grid[0][0])):
# Euklidische Distanz für jede Zelle zum Ziel berechnen
dist=((x-goal[0])**2+(y-goal[1])**2+(z-goal[2])**2)**(1/2)
heuristic[z][y][x]=dist
“‘
for i in range(len(heuristic)):
print(heuristic[i])
“‘
return heuristic
def smooth(path, weight_data = 0.5, weight_smooth = 0.3, tolerance = 0.00001):
# Make a deep copy of path into newpath
newpath = [[0 for row in range(len(path[0]))] for col in range(len(path))]
for i in range(len(path)):
for j in range(len(path[0])):
newpath[i][j] = path[i][j]
change = tolerance
while change >= tolerance:
change = 0.0
for i in range(1, len(path)-1): # 1. und letzten Punkt unberuhrt lassen
for j in range(len(path[0])):
aux = newpath[i][j]
newpath[i][j] += weight_data * (path[i][j] – newpath[i][j])
newpath[i][j] += weight_smooth * (newpath[i-1][j] \
+ newpath[i+1][j] – (2.0*newpath[i][j]))
change += abs(aux- newpath[i][j])
print(‚\nSmoothed Path‘)
for i in range(len(path)):
print(path[i], newpath[i])
return newpath
# Main Program
start = time.clock()
calcheuristic(grid,goal)
print(‚Calcheuristic: %0.3fs‘ % (time.clock() – start))
start = time.clock()
path=search()
print(‚A*: %0.3fs‘ % (time.clock() – start))
start = time.clock()
spath=smooth(path)
print(‚Path Smoothing: %0.3fs‘ % (time.clock() – start))
[/python]
Visualisierung des 3D A* Pathplanning Algorithmus‘ in Blender
httpvh://www.youtube.com/watch?v=6REuI63sAj8
Angepasste Heuristic zum Ausweichen von Hindernissen
Möchte man jetzt Hindernissen ausweichen und zur Not „drüber springen“, so muss man die Heuristic etwas anpassen.
[python]
# Heuristic berechnen
def calcheuristic(grid,goal):
for z in range(len(grid)):
for y in range(len(grid[0])):
for x in range(len(grid[0][0])):
# Euklidische Distanz für jede Zelle zum Ziel berechnen
dist=((x-goal[0])**2+(y-goal[1])**2+(z-goal[2])**2)**(1/2)
# Höhe
zheu = 5*z**2
# Horizontale von Soll
yheu = (y – goal[1])**2
# und Höhe und Abweichung von y=0
heuristic[z][y][x]=dist+ zheu + yheu
return heuristic
[/python]
Die Heuristic, je nach Höhe (z-Dimension) ist:
Ein höherer Wert bedeutet eine zusätzliche „Bestrafung“ für den Pfad.
Das „Bestrafen“ der Querabweichung zum Soll sowie das „Bestrafen“, wenn man vom Boden abheben muss, führen dazu, dass der Pfad immer möglichst auf dem Boden und möglichst mittig verläuft.