ハイブリッド A* を使用したパス プランニング用のコードの生成

この例では、ハイブリッド A* アルゴリズムを使用して、マップを介してビークルの衝突のないパスを計画するためにコード生成を実行する方法について説明します。MATLAB® でアルゴリズムを検証した後に、関数codegen (MATLAB Coder)を使用して MEX 関数を生成します。アルゴリズムで生成された MEX ファイルを使用して、計画されたパスを可視化します。

パスを計画するアルゴリズムの作成

plannerHybridAStarオブジェクトを使用して、マップで開始姿勢からゴール姿勢までのパスを計画する関数 codegenPathPlanner を作成します。

function path = codegenPathPlanner(mapData,startPose,goalPose)
    %#codegen
       
    % Create a binary occupancy map
    map = binaryOccupancyMap(mapData);

    % Create a state space object
    stateSpace = stateSpaceSE2;

    % Update state space bounds to be the same as map limits.
    stateSpace.StateBounds = [map.XWorldLimits;map.YWorldLimits;[-pi pi]];

    % Construct a state validator object using the statespace and map object
    validator = validatorOccupancyMap(stateSpace,Map=map);
    
    % Set the validation distance for the validator
    validator.ValidationDistance = 0.01;
    
    % Assign the state validator object to the plannerHybridAStar object
    planner = plannerHybridAStar(validator);
    
    % Compute a path for the given start and goal poses
    pathObj = plan(planner,startPose,goalPose);
    
    % Extract the path poses from the path object
    path = pathObj.States;

end

この関数は、標準ハイブリッド A*パス プランニング呼び出しのラッパーの役割を果たします。標準入力を受け入れ、衝突のないパスを配列として返します。コード生成がサポートされる関数の入出力としてハンドル オブジェクトを使用することはできないため、関数内にプランナー オブジェクトを作成します。関数 codegenPathPlanner を現在のフォルダーに保存します。

MATLAB でのパス プランニング アルゴリズムの検証

コードを生成する前に MATLAB でパス プランニング アルゴリズムを検証します。

ランダムな 2 次元迷路マップを生成します。

map = mapMaze(5,MapSize=[25 25],MapResolution=1);
mapData = occupancyMatrix(map);

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

startPose = [3 3 pi/2];
goalPose = [22 22 pi/2];

指定した開始姿勢、ゴール姿勢、およびマップについてパスを計画します。

path = codegenPathPlanner(mapData,startPose,goalPose);

計算されたパスを可視化します。

show(binaryOccupancyMap(mapData))
hold on
% Start state
scatter(startPose(1,1),startPose(1,2),"g","filled")
% Goal state
scatter(goalPose(1,1),goalPose(1,2),"r","filled")
% Path
plot(path(:,1),path(:,2),"r-",LineWidth=2)
legend("Start Pose","Goal Pose","MATLAB Generated Path")
legend(Location="northwest")

パス プランニング アルゴリズムのコード生成

関数codegen (MATLAB Coder)またはMATLAB Coder (MATLAB Coder)アプリを使用してコードを生成できます。この例では、MATLAB コマンド ラインで codegen を呼び出して MEX ファイルを生成します。-args オプションおよび func_inputs 入力引数を使用して、関数への各入力のサンプル入力引数を指定します。

関数 codegen を呼び出し、cell 配列で入力引数を指定します。この関数により、別個の関数 codegenPathPlanner_mex が作成されて使用されます。options 入力引数を使用して C コードを生成することもできます。この手順には時間がかかることがあります。

codegen -config:mex codegenPathPlanner -args {mapData,startPose,goalPose}

Code generation successful.

生成された MEX 関数を使用した結果の検証

指定した開始姿勢、ゴール姿勢、およびマップについてパス プランニング アルゴリズムの MEX バージョンを呼び出してパスを計画します。

mexPath = codegenPathPlanner_mex(mapData,startPose,goalPose);

パス プランニング アルゴリズムの MEX バージョンで計算されたパスを可視化します。

scatter(mexPath(:,1),mexPath(:,2),...
        Marker="o",...
        MarkerFaceColor="b",...
        MarkerEdgeColor="b")
legend("Start Pose","Goal Pose","MATLAB Generated Path","MEX Generated Path")
legend(Location="northwest")
hold off

生成されたコードのパフォーマンスの確認

timeitを使用して、生成された MEX 関数の実行時間を元の関数の実行時間と比較します。

time = timeit(@() codegenPathPlanner(mapData,startPose,goalPose))

time = 0.3735

mexTime = timeit(@() codegenPathPlanner_mex(mapData,startPose,goalPose))

mexTime = 0.0474

time/mexTime

ans = 7.8872

この例では、MEX 関数の実行の方が 5 倍高速です。結果はシステムによって異なる可能性があります。

生成された MEX 関数を使用した新しいマップでのパスの計画

新しいマップで新しい開始姿勢とゴール姿勢についてパスを計画します。新しいマップのサイズは、MEX 関数の生成に使用したマップと同じでなければなりません。

ランダムな 2 次元迷路マップを生成します。

mapNew = mapMaze(5,MapSize=[25 25],MapResolution=1);
mapDataNew = occupancyMatrix(mapNew);

開始姿勢とゴール姿勢を指定します。

startPoseNew = [22 3 pi/2];
goalPoseNew = [3 22 pi/2];

指定した開始姿勢、ゴール姿勢、およびマップについてパスを計画します。

pathNew = codegenPathPlanner_mex(mapDataNew,startPoseNew,goalPoseNew);

MEX 関数で計算された新しいパスを可視化します。

show(binaryOccupancyMap(mapDataNew))
hold on
% Start state
scatter(startPoseNew(1,1),startPoseNew(1,2),"g","filled")
% Goal state
scatter(goalPoseNew(1,1),goalPoseNew(1,2),"r","filled")
% Path
plot(pathNew(:,1),pathNew(:,2),"r-",LineWidth=2)
legend("Start Pose","Goal Pose","MEX Generated Path")
legend(Location='northeast')