Pure pursuit: seguimento de trajetória com um único parâmetro

A etapa que quase ninguém ensina

Um planejador fornece uma trajetória. O A* fornece uma lista de células da grade; um levantamento fornece uma lista de pontos de passagem; uma missão fornece uma linha em um mapa. Nenhum deles diz o que fazer com as rodas.

É nessa etapa que entra o Pure pursuit, que cabe em um parágrafo de geometria:

Encontre o ponto da trajetória que está a uma distância fixa à sua frente. Percorra o arco circular que passa por ele. Repita.

Não há sinal de erro, termo integral nem tabela de ganhos. Há um único número — a distância de antecipação — e ele decide tudo.

A geometria

Coloque o ponto-alvo no referencial próprio do robô: x para a frente, y para a esquerda, à distância L_d. Um arco circular que passa pela posição atual do robô e por esse ponto tem curvatura

κ = 2y / L_d²

Vale a pena ler essa expressão duas vezes, porque tudo decorre dela. O deslocamento lateral y está no numerador, portanto o arco se curva mais à medida que o ponto-alvo se afasta do eixo. A distância de antecipação está ao quadrado no denominador, portanto dobrá-la reduz a curvatura a um quarto.

Ao converter a expressão para raio e para o ângulo de direção de um veículo com entre-eixos b:

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

Para um acionamento diferencial não existe ângulo de direção — você comanda diretamente uma taxa angular, ω = v κ. A curvatura é a mesma; o atuador é outro.

O único parâmetro e por que ele entra em conflito consigo mesmo

Distância de antecipação curta. O ponto-alvo está perto, portanto um pequeno deslocamento lateral produz uma curvatura grande. O robô acompanha a trajetória com precisão — e reage violentamente a cada pequena oscilação, inclusive às provocadas pela própria correção de direção.

Distância de antecipação longa. O ponto-alvo está longe, a curvatura é suave e a direção é estável — mas, em um canto, o ponto-alvo já contornou a curva enquanto o robô ainda não, de modo que o robô corta o canto.

Essas duas condições apontam em direções opostas, e o algoritmo não contém nenhum mecanismo que as equilibre. Você escolhe um número.

A armadilha de toda dedução em manual

Escreva o Pure pursuit no papel para um robô que responde instantaneamente à direção e menor será sempre melhor. O erro diminui de forma monotônica à medida que a antecipação é reduzida. Não existe um ótimo, e a curva em forma de U acima também não existiria.

Robôs reais têm atraso. A direção leva tempo para alcançar o ângulo comandado; a pose usada pelo controlador já tem algumas dezenas de milissegundos quando as rodas respondem. O simulador modela uma constante de tempo de direção de 0,25 s e 0,15 s de atraso de sensores e computação — 0,4 s de atraso total, algo nada incomum para um rover hobby.

Esse atraso é a razão pela qual o ótimo existe. Uma antecipação curta comanda uma correção, a correção chega tarde, o robô já deixou o erro para trás e o próximo comando corrige demais na direção oposta. Um controlador mais rápido que seu próprio atuador oscila. Esse é o mesmo fato que governa o ganho derivativo do PID e também está por trás da distância de parada: a latência não é um detalhe; é o que determina o ajuste.

Como escolher a distância de antecipação

Os ótimos medidos e seus equivalentes em unidades que podem ser comparadas entre robôs:

Há duas conclusões claras dessa tabela.

A antecipação cresce com a velocidade, mas mais lentamente do que na mesma proporção. O conselho comum “use um número fixo de segundos de deslocamento” é próximo, mas está errado — o tempo cai de 0,90 s para 0,47 s nessa faixa.

A distância fica entre uma e duas vezes a distância percorrida durante o próprio atraso de controle. Essa é a regra útil, porque é mensurável: determine seu atraso, multiplique-o pela velocidade e comece por esse valor. Neste robô, o produto entre velocidade e atraso (v × lag) varia de 0,4 m a 1,2 m, e os ótimos ficam entre 0,9 m e 1,4 m.

Na prática, implemente uma pequena programação linear e limite seus valores:

float lookahead = constrain(K_LOOK * speed + L_MIN, L_MIN, L_MAX);

L_MIN importa mais do que K_LOOK. Com o robô parado, a expressão tenderia a zero, produzindo uma divisão por zero disfarçada.

Implementação

// Pure pursuit over a waypoint list. Returns the curvature to command.
float pursue(float x, float y, float heading, float lookahead) {
  // 1. Where am I on the path? Search only near the arc length I last reached —
  //    a global search latches onto whichever segment happens to be nearest, and a
  //    path that doubles back will teleport your progress to a corner you passed
  //    twenty seconds ago.
  float s = projectOntoPath(x, y, lastS);
  lastS = max(lastS, s);

  // 2. The goal point, one lookahead further along.
  Point goal = pointAtArcLength(lastS + lookahead);

  // 3. Into the robot frame, then straight into the curvature formula.
  float dx = goal.x - x, dy = goal.y - y;
  float lateral = -sin(heading) * dx + cos(heading) * dy;
  float ld = max(0.3f, hypotf(dx, dy));
  return 2.0f * lateral / (ld * ld);
}

Três coisas em doze linhas valem mais do que o restante:

• lastS é um estado, e só avança. Sem ele, qualquer trajetória que se cruza pode deixar o controlador perdido.
• Limite ld. Quando o ponto-alvo se aproxima, a curvatura tende ao infinito.
• O ponto-alvo está na trajetória, não no ponto de passagem mais próximo. Mirar pontos de passagem faz o robô zigue-zaguear entre eles; mirar um ponto no segmento é o que torna o movimento suave.

O que ele faz e o que não faz

Ele não consegue ficar parado e girar para ficar de frente à trajetória. O Pure pursuit não tem uma noção de imobilidade — a curvatura descreve uma forma, e uma forma exige movimento para a frente. Inicie o robô aproximadamente alinhado ou percorra primeiro um pequeno arco inicial.

Ele tem um raio mínimo de giro, e a trajetória pode não respeitá-lo. Se um canto for mais fechado que b / tan(δ_max), o robô não pode segui-lo, qualquer que seja o ajuste. Suavize a trajetória ou diminua a velocidade no canto — é para isso que existe um perfil de movimento.

Ele sempre corta cantos, por definição. Na execução medida a 2 m/s, percorreu 0,8% a menos do que a trajetória comandada. Se isso for inaceitável, você precisa de outro controlador — Stanley usa o erro transversal à trajetória e faz uma curva mais aberta, em vez de cortar.

Ele não corrige uma estimativa de posição ruim. Tudo acima pressupõe que o robô sabe onde está. Forneça ao controlador atualizações de posição GPS a 1 Hz e o erro RMS vai de 0,09 m para 1,25 m — veja navegação por pontos de passagem GPS.

Quando dá errado

Execute os dois controladores na mesma trajetória no simulador de seguimento de trajetória — arraste a antecipação e toda a execução será redesenhada. Depois construa o rover de pontos de passagem GPS, em que a estimativa de posição se torna a parte difícil.