完全覆盖路径规划
完全覆盖路径规划
提问于 2014-07-28 15:20:23
我正试图从头开始编写一些python代码。
该代码需要使机器人(表示为节点)覆盖所有的工作空间并避免障碍物(对障碍物的位置有先验的了解)。
我认为工作空间可以表示为一个矩阵系统。
使用波前距离变换(如下图所示：images/fig/0490390507009.png)
我如何编写代码，使机器人从一个节点移动到下一个最高值节点，同时跟踪访问的节点？
谢谢
编辑:这是我在网上获得的python代码(我有中级编程技能)，我正试图通过波前距离变换来适应boustrophedon运动(来回)。
当前，代码被卡在第一行(x=0和y=0)中，试图移动到下一行并运行最高的节点值。
尝试:导入numpy除了:打印“numpy数学库没有安装”。导入时间
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)如果名称 == "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
Stack Overflow用户
发布于 2014-07-28 15:33:25
- 将网格保存在矩阵中，每个节点都有您需要的信息: value，它是否是一个障碍，等等。
- 一种方法是使用深度优先搜索来获取所有可能的路径，并从那些引导您到达终点的路径中选择最高值的路径。
Rant:这个问题被标记为Node.js，您应该删除标记，因为它与任何时候都没有关系。
页面原文内容由Stack Overflow提供。腾讯云小微IT领域专用引擎提供翻译支持
原文链接：
https://stackoverflow.com/questions/24998526
复制相关文章
相似问题
腾讯云开发者
