複雑度の異なる環境でのパス計画

この例では、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)