plan

2 つの姿勢間で障害物のないパスを検索

構文

path = plan(planner,start,goal)

[path,directions] = plan(planner,start,goal)

[path,directions,solutionInfo] = plan(planner,start,goal)

[___] = plan(___,"SearchMode",mode)

説明

path = plan(planner,start,goal) は、入力 plannerHybridAStar オブジェクトを使用して、[x y theta] ベクトルとして指定された開始姿勢とゴール姿勢の間で障害物のないパスを計算します。

[path,directions] = plan(planner,start,goal) は、パスに沿った各姿勢の運動方向 directions も列ベクトルとして返します。値 1 は前進方向を示し、値 -1 は後退方向を示します。プランナーがパスを見つけられない場合、関数は空の列ベクトルを返します。

[path,directions,solutionInfo] = plan(planner,start,goal) は、パス プランニングの解法情報が含まれた solutionInfo も構造体として返します。

[___] = plan(___,"SearchMode",mode) は、前述した構文の引数の任意の組み合わせに加え、探索アルゴリズム モード mode を指定します。

例

ハイブリッド A* を使用した障害物なしのパス計画

ハイブリッド A* アルゴリズムを使用して、駐車場を通るビークルの衝突のないパスを計画します。

マップの作成と状態バリデーターへの割り当て

駐車場のビークル コストマップでセルのコスト値を読み込みます。

load parkingLotCostVal.mat % costVal

コスト値をもつbinaryOccupancyMapを作成します。

resolution = 3;
map = binaryOccupancyMap(costVal,resolution);

状態空間を作成します。

ss = stateSpaceSE2;

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

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

衝突チェック用の状態バリデーター オブジェクトを作成します。

sv = validatorOccupancyMap(ss);

マップを状態バリデーター オブジェクトに割り当てます。

sv.Map = map;

パスの計画と可視化

plannerHybridAStarオブジェクトを状態バリデーター オブジェクトで初期化します。プランナーの MinTurningRadius および MotionPrimitiveLength プロパティを指定します。

planner = plannerHybridAStar(sv, ...
                             MinTurningRadius=4, ...
                             MotionPrimitiveLength=6);

ビークルの開始姿勢とゴール姿勢を [x, y, theta] ベクトルとして定義します。"x" と "y" は位置をメートル単位で指定し、"theta" は向きの角度をラジアン単位で指定します。

startPose = [4 9 pi/2]; % [meters, meters, radians]
goalPose = [30 19 -pi/2];

開始姿勢からゴール姿勢までのパスを計画します。

refpath = plan(planner,startPose,goalPose,SearchMode='exhaustive');     

関数 show を使用してパスを可視化します。

show(planner)

入力引数

planner — ハイブリッド A* パス プランナー

plannerHybridAStar オブジェクト

ハイブリッド A* パス プランナー。plannerHybridAStar オブジェクトとして指定します。

start — パスの開始位置

3 要素ベクトル

パスの開始位置。[x y theta] 形式の 1 行 3 列のベクトルとして指定します。x と y は位置をメートル単位で指定し、theta は向きの角度をラジアン単位で指定します。

例: [5 5 pi/2]

データ型: double

goal — パスの最終位置

3 要素ベクトル

パスの最終位置。[x y theta] 形式の 1 行 3 列のベクトルとして指定します。x と y は位置をメートル単位で指定し、theta は向きの角度をラジアン単位で指定します。

例: [45 45 pi/4]

データ型: double

mode — 探索アルゴリズム モード

"greedy" (既定値) | "exhaustive"

探索アルゴリズム モード。次のオプションのいずれかとして指定します。

• "greedy" — 平均して最短時間で解を求めることを優先します。

• "exhaustive" — 開いているセット内のノード数を増やして解を最適化します。

例: plan(phastar,start,goal,"SearchMode","greedy")

データ型: string | char

出力引数

path — 障害物のないパス

navPath オブジェクト

障害物のないパス。navPath オブジェクトとして返されます。

directions — 運動方向

1 (前進) と –1 (後退) の列ベクトル

パスに沿った各姿勢の運動方向。1 (前進) と –1 (後退) の列ベクトルとして返されます。

データ型: double

solutionInfo — 解法情報

構造体

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

solutionInfo のフィールド

フィールド	説明
IsPathFound	パスが見つかったかどうかを示します。パスが見つかった場合は、1 を返します。それ以外の場合は、0 を返します。
NumNodes	プランナーが終了したときの探索木内のノード数 (ルート ノードを除く)。
NumIterations	実行されたプランニングの反復回数。
ExitFlag	プランナーの終了ステータスを示します。以下のように返されます。 1 — ゴールに達した場合 2 — ヒューリスティック コストが無限大の場合 3 — パスが検出されなかった場合

データ型: struct

拡張機能

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

バージョン履歴

R2019b で導入

R2022b: 名前と値の引数を使用した探索アルゴリズム モードの設定

名前と値の引数 SearchMode を使用して探索アルゴリズム モードを設定します。

参考

validatorOccupancyMap | validatorVehicleCostmap | navPath | shortenpath

トピック

• Enable Vehicle Collision Checking for Path Planning Using Hybrid A*