A* パス プランナーを使用したパスの計画

A* グリッド アルゴリズムを使用して、駐車スポットへの最短車両パスを計画します。次に、車両に対して非ホロノミック拘束を課して、ハイブリッド A* アルゴリズムを使用してパスを再計画します。

占有マップの作成

駐車場のコストマップを読み込みます。コストマップ オブジェクトのプロパティを使用してoccupancyMap (Navigation Toolbox)オブジェクトを作成します。占有マップを可視化します。

data = load('parkingLotCostmapReducedInflation.mat');
costmapObj = data.parkingLotCostmapReducedInflation;
resolution = 1/costmapObj.CellSize;
oMap = occupancyMap(costmapObj.Costmap,resolution);
oMap.FreeThreshold = costmapObj.FreeThreshold;
oMap.OccupiedThreshold = costmapObj.OccupiedThreshold;
show(oMap)

A* グリッド プランナーを使用したパスの計画

占有マップを使用してplannerAStarGrid (Navigation Toolbox)オブジェクトを作成します。

gridPlanner = plannerAStarGrid(oMap);

ワールド座標系で開始位置とゴール位置を定義します。この座標系の原点はマップの左下隅です。

startPos = [11,10];
goalPos  = [31.5,18];

ワールド座標で開始点からゴール点までのパスを計画します。

path = plan(gridPlanner,startPos,goalPos,"world");

オブジェクト関数 show を使用してパスおよび探索されたノードを可視化します。

show(gridPlanner)

非ホロノミック拘束を課す設定およびハイブリッド A* プランナーを使用した再計画

衝突チェックを使用して計画されたパスを検証するための状態バリデーター オブジェクトを作成します。占有マップを状態バリデーター オブジェクトに割り当てます。

validator = validatorOccupancyMap;
validator.Map = oMap;

plannerHybridAStar (Navigation Toolbox)オブジェクトを状態バリデーター オブジェクトで初期化します。プランナーの MinTurningRadius および MotionPrimitiveLength プロパティを指定して、最小回転半径と運動プリミティブの長さの非ホロノミック拘束を課します。

hybridPlanner = plannerHybridAStar(validator,MinTurningRadius=4,MotionPrimitiveLength=6);

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

startPose = [4 4 pi/2]; % [meters, meters, radians]
goalPose = [45 27 -pi/2];

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

refpath = plan(hybridPlanner,startPose,goalPose);

オブジェクト関数 show を使用してパスを可視化します。

show(hybridPlanner)