PRN （20201012） ： Melhoria na atualização dos mapas de distância euclidianos e diagramas de Voronoi

[*] Lau B, Sprunk C, Burgard W. Atualização aprimorada de mapas de distância euclidianos e diagramas de Voronoi [C] // IEEE / RSJ International Conference on Intelligent Robots & Systems. IEEE, 2010.

Este artigo implementa o algoritmo de Mapa de Ocupação para Mapas de Distância Euclidiana em [*] em python.

Os algoritmos relevantes no artigo são os seguintes:

O algoritmo pode atualizar um mapa de distância europeu com base na adição ou remoção de obstáculos que ocupam o mapa de grade.

Entre eles, o artigo explica pop (abrir) e inserir (abrir, s, d) duas funções sem pseudocódigo. De acordo com o artigo, pop (abrir) é encontrar um elemento com a menor distância de aberto e devolvê-lo, e excluí-lo de aberto. No processo de implementação, ao retornar de acordo com a distância mínima, quando um obstáculo é removido de um mapa com vários obstáculos, o programa cairá em um loop infinito. Verificou-se que pode operar normalmente se retornar de acordo com a distância máxima.

O jornal também acrescentou que adicionar um atributo a cada grade: processo, para atingir o objetivo semelhante a adicionar e remover filas abertas.

Implementação de algoritmo python

import numpy as np
import matplotlib.pyplot as plt
%matplotlib inline
open_list = []

dist = 99999.0*np.ones((30, 30))
obst = -1*np.ones((30, 30))
toRaise = np.zeros((30, 30))
process = np.zeros((30, 30))

def S2IJ(s):
    i = int(s//30)
    j = int(s%30)
    return i,j

def SetObstacle(i,j):
    obst[i,j] = i*30+j
    dist[i,j] = 0
    process[i,j] = 1
#     open_list.append([i,j])
    
    
def RemoveObstacle(i,j):
    obst[i,j] = -1
    toRaise[i,j] = 1
    dist[i, j] = 99999.0
    process[i, j] = 1
#     open_list.append([i,j])
    
def Raise(i,j):
    for ni in [-1, 0, 1]:
        for nj in [-1, 0, 1]:
            if ni == 0 and nj == 0:
                continue
                
            xi = i + ni
            xj = j + nj
            if xi < 0 or xi >= 30 or xj < 0 or xj >= 30:
                continue
                
            if (obst[xi, xj] >= 0) and (toRaise[xi, xj] == 0):
                oi, oj = S2IJ(obst[xi,xj])  
                if (oi != xi) or (oj != xj):
                    dist[xi,xj] = 99999.0
#                     print(xi,xj,oi,oj,obst[xi, xj])
                    obst[xi, xj] = -1
                    toRaise[xi, xj] = 1
#                 else:
#                     for i in range(30):
#                         for j in range(30):
#                             toRaise[i,j] = 0
                process[xi, xj] = 1
#                 open_list.append([xi, xj])
    toRaise[i,j] = 0
    
def Lower(i, j):
    si, sj = S2IJ(obst[i, j])
    for ni in [-1, 0, 1]:
        for nj in [-1, 0, 1]:
            if ni == 0 and nj == 0:
                continue
                
            xi = i + ni
            xj = j + nj
            if xi < 0 or xi >= 30 or xj < 0 or xj >= 30:
                continue            
            if toRaise[xi, xj] == 0:
                d = np.sqrt((si-xi)*(si-xi)+(sj-xj)*(sj-xj))
                if (d < dist[xi, xj]) or (obst[xi, xj] < 0):
                    dist[xi, xj] = d
                    obst[xi, xj] = si*30+sj
                    process[xi, xj] = 1
#                     open_list.append([xi, xj])

def Pops(): # get the element with max distance
    max_index = -1
    max_dist = -1.0
    for i in range(30):
        for j in range(30):
            if process[i,j] == 1:
                if dist[i,j] > max_dist:
                    max_dist = dist[i,j]
                    max_index = i*30+j
    if max_index < 0:
        return False, max_index
    else:
        oi, oj = S2IJ(max_index)
        process[oi, oj] = 0
        return True, max_index
    

            
    
def UpdateDistanceMap():
    not_empty, s = Pops()
    while not_empty:
        i,j = S2IJ(s)
        if toRaise[i,j] == 1:
            Raise(i,j)   
#             print("raise")
#             print(i,j)
        else:
            if obst[i,j] >= 0:
                Lower(i,j)
#                 print("lower")
#                 print(i,j)
        not_empty, s = Pops()
        

Adicione um obstáculo do mapa em branco

SetObstacle(15, 15)
%time UpdateDistanceMap()
plt.imshow(dist)
plt.show()

Custo de
tempo: tempo de parede: 325 ms

Mapa de distância europeu:

Adicione um obstáculo

SetObstacle(5,5)
%time UpdateDistanceMap()
plt.imshow(dist)
plt.show()

Custo de
tempo: tempo de parede: 93,8 ms

Mapa de distância europeu:

Adicione outro obstáculo

SetObstacle(10, 25)
%time UpdateDistanceMap()
plt.imshow(dist)
plt.show()

Custo de
tempo: tempo de parede: 71,9 ms

Mapa de distância europeu:

Remova um obstáculo do mapa

RemoveObstacle(5,5)
%time UpdateDistanceMap()
plt.imshow(dist)
plt.show()

Custo de
tempo: tempo de parede: 854 ms

Mapa de distância europeu:

Adicionar novamente um novo obstáculo

SetObstacle(25,25)
%time UpdateDistanceMap()
plt.imshow(dist)
plt.show()

O tempo custa
tempo na parede: 58,6 ms

Mapa de distância europeu

suplemento

O mapa de distância euclidiana pode obter ainda o mapa de Voronoi (conforme o exemplo dado no artigo):

Observamos cuidadosamente o mapa de distância euclidiana gerado no programa acima e descobrimos que existem grades com limites semelhantes entre os obstáculos:

Portanto, após obter o mapa de distância euclidiana, você pode facilmente obter o mapa de Voronoi.

Em conclusão

O custo de tempo de geração de mapas de distâncias euclidianos é muito alto e dificilmente pode ser usado para gerar mapas de custos locais para direção não tripulada. Mas pode ser aplicado a cenas relativamente estáticas.