Simulador de seguimento de caminho: pure pursuit e Stanley

Controles

Controlador alterna entre pure pursuit e Stanley. Distância de antecipação é o único ajuste do pure pursuit e fica desativada para Stanley, que não usa essa distância — isso já é o ponto.

Velocidade e a distância de antecipação interagem, e é isso que vale a pena experimentar: defina 1 m/s e encontre a melhor distância de antecipação; depois defina 3 m/s e encontre-a novamente.

Fonte de posição troca a odometria perfeita por uma correção de 1 Hz com ruído na escala de metros, mantida entre atualizações. O erro é sempre pontuado contra a posição verdadeira, portanto a penalidade é real, não oculta.

Arraste qualquer ponto de referência, ou pressione 1–8 para selecionar um e movê-lo com as setas. Toda a execução é recalculada enquanto você arrasta.

Teoria

Ambos os controladores respondem à mesma pergunta — dado onde estou e onde está o caminho, qual ângulo de direção? — mas respondem de formas completamente diferentes.

Pure pursuit é geometria. Encontre o ponto no caminho à frente, a uma distância de antecipação L, e conduza o arco circular que passa por ele:

κ = 2y / L_d²          δ = atan(2 b sin α / L_d)

com y sendo o deslocamento lateral do ponto-alvo no referencial do robô e b a distância entre eixos. Há um parâmetro, e ele é uma distância, não um ganho — portanto ele é transferível entre robôs.

Stanley é controle. Tome o eixo dianteiro como referência e some dois termos:

δ = θ_e + atan(k · e / v)

Erro de orientação mais uma arcotangente do erro transversal. Dividir pela velocidade é o que permite que um único ganho funcione em uma faixa de velocidades. k é um verdadeiro ganho de controle e não é transferível.

O laboratório acrescenta o que as derivações de livros didáticos deixam de fora: 0,4 s de atraso total, como uma resposta de esterço de primeira ordem de 0,25 s mais atraso de medição e computação de 0,15 s. Sem ele, pure pursuit melhora monotonicamente à medida que a distância de antecipação diminui e toda a questão do ajuste desaparece.

Algoritmo

A cada passo, para ambos os controladores:

1. Projete sobre o caminho, procurando uma janela ao redor do comprimento de arco alcançado mais recentemente, em vez de todo o caminho. Uma busca global escolhe o segmento mais próximo, portanto um trajeto que passa sobre si mesmo faz o progresso do robô saltar para um canto que ele passou há um minuto.
2. Resolva um empate em um vértice para o segmento de saída. Depois de uma esquina, os dois segmentos adjacentes se projetam no mesmo ponto à mesma distância; manter o segmento de entrada entrega ao controlador um erro de orientação e um deslocamento lateral próximos de zero enquanto ele segue em frente e sai do caminho, sem que o controlador perceba.
3. Calcule o ângulo de direção pela regra do próprio controlador e limite-o ao limite mecânico.
4. Aplique o atraso e integre o modelo de bicicleta.
5. Pontue contra a posição verdadeira, nunca contra a estimativa.

O passo 2 não é uma otimização. Foi a diferença entre Stanley funcionar e Stanley deixar o trajeto sem perceber o problema em exatamente 78% de todas as execuções.

Código-fonte

Pure pursuit, completo, na forma que assume em um robô real:

float lastS = 0;   // arc length last reached — state, and it only moves forward

float pursue(float x, float y, float heading, float speed) {
  // The lookahead is scheduled on speed, with a hard floor: at a standstill a
  // proportional schedule asks for zero, which is a division by zero in disguise.
  float lookahead = constrain(0.7f * speed + 0.4f, 0.4f, 3.0f);

  float s = projectOntoPath(x, y, lastS);      // windowed around lastS
  lastS = max(lastS, s);
  Point goal = pointAtArcLength(lastS + lookahead);

  float dx = goal.x - x, dy = goal.y - y;
  float lateral = -sinf(heading) * dx + cosf(heading) * dy;
  float ld = max(0.3f, hypotf(dx, dy));        // clamp, or curvature blows up

  float curvature = 2.0f * lateral / (ld * ld);
  return curvature;                            // omega = v * curvature
}

As três linhas que importam são o limite mínimo do agendamento, a monotonicidade de lastS e o limite de ld. Todo o resto é a fórmula.

Premissas e diferenças do hardware

O laboratório dá ao robô um mapa perfeito do caminho, uma distância entre eixos perfeitamente conhecida, nenhuma derrapagem nas rodas e um atraso modelado de forma limpa como um sistema de primeira ordem. Um robô real não tem nada disso.

Derrapagem das rodas torna a estimativa de posição errada de uma forma que nenhum controlador consegue detectar. Folga da caixa de engrenagens adiciona perda de curso a cada reversão do esterço, o que se comporta como atraso adicional e aumenta a distância de antecipação ótima. E um acionamento diferencial não tem ângulo de direção algum — tem uma taxa de rotação comandada, portanto ω = v κ substitui o atan e a distância entre eixos só importa para determinar até que ponto ele consegue virar.

O que sobrevive a tudo isso é a forma: um ótimo interno que cresce com a velocidade e um erro nesse ótimo que também cresce com a velocidade.

Diagrama do circuito

O controlador é a parte barata; o restante do robô, do qual ele precisa, é a parte cara.

Posição. Encoders em quadratura em ambas as rodas e odometria de acionamento diferencial integrando-os, na taxa do laço. Em ambientes internos isso basta para algumas dezenas de metros.

Orientação. Um giroscópio para o curto prazo, um magnetômetro para a referência absoluta, ou ambos. O erro de orientação é o termo que recebe mais peso nos dois controladores, e é o primeiro que a odometria perde.

Uma correção absoluta, ao ar livre. Um módulo GPS fundido à estimativa, em vez de segui-lo diretamente — a diferença é 0,09 m de erro RMS contra 1,25 m.

Para a teoria completa, consulte pure pursuit e erro transversal; para construir, o rover GPS com pontos de referência começa em ambientes internos com encoders e só sai para o ar livre depois que o controlador de seguimento funcionar.