Resumo Técnico: FusionCore

Declaração do Problema

A estimativa precisa de pose é fundamental para a navegação de robôs autônomos, no entanto, a implantação prática de sistemas de fusão de sensores permanece desafiadora. As soluções existentes enfrentam várias limitações críticas:

• Deriva de Viés: Os vieses dos giroscópios e acelerômetros do IMU derivam continuamente, e os vieses de taxa de guinada dos codificadores de roda (desvios sistemáticos em tração diferencial) raramente são tratados como estados do filtro, levando ao acúmulo de erro de rumo não observável durante interrupções do GPS.
• Fragilidade Arquitetural: Pacotes ROS dominantes como robot_localization dependem de nós separados para transformação de coordenadas GPS (UTM), introduzindo falhas nas fronteiras de zonas. Eles também utilizam covariâncias de ruído fixas e limiares de rejeição de outliers escalares que não levam em conta a dimensionalidade da medição.
• Lacunas Operacionais: Os sistemas atuais lutam com operação sem GPS, recuperação de reinicialização de mapa e a integração de sensores diversos como SLAM Visual (VSLAM) e radar sem coordenação complexa.

Metodologia

O FusionCore é um pacote ROS 2 de código aberto que implementa um Filtro de Kalman Unscented (UKF) de 23 estados que funde IMU, odometria de codificador de roda, GPS e pose do SLAM Visual em um único fluxo de odometria de 100 Hz.

Arquitetura Central

• Vetor de Estado: O filtro mantém um vetor de estado de 23 dimensões incluindo posição ($p$), orientação ($q$), velocidade linear/angulada ($v, \omega$), aceleração linear ($a$), viés do giroscópio ($b_g$), viés do acelerômetro ($b_a$) e um viés crítico de taxa de guinada do codificador de roda ($b_{ewz}$).
• Estimação de Viés: Diferentemente de abordagens anteriores, os vieses do giroscópio, acelerômetro e codificador de roda são estados de primeira classe do filtro com ruído de processo dedicado. Isso permite que o filtro converja para valores de viés específicos do sensor online, sem uma fase de calibração separada.
• Fusão de GPS: O sistema funde dados de GPS nativamente no referencial Earth-Centered, Earth-Fixed (ECEF), convertendo para ENU apenas para o estado do filtro. Isso elimina a necessidade de um nó navsat_transform separado e evita falhas nas fronteiras de zonas UTM.
• Modelos de Medição:
  • IMU: Lida com dados brutos de 6-DOF e atualizações de orientação de 2/3-DOF, considerando gravidade e viés.
  • Codificadores de Roda: Subtrai o $b_{ewz}$ estimado da velocidade angular reportada antes da integração. Impõe restrições não holonômicas (velocidade e aceleração verticais zero) com ruído adaptativo.
  • VSLAM: Aceita medições de pose de 6-DOF de qualquer sistema de odometria visual/SLAM, incluindo recuperação automática de reinicialização de mapa ao reancorar a transformação após rejeições consecutivas de outliers.
  • Velocidade: Suporta velocidade Doppler do GPS e velocidade própria Doppler do Radar para detecção de deslizamento.

Inovações no Design do Filtro

1. Portaria Qui-Quadrado por Sensor: Implementa rejeição de outliers baseada na distância de Mahalanobis com limiares calibrados especificamente para os graus de liberdade (DOF) de cada caminho de sensor (por exemplo, GPS de 3-DOF vs. rumo de 1-DOF), em vez de usar um único limiar escalar.
2. Covariância de Ruído Adaptativa: Ajusta automaticamente a covariância de ruído do sensor ($R$) em tempo real usando uma janela deslizante de sequências de inovação e uma média móvel exponencial, eliminando a necessidade de reajuste manual devido a mudanças ambientais.
3. Compensação de Atraso: Utiliza um buffer circular de IMU de 1 segundo para realizar retrodicação, aplicando medições atrasadas de GPS ou VSLAM ao estado histórico correto antes de repropagar para frente.
4. Atualizações de Velocidade Zero (ZUPT): Detecta períodos de estacionariedade para suprimir a deriva de viés.
5. Modo de Costura: Durante apagões de GPS, o viés de guinada do codificador estimado é subtraído dos dados do codificador para reduzir a deriva de rumo.

Contribuições Principais

O artigo apresenta dez contribuições específicas:

1. Uma formulação de UKF de 23 estados com estimação online contínua de vieses de giroscópio, acelerômetro e taxa de guinada do codificador de roda.
2. Fusão de GPS nativa em ECEF, removendo dependências de projeção UTM.
3. Portaria qui-quadrado de Mahalanobis por sensor com limiares calibrados por dimensionalidade.
4. Adaptação automática de covariância de ruído a partir de sequências de inovação.
5. Atualizações automáticas de Velocidade Zero (ZUPT) para períodos estacionários.
6. Retrodicação com buffer circular de IMU para compensação de atraso.
7. Fusão de pose do SLAM Visual com recuperação automática de reinicialização de mapa.
8. Fusão de velocidade do GPS para detecção de deslizamento entre sensores.
9. Fusão de velocidade própria Doppler do Radar para plataformas de radar de imagem 4D.
10. Uma avaliação quantitativa contra o robot_localization em doze sequências completas do NCLT.

Avaliação e Resultados

O sistema foi avaliado em doze sequências completas (55–92 minutos cada) do conjunto de dados público NCLT, cobrindo diversas condições sazonais. A avaliação comparou o FusionCore com o EKF e UKF do robot_localização usando o Erro de Trajetória Absoluta (ATE) RMSE.

• Desempenho: O FusionCore alcançou um ATE menor que o EKF do robot_localization em 10 de 12 sequências. As melhorias nas sequências vencedoras variaram de 1,2× a 22,2×.
• Estabilidade: O UKF do robot_localization divergiu numericamente (NaN) em todas as doze sequências nos primeiros 30 segundos, confirmando problemas de instabilidade conhecidos com entradas de alta taxa.
• Análise de Falhas:
  • 2012-06-15 (Perda do FC): O FusionCore perdeu para o EKF do robot_localization (49,2m vs 18,2m) devido a um apagão de GPS de 7,7 minutos. O artigo atribui isso às limitações da navegação estimada baseada em codificadores por tais durações longas sem uma referência de rumo absoluta (por exemplo, magnetômetro).
  • 2012-08-20 (Perda do FC): O FusionCore perdeu significativamente (98,3m vs 10,6m) devido a um "cluster de GPS adversário" de 105 correções aparecendo a 720–840m da verdade fundamental na fronteira de um apagão. O artigo observa que a porta mal calibrada do robot_localization acabou rejeitando esses outliers, enquanto a porta relaxada do FusionCore (necessária para recuperação) os aceitou. Os autores identificam uma verificação prévia de consistência de velocidade como a solução.
• Incompatibilidade de Covariância: O artigo destaca que a dependência do robot_localization de relatórios de covariância de GPS fixos, muitas vezes imprecisos, leva à rejeição de dados válidos e a um colapso na navegação estimada. O modelo de ruído adaptativo do FusionCore mitiga isso inflando o modelo de ruído quando os erros de inovação excedem as covariâncias reportadas.

Significado e Alegações

O artigo afirma que o FusionCore aborda as limitações práticas das pilhas de fusão de sensores ROS 2 existentes ao fornecer um único nó de ciclo de vida que não requer coordenadores externos. Seu significado reside em:

• Robustez: Alcançar precisão de trajetória superior em cenários de longa duração e do mundo real onde implementações existentes de UKF falham numericamente.
• Adaptabilidade: Lidar com qualidade dinâmica de sensores e incompatibilidades de covariância de GPS por meio de ruído adaptativo e portaria consciente da dimensionalidade.
• Continuidade Operacional: Habilitar operação sem GPS e recuperação transparente de reinicialização de mapa do VSLAM.
• Reprodutibilidade: Fornecer uma implementação totalmente de código aberto com scripts de benchmark e configurações para plataformas padrão (TurtleBot3, Husky) e integração com rtabmap_ros.

Os autores concluem que, embora modos de falha específicos (apagões longos, clusters de GPS adversários) existam, eles são diagnosticáveis e identificaram correções arquitetônicas (integração de magnetômetro, verificações prévias de velocidade) para trabalhos futuros.