时间戳

首页 > 解决方案 > 如何在障碍物或机器人周围添加安全区？

python - 如何在障碍物或机器人周围添加安全区？

问题描述

我正在用 Python 研究这个 A_star 算法，当算法避开障碍物时，我需要在障碍物或机器人本身周围创建一个安全区，直到机器人靠近障碍物时，将没有机会击中障碍。那么，我该如何添加这个 safe_zone？请问有什么帮助吗？

我的代码如下所示：

from __future__ import print_function
import random

grid = [[0, 1, 0, 0, 0, 0],
        [0, 1, 0, 0, 0, 0],#0 are free path whereas 1's are obstacles
        [0, 1, 0, 0, 0, 0],
        [0, 1, 0, 0, 1, 0],
        [0, 0, 0, 0, 1, 0]]


init = [0, 0]
goal = [len(grid)-1, len(grid[0])-1] #all coordinates are given in format [y,x] 
cost = 1


#the cost map which pushes the path closer to the goal
heuristic = [[0 for row in range(len(grid[0]))] for col in range(len(grid))]
for i in range(len(grid)):    
    for j in range(len(grid[0])):            
        heuristic[i][j] = abs(i - goal[0]) + abs(j - goal[1])




#the actions we can take
delta = [[-1, 0 ], # go up
         [ 0, -1], # go left
         [ 1, 0 ], # go down
         [ 0, 1 ]] # go right


#function to search the path
def search(grid,init,goal,cost,heuristic):

    closed = [[0 for col in range(len(grid[0]))] for row in range(len(grid))]# the referrence grid
    closed[init[0]][init[1]] = 1
    action = [[0 for col in range(len(grid[0]))] for row in range(len(grid))]#the action grid

    x = init[0]
    y = init[1]
    g = 0

    f = g + heuristic[init[0]][init[0]]
    cell = [[f, g, x, y]]

    found = False  # flag that is set when search is complete
    resign = False # flag set if we can't find expand

    while not found and not resign:
        if len(cell) == 0:
            resign = True
            return "FAIL"
        else:
            cell.sort()#to choose the least costliest action so as to move closer to the goal
            cell.reverse()
            next = cell.pop()
            x = next[2]
            y = next[3]
            g = next[1]
            f = next[0]


            if x == goal[0] and y == goal[1]:
                found = True
            else:
                for i in range(len(delta)):#to try out different valid actions
                    x2 = x + delta[i][0]
                    y2 = y + delta[i][1]
                    if x2 >= 0 and x2 < len(grid) and y2 >=0 and y2 < len(grid[0]):
                        if closed[x2][y2] == 0 and grid[x2][y2] == 0:
                            g2 = g + cost
                            f2 = g2 + heuristic[x2][y2]
                            cell.append([f2, g2, x2, y2])
                            closed[x2][y2] = 1
                            action[x2][y2] = i
    invpath = []
    x = goal[0]
    y = goal[1]
    invpath.append([x, y])#we get the reverse path from here
    while x != init[0] or y != init[1]:
        x2 = x - delta[action[x][y]][0]
        y2 = y - delta[action[x][y]][1]
        x = x2
        y = y2
        invpath.append([x, y])

    path = []
    for i in range(len(invpath)):
        path.append(invpath[len(invpath) - 1 - i])
    print("ACTION MAP")
    for i in range(len(action)):
        print(action[i])

    return path

a = search(grid,init,goal,cost,heuristic)
for i in range(len(a)):
    print(a[i])

标签： pythoncollision-detectionshortest-pathpath-findinga-star

解决方案

实现这一点的典型方法是在进行搜索之前增加障碍。

假设您的机器人是半径为 25 厘米的圆形。如果机器人中心距离障碍物小于 25 厘米，机器人的边缘会撞到障碍物，对吧？因此，您将障碍物扩大（膨胀）25 厘米（这意味着距离原始障碍物小于 25 厘米的任何点本身都会成为障碍物），这样您就可以规划机器人中心的运动。

如果您想要一个额外的安全余量，例如 10 厘米（即机器人边缘距离障碍物超过 10 厘米），您可以将障碍物充气 35 厘米而不是 25 厘米。

对于非圆形机器人，障碍物应至少膨胀最长轴的一半，以确保不会与障碍物发生碰撞。例如，如果Robot的形状为50x80，则应将障碍物充气80/2 = 40，以确保轨迹安全。

另外，请注意，障碍物膨胀方法最适合圆形/方形机器人。对于具有较长轴的矩形机器人/机器人，当机器人周围的网格图具有狭窄通道时，即使机器人可以真实地通过它，障碍物膨胀也会使其不可行，这可能会出现问题。

这种障碍物膨胀可以通过在被视为图像的地图上使用形态学操作以编程方式完成。例如，参见scikit-image.morphology中的膨胀。

推荐阅读