Resumo Técnico: Questões de Terminal: Planejamento Cinodinâmico com Custo Terminal e Incerteza Aprendida no Espaço de Estado-Custo de Crença

Declaração do Problema

Sistemas robóticos operando em ambientes do mundo real devem gerar movimentos dinamicamente viáveis que satisfaçam restrições cinodinâmicas, ao mesmo tempo em que consideram a incerteza de execução decorrente de erros de modelagem, ruído de percepção e interações de contato complexas. Embora planejadores cinodinâmicos baseados em amostragem existentes (por exemplo, AO-RRT, SST) ofereçam completude probabilística e optimalidade assintótica, eles geralmente otimizam apenas o custo de execução (acumulado ao longo da trajetória, como comprimento do caminho ou energia) e tratam a chegada ao objetivo meramente como uma verificação de viabilidade.

Essa abordagem apresenta duas limitações críticas:

1. Qualidade do Estado Terminal: Muitas tarefas dependem da qualidade do estado final, e não apenas do caminho percorrido. Por exemplo, em empurramento planar, um caminho mais curto pode resultar em alta incerteza terminal, reduzindo a probabilidade de alcançar com sucesso o objetivo. Em cenários de múltiplos objetivos (por exemplo, estacionamento de carros), os planejadores podem selecionar um objetivo geometricamente mais próximo que é menos desejável ou mais difícil de abandonar, ignorando preferências por regiões terminais específicas.
2. Propagação de Incerteza: O planejamento no espaço de crença (onde o estado é uma distribuição de probabilidade) é essencial para robustez. No entanto, os métodos existentes frequentemente dependem de limiares de probabilidade rígidos ou substitutos por trajetória, em vez de otimizar explicitamente o estado final de crença. Além disso, sistemas complexos (como empurramento rico em contato) frequentemente carecem de modelos analíticos de incerteza, tornando necessárias abordagens baseadas em dados que raramente são integradas de forma estreita aos planejadores no espaço de crença.

Metodologia

Os autores propõem KiTe (Planejamento Cinodinâmico com Custo Terminal), um framework que amplia a Árvore Aleatória de Exploração Rápida Assintoticamente Ótima (AO-RRT) com um objetivo explícito de custo terminal. A metodologia opera em três camadas principais:

1. KiTe com Custo Terminal no Espaço de Estados

O algoritmo central reformula o problema de planejamento cinodinâmico ótimo em um espaço de estado-custo ampliado $\mathcal{Y} = \mathcal{X} \times \mathbb{R}^+$.

• Dinâmica Ampliada: O estado é estendido de $x$ para $y = (x, c)$, onde $c$ acumula o custo de execução $\ell(x, u)$. As dinâmicas são estendidas para incluir a taxa de custo: $\dot{y} = (f(x, u), \ell(x, u))$.
• Integração de Custo Terminal: Diferentemente de formulações anteriores onde o custo terminal é ignorado durante a busca, o KiTe avalia o custo total $C_T = \int \ell dt + \phi(x(T))$ apenas ao alcançar a região do objetivo.
• Estratégia de Busca: O algoritmo mantém um limite de custo $c_{best}$. Quando uma solução é encontrada, o limite é atualizado para $c_{new} + \phi(x_{new})$. A estratégia de amostragem então foca no subconjunto de nível definido por esse limite, podando ramos que não podem melhorar o custo total (execução + terminal).
• Garantia Teórica: Os autores fornecem uma prova rigorosa de que o AO-RRT com custo terminal preserva a optimalidade assintótica, desde que as dinâmicas do sistema, o custo de execução e o custo terminal satisfaçam condições de continuidade de Lipschitz.

2. Extensão para o Espaço de Crença

A formulação é estendida para o espaço de estado-custo de crença $\mathcal{Y}_b = \mathcal{B} \times \mathbb{R}^+$, onde $\mathcal{B}$ é o espaço de distribuições de probabilidade sobre o estado.

• Métrica: A distância de Wasserstein-2 é adotada como a métrica para o espaço de crença, pois captura naturalmente mudanças tanto na média quanto na covariância de crenças Gaussianas.
• Limite Inferior para Chegada ao Objetivo: Uma contribuição teórica fundamental é a prova de que a probabilidade de alcançar uma região de objetivo $G$ (definida como uma bola de raio $r$) é limitada inferiormente pela distância de Wasserstein entre a crença terminal $b(T)$ e a medida de Dirac no objetivo $\nu_g$:
  $$\text{Pr}(x(T) \in G) \geq 1 - \frac{W_2^2(b(T), \nu_g)}{r^2}$$
  Isso permite que o planejador use a distância de Wasserstein como um custo terminal $\psi(b(T))$, otimizando diretamente um limite inferior para o sucesso de chegada ao objetivo sem exigir limiares de probabilidade rígidos e manualmente ajustados.
• Continuidade de Lipschitz: Os autores provam que, sob suposições leves sobre as dinâmicas estocásticas (especificamente regarding a continuidade de Lipschitz da incerteza do processo e injeção de ruído), as dinâmicas de crença e as funções de custo permanecem contínuas de Lipschitz em relação à métrica de Wasserstein. Isso garante que a optimalidade assintótica do KiTe se estenda ao espaço de crença.

3. Aprendizado de Dinâmicas de Crença

Para abordar sistemas onde modelos analíticos de incerteza estão indisponíveis (por exemplo, empurramento rico em contato), o KiTe integra um modelo de dinâmicas de crença aprendido.

• Arquitetura do Modelo: Uma rede neural (MLP) é treinada para prever a transição estocástica local. Dado um estado $x_i$ e controle $u_i$, a rede produz uma transição média $\Delta x_i$ e um parâmetro de covariância do processo parametrizado por um vetor de variância $\sigma_i^2$.
• Objetivo de Treinamento: O modelo é treinado usando a perda de Negativo Log-Likelihood (NLL) em dados de interação. Isso permite que a rede aprenda tanto as dinâmicas médias quanto a incerteza aleatória (ruído do processo) diretamente dos dados.
• Propagação: A média e a covariância aprendidas são usadas para propagar o estado de crença (média e covariância) para frente no planejador, suportando tanto espaços de estados Euclidianos quanto de grupos de Lie (por exemplo, SE(2)).

Contribuições Principais

1. Extensão Algorítmica e Prova: Implementação do AO-RRT com um custo terminal explícito, acompanhada de uma prova rigorosa de que a optimalidade assintótica é preservada tanto no espaço de estados quanto no espaço de crença.
2. Vínculo Teórico com Probabilidade de Sucesso: Prova de que minimizar a distância de Wasserstein entre a crença terminal e o objetivo induz um limite inferior na probabilidade de sucesso de chegada ao objetivo, fornecendo um objetivo terminal principiado para confiabilidade.
3. Propagação de Crença Baseada em Dados: Demonstração de como aprender modelos de dinâmicas de crença (média e incerteza) diretamente de dados de interação usando perda NLL e integrá-los ao framework KiTe para sistemas que carecem de modelos analíticos.
4. Validação Empírica: Avaliação abrangente em três tarefas diversas (Flappy Bird, Estacionamento de Carros, Empurramento Planar) tanto em simulação quanto no mundo real, comparando o KiTe com bases de referência state-of-the-art (AO-RRT, SST, Árvore de Crença Gaussiana).

Resultados Experimentais

O artigo valida o KiTe em três tarefas:

• Flappy Bird (Determinístico): Demonstra que o KiTe pode codificar preferências de objetivo (por exemplo, centralizar o pássaro no vão) otimizando o custo terminal, enquanto as bases de referência param de refinar assim que a região do objetivo é alcançada. O KiTe alcança trajetórias mais próximas do centro do objetivo com apenas um aumento modesto no custo de execução.
• Estacionamento de Carros (SE(2) com Incerteza): Em uma tarefa com múltiplas vagas de estacionamento viáveis, o KiTe prioriza com sucesso uma região de objetivo "preferida" (vaga frontal) e melhora as taxas de sucesso de chegada ao objetivo em comparação com bases de referência que otimizam apenas o comprimento do caminho ou usam restrições de chance. O planejamento no espaço de crença com custo terminal produziu as maiores taxas de sucesso de execução.
• Empurramento Planar (Rico em Contato, Mundo Real): Usando dinâmicas aprendidas para objetos como um Caminhão de Lixo e uma Caixa de Biscoitos, o KiTe superou as bases de referência (incluindo Planejamento Ativo com incerteza de modelo) nas taxas de sucesso de execução. O planejador selecionou caminhos geométricos mais longos que resultaram em menor incerteza terminal e maiores probabilidades de sucesso, demonstrando o valor de otimizar a qualidade da crença terminal.

Significado e Alegações

O artigo alega que o KiTe aborda uma lacuna fundamental no planejamento cinodinâmico: a separação da otimização de trajetória da qualidade do estado terminal. Ao otimizar explicitamente o estado terminal, o método:

• Melhora a Confiabilidade: Aumenta diretamente a probabilidade de alcançar o objetivo sob incerteza, minimizando a distância de Wasserstein até o objetivo.
• Codifica Preferências: Permite que os planejadores distingam entre múltiplos objetivos viáveis com base na desejabilidade (por exemplo, facilidade de abandono, estabilidade) em vez de apenas proximidade geométrica.
• Une Aprendizado e Planejamento: Fornece um framework onde modelos de incerteza aprendidos não são usados apenas para enviesar a amostragem, mas são integrados de forma estreita ao loop de propagação de crença e otimização.

Os autores concluem que, embora o método introduza parâmetros de projeto (pesos terminais) e dependa de modelos aprendidos, ele oferece uma abordagem principial e teoricamente fundamentada para gerar movimentos que não são apenas dinamicamente viáveis, mas também confiáveis e conscientes de preferências em ambientes incertos.