完整的覆盖路径规划

时间:2014-07-28 15:20:23

标签: python

我试图从头开始编写一些python代码。

此代码需要使机器人(表示为节点)覆盖所有工作空间并避开障碍物(对障碍物的位置有先验知识)。

我发现工作空间可以表示为矩阵系统。

使用波前距离变换(如图所示:http://www.emeraldinsight.com/content_images/fig/0490390507009.png

如何编写代码,使机器人从一个节点移动到下一个最高值的节点,同时跟踪被访问的节点?

由于

编辑:这是我上网的python代码(我有中级编程技巧),我试图通过波前距离变换来适应boustrophedon运动(来回)。

代码当前卡在第一行(x = 0和y = 0),试图移动到下一行并运行最高节点值。

尝试:     导入numpy 除了:     print“未安装numpy数学库。” 进口时间

class 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)

如果名称 ==“主要”:     “””     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 :(得分:0)

  1. 将网格保持在矩阵内,每个节点都包含您需要的信息:值,是障碍等等。

  2. 一种方法是使用深度优先搜索来获取所有可能的路径,并从引导您到达目的地的路径中选择最高价值路线。

  3. Rant:这个问题被标记为Node.js,你应该删除标签,因为它没有任何关系。