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