Main Content

RRT を使用したモバイル ロボットのパスの計画

この例では、Rapidly-exploring Random Tree (RRT) アルゴリズムを使用して、既知のマップで車両のパスを計画する方法を説明します。カスタム状態空間により、特殊な車両制約も適用されます。カスタム状態空間とパス検証オブジェクトを使って、独自のプランナーを任意のナビゲーション アプリケーション用に調整できます。

占有マップの読み込み

小さなオフィス空間の既存の占有マップを読み込みます。マップ上に、車両の開始姿勢と目標姿勢をプロットします。

load("office_area_gridmap.mat","occGrid")
show(occGrid)

% Set start and goal poses.
start = [-1.0,0.0,-pi];
goal = [14,-2.25,0];

% Show start and goal positions of robot.
hold on
plot(start(1),start(2),'ro')
plot(goal(1),goal(2),'mo')

% Show start and goal headings.
r = 0.5;
plot([start(1),start(1) + r*cos(start(3))],[start(2),start(2) + r*sin(start(3))],'r-')
plot([goal(1),goal(1) + r*cos(goal(3))],[goal(2),goal(2) + r*sin(goal(3))],'m-')
hold off

Figure contains an axes object. The axes object with title Occupancy Grid contains 5 objects of type image, line.

状態空間の定義

stateSpaceDubins オブジェクトを使用し、状態の範囲を指定して、車両の状態空間を指定します。このオブジェクトは、状態の範囲内で車両をステアリングするために、サンプリングされる状態を実行可能な Dubins 曲線に制限します。0.4 メートルの回転半径で、この小環境での急カーブが可能となります。

bounds = [occGrid.XWorldLimits; occGrid.YWorldLimits; [-pi pi]];

ss = stateSpaceDubins(bounds);
ss.MinTurningRadius = 0.4;

パスの計画

パスを計画するために、RRT アルゴリズムは状態空間内でランダムな状態をサンプリングし、パスの接続を試みます。これらの状態および接続は、マップの制約に基づいて検証または除外する必要があります。車両は、マップ内で定義されている障害物に衝突してはなりません。

指定された状態空間を使って validatorOccupancyMap オブジェクトを作成します。Map プロパティを、読み込んだ occupancyMap オブジェクトに設定します。0.05 m の ValdiationDistance を設定します。この検証距離により、パス接続が離散化され、それに基づいてマップ内の障害物がチェックされます。

stateValidator = validatorOccupancyMap(ss); 
stateValidator.Map = occGrid;
stateValidator.ValidationDistance = 0.05;

パス プランナーを作成し、より多くの状態を接続するために最大接続距離を増やします。状態のサンプリングの最大反復数を設定します。

planner = plannerRRT(ss,stateValidator);
planner.MaxConnectionDistance = 2.0;
planner.MaxIterations = 30000;

関数 GoalReached をカスタマイズします。この例の補助関数は、設定されたしきい値内で、実行可能なパスが目標に達するかどうかをチェックします。目標に達すると関数は true を返し、プランナーは停止します。

planner.GoalReachedFcn = @exampleHelperCheckIfGoal;
function isReached = exampleHelperCheckIfGoal(planner, goalState, newState)
    isReached = false;
    threshold = 0.1;
    if planner.StateSpace.distance(newState, goalState) < threshold
        isReached = true;
    end
end

開始位置と目標位置間のパスを計画します。結果を再現できるように、乱数発生器をリセットします。

rng default

[pthObj, solnInfo] = plan(planner,start,goal);

パスのプロット

占有マップを表示します。solnInfo から探索木をプロットします。最終的なパスを内挿してオーバーレイします。

show(occGrid)
hold on

% Plot entire search tree.
plot(solnInfo.TreeData(:,1),solnInfo.TreeData(:,2),'.-');

% Interpolate and plot path.
interpolate(pthObj,300)
plot(pthObj.States(:,1),pthObj.States(:,2),'r-','LineWidth',2)

% Show start and goal in grid map.
plot(start(1),start(2),'ro')
plot(goal(1),goal(2),'mo')
hold off

Figure contains an axes object. The axes object with title Occupancy Grid contains 5 objects of type image, line.

Dubins 車両制約のカスタマイズ

カスタム車両制約を指定するには、状態空間オブジェクトをカスタマイズします。この例では、stateSpaceDubins クラスを基にした ExampleHelperStateSpaceOneSidedDubins を使用します。このヘルパー クラスは、boolean プロパティ GoLeft に基づいて、曲がる方向を右または左に制限します。このプロパティは、dubinsConnection オブジェクトのパス タイプを実質的に無効にします。

例の補助関数を使用して状態空間オブジェクトを作成します。同じ状態の範囲を指定して、新しい boolean パラメーターを true (左折のみ) に指定します。

% Only making left turns
goLeft = true;

% Create the state space
ssCustom = ExampleHelperStateSpaceOneSidedDubins(bounds,goLeft);
ssCustom.MinTurningRadius = 0.4;

パスの計画

カスタム Dubins 制約をもつ新しいプランナー オブジェクトと、これらの制約に基づくバリデーターを作成します。同じ関数 GoalReached を指定します。

stateValidator2 = validatorOccupancyMap(ssCustom); 
stateValidator2.Map = occGrid;
stateValidator2.ValidationDistance = 0.05;

planner = plannerRRT(ssCustom,stateValidator2);
planner.MaxConnectionDistance = 2.0;
planner.MaxIterations = 30000;
planner.GoalReachedFcn = @exampleHelperCheckIfGoal;

開始位置と目標位置間のパスを計画します。乱数発生器を再度リセットします。

rng default
[pthObj2,solnInfo] = plan(planner,start,goal);

パスのプロット

マップ上に新しいパスを描画します。このパスは、左折のみを実行して目標に到達する必要があります。

figure
show(occGrid)

hold on

% Show the search tree.
plot(solnInfo.TreeData(:,1),solnInfo.TreeData(:,2),'.-');

% Interpolate and plot path.
pthObj2.interpolate(300)
plot(pthObj2.States(:,1), pthObj2.States(:,2), 'r-', 'LineWidth', 2)

% Show start and goal in grid map.
plot(start(1), start(2), 'ro')
plot(goal(1), goal(2), 'mo')
hold off

Figure contains an axes object. The axes object with title Occupancy Grid contains 5 objects of type image, line.