A Terrain-Adaptive epsilon-Constraint MPC for Uneven Terrain Kinodynamic Planning

Resumo Técnico: Um MPC de Restrição-$\epsilon$ Adaptativo ao Terreno para Planejamento Cinodinâmico em Terrenos Irregulares

Declaração do Problema
A navegação autônoma para veículos semelhantes a carros em ambientes não estruturados e irregulares (por exemplo, canteiros de obras, zonas de desastres, exploração planetária) apresenta um duplo desafio: gerar caminhos livres de colisões enquanto respeita restrições cinodinâmicas complexas. Planejadores baseados em malha tradicionais, como aqueles que utilizam o Método de Marcha Rápida (FMM), destacam-se na produção de caminhos geométricos conscientes do terreno, mas frequentemente falham em incorporar a viabilidade dinâmica, como limites de velocidade e estabilidade do veículo. Por outro lado, planejadores cinodinâmicos frequentemente lutam para equilibrar objetivos concorrentes — especificamente eficiência do caminho versus estabilidade de pose (prevenção de tombamento) — sem cair em ótimos locais ou depender de suposições 2D excessivamente simplificadas. Abordagens baseadas em dados existentes (por exemplo, Aprendizado por Reforço Profundo) frequentemente carecem de generalização através de diversas configurações de início-objetivo e falham em integrar estruturas de orientação global contínuas. Além disso, métodos padrão de otimização multiobjetivo frequentemente dependem de escalarização ponderada, o que pode permitir que o comprimento do caminho domine a solução, negligenciando restrições críticas de estabilidade em terrenos acidentados.

Metodologia
Os autores propõem um framework de controle de horizonte recedente que integra um modelo semi-paramétrico veículo-terreno com um Controle Preditivo por Modelo (MPC) de restrição-$\epsilon$ adaptativo ao terreno.

1. Modelagem Veículo-Terreno:

   • Abordagem Semi-Paramétrica: O sistema combina um modelo cinemático analítico para veículos semelhantes a carros com uma correção residual baseada em dados. O modelo nominal prevê a evolução do estado com base em velocidades lineares e angulares.
   • Processo Gaussiano Esparsos (SGP): Para contabilizar efeitos de terreno não modelados (por exemplo, deslizamento, dinâmicas de contato complexas), um SGP é treinado em descritores locais de terreno para prever o erro residual entre o modelo nominal e o comportamento real do sistema.
   • Descritores de Terreno: O modelo utiliza um conjunto rico de descritores incluindo vetores normais ponderados por área, rugosidade superficial ($\sigma_z$), curvaturas gaussianas e médias, e área total do vizinhança.
   • Função de Altura: Um método baseado em projeção determina a altura do terreno $h(x,y)$ mapeando pontos de consulta para a face mais próxima em uma malha triangular, garantindo cálculos precisos de orientação.

2. Framework de Otimização:

   • Formulação Multiobjetivo: O planejador otimiza dois objetivos concorrentes:
     • Eficiência do Caminho ($f_1$): Minimiza o desalinhamento direcional com um campo vetorial global FMM, ponderado por um fator de escala de proximidade ao objetivo.
     • Estabilidade de Pose ($f_2$): Minimiza desvios cumulativos de orientação (rolagem e arfagem) relativos à normal local do terreno, penalizando desvios além de um limiar seguro ($\alpha_{safe}$) e impondo um limite rígido ($\alpha_{max}$) para prevenir tombamento.
   • Restrição-$\epsilon$ Adaptativa ao Terreno: Em vez de escalarização ponderada, o método converte o problema em uma otimização de objetivo único com restrições. A restrição de estabilidade ($f_2 \leq \epsilon_{stab}$) é ajustada dinamicamente com base na rugosidade e inclinação locais do terreno. Em terrenos desafiadores, o orçamento de estabilidade se estreita; em terrenos benignos, ele se relaxa, permitindo uma exploração mais agressiva.
   • Estratégia de Amostragem: Sequências de controle candidatas são geradas via uma abordagem híbrida:
     • Inicialização Quente: Deslocamento de sequências de controle anteriores bem-sucedidas com ruído adicionado para garantir continuidade temporal.
     • Amostragem Viciada por FMM: Mistura de controles sugeridos pelo campo vetorial global FMM com exploração aleatória para escapar de mínimos locais.

Contribuições Principais
O artigo afirma quatro contribuições primárias:

1. Um modelo semi-paramétrico para movimento de veículos semelhantes a carros em malhas triangulares irregulares, combinando dinâmicas analíticas com aprendizado residual baseado em SGP.
2. Uma função de custo para navegação em malha derivada de um campo vetorial FMM contínuo.
3. Uma abordagem de solução utilizando otimização de restrição-$\epsilon$ adaptativa ao terreno dentro de um framework de horizonte recedente para gerar trajetórias não dominadas que equilibram eficiência e estabilidade.
4. Um planejador compatível com ROS (Move Base Flex) projetado para implantação em tempo real.

Resultados Experimentais
O método proposto ($\epsilon$-MPC) foi avaliado em simulação (Gazebo) e experimentos do mundo real usando um robô AgileX Scout Mini em dois terrenos de malha 3D distintos. Foi comparado contra as linhas de base Model Predictive Path Integral (MPPI) e Cinodinâmica Algorítmica Genética (GAKD).

• Métricas de Desempenho: O método proposto alcançou uma taxa de sucesso de navegação de 94% na simulação, superando o MPPI (87%) e o GAKD (89%).
• Estabilidade: Reduziu o desvio máximo de orientação em 24% em comparação com o MPPI e melhorou a métrica de trade-off multiobjetivo ($T$) em 23%.
• Estudos de Ablação:
  • Viés FMM: Uma força de viés FMM de $\alpha_{FMM}=0.7$ produziu resultados ótimos, melhorando as taxas de sucesso em 11–27% sobre amostragem aleatória pura.
  • Adaptatividade: A abordagem de restrição adaptativa ao terreno eliminou todos os incidentes de tombamento observados com restrições epsilon fixas e reduziu o desvio máximo de orientação em 32%.
  • Implantação no Mundo Real: Em experimentos físicos, o método manteve uma taxa de sucesso de 80% (vs. 67% para o MPPI), navegando com sucesso em seções íngremes onde as linhas de base falharam devido a ângulos de rolagem excessivos. Os tempos de planejamento permaneceram dentro das restrições de tempo real (~35–48 ms por iteração).

Significado e Alegações
O artigo afirma que seu significado primário reside em fornecer um framework principiado que aborda explicitamente a natureza multiobjetivo do planejamento cinodinâmico em terrenos irregulares. Ao ajustar dinamicamente as restrições de estabilidade com base em descritores locais de terreno, a abordagem alcança um equilíbrio superior entre optimalidade do caminho e segurança física sem depender de buscas evolutivas complexas e computacionalmente caras (como NSGA-II, que foi encontrado ser muito lento para uso em tempo real). Os autores concluem que, enquanto métodos de escalarização ponderada podem otimizar objetivos individuais, o método de restrição-$\epsilon$ adaptativo ao terreno é mais eficaz para garantir navegação viável e segura em ambientes externos não estruturados, oferecendo uma solução prática que escala da simulação para a implantação no mundo real.