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