Navegação por pontos de passagem GPS ao ar livre

Graus são a unidade errada para um robô

Um módulo GPS fornece latitude e longitude em graus. Todo controlador que você tenha espera metros. A conversão é a primeira tarefa e é mais fácil do que a Internet sugere.

Ao longo de alguns centenas de metros percorridos por um rover de hobby, a Terra é plana o suficiente para que uma aproximação equirretangular seja exata muito além do próprio ruído do receptor:

north (m) = (lat − lat₀) × 111 320
east  (m) = (lon − lon₀) × 111 320 × cos(lat₀)

Escolha uma origem lat₀, lon₀ uma vez — o início da missão — e cada ponto de passagem se tornará um par simples (east, north), em metros, que o pure pursuit poderá seguir sem saber o que é um grau.

A fórmula de haversine é mais correta, mas você não precisa dela. Ela importa em centenas de quilômetros; a 500 m, as duas concordam em poucos milímetros, três ordens de grandeza abaixo do seu erro.

O número que vale lembrar: um grau de latitude equivale a 111,3 km; portanto, cinco casas decimais equivalem a 1,1 m e seis equivalem a 11 cm. Armazene os pontos de passagem com seis casas. Qualquer precisão além disso é apenas ruído, e um float em um AVR carrega cerca de sete algarismos significativos — suficiente para 19.076123 e insuficiente para qualquer valor mais preciso. Por isso, converta para metros relativos a uma origem em vez de fazer aritmética com graus brutos.

Três problemas de uma posição do GPS

Ela chega atrasada. Um NEO-6M típico informa a 1 Hz. A 1,5 m/s, o robô percorre 1,5 m entre posições, e a posição recebida descreve onde ele estava quando a solução foi calculada, não onde está agora.

Ela contém ruído. Um GPS de consumo chega a cerca de 2,5 m da posição verdadeira metade das vezes, ao ar livre e com o céu limpo. Sob árvores, ao lado de um prédio ou com uma geometria de satélites ruim, o resultado é muito pior — e a falha não é uma perda de sinal que você possa detectar, mas uma resposta errada porém plausível.

Ela não tem orientação. Uma posição do GPS é apenas uma posição. Ela não informa para onde o robô está apontando, porque um robô parado apontando para o norte e o mesmo robô apontando para o sul produzem posições idênticas. O rumo sobre o solo é derivado de posições consecutivas e só tem significado enquanto o robô está realmente se movendo — abaixo de cerca de 1 m/s, ele é principalmente ruído.

O que isso provoca em um seguidor de caminho

Dê ao mesmo controlador de pure pursuit o mesmo rumo duas vezes: uma com odometria perfeita e outra com uma posição de 1 Hz, com erro de ±1,5 m, mantida entre as atualizações:

Observe o que não está acontecendo: o robô nunca se perde. Ele permanece aproximadamente sobre o rumo e chega ao fim. A navegação somente com GPS funciona — é apenas muito pior do que parece na folha de especificações, e o custo recai principalmente sobre o atuador.

As três soluções, em ordem de valor

1 — Combine, não siga diretamente. Integre a odometria das rodas e uma fonte de orientação na frequência do seu laço e use cada posição do GPS para corrigir essa estimativa, não para substituí-la. Mesmo uma combinação complementar rudimentar transforma o resultado:

// Between fixes: dead reckon. This runs at the loop rate.
x += cos(heading) * v * dt;
y += sin(heading) * v * dt;

// On a fix: pull the estimate toward it, gently. alpha near 0.1 trusts odometry
// over the second between fixes, which is exactly the right instinct.
if (newFix && fix.hdop < 2.5) {
  x += ALPHA * (fixX - x);
  y += ALPHA * (fixY - y);
}

Esse é um filtro complementar fazendo o mesmo trabalho que faz com a inclinação: um sinal que deriva, mas é suave, corrigido por outro que contém ruído, mas tem valores limitados.

2 — Obtenha a orientação de outra fonte. Um magnetômetro fornece orientação absoluta mesmo com o robô parado e exige calibração de ferro duro e mole, além de distância dos motores. Um giroscópio fornece uma ótima orientação de curto prazo, mas deriva. Juntos, eles formam o par usado por todo rover real. Quando o movimento é rápido o suficiente para ser confiável, o rumo sobre o solo do GPS corrige esse par.

3 — Aumente a taxa antes de aumentar a precisão. Um módulo de 10 Hz corrige o atraso que causa a maior parte da serpentização. Normalmente, essa é uma melhoria inicial melhor do que perseguir precisão em centímetros, e ela é muito mais barata.

Lendo o módulo

Módulos GPS falam NMEA 0183 por uma porta serial a 9600 baud: frases separadas por vírgulas, uma por linha. Você precisa de duas delas.

• $GPRMC — hora, validade, latitude, longitude, velocidade sobre o solo e rumo sobre o solo.
• $GPGGA — qualidade da solução, satélites usados e HDOP, a diluição horizontal de precisão.

Analise a posição, mas faça uma verificação pela qualidade. Duas regras evitam a maioria dos acidentes ao ar livre:

• Rejeite uma solução com status V (void) ou qualidade de solução 0. Ela ainda conterá números com aparência plausível.
• Rejeite um HDOP acima de cerca de 2,5. O HDOP descreve a geometria dos satélites, e uma geometria ruim produz uma resposta confiante que pode estar dezenas de metros fora.

Use uma biblioteca de análise em vez de escrever a sua — mas leia frases suficientes para saber o que está descartando. Além disso: uma partida a frio leva de 30 segundos a vários minutos sem almanaque. Não trate um robô que ainda não tem uma posição válida como um robô que está na origem. Aguarde e indique visualmente que ainda está esperando.

Defina uma cerca geográfica antes de testar

Um robô que opera em ambientes externos e se comporta mal sai da área. Antes do primeiro teste de campo, adicione um limite rígido em software: uma distância máxima do ponto inicial e uma parada se ela for excedida.

if (hypotf(x, y) > FENCE_RADIUS) { stop(); return; }   // before anything else

Dez linhas, e isso é a diferença entre um bug e um robô em um canal. Combine com um tempo-limite de missão e um corte manual acessível — uma chave de impacto no painel superior funciona e não precisa de conexão de rádio.

Quando dá errado

Percorra o mesmo rumo com as duas fontes de posição no simulador de seguimento de caminho e depois construa o rover de pontos de passagem GPS, no qual o módulo em si e a combinação acima constituem o projeto inteiro.