ドキュメンテーション

このページは前リリースの情報です。該当の英語のページはこのリリースで削除されています。

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

はじめに

この例では、Probabilistic Roadmap (PRM) パス プランナーを使用して、特定のマップ上の 2 つの位置の間で障害物のないパスを計算する方法を説明します。PRM パス プランナーは、フリー スペース内でランダムにサンプリングされたノードを使用し、それらを相互に接続することにより、指定されたマップのフリー スペースにロードマップを構築します。ロードマップが構築されたら、マップ上の指定した開始位置から指定した終了位置へのパスをクエリできます。

この例では、マップはインポートされたデータを使用して占有グリッド マップとして表現されます。マップのフリー スペースでノードをサンプリングするとき、PRM はこのバイナリ占有グリッド表現を使用してフリー スペースを推定します。さらに、マップ上で障害物のないパスを計算するとき、PRM はロボットの寸法を考慮しません。したがって、ロボットのサイズを考慮して実際のロボットの衝突を確実に回避する障害物のないパスを計算できるように、ロボットの寸法に応じてマップをインフレートする必要があります。マップで開始位置と終了位置を定義して、PRM パス プランナーが障害物のないパスを見つけられるようにします。

パスを計画するためのサンプル マップのインポート

filePath = fullfile(fileparts(which('PathPlanningExample')),'data','exampleMaps.mat');
load(filePath)

インポートされるマップは、simpleMapcomplexMap および ternaryMap です。以下により、変数名に文字列 'Map' が含まれる変数を検索します。

whos *Map*
  Name              Size               Bytes  Class      Attributes

  complexMap       41x52                2132  logical              
  simpleMap        26x27                 702  logical              
  ternaryMap      501x501            2008008  double               

インポートされた simpleMap データを使用し、robotics.BinaryOccupancyGrid クラスを使用して占有グリッド表現を構築します。このマップでは、分解能を 1 メートルあたり 2 セルに設定します。

map = robotics.BinaryOccupancyGrid(simpleMap, 2)
map = 

  BinaryOccupancyGrid with properties:

               GridSize: [26 27]
             Resolution: 2
           XWorldLimits: [0 13.5000]
           YWorldLimits: [0 13]
    GridLocationInWorld: [0 0]

関数 showrobotics.BinaryOccupancyGrid オブジェクトに使用してマップを表示します。

show(map)

ロボットの寸法の定義とマップのインフレート

ロボットが障害物に衝突しないことを確実にするために、ロボットの寸法に応じてマップをインフレートしてから PRM パス プランナーに提供する必要があります。

ここでは、ロボットの寸法は半径 0.2 メートルの円と仮定します。関数 inflate を使用して、この寸法に応じてマップをインフレートできます。

robotRadius = 0.2;

前述したように、PRM はロボットの寸法を考慮しません。したがって、インフレートされたマップを PRM に提供することにより、ロボットの寸法を考慮に入れるのです。元のマップを保持するために、関数 inflate を使用する前にマップのコピーを作成します。

mapInflated = copy(map);
inflate(mapInflated,robotRadius);

インフレートされたマップを表示します。

show(mapInflated)

PRM の構築とパラメーターの設定

ここで、パス プランナーを定義する必要があります。robotics.PRM オブジェクトを作成して関連する属性を定義します。

prm = robotics.PRM
prm = 

  PRM with properties:

                   Map: [0x1 robotics.BinaryOccupancyGrid]
              NumNodes: 50
    ConnectionDistance: Inf

インフレートされたマップを 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 =

    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 = robotics.BinaryOccupancyGrid(complexMap, 1)
map = 

  BinaryOccupancyGrid with properties:

               GridSize: [41 52]
             Resolution: 1
           XWorldLimits: [0 52]
           YWorldLimits: [0 41]
    GridLocationInWorld: [0 0]

マップを表示します。

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)
path =

     []

大規模で複雑なマップ上でパスを計画しているため、より多くのノードが必要な場合があります。ただし、多くの場合、ノードがいくつあれば十分かは明らかではありません。開始位置と終了位置の間に実行可能なパスを確保できるように、ノードの数を調整します。

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
% Display path
path
path =

    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
   44.3936   34.8592
   45.0000   35.0000

PRM の解を表示します。

show(prm)

確率的占有グリッドでの PRM の使用

インポートされた ternaryMap データを使用して robotics.OccupancyGrid オブジェクトを構築します。ternaryMap は、確率を使用して環境を表現します。占有の確率は、フリー スペースの場合は 0、占有スペースの場合は 1、不明なスペースの場合は 0.5 です。ここでは、1 メートルあたり 20 セルの分解能を使用します。

map = robotics.OccupancyGrid(ternaryMap, 20)
map = 

  OccupancyGrid with properties:

        OccupiedThreshold: 0.6500
            FreeThreshold: 0.2000
    ProbabilitySaturation: [0.0010 0.9990]
                 GridSize: [501 501]
               Resolution: 20
             XWorldLimits: [0 25.0500]
             YWorldLimits: [0 25.0500]
      GridLocationInWorld: [0 0]

マップを表示します。

show(map)

ロボットの寸法に基づいたマップのインフレート

障害物回避のため、マップをコピーしてインフレートしてロボットのサイズを考慮に入れます。

mapInflated = copy(map);
inflate(mapInflated, robotRadius);

インフレートされたマップを表示します。

show(mapInflated)

既存の PRM オブジェクトと新しいマップの関連付けとパラメーターの設定

新しくインフレートされたマップで PRM オブジェクトを更新し、その他の属性を定義します。PRM は、OccupancyGrid オブジェクトで FreeThreshold を使用して障害物のないスペースを特定し、この障害物のないスペース内でパスを計算します。ternaryMap 内の不明なセルの値は 0.5 で、OccupancyGrid オブジェクト mapInflated 上の既定の FreeThreshold は 0.2 です。その結果、PRM は、不明な領域ではパスを計画しません。

prm.Map = mapInflated;

NumNodes プロパティと ConnectionDistance プロパティを設定します。

prm.NumNodes = 60;
prm.ConnectionDistance = 5;

% Display PRM graph
show(prm)

構築した PRM で実行可能なパスを検出

マップ上で開始位置と終了位置を定義して、障害物のないパスを見つけます。

startLocation = [7 22];
endLocation = [15 5];

% Search for a solution between start and end location.
path = findpath(prm, startLocation, endLocation);
while isempty(path)
    prm.NumNodes = prm.NumNodes + 10;
    update(prm);
    path = findpath(prm, startLocation, endLocation);
end

% Display path
path
path =

    7.0000   22.0000
    7.9059   22.1722
   10.6881   22.3734
   11.7508   19.3716
   13.7982   17.1659
   17.5826   14.6769
   16.8227    9.9964
   15.1329    8.3100
   14.9489    5.7648
   15.0000    5.0000

PRM の解を表示します。

show(prm)

参考