plannerRRTStar

オプションの RRT パス プランナー (RRT*) の作成

R2019b 以降

説明

plannerRRTStar オブジェクトは、漸近的に最適な RRT プランナー (RRT*) を作成します。RRT* アルゴリズムは、状態空間距離について最適なソリューションに収束します。また、そのランタイムは RRT アルゴリズムのランタイムの定数係数です。RRT* は幾何学的プランニング問題を解決するために使用されます。幾何学的プランニング問題では、状態空間から取得された 2 つの任意のランダム状態が接続可能であることが必要です。

作成

構文

planner = plannerRRTStar(stateSpace,stateVal)

planner = plannerRRTStar(___,Name=Value)

説明

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

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

プロパティ

StateSpace — プランナーの状態空間

状態空間オブジェクト

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

StateValidator — プランナーの状態バリデーター

状態バリデーター オブジェクト

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

BallRadiusConstant — 近傍探索半径を推定するために使用される定数

100 (既定値) | 正のスカラー

近傍探索半径を推定するために使用される定数。正のスカラーとして指定します。半径は以下のように推定されます。

$$r=\min \left(\gamma {\left(\frac{\log (n)}{n}\right)}^{\frac{1}{d}},\eta \right)$$

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

• γ — BallRadiusConstant プロパティの値

• n — ツリー内の現在のノード数

• d — 状態空間の次元

• η — MaxConnectionDistance プロパティの値

γ は以下のように定義されます。

$$\gamma ={{\displaystyle 2}}^d\left(1+\frac{1}{d}\right)\left(\frac{{{\displaystyle V}}_{Free}}{{{\displaystyle V}}_{Ball}}\right)$$

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

• V_Free — 検索空間内の近似自由体積

• V_Ball — d 次元における単位球の体積

上記の式は、特定の空間の "適切な" サイズの BallRadiusConstant を定義します。つまり、空間を埋めるノードの数が増加して半径が小さくなるにつれて、予期される近傍の数が対数的に増加します。値が大きくなると、反復ごとの d 球内の近傍の平均数が大きくなり、再結線候補が増加します。ただし、この推奨最小値を下回る値では、近傍が 1 つだけになることがあり、この場合漸近的に最適な結果が得られなくなります。

例: BallRadiusConstant=80

データ型: single | double

ContinueAfterGoalReached — ゴール到達後も最適化を継続

false (既定値) | true

プランナーがゴールに到達後も最適化を続けるかどうかを決定します。false または true として指定します。また、プランナーは、反復の最大回数またはツリー ノードの最大数に到達した場合、このプロパティの値に関係なく終了します。

例: ContinueAfterGoalReached=true

データ型: logical

MaxNumTreeNodes — 探索木内の最大ノード数

1e4 (既定値) | 正の整数

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

例: MaxNumTreeNodes=2500

データ型: single | double

MaxIterations — 反復の最大回数

1e4 (既定値) | 正の整数

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

例: MaxIterations=2500

データ型: single | double

MaxConnectionDistance — 運動の最大長

0.1 (既定値) | 正のスカラー

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

例: MaxConnectionDistance=0.3

データ型: single | double

GoalReachedFcn — ゴールに到達したかどうかを判別するコールバック関数

@nav.algs.checkIfGoalIsReached | 関数ハンドル

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

 function isReached = myGoalReachedFcn(planner,currentState,goalState)

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

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

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

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

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

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

データ型: function handle

GoalBias — 状態のサンプリング中にゴール状態を選択する確率

0.05 (既定値) | 範囲 [0,1] の実数のスカラー

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

例: GoalBias=0.1

データ型: single | double

オブジェクト関数

plan	2 つの状態間のパスの計画
copy	Create copy of planner object

例

2 つの状態間の最適なパスを計画する

状態空間を作成します。

ss = stateSpaceSE2;

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

sv = validatorOccupancyMap(ss);

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

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

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

sv.ValidationDistance = 0.01;

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

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

RRT* パス プランナーを作成し、ゴール到達後にさらに最適化できます。最大反復回数を減らし、最大接続距離を増やします。

planner = plannerRRTStar(ss,sv, ...
          ContinueAfterGoalReached=true, ...
          MaxIterations=2500, ...
          MaxConnectionDistance=0.3);

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

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

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

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

結果を可視化します。

map.show
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 = plannerRRTStar(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] Karaman, S. and E. Frazzoli. "Sampling-Based Algorithms for Optimal Motion Planning." The International Journal of Robotics Research. Vol. 30, Number 7, 2011, pp 846 – 894.

拡張機能

C/C++ コード生成

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

使用に関するメモと制限:

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

バージョン履歴

R2019b で導入

参考

オブジェクト

• plannerRRT | plannerBiRRT | stateSpaceReedsShepp | stateSpaceDubins | stateSpaceSE2 | stateSpaceSE3 | validatorOccupancyMap | validatorVehicleCostmap | validatorOccupancyMap3D

関数

• plan | copy

×

例を開く

この例の変更されたバージョンがあります。編集された方の例を開きますか?

いいえ、変更されたバージョンを上書きします。 はい