複雑度の異なる環境でのパス計画
この例では、Probabilistic Roadmap (PRM) パス プランナーを使用して、指定されたマップ上の 2 つの位置間で障害物のないパスを計算する方法を説明します。PRM パス プランナーは、フリー スペース内でランダムにサンプリングされたノードを使用し、それらを相互に接続することにより、指定されたマップのフリー スペースにロードマップを構築します。ロードマップが構築されたら、マップ上の指定した開始位置から指定した終了位置へのパスをクエリできます。
この例では、マップはインポートされたデータを使用して占有グリッド マップとして表現されます。マップのフリー スペースでノードをサンプリングするとき、PRM はこのバイナリ占有グリッド表現を使用してフリー スペースを推定します。さらに、マップ上で障害物のないパスを計算するとき、PRM はロボットの寸法を考慮しません。したがって、ロボットのサイズを考慮して実際のロボットの衝突を確実に回避する障害物のないパスを計算できるように、ロボットの寸法に応じてマップをインフレートする必要があります。マップ上で開始位置と終了位置を定義して、PRM パス プランナーが障害物のないパスを見つけられるようにします。
パスを計画するためのサンプル マップのインポート
load exampleMaps.mat
インポートされるマップは、simpleMap
、complexMap
および ternaryMap
です。
whos *Map*
Name Size Bytes Class Attributes complexMap 41x52 2132 logical emptyMap 26x27 702 logical simpleMap 26x27 702 logical ternaryMap 501x501 2008008 double
インポートされた simpleMap
データを使用し、binaryOccupancyMap
オブジェクトを使用して占有グリッド表現を構築します。このマップでは、分解能を 1 メートルあたり 2 セルに設定します。
map = binaryOccupancyMap(simpleMap,2);
関数 show
を binaryOccupancyMap
オブジェクトに使用してマップを表示します。
show(map)
ロボットの寸法の定義とマップのインフレート
ロボットが障害物に衝突しないことを確実にするために、ロボットの寸法に応じてマップをインフレートしてから PRM パス プランナーに提供する必要があります。
ここでは、ロボットの寸法は半径 0.2 メートルの円と仮定します。関数inflate
を使用して、この寸法に応じてマップをインフレートできます。
robotRadius = 0.2;
前述したように、PRM はロボットの寸法を考慮しません。したがって、インフレートされたマップを PRM に提供することにより、ロボットの寸法を考慮に入れるのです。元のマップを保持するために、関数 inflate
を使用する前にマップのコピーを作成します。
mapInflated = copy(map); inflate(mapInflated,robotRadius);
インフレートされたマップを表示します。
show(mapInflated)
PRM の構築とパラメーターの設定
ここで、パス プランナーを定義する必要があります。mobileRobotPRM
オブジェクトを作成して関連する属性を定義します。
prm = mobileRobotPRM;
インフレートされたマップを PRM オブジェクトに割り当てます。
prm.Map = mapInflated;
PRM 構築時に使用する PRM ノードの数を定義します。PRM は、指定されたマップ上で指定された数のノードを使用してロードマップを構築します。入力マップの寸法と複雑度に基づいて、マップ上の 2 点の間の解を得るためには、これは最初に調整するべき属性の 1 つです。ノードの数が多いと密なロードマップが作成され、パスが見つかる確率が高くなります。ただし、ノード数が多いと、ロードマップ作成の際と解を求める際の両方で計算時間が長くなります。
prm.NumNodes = 50;
マップ上の 2 つの接続されているノード間での最大許容距離を定義します。PRM は、マップ上で互いの距離がこれ以下であるすべてのノードを接続します。入力マップが大きい場合や複雑な場合は、この属性も調整が必要です。接続距離が大きいと、ノード間の接続が増えてパスを簡単に見つけることができますが、ロードマップ作成の計算時間が長くなる可能性があります。
prm.ConnectionDistance = 5;
構築した PRM で実行可能なパスを検出
パス プランナーが使用する開始位置と終了位置をマップ上で定義します。
startLocation = [2 1]; endLocation = [12 10];
関数findpath
を使用して、開始位置と終了位置の間のパスを探索します。解は、開始位置から終了位置への一連のウェイポイントです。PRM アルゴリズムの確率的性質により、path
が異なることに注意してください。
path = findpath(prm, startLocation, endLocation)
path = 7×2
2.0000 1.0000
1.9569 1.0546
1.8369 2.3856
3.2389 6.6106
7.8260 8.1330
11.4632 10.5857
12.0000 10.0000
PRM の解を表示します。
show(prm)
大規模で複雑なマップへの PRM の使用
大規模で複雑なフロア プランを表す、インポートされた complexMap
データを使用して、指定された分解能 (1 メートルあたり 1 セル) のバイナリ占有グリッド表現を構築します。
map = binaryOccupancyMap(complexMap,1);
マップを表示します。
show(map)
ロボットの寸法に基づいたマップのインフレート
障害物回避のため、マップをコピーしてインフレートしてロボットのサイズを考慮に入れます。
mapInflated = copy(map); inflate(mapInflated, robotRadius);
インフレートされたマップを表示します。
show(mapInflated)
既存の PRM オブジェクトと新しいマップの関連付けとパラメーターの設定
新しくインフレートされたマップで PRM オブジェクトを更新し、その他の属性を定義します。
prm.Map = mapInflated;
NumNodes
プロパティと ConnectionDistance
プロパティを設定します。
prm.NumNodes = 20; prm.ConnectionDistance = 15;
PRM グラフを表示します。
show(prm)
構築した PRM で実行可能なパスを検出
マップ上で開始位置と終了位置を定義して、障害物のないパスを見つけます。
startLocation = [3 3]; endLocation = [45 35];
開始位置と終了位置の間で解を検索します。複雑なマップの場合は、指定した数のノードに対して実行可能なパスがない場合があります (空のパスが返されます)。
path = findpath(prm, startLocation, endLocation);
大規模で複雑なマップ上でパスを計画しているため、より多くのノードが必要な場合があります。ただし、多くの場合、ノードがいくつあれば十分かは明らかではありません。開始位置と終了位置の間に実行可能なパスを確保できるように、ノードの数を調整します。
while isempty(path) % No feasible path found yet, increase the number of nodes prm.NumNodes = prm.NumNodes + 10; % Use the |update| function to re-create the PRM roadmap with the changed % attribute update(prm); % Search for a feasible path with the updated PRM path = findpath(prm, startLocation, endLocation); end
パスを表示します。
path
path = 12×2
3.0000 3.0000
4.2287 4.2628
7.7686 5.6520
6.8570 8.2389
19.5613 8.4030
33.1838 8.7614
31.3248 16.3874
41.3317 17.5090
48.3017 25.8527
49.4926 36.8804
⋮
PRM の解を表示します。
show(prm)