完整覆盖路径规划
我正在尝试从零开始写一些Python代码。
这段代码需要让一个机器人（用节点表示）覆盖所有的工作区域，并避开障碍物（我事先知道障碍物的位置）。
我发现工作区域可以用一个矩阵来表示。
我打算使用波前距离变换（可以参考这个图：http://www.emeraldinsight.com/content_images/fig/0490390507009.png）。
我该如何写代码，让机器人从一个节点移动到下一个值最高的节点，同时记录已经访问过的节点呢？
谢谢！
编辑：这是我在网上找到的Python代码（我有中级编程技能），我正在尝试将其改编成使用波前距离变换实现的往返运动。
目前代码在第一行（x=0和y=0）卡住了，试图移动到下一行并遍历最高的节点值。
尝试：
import numpy
except:
print "没有安装numpy数学库。"
import time
定义一个类 waveFrontPlanner：
def __init__(self, map, slow=False):
self.__slow = slow
self.__map = map
if str(type(map)).find("numpy") != -1:
#If its a numpy array
self.__height, self.__width = self.__map.shape
else:
self.__height, self.__width = len(self.__map), len(self.__map[0])
self.__nothing = 0
self.__wall = 999
self.__goal = 1
self.__path = "PATH"
#Robot value
self.__robot = 254
#Robot default Location
self.__robot_x = 0
self.__robot_y = 11
#default goal location
self.__goal_x = 18
self.__goal_y = 0
#temp variables
self.__counter = 0
self.__steps = 0 #determine how processor intensive the algorithm was
#when searching for a node with a lower value
self.__maximum_node = 250
self.__max_node_location = 250
self.__reset_max = 250 #above this number is a special (wall or robot)
###########################################################################
def run(self, prnt=False):
"""
The entry point for the robot algorithm to use wavefront propagation.
"""
path = []
while self.__map[self.__robot_x][self.__robot_y] != self.__goal:
if self.__steps > 20000:
print "Cannot find a path."
return
#find new location to go to
self.__new_state = self.propagateWavefront()
#update location of robot
if self.__new_state == 1:
self.__robot_x -= 1
if prnt:
print "Move to x=%d y=%d\n\n" % \
(self.__robot_x, self.__robot_y)
path.append((self.__robot_x, self.__robot_y))
if self.__new_state == 2:
self.__robot_y += 1
if prnt:
print "Move to x=%d y=%d\n\n" % \
(self.__robot_x, self.__robot_y)
path.append((self.__robot_x, self.__robot_y))
if self.__new_state == 3:
self.__robot_x += 1
if prnt:
print "Move to x=%d y=%d\n\n" % \
(self.__robot_x, self.__robot_y)
path.append((self.__robot_x, self.__robot_y))
if self.__new_state == 4:
self.__robot_y -= 1
if prnt:
print "Move to x=%d y=%d\n\n" % \
(self.__robot_x, self.__robot_y)
path.append((self.__robot_x, self.__robot_y))
self.__old_state = self.__new_state
msg = "Found the goal in %i steps:\n" % self.__steps
msg += "Map size= %i %i\n\n" % (self.__height, self.__width)
if prnt:
print msg
self.printMap()
return path
###########################################################################
def propagateWavefront(self, prnt=False):
"""
"""
#self.unpropagate()
#Old robot location was deleted, store new robot location in map
self.__map[self.__robot_x][self.__robot_y] = self.__robot
self.__path = self.__robot
#Start location to begin scan at goal location
self.__map[self.__goal_x][self.__goal_y] = self.__goal
counter = 0
while counter < 200: #allows for recycling until robot is found
x = 0
y = 0
time.sleep(0.00001)
#while the map hasnt been fully scanned
while x < self.__height and y < self.__width:
#if this location is a wall or the goal, just ignore it
if self.__map[x][y] != self.__wall and \
self.__map[x][y] != self.__goal:
#a full trail to the robot has been located, finished!
maxLoc = self.maxSurroundingNodeValue(x, y)
if maxLoc < self.__reset_max and \
self.__map[x][y] == self.__robot:
if prnt:
print "Finished Wavefront:\n"
self.printMap()
# Tell the robot to move after this return.
return self.__max_node_location
#record a value in to this node
elif self.__maximum_node != self.__reset_max:
#if this isnt here, 'nothing' will go in the location
self.__map[x][y] = self.__maximum_node + 1
#go to next node and/or row
y += 1
if y == self.__width and x != self.__height:
x += 1
y = 0
#print self.__robot_x, self.__robot_y
if prnt:
print "Sweep #: %i\n" % (counter + 1)
self.printMap()
self.__steps += 1
counter += 1
return 0
###########################################################################
#def unpropagate(self):
"""
clears old path to determine new path
stay within boundary
"""
#for x in range(0, self.__height):
#for y in range(0, self.__width):
#if self.__map[x][y] != self.__wall and self.__map[x][y] != self.__goal:
#self.__map[x][y] = self.__path
#if this location is a wall or goal, just ignore it
#clear that space
###########################################################################
def maxSurroundingNodeValue(self, x, y):
"""
this method looks at a node and returns the lowest value around that
node.
"""
#reset minimum
self.__maximum_node = self.__reset_max
if y > 0 and x < (self.__height - 1):
if self.__map[x][y-1] < self.__maximum_node and self.__map[x+1][y] < self.__maximum_node and\
self.__map[x][y-1] != self.__wall and self.__map[x+1][y] != self.__wall:
if self.__map[x][y-1] >= self.__map[x+1][y]:
self.__maximum_node = self.__map[x][y-1]
self.__max_node_location = 4
else:
self.__maximum_node = self.__map[x+1][y]
self.__max_node_location = 3
if y < self.__width - 1 and x < self.__height - 1:
if self.__map[x][y+1] < self.__maximum_node and self.__map[x+1][y] < self.__maximum_node:
if self.__map[x][y+1] >= self.__map[x+1][y]:
self.__maximum_node = self.__map[x][y+1]
self.__max_node_location = 2
else:
self.__maximum_node = self.__map[x+1][y]
self.__max_node_location = 3
if x > 0 and y > 0:
if self.__map[x-1][y] < self.__maximum_node and self.__map[x][y-1] < self.__maximum_node:
if self.__map[x-1][y] >= self.__map[x][y-1]:
self.__maximum_node = self.__map[x-1][y]
self.__max_node_location = 1
else:
self.__maximum_node = self.__map[x][y-1]
self.__max_node_location = 4
if y < self.__width - 1 and x > 0:
if self.__map[x-1][y] < self.__maximum_node and self.__map[x][y+1] < self.__maximum_node:
if self.__map[x-1][y] >= self.__map[x][y+1]:
self.__maximum_node = self.__map[x-1][y]
self.__max_node_location = 1
else:
self.__maximum_node = self.__map[x][y+1]
self.__max_node_location = 2
return self.__maximum_node
###########################################################################
def printMap(self):
"""
Prints out the map of this instance of the class.
"""
msg = ''
for temp_B in range(0, self.__height):
for temp_A in range(0, self.__width):
if self.__map[temp_B][temp_A] == self.__wall:
msg += "%04s" % "[X]"
elif self.__map[temp_B][temp_A] == self.__robot:
msg += "%04s" % "o"
elif self.__map[temp_B][temp_A] == self.__goal:
msg += "%04s" % "G"
else:
msg += "%04s" % str(self.__map[temp_B][temp_A])
msg += "\n\n"
msg += "\n\n"
print msg
#
if self.__slow == True:
time.sleep(0.05)
如果 name == "main"：
""" X是垂直的，Y是水平的
"""
floormap = [[19,19,19,19,19,19,19,19,19,19,19,19], \
[18,18,18,18,18,18,18,18,18,18,18,18], \
[17,17,17,17,17,17,17,17,17,17,17,17], \
[16,16,16,16,16,16,16,16,16,16,16,16], \
[15,15,15,15,15,15,15,15,15,15,15,15], \
[14,14,14,14,14,14,14,14,14,14,14,14], \
[13,13,13,13,13,13,13,13,13,13,13,13], \
[12,12,12,12,12,12,12,12,12,12,12,12], \
[11,11,11,11,11,11,11,11,11,11,11,12], \
[10,10,10,10,10,10,10,10,10,10,11,12], \
[9,9,9,9,9,9,9,9,9,10,11,12], \
[8,8,8,8,8,8,8,8,9,10,11,12], \
[7,7,7,7,7,7,7,8,9,10,11,12], \
[6,6,6,6,6,6,7,8,9,10,11,12], \
[5,5,5,5,5,6,7,8,9,10,11,12], \
[4,4,4,4,5,6,7,8,9,10,11,12], \
[3,3,3,4,5,6,7,8,9,10,11,12], \
[2,2,3,4,5,6,7,8,9,10,11,12], \
[1,2,3,4,5,6,7,8,9,10,11,12]]
start = time.time()
planner = waveFrontPlanner(floormap, False)
#time.sleep(2)
planner.run(True)
end = time.time()
print "Took %f seconds to run wavefront simulation" % (end - start)
1 个回答
0
把你的网格放在一个矩阵里，每个节点都要包含你需要的信息，比如它的值、是否是障碍物等等。
一种方法是使用深度优先搜索，这样可以找到所有可能的路径，然后从那些能到达终点的路径中选择一个值最高的。
抱怨：这个问题被标记为Node.js，但其实根本没有关系，应该把这个标签去掉。