Real-time tightly coupled GNSS and IMU integration via Factor Graph Optimization

Este trabalho apresenta um método de otimização de grafos de fatores (FGO) para integração GNSS-IMU em tempo real e acoplamento rigoroso, que utiliza marginalização com atraso fixo para permitir estimativa causal de estados e demonstrar alta robustez em ambientes urbanos degradados.

O essencial

Imagine que você está dirigindo um carro em uma cidade muito cheia de arranha-céus, como Hong Kong ou Nova York. Você tenta usar o GPS do seu celular para saber onde está. De repente, os prédios altos bloqueiam o sinal dos satélites, ou o sinal reflete nas janelas de vidro e chega "confuso" ao seu aparelho. É como tentar ouvir uma conversa em um estádio lotado: o som (o sinal) chega distorcido ou some completamente.

Neste cenário, o GPS sozinho falha. É aí que entra a tecnologia descrita neste artigo.

O Problema: O GPS "cega" na cidade

O GPS é ótimo em campo aberto, mas nas cidades densas ele sofre muito. Ele perde o sinal (bloqueio) ou recebe ecos falsos (multicaminho). Quando isso acontece, o carro ou o pedestre pode "se perder" rapidamente.

A Solução Clássica: GPS + Um "Giroscópio" (IMU)

Para resolver isso, os engenheiros usam um truque: combinam o GPS com um sensor chamado IMU (Unidade de Medição Inercial). Pense no IMU como um passageiro cego, mas muito atento, que está no carro.

• Ele não vê nada lá fora.
• Mas ele sente cada aceleração, cada freio e cada curva que o carro faz.
• Se o GPS falhar por 10 segundos, o IMU consegue estimar onde o carro está baseado no movimento que ele sentiu.

O problema é que o IMU é imperfeito. Com o tempo, ele acumula erros (como um relógio que atrasa um pouco a cada hora). Se você confiar só nele, depois de alguns minutos, ele vai dizer que você está em outro continente.

A Técnica Antiga vs. A Nova Técnica (FGO)

Até agora, a maioria dos sistemas usava uma abordagem chamada "Filtro de Kalman". Imagine que é como um jogador de xadrez que só pensa na próxima jogada. Ele olha o que aconteceu agora, ajusta a posição e segue em frente. Se ele erra, o erro fica lá e se acumula.

Os autores deste artigo propõem algo mais inteligente: Otimização por Gráficos de Fatores (FGO).
Pense no FGO não como um jogador que pensa uma jogada de cada vez, mas como um detetive que revisa todo o caso.

• Em vez de apenas olhar o "agora", o detetive olha para o passado recente (os últimos segundos).
• Ele pega todas as pistas (dados do GPS e do IMU) e pergunta: "Se eu ajustar um pouco a posição de 5 segundos atrás, isso faz todo o resto do caminho fazer mais sentido?"
• Isso permite corrigir erros passados à medida que novas informações chegam. É como se o detetive pudesse apagar e reescrever partes da história para torná-la perfeita.

O Desafio: Fazer esse "detetive" revisar o passado exige muito poder de computador e tempo. Geralmente, isso só era feito depois da viagem (offline), quando você tinha todo o tempo do mundo para calcular.

A Grande Inovação: Tempo Real e "Janela Deslizante"

O grande feito deste trabalho é fazer esse "detetive" funcionar em tempo real, enquanto você está dirigindo.

Como eles fizeram isso?

1. A Janela Deslizante (Fixed-Lag): Em vez de tentar lembrar de toda a viagem desde o início do mundo (o que seria lento demais), o sistema mantém apenas uma "janela" de tempo (por exemplo, os últimos 60 segundos).
2. Esquecer o Esquecível: Quando o tempo passa, o sistema "descarta" os dados muito antigos (marginalização), mas guarda o resumo do que eles ensinaram. É como limpar a mesa de um jogo de cartas: você joga fora as cartas que já foram usadas, mas lembra da estratégia que elas ajudaram a formar.
3. Tight Coupling (Acoplamento Apertado): Eles não apenas misturam os dados de forma simples. Eles misturam tudo de forma tão íntima que o sistema usa o IMU para ajudar o GPS a entender o que está acontecendo, e o GPS para corrigir o IMU, tudo ao mesmo tempo, sem precisar de sensores extras de direção.

O Resultado: Um GPS que não desiste

Os autores testaram isso em um cenário real de cidade densa (o conjunto de dados UrbanNav).

• O GPS sozinho: Perdeu o sinal com frequência e deu posições erradas.
• O sistema antigo (Loose Coupling): Funcionou melhor, mas ainda tinha falhas.
• O novo sistema (RTFGO-TC):
  • Disponibilidade: Conseguia fornecer uma posição válida em 80% do tempo, mesmo quando o GPS estava quase cego (o GPS sozinho só dava 40%).
  • Precisão: O carro sabia exatamente onde estava na horizontal (na rua), mesmo com os prédios atrapalhando.
  • Velocidade: Tudo isso acontecia em milissegundos, pronto para ser usado em carros autônomos ou aplicativos de navegação agora mesmo.

Resumo em uma Analogia Final

Imagine que você está em um labirinto escuro (a cidade com prédios altos).

• O GPS é uma lanterna que às vezes apaga ou brilha em lugares errados.
• O IMU é um mapa mental que você desenha enquanto anda, mas que pode ficar torto com o tempo.
• O sistema antigo era como andar e tentar corrigir o mapa apenas no momento em que a lanterna piscava.
• Este novo sistema é como ter um guia experiente que, a cada passo, olha para trás, ajusta o mapa mental com base no que a lanterna mostrou nos últimos 60 segundos, e garante que você continue andando na direção certa, mesmo que a lanterna apague por um tempo.

Em suma, o papel apresenta um método inteligente e rápido para que nossos carros e celulares continuem sabendo exatamente onde estão, mesmo nas cidades mais confusas e com mais prédios do mundo.

Resumo técnico

Aqui está um resumo técnico detalhado do artigo "Real-time tightly coupled GNSS and IMU integration via Factor Graph Optimization", apresentado em português:

1. O Problema

O posicionamento confiável em ambientes urbanos densos continua sendo um desafio significativo para aplicações veiculares e de pedestres. A tecnologia de navegação baseada em GNSS (Sistema Global de Navegação por Satélite) sofre degradação severa ou torna-se indisponível nessas condições devido a:

• Bloqueio frequente de sinais: Edifícios altos impedem a linha de visada direta (NLOS - Non-Line-of-Sight).
• Multipath: Reflexões de sinais que causam erros de medição.
• Geometria de satélite variável: A disponibilidade de satélites muda rapidamente.

Embora as Unidades de Medição Inercial (IMU) ofereçam estimativas de movimento de alta taxa e sejam imunes a interferências ambientais, elas sofrem de acumulação de erro (deriva) ao longo do tempo. A fusão de GNSS e IMU é essencial, mas a maioria dos sistemas atuais utiliza Filtros de Kalman (KF), que podem limitar a precisão devido à natureza não linear dos modelos e à falta de otimização multi-passos. Além disso, as abordagens existentes baseadas em Otimização de Grafos de Fatores (FGO) são predominantemente offline (suavização de todo o trajeto), o que as torna inadequadas para sistemas de navegação em tempo real que exigem baixa latência e estimativas causais (usando apenas dados passados e presentes).

2. Metodologia

O artigo propõe o RTFGO-TC, um método de integração GNSS-IMU tightly coupled (acoplamento estrito) baseado em Otimização de Grafos de Fatores, projetado especificamente para operação em tempo real.

• Arquitetura de Acoplamento Estrito: Diferente do acoplamento solto (que funde posições e velocidades já calculadas), o método proposto funde as medições brutas (pseudo-distâncias e taxas de pseudo-distância/Doppler) de todos os satélites visíveis diretamente com as medições da IMU em um único problema de otimização.
• Estimativa de Estado: O vetor de estado inclui posição, velocidade, atitude (matriz de rotação), vieses do acelerômetro e giroscópio, além dos vieses e deriva dos relógios do receptor GNSS para cada constelação.
• Otimização Incremental com Marginalização de Lag Fixo:
  • Para garantir a operação em tempo real, o sistema utiliza o solver incremental iSAM2.
  • Em vez de manter todo o histórico de estados (o que causaria crescimento infinito da complexidade computacional), o método emprega suavização de lag fixo. Estados mais antigos que um determinado intervalo de tempo são marginalizados (removidos do grafo), mas sua informação é preservada através de um prior condensado.
  • Isso permite estimativas causais com complexidade computacional limitada.
• Fatores do Grafo:
  • Fatores IMU: Usam pré-integração IMU para restringir o movimento relativo entre estados.
  • Fatores GNSS: Incluem pseudo-distâncias (para posição e viés do relógio) e efeitos Doppler (para velocidade e deriva do relógio).
  • Propagação de Passeio Aleatório: Modela a evolução lenta dos vieses da IMU e do relógio.
• Observabilidade de Atitude: O método não depende de sensores externos de atitude (como bússolas ou câmeras). A atitude e os vieses inerciais tornam-se observáveis indiretamente através do acoplamento dinâmico com a velocidade e a posição, exigindo trajetórias com dinâmica suficiente (curvas, acelerações) para convergência.

3. Principais Contribuições

1. Primeira Implementação em Tempo Real de FGO Tightly Coupled: O trabalho preenche a lacuna entre a alta precisão da otimização por grafos e a necessidade de baixa latência, permitindo estimativas causais sem sacrificar completamente a robustez da suavização.
2. Integração de Dados Brutos sem Sensores Adicionais: O sistema estima atitude e vieses puramente através da fusão de GNSS e IMU, sem necessidade de restrições de atitude externas.
3. Análise de Compromisso (Trade-off): O estudo quantifica o equilíbrio entre o tamanho da janela de marginalização (lag), a precisão da estimativa e o custo computacional.
4. Código Aberto: O código-fonte da implementação em Python (baseado na biblioteca GTSAM) foi disponibilizado publicamente para facilitar a pesquisa futura.

4. Resultados Experimentais

A avaliação foi realizada utilizando o conjunto de dados UrbanNav-HK-MediumUrban-1, que simula um ambiente urbano denso em Hong Kong com severas condições de degradação de sinal (disponibilidade de GNSS entre 38% e 42%).

• Disponibilidade de Serviço: O método RTFGO-TC demonstrou uma disponibilidade de serviço significativamente superior (cerca de 80% para erro de 10m) em comparação com soluções apenas GNSS ou acoplamento solto (cerca de 40%). O sistema permanece operacional mesmo durante períodos de bloqueio de satélite onde outros métodos falham.
• Precisão de Posicionamento (2D vs 3D):
  • Horizontal (2D): O RTFGO-TC superou consistentemente as soluções GNSS-only e acoplamento solto, reduzindo o RMSE (Raiz do Erro Quadrático Médio) de 8.8m (GNSS-only) para **6.4m** no Loop 1.
  • Vertical (3D): Houve uma degradação na precisão 3D em alguns casos (devido à fraca observabilidade vertical em ambientes urbanos e à falta de restrições de altura explícitas), resultando em maior deriva de altitude. No entanto, o erro horizontal, crítico para navegação veicular, foi otimizado.
• Desempenho Computacional:
  • A abordagem em tempo real (RTFGO-TC) exige mais poder de processamento (59 ms para uma janela de 60s) em comparação com a versão acoplada solta (6 ms), mas é viável para processamento em tempo real em hardware moderno (Apple M3).
  • Aumentar o lag de marginalização trouxe ganhos de precisão marginais, mas aumentou linearmente o tempo de computação.

5. Significado e Conclusão

O artigo demonstra que é possível implementar Otimização de Grafos de Fatores em tempo real para navegação GNSS-IMU acoplada estritamente. O RTFGO-TC oferece uma alternativa robusta aos filtros de Kalman, especialmente em ambientes urbanos desafiadores onde a geometria dos satélites é pobre e os sinais são intermitentes.

A principal vantagem é a capacidade de manter um serviço de navegação contínuo e preciso (especialmente no plano horizontal) durante bloqueios de sinal, explorando a redundância temporal e a modelagem não linear superior do FGO, enquanto mantém a latência baixa necessária para aplicações autônomas. Trabalhos futuros visam integrar medições de fase de portadora (PPP/RTK) e restrições de movimento específicas de veículos/pedestres para melhorar ainda mais a precisão vertical e a robustez.