動作計画

概要

動作計画は、ロボットが開始コンフィギュレーションから目標まで衝突せずに移動する方法を見つけます。次に、その経路を関節コントローラが追従できる軌道に変換します。出力は関節位置の列で、移動ロボットの場合はベース姿勢の列です。どちらにも時刻が付きます。このハブは、ライブラリのプランナを呼び出していて、それが失敗する理由や遅い理由を知りたいエンジニアを対象にしています。

計画はコンフィギュレーション空間、つまりすべての関節値からなる空間で行います。6 自由度（DoF）のアームのコンフィギュレーション空間は6次元です。作業空間の障害物は、この空間内の不規則な領域に対応します。実用的なプランナでその領域を計算するものはありません。プランナは衝突判定器に、ある1つのコンフィギュレーション、または2つのコンフィギュレーション間の短い動作が干渉しないかを問い合わせるだけです。この問い合わせが計算時間の大部分を占めます。

重要な考え方

グラフ上の探索：A*とラティス

グリッドや動作プリミティブのラティス上では、計画はグラフ探索になります。A*（Hart、Nilsson、Raphael、1968）は、それまでのコストと残りコストのヒューリスティック推定値の和が最小のノードを展開します。ヒューリスティックが決して過大評価しなければ、A* が最初に返す経路はそのグラフ上で最適です。グリッド探索は完全で再現性があるため、移動ロボットの大域プランナは今もこれに頼っています。Nav2にはNavFn、Theta*、Smacプランナが含まれており、ベース用の2Dグリッドと状態ラティスを探索します。代償はメモリです。グリッドは次元に対して指数的に大きくなります。そのため探索は (x, y, 方位) で表すベースには使えますが、7 自由度のアームでは破綻します。

サンプリングベースのプランナ：PRMとRRT

サンプリングプランナはグリッドを構築しません。確率的ロードマップ（PRM）（Kavraki et al.、1996）は、干渉しないランダムなコンフィギュレーションを多数配置します。次に、衝突判定を通過する直線動作で近傍同士を結び、そのロードマップ上のグラフ探索で問い合わせに答えます。1つの静的なシーンで多くの問い合わせを行う場合に効果を発揮します。高速探索ランダム木（RRT）（LaValle、1998）は、開始点からランダムなサンプルに向けて木を伸ばします。変化するシーンでの単発の問い合わせに向いています。RRT-Connectは両端から1本ずつ、計2本の木を伸ばし、両者が出会うまで続けます。

これらのプランナは確率的完全性を持ちます。経路が存在すれば、サンプリングを続けるにつれて、それを発見する確率は1に近づきます。素のRRTとPRMは経路の質について何も保証しません。KaramanとFrazzoli（2011）は、RRT* と PRM* という変種が最適経路に収束することを示しました。ただし衝突判定の回数は大幅に増えます。生の出力はぎくしゃくしていて必要以上に長いため、その後に必ずショートカットや平滑化の処理を行います。OMPLは、PRM、RRT、EST、KPIECE、FMTの各ファミリーなどを1つのインターフェースの背後に実装しています。

軌道最適化

最適化ベースのプランナは初期推定から始めます。初期推定は多くの場合、関節空間での直線です。プランナは、経路長や速度の二乗などのコストを最小化しながら、それを障害物から押しのけます。CHOMP（Zucker et al.、2013）は符号付き距離場の勾配に従います。TrajOpt（Schulman et al.、2014）は凸問題の列を解き、各ステップで衝突制約を線形化します。STOMPはノイズで軌道に摂動を加え、良いサンプルを残します。そのため勾配を必要としません。MoveIt 2はCHOMPとSTOMPをプランナプラグインとして提供しています。Drakeは運動学的軌道最適化と凸集合グラフ（graphs of convex sets）を提供しています。最適化手法は高速で滑らかな動作を生みますが、局所的です。初期推定が悪いと、障害物を通り抜けたままの局所最小解に陥ることがあります。よく使われるのは、サンプリングプランナの経路を最適化手法の初期値にする方法です。

衝突判定と時間パラメータ化

衝突判定器は形状のペアを判定します。FCLなどのライブラリは、バウンディングボリューム階層を使って遠いペアを安価に除外し、近いペアについてだけ厳密な接触や距離を計算します。完全なCADメッシュでは判定が遅くなります。プリミティブ（ボックス、円柱、カプセル）や凸分解のほうが判定ははるかに安価です。常に接触している隣接リンクは、許容衝突行列（allowed collision matrix）に登録する必要があります。そうしないと、プランナはすべてのコンフィギュレーションを衝突状態と報告します。2つのコンフィギュレーション間の動作は離散的な点で判定します。そのため分解能が粗すぎると、薄い障害物が2つの判定点の間をすり抜けることがあります。

幾何学的な経路には時間がありません。時間パラメータ化は、すべての関節が速度・加速度の制限内に収まるように、経路に沿ってどの速さで動くかを決めます。時間最適版では、あらゆる瞬間に少なくとも1つの関節が制限に達します。TOPP-RA（Pham、Pham）は到達可能性解析によってこれを解きます。経路を離散化し、各点で小さな線形計画問題を解きます。MoveIt 2はデフォルトでTime-Optimal Trajectory Generation（TOTG）を使います。その後にRuckigを適用してジャークを制限することもできます。

どこから始めるか

まずグリッド経路計画から始めてください。占有格子上で A* を構築します。逆運動学では、目標姿勢を目標コンフィギュレーションに変換する方法を扱います。これはアームのあらゆる問い合わせの最初のステップです。差動二輪 A* プロジェクトでは、シミュレーション上の移動ロボットについて経路を計画し、追従します。アームについては、MuJoCoでアームをシミュレーションするで、衝突判定の対象となるモデルが得られます。

ツールとプラットフォーム

よくある間違い

• 逆運動学に複数の解がある状態で姿勢を目標に計画し、肘の反転を招くこと。現在のコンフィギュレーションに最も近い解を選ぶか、関節目標に対して計画してください。
• 完全なCADメッシュから衝突モデルを作ること。プリミティブか、数ミリメートルのパディングを付けた凸包を使ってください。
• グリッパを交換したのに、許容衝突行列を元のままにしておくこと。
• プランナのタイムアウトを、経路が存在しない証拠とみなすこと。サンプリングプランナは確率的完全性しか持ちません。
• 時間付けしていない経路をコントローラに送ること。または、データシートの制限値をそのまま使い、余裕なしに時間付けすること。
• 1回の実行だけでプランナを比較すること。サンプリングプランナはランダムなので、多数のシードでの成功率と時間の中央値で比較してください。

さらに深く

• Hart、Nilsson、Raphael、A Formal Basis for the Heuristic Determination of Minimum Cost Paths、1968年。
• Kavraki、Švestka、Latombe、Overmars、Probabilistic roadmaps for path planning in high-dimensional configuration spaces、1996年。
• LaValle、Rapidly-exploring random trees: A new tool for path planning、TR 98-11、Iowa State University、1998年。著者のRRT関連論文ページに掲載。
• Karaman、Frazzoli、Sampling-based Algorithms for Optimal Motion Planning、2011年。
• Zucker et al.、CHOMP: Covariant Hamiltonian optimization for motion planning、2013年。および Schulman et al.、Motion planning with sequential convex optimization and convex collision checking、2014年。
• Pham、Pham、A New Approach to Time-Optimal Path Parameterization based on Reachability Analysis、arXiv、2017年。
• LaValle、Planning Algorithms、Cambridge University Press、2006年。著者がオンラインで無料公開しています。
• MoveIt 2の時間パラメータ化チュートリアル。