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上plot(start(1),start(2),'ro'）情节（目标（1），目标（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-')情节([目标(1),目标(1)+ r * cos(目标(3))],[目标(2),goal(2) + r*sin(goal(3))],'m-') holdoff

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;ockgrid.yworldlimits;[-pi pi]];ss = state -pacedubins（边界）;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;
functionisReached = exampleHelperCheckIfGoal(planner, goalState, newState) isReached = false; threshold = 0.1;ifplanner.StateSpace.distance(newState, goalState) < threshold isReached = true;endend

开始位置位置のパス计画ます。を再现できるように，乱数乱数発生器器ししし

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

パスのプロット

占有マップ表示します。solnInfoから探索プロットます。最终的を内插してオーバーレイし。

显示（叶胶状）保留上% 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'）情节（目标（1），目标（2），'mo') holdoff

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

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

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

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

% Only making left turnsgoLeft = true;% Create the state spacessCustom = 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;

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

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

パスのプロット

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

图显示（叶胶状）保持上% 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') holdoff

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