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

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

メモ: 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.3 メートル/秒に設定します。

controller.DesiredLinearVelocity = 0.6;

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

controller.MaxAngularVelocity = 2;

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

controller.LookaheadDistance = 0.3;

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

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

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

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

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

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

% Initialize the simulation loop
sampleTime = 0.1;
vizRate = rateControl(1/sampleTime);

% Initialize the figure
figure

% Determine vehicle frame size to most closely represent vehicle with plotTransforms
frameSize = robot.TrackWidth/0.8;

while( distanceToGoal > goalRadius )
    
    % Compute the controller outputs, i.e., the inputs to the robot
    [v, omega] = controller(robotCurrentPose);
    
    % Get the robot's velocity using controller inputs
    vel = derivative(robot, robotCurrentPose, [v omega]);
    
    % Update the current pose
    robotCurrentPose = robotCurrentPose + vel*sampleTime; 
    
    % Re-compute the distance to the goal
    distanceToGoal = norm(robotCurrentPose(1:2) - robotGoal(:));
    
    % Update the plot
    hold off
    
    % Plot path each instance so that it stays persistent while robot mesh
    % moves
    plot(path(:,1), path(:,2),"k--d")
    hold all
    
    % Plot the path of the robot as a set of transforms
    plotTrVec = [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 に沿ったパス追従コントローラーの使用

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

load exampleMaps
map = 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×2

    4.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 figure
figure

while( distanceToGoal > goalRadius )
    
    % Compute the controller outputs, i.e., the inputs to the robot
    [v, omega] = controller(robotCurrentPose);
    
    % Get the robot's velocity using controller inputs
    vel = derivative(robot, robotCurrentPose, [v omega]);
    
    % Update the current pose
    robotCurrentPose = robotCurrentPose + vel*sampleTime; 
    
    % Re-compute the distance to the goal
    distanceToGoal = norm(robotCurrentPose(1:2) - robotGoal(:));
    
    % Update the plot
    hold off
    show(map);
    hold all

    % Plot path each instance so that it stays persistent while robot mesh
    % moves
    plot(path(:,1), path(:,2),"k--d")
    
    % Plot the path of the robot as a set of transforms
    plotTrVec = [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

参考