この ebook では、モーション プランニングの仕組みと、さまざまな自律システムへの適用方法を学習します。また、MATLAB® と Navigation Toolbox™ で使用可能なモーション プランニング アルゴリズムと、用途に適したアルゴリズムを選択する方法についても学びます。
はじめに
車両や機械に、ドライバーや操縦者が必要な時代は終わりました。今日の自動運転車は高性能であり、車線変更や通行中の歩行者の認識、さらには縦列駐車も可能となっています。
モーション プランニングは、自動運転車、ロボット マニピュレーター、UGV、UAV などのシステムの自律化に必要な 3 要素の 1 つです。他の 2 つは知覚と制御です。
自律システムのワークフロー。
人間と同じように、自律システムは新しい環境をスキャンして自らの位置と周囲の状況を把握します。環境マップを作成した後、モーション プランニング アルゴリズムによって、障害物にぶつからずに指定の目的地にたどり着く経路を計画します。この経路に沿って次のステップに関する決定に基づき、コントローラーはアクチュエーターにコマンドを送信し、システムの移動を促します。
モーション プランニングの一般的な用途には、次のようなものがあります。
倉庫内の自律移動ロボットのナビゲーション。
ドローンによるラストワンマイル配送。
ロボット マニピュレーターによるピックアンドプレース。
モーション プランニングとは
モーション プランニングは、ロボットや車両を初期状態から目標状態に遷移させる連続した動作を算出する計算問題です。「モーション プランニング」と「パスプランニング」は同じ意味で用いられることがよくありますが、両者には大きな違いがあります。モーション プランニングは、経時的な位置の変化に伴う車両の動作を生成しますが、パスプランニングは車両の経路しか生成しません。モーション プランニングでは、自動運転車に関する次の 2 件のシナリオのように、車両の動作はその車両が経路を追従する間に変化する可能性があります。
シナリオ 1: 車は赤信号で停止する前に減速し、信号が青に変わったら進行します。これは動作の変化ですが、計画された経路ではありません。
シナリオ 1: モーション プランニング。
シナリオ 2: 車が高速道路で車線を変更します。これは経路と動作の両方の変化です。
シナリオ 2: パスプランニング。
状態空間とその他のモーション プランニングに関する重要な概念
実用に際しては、モーション プランニングが機能する上で複数の機能要素が必要です。例として、SLAM (自己位置推定と環境地図作成の同時実行) アルゴリズムを使用して生成された環境マップ、ロボットまたは車両の状態 (位置と向き) などが挙げられます。ロボットの状態の遷移によって、その動作が定義されます。ロボットに適用できる連続的な遷移は、状態空間またはコンフィギュレーション空間 (C space) といいます。コンフィギュレーション空間の例としては、自由空間 (ロボットの状態が有効とみなされる空間) と障害物空間 (ロボットの状態が無効とみなされる空間) が挙げられます。
たとえば、自動運転車では、車の位置と姿勢または向きが総合的に自動運転車の状態を表します。自動運転車の自動駐車の場合は、駐車場のマップが自由空間と障害物空間を識別し、状態空間が動作モデルを使用して定義されたすべての操縦可能な前進動作と後進動作の組み合わせを表します。
パスコスト、最適性、完全性
パスコスト
ロボットや車両が経路を探索する際の各ステップは、コストと関連付けることができます。自由空間を移動するコストは通常ゼロに設定され、障害物空間を移動するコストは無限大に設定されます。
最適性
パスプランニング アルゴリズムは、常に最適な経路を求めるものを最適と呼びます。経路が最適であるためには、初期位置から目的地までのすべての経路において、その遷移コスト (エッジコスト) の合計が最小である必要があります。
完全性
パスプランニング アルゴリズムは、限られた時間内に経路を探すことができれば完全であると言えます (経路が存在する場合)。経路が存在しない場合はレポートが返されます。
最適かつ完全なパスプランニング アルゴリズムが算出する経路は、最短ではなくても最小限のコストしか発生しません。たとえば、屋内ロボットを廊下に沿って移動させるケースでは、ロボットに廊下の中央を進ませるコストを、壁に近づかせるコストよりも少なく定義できます。このシナリオでの最適な経路は、ロボットを中央に移動させ、壁に衝突する可能性を減らすルートです。
一般的なモーション プランニングのタイプ
モーション プランニングにはさまざまな手法があります。最も一般的な手法は、次のものです。
- 探索木またはグラフの作成方法に基づく探索ベースプランニングとサンプリングベース プランニング。
- プランニングがマップ全体で行われるかサブセットで行われるかに基づくグローバル パスプランニングとローカル パスプランニング。
それぞれの手法ををさらに詳しく見ていきましょう。
探索ベースプランニング
探索ベースプランニングでは、探索可能なグラフが作成され、各車両の状態またはコンフィギュレーションがノードとして識別されます。グラフは、コストとヒューリスティックベースの手法を使用して開始ノードから目標ノードまで展開し、最短経路を探します。探索ベースプランニングは通常、離散化されたマップで実行されます。このマップはグリッドセルに分割され、状態の数は有限または可算無限です（各状態には一意の整数を割り当てることができます）。
多くの場合、離散状態空間は 2D グリッドマップで表現され、セルの中心が探索対象の状態です。一般的なマップ表現の 1 つは、占有グリッドマップです。
A* アルゴリズム は、離散グリッドマップで経路を探索する際に一般的に使用される探索ベースの手法です。
探索ベースプランニング。
グリッドマップに適用される A* アルゴリズム。
グリッドマップでの探索ベースプランニングは、多くの場合、車両やロボットを 1 つの点とみなすことができ、プランニング段階に動作モデルや運動方程式が含まれていない場合に適しています。パスプランニング アルゴリズムがロボットの追従するウェイポイントを算出した後、制御アルゴリズムを使用して運動学的拘束を考慮することができます。
サンプリングベース プランニング
サンプリングベース プランニングでは、状態空間にノードをランダムに追加することで、探索木またはロードマップが作成されます。想定される衝突を回避する経路は、連続動作モデルを使用して検出されます。多くの場合、サンプリングベース プランニングでは、ヒューリスティックを使用して探索空間を探索し、探索方向にバイアスをかけます。作成後、探索木やロードマップは、衝突チェック方法や探索方法を使用して、目標位置への最短経路を探索します。
RRT アルゴリズムは、連続した状態空間で経路を探索する際に一般的に使用される探索ベースの手法です。
探索ベースプランニング。
サンプリングベースのモーション プランニングは、ロボットアームでオブジェクトをピックアップするための有効なコンフィギュレーション セットを探索する場合などに使用される、高次元の探索空間に適しています。サンプリングベース プランニングは実用的な幅広い用途に適しているため、完全なソリューションは提供されていないものの、広く利用されています。探索木の密度によりサンプルを十分に接近させると、解 (存在する場合に) が得られる確率は 1 に収束します。これにより、RRT や RRT* などのサンプリングベース プランナーが確率的に完了します。
グローバル パス プランニングとローカル パス プランニング
グローバル パス プランニング (またはマップベース プランニング) では、環境に関する 先験的な知識に基づいて最適な経路を探索します。グローバル プランニング アルゴリズムは、環境内の既知の静的障害物を回避する初期経路を計画します。たとえば、自律移動ロボットは、壁などの静的障害物がある廊下を通ってあるオフィスから別のオフィスに本を配送するグローバルな経路を計画できます。
ローカル パス プランニング (または動的再プランニング) は、経路を再計算して、未知の動的障害物を回避します。ローカル プランニング アルゴリズムは、新たに発生した障害物を回避しながら、グローバルプランを追跡し、ローカル軌道を作成します。たとえば、自動運転車は、他の車両を回避するために車線を変更し、目的地に到達するためにグローバル経路に合流するようにローカル軌道を計画できます。
自動運転車が車線変更を行うためのローカル パス プランニング。
MATLAB を使用した 4 ステップのモーション プランニング ワークフロー
Navigation Toolbox には、A* などの一般的な探索ベース プランナーや、RRT/RRT* などのサンプリングベース プランナーを含む、さまざまなプランニング アルゴリズムを実装するためのクラスが用意されています。また、障害物クリアランスと経路平滑化について計画されたパスを評価するパスメトリクスも用意されています。
パスメトリクス: 平滑化。
パスメトリクス: クリアランス。
さらに、Navigation Toolbox には、次の体系的な 4 ステップのワークフローでサンプリングベースのモーション プランニング アルゴリズムを実装できるインターフェイスが用意されています。
- 状態空間を表す。
- 状態バリデーターを定義する。
- 新しい状態をサンプリングし、有効性を確認する。
- 有効な状態の集合を経路として表す。
状態空間を表す
カスタム状態空間クラスの nav.StateSpace は、任意の用途で使用する可能性のある状態またはコンフィギュレーションを含む状態空間を定義できます。たとえば、stateSpaceDubins と stateSpaceReedsShepp 状態空間は、自動車型ロボットまたはアッカーマン ステアリングを備えたロボットの動きを模倣するように状態空間内の任意の 2 つの状態を接続することで、自動駐車用のプランニングをサポートします。
Navigation Toolbox には、次のようなすぐに使用できる状態空間が用意されています。
| 状態空間 | コンフィギュレーション | 環境 | 用途 |
|---|---|---|---|
stateSpaceSE2 |
(x, y, θ) | 2D | ホロノミックロボット |
stateSpaceSE3 |
(x, y, z, qw, qx, qy, qz) | 3D | ロボット マニピュレーター |
stateSpaceDubins |
(x, y, θ) | 2D | 最小回転半径の非ホロノミック車両 |
stateSpaceReedsShepp |
(x, y, θ) | 2D | 最小回転半径の非ホロノミック車両 |
SE2 状態空間。
Dubins 状態空間。
状態バリデーターの定義
状態バリデーターは、状態空間に基づいており、SLAM アルゴリズムから取得したマップに対応しています。状態や、2 つのサンプリングされた状態間の動作の有効性を確認します。たとえば、衝突チェッカーは、ロボットの状態やコンフィギュレーションが障害物と衝突したことを示す状態バリデーターの一種です。
Navigation Toolbox には、2D および 3D 占有マップの状態と離散化された動作を検証するための次の状態バリデーターが用意されています。
| 状態バリデーター | タイプ | 用途 |
|---|---|---|
validatorOccupancyMap |
occupancyMap、binaryOccupancyMap |
2D 占有マップ |
validatorVehicleCostmap |
vehicleCostmap |
2D 占有マップ |
validatorOccupancyMap3D |
occupancyMap3D |
3D 占有マップ |
上記の状態バリデーターは、このツールボックスで使用可能なカスタムの状態バリデーターである nav.StateValidator から派生しています。この状態バリデーターを使用すると、状態の有効性や、任意の 2 つの状態間の動作を判定できます。
新しい状態のサンプリングと有効性の確認
サンプリングベースのプランニング アルゴリズムは、定義された状態空間内の状態をランダムにサンプリングし、状態バリデーターを使用して開始位置から目標位置まで障害物のない経路を作成します。RRT や PRM などのアルゴリズムは、異なるサンプリングスキームを使用して状態をサンプリングし、探索木やロードマップを作成します。
指定されたマップ (SLAM アルゴリズムから取得) 内の状態をサンプリングするには、マップの外側限界に対応する状態空間の範囲が適用されます。
サンプリングされた状態の集合の表現
Navigation Toolbox の関数 plan を使用して、プランニング アルゴリズムの出力を木のデータ構造に統合できます。クラス navPath を使用して、状態の集合を特定の状態空間に格納し、格納した集合を補間して経路を取得できます。
モーション プランニング アルゴリズムの選択
次のモーション プランニング アルゴリズムは、Navigation Toolbox で使用できます。
| アルゴリズム | タイプと範囲 | メリット |
|---|---|---|
| グリッドベース A* | グローバル プランニング |
|
| ハイブリッド A* | グローバル プランニング |
|
| 確率的ロードマップ (PRM) | グローバル プランニング |
|
| RRT (Rapidly Exploring Random Tree) | グローバル プランニング |
|
| RRT* | グローバル プランニング |
|
| フレネ最適軌道 | ローカル軌道ジェネレーター |
|
グリッドベース A*
A* アルゴリズムは、開始ノードと目標ノードを接続する重み付きグラフを作成することでグリッドマップを操作できる離散パスプランナーです。A* は、グリッドマップで機能し、xy 線形接続を使用して探索木を展開します。ノードは、ノード間を移動するためのコストを推定するコスト関数に基づいて 1 つずつ探索されます。
仕組み
コストが最小の経路を探索するため、A* はコスト関数 f(n) を最小化するよう機能します。
f(n) = g(n) + h(n)
ここでは、次を表します。
- n は経路上の次のノード
- g(n) は、開始ノードから n までの経路のコスト
- h(n) は、n から目標までの最もコストが低い経路のコストを推定するヒューリスティック関数
A* アルゴリズム。
MATLAB の A* アルゴリズム
Navigation Toolbox の plannerAStarGrid には、ユークリッドやマンハッタンなど、よく使用されるヒューリスティック関数が用意されています。h(n) と g(n) には、事前定義されたコスト関数またはカスタムのコスト関数のいずれかを使用できます。binaryOccupancyMap または occupancyMap オブジェクトのいずれかをプランナーへの入力として指定できます。
用途
A* アルゴリズムは、オムニドライブ車輪付きロボットに直接適用できます。A* は、ビデオゲームの経路探索など、コンピューターベースのアプリケーションでもよく利用されています。
ハイブリッド A*
ハイブリッド A* は A* を拡張したアルゴリズムです。A* と同様に、ハイブリッド A* は離散探索空間で機能しますが、各グリッドセルを車両の連続 3D 状態 (x、y、θ) に関連付けます。モーション プリミティブで構成される連続状態空間を使用して、スムーズに走行可能な経路を生成します。
仕組み
ハイブリッド A* は、探索木を目標の方向に展開する効率的なガイド ヒューリスティック関数を使用します。また、経路の分析的拡張を使用して、精度を向上させ、プランニング時間を短縮します。
ハイブリッド A は、提供された正確な目標姿勢に基づいて、車両がスムーズに走行可能な経路を生成します。
開始ノード、S = (1.5, 2.5, pi/2)
目標ノード 1、G1 = (5.5, 5.5, 0)
目標ノード 2、G2 = (5.5, 5.5, -pi/2)
MATLAB のハイブリッド A*
Navigation Toolbox には、plannerHybridAStar が用意されています。このオブジェクトでは、validatorOccupancyMap や validatorVechicleCostmap などの状態バリデーターを使用して占有マップで衝突検出を行い、用途に特化したコストと調整のためのパラメーターを含めることができます。
用途
従来の A* とは異なり、ハイブリッド A* は、自動運転車など、非ホロノミックな拘束のある車両に適しています。運動学的な実現可能性を保証し、車両の向きと速度などの微分拘束を考慮に入れます。
自動駐車向けハイブリッド A*。
PRM
探索ベースのアルゴリズムは、自由度の高いアプリケーションや、マップのサイズが非常に大きいアプリケーションには適していません。大きなグリッドマップからデータを格納するには、大量の計算が必要となります。PRM は、このような場合に役立つサンプリングベースのプランニング アルゴリズムです。
仕組み
PRM は、マップ内で想定されるさまざまな経路を示すネットワークグラフです。グラフは、特定の領域内の限られた数のランダムな点またはノードで生成されます。各ノードをランダムにサンプリングした後に、PRM アルゴリズムは、固定半径内のノードをすべて接続することでノードの複数のクラスターを作成します。
ロードマップが作成されると、マップ上の特定の開始位置から特定の目標位置までの経路を探索できます。PRM では、同じロードマップでそれぞれ別の開始位置と目標位置に対して複数のクエリを実行できるため、マップが静的である (経時的に変化しない) 場合、計算時間を短縮できます。PRM は、A* プランナーなどのグラフ探索アルゴリズムを使用して、作成したロードマップ内の経路を検索します。
PRM プランナー。
MATLAB の PRM
Robotics System Toolbox™ に用意されている mobileRobotPRM は、binaryOccupancyMap を入力として使用し、ランダムにサンプリングされたノードを接続することでマップの自由空間にロードマップを作成します。関数 findpath を使用すると、開始位置から目標位置までのパスを探索できます。PRM は障害物のない経路を探索する際に車両の寸法を考慮しないため、関数 inflate を使用して車両の半径でマップを拡張します。
用途
MATLAB での PRM 実装は、未知の障害物がない静的な倉庫環境など、ロボットがマップ全体を再学習せずに開始位置と目標位置を変更することが望ましいホロノミックなモバイル ロボット アプリケーションに適しています。
PRM プランナー。
RRT
RT は、非ホロノミックな拘束に適したサンプリングベース アルゴリズムです。RRT アルゴリズムは、非凸の高次元空間を効率的に探索します。定義された状態空間でランダムなサンプルを使用して、探索木を段階的に作成します。探索木は最終的に探索空間にまたがり、開始状態を目標状態に接続します。
仕組み
RRT プランナーは、次の手順に従って開始状態 Xstart をルートとする探索木を成長させます。
- プランナーが状態空間でランダムな状態 Xrand をサンプリングします。
- プランナーが状態 Xnear を探索します。これは探索木にすでにある状態で、Xrand に最も接近しています（状態空間の距離定義に基づく）。
- プランナーが状態 Xnew に達するまで Xnear から Xrand に向かって拡張します。
- 新しい状態 Xnew が探索木に追加されます。
このプロセスは、探索木が Xgoal に達するまで繰り返されます。新しいノード Xnew がサンプリングされるたびに、他のノードとの接続が衝突していないか確認されます。Xstart から Xgoal への走行可能な経路を実現するには、Reeds-Shepp などのモーション プリミティブや動作モデルを使用できます。
MATLAB の RRT
Navigation Toolbox には、第 3 章で説明されているカスタマイズ可能なサンプリングベース プランニングのインターフェイスに従う plannerRRT が用意されています。
双方向 RRT (biRRT) は、RRT の変形であり、開始状態と目標状態の両方から同時に探索を開始する 2 つの探索木を作成します。biRRT は、高次元空間で探索速度を向上させるため、ロボット マニピュレーターに使用すると有効です。Robotics System Toolbox では、manipulatorRRT として利用可能です。
用途
RRT は、障害物、ロボット マニピュレーターなどの自由度の高いロボットの微分拘束、モバイルロボットの経路計画など、パスプランニングの問題に特に適しています。RRT によるプランニングでは経路が急に曲がる可能性がありますが、パス平滑化アルゴリズムを使用すると、こうした不規則性を補正できます。
PRM プランナー。
RRT*
RRT アルゴリズムは有効な経路を算出しますが、最短経路とは限りません。RRT* は、RRT アルゴリズムを最適化したものです。実際には実行不可能であるものの、理論的には、ノードの数が無限に近づくと、RRT* アルゴリズムは目標までの最短経路を算出できます。
仕組み
RRT* の基本原理は RRT と同じですが、2 つの重要な機能が追加されており、大幅に異なる結果を生成できます。
- RRT* には、親ノードからの距離で定義される各ノードのコストが含まれています。常に、Xnew 近傍の固定半径内で最低コストのノードを探索します。
- RRT* は、ノードのコストの減少を調べ、探索木を再結線して、より短く平滑な経路を取得します。
RRT* プランナー。
MATLAB の RRT*
Navigation Toolbox の plannerRRTStar は、幾何学的プランニング問題の解決に適しています。
用途
RRT* は漸近的に最適解を提供するため、ロボットマニピュレーターなど、高次元の問題に特に有効です。また、多くの障害物が存在する密集した環境でも有効です。RRT* は最も少ないノードで最短経路を探索しますが、非ホロノミック車両には適していません。比較すると、RRT は非ホロノミック車両に使用でき、微分拘束を処理できます。
RRT* と MPC 追跡コントローラーを使用した縦列駐車。
フレネ最適軌道
フレネ最適軌道 は、グローバル参照パスに基づいて軌道をプランニングするローカルプランナーです。軌道は、状態に時間関数である変数を含む、一連の状態です。軌道計画は、速度が考慮される状況で有効です。
仕組み
ローカルプランナーとして、フレネ最適軌道には、一連の [xyθ] ウェイポイントとして提供されるグローバル参照パスが必要です。高速道路などの曲線的で連続的な参照パスに沿ったプランニングでは、走行距離と参照パスからの横方向の距離で構成されるフレネ座標を使用します。
フレネ最適軌道は、参照パスから横方向の距離だけ若干ずれている、初期状態からの代替軌道をサンプリングします。参照のフレネフレームと、次の 2 種類の状態を使用します。
- 直交状態: \([𝑥\,𝑦\, 𝜃\, 𝜅\, 𝑥̇ \,𝑥̈] \)
- Frenet 状態: \([𝑠\,𝑠̇ \,𝑠̈\, 𝑙 \,𝑙̇ \,𝑙̈] 𝑠=𝑙𝑜𝑛𝑔𝑖𝑡𝑢𝑑𝑒𝑎𝑙𝑜𝑛𝑔𝑅𝑒𝑓𝑃𝑎𝑡ℎ, 𝑙=𝑙𝑎𝑡𝑒𝑟𝑎𝑙𝐷𝑒𝑣𝑖𝑎𝑡𝑖𝑜𝑛𝑤𝑟𝑡𝑅𝑒𝑓𝑃𝑎𝑡ℎ\)
初期状態は、5 次多項式を介してサンプリングされた最終状態に接続されます。これにより、急な加速が最小限に抑えられ、状態に沿った連続性が保証されます。サンプリングされた軌道では、運動学的な実現可能性、衝突、コストが評価されます。
高速道路走行の軌道計画。
MATLAB のフレネ最適軌道
Navigation Toolbox の trajectoryOptimalFrenet は、ハイブリッド A* や RRT などのグローバルプランナーによって参照ウェイポイントが生成される参照パスに沿って、最適な軌道を探索します。trajectoryOptimalFrenet は複数の代替経路を生成し、参照パスと最終状態の偏差、経路の平滑性、時間、距離に基づいて、コストを評価します。validatorOccupancyMap 状態バリデーターを使用して、状態の有効性を確認します。
用途
フレネ最適軌道は、グローバルプランナーと車両またはロボットコントローラーの間のローカルプランナーとして使用できます。車線変更操作や車間距離制御 (速度維持を含む) などの高速道路走行用途に利用できます。また、動的な再プランニング用に、モバイルロボットやその他のアッカーマンタイプの車両で使用することもできます。
また、以下のリストから Web サイトを選択することもできます。
南北アメリカ
- América Latina (Español)
- Canada (English)
- United States (English)
ヨーロッパ
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)
アジア太平洋地域
- Australia (English)
- India (English)
- New Zealand (English)
- 中国
- 日本Japanese (日本語)
- 한국Korean (한국어)