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

オブジェクトの追跡と追従

この例では、Kinect® カメラを組み込んだ自律動作を調べます。このアルゴリズムでは TurtleBot® が青色のボールを探し、そのボールから一定の距離をとって留まります。衝突検知や段差検知などの安全機能を組み込みます。

"この例を実行するには、Image Processing Toolbox™ が必要です。"

必要条件: TurtleBot との通信TurtleBot の基本動作の確認遠隔操作による TurtleBot の制御TurtleBot による障害物回避

TurtleBot のハードウェア サポート パッケージ

この例では、ネイティブの ROS インターフェイスを使用した TurtleBot の操作の概要を示します。TurtleBot ベースのロボット向けの Robotics System Toolbox™ サポート パッケージは、TurtleBot に対するより効率のよいインターフェイスを提供します。次を行うことができます。

ROS コマンドを明示的に呼び出すことなくセンサー データを取得し、制御コマンドを送信

Gazebo のシミュレートされたロボットまたは物理的な TurtleBot と透過的に通信

サポート パッケージをインストールするには、MATLAB® の [ホーム] タブで [アドオン]、[ハードウェア サポート パッケージの入手] を開いて、[TurtleBot-Based Robots] を選択します。あるいは、roboticsAddons コマンドを使用します。

TurtleBot への接続

Gazebo® でのシミュレーション内で、または実際のハードウェア上で TurtleBot が実行されていることを確認します。起動手順については、Gazebo およびシミュレートされた TurtleBot の入門または実際の TurtleBot 入門を参照してください。ハードウェアを使用する場合は、追跡に使用する青色のボールを用意します。Gazebo® を使用する場合は、青色のボールはワールド内のロボットの前方になければなりません (必ず Gazebo TurtleBot World を使用すること)。

ROS を初期化します。ipaddress を TurtleBot の IP アドレスに置き換えて、TurtleBot に接続します。

ipaddress = '192.168.203.129';
rosinit(ipaddress)
Initializing global node /matlab_global_node_46760 with NodeURI http://192.168.203.1:53697/

実際の TurtleBot ハードウェアを操作する場合は、Kinect カメラが起動していることを確認してください。カメラを起動するコマンドは次のとおりです。

roslaunch turtlebot_bringup 3dsensor.launch. 

これを TurtleBot のターミナルに入力しなければなりません。

カラー カメラ、段差センサー、および衝突センサーのサブスクライバーを作成します。音声送出用と、ロボットの速度メッセージの制御用のパブリッシャーを作成します。

handles.colorImgSub = exampleHelperTurtleBotEnableColorCamera;
Successfully Enabled Camera (raw image)
handles.cliffSub = rossubscriber('/mobile_base/events/cliff', 'BufferSize', 5);
handles.bumpSub = rossubscriber('/mobile_base/sensors/bumper_pointcloud', 'BufferSize', 5);
handles.soundPub = rospublisher('/mobile_base/commands/sound', 'kobuki_msgs/Sound');
handles.velPub = rospublisher('/mobile_base/commands/velocity');

青色のボール検出の調整

イメージのフィルター処理用にパラメーターを設定します。アルゴリズムで使用するデータ構造にそれらを追加します。

blueBallParams.blueMax = 120;  % Maximum permissible deviation from pure blue
blueBallParams.darkMin = 30;   % Minimum acceptable darkness value

ボールを可視化して、ボール検出パラメーターがボールを探せることを確認します。関数 exampleHelperTurtleBotFindBlueBall を実行して、円が見つかるかどうかを確認します。見つかる場合、cm は割り当てられる値です。ball はイメージに青フィルターと暗さフィルターを適用して作成されたバイナリ イメージです。ball を表示して、青色のボールが適切に分離されたかどうかを確認します。

latestImg = readImage(handles.colorImgSub.LatestMessage);
[c,~,ball] = exampleHelperTurtleBotFindBlueBall(latestImg,blueBallParams);

この例のヘルパーを使用して Figure に現実のバイナリ イメージを表示し、ボールの中心に赤の十字をプロットします。

exampleHelperTurtleBotPlotObject(latestImg,ball,c);

ボールが見つからない場合は、blueBallParams.blueMax および blueBallParams.darkMin を増減してみます。ボールが見つかるまで、プロットの表示を繰り返します。このメソッドは、コントローラーを使用する前にボール検出アルゴリズムを微調整するための優れた方法です。

Gazebo ではしきい値に余裕がありすぎるため、使用されたパラメーターではボールが検出されない可能性があります。Gazebo のイメージ (左側の Figure) では、壁と他のオブジェクトの一部が白の領域に含まれています。現実のイメージ (右側の Figure) は白がかなり浸透しているように見えます。より限定的になるようにパラメーターを変更してみます。

blueBallParams.blueMax = 10; % Maximum permissible deviation from pure blue
blueBallParams.darkMin = 220; % Minimum acceptable darkness value
latestImg = readImage(handles.colorImgSub.LatestMessage);
[c,~,ball] = exampleHelperTurtleBotFindBlueBall(latestImg,blueBallParams);

この例のヘルパーを使用して Figure を表示します。

exampleHelperTurtleBotPlotObject(latestImg,ball,c);

今度はパラメーターが限定的になり過ぎました。Gazebo のイメージではボールの一部が表示されず、現実のイメージでは何も見えません。さらにパラメーターを調整すると、妥協点を見つけることができます。Gazebo では以下のパラメーターが良好に機能します。ハードウェアの場合、周囲光のため、パラメーターの微調整に長時間を要することがあります。

  blueBallParams.blueMax = 30; % Maximum permissible deviation from pure blue
  blueBallParams.darkMin = 90; % Minimum acceptable darkness value
  latestImg = readImage(handles.colorImgSub.LatestMessage);
  [c,~,ball] = exampleHelperTurtleBotFindBlueBall(latestImg,blueBallParams);

この例のヘルパーを使用して Figure を表示します。

exampleHelperTurtleBotPlotObject(latestImg,ball,c);

Gazebo のようなシミュレーション環境での調整と比較して、色のしきい値の調整は困難です。

パラメーターを微調整したら、ボール追跡アルゴリズムで使用される handles オブジェクトに追加します。

  handles.params = blueBallParams;

固定距離コントローラーのテスト

TurtleBot のコントローラー ゲインを設定します。TurtleBot は PID コントローラーを使用して、ボールから一定の距離に留まります。

最初のコントローラー ゲインのセットは、Gazebo での TurtleBot に適しています。2 番目のセットは実際のハードウェアでの TurtleBot に適しています。適切と思う値にゲインを調整します。

以下は、struct 値のコンパクトな割り当て方法です。

Gazebo シミュレーションで効果的なゲイン:

  gains.lin = struct('pgain',1/100,'dgain',1/100,'igain',0,'maxwindup',0','setpoint',0.65);
  gains.ang = struct('pgain',1/400,'dgain',1/500,'igain',0,'maxwindup',0','setpoint',0.5);
 

TurtleBot ハードウェアで効果的なゲイン:

gains.lin = struct('pgain',1/100,'dgain',1/1000,'igain',0,'maxwindup',0','setpoint',0.75);
gains.ang = struct('pgain',1/100,'dgain',1/3000,'igain',0,'maxwindup',0','setpoint',0.5);

変数 handlesgains struct を必ず追加します。

handles.gains = gains;

コールバックを介してボール追跡動作を実行するタイマーを定義します。ROS をシャットダウンする停止関数を定義します。タイマーのコールバック関数にハンドルを含めます。

timer2 = timer('TimerFcn',{@exampleHelperTurtleBotTrackingTimer,handles},'Period',0.1,'ExecutionMode','fixedSpacing');
timer2.StopFcn = {@exampleHelperTurtleBotStopCallback};

以下のコマンドを使用してタイマーを開始します。TurtleBot がボールを探してワールドを動き回り始めることを確認します。Kinect イメージ内でボールを見つけると、ロボットはコントローラーを使用して一定距離に留まります。

start(timer2);
pause(1);

シミュレーションで衝突センサーは有効になっていないため、TurtleBot は壁に衝突すると回復しないことがあります。

青色のボールをあちこちに動かす場合は、次のコマンドを使用して力を加えます。

g = ExampleHelperGazeboCommunicator();
    ballhandle = ExampleHelperGazeboSpawnedModel('unit_sphere_1',g)
    duration = 2;
    forceVector = [0 4 0];
    applyForce(ballhandle,'link',duration,forceVector)

Gazebo によるシミュレーション制御をさらに調べる場合は、Gazebo からのモデルおよびシミュレーションの特性の読み取りおよびGazebo でのオブジェクトの追加、作成、および削除を参照してください。

ロボットの動作の停止

タイマーと自律動作を停止するには、次のコマンドを使用します。

stop(timer2);

停止する前にタイマーがワークスペースからクリアされた場合は、他の方法でタイマーを削除しなければなりません。すべてのタイマー (バックグラウンドのタイマーも含む) を停止するには、次のコマンドを実行します。

delete(timerfindall)

作業が終了したら、パブリッシャー、サブスクライバー、およびその他 ROS 関連のオブジェクトをワークスペースからクリアします。

clear

詳細

メモ: この節のコードは、MATLAB のコマンド ラインで実行するためのものではありません。

この例では、サポートするファイルの構成により、コードのカスタマイズと転用が非常に柔軟に行えるようになります。handles struct の値を変更することにより、ボール検出パラメーターとコントローラー ゲインを変更できます。この例には、制御アルゴリズムのあらゆる面を管理する timer が組み込まれています。このタイマーは exampleHelperTurtleBotTrackingTimer です。このタイマーには Period および ExecutionMode の名前と値のペアがあり、これらを設定して、タイマー コールバックを呼び出す頻度が決定されます。さらに、停止コールバックが使用されます。必要に応じて、追加のコールバック関数を組み込むことができます。

タイマーに渡されるハンドルには、ボール検出用の params とコントローラー用の gains が含まれています。

exampleHelperTurtleBotTrackingTimer の構造はシンプルです。これは、いくつかの初期化ステップをもつ基本的なステート マシンです。段差または衝突からの回復状態でない場合、使用する追跡アルゴリズムとコントローラーを初期化関数が決定します。関数は次のようになります。

function [objectTrack, imgControl] = initControl()
   % INITCONTROL - Initialization function to determine which control
   % and object detection algorithms to use
   objectTrack = @exampleHelperTurtleBotFindBlueBall;
   imgControl = @exampleHelperTurtleBotPointController;     

この例の追跡関数は exampleHelperTurtleBotFindBlueBall で、コントローラーは exampleHelperTurtleBotPointController です。この関数とコントローラーを、同じ入出力引数の構造体をもつ任意のユーザー定義関数に置き換えることができます。exampleHelperTurtleBotFindBlueBall の入力引数は、カラー イメージと、ボール検出パラメーターの struct です。出力引数は、求められたオブジェクトの中心、大きさ、およびバイナリ イメージです。exampleHelperTurtleBotPointController の入力引数は、オブジェクトの中心、大きさ (ただし、この例で大きさは使用しない)、イメージ サイズ、およびコントローラー ゲイン (struct) です。出力引数は線形速度と角速度です。

exampleHelperTurtleBotTrackingTimer で使用される基本的なステート マシンは以下のとおりです。

switch state
        case ExampleHelperTurtleBotStates.Seek
            % Object-finding state
            [center, scale] = findObject(handles.Tbot.ImColor,handles.params);
            % Wander if no circle is found, target the circle if it exists
            if isempty(center)
                [linearV, angularV] = exampleHelperTurtleBotWanderController();
            else
                [linearV, angularV] = imageControl(center, scale, size(handles.Tbot.ImColor),handles.gains);
                setSound(handles.Tbot,2);
            end
            state = ExampleHelperTurtleBotStates.Seek;           
        case ExampleHelperTurtleBotStates.Bumper
            % Bumper contact state
        case ExampleHelperTurtleBotStates.Spin
            % Spin state            
        case ExampleHelperTurtleBotStates.Cliff
            % Cliff avoidance
end

このステート マシンから、ケースの追加や削除を行うことができます。ステート名を変更する場合は、ExampleHelperTurtleBotStates クラスを使用します。

ボール検出アルゴリズムはモジュール形式であり、変更可能です。2 つのイメージ フィルター (1 つは暗さ、もう 1 つは青色が対象) を合わせたマスクを使用して、青色のボールを分離します。マスクを変更して、代わりに赤や緑のボールを見つけることができます。形状追跡の他の形式を調べる場合、基本的なワークフローは同じです。

青のチャネルが分離され (いくつかのスケーリング係数を使用)、しきい値を適用してバイナリ イメージのマスクを生成します。

blueImg = img(:,:,1)/2 + img(:,:,2)/2 - img(:,:,3)/2;
blueThresh = blueImg < params.blueMax;

次のコマンドは青の補色を分離し (異なるスケーリングを使用)、暗さを強調します。しきい値が適用されます。

darkIso = -img(:,:,1)/2 - img(:,:,2)/2 + 3*img(:,:,3) - 2*rgb2gray(img);
darkThresh = darkIso > params.darkMin;

2 つのバイナリ イメージをいっしょにマスクして、暗青色のボールを分離します。

ball1 = blueThresh & darkThresh;

イメージに適用する定数とスケーリング係数は、特定の色を分離するためにユーザーが決定します。さまざまな組み合わせを試すことができます。

また、Image Processing Toolbox に含まれる regionprops を使用して、フィルター処理されたイメージ内にある連続領域を検出することもできます。

s = regionprops(ball1, {'Centroid','Area','EquivDiameter'});

この領域からボールを検出する追加のステップが exampleHelperTurtleBotFindBlueBall にあります。

関数 exampleHelperTurtleBotPointControllerExampleHelperPIDControl クラスを使用して、指定点 (この場合はボールの中心) をイメージ内の正確な位置に維持します。

例のコードがもつモジュール性と柔軟性により、独自のアルゴリズムと関数を試すことができます。