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

TL;DR

The paper tackles MPC for systems evolving on nonlinear manifolds (e.g., $SO(3)$) where standard Euclidean formulations fail. It introduces FactorMPC, a factor-graph based toolkit that unifies dynamics, constraints, and objectives for on-manifold states, leveraging tangent-space Gaussian uncertainties and sparsity for real-time optimization. Key contributions include a universal dynamics factor, velocity-extended CBF safety factors, and an open-source GTSAM-based toolkit with iSAM integration, validated on quadrotor platforms against baselines. The results show improved trajectory tracking and obstacle avoidance with real-time performance, highlighting the framework's scalability and potential for rapid experimentation in integrated planning and control on manifolds.

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.

Figures (11)

• Figure 1: The proposed FactorMPC framework: A unified factor graph-based toolkit for on-manifold model predictive control (MPC). (a) System architecture. (b) Key constraint formulations including dynamics, safety barriers, and state boundaries. (c) Factor graph representation of the integrated planning-control pipeline with corresponding error-state linearization details. (d) Benchmark applications demonstrating the toolkit's versatility.
• Figure 2: The factor graph of MPC.
• Figure 3: The illustration of CBFs. (a) Sphere obstacle. (b) Geofence.
• Figure 4: The quadrotor velocity reference curve.
• Figure 5: (a) The quadrotor’s path using distance-based CBF under different conditions: blue dashed line depicts $\alpha = 1.2$ and purple solid line depicts $\alpha = 0.3$. (b) The quadrotor’s velocity when $\alpha = 0.3$. (c) The boundary distance between the obstacle and quadrotor.