plan

2 つの状態間のパスの計画

構文

path = plan(planner,startState,goalState)

[path,solutionInfo] = plan(planner,startState,goalState)

説明

path = plan(planner,startState,goalState) は、開始状態からゴール状態までの path を返します。

[path,solutionInfo] = plan(planner,startState,goalState) は、パス プランニングの解法情報が含まれた solutionInfo も返します。

例

2 つの状態間のパスの計画

状態空間を作成します。

ss = stateSpaceSE2;

作成された状態空間を使用して、occupancyMap ベースの状態バリデーターを作成します。

sv = validatorOccupancyMap(ss);

例のマップから占有マップを作成し、マップの分解能を 1 メートルあたり 10 セルとして設定します。

load exampleMaps
map = occupancyMap(simpleMap,10);
sv.Map = map;

バリデーターの検証距離を設定します。

sv.ValidationDistance = 0.01;

状態空間の範囲を更新してマップ制限と同じにします。

ss.StateBounds = [map.XWorldLimits;map.YWorldLimits;[-pi pi]];

パス プランナーを作成し、最大接続距離を増やします。

planner = plannerRRT(ss,sv,MaxConnectionDistance=0.3);

開始状態とゴール状態を設定します。

start = [0.5 0.5 0];
goal = [2.5 0.2 0];

既定の設定を使用してパスを計画します。

rng(100,'twister'); % for repeatable result
[pthObj,solnInfo] = plan(planner,start,goal);

結果を可視化します。

show(map)
hold on
% Tree expansion
plot(solnInfo.TreeData(:,1),solnInfo.TreeData(:,2),'.-')
% Draw path
plot(pthObj.States(:,1),pthObj.States(:,2),'r-','LineWidth',2)

RRT プランナーを使用した 3 次元占有マップを通るパスの計画

都市ブロックの 3 次元占有マップをワークスペースに読み込みます。セルを障害物なしとして見なすしきい値を指定します。

mapData = load("dMapCityBlock.mat");
omap = mapData.omap;
omap.FreeThreshold = 0.5;

占有マップをインフレートして、障害物の周囲の安全な動作のためにバッファー ゾーンを追加します。

inflate(omap,1)

状態変数の範囲を指定して SE(3) 状態空間オブジェクトを作成します。

ss = stateSpaceSE3([0 220;0 220;0 100;inf inf;inf inf;inf inf;inf inf]);

作成された状態空間を使用して、3 次元占有マップの状態バリデーターを作成します。占有マップを状態バリデーター オブジェクトに割り当てます。サンプリング距離間隔を指定します。

sv = validatorOccupancyMap3D(ss, ...
     Map = omap, ...
     ValidationDistance = 0.1);

最大接続距離を増やし、最大反復回数を減らして、RRT パス プランナーを作成します。ターゲットまでのユークリッド距離がしきい値の 1 メートルを下回っている場合にパスがゴールに到達していると判定するカスタムのゴール関数を指定します。

planner = plannerRRT(ss,sv, ...
          MaxConnectionDistance = 50, ...
          MaxIterations = 1000, ...
          GoalReachedFcn = @(~,s,g)(norm(s(1:3)-g(1:3))<1), ...
          GoalBias = 0.1);

開始姿勢とゴール姿勢を指定します。

start = [40 180 25 0.7 0.2 0 0.1];
goal = [150 33 35 0.3 0 0.1 0.6];

結果を再現できるように乱数発生器を構成します。

rng(1,"twister");

パスを計画します。

[pthObj,solnInfo] = plan(planner,start,goal);

計画されたパスを可視化します。

show(omap)
axis equal
view([-10 55])
hold on
% Start state
scatter3(start(1,1),start(1,2),start(1,3),"g","filled")
% Goal state
scatter3(goal(1,1),goal(1,2),goal(1,3),"r","filled")
% Path
plot3(pthObj.States(:,1),pthObj.States(:,2),pthObj.States(:,3), ...
      "r-",LineWidth=2)

入力引数

planner — パス プランナー

plannerRRT オブジェクト | plannerRRTStar オブジェクト

パス プランナー。plannerRRT オブジェクトまたは plannerRRTStar オブジェクトとして指定します。

startState — パスの開始状態

N 要素の実数値ベクトル

パスの開始状態。N 要素の実数値ベクトルとして指定します。N は状態空間の次元です。

例: [1 1 pi/6]

例: [40 180 25 0.7 0.2 0 0.1]

データ型: single | double

goalState — パスのゴール状態

N 要素の実数値ベクトル

パスのゴール状態。N 要素の実数値ベクトルとして指定します。N は状態空間の次元です。

例: [2 2 pi/3]

例: [150 33 35 0.3 0 0.1 0.6]

データ型: single | double

出力引数

path — 計画されたパス情報を保持するオブジェクト

navPath オブジェクト

計画されたパス情報を保持するオブジェクト。navPath オブジェクトとして返されます。

solutionInfo — 解法情報

構造体

解法情報。構造体として返されます。構造体のフィールドは以下のとおりです。

solutionInfo のフィールド

フィールド	説明
IsPathFound	パスが見つかったかどうかを示します。パスが見つかった場合は、1 を返します。それ以外の場合は、0 を返します。
ExitFlag	プランナーの終了ステータスを示します。以下のように返されます。 1 — ゴールに達した場合 2 — 最大反復回数に達した場合 3 — ノードの最大数に達した場合
NumNodes	プランナーが終了したときの探索木内のノード数 (ルート ノードを除く)。
NumIterations	実行された "延長" ルーチンの数。
TreeData	プランナーが終了したときの探索木のステータスを反映した、探索された状態のコレクション。各個別エッジを区切るために NaN 値が区切り文字として挿入されます。
PathCosts	各反復におけるパスのコストが含まれます。パスがゴールに達していないときの反復の値は、NaN で示されます。配列のサイズは NumIterations 行 1 列です。最後の要素には、最終パスのコストが含まれます。 メモ このフィールドは、plannerRRTStar オブジェクトの場合にのみ適用されます。

データ型: structure

拡張機能

C/C++ コード生成
MATLAB® Coder™ を使用して C および C++ コードを生成します。

バージョン履歴

R2019b で導入

参考

オブジェクト

• navPath | plannerRRT | plannerRRTStar

関数

• copy | shortenpath