我正在为一个机器人项目做一个前端(一个’自主’汽车,它使用一些传感器和一个地图 – 从SVG文件生成本地化).
为了使机器人可控,我们必须在其当前位置和目标之间生成路径.我使用了最简单的算法:A *.
我得到了一些奇怪的结果:汽车倾向于达到45°的倍数,还有一个特别恼人的问题:一些生成的路径非常嘈杂!
在这种情况下,请查看橙色矩形附近的嘈杂路径:
反正有没有避免那些奇怪/嘈杂的结果?最终我们想要建立一个具有最小数量的航向角变化的路径. (汽车可以在不移动的情况下转弯,因此我们不需要任何路径’平滑’).
这是我的A *实现:
def search(self, begin, goal):
if goal.x not in range(self.width) or goal.y not in range(self.height):
print "Goal is out of bound"
return []
elif not self.grid[begin.y][begin.x].reachable:
print "Beginning is unreachable"
return []
elif not self.grid[goal.y][goal.x].reachable:
print "Goal is unreachable"
return []
else:
self.cl = set()
self.ol = set()
curCell = begin
self.ol.add(curCell)
while len(self.ol) > 0:
# We choose the cell in the open list having the minimum score as our current cell
curCell = min(self.ol, key = lambda x : x.f)
# We add the current cell to the closed list
self.ol.remove(curCell)
self.cl.add(curCell)
# We check the cell's (reachable) neighbours :
neighbours = self.neighbours(curCell)
for cell in neighbours:
# If the goal is a neighbour cell :
if cell == goal:
cell.parent = curCell
self.path = cell.path()
self.display()
self.clear()
return self.path
elif cell not in self.cl:
# We process the cells that are not in the closed list
# (processing <-> calculating the "F" score)
cell.process(curCell, goal)
self.ol.add(cell)
编辑1:通过popuplar需求,这里是分数计算功能(过程):
def process(self, parent, goal):
self.parent = parent
self.g = parent.distance(self)
self.h = self.manhattanDistance(goal)
self.f = self.g + self.h
编辑这是邻居方法(根据user1884905的回答更新):
def neighbours(self, cell, radius = 1, unreachables = False, diagonal = True):
neighbours = set()
for i in xrange(-radius, radius + 1):
for j in xrange(-radius, radius + 1):
x = cell.x + j
y = cell.y + i
if 0 <= y < self.height and 0 <= x < self.width and ( self.grid[y][x].reachable or unreachables ) and (diagonal or (x == cell.x or y == cell.y)) :
neighbours.add(self.grid[y][x])
return neighbours
(这看起来很复杂,但它只给出了一个单元的8个邻居 – 包括对角线邻居;它也可以采用与1不同的半径,因为它用于其他功能)
和距离计算(取决于对角线的使用与否:)
def manhattanDistance(self, cell):
return abs(self.x - cell.x) + abs(self.y - cell.y)
def diagonalDistance(self, cell):
xDist = abs(self.x - cell.x)
yDist = abs(self.y - cell.y)
if xDist > yDist:
return 1.4 * yDist + (xDist - yDist)
else:
return 1.4 * xDist + (yDist - xDist)
解决方法:
如果无法看到你如何实现邻居和距离函数,我仍然可以很好地猜测出现了什么问题:
如果允许对角线遍历,则不应使用曼哈顿距离.
目标函数中的曼哈顿距离应该是距离目标的最短距离的度量. (如果你可以沿着对角线穿过积木,那就不是了.)
解决此问题的最简单方法是将曼哈顿距离保持为目标函数,并将邻居的定义更改为仅包括四个左右上下相邻单元格.
编辑
您的代码仍然存在问题.以下伪代码取自wikipedia.我已标记了您必须检查的重要行.如果找到更好的解决方案,则必须确保i)更新开放集中的节点; ii)您始终考虑先前行驶的距离.
function A*(start,goal)
closedset := the empty set // The set of nodes already evaluated.
openset := {start} // The set of tentative nodes to be evaluated, initially containing the start node
came_from := the empty map // The map of navigated nodes.
g_score[start] := 0 // Cost from start along best known path.
// Estimated total cost from start to goal through y.
f_score[start] := g_score[start] + heuristic_cost_estimate(start, goal)
while openset is not empty
current := the node in openset having the lowest f_score[] value
if current = goal
return reconstruct_path(came_from, goal)
remove current from openset
add current to closedset
for each neighbor in neighbor_nodes(current)
// -------------------------------------------------------------------
// This is the way the tentative_g_score should be calculated.
// Do you include the current g_score in your calculation parent.distance(self) ?
tentative_g_score := g_score[current] + dist_between(current,neighbor)
// -------------------------------------------------------------------
if neighbor in closedset
if tentative_g_score >= g_score[neighbor]
continue
// -------------------------------------------------------------------
// You never make this comparrison
if neighbor not in openset or tentative_g_score < g_score[neighbor]
// -------------------------------------------------------------------
came_from[neighbor] := current
g_score[neighbor] := tentative_g_score
f_score[neighbor] := g_score[neighbor] + heuristic_cost_estimate(neighbor, goal)
if neighbor not in openset
add neighbor to openset
return failure
function reconstruct_path(came_from, current_node)
if current_node in came_from
p := reconstruct_path(came_from, came_from[current_node])
return (p + current_node)
else
return current_node