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 heading angle using a line.
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

ロボットの基本的な巡回モデルを考えます。ロボットが障害物に接近しすぎないように、マップの障害物を少しインフレートします。

% Make a copy of the original map and infate it by 0.1 meters. Use this inflated map for path planning. 
% Use the original map for visualization purpose. 
inflatedMap = copy(occGrid);
inflate(inflatedMap,0.1); 

状態空間の定義

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

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

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

パスの計画

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

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

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

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

planner = plannerRRT(ss,stateValidator);
planner.MaxConnectionDistance = 2.5;
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);

パスの短縮

shortenpath 関数を使用してパス上の冗長なノードを削除します。関数は不要なノードを削除し、衝突につながらないような、連続していないノードを接続して、衝突のないパスを生成します。

shortenedPath = shortenpath(pthObj,stateValidator);

元のパスと短縮されたパスの長さを計算します。

originalLength = pathLength(pthObj)

originalLength = 
33.8183

shortenedLength = pathLength(shortenedPath)

shortenedLength = 
29.0853

短縮によってパスの長さが短くなることを確認できます。

元のパスと短縮されたパスのプロット

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

show(occGrid)
hold on

% Plot entire search tree.
plot(solnInfo.TreeData(:,1),solnInfo.TreeData(:,2),plannerLineSpec.tree{:})

% Interpolate and plot path.
interpolate(pthObj,300)
plot(pthObj.States(:,1),pthObj.States(:,2),plannerLineSpec.path{:})

% Interpolate and plot path.
interpolate(shortenedPath,300);
plot(shortenedPath.States(:,1),shortenedPath.States(:,2),'g-','LineWidth',3)

% Show start and goal in grid map.
plot(start(1),start(2),'ro')
plot(goal(1),goal(2),'mo')
legend('search tree','original path','shortened path')
hold off

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 = inflatedMap;
stateValidator2.ValidationDistance = 0.05;

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

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

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

パスの短縮

カスタムの動きの制約を維持してパスを短縮します。

shortenedPath2 = shortenpath(pthObj2,stateValidator2);

元のパスと短縮されたパスの長さを計算します。

originalLength2 = pathLength(pthObj2)

originalLength2 = 
46.7841

shortenedLength2 = pathLength(shortenedPath2)

shortenedLength2 = 
45.7649

短縮によってパスの長さが短くなることを確認できます。

パスのプロット

ゴールに到達するには、パスで左折のみを実行します。

figure
show(occGrid)
hold on
% Interpolate and plot path.
interpolate(pthObj2,300)
h1 = plot(pthObj2.States(:,1),pthObj2.States(:,2),plannerLineSpec.path{:});

% Interpolate and plot path.
interpolate(shortenedPath2,300)
h2 = plot(shortenedPath2.States(:,1),shortenedPath2.States(:,2),'g-','LineWidth',3);

% Show start and goal in grid map.
plot(start(1),start(2),'ro')
plot(goal(1),goal(2),'mo')
legend([h1 h2],'original path','shortened path')
hold off