Como planejar a rota ótima de um robô pontual em um grid com obstáculos?

Question

Imagine que temos um robô pontual (ou seja, o corpinho dele é só um ponto), e que ele possa fazer quatro movimentos (para cima, para baixo, para a direita e para a esquerda) e que ele esteja num grid com obstáculos, esses obstáculos tambeḿ são pontuais, mas um conjunto de obstáculos pode formar qualquer figura geométrica (mas vai ser discreto e a precisão vai depender do tamanho do grid).

Esse grid é como (ou melhor, é) uma matriz de 0s e 1s, as posições que tem 1 são as posições com obstáculos.

Pede-se que se encontre um caminho ótimo de um ponto inicial a algum ponto final desse grid

Answer 1

Vamos resolver o problea fazendo uma "busca em largura". O grid pode ser visto como um grafo, por exemplo, a partir da posição [3,1], o robo pode ir para [2,1], [4,1], [3,2], [3,0] desde que essas posições existam.

Então vamos fazer uma busca em largura, guardando no caminho quanto já se andou para chegar até ali.

Note que cada posição que avaliarmos, e que dermos um número de quantos passos para chegar até ali, esse vai ser o número mínimo, porque estamos fazendo uma busca em largura.

Além disso, vamos fazer de modo que vamos marcar todos as popsições que se alcança com n movimentos, e depois vamos olhar n+1 movimentos, então, quando alcançarmos o goal, saberemos que esse é o mínimo de movimentos para alcançar o goal.

import numpy as np
import matplotlib.pyplot as plt


delta = [[-1,0], #para cima
         [0,-1], #direita
        [1,0], #abaixo
        [0,1]] #esquera
cost = 1 
def search(grid, init, goal):
    closed = [[0 for row in range(len(grid[0]))] for col in range(len(grid))]
    value_grid = [[0 for row in range(len(grid[0]))] for col in range(len(grid))]
    closed[init[0]][init[1]]=1
    x = init[0]
    y = init[1]
    g=0
    abertos = [[g,x,y]]

    found = False
    resign = False

    abertos_maximo = (len(grid)+1)*(len(grid[0])+1)-1        

    while not found and not resign:
        if len(abertos)==abertos_maximo:
            resign = True
            return 'fail'
        else:
            abertos.sort()
            abertos.reverse()
            proximo = abertos.pop()
            x = proximo[1]
            y = proximo[2]
            g = proximo[0]

            if x == goal[0] and y == goal[1]:
                found = True
            else:
                for i in range(len(delta)):
                    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
                            value_grid[x2][y2]=g2
                            abertos.append([g2, x2, y2])
                            closed[x2][y2] =1
    return value_grid


def mostra_caminho(value_grid, init, goal):
    label_obstatulos = 0
    for i in range(len(value_grid)): #linhas
        for j in range(len(value_grid[0])): #colunas
            if i==init[0] and j==init[1]:
                plt.plot([j], [i], marker='o', markersize=8, color="blue", label = 'Inicial')
            elif i==goal[0] and j == goal[1]:
                maximo = value_grid[i][j]
                plt.plot([j], [i], marker='o', markersize=8, color="yellow", label = 'Goal')
            elif value_grid[i][j]==0:
                if label_obstatulos == 0:
                    plt.plot([j], [i], marker='o', markersize=5, color="red", label = 'Obstaculos')
                    label_obstatulos =1
                else:
                    plt.plot([j], [i], marker='o', markersize=5, color="red")
    #tenho o maximo e tenho o goal
    caminho = []
    caminho.append(goal)
    label = 0
    for k in range(0,maximo):
        achei = 0
        proximo = caminho.pop()
        #print('proximo é', proximo)
        x = proximo[0]
        y = proximo[1]
        for i in range(len(delta)):
            x2 = x + delta[i][0]
            y2 = y + delta[i][1]
            if x2>=0 and x2<len(value_grid) and y2>=0 and y2<len(value_grid[0]):
                if value_grid[x2][y2] == maximo-1-k and achei == 0:

                    achei = 1
                    if label == 0:
                        plt.plot([y2], [x2], marker='o', markersize=5, color="green", label = 'Caminho ótimo')
                        label = 1
                    else:
                        plt.plot([y2], [x2], marker='o', markersize=5, color="green")
                    pos = [x2,y2]
                    caminho.append(pos)


    plt.grid()
    plt.legend(bbox_to_anchor=(1.05, 1), loc=2, borderaxespad=0.)
    #plt.legend()
    plt.show()

def busca_caminho(grid, init, goal):
    value_grid = search(grid, init, goal)
    mostra_caminho(value_grid, init, goal)


if __name__=='__main__':
    grid = [[0,0,1,0,0,0, 0,0,0,0,0,1,0,0,0, 0,0,0],   #1 = ocupado; 0=livre
       [0,0,1,0,1,0, 0,0,0,0,0,1,0,0,0, 0,0,0],
       [0,0,0,0,0,1, 1, 0, 0,0,0,1,0,0,0, 0,0,0],
       [0,0,1,0,1,0, 1,0,1,0,0,0,0,0,0, 0,0,0],
       [0,1,0,0,1,0, 1,1,0,0,0,0,0,0,0, 1,1,0],
       [0,0,0,0,0,0, 1, 1, 0,0,0,0,0,0,1, 1,0,0],
       [0,0,1,0,1,0, 0,0,0,0,0,1,0,0,0, 0,0,0],
       [1,0,0,0,1,0, 0,0,0,0,0,1,0,0,0, 0,0,0],
       [0,0,0,0,1,0, 1,1,0,0,0,1,0,0,0, 0,0,0],
       [0,0,0,0,0,0, 1, 1, 0,0,0,1,0,0,0, 0,0,0],
       [0,0,1,0,1,0, 0,0,0,0,0,1,0,0,0, 0,0,0],
       [0,0,0,0,1,0, 0,0,0,0,0,1,0,0,0, 0,0,0]]

init = [0,0] #starting position
goal = [len(grid)-1, len(grid[0])-1] #goal position

value_grid = search(grid, init, goal)   #mostra matriz do mínimo de movimentos (ao custo de 1 o movimento) para chegar até ali

mostra_caminho(value_grid, init, goal)  #mostra um caminho ótimo a partir da matriz de mínimo de movimentos

#alternativamente, pode fazer tudo junto usando:

#busca_caminho(grid, init, goal)

Para entender melhor como a contagem dos movimentos do grid funciona, vamos fazer um grid bem simples:

grid = [[0,0,1,0,1],
       [0,0,1,1,0],
       [1,0,0,0,0],
       [0,1,0,0,0]]

init = [0,0] #starting position
goal = [len(grid)-1, len(grid[0])-1] #goal position

value_grid = search(grid, init, goal)
print(value_grid)

cujo resultado é:

[[0, 1, 0, 0, 0],
[1, 2, 0, 0, 7],
[0, 3, 4, 5, 6],
[0, 0, 5, 6, 7]]

(as posições que ficaram com zero é porque não foram alcançadas, ou era a inicial)

Certamente existem muitos métodos mais eficientes de se calcular essas trajetórias :-)

Answer 2

Uma maneira mais eficiente de se resolver esse problema é usando o algoritmo A*. Neste caso, em vez de guardarmos apenas g, a quantidade mínimas de movimentos para se chegar até aquele ponto, guardamos também h, uma heurística, que nesse caso pode ser a distância mínima sem obstáculos até o objetivo. Note que, da maneira que o grid foi construído, a distãncia mínima sem obstáculos é (delta x)+(delta y) porque aqui a diagonal não traz vantagens.

Neste caso não vamos fazer uma busca em largura, mas em profundidade. Isso porque vamos ir explorando os nós promissores (com a menor soma h+g), até que algum nó anterior seja mais ou igualmente promissor (quando então volta por uma busca em largura).

Comparando-se os resultados, nota-se que com o algortmo A* muito menos nós precisam ser explorados para chegar na resposta final. Nota-se também que a diferença no número de nós explorados, ou seja, a vantagem do A*, é dependente da topologia do grid.

import numpy as np
import matplotlib.pyplot as plt


delta = [[-1,0], #para cima
         [0,-1], #direita
        [1,0], #abaixo
        [0,1]] #esquera
cost = 1 


def h(a,b):
    h = abs(a[0]-b[0])+abs(a[1]-b[1])
    return h

def search_astar(grid, init, goal):

    closed = [[0 for row in range(len(grid[0]))] for col in range(len(grid))]
    value_grid = [[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])):
            if grid[i][j] == 1:
                value_grid[i][j] = -1

    closed[init[0]][init[1]]=1
    x = init[0]
    y = init[1]
    g=0
    hh = h(init, goal)
    abertos = [[hh, g,x,y]]

    found = False
    resign = False

    abertos_maximo = (len(grid)+1)*(len(grid[0])+1)-1        

    while not found and not resign:
        if len(abertos)==abertos_maximo:
            resign = True
            return 'fail'
        else:
            abertos.sort()
            abertos.reverse()
           # print('abertos', abertos)
            proximo = abertos.pop()
            #print('proximo', proximo)
            x = proximo[2]
            y = proximo[3]
            g = proximo[1]

            if x == goal[0] and y == goal[1]:
                found = True
            else:
                for i in range(len(delta)):
                    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 + h(goal, [x2, y2])
                            value_grid[x2][y2]=g2
                            abertos.append([f2, g2, x2, y2])
                            closed[x2][y2] =1
    return value_grid






def search(grid, init, goal):
    closed = [[0 for row in range(len(grid[0]))] for col in range(len(grid))]
    value_grid = [[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])):
            if grid[i][j] == 1:
                value_grid[i][j] = -1



    closed[init[0]][init[1]]=1
    x = init[0]
    y = init[1]
    g=0
    abertos = [[g,x,y]]

    found = False
    resign = False

    abertos_maximo = (len(grid)+1)*(len(grid[0])+1)-1        

    while not found and not resign:
        if len(abertos)==abertos_maximo:
            resign = True
            return 'fail'
        else:
            abertos.sort()
            abertos.reverse()
            proximo = abertos.pop()
            x = proximo[1]
            y = proximo[2]
            g = proximo[0]

            if x == goal[0] and y == goal[1]:
                found = True
            else:
                for i in range(len(delta)):
                    x2 = x + delta[i][0]
                    y2 = y + delta[i][2]
                    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
                            value_grid[x2][y2]=g2
                            abertos.append([g2, x2, y2])
                            closed[x2][y2] =1
    return value_grid


def mostra_caminho(value_grid, init, goal):
    label_obstatulos = 0
    label_abertos = 0
    for i in range(len(value_grid)): #linhas
        for j in range(len(value_grid[0])): #colunas
            if i==init[0] and j==init[1]:
                plt.plot([j], [i], marker='s', markersize=8, color="blue", label = 'Inicial')
            elif i==goal[0] and j == goal[1]:
                maximo = value_grid[i][j]
                plt.plot([j], [i], marker='*', markersize=12, color="yellow", label = 'Goal')
            elif value_grid[i][j]==-1:
                if label_obstatulos == 0:
                    plt.plot([j], [i], marker='^', markersize=5, color="red", label = 'Obstaculos')
                    label_obstatulos =1
                else:
                    plt.plot([j], [i], marker='^', markersize=5, color="red")
            elif value_grid[i][j] != 0: #and i != init[0] and j != init[1]:
                if label_abertos == 0:
                    plt.plot([j], [i], marker='o', markersize=3, color="gray", label = 'nós abertos')
                    label_abertos =1
                else:
                    plt.plot([j], [i], marker='o', markersize=3, color="gray")

    #tenho o maximo e tenho o goal
    caminho = []
    caminho.append(goal)
    label = 0
    for k in range(0,maximo):
        achei = 0
        proximo = caminho.pop()
        #print('proximo é', proximo)
        x = proximo[0]
        y = proximo[1]
        for i in range(len(delta)):
            x2 = x + delta[i][0]
            y2 = y + delta[i][3]
            if x2>=0 and x2<len(value_grid) and y2>=0 and y2<len(value_grid[0]):
                if value_grid[x2][y2] == maximo-1-k and achei == 0:

                    achei = 1
                    if label == 0:
                        plt.plot([y2], [x2], marker='o', markersize=7, color="green", label = 'Caminho ótimo')
                        label = 1
                    else:
                        plt.plot([y2], [x2], marker='o', markersize=7, color="green")
                    pos = [x2,y2]
                    caminho.append(pos)


    plt.grid()
    plt.legend(bbox_to_anchor=(1.05, 1), loc=2, borderaxespad=0.)
    #plt.legend()
    plt.show()

No código acima temos 3 funções:

valuegrid = search(grid, init, goal) que calcula o grid dos g pelo algoritmo anterior
valuegrid = searchastar(grid, init, goal) que calcula o grid dos g pelo algoritmo A*
mostracaminho(value_grid, init, goal) que mostra o caminho, os nós abertos num grafico

Comparando resultados:

(1) grid1 algoritmo busca exaustiva

(2) grid1 algoritmo A*

(3) grid2 algoritmo busca exaustiva

(4) grid2 algoritmo A*

Outra coisa que se nota é que o algoritmo