最新のリリースでは、このページがまだ翻訳されていません。 このページの最新版は英語でご覧になれます。

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

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

メモ: 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];

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

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

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

initialOrientation = 0;

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

robotCurrentPose = [robotCurrentLocation initialOrientation];

ロボット シミュレーターの初期化

この例では、指定した制御入力に対する差動駆動型ロボットの姿勢を更新して返す、単純なロボット シミュレーターを使用します。外部のシミュレーターや物理的ロボットの場合は、ロボットの更新された姿勢を提供するために位置推定メカニズムが必要です。

ロボット シミュレーターを初期化して初期姿勢を割り当てます。シミュレートされるロボットは、2 輪の差動駆動型ロボットの運動の運動学的方程式をもちます。このシミュレートされるロボットへの入力は、線形速度と角速度です。ロボットの現在位置を表示したり、ロボットの軌跡を描画したりするプロット機能もあります。

robotRadius = 0.4;
robot = ExampleHelperRobotSimulator('emptyMap',2);
robot.enableLaser(false);
robot.setRobotSize(robotRadius);
robot.showTrajectory(true);
robot.setRobotPose(robotCurrentPose);

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

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

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

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

controller = robotics.PurePursuit;

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

controller.Waypoints = path;

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

controller.DesiredLinearVelocity = 0.3;

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

controller.MaxAngularVelocity = 2;

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

controller.LookaheadDistance = 0.5;

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

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

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

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

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

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

controlRate = robotics.Rate(10);
while( distanceToGoal > goalRadius )
    
    % Compute the controller outputs, i.e., the inputs to the robot
    [v, omega] = controller(robot.getRobotPose);
    
    % Simulate the robot using the controller outputs.
    drive(robot, v, omega);
    
    % Extract current location information ([X,Y]) from the current pose of the
    % robot
    robotCurrentPose = robot.getRobotPose;
    
    % Re-compute the distance to the goal
    distanceToGoal = norm(robotCurrentPose(1:2) - robotGoal);
    
    waitfor(controlRate);
    
end

パス追従コントローラーを目的のパスに沿って使用して、シミュレートされたロボットがゴール位置に到達しました。シミュレーションを終了します。

delete(robot)

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

目的の一連の中間点がパス プランナーによって計算されている場合、パス追従コントローラーを同じ方法で使用できます。単純なマップでロボット シミュレーターを起動します。

robot = ExampleHelperRobotSimulator('simpleMap',2);
robot.enableLaser(false);
robot.setRobotSize(robotRadius);

robot.showTrajectory(true);

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

mapInflated = copy(robot.Map);
inflate(mapInflated,robotRadius);
prm = robotics.PRM(mapInflated);
prm.NumNodes = 100;
prm.ConnectionDistance = 10;

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

startLocation = [2.0 1.0];
endLocation = [12.0 10.0];
path = findpath(prm, startLocation, endLocation)
path = 6×2

    2.0000    1.0000
    1.5851    1.3808
    4.0668    6.7417
    7.0353    8.6624
   11.8252   10.6359
   12.0000   10.0000

パスを表示します。

show(prm, 'Map', 'off', 'Roadmap', 'off');

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

release(controller);
controller.Waypoints = path;

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

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

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

initialOrientation = 0;

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

robotCurrentPose = [robotCurrentLocation initialOrientation];

シミュレートするロボットの現在位置を、パスの開始位置にリセットします。

robot.setRobotPose(robotCurrentPose)
show(prm, 'Map', 'off', 'Roadmap', 'off');

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

distanceToGoal = norm(robotCurrentLocation - robotGoal);

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

goalRadius = 0.1;

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

reset(controlRate);
while( distanceToGoal > goalRadius )
    
    % Compute the controller outputs, i.e., the inputs to the robot
    [v, omega] = controller(robot.getRobotPose);
    
    % Simulate the robot using the controller outputs
    drive(robot, v, omega);
    
    % Extract current location information from the current pose
    robotCurrentPose = robot.getRobotPose;
    
    % Re-compute the distance to the goal
    distanceToGoal = norm(robotCurrentPose(1:2) - robotGoal);
    
    waitfor(controlRate);
end

パス追従コントローラーを目的のパスに沿って使用して、シミュレートされたロボットがゴール位置に到達しました。ロボットを停止します。

drive(robot, 0, 0);

シミュレーションを終了します。

delete(robot);

参考