差動駆動型ロボットのパス追従

ライブスクリプトを開く

この例では、ロボットシミュレーターを使用して、目的のパスに従うようにロボットを制御する方法を説明します。この例では、単純追跡パス追従コントローラーを使用して、あらかじめ決められたパスに沿ってシミュレートされたロボットを駆動します。目的のパスは、明示的に定義した、またはパスプランナー (複雑度の異なる環境でのパス計画を参照) を使用して計算した一連の中間点です。シミュレートされた差動駆動型ロボット用の単純追跡パス追従コントローラーが作成され、指定したパスに従うための制御コマンドを計算します。計算された制御コマンドを使用して、単純追跡コントローラーに基づいて目的のパスを追従する目的の軌跡に沿って、シミュレートされたロボットが駆動されます。

メモ: R2016b 以降では、step メソッドを使用して、System object™ によって定義された演算を実行する代わりに、引数を指定したオブジェクトを関数であるかのように呼び出すことができます。たとえば、y = step(obj,x)とy = obj(x)は同等の演算を実行します。

中間点の定義

ロボットの目的のパスの、一連の中間点を定義します。

path = [2.00 1.00; 1.25 1.75; 5.25 8.25; 7.25 8.75; 11.75 10.75; 12.00 10.00];

パスで定義された、ロボットの現在位置とゴール位置を設定します。

robotInitialLocation = path(1,:); robotGoal = path(end,:);

ロボットの最初の向きを仮定します (ロボットの向きとは、ロボットの進行方向と正の X 軸の間の角度を反時計回りに測定したものです)。

initialOrientation = 0;

ロボットの現在の姿勢 [x y theta] を定義します。

robotCurrentPose = [robotInitialLocation initialOrientation]';

運動学的ロボットモデルの作成

ロボットモデルを初期化して初期姿勢を割り当てます。シミュレートされるロボットは、2 輪の差動駆動型ロボットの運動の運動学的方程式をもちます。このシミュレートされるロボットへの入力は、線形速度と角速度です。

robot = differentialDriveKinematics("TrackWidth", 1,"VehicleInputs","VehicleSpeedHeadingRate");

目的のパスを可視化します。

figure plot(path(:,1), path(:,2),'k--d') xlim([0 13]) ylim([0 13])

パス追従コントローラーの定義

上記で定義したパスとロボット運動モデルに基づいて、ロボットをパスに沿って駆動するパス追従コントローラーが必要です。controllerPurePursuitオブジェクトを使用してパス追従コントローラーを作成します。

controller = controllerPurePursuit;

上記で定義したパスを使用して、コントローラーの目的の中間点を設定します。

controller.Waypoints = path;

パス追従コントローラーのパラメーターを設定します。この例では、目的の線形速度を 0.6 メートル/秒に設定します。

controller.DesiredLinearVelocity = 0.6;

最大角速度は回転速度の飽和制限の役割を果たします。この例ではこれを 2 ラジアン/秒に設定します。

controller.MaxAngularVelocity = 2;

原則として,パスを滑らかにするために,前方注視距離は目的の線形速度より大きくする必要があります。前方注視距離を大きくすると、ロボットは近道をする場合があります。対照的に、前方注視距離を小さくすると、パス追従動作が不安定になる可能性があります。この例では、0.3 m の値を選択しました。

controller.LookaheadDistance = 0.3;

パス追従コントローラーを使用して、目的の中間点に沿ってロボットを駆動

パス追従コントローラーは入力制御信号をロボットに提供します。これを使用して、ロボットは目的のパスに沿って駆動します。

ゴールの半径を定義します。これは、ロボットの最終位置とゴール位置の間の望ましい距離しきい値です。ロボットは、ゴールからこの距離以内に達すると停止します。さらに、ロボットの位置とゴール位置の現在の距離を計算します。この距離がゴール半径に対して継続的にチェックされ、この距離がゴール半径より小さくなったときにロボットは停止します。

ゴール半径の値が小さすぎると、ロボットがゴールを見失い、ゴール近くで予期しない動作が発生する場合があります。

goalRadius = 0.1; distanceToGoal = norm(robotInitialLocation - robotGoal);

controllerPurePursuitオブジェクトは、ロボットの制御コマンドを計算します。これらの制御コマンドを使用して、ゴール半径内に達するまでロボットを駆動します。外部のシミュレーターまたは物理的ロボットを使用している場合は、コントローラーの出力をロボットに適用しなければならず、ロボットの姿勢を更新するために位置推定システムが必要になる場合があります。コントローラーは 10 Hz で動作します。

% Initialize the simulation loopsampleTime = 0.1; vizRate = rateControl(1/sampleTime);% Initialize the figurefigure% Determine vehicle frame size to most closely represent vehicle with plotTransformsframeSize = robot.TrackWidth/0.8;while( distanceToGoal > goalRadius )% Compute the controller outputs, i.e., the inputs to the robotv,ω=控制器(robotCurrentPose);% Get the robot's velocity using controller inputsvel = derivative(robot, robotCurrentPose, [v omega]);% Update the current poserobotCurrentPose = robotCurrentPose + vel*sampleTime;% Re-compute the distance to the goaldistanceToGoal = norm(robotCurrentPose(1:2) - robotGoal(:));% Update the plotholdoff% Plot path each instance so that it stays persistent while robot mesh% movesplot(path(:,1), path(:,2),"k--d") holdall% Plot the path of the robot as a set of transformsplotTrVec = [robotCurrentPose(1:2); 0]; plotRot = axang2quat([0 0 1 robotCurrentPose(3)]); plotTransforms(plotTrVec', plotRot,"MeshFilePath","groundvehicle.stl","Parent", gca,"View","2D","FrameSize", frameSize); light; xlim([0 13]) ylim([0 13]) waitfor(vizRate);end

PRM に沿ったパス追従コントローラーの使用

目的の一連の中間点がパスプランナーによって計算されている場合、パス追従コントローラーを同じ方法で使用できます。まず、マップを可視化します。

loadexampleMapsmap = binaryOccupancyMap(simpleMap); figure show(map)

pathは、PRM パス計画アルゴリズムを使用して計算できます。詳細については、複雑度の異なる環境でのパス計画を参照してください。

mapInflated = copy(map); inflate(mapInflated, robot.TrackWidth/2); prm = robotics.PRM(mapInflated); prm.NumNodes = 100; prm.ConnectionDistance = 10;

開始位置と終了位置の間のパスを見つけます。PRM アルゴリズムの確率的性質により、pathが異なることに注意してください。

startLocation = [4.0 2.0]; endLocation = [24.0 20.0]; path = findpath(prm, startLocation, endLocation)

path =8×24.0000 2.0000 3.1703 2.7616 7.0797 11.2229 8.1337 13.4835 14.0707 17.3248 16.8068 18.7834 24.4564 20.6514 24.0000 20.0000

インフレートされたマップ、ロードマップおよび最終パスを表示します。

show(prm);

上記で定義したパス追従コントローラーを再利用して、このマップ上のロボットの制御コマンドを計算できます。他の情報を同じ状態に維持したまま、コントローラーを再利用して中間点を再定義するには、関数releaseを使用します。

release(controller); controller.Waypoints = path;

ロボットの初期位置とゴールを、パスによる定義で設定します。

robotInitialLocation = path(1,:); robotGoal = path(end,:);

ロボットの最初の向きを仮定します。

initialOrientation = 0;

ロボット運動の現在の姿勢 [x y theta] を定義します。

robotCurrentPose = [robotInitialLocation initialOrientation]';

ゴール位置までの距離を計算します。

distanceToGoal = norm(robotInitialLocation - robotGoal);

ゴール半径を定義します。

goalRadius = 0.1;

指定したマップ上でコントローラー出力を使用して、ゴールに到達するまでロボットを駆動します。コントローラーは 10 Hz で動作します。

reset(vizRate);% Initialize the figurefigurewhile( distanceToGoal > goalRadius )% Compute the controller outputs, i.e., the inputs to the robotv,ω=控制器(robotCurrentPose);% Get the robot's velocity using controller inputsvel = derivative(robot, robotCurrentPose, [v omega]);% Update the current poserobotCurrentPose = robotCurrentPose + vel*sampleTime;% Re-compute the distance to the goaldistanceToGoal = norm(robotCurrentPose(1:2) - robotGoal(:));% Update the plotholdoffshow(map); holdall% Plot path each instance so that it stays persistent while robot mesh% movesplot(path(:,1), path(:,2),"k--d")% Plot the path of the robot as a set of transformsplotTrVec = [robotCurrentPose(1:2); 0]; plotRot = axang2quat([0 0 1 robotCurrentPose(3)]); plotTransforms(plotTrVec', plotRot,'MeshFilePath','groundvehicle.stl','Parent', gca,"View","2D","FrameSize", frameSize); light; xlim([0 27]) ylim([0 26]) waitfor(vizRate);end