[论文解读] Batch and Incremental Kinodynamic Motion Planning using Dynamic Factor Graphs

Mandy Xie, Frank Dellaert|arXiv (Cornell University)|May 26, 2020

Robotic Path Planning Algorithms参考文献 42被引用 4

一句话总结

本文提出一种基于动态因子图（DFG）的批量与增量式动力学运动规划方法，通过优化完整的机器人动力学（包括重力、惯性和力矩限制），生成节能的机器人轨迹。通过利用GTSAM中的稀疏非线性优化与增量贝叶斯树求解器，该方法实现了最先进的规划速度——比批量方法快达20倍——并可在最小重新计算的前提下实现高效的重规划。

ABSTRACT

This paper presents a kinodynamic motion planner that is able to produce energy efficient motions by taking the full robot dynamics into account, and making use of gravity, inertia, and momentum to reduce the effort. Given a specific goal state for the robot, we use factor graphs and numerical optimization to solve for an optimal trajectory, which meets not only the requirements of collision avoidance, but also all kinematic and dynamic constraints, such as velocity, acceleration and torque limits. By exploiting the sparsity in factor graphs, we can solve a kinodynamic motion planning problem efficiently, on par with existing optimal control methods, and use incremental elimination techniques to achieve an order of magnitude faster replanning.

研究动机与目标

• 通过整合完整的机器人动力学（包括惯性、重力和力矩限制）来应对生成节能机器人运动的挑战。
• 克服采样方法在处理复杂动态约束和高维状态空间时的局限性。
• 通过利用稀疏性与增量优化技术，实现实时动态环境下的快速、增量式重规划。
• 通过将因子图的表达能力与稀疏非线性求解器的效率相结合，改进现有最优控制方法。
• 在性能上达到与最先进批量规划方法相当的水平，同时通过增量更新实现数量级的更快重规划速度。

提出的方法

• 将动力学运动规划建模为使用动态因子图（DFG）的最优控制问题，其中机器人动力学以图模型中的因子形式编码。
• 将轨迹优化目标与动态约束（如速度、加速度、力矩限制）表示为因子图中的因子，状态变量为配置和速度。
• 使用GTSAM库进行稀疏非线性优化，结合自动微分，实现高效计算与基于梯度的收敛。
• 通过增量贝叶斯树求解器实现增量重规划，仅在轨迹部分需要更新时复用先前计算结果。
• 采用两种重规划初始化策略：直线加速度平滑路径（DFGP-L）与从前一解进行热启动（DFGP-W）。
• 在目标函数中引入最小力矩约束，以促进能量效率，尤其在涉及重载荷或动态操作的任务中。

实验结果

研究问题

• RQ1 动态因子图能否有效用于建模和求解包含完整机器人动力学的动力学运动规划问题？
• RQ2 与现有最优控制方法和采样方法相比，该方法在规划效率与解质量方面表现如何？
• RQ3 在动态环境中，增量优化技术能在多大程度上减少重规划时间而不牺牲解的质量？
• RQ4 该方法在最小化力矩使用的同时满足动态与几何约束的前提下，生成节能轨迹的效率如何？
• RQ5 不同初始化策略对增量重规划的成功率与收敛速度有何影响？

主要发现

• 所提出的DFGP算法在规划性能上与最先进批量最优控制方法相当，平均计算时间分别为1.98秒（任务1）和1.68秒（任务2）。
• 增量式iDFGP算法将平均重规划时间降低至0.119秒（任务1）和0.074秒（任务2），在热启动初始化下相比批量方法实现高达20倍的加速。
• iDFGP保持了较高的成功率（任务1为80%，任务2为96%），与DFGP-W（任务2为98%）相比仅有轻微下降，表明对目标变化具有鲁棒性。
• 引入最小力矩约束后，平均力矩显著降低（iDFGP为1489.03 N·m），证实了能量效率的提升。
• 当目标改变时，增量求解器仅更新轨迹受影响部分，如图8(f)–(j)与图9(k)–(o)所示，尽可能保持原有路径结构。
• 在涉及举升和推动重物等复杂任务中，该方法表现出卓越性能，通过最优轨迹整形最小化关节力矩，尤其在动态约束与能量效率至关重要的场景下。