Integrated Planning and Control on Manifolds: Factor Graph Representation and Toolkit

2025-10-05

AI Summary

To address singularity, parameter redundancy, and convergence challenges in model predictive control (MPC) for systems evolving on nonlinear manifolds (e.g., SO(3)), this paper introduces the first geometrically consistent MPC framework based on factor graphs. Our method unifies manifold-valued states and tangent-space Gaussian uncertainties within a single factor graph formulation; introduces velocity-extended manifold control barrier functions to tightly couple safety constraints with geometric dynamics; and integrates sparse nonlinear optimization with tangent-space probabilistic modeling to enable real-time execution under high-dimensional, complex constraints. Experimental validation on a quadrotor platform demonstrates significant improvements in trajectory tracking accuracy and robustness to dynamic obstacle avoidance. An open-source toolkit ensures reproducibility and modular deployment.

Technology Category

• Intelligent Robots: State Estimation
• Machine Learning: Learning with Manifolds
• Constraint Satisfaction and Optimization: Mixed Discrete/Continuous Optimization

Application Category

• Graph Algorithms and Modeling for the Web: Efficient manipulation of static and dynamic Web-related graphs
• User Modeling, Personalization and Recommendation: User privacy protection in personalized systems
• Systems and Infrastructure for Web, Mobile and WoT: Experiences and lessons learnt from Web-based algorithms and system deployments

Abstract

Model predictive control (MPC) faces significant limitations when applied to systems evolving on nonlinear manifolds, such as robotic attitude dynamics and constrained motion planning, where traditional Euclidean formulations struggle with singularities, over-parameterization, and poor convergence. To overcome these challenges, this paper introduces FactorMPC, a factor-graph based MPC toolkit that unifies system dynamics, constraints, and objectives into a modular, user-friendly, and efficient optimization structure. Our approach natively supports manifold-valued states with Gaussian uncertainties modeled in tangent spaces. By exploiting the sparsity and probabilistic structure of factor graphs, the toolkit achieves real-time performance even for high-dimensional systems with complex constraints. The velocity-extended on-manifold control barrier function (CBF)-based obstacle avoidance factors are designed for safety-critical applications. By bridging graphical models with safety-critical MPC, our work offers a scalable and geometrically consistent framework for integrated planning and control. The simulations and experimental results on the quadrotor demonstrate superior trajectory tracking and obstacle avoidance performance compared to baseline methods. To foster research reproducibility, we have provided open-source implementation offering plug-and-play factors.

Problem

• Overcoming MPC limitations on nonlinear manifolds with singularities
• Unifying dynamics, constraints, and objectives in modular optimization structure
• Enabling real-time safety-critical control with manifold-valued states

Innovation

• Factor-graph based MPC toolkit for manifolds
• Native manifold state support with Gaussian uncertainties
• Real-time performance via sparse probabilistic structure