Planejamento de caminho A* em uma grade de ocupação

Um planejador de robô responde a uma pergunta diferente daquela feita por um controlador de desvio de obstáculos. O planejador pergunta: “Quais células livres conectam o início ao objetivo pelo menor custo?”. O controlador pergunta: “Qual movimento é seguro neste momento?”. A* é um algoritmo de busca em grafo que combina o custo já percorrido com uma estimativa do custo restante, tornando-se uma primeira escolha prática para planejar em uma grade de ocupação bidimensional.

A segurança da rota depende da qualidade da grade e do envelope do robô fornecidos ao planejador. Uma linha matematicamente mais curta, passando pelos centros das células livres, ainda pode raspar em uma parede se os obstáculos não forem inflados.

Transforme o mapa em um grafo

Uma grade de ocupação divide o espaço em células de mesmo tamanho. Cada célula pode estar livre, ocupada ou desconhecida. Para um planejador básico, cada célula atravessável é um nó do grafo conectado aos seus vizinhos.

Com movimento em quatro direções, os vizinhos são norte, leste, sul e oeste, cada um com custo 1. Com movimento em oito direções, adicione as diagonais com custo √2. Multiplicar depois pela resolução do mapa converte a distância da grade em metros.

Decida como tratar células desconhecidas. Um robô indoor cuidadoso pode considerá-las bloqueadas; um robô de exploração pode permitir a travessia com um custo extra. Torne essa escolha um parâmetro explícito da política, não uma comparação acidental com o valor numérico errado.

Entenda os três custos do A*

Para cada nó n, A* acompanha:

• g(n): custo conhecido do início até n;
• h(n): estimativa heurística de n até o objetivo;
• f(n) = g(n) + h(n): custo total estimado da rota que passa por n.

O conjunto aberto contém nós descobertos que podem ser expandidos. Em cada etapa, remova o nó com o menor f. Se for o objetivo, reconstrua a rota seguindo as ligações de pai até o início. Caso contrário, faça o relaxamento de cada vizinho válido: se o novo g for menor que o custo registrado, atualize seu custo e seu pai.

A heurística orienta a busca sem alterar o custo já pago. Com movimento em quatro direções e custo uniforme, a distância de Manhattan é adequada:

h = |goalX - x| + |goalY - y|

Para movimento em oito direções com custo diagonal √2, use a distância octil. A distância euclidiana também permanece menor ou igual ao custo real do caminho em espaço livre. Superestimar pode tornar a busca mais rápida, mas sacrifica a garantia de rota ótima.

Implemente a busca com uma fila de prioridade

A biblioteca padrão do Python é suficiente para uma implementação de referência compacta:

from heapq import heappop, heappush

def astar(start, goal, neighbors, heuristic):
    frontier = [(heuristic(start, goal), 0, start)]
    came_from = {start: None}
    cost = {start: 0}

    while frontier:
        _, current_cost, current = heappop(frontier)
        if current_cost != cost.get(current):
            continue  # stale queue entry
        if current == goal:
            break

        for next_cell, step_cost in neighbors(current):
            new_cost = current_cost + step_cost
            if new_cost < cost.get(next_cell, float("inf")):
                cost[next_cell] = new_cost
                came_from[next_cell] = current
                heappush(frontier, (
                    new_cost + heuristic(next_cell, goal),
                    new_cost,
                    next_cell,
                ))

    if goal not in came_from:
        return None

    path, cell = [], goal
    while cell is not None:
        path.append(cell)
        cell = came_from[cell]
    return list(reversed(path))

Em vez de tentar alterar uma entrada já presente no heap, insira o valor melhorado e descarte as entradas obsoletas quando forem extraídas. Isso mantém a implementação pequena e correta. Em um controlador com memória limitada, uma grade de tamanho fixo e arrays indexados evitam mapas dinâmicos.

Evite cortar cantos na diagonal

Com movimento em oito direções, um vizinho diagonal deve ser rejeitado quando qualquer uma das células cardinais adjacentes estiver ocupada. Caso contrário, o planejador pode passar entre dois obstáculos que se tocam em um canto — um movimento impossível para um robô com largura diferente de zero.

Para um movimento de (x, y) para (x+1, y+1), verifique se (x+1, y) e (x, y+1) são atravessáveis. Depois aplique a inflação do envelope como uma camada de segurança separada.

Inflare obstáculos para o robô real

Planejar o centro do robô através de células ocupadas brutas trata o robô como um ponto. Inflare cada obstáculo pelo menos pelo raio inscrito do robô mais uma margem de segurança. Equivalentemente, expanda os obstáculos e planeje o centro do robô, tratado como um ponto, pelo espaço livre restante.

Um raio de inflação rígido marca como bloqueadas as células próximas. Um campo de custo gradual adiciona um custo maior perto das paredes, favorecendo o afastamento sem declarar impossíveis todas as células próximas. O raio escolhido deve incluir a saliência do corpo, a incerteza de localização e o erro de rastreamento.

A resolução do mapa altera o número de células: inflationCells = ceil(inflationMetres / resolution). Teste a conversão explicitamente. Um raio de 0,20 m corresponde a quatro células a 0,05 m/célula, não a 0,20 célula.

Adicione custos de travessia úteis

A* com custos uniformes retorna o caminho de grade mais curto. Robôs frequentemente precisam do caminho de menor custo. Adicione custo para células próximas a obstáculos, movimento em marcha à ré, curvas acentuadas ou terreno irregular. Mantenha todos os custos de aresta não negativos e, se a otimização for importante, mantenha a heurística não maior que o custo restante sob esse modelo.

Não adicione uma penalidade de curva sem incluir a orientação no estado. Se chegar à mesma célula orientada para leste ou para norte produzir custos futuros diferentes, (x, y) deixa de descrever o nó; use (x, y, heading) ou aceite que a penalidade seja apenas uma preferência de pós-processamento.

Valide o planejador sistematicamente

Comece com mapas minúsculos cuja resposta seja óbvia:

1. Grade vazia: o caminho deve se aproximar do ótimo em linha reta.
2. Uma parede com uma abertura: a rota deve passar pela abertura.
3. Objetivo bloqueado: retornar nenhum caminho sem entrar em laço.
4. Início igual ao objetivo: retornar a única célula inicial.
5. Obstáculos que se tocam diagonalmente: não cortar cantos.
6. Corredor estreito menor que o envelope inflado: nenhum caminho.

Para cada caminho retornado, verifique se começa e termina corretamente, se cada passo conecta células adjacentes, se nenhuma célula está bloqueada e se o custo total é igual à soma dos custos dos passos. Visualize as células exploradas, assim como a rota final; uma expansão excessiva costuma revelar uma heurística ruim ou unidades incompatíveis.

Diagnose falhas comuns do planejador

• A rota atravessa uma parede: a indexação da ocupação, a ordem linha/coluna ou a validação da diagonal está errada.
• A rota corta cantos: os obstáculos não foram inflados ou o corte de canto na diagonal é permitido.
• A retorna uma rota mais longa que o esperado:* a heurística ou as unidades do custo por passo estão incompatíveis, ou custos obsoletos são aceitos.
• A busca consome muita memória: a resolução da grade é desnecessariamente fina ou entradas duplicadas da fila nunca são descartadas.
• O robô não consegue seguir um caminho válido: o plano ignora o envelope, o raio de curva, os limites do controlador ou o erro de localização.
• O caminho pisca após cada atualização do sensor: o planejamento global está sendo reiniciado para pequenas mudanças sem histerese de custo ou política de replanejamento.

O Simulador de Labirintos demonstra um método relacionado de campo de distância para um pequeno labirinto desconhecido. O Simulador de Desvio de Obstáculos mostra por que um caminho global ainda precisa de um controlador local de segurança. E, quando a tarefa é visitar todos os lugares em vez de alcançar um objetivo, nada disso se aplica — o planejamento de caminhos de cobertura não tem célula de objetivo nem heurística, e é avaliado em distância por metro quadrado. Em ambos os casos, ainda é preciso percorrer o plano, o que é um problema separado, com seu próprio controlador — veja pure pursuit.