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

TurtleBot による障害物回避

この例では、TurtleBot® での VFH+ 障害物回避アルゴリズムの実装を説明します。タイマーの使い方について調べ、自律アルゴリズムにおけるその威力を明らかにします。

VFH+ アルゴリズムは、ロボットが障害物に衝突することなく空間を移動する上で役立つ、シンプルなローカル メソッドです。このアルゴリズムは局所情報のみを組み込むため、ロボットがターゲット ポイントに到達することは保証されません。コーナー (局所的最小値) で立ち往生することがあります。目標への照準ゲインが障害物回避のゲインよりかなり大きい場合、壁に衝突することがあります。目標への照準ゲインが障害物回避のゲインよりかなり小さい場合、闇雲に動くことがあります。アルゴリズムのゲインとパラメーターを試して、ロボットの異なるタイプの動作を調べます。

この例では、TurtleBot でのシンプルな障害物回避を作成してテストする方法を説明します。Robotics System Toolbox™ には、VFH+ 障害物回避アルゴリズムのより強力な実装が robotics.VectorFieldHistogram に含まれています。障害回避アルゴリズムのパフォーマンスは、Kinect® センサーの制限、つまり最小探知距離と制約された視野の制限を受けます。

必要条件: 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® TurtleBot World を使用すると非常に興味深い環境が提供されます。

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

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

実際の TurtleBot ハードウェアを操作する場合は、Kinect カメラが起動していることを確認してください。カメラを起動するコマンドは roslaunch turtlebot_bringup 3dsensor.launch です。このコマンドを TurtleBot のターミナルで実行します。

障害物回避アルゴリズムの初期化

VFH+ アルゴリズムで使用されるゲインを含む struct を生成します。ロボットの動作を変更するには、タイマーを初期化する前にこれらのゲインを変更します。ゲインは、目標への照準、直線での移動、連続パス上の移動、および障害物との衝突回避の 4 つの動作を制御します。ゲインが異なると動作も異なります。たとえば、obstaclesAvoid = 0 の場合、距離に関係なくロボットは障害物をかき分けて進もうとします。goalTargeting = 0 の場合、ロボットは障害物を回避しながら闇雲に動き回ります。適切なパラメーターの選択は難しく、ロボットの周辺環境に左右されます。この例では、適切なパラメーターの選択に役立つようにより多くの情報を提供します。以下のゲインは、目標への照準と障害物回避のバランスをうまくとるよう試みるものです。

gains.goalTargeting = 100;          % Gain for desire to reach goal
gains.forwardPath = 0;            % Gain for moving forward 
gains.continuousPath = 0;         % Gain for maintaining continuous path
gains.obstacleAvoid = 5;        % Gain for avoiding obstacles

メモ: Kinect レーザー スキャンには最小探知距離があります。この最小探知距離があるため、TurtleBot は一部の障害物をうまく回避した後、障害物が非常に近くなったときに向きを変え、障害物に向かって追突することがあります。この動作の理由は、レーザーが障害物を検出しないことにあります。この動作は出入り口で発生することが多々あります。出入り口では、TurtleBot は完全に敷居を越えないうちに目標位置へと向きを変えます。この時点でドア フレームは近すぎて検出できず、TurtleBot はその存在を認識せずにドア フレームに向かって進みます。この問題は、ローカル プラニング アルゴリズムとレーザー スキャナーの制限された探知距離の組み合わせによる欠点です。実際のハードウェアでは、この場合衝突センサーを有効にしなければなりませんが、シミュレーションでは衝突センサーは機能しないため、ロボットがドア フレームに引っかかることがあります。

パブリッシャーとサブスクライバーを作成し、タイマーの作成時にタイマーに渡される struct (timerHandles) の一部とします。パブリッシャーは速度用であり、サブスクライバーはレーザー スキャナー、オドメトリ、および衝突センサー用です。

timerHandles.pub = rospublisher('/mobile_base/commands/velocity'); % Set up publisher
timerHandles.pubmsg = rosmessage('geometry_msgs/Twist');

timerHandles.sublaser = rossubscriber('/scan');  % Set up subscribers
timerHandles.subodom = rossubscriber('/odom');
timerHandles.subbump = rossubscriber('/mobile_base/sensors/bumper_pointcloud');

先に進む前にオドメトリをリセットする場合は、reset_odometry トピックをサブスクライブし、それに空のメッセージを送信しなければなりません。

odomresetpub = rospublisher('/mobile_base/commands/reset_odometry');  % Reset odometry 
odomresetmsg = rosmessage('std_msgs/Empty');
send(odomresetpub,odomresetmsg)
pause(2);     % Wait until odometry is reset

ゲインを timerHandles struct に追加します。

timerHandles.gains = gains;

障害物回避のテスト

タイマーを初期化します。関数 timer は一連の名前と値の引数ペアを受け取ります。最初のペアはタイマーのコールバック関数であり、これは先に定義した struct も含みます。2 番目のペアはタイマーの期間を定義します (この例ではループあたり 0.1 秒)。3 番目で最後の名前と値のペアは、固定間隔の実行モードを定義します。タイマーの停止関数も定義でき、この場合では、タイマーが停止すると ROS をシャットダウンします。

timer1 = timer('TimerFcn',{@exampleHelperTurtleBotObstacleTimer,timerHandles},'Period',0.1,'ExecutionMode','fixedSpacing');
timer1.StopFcn = {@exampleHelperTurtleBotStopCallback};

タイマーを開始する前に、コマンド ラインでアルゴリズムの一部のステップを可視化できます。関数 exampleHelperTurtleBotShowGrid を使用して、VFH+ アルゴリズムの基本的な動作を確認できます。3 つのプロットが表示されます。図 1 には、生のレーザー データを並べ替えた後の 2 次元ヒストグラムが示されます。図 2 には、ロボットの幅を考慮して平滑化した後のヒストグラムが示されます。図 3 には、角度のヒストグラムが示されます。これは、ロボットが移動可能な方向に従って障害物をビンにグループ化することで作成されます。VFH+ アルゴリズムはこれらのステップを使用して、目標位置に照準しつつ障害物を回避する方法を決定します。

exampleHelperTurtleBotShowGrid(timerHandles.sublaser);

"角度ビン" のプロットは、2 つの x-y グリッド プロットを視覚的に反転したものです。グリッドは実際の 2 次元平面と実際の X 軸および Y 軸を表します (ただし、座標はビンを表しているため現実世界には対応しません)。角度プロットにおいて、角度ビンは左から右へと増加順にリストされますが、慣例により 2 次元平面でのこれらの角度は、右から左への動きによる空間に対応します。

ロボットの実行

以下のコマンドを入力してタイマーを開始します。TurtleBot を送信する点を選択します。TurtleBot が動き始め、検出されたすべての障害物を回避します。ロボットがターゲットに到達するとタイマーは自動停止します。

start(timer1);
while strcmp(timer1.Running, 'on')
    exampleHelperTurtleBotShowGrid(timerHandles.sublaser);
    pause(0.5);    
end

ループの途中でタイマーを停止させるには、Figure ウィンドウを閉じます。タイマーがクリアされない場合、すべてのタイマーをクリアする安全な方法は次のコマンドを使用することです。

delete(timerfindall)

起動時の Gazebo シミュレーションとワールドのプロットは次の図のようになります。

隣室内の点をターゲットとして選択すると、次の図のような表示が得られます。

ROS ネットワークのシャットダウン

終了したら、パブリッシャーとサブスクライバーをクリアします。ROS ネットワークをシャットダウンして Turtlebot から切断します。

clear
rosshutdown
Shutting down global node /matlab_global_node_17598 with NodeURI http://192.168.203.1:50399/

詳細

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

この例のコードは変更可能であり、より自由に TurtleBot を詳しく調べることができます。以下は、例の特徴の説明と、変更や別の使用法に関する推奨事項です。

このスクリプトではtimerを使用しており、これは多くの異なる方法で使用できます。この例では TimerFcn および StopFcn のコールバックを使用します。

タイマー コールバックおよびベース ワークスペースは、ゲイン、パブリッシャー、およびサブスクライバーを含む timerHandles struct を介して情報を共有します。タイマー コールバックに含まれるベース ワークスペースからの追加情報が必要な場合は、それを timerHandles struct に追加します。

タイマーが開始すると、タイマー コールバックはパラメーターに従って実行されます。この例で、タイマー コールバック (exampleHelperTurtleBotObstacleTimer) はアルゴリズムの主な関数を実行する 2 つの主要な関数を呼び出します。以下に基本構造を示します (宣言、初期化、およびコールバック関数を除く)。

function exampleHelperTurtleBotObstacleTimer(mTimer, event, handles)
   % Declarations and Initializations would be here
   % Determine current time
   currentTime = datetime(event.Data.time);
   % Execute the VFH+ algorithm to determine the desired angle trajectory
   angleTarget = exampleHelperTurtleBotComputeTargetAngle(goal, data, pose, anglePrev, handles.gains);
   % Execute the main control loop        
   [linV, angV] = turtlebotController(currentTime,angleTarget,bumper);
   % Set the velocities
   handles.pubmsg.Linear.X = linV;
   handles.pubmsg.Angular.Z = angV;
   send(handles.pub,handles.pubmsg);
   % Set the previous angle
   anglePrev = angleTarget;
   % Callback functions would be here

最初の主要な関数は exampleHelperTurtleBotComputeTargetAngle であり、ロボットが向きを変えるターゲット角度 (ロボットの現在の向きが基準) を返します。この例の computeTargetAngle は VFH+ アルゴリズムの実装です。関数 computeTargetAngle は、ターゲット角度を返す任意のユーザー定義関数に置き換えることができます。その可能性をもつ多くのアルゴリズムを調べることができます。VFH+ アルゴリズムの基本ステップは Ulrich および Borenstein の論文から引用されたものです [1]。これらはアルゴリズムの重要なステップです。

ヒストグラムのビンのサイズと、レーザー スキャンおよび角度のヒストグラムの最大値と最小値を調整する場合は、exampleHelperTurtleBotComputeTargetAngle 内の setBin というローカル関数を参照してください。x と y の最小値と最大値やステップ サイズなど、任意のパラメーターを調整できます。ロボットの物理的な制限に注意してください (たとえば、ロボットは後方を見ることができないため、y の最小値が負になってはなりません)。

タイマー コールバックの 2 番目の主要メソッドは turtlebotController であり、目標位置、現在の姿勢、ターゲット角度などの引数が指定されると、線形速度と角速度を返します。このコントローラーは、線形速度と角速度 (m/s 単位) を返す任意のユーザー定義コントローラーに置き換えることができます。関数 turtlebotController 内に ExampleHelperPIDControl クラスがあります。現在実装されているように、turtlebotController は比例制御のみを使用します。ただし、PID 制御クラスには比例、微分、および積分の制御オプションがあり、次の方法で使用できます。linGains.pgain = 0.2; linGains.dgain = 0; linGains.igain = 0; linGains.maxwindup = 0; linGains.setpoint = 0; linPID = ExampleHelperPIDControl(linGains);

ゲインをより素早く設定する方法は次のとおりです。linGains = struct('pgain',0.2,'dgain',0,'igain',0,'maxwindup',0','setpoint',0);

ゲインはロボットの動作の大きな違いにつながるため、選択したゲインのテストと調整を必ず行ってください。setpoint はその付近で制御を行う点です。構造体の他の要素には、わかりやすい名前が付けられています。コントローラーを更新して制御値を返すには、関数 update を使用します。

       controlvalue = update(linPID,currentpoint);

サブスクライバーのコールバック関数は、タイマー コールバック関数内の入れ子関数として定義されています。posegoalbumper などの重要な変数はそれらのコールバック内で定義されています。タイマー コールバック関数とサポート関数はモジュール方式であり、カスタマイズでの柔軟性が全体的に高くなっています。

次のステップ

参考文献

[1] I. Ulrich, J. Borenstein, "VFH+: reliable obstacle avoidance for fast mobile robots," In Proceedings of IEEE® International Conference on Robotics and Automation (ICRA), 1998, vol. 2, pp. 1572-1577