SE(3)-LIO: Smooth IMU Propagation With Jointly Distributed Poses on SE(3) Manifold for Accurate and Robust LiDAR-Inertial Odometry

Resumo Técnico: SE(3)-LIO

1. Problema Identificado

A estimativa precisa de odometria é crucial para a navegação autônoma. Embora a Unidade de Medição Inercial (IMU) seja amplamente utilizada devido à sua alta taxa de amostragem para prever movimento e compensar distorções, os métodos existentes apresentam limitações significativas em dois aspectos principais:

• Previsão de Movimento (IMU Propagation): Os métodos convencionais representam o pose 6-DoF (6 Graus de Liberdade) separando rotação e translação, propagando-os em variedades (manifolds) distintas (SO(3) e $\mathbb{R}^3$). Isso impede que a variação rotacional seja efetivamente incorporada à propagação da translação, levando a previsões de movimento imprecisas, especialmente sob movimentos agressivos ou dinâmicos.
• Compensação de Movimento (Motion Compensation): Para corrigir distorções induzidas pelo movimento em sensores como LiDAR, os pontos são transformados para um pose de referência comum usando a transformação relativa entre poses preditas. Como as poses preditas contêm erros inerentes (ruído do sensor e integração numérica), a transformação relativa possui incerteza. Métodos anteriores frequentemente tratam essas poses como independentes, ignorando a correlação entre elas, o que leva a uma superestimação da incerteza ou a uma caracterização imprecisa do ruído nas medições compensadas.

2. Metodologia Proposta

Os autores propõem o SE(3)-LIO, um framework de odometria LiDAR-Inercial (LIO) baseado em Filtro de Kalman de Estado de Erro (ESKF), que integra duas inovações principais:

A. Propagação Suave de IMU na Variedade SE(3)

• Em vez de separar rotação e translação, o método propõe representar e propagar o pose diretamente na variedade SE(3) (Grupo de Lie de movimentos rígidos).
• O estado é definido como $x = [T, Bv, b_\omega, b_a]$, onde $T$ é a matriz de transformação rígida.
• A propagação utiliza um modelo cinemático contínuo que considera o "twist" (torção) no referencial do corpo. Isso permite que a translação propagada incorpore corretamente a variação rotacional durante o intervalo de tempo ($\Delta t$), resultando em trajetórias mais suaves e precisas, mesmo em cenários de alta dinâmica.

B. Compensação de Movimento Consciente de Incerteza (UAMC)

• O método deriva a distribuição conjunta das poses preditas ao longo do tempo de propagação recursiva da IMU.
• Ao calcular a transformação relativa entre o pose de referência e o pose de cada amostra (ponto LiDAR), o método considera explicitamente a correlação entre as poses preditas (termos cruzados na covariância).
• Essa incerteza da transformação relativa é incorporada ao ruído de medição de cada ponto LiDAR, gerando uma nuvem de pontos probabilística não distorcida. Isso permite que o filtro de Kalman pondere corretamente a confiabilidade de cada ponto, especialmente em distâncias maiores onde pequenos erros rotacionais causam grandes desvios espaciais.

C. Integração no Framework LIO

• O sistema utiliza um Filtro de Kalman de Estado de Erro (ESKF).
• A propagação da IMU fornece uma distribuição a priori suave.
• A nuvem de pontos não distorcida (com ruído adaptativo via UAMC) é usada para construir resíduos em relação a um mapa local (utilizando uma estrutura de mapa de voxel, similar ao VoxelMap).
• O sistema otimiza simultaneamente a distribuição a priori e a distribuição de medição.

3. Principais Contribuições

1. Propagação de IMU em SE(3): Um método de propagação suave que integra variações rotacionais na propagação translacional, melhorando a precisão da previsão de movimento.
2. UAMC (Uncertainty-Aware Motion Compensation): Uma abordagem que caracteriza precisamente a incerteza da transformação relativa considerando a correlação entre poses, construindo medições não distorcidas probabilisticamente.
3. Framework SE(3)-LIO: Uma implementação completa de LIO que integra as duas contribuições acima, demonstrando superioridade em precisão e robustez.
4. Validação Experimental: Resultados abrangentes em diversos cenários desafiadores, incluindo drones com movimentos agressivos e veículos terrestres em terrenos acidentados.

4. Resultados Experimentais

O método foi validado em três conjuntos de dados: NTU-VIRAL (drone com movimentos agressivos), Newer-College (plataforma manual com oscilações) e um conjunto de dados próprio (veículo terrestre em terreno acidentado de grande escala).

• Precisão (RMSE de ATE): O SE(3)-LIO alcançou o menor erro de trajetória absoluta (ATE) na maioria das sequências, superando métodos state-of-the-art como FAST-LIO2, DLIO, MA-LIO e LIO-EKF.
  • No conjunto NTU-VIRAL, superou concorrentes em sequências com velocidades rotacionais e translacionais extremas (>1.6 rad/s e >5.0 m/s).
  • No conjunto de dados próprio (terreno acidentado), reduziu o erro em mais de 50% comparado ao segundo melhor método em sequências específicas.
• Robustez: Enquanto outros métodos falharam ou apresentaram grande deriva (drift) em ambientes com medições esparsas ou movimentos agressivos, o SE(3)-LIO manteve a estabilidade.
• Eficiência Computacional: O overhead computacional das novas etapas (propagação SE(3) e UAMC) é mínimo (aprox. 9.6% e 8.9% do tempo total, respectivamente). O tempo total de processamento é comparável ao FAST-LIO2, mantendo a viabilidade para aplicações em tempo real.
• Estudo de Ablação: A comparação entre a propagação em SO(3) $\times$ R3 (baseline) e SE(3) mostrou melhorias substanciais, especialmente em movimentos agressivos. A adição do UAMC melhorou consistentemente a precisão.

5. Significado e Impacto

O trabalho aborda uma lacuna fundamental na teoria de propagação de IMU ao tratar o movimento rígido como uma entidade unificada na variedade SE(3), em vez de componentes separados. Além disso, a abordagem rigorosa para a caracterização da incerteza na compensação de movimento resolve um problema de longo prazo na fusão sensorial: como lidar com a correlação temporal dos erros de predição.

O SE(3)-LIO estabelece um novo padrão para sistemas de odometria LiDAR-Inercial, oferecendo maior robustez em cenários dinâmicos e terrenos não estruturados, sem sacrificar a eficiência computacional. Isso é particularmente relevante para aplicações de robótica autônoma que operam em ambientes complexos e imprevisíveis. O código-fonte e materiais adicionais foram disponibilizados publicamente.