[論文レビュー] 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ライブラリを用いて、自動微分を介したスパース非線形最適化を実行し、効率的な計算と勾配ベースの収束を実現する。
• 部分的な軌道の更新が必要な場合にのみ前回の計算を再利用するインクリメンタルベイズツリー解法を用いて、インクリメンタル再計画を実装する。
• 再計画用の2つの初期化戦略を適用する：直線的で加速度を滑らかにしたパス（DFGP-L）と、直前の解からのウォームスタート（DFGP-W）。
• エネルギー効率を高めるために、最小トルク制約を目的関数に組み込み、特に重い荷物を運ぶか、ダイナミックな動きを伴うタスクにおいて有効に活用する。

実験結果

リサーチクエスチョン

• RQ1 完全なロボット動的特性を扱うキノダイナミクス運動計画問題を効果的にモデル化・解法するために、動的要因グラフ（DFG）を用いることは可能か？
• RQ2 提案手法は、既存の最適制御法およびサンプリングベースの手法と比較して、計画効率と解の質の点でどのように差をつけるか？
• RQ3 インクリメンタル最適化技術を用いることで、解の質を損なわず、動的環境における再計画時間をどの程度短縮できるか？
• RQ4 動的制約および幾何的制約を満たしつつ、トルク使用量を最小化することで、エネルギー効率の良い軌道を生成する手法として、その有効性はどの程度か？
• RQ5 異なる初期化戦略が、インクリメンタル再計画の成功確率および収束速度に与える影響は何か？

主な発見

• 提案されたDFGPアルゴリズムは、最新のバッチ最適制御手法と同等の計画性能を達成し、平均計算時間はタスク1で1.98秒、タスク2で1.68秒であった。
• インクリメンタルなiDFGPアルゴリズムは、平均再計画時間をタスク1で0.119秒、タスク2で0.074秒にまで短縮し、ウォームスタート初期化を用いることでバッチ手法と比較して最大20倍速くなった。
• iDFGPは高い成功確率（タスク1で80%、タスク2で96%）を維持しており、DFGP-W（タスク2で98%）と比較してわずかな低下にとどまり、ゴールの変更に対しても頑健であることが示された。
• 最小トルク制約の導入により、平均トルクがiDFGPで1489.03 N·mにまで低下し、制約なしの解と比較して著しく低い値となった。これは、エネルギー効率の向上を確認するものである。
• ゴールが変更された際、インクリメンタルソルバは変更が必要な部分のみを更新し、図8(f)～(j)および図9(k)～(o)に示すように、可能な限り元の軌道構造を保持した。
• 特に動的制約とエネルギー効率が重要な重いブロックの持ち上げや押しだしタスクにおいて、最適な軌道形状により関節トルクを最小化することで、本手法が優れた性能を発揮した。