このページは前リリースの情報です。該当の英語のページはこのリリースで削除されています。

Simulink® での障害物回避を伴うパス追従

この例では、Simulink を使用して、差動駆動型ロボットのパスを追従しながら障害物を回避する方法を説明します。この例では ROS を使用して、MATLAB® ベースのシミュレーターと情報を送受信します。このシミュレーターは、Gazebo® など他の ROS ベースのシミュレーターに置き換えることができます。

必要条件: Simulink® から ROS 対応ロボットへの接続

はじめに

この例では、障害物回避付きのパス追従コントローラーを実装したモデルを使用します。このコントローラーは、シミュレートされたロボットからロボットの姿勢とレーザー スキャンのデータを受信し、指定されたパス上でロボットを駆動する速度コマンドを送信します。モデルの実行中にパラメーターを調整して、シミュレートされたロボットへの影響を観察できます。

タスク 1 - ロボット シミュレーターの起動

シンプルな MATLAB ベースのシミュレーターを起動します。

  • MATLAB コマンド ラインで rosinit と入力します。これにより、ネットワーク アドレス (URI) http://localhost:11311 をもつローカルの ROS マスターが作成されます。

  • ExampleHelperSimulinkRobotROS('ObstacleAvoidance') と入力してロボット シミュレーターを起動します。これにより、Figure ウィンドウが開きます。

この MATLAB ベースのシミュレーターは、差動駆動型ロボットの ROS ベースのシミュレーターです。このシミュレーターは以下のトピック上でメッセージを送受信します。

  • 速度コマンドを "/mobile_base/commands/velocity" トピック上で "geometry_msgs/Twist" タイプのメッセージとして受信

  • ロボットのグラウンド トゥルース姿勢情報を "nav_msgs/Odometry" タイプのメッセージとして "/ground_truth_pose" トピックに送信

  • レーザー距離データを "sensor_msgs/LaserScan" タイプのメッセージとして "/scan" トピックに送信

MATLAB ベースのシミュレーターを Gazebo に置き換える:

TurtleBot® をシミュレートする Gazebo シミュレーターを使用することもできます。Gazebo 環境を設定する手順については、Gazebo およびシミュレートされた TurtleBot の入門を参照してください。Gazebo とのネットワーク接続を設定する手順については、Simulink® から ROS 対応ロボットへの接続を参照してください。Gazebo でロボットの姿勢を取得する位置推定アルゴリズムも必要です。Gazebo 環境でロボットの位置を検出する手順については、モンテカルロ位置推定を使用した TurtleBot の位置推定を参照してください。

タスク 2 - 既存のモデルを開く

このモデルは、障害物回避アルゴリズムをもつパス追従を実装します。このモデルは 4 つのサブシステムに分かれています。以降の節で各サブシステムについて説明します。

open_system('pathFollowingWithObstacleAvoidanceExample');

入力の処理

'Inputs' サブシステムは、アルゴリズムへのすべての入力を処理します。

シミュレーターからデータを受信する 2 つのサブスクライバーがあります。最初のサブスクライバーは "/scan" トピックの送信されたメッセージを受信します。次にレーザー スキャン メッセージが処理されて、スキャンの距離と角度が抽出されます。2 番目のサブスクライバーは "/ground_truth_pose" トピックの送信されたメッセージを受信します。次に姿勢メッセージから、ロボットの (x,y) 位置とヨー回転が抽出されます。

パスは一連の中間点として指定されます。この例では 3 行 2 列の定数入力を使用します。任意の数の中間点を N 行 2 列の配列として指定できます。実行時にパスのサイズを変更するには、可変サイズの信号を使用するか、NaN パディング付きの固定サイズ信号を使用できます。この例では、未知の中間点を考慮して NaN パディング付きの固定サイズ入力を使用します。

open_system('pathFollowingWithObstacleAvoidanceExample/Inputs', 'tab');

パス追従の速度と方向の計算

'Compute Velocity and Heading for Path Following' サブシステムは Pure Pursuit ブロックを使用して、線形速度と角速度のコマンド、およびターゲットの移動方向を計算します。Pure Pursuit ブロックは、ライブラリ ブラウザーの [Robotics System Toolbox] タブ内の Mobile Robot Algorithms サブライブラリにあります。あるいは、コマンド ラインで robotalgslib と入力して、Mobile Robot Algorithms サブライブラリを開くこともできます。

また、ロボットが目標点に到達したら、ロボットを停止しなければなりません。この例では、目標はパス上の最後の中間点です。このサブシステムはまた、現在のロボットの姿勢と目標点を比較して、ロボットが目標に近いかどうかの判定も行います。

open_system('pathFollowingWithObstacleAvoidanceExample/Compute Velocity and Heading for Path following', 'tab');

障害物を回避するための速度の調整

'Adjust Velocities to Avoid Obstacles' サブシステムは、パス追従のサブシステムにより計算された線形速度と角速度に対する調整を計算します。

Vector Field Histogram ブロックはレーザー距離の読み取り値を使用して、Pure Pursuit ブロックを使用して計算されたターゲット方向に障害物がないかどうかを、レーザー スキャン データを基にチェックします。ターゲット方向に障害物がある場合、Vector Field Histogram ブロックは、障害物がなくターゲット方向に最も近いステアリング方向を計算します。Vector Field Histogram ブロックも、Mobile Robot Algorithms サブライブラリにあります。

センサーの視野のすべての方向に障害物がある場合、ステアリング方向は NaN 値になります。この場合、障害物のない方向が見つかるまでロボットがその場で回転する回復動作が必要です。

ステアリング方向に基づいて、このサブシステムは線形速度と角速度の調整を計算します。

open_system('pathFollowingWithObstacleAvoidanceExample/Adjust Velocities to Avoid Obstacles', 'tab');

速度コマンドの送信

'Outputs' サブシステムは、シミュレートされたロボットを駆動する線形速度と角速度をパブリッシュします。Pure Pursuit のパス追従アルゴリズムを使用して計算された速度に、Vector Field Histogram の障害物回避アルゴリズムを使用して計算された調整値を加算します。最終的な速度は "geometry_msgs/Twist" メッセージに設定され、"/mobile_base/commands/velocity" トピックでパブリッシュされます。

これは、新規のレーザー メッセージが受信されるとトリガーされる Enabled Subsystem です。つまり、速度コマンドは、新規のセンサー情報が使用可能な場合にのみパブリッシュされます。これにより、センサー情報の受信が遅延した場合に、ロボットによる障害物への衝突が防止されます。

open_system('pathFollowingWithObstacleAvoidanceExample/Outputs', 'tab');

タスク 3 - モデルの構成と実行

モデルを構成して実行し、シミュレーターでロボットの動きを観察します。

  • シミュレーションの [終了時間] を Inf に設定します。

  • [再生] ボタンをクリックしてシミュレーションを開始します。シミュレーションでロボットが動き始めたことを観察します。

  • シミュレーションの実行中に 'Compute Velocity and Heading for Path Following' サブシステムを開き、Pure Pursuit ブロックをダブルクリックします。目的の線形速度のパラメーターを 0.5 に変更します。ロボットの速度上昇を観察します。

  • 既定のパス [2 2;8 8] 上に障害物があります。ロボットが障害物を迂回してパスの終点に到達することを観察します。

  • 'Inputs' サブシステムを開いて Waypoints Input ブロックをダブルクリックします。定数値を [2 2;8 8;NaN NaN] から [2 2;8 8;12 5] に変更します。ロボットは引き続き新規のパスに追従し、障害物を回避しながら新規の目標点 [12, 5] に到達します。

  • シミュレーションを停止するには、[停止] ボタンをクリックします。

まとめ

この例では、障害物回避付きのパス追従コントローラーを作成する方法を説明しました。

参考