Efficient Collision-Avoidance Constraints for Ellipsoidal Obstacles in Optimal Control: Application to Path-Following MPC and UAVs

Resumo Técnico: Restrições de Evitação de Colisão Eficientes para Obstáculos Elipsoidais em Controle Ótimo

Definição do Problema
Em aplicações robóticas, particularmente para veículos aéreos não tripulados (VANTs), garantir uma operação segura em ambientes complexos requer uma evitação de colisão eficaz. Embora planejadores de trajetória globais possam gerar caminhos geométricos evitando obstáculos estáticos conhecidos, eles frequentemente falham ao lidar com comportamentos dinâmicos ou obstáculos locais desconhecidos a priori, descobertos em tempo real. Replanejar trajetórias globais continuamente é computacionalmente proibitivo. Por outro lado, controladores locais que integram a evitação de obstáculos diretamente no loop de controle enfrentam desafios significativos. Especificamente, incorporar restrições de anti-colisão em Problemas de Controle Ótimo (OCPs) de uma maneira que seja simultaneamente diferenciável e computacionalmente eficiente é não trivial. Implementações ingênuas resultam frequentemente em solvers lentos e pouco confiáveis, dificultando a operação de malha fechada em tempo real. Além disso, métodos existentes frequentemente têm dificuldade em se estender além de cenários planares (2D) para tarefas totalmente tridimensionais (3D) mantendo a estabilidade numérica.

Metodologia
Os autores propõem um framework de controle ótimo modular projetado para a evitação de obstáculos elipsoidais 3D local, aplicado especificamente ao Controle de Seguimento de Trajetória por MPC (Model Predictive Path-Following Control).

1. Representação Elipsoidal e Detecção de Colisão:
   Tanto o robô (VANT) quanto os obstáculos são modelados como elipsoides definidos por uma forma quadrática $E(P, r) = \{x \in \mathbb{R}^3 \mid (x-r)^\top P (x-r) \leq 1\}$. Para detectar colisões de forma eficiente, o framework utiliza uma condição computacionalmente eficiente e continuamente diferenciável derivada da interseção de dois elipsoides. Esta condição baseia-se em um elipsoide auxiliar $E_\lambda$, construído como uma combinação convexa das matrizes de forma do robô e do obstáculo. A não intersecção de dois elipsoides é garantida se existir um parâmetro $\lambda \in [0, 1]$ tal que uma função escalar $K(\lambda)$ seja negativa. Especificamente, a condição $K(\lambda) < 0$ implica que os elipsoides não se interceptam, enquanto $K(\lambda) > 0$ indica uma colisão. Esta formulação permite que a restrição de colisão seja integrada diretamente no OCP como uma restrição suave e diferenciável.

2. Formulação de MPC de Seguimento de Trajetória:
   A tarefa de controle é definida como o seguimento de uma trajetória geométrica independente do tempo $P$ enquanto evita obstáculos locais. Uma lei de temporização $g(z, \nu)$ governa a evolução do parâmetro da trajetória $s$, tratando-o como uma variável de estado com uma entrada virtual $\nu$. O MPC minimiza um custo de estágio que penaliza desvios da trajetória, esforço de controle e a entrada virtual, sujeito à dinâmica do sistema e restrições de estado/entrada. A condição de evitação de colisão é formulada como $K(\lambda, x) \leq 0$ para cada passo de predição. Esta desigualdade permite o caso onde $K=0$ (elipsoides se tocando, mas sem sobreposição) ou $K<0$ (sem sobreposição), enquanto proíbe estritamente $K>0$ (colisão).

3. Abordagem de Otimização de Dois Estágios:
   Resolver o OCP resultante apresenta desafios numéricos, principalmente devido à ausência do parâmetro de colisão $\lambda$ no custo de estágio, o que pode levar ao mau condicionamento em solvers de Programação Quadrática Sequencial (SQP). Para mitigar isso, os autores introduzem um esquema de otimização de dois estágios:

• Estágio 1: Para uma determinada predição de estado, o parâmetro $\lambda$ é otimizado para minimizar $K(\lambda, x)$ ao longo do horizonte. Esta é uma otimização escalar convexa e de baixo custo.
• Estágio 2: Os valores otimizados de $\lambda$ são tratados como parâmetros fixos no problema principal de MPC, que é então resolvido para as entradas de controle e trajetória de estado.
  Este processo é iterativo (Algoritmo 1), alternando entre a atualização de $\lambda$ e a resolução do MPC, tipicamente convergindo dentro de uma única iteração na prática.

4. Modelo Dinâmico:
   O framework é validado usando um quadrotor Crazyflie 2.1. O MPC de alto nível opera a 50 Hz, fornecendo sinais de referência para um controlador de atitude de baixo nível rápido (500 Hz). A dinâmica do sistema é modelada como um sistema de tempo contínuo incorporando os efeitos do loop de atitude de baixo nível via elementos de atraso de primeira ordem para roll e pitch.

Principais Contribuições

• Extensão 3D: O trabalho estende abordagens anteriores de MPC de evitação de colisão 2D para o espaço totalmente tridimensional, lidando com obstáculos elipsoidais estáticos e móveis.
• Restrição de Colisão Diferenciável: Emprega uma condição continuamente diferenciável para detecção de colisão elipsoidal, tornando-a adequada para solvers de controle ótimo baseados em gradiente.
• Estabilidade Numérica: Uma nova abordagem de otimização de dois estágios é introduzida para resolver problemas de mau condicionamento numérico decorrentes da estrutura do OCP, garantindo tempos de computação curtos adequados para aplicações em tempo real.
• Demonstração em Hardware: A abordagem é validada através de experimentos no mundo real, marcando um passo significativo para o controle de VANTs práticos.

Resultados Experimentais
O método proposto foi testado em um ambiente interno utilizando um sistema de captura de movimento (OptiTrack) para estimativa de estado.

• Cenário de Obstáculo Estático: Em um experimento real, o Crazyflie seguiu com sucesso uma trajetória geométrica enquanto evitava um obstáculo elipsoidal estático e desconhecido. O drone desviou-se da trajetória apenas quando necessário para manter uma distância segura, demonstrando o compromisso entre o seguimento da trajetória e a segurança.
• Simulação de Obstáculo Dinâmico: Foram realizadas simulações com um obstáculo móvel, mostrando a capacidade do controlador de se adaptar a posições de obstáculos variantes no tempo.
• Desempenho: O algoritmo de otimização de dois estágios demonstrou alta eficiência computacional. A análise da função de distribuição cumulativa empírica (eCDF) mostrou que 75% dos passos de otimização completaram em menos de 2 ms, com um tempo máximo de 5,6 ms, bem dentro do intervalo de amostragem de 20 ms.
• Robustez: O sistema lidou com pequenas imprecisões na estimativa de estado utilizando restrições suaves (soft constraints), evitando falhas no solver enquanto mantinha a segurança.

Significância e Alegações
Os autores afirmam que este trabalho representa a primeira demonstração em hardware de um controlador MPC deste tipo específico (integrando evitação de colisão elipsoidal via otimização de dois estágios) para VANTs em uma tarefa tridimensional. O artigo destaca que, embora as garantias teóricas de convergência para o algoritmo iterativo permaneçam uma questão aberta, os resultados experimentais confirmam a viabilidade prática e a confiabilidade da abordagem para evitação de colisão em tempo real em ambientes 3D complexos. O framework é apresentado como uma solução modular que pode ser adaptada para várias plataformas robóticas além do quadrotor.