Resumo Técnico: DynoJEPP – Estimação, Predição e Planejamento Conjuntos em Ambientes Dinâmicos

1. Formulação do Problema

A autonomia robótica em ambientes do mundo real é fundamentalmente desafiada pela presença de objetos dinâmicos com movimentos complexos e imprevisíveis. Embora avanços recentes tenham unificado com sucesso a estimação do estado do robô e o planejamento de trajetória dentro de estruturas de grafos de fatores para ambientes estáticos, a extensão dessas abordagens para cenas dinâmicas permanece limitada.

As soluções existentes geralmente enfrentam uma de duas limitações:

1. Suposição de Conhecimento Perfeito: Elas assumem que as trajetórias futuras dos objetos são conhecidas a priori, o que é irrealista em configurações dinâmicas.
2. Estratégias Reativas: Elas dependem de evasão puramente reativa, carecendo da previsão de trajetória.

Uma questão crítica, frequentemente negligenciada em estruturas de otimização simultânea, é o fluxo bidirecional de informações. Em formulações conjuntas convencionais, informações dos módulos de planejamento e predição retroalimentam o módulo de estimação de estado. Esse ciclo de feedback corrompe as estimativas de estado com vieses "otimistas" derivados de planos futuros, levando à degradação da precisão da estimação, comportamentos indesejados e, em última instância, planos inseguros e colisões.

2. Metodologia: A Estrutura DynoJEPP

Os autores propõem o DynoJEPP, uma estrutura unificada baseada em grafos de fatores que otimiza simultaneamente a estimação do estado do robô e dos objetos, a predição de trajetória e o planejamento local. A inovação central reside na introdução de um fator direcionado novel para controlar o fluxo de informações.

A. Fatores Direcionados

Para evitar que o planejamento e a predição degradem a estimação, o DynoJEPP introduz um fator direcionado que impõe fluxo unidirecional de informações:

• Estimação $\to$ Predição/Planejamento: Os resultados da estimação (pose atual do robô e movimentos dos objetos) propagam-se para frente para informar a predição e o planejamento.
• Predição/Planejamento $\nrightarrow$ Estimação: Estados futuros e restrições de planejamento são impedidos de influenciar a estimação de estados atuais ou passados.

Tecnicamente, isso é alcançado via manipulação da Jacobiana. Ao zerar as entradas da Jacobiana em relação à variável fonte (o estado de estimação) nas equações normais ($J^\top J \delta = -J^\top r$), a estrutura garante que o gradiente do custo de planejamento/predição não atualize as variáveis de estimação. Isso desacopla estruturalmente o sub-grafo de estimação do sub-grafo de planejamento, mantendo ao mesmo tempo a dependência necessária para que o planejamento utilize as estimativas atuais.

B. Componentes da Formulação Conjunta

A estrutura integra três módulos em um único grafo de fatores:

1. Estimação de Estado: Baseia-se no SLAM Dinâmico Híbrido para estimar a pose do robô ($W X_k$) e os movimentos de corpo rígido SE(3) de objetos dinâmicos ($W eH_k$). Utiliza medições de pontos, odometria e fatores de suavização de movimento para rastrear os centros de massa dos objetos.
2. Predição de Movimento de Objetos: Utiliza os movimentos SE(3) estimados para prever trajetórias futuras dos objetos ao longo de um horizonte $N$. Emprega um modelo de velocidade constante (imposto por fatores de suavização), mas relaxa essa suposição próximo a obstáculos estáticos usando fatores Conscientes de Cena Estática (baseados em ESDF).
3. Planejamento Local: Emprega um Controle Preditivo por Modelo (MPC) baseado em grafos de fatores para otimizar poses, velocidades e acelerações futuras do robô. Incorpora:
   • Consciência de Cena Estática: Utiliza Campos de Distância Sinalizada Euclidiana (ESDF) para evitar obstáculos estáticos.
   • Consciência de Cena Dinâmica: Utiliza fatores direcionados de obstáculos dinâmicos para penalizar a proximidade a objetos em movimento com base em suas trajetórias previstas.

C. Extensão Cooperativa (C-DynoJEPP)

O artigo introduz o DynoJEPP Cooperativo (C-DynoJEPP), que permite ao robô egoísta incorporar comportamentos cooperativos em seu planejamento. Nesta extensão, os fatores de obstáculos dinâmicos são direcionados da predição para o planejamento. Isso cria um ciclo direcionado entre predição e planejamento, modelando a influência mútua onde o robô reage ao objeto, e espera-se que o objeto reaja à trajetória pretendida pelo robô. Isso permite uma navegação menos conservadora e mais assertiva em cenários interativos.

3. Contribuições Principais

1. Estrutura DynoJEPP: A primeira estrutura baseada em grafos de fatores a estimar conjuntamente o movimento de objetos dinâmicos, prever sua evolução e incorporar essas previsões nas ações de controle do robô dentro de um único problema de otimização.
2. Fator Direcionado Novel: Um mecanismo para especificar a direção da influência em grafos de fatores, permitindo fluxo de informações controlado entre estimação, predição e planejamento. Isso previne a "corrupção" das estimativas de estado por restrições de planejamento futuras.
3. Otimização Cooperativa: Uma extensão (C-DynoJEPP) que parametriza o nível de cooperação de objetos dinâmicos, permitindo influência mútua entre o plano do robô e a predição do objeto sem comprometer a independência da estimação de estado.

4. Resultados Experimentais

Os autores avaliaram a estrutura em um ambiente de armazém simulado (Gazebo) com robôs de tração diferencial e obstáculos dinâmicos. Compararam três configurações: Não Direcionada (fluxo bidirecional), Direcionada (DynoJEPP) e Desacoplada (estimação separada seguida de predição/planejamento conjuntos).

• Segurança e Taxa de Sucesso: Em 10 experimentos de navegação envolvendo obstáculos dinâmicos, a configuração Não Direcionada falhou na maioria dos casos (colidindo em 8 de 10 ensaios dinâmicos) devido a um planejamento excessivamente otimista que corrompia as estimativas. Em contraste, a configuração Direcionada alcançou uma taxa de sucesso de 100% em todos os ensaios.
• Precisão da Estimação: O método Direcionado manteve a precisão da estimação comparável à linha de base Desacoplada, superando significativamente a abordagem Não Direcionada. O método Não Direcionado mostrou degradação significativa tanto nas estimativas de trajetória do robô quanto dos objetos.
• Navegação Cooperativa: Em cenários com obstáculos dinâmicos reativos, o C-DynoJEPP demonstrou a capacidade de navegar de forma mais assertiva do que o DynoJEPP padrão. Alcançou os objetivos mais rapidamente (82 passos versus 92 passos para a execução mais rápida do DynoJEPP), aproveitando pesos cooperativos para influenciar as previsões.

5. Significado e Alegações

O artigo alega que controlar a direcionalidade do fluxo de informações é crítico para uma operação segura em ambientes dinâmicos. Sem fatores direcionados, o acoplamento de planejamento e estimação leva a um ciclo de feedback onde objetivos de planejamento distorcem as estimativas de estado, resultando em comportamentos inseguros.

Os autores enfatizam que, embora a estimação não beneficie inerentemente da otimização conjunta com o planejamento, a predição e o planejamento beneficiam-se de serem otimizados conjuntamente com a estimação, particularmente ao modelar comportamentos interativos. A estrutura proposta fornece uma solução robusta que permite otimização simultânea segura sem sacrificar a integridade da estimação de estado.

O trabalho conclui observando que pesquisas futuras focarão na implementação de uma versão eficiente, incremental e multithread do DynoJEPP para alcançar desempenho em tempo real, com comparação de benchmarks contra padrões da indústria como o Nav2.