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

モンテカルロ位置推定を使用した TurtleBot の位置推定

この例では、シミュレートされた Gazebo® 環境でモンテカルロ位置推定 (MCL) アルゴリズムを TurtleBot® に適用する方法を説明します。

モンテカルロ位置推定 (MCL) は、粒子フィルターを使用してロボットの位置を推定するアルゴリズムです。このアルゴリズムには既知のマップが必要であり、タスクは、ロボットの運動と検出に基づいてマップ内のロボットの姿勢 (位置と向き) を推測することです。このアルゴリズムは、ロボットの姿勢の確率分布に関する初期の信念から開始し、そのような信念に従って分布された粒子によって表現されます。これらの粒子は、ロボットの姿勢が変化するたびに、ロボットの運動モデルに従って伝播されます。新しいセンサー読み取り値を受信すると、現在の姿勢でそのようなセンサー読み取り値を受信する確率をチェックすることにより、各粒子は自身の精度を評価します。次に、アルゴリズムが、粒子をより正確なバイアス粒子に再分布 (リサンプリング) します。こうした移動、検出、リサンプリングの手順を繰り返して位置推定が成功すると、ロボットの実際の姿勢に近い単一のクラスターにすべての粒子が収束します。

適応モンテカルロ位置推定 (AMCL) は、 に実装されている MCL のバリアントです。AMCL は KL 距離 [1] に基づいて粒子の数を動的に調整し、過去のセンサーや運動のすべての測定値に基づいて、高い確率で、粒子分布がロボットの状態の実際の分布に確実に収束するようにします。

現在の MATLAB® AMCL 実装は、レンジ ファインダーを備えたすべての差動駆動型ロボットに適用できます。

"この例を機能させるには、Gazebo TurtleBot シミュレーションを実行しなければなりません。"

必要条件: Gazebo およびシミュレートされた TurtleBot の入門ROS の tf 変換ツリーへのアクセスROS のパブリッシャーおよびサブスクライバーとのデータ交換

メモ: R2016b 以降では、step メソッドを使用して、System object によって定義された演算を実行する代わりに、引数を指定したオブジェクトを関数であるかのように呼び出すことができます。たとえば、y = step(obj,x)y = obj(x) は同等の演算を実行します。

Gazebo での TurtleBot への接続

まず、Gazebo およびシミュレートされた TurtleBot の入門の手順に従ってデスクトップから Gazebo TurtleBot World を起動して、仮想マシンのオフィス環境内でシミュレートされた TurtleBot を発生させます。

ホスト コンピューター上の MATLAB インスタンスで、次のコマンドを実行して MATLAB 内の ROS グローバル ノードを初期化し、IP アドレス ipaddress を使用して仮想マシン内の ROS マスターに接続します。ipaddress は、仮想マシン内の実際の TurtleBot の IP アドレスに置き換えてください。

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

シミュレートしたオフィス環境のレイアウトは次のとおりです。

シミュレーション ワールドのマップの読み込み

Gazebo でオフィス環境のバイナリ占有グリッドを読み込みます。マップは、オフィス環境内で TurtleBot を動かすことによって生成されます。マップは、Kinect® のレンジ-方位の読み取り値と、gazebo/model_states トピックのグラウンド トゥルース姿勢を使用して構築されます。詳細については、既知の姿勢を使用するマッピングを参照してください。

load officemap.mat
show(map)

レーザー センサー モデルと TurtleBot 運動モデルの設定

TurtleBot は差動駆動型ロボットとしてモデル化でき、その運動はオドメトリ データを使用して推定できます。Noise プロパティは、ロボットの回転運動および線形運動の不確定性を定義します。odometryModel.Noise プロパティを大きくすると、オドメトリ測定値を使用して粒子を伝播するときに広く拡散させることができます。プロパティの詳細については、odometryMotionModel を参照してください。

odometryModel = robotics.OdometryMotionModel;
odometryModel.Noise = [0.2 0.2 0.2 0.2];

TurtleBot 上のセンサーは、Kinect の読み取り値から変換した、シミュレートされたレンジ ファインダーです。尤度場の手法を使用して、レンジ ファインダー測定値の終点を占有マップと比較することによって一連の測定値を感知する確率を計算します。占有マップ内の占有点と終点が一致する場合は、そのような測定値を感知する確率が高くなります。より良いテスト結果を得るためには、実際のセンサー プロパティに一致するようにセンサー モデルを調整する必要があります。プロパティ SensorLimits は、センサー読み取り値の最小と最大の範囲を定義します。プロパティ Map は、尤度場の計算に使用する占有マップを定義します。プロパティの詳細については、 を参照してください。

rangeFinderModel = robotics.LikelihoodFieldSensorModel;
rangeFinderModel.SensorLimits = [0.45 8];
rangeFinderModel.Map = map;

rangeFinderModel.SensorPose を、ロボットのベースに対する固定カメラの座標変換に設定します。これは、レーザーの読み取り値をカメラ座標系から TurtleBot のベース座標系に変換するために使用します。座標変換の詳細については、ROS の tf 変換ツリーへのアクセスを参照してください。

現時点の SensorModel は、ロボットのフレームに固定されているセンサーのみと両立します。つまり、センサー変換は一定であるということです。

% Query the Transformation Tree (tf tree) in ROS.
tftree = rostf;
waitForTransform(tftree,'/base_link','/camera_depth_frame');
sensorTransform = getTransform(tftree,'/base_link', '/camera_depth_frame');

% Get the euler rotation angles.
laserQuat = [sensorTransform.Transform.Rotation.W sensorTransform.Transform.Rotation.X ...
    sensorTransform.Transform.Rotation.Y sensorTransform.Transform.Rotation.Z];
laserRotation = quat2eul(laserQuat, 'ZYX');

% Setup the |SensorPose|, which includes the translation along base_link's
% +X, +Y direction in meters and rotation angle along base_link's +Z axis
% in radians.
rangeFinderModel.SensorPose = ...
    [sensorTransform.Transform.Translation.X sensorTransform.Transform.Translation.Y laserRotation(1)];

TurtleBot からセンサー測定値を受信し、TurtleBot に速度コマンドを送信するためのインターフェイス

TurtleBot からセンサーおよびオドメトリの測定値を取得するための ROS サブスクライバーを作成します。

laserSub = rossubscriber('/scan');
odomSub = rossubscriber('/odom');

TurtleBot に速度コマンドを送信するための ROS パブリッシャーを作成します。TurtleBot は速度コマンドを求めて '/mobile_base/commands/velocity' をサブスクライブします。

[velPub,velMsg] = ...
    rospublisher('/mobile_base/commands/velocity','geometry_msgs/Twist');

AMCL オブジェクトの初期化

AMCL オブジェクト amcl をインスタンス化します。クラスの詳細については、を参照してください。

amcl = robotics.MonteCarloLocalization;
amcl.UseLidarScan = true;

amcl オブジェクトの MotionModel プロパティと SensorModel プロパティを割り当てます。

amcl.MotionModel = odometryModel;
amcl.SensorModel = rangeFinderModel;

粒子フィルターは、ロボットの動きが UpdateThresholds を超えたときにのみ粒子を更新します。これは、フィルター更新をトリガーする [x, y, yaw] における最小変位を定義したものです。これで、センサー ノイズによって更新頻度が高くなりすぎることを回避できます。粒子のリサンプリングは、amcl.ResamplingInterval フィルターの更新後に行われます。大きい数値を使用すると、粒子の枯渇を遅らせることができますが、粒子の収束も遅くなります。

amcl.UpdateThresholds = [0.2,0.2,0.2];
amcl.ResamplingInterval = 1;

初期姿勢の推定による位置推定のための AMCL オブジェクトの設定

amcl.ParticleLimits は、リサンプリング プロセス中に生成される粒子の数の下限と上限を定義します。生成される粒子の数を多くすると、実際のロボットの姿勢に収束する可能性が高くなりますが、計算速度に影響が出て、粒子の収束までに長時間かかる場合や、失敗する場合があります。粒子の数に関して妥当な境界値を計算する方法については、[1] の「KL-D Sampling」節を参照してください。グローバル位置推定では、初期姿勢推定による位置推定と比較して、必要な粒子の数が大幅に増える場合があります。いくぶん不確実でも、ロボットが初期姿勢を認識していれば、そのような追加情報を活用して AMCL はより短時間で、より少数の粒子でロボットの位置を推定できます。つまり、amcl.ParticleLimits の上限に小さい値を使用できます。

ここで、amcl.GlobalLocalization を false に設定し、推定される初期姿勢を AMCL に提供します。これにより、AMCL はロボットの実際の姿勢が、平均が amcl.InitialPose に等しく共分散行列が amcl.InitialCovariance に等しいガウス分布に従うという初期信念を保持します。初期姿勢の推定は、設定に従って入手する必要があります。この例のヘルパーでは、ロボットの現在の実際の姿勢を Gazebo から取得します。

グローバル位置推定の使用例については、「グローバル位置推定のための AMCL オブジェクトの設定」節を参照してください。

amcl.ParticleLimits = [500 5000];
amcl.GlobalLocalization = false;
amcl.InitialPose = ExampleHelperAMCLGazeboTruePose;
amcl.InitialCovariance = eye(3)*0.5;

可視化および TurtleBot 駆動のためのヘルパーの設定

マップをプロットし、マップ上でロボットの推定姿勢、粒子、レーザー スキャンの読み取り値を更新する ExampleHelperAMCLVisualization を設定します。

visualizationHelper = ExampleHelperAMCLVisualization(map);

ロボット運動は AMCL アルゴリズムに不可欠です。この例では、ExampleHelperAMCLWanderer クラスを使用して TurtleBot をランダムに駆動します。ロボットは環境内で駆動され、controllerVFH クラスを使用して障害物を回避します。

wanderHelper = ...
    ExampleHelperAMCLWanderer(laserSub, sensorTransform, velPub, velMsg);

位置推定の手順

AMCL アルゴリズムは、ロボットが動き回っているときに、タイム ステップごとにオドメトリとセンサーの読み取り値で更新されます。粒子が初期化され、Figure にプロットされるまで、数秒時間を見てください。この例では、numUpdates AMCL 更新を実行します。ロボットが正しいロボット姿勢に収束しない場合は、numUpdates の値を大きくすることを検討してください。

numUpdates = 60;
i = 0;
while i < numUpdates
    % Receive laser scan and odometry message.
    scanMsg = receive(laserSub);
    odompose = odomSub.LatestMessage;
    
    % Create lidarScan object to pass to the AMCL object.
    scan = lidarScan(scanMsg);
    
    % For sensors that are mounted upside down, you need to reverse the
    % order of scan angle readings using 'flip' function.
    
    % Compute robot's pose [x,y,yaw] from odometry message.
    odomQuat = [odompose.Pose.Pose.Orientation.W, odompose.Pose.Pose.Orientation.X, ...
        odompose.Pose.Pose.Orientation.Y, odompose.Pose.Pose.Orientation.Z];
    odomRotation = quat2eul(odomQuat);
    pose = [odompose.Pose.Pose.Position.X, odompose.Pose.Pose.Position.Y odomRotation(1)];
    
    % Update estimated robot's pose and covariance using new odometry and
    % sensor readings.
    [isUpdated,estimatedPose, estimatedCovariance] = amcl(pose, scan);
    
    % Drive robot to next pose.
    wander(wanderHelper);
    
    % Plot the robot's estimated pose, particles and laser scans on the map.
    if isUpdated
        i = i + 1;
        plotStep(visualizationHelper, amcl, estimatedPose, scan, i)
    end
    
end

MATLAB での TurtleBot の停止と ROS のシャットダウン

stop(wanderHelper);
rosshutdown
Shutting down global node /matlab_global_node_68982 with NodeURI http://192.168.203.1:55528/

初期姿勢の推定による AMCL 位置推定のサンプル結果

"AMCL は確率的アルゴリズムであり、実際のコンピューター上でのシミュレーション結果は、ここに示すサンプル実行とは少し異なる場合があります。"

AMCL の最初の更新後、平均が amcl.InitialPose に等しく共分散が amcl.InitialCovariance に等しいガウス分布をサンプリングすることによって粒子が生成されます。

8 回の更新後、粒子は尤度の高い領域に収束し始めます。

60 回の更新後に、すべての粒子が正しいロボット姿勢に収束し、レーザー スキャンはマップのアウトラインに近付きます。

グローバル位置推定のための AMCL オブジェクトの設定

初期ロボット姿勢の推定が使用できない場合、AMCL はロボットの初期位置が不明な状態でロボットの位置を推定しようとします。このアルゴリズムは最初、オフィスのフリー スペースのあらゆる場所でロボットが存在する確率は等しく、そのようなスペース内では一様分布の粒子が生成されると仮定します。グローバル位置推定では、初期姿勢の推定による位置推定と比較して、必要な粒子が大幅に増えます。

AMCL グローバル位置推定機能を有効にするには、「初期姿勢の推定による位置推定のための AMCL オブジェクトの設定」のコード セクションを、このセクションのコードに置き換えます。

     amcl.GlobalLocalization = true;
     amcl.ParticleLimits = [500 50000];

AMCL グローバル位置推定のサンプル結果

"AMCL は確率的アルゴリズムであり、実際のコンピューター上でのシミュレーション結果は、ここに示すサンプル実行とは少し異なる場合があります。"

AMCL の最初の更新後、粒子はオフィスのフリー スペース内で一様分布されます。

8 回の更新後、粒子は尤度の高い領域に収束し始めます。

60 回の更新後に、すべての粒子が正しいロボット姿勢に収束し、レーザー スキャンはマップのアウトラインに近付きます。

参考

既知の姿勢を使用するマッピング

参考文献

[1] S. Thrun, W. Burgard and D. Fox, Probabilistic Robotics.Cambridge, MA: MIT Press, 2005.