[論文レビュー] Trajectory Optimization on Manifolds: A Theoretically-Guaranteed Embedded Sequential Convex Programming Approach

Riccardo Bonalli, Andrew Bylard+3 more|arXiv (Cornell University)|May 18, 2019

Robotic Path Planning Algorithms被引用数 26

ひとこと要約

本稿では、多様体上の軌跡最適化のための埋め込み逐次凸計画法（E-SCP）フレームワークを提案する。幾何学的埋め込みを用いて多様体制約付き問題を可解なユークリッド空間に持ち上げることで、理論的保証付き収束を達成し、ペナルティ法に比べて優れた妥当性と最適性を実現する。数値結果では、コストが低く、多様体制約の誤差は無視できるほど小さいことが示された。

ABSTRACT

Sequential Convex Programming (SCP) has recently gained popularity as a tool for trajectory optimization due to its sound theoretical properties and practical performance. Yet, most SCP-based methods for trajectory optimization are restricted to Euclidean settings, which precludes their application to problem instances where one must reason about manifold-type constraints (that is, constraints, such as loop closure, which restrict the motion of a system to a subset of the ambient space). The aim of this paper is to fill this gap by extending SCP-based trajectory optimization methods to a manifold setting. The key insight is to leverage geometric embeddings to lift a manifold-constrained trajectory optimization problem into an equivalent problem defined over a space enjoying a Euclidean structure. This insight allows one to extend existing SCP methods to a manifold setting in a fairly natural way. In particular, we present a SCP algorithm for manifold problems with refined theoretical guarantees that resemble those derived for the Euclidean setting, and demonstrate its practical performance via numerical experiments.

研究の動機と目的

• ループ閉じやSO(3)ダイナミクスなどの多様体制約付き問題に対して、理論的保証付きの軌跡最適化手法が不足しているという問題に対処すること。
• 多様体をユークリッド空間に埋め込むことで、非ユークリッド設定への逐次凸計画法（SCP）の拡張を図り、問題構造を保持すること。
• implicitly 定義されたまたは局所パラメータ化された多様体（例えばトーラスやリー群）においても、解の収束性と妥当性を保証すること。
• 制御限界やゴール領域を含む多様なロボット制約を扱える一般性・信頼性・計算効率に優れたフレームワークを提供すること。
• ペナルティに基づくアプローチに比べ、解の品質と制約満たしの両面で実用的優位性を示すこと。

提案手法

• 滑らかな埋め込みを用いて多様体制約付き最適制御問題を等価なユークリッド問題に持ち上げ、多様体の幾何的構造を保持すること。
• 埋め込み問題に逐次凸計画法（SCP）を適用し、コスト関数および制約の凸近似から得られる凸部分問題を繰り返し解くこと。
• チャートによる多様体の局所パラメータ化を用いて埋め込みを定義し、サブマージョン E に対して E(x)=0 のような暗黙的制約を扱えるようにすること。
• 動的および制約関数の有界性とリプシッツ連続性という弱い仮定の下で、標準SCPの収束保証がE-SCPアルゴリズムに継承されることを証明することで理論的収束を保証すること。
• シューティング法をE-SCPと統合し、初期軌跡推定値を改善することで収束を加速し、SCP反復回数を最大59.4%まで削減すること。
• 埋め込みによって多様体制約を正確に満たすことで、解が不実行解や最適でないコストに陥る可能性のあるペナルティ項を回避すること。

実験結果

リサーチクエスチョン

• RQ1 SCPに基づく軌跡最適化は、理論的収束保証を維持したまま、多様体制約付き問題へ拡張可能か？
• RQ2 幾何学的埋め込みをどのように用いることで、非ユークリッド軌跡最適化問題をSCPに適した等価なユークリッド問題に変換できるか？
• RQ3 提案されたE-SCP法は、SO(3) やトーラスのような複雑な多様体において、ペナルティ法に比べてより優れた解の品質と妥当性を達成するか？
• RQ4 シューティング法は、高次元または複雑な環境におけるE-SCPの収束速度と解の品質をどの程度向上できるか？
• RQ5 明示的パラメータ化やヒューリスティック補正を必要とせず、E(x)=0 のような暗黙的多様体制約をE-SCPが処理できるか？

主な発見

• マニピュレータアーム実験では、E-SCPが真のコスト0.105を達成したのに対し、TrajOptは0.215、TrajOpt-Pは0.792であった。最適性の優位性が明確に示された。
• E-SCPの多様体制約誤差は3.9×10⁻²であり、TrajOptの1.6×10⁻²やTrajOpt-Pの3.1×10⁻⁶と比較して無視できるほど小さい。妥当性の優位性が裏付けられた。
• シューティング法と組み合わせたE-SCPでは、SCP反復回数が59.4%削減され、平均計算時間は0.466秒から0.191秒に短縮された。
• E-SCPの動的制約誤差は1.9×10⁻³であり、TrajOptの8.5×10⁻³やTrajOpt-Pの4.8×10⁻³より低く、数値的精度の優位性が示された。
• E-SCPは動的誤差に比例して多様体制約誤差がほぼゼロに近づく傾向を示し、SO(3) や閉ループキネマティクスチェーンのような複雑な多様体に対しても頑健であることが示唆された。
• 初期値が悪いためSCP単体では収束しなかったケースでも、E-SCP＋シューティング法は成功した。収束性の向上が明確に示された。