plannerRRTStar

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

説明

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 つ以上の名前と値の引数を使用してプロパティを設定します。StateSampler、BallRadiusConstant、ContinueAfterGoalReached、MaxNumTreeNodes、MaxIterations、MaxConnectionDistance、GoalReachedFcn および GoalBias の各プロパティを名前と値の引数として指定できます。

プロパティ

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

状態空間オブジェクト

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

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

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

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

StateSampler — 入力空間のサンプリング用の状態空間サンプラー

stateSamplerUniform オブジェクト (既定値) | stateSamplerGaussian オブジェクト | stateSamplerMPNET オブジェクト | nav.StateSampler オブジェクト

R2023b 以降

入力空間で状態サンプルの検索に使用する状態空間サンプラー。stateSamplerUniform オブジェクト、stateSamplerGaussian オブジェクト、stateSamplerMPNET オブジェクト、または nav.StateSampler オブジェクトとして指定します。既定では、plannerRRTStar は均一状態サンプリングを使用します。

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)

詳細

ボール半径定数

RRT と RRT* の主な違いは、RRT* アルゴリズムの漸近的最適性を確保する "再結線" 動作です。RRT ベースのプランナーが新しいノードを生成すると、プランナーはツリー内で最も近いノードを見つけます。ノード間のパスに衝突がなく、その他の面では有効な場合、RRT アルゴリズムはノードを接続しますが、RRT* アルゴリズムはノードの接続後に追加の手順を実行してツリーを最適化します。まず、RRT* は、新しいノードまで一定の距離内にあるツリー内のすべてのノードを見つけます。次に、RRT* は、開始ノードに戻る有効な最短パスを新しいノードに提供するノードを探し、そのノードと新しいノードの間にエッジを追加します。最後に、プランナーは再結線操作を実行し、新しいノードが最も近いノードのそれぞれに、開始ノードに戻る短いルートを提供できるかどうかを確認します。より短いパスがある場合、ノードは現在の親から切断され、より近いノードの親に設定し直されます。

再結線が行われる半径は、ボール半径定数と呼ばれます。RRT* の目標は、追加のオーバーヘッド計算を制限しながら漸近的最適性を確保することであるため、適切なボール半径定数を選択することが重要です。ボール半径定数が大きすぎると、RRT* の実行時間が長くなります。ボール半径定数が小さすぎると、アルゴリズムが最適な結果に収束しない可能性があります。

plannerRRTStar は、[1]から適用された次の距離公式を使用して最近傍を見つけます。

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

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

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

• d — 状態空間の次元の数

• η — 最大接続距離

• γ — ボール半径定数。次のように定義されます。

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

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

  • V_Free — ルベーグ測度。検索空間内の近似自由体積

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

ボール半径定数の意味と直感的概念

r と γ の式は、特定の空間とサンプリング密度の適切なサイズの半径を定義します。空間を埋めるノードの数が線形的に増加するにつれて、半径が小さくなり、縮小するボール内の近傍の数が対数的に増加します。

この直感的概念は、ツリー内で新しくサンプリングされたすべての点が、コンフィギュレーション空間の自由な部分から均一かつ独立してサンプリングされているという予測から得られます。この方法で点をサンプリングすることにより、点は均一なポアソン点プロセスを使用して生成されたと言えます。これは、RRT* の各反復で、自由空間内で n 点が均一にサンプリングされているため、単位体積あたりの点の平均密度 λ が存在するはずであることを意味します。任意の次元の空間の場合、単位測定あたりの点の強度が存在します。

$$\lambda =\frac{N}{V_{Free}}$$

したがって、計画空間の任意の部分に存在すると予期される点の数は、その部分の体積に密度を乗じて計算されます。RRT* の場合、半径 r の d 次元のボール内の点の数に焦点を当てます。

$$n_{1,d}=V_{Ball}·\lambda =\frac{V_{Ball}}{V_{Free}}·n$$

$${\text{n}}_{1,d}={\text{n}}_{1,d}·r^d$$

ここで、

• n_1,d — d 次元の単位ボール内の予期される点の数

• n_r,d — 半径 r をもつ d 次元のボール内の予期される点の数

また、近傍数の目標が、n が無限大に近づくにつれて対数的に増加することであることを踏まえ、n_r,d=log(n) を設定し、r を求解できます。

$$\text{r=}{\left(\frac{V_{Free}}{V_{Ball}}·\frac{\log (n)}{n}\right)}^{\frac{1}{d}}$$

式 2 の残りの係数は、[1]の収束証明から導出されます。ただし、n を削除すると、ボール半径定数は、サンプル領域の自由空間と単位ボールの測定値に次元固有の定数を乗じた比率であることがわかります。

参照

[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 で導入

R2023b: パス プランニングのサンプリング方法の指定

均一サンプリング、ガウス サンプリング、MPNet サンプリング、またはカスタムのサンプリング方法を指定して、パス プランニング用のサンプルを生成できるようになりました。名前と値の引数 StateSampler を使用してサンプリング方法を指定します。

参考

オブジェクト

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

関数

• plan | copy

トピック

• Calculating an Appropriate BallRadiusConstant for PlannerRRStar