Main Content

plannerRRT

幾何学的プランニングのための RRT プランナーの作成

R2019b 以降

このページをすべて展開する

説明

plannerRRT オブジェクトは、幾何学的プランニング問題を解決するための Rapidly-exploring Random Tree (RRT) プランナーを作成します。RRT は、特定の状態空間からランダムに取得されたサンプルから段階的に探索木を作成するツリーベースのモーション プランナーです。ツリーは最終的に探索空間に広がり、開始状態からゴール状態まで接続します。一般的なツリーの成長過程は次のとおりです。

  1. プランナーが状態空間でランダムな状態 xrand をサンプリングします。

  2. プランナーが状態 xnear を探します。これは探索木に既にある状態で、xrand に最も近いもの (状態空間の距離定義に基づく) になります。

  3. プランナーが xnear から xrand に向かって状態 xnew に到達するまで展開します。

  4. その後、新しい状態 xnew が探索木に追加されます。

幾何学的 RRT の場合、プランナー オブジェクトの状態空間で指定された拘束に違反せずに、2 つの状態間の展開と接続を解析的に見つけることができます。

作成

構文

planner = plannerRRT(stateSpace,stateVal)
planner = plannerRRT(___,Name=Value)

説明

planner = plannerRRT(stateSpace,stateVal) は、状態空間オブジェクト stateSpace および状態バリデーター オブジェクト stateVal から RRT プランナーを作成します。stateVal の状態空間は stateSpace と同じでなければなりません。stateSpace および stateVal は planner の StateSpace プロパティと StateValidator プロパティも設定します。

例

planner = plannerRRT(___,Name=Value) は、前述の構文の入力引数に加え、1 つ以上の名前と値の引数を使用してプロパティを設定します。MaxNumTreeNodes、MaxIterations、MaxConnectionDistance、GoalReachedFcn、および GoalBias の各プロパティを名前と値の引数として指定できます。

プロパティ

すべて展開する

プランナーの状態空間。状態空間オブジェクトとして指定します。stateSpaceSE2、stateSpaceDubins、stateSpaceReedsShepp、stateSpaceSE3 などの状態空間オブジェクトを使用できます。nav.StateSpace オブジェクトを使用して状態空間オブジェクトをカスタマイズすることもできます。

プランナーの状態バリデーター。状態バリデーター オブジェクトとして指定します。validatorOccupancyMap、validatorVehicleCostmap、validatorOccupancyMap3D などの状態バリデーター オブジェクトを使用できます。

探索木内の最大ノード数 (ルート ノードを除く)。正の整数として指定します。

例: MaxNumTreeNodes=2500

データ型: single | double

反復の最大回数。正の整数として指定します。

例: MaxIterations=2500

データ型: single | double

ツリー内で許可される運動の最大長。スカラーとして指定します。

例: MaxConnectionDistance=0.3

データ型: single | double

ゴールに到達したかどうかを評価するコールバック関数。関数ハンドルとして指定します。独自のゴール到達関数を作成できます。この関数は次の構文に従う必要があります。

 function isReached = myGoalReachedFcn(planner,currentState,goalState)

ここで、次のようになります。

  • planner — 作成されたプランナー オブジェクト。plannerRRT オブジェクトとして指定します。

  • currentState — 現在の状態。3 要素の実数ベクトルとして指定します。

  • goalState — ゴールの状態。3 要素の実数ベクトルとして指定します。

  • isReached — 現在の状態がゴール状態に到達しているかどうかを示す boolean 変数。true または false として返されます。

コード生成ワークフローでカスタムの GoalReachedFcn を使用するには、関数 plan を呼び出す前にこのプロパティをカスタムの関数ハンドルに設定する必要があります。このプロパティを初期化後に変更することはできません。

データ型: function handle

状態のサンプリング中にゴール状態を選択する確率。範囲 [0,1] の実数のスカラーとして指定します。このプロパティは、状態空間から状態をランダムに選択するプロセス中に、実際のゴール状態を選択する確率を定義します。最初は確率を 0.05 などの小さい値に設定できます。

例: GoalBias=0.1

データ型: single | double

オブジェクト関数

planPlan path between two states
copyCreate copy of planner object

例

すべて折りたたむ

ライブ スクリプトを開く

状態空間を作成します。

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)

ライブ スクリプトを開く

都市ブロックの 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)

参照

[1] S.M. Lavalle and J.J. Kuffner. "Randomized Kinodynamic Planning." The International Journal of Robotics Research. Vol. 20, Number 5, 2001, pp. 378 – 400.

拡張機能

バージョン履歴

R2019b で導入

参考

オブジェクト

関数