Integrated Planning and Control on Manifolds: Factor Graph Representation and Toolkit
PEIWEN YANG, Weisong Wen, Runqiu Yang, Yuanyuan Zhang, Jiahao Hu, Yingming Chen, Naigui XIAO, Jiaqi ZHAO
AI summary
Problem
Traditional model predictive control struggles with nonlinear manifolds due to singularities, over-parameterization, and poor convergence, while ensuring safety in dynamic environments remains computationally challenging.
Approach
FactorMPC formulates MPC as a modular factor graph optimization that natively supports manifold-valued states, Gaussian uncertainties, and velocity-extended control barrier functions for safe obstacle avoidance.
Key results
- Unified factor graph framework with explicitly derived on-manifold residuals and Jacobians
- Novel velocity-extended control barrier function for dynamic obstacle avoidance
- Open-source, plug-and-play toolkit validated on quadrotor simulations and real-world experiments
- 100 Hz real-time control with ~5.4 ms average computation time and superior tracking performance
Why it matters
Provides robotics researchers and practitioners with a scalable, geometrically consistent toolkit for safe, real-time planning and control of nonlinear dynamic systems.
Abstract
Model predictive control (MPC) faces significant limitations when applied to systems evolving on nonlinear mani- folds, such as robotic attitude dynamics and constrained motion planning, where traditional Euclidean formulations struggle with singularities, over-parameterization, and poor conver- gence. To overcome these challenges, this paper introduces Fac- torMPC, 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 uncer- tainties 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. Code and supplementary materials available at: https://github. com/RoboticsPolyu/FactorMPC.