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

📄 arXiv: 2510.04278v1 📥 PDF

作者: Peiwen Yang, Weisong Wen, Runqiu Yang, Yuanyuan Zhang, Jiahao Hu, Yingming Chen, Naigui Xiao, Jiaqi Zhao

分类: cs.RO

发布日期: 2025-10-05

💡 一句话要点

提出FactorMPC：基于因子图的流形上集成规划与控制工具箱

🎯 匹配领域: 支柱一：机器人控制 (Robot Control)

关键词: 模型预测控制 因子图 流形优化 机器人控制 控制障碍函数

📋 核心要点

  1. 传统MPC在处理流形上的系统时，面临奇异性、过度参数化和收敛性差等问题，限制了其在机器人姿态控制和约束运动规划中的应用。
  2. FactorMPC采用因子图优化框架，统一处理系统动力学、约束和目标，原生支持流形状态和高斯不确定性，实现高效优化。
  3. 实验结果表明，FactorMPC在四旋翼飞行器上实现了优越的轨迹跟踪和避障性能，并提供了开源实现以促进研究复现。

📝 摘要（中文）

模型预测控制(MPC)在应用于非线性流形上演化的系统时面临显著限制，例如机器人姿态动力学和约束运动规划，因为传统的欧几里得公式在奇异性、过度参数化和收敛性差等方面存在问题。为了克服这些挑战，本文介绍FactorMPC，一个基于因子图的MPC工具箱，它将系统动力学、约束和目标统一到一个模块化、用户友好和高效的优化结构中。我们的方法原生支持流形值状态，并在切空间中建模高斯不确定性。通过利用因子图的稀疏性和概率结构，该工具箱即使对于具有复杂约束的高维系统也能实现实时性能。基于速度扩展的流形控制障碍函数(CBF)的避障因子是为安全关键应用而设计的。通过将图形模型与安全关键MPC连接起来，我们的工作为集成规划和控制提供了一个可扩展且几何一致的框架。四旋翼飞行器上的仿真和实验结果表明，与基线方法相比，轨迹跟踪和避障性能更优越。为了促进研究的可重复性，我们提供了提供即插即用因子的开源实现。

🔬 方法详解

问题定义：传统模型预测控制(MPC)在处理机器人姿态动力学等非线性流形上的系统时，由于欧几里得空间的局限性，容易出现奇异性、过度参数化以及收敛性差等问题。这些问题导致难以进行有效的轨迹规划和控制，尤其是在存在复杂约束和需要保证安全性的场景下。

核心思路：FactorMPC的核心思路是将MPC问题构建成一个因子图，利用因子图的概率结构和稀疏性进行高效优化。通过在流形的切空间中建模状态变量，避免了欧几里得空间的局限性。同时，将系统动力学、约束和目标函数表示为不同的因子，使得问题具有模块化和可扩展性。

技术框架：FactorMPC的整体框架包括以下几个主要模块：1) 系统建模：将系统动力学表示为因子图中的因子。2) 约束建模：将状态约束和控制约束表示为因子图中的因子，例如使用控制障碍函数(CBF)来保证安全性。3) 目标函数建模：将轨迹跟踪等目标表示为因子图中的因子。4) 因子图优化：使用高效的优化算法（例如高斯-牛顿法）求解因子图，得到最优的控制序列。

关键创新：FactorMPC的关键创新在于：1) 原生支持流形上的状态变量，避免了欧几里得空间的局限性。2) 将MPC问题表示为因子图，利用因子图的稀疏性和概率结构进行高效优化。3) 提出了基于速度扩展的流形控制障碍函数(CBF)的避障因子，保证了安全关键应用中的安全性。

关键设计：FactorMPC的关键设计包括：1) 使用李群和李代数来表示流形上的状态变量。2) 在切空间中建模高斯不确定性。3) 设计了多种类型的因子，包括动力学因子、约束因子和目标函数因子。4) 使用高斯-牛顿法等优化算法求解因子图。5) CBF的设计需要仔细选择参数，以保证安全性和性能。

🖼️ 关键图片

fig_0
fig_1
fig_2

📊 实验亮点

实验结果表明，FactorMPC在四旋翼飞行器上实现了优越的轨迹跟踪和避障性能。与基线方法相比，FactorMPC能够更准确地跟踪目标轨迹，并且能够有效地避开障碍物。此外，FactorMPC还具有实时性能，能够满足实际应用的需求。开源实现也使得其他研究者可以方便地使用和扩展该工具箱。

🎯 应用场景

FactorMPC适用于各种需要在非线性流形上进行规划和控制的机器人系统，例如无人机、无人车、机械臂等。特别是在需要保证安全性的场景下，例如避障、协同控制等，FactorMPC具有重要的应用价值。此外，该工具箱的模块化设计使得用户可以方便地添加自定义的因子，从而扩展其应用范围。

📄 摘要（原文）

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.