[论文解读] A Factor-Graph Approach for Optimization Problems with Dynamics Constraints

Mandy Xie, Alejandro Escontrela+1 more|arXiv (Cornell University)|Nov 11, 2020

Model-Driven Software Engineering Techniques参考文献 51被引用 14

一句话总结

本文提出了动力学因子图（DFGs），一种图形化框架，将刚体动力学建模为因子图，以解决机器人学中的优化问题，包括运动学动力学路径规划和全身动力学问题。通过将动力学约束和目标函数表示为因子，DFGs 能够利用稀疏非线性求解器（如 GTSAM）实现高效、高维的优化，成功实现了从小车-摆系统到12自由度四足机器人系统的轨迹优化，且无需简化动力学模型或依赖专家先验的动力学知识。

ABSTRACT

In this paper, we introduce dynamics factor graphs as a graphical framework to solve dynamics problems and kinodynamic motion planning problems with full consideration of whole-body dynamics and contacts. A factor graph representation of dynamics problems provides an insightful visualization of their mathematical structure and can be used in conjunction with sparse nonlinear optimizers to solve challenging, high-dimensional optimization problems in robotics. We can easily formulate kinodynamic motion planning as a trajectory optimization problem with factor graphs. We demonstrate the flexibility and descriptive power of dynamics factor graphs by applying them to control various dynamical systems, ranging from a simple cart pole to a 12-DoF quadrupedal robot.

研究动机与目标

• 开发一种统一、直观的图形化表示方法，用于建模机器人学中复杂的动力学问题。
• 在不依赖简化动力学模型的前提下，实现高维、运动学动力学约束路径规划问题的优化。
• 将动力学约束、不等式约束（如接触力、关节限位）以及目标函数（如最小扭矩）整合到单一优化框架中。
• 展示该框架在从简单摆到复杂腿式机器人等多样化机器人系统中的灵活性与可扩展性。
• 提供即插即用的轨迹优化解决方案，避免对专家定制的动力学算法或物理引擎黑箱的依赖。

提出的方法

• 将刚体动力学表示为因子图，其中动力学方程（如速度与加速度约束）作为作用于状态变量（如关节位置、速度、力矩）的因子进行编码。
• 使用浮体动力学算法（ABA）和复合刚体动力学算法（CRBA）作为底层动力学求解器，并将其嵌入因子图结构中以确保一致性。
• 通过在图中添加轨迹平滑性因子和动力学约束，将运动学动力学路径规划问题形式化为轨迹优化问题。
• 采用铰链损失（hinge loss）公式化不等式约束（如接触力、关节限位），以在优化框架内实现安全且可行的解。
• 利用 GTSAM 实现的先进稀疏非线性优化技术，高效求解由因子图产生的高维稀疏方程组。
• 通过定义适当的因子类型和目标函数，将该框架应用于多样化系统（小车-摆、Kuka 机械臂、12自由度四足机器人），而无需修改核心算法。

实验结果

研究问题

• RQ1 统一的图形化框架是否能够同时表示经典动力学问题与复杂的运动学动力学路径规划问题（包含全动力学）？
• RQ2 动力学约束、接触力与不等式约束能否在因子图框架中自然编码并实现优化？
• RQ3 该方法是否能在不使用简化动力学模型的前提下，为多样化机器人系统生成精确、节能且动力学可行的轨迹？
• RQ4 与模型预测控制或简化模型相比，DFG 框架在开环执行中在轨迹精度与鲁棒性方面提升了多少？
• RQ5 相同的优化流水线是否可复用于不同复杂度的系统，且仅需极少的重新配置？

主要发现

• DFG 框架成功建模并求解了前向与反向动力学问题，且能够通过因子图中特定的消去顺序恢复已知算法（如 ABA、CRBA）。
• 在小车-摆系统中，DFGP 生成的轨迹在两种配置下均实现了对位置与速度目标的精确跟踪，误差极小。
• 在 Kuka LBR iiwa 机械臂上，采用最小扭矩目标的 DFGP 解决方案，使第2和第4关节的峰值力矩相比无约束优化降低了高达30%，证实了更高的能量效率。
• 在12自由度四足机器人上，优化后的轨迹在开环执行中实现了高精度跟踪，尽管存在模型不确定性，实际质心轨迹仍紧密跟随期望路径。
• 引入接触因子与关节限位约束后，四足机器人沿六边形路径行进过程中未发生打滑或关节越界现象。
• 该框架在从2自由度到12自由度的各类系统中均表现出端到端的适用性，仅使用相同的底层算法与求解器栈（GTSAM）。