[Paper Review] Batch and Incremental Kinodynamic Motion Planning using Dynamic Factor Graphs

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

Robotic Path Planning Algorithms42 references4 citations

TL;DR

This paper proposes a batch and incremental kinodynamic motion planner using dynamic factor graphs (DFG) to generate energy-efficient robot trajectories by optimizing full robot dynamics, including gravity, inertia, and torque limits. By leveraging sparse nonlinear optimization via GTSAM and incremental Bayes tree solvers, the method achieves state-of-the-art planning speed—up to 20× faster than batch methods—and enables efficient replanning with minimal re-computation.

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.

Motivation & Objective

• Address the challenge of generating energy-efficient robot motions by incorporating full robot dynamics, including inertia, gravity, and torque limits.
• Overcome the limitations of sampling-based methods in handling complex dynamic constraints and high-dimensional state spaces.
• Enable fast, incremental replanning for dynamic environments by exploiting sparsity and incremental optimization techniques.
• Improve upon existing optimal control methods by combining the expressiveness of factor graphs with the efficiency of sparse nonlinear solvers.
• Achieve performance on par with state-of-the-art batch planners while enabling orders-of-magnitude faster replanning via incremental updates.

Proposed method

• Formulate kinodynamic motion planning as an optimal control problem using dynamic factor graphs (DFG), where robot dynamics are encoded as factors in a graphical model.
• Represent trajectory optimization objectives and dynamic constraints (e.g., velocity, acceleration, torque limits) as factors in a factor graph, with configuration and velocity states as variables.
• Use the GTSAM library to perform sparse nonlinear optimization with automatic differentiation, enabling efficient computation and gradient-based convergence.
• Implement incremental replanning via an incremental Bayes tree solver that reuses prior computation when only parts of the trajectory need updating.
• Apply two initialization strategies for replanning: a straight-line acceleration-smoothed path (DFGP-L) and warm-starting from the previous solution (DFGP-W).
• Integrate minimum torque constraints into the objective to promote energy efficiency, particularly in tasks involving heavy payloads or dynamic maneuvers.

Experimental results

Research questions

• RQ1 Can dynamic factor graphs be effectively used to model and solve kinodynamic motion planning problems with full robot dynamics?
• RQ2 How does the proposed method compare to existing optimal control and sampling-based approaches in terms of planning efficiency and solution quality?
• RQ3 To what extent can incremental optimization techniques reduce replanning time in dynamic environments without sacrificing solution quality?
• RQ4 How effective is the method in generating energy-efficient trajectories by minimizing torque usage while satisfying dynamic and geometric constraints?
• RQ5 What impact do different initialization strategies have on the success rate and convergence speed of incremental replanning?

Key findings

• The proposed DFGP algorithm achieves planning performance on par with state-of-the-art batch optimal control methods, with average computation times of 1.98s (Task 1) and 1.68s (Task 2).
• The incremental iDFGP algorithm reduces average replanning time to 0.119s (Task 1) and 0.074s (Task 2), achieving up to 20× speedup over batch methods with warm-start initialization.
• iDFGP maintains high success rates (80% for Task 1, 96% for Task 2), with only a minor drop compared to DFGP-W (98% for Task 2), indicating robustness to goal changes.
• The inclusion of minimum torque constraints results in significantly lower average torques (1489.03 N·m for iDFGP) compared to unconstrained solutions, confirming improved energy efficiency.
• The incremental solver successfully updates only the affected parts of the trajectory when the goal is changed, as shown in Figures 8(f)–(j) and 9(k)–(o), preserving the original path structure where possible.
• The method demonstrates superior performance in complex tasks such as lifting and pushing heavy blocks, where dynamic constraints and energy efficiency are critical, by minimizing joint torques through optimal trajectory shaping.