A Sliding-Window Filter for Online Continuous-Time Continuum Robot State Estimation

Resumo Técnico: Um Filtro de Janela Deslizante para Estimação de Estado Contínua no Tempo em Tempo Real de Robôs Contínuos

Declaração do Problema
Robôs contínuos (RCs) são manipuladores flexíveis capazes de navegar em espaços confinados, contudo, seu controle depende de uma estimação de estado precisa. Os métodos probabilísticos existentes enfrentam uma dicotomia:

• Métodos de filtragem (por exemplo, Filtros de Kalman Estendidos) são computacionalmente eficientes e adequados para operação em tempo real, mas frequentemente sofrem com redução de precisão devido a aproximações de linearização que não capturam plenamente a estrutura de Markov de sistemas não lineares.
• Métodos de suavização em lote oferecem precisão superior ao otimizar conjuntamente todos os estados, mas são tipicamente restritos a operação offline, pois exigem acesso retrospectivo a todas as medições e não escalonam bem para controle em tempo real.
• Abordagens contínuas no tempo mostraram-se promissoras para RCs, mas anteriormente limitaram-se à otimização em lote offline. Além disso, tentativas existentes de janela deslizante para RCs basearam-se em modelos simplificados, não probabilísticos (por exemplo, aproximações de curvatura constante ou elos rígidos) ou careciam de representações estocásticas para quantificar a incerteza.

Metodologia
Os autores propõem um Filtro de Janela Deslizante (SWF) projetado especificamente para a estimação de estado contínua no tempo, em tempo real, de RCs. O método é construído sobre uma estrutura de grafo de fatores derivada de técnicas recentes de otimização em lote.

• Estrutura de Estimação: O sistema estima a pose $T(s,t)$, a velocidade $\varpi(s,t)$ e a deformação $\epsilon(s,t)$ do robô continuamente ao longo do comprimento do arco $s$ e do tempo $t$. O problema é formulado como uma estimação de Máxima A Posteriori (MAP), minimizando o log-verossimilhança negativa de fatores de prioridade e medição.
  • Prioridades: Utilizam um modelo aproximado de haste de Cosserat baseado em uma prior de movimento de "ruído branco na aceleração".
  • Medições: Incorporam medições de pose da ponta e dados de giroscópio.
• Formulação de Janela Deslizante: O filtro mantém uma janela de tempo de comprimento fixo contendo os estados $x_{a:k}$.
  • Fatorização: A posteriori conjunta é decomposta em fatores de prioridade marginalizados (que encapsulam o histórico antes da janela), fatores espaciais (ao longo do comprimento do arco do robô) e fatores de movimento/medição temporais (dentro da janela).
  • Marginalização: Para manter um tamanho de janela fixo enquanto propaga informações, o passo de tempo mais antigo é marginalizado usando o complemento de Schur na forma de informação. Isso cria uma prioridade efetiva para a nova janela, preservando a propriedade de Markov enquanto mitiga erros inerentes a suposições estritas de Markov.
• Detalhes de Implementação:
  • Expansão da Janela: Novos estados são inicializados pela média de uma estimativa consistente com a prioridade espacial e uma estimativa consistente com a prioridade temporal.
  • Extração de Estado: A média e a covariância são extraídas da traseira da janela para utilizar todas as medições disponíveis, introduzindo uma pequena latência (tipicamente $\leq 0,1$s).
  • Tratamento de Covariância: Para abordar descontinuidades causadas por re-linearização, estimativas conjuntas de covariância entre passos de tempo vizinhos são armazenadas à medida que os estados são travados.

Contribuições Principais

1. Primeiro SWF Probabilístico para RCs: Este trabalho apresenta o primeiro filtro de janela deslizante estocástico projetado especificamente para robôs contínuos, permitindo a estimação de formas de curvatura variável ao longo do espaço e do tempo.
2. Operação Contínua no Tempo em Tempo Real: O método permite que a estimação de estado contínua no tempo seja executada online, superando a restrição offline de métodos anteriores de otimização em lote contínua no tempo.
3. Desempenho Equilibrado: Alcança um compromisso entre a eficiência computacional da filtragem e a precisão da suavização em lote, operando em velocidades mais rápidas que o tempo real enquanto mantém representações estocásticas de incerteza.
4. Disponibilidade de Código Aberto: Uma implementação de código aberto é fornecida à comunidade para facilitar pesquisas futuras.

Resultados
O método foi validado em um robô contínuo acionado por tendões, impresso em 3D, real, através de cinco trajetórias distintas (incluindo movimento no espaço livre, contato rápido e interações de impulso).

• Precisão: O SWF (com uma janela de 0,1s) superou significativamente a linha de base do filtro (janela de 0s) em termos de Erro Quadrático Médio (RMSE) de Posição e Rotação da Ponta. Por exemplo, na trajetória "Contato Rápido", o SWF reduziu o RMSE de posição de 1,75% para 1,29%.
• Comparação com Lote: O SWF alcançou precisão comparável à linha de base de otimização em lote completa de 10s (por exemplo, 1,29% vs. 1,29% de RMSE em Contato Rápido), mas com custo computacional drasticamente reduzido.
• Eficiência Computacional: O SWF operou em aproximadamente 10ms por passo de tempo (100Hz), bem dentro das restrições de tempo real (limiar de 33,3ms), enquanto o método em lote exigiu ~1,3 segundos por passo.
• Sensibilidade ao Tamanho da Janela: Os resultados indicaram retornos decrescentes em precisão para tamanhos de janela maiores que 0,1s. Notavelmente, mesmo janelas muito pequenas (0,033s) forneceram melhorias significativas de precisão sobre o filtro padrão.
• Robustez: O filtro demonstrou a capacidade de rastrear o robô durante movimentos rápidos e recuperar-se rapidamente de quedas temporárias de sensores, com estimativas de incerteza expandindo-se corretamente durante a perda de dados.

Significado e Alegações
Os autores afirmam que este trabalho representa uma "direção promissora" para o campo ao preencher a lacuna entre filtragem e suavização. O significado principal reside em permitir a estimação de estado estocástica, contínua no tempo e em tempo real para robôs contínuos, uma capacidade anteriormente indisponível.

O artigo nota modestamente que, embora o método introduza uma pequena latência e uma suavidade ligeiramente reduzida em comparação com métodos completos em lote, ele mantém as propriedades contínuas no tempo da estrutura original. Os autores sugerem que a margem computacional restante (executando a >200Hz equivalente para a variante apenas de filtro) poderia suportar aplicações a jusante, como controladores e planejadores. Eles reconhecem que o desempenho pode variar com diferentes configurações de sensores e perfis de ruído, e que o aumento inesperado do RMSE em uma trajetória específica ("Fora dos Limites") com janelas maiores pode ser devido à degradação do sensor nas fronteiras, sugerindo que os tamanhos de janela ótimos são dependentes do contexto.