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

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

この例では、センサー読み取り時のロボットの位置が既知である場合に、距離センサーの読み取り値を使用して環境のマップを作成する方法を説明します。この例ではまた、Robotics System Toolbox™ の変換関数 (quat2eul など) を使用する方法も説明します。

この例では、距離センサーの読み取り値とロボットの既知の姿勢からマップを作成します。この例の目的に沿って MATLAB® ベースのシミュレーターを使用してロボットを駆動し、距離センサーの読み取り値とロボットの姿勢を観察します。シミュレーターを実際のロボットや Gazebo シミュレーターでシミュレートされるロボットに置き換えることができます。この場合、センサーの読み取り時にロボットの真の位置を取得する何らかの手段が必要です。

必要条件: 差動駆動型ロボットのパス追従ROS の tf 変換ツリーへのアクセスROS のパブリッシャーおよびサブスクライバーとのデータ交換

ロボット シミュレーターの初期化

MATLAB で ROS マスターを起動します。

rosinit
Initializing ROS master on http://bat800808glnxa64:36987/.
Initializing global node /matlab_global_node_26307 with NodeURI http://bat800808glnxa64:43833/

ロボット シミュレーターを初期化して初期姿勢を割り当てます。シミュレートされるロボットは 2 輪差動駆動型ロボットで、レーザー距離センサーを備えています。

sim = ExampleHelperRobotSimulator('simpleMap');

setRobotPose(sim,[2 3 -pi/2]);

シミュレーター用に ROS インターフェイスを有効にします。シミュレーターは、ROS 経由でデータを送受信するパブリッシャーとサブスクライバーを作成します。

enableROSInterface(sim,true);

マップを構築しやすくするために、シミュレーターのレーザー センサーの解像度を上げます。

sim.LaserSensor.NumReadings = 50;

ROS インターフェイスの設定

シミュレーターと通信する ROS のパブリッシャーとサブスクライバーを作成します。シミュレーターからレーザー センサー データを受信する rossubscriber を作成します。

scanSub = rossubscriber('scan');

マップを作成するために、ロボットがマップ上を動き回ってセンサー データを収集します。速度コマンドをロボットに送信する rospublisher を作成します。

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

MATLAB シミュレーターは、マップ原点に対するロボットの位置を、変換としてトピック /tf でパブリッシュします。これは、この例ではグラウンド トゥルースのロボット姿勢のソースとして使用されます。シミュレーターはこの変換での座標系の名前として maprobot_base を使用します。

関数 rostf を使用して、ROS 変換ツリー オブジェクトを作成します。マップを作成するには、ロボットの姿勢とレーザー センサーの読み取りが同じ時点に対応していることが不可欠です。変換ツリーにより、レーザー センサーの読み取りが観測された時点でロボットの姿勢を検出できるようになります。

tftree = rostf;

変換ツリー オブジェクトが初期化を終了するまで、少しの間一時停止します。

pause(1)

ROS インターフェイスがロボット シミュレーターと通信し、シミュレーション対象のロボットのステータスが Figure に表示されます。

パス コントローラーの作成

レーザー センサー データを収集してマップ全体を作成するために、ロボットはマップ全体を動き回る必要があります。マップ全体を網羅する中間点をもつパスを割り当てます。

path = [2 3; 3.25 6.25; 2 11; 6 7; 11 11; 8 6; 10 5; 7 3; 11 1.5];

ロボット シミュレーターでパスを可視化します。

plot(path(:,1),path(:,2),'k--d');

上記で定義したパスとロボット運動モデルに基づいて、ロボットをパスに沿って駆動するパス追従コントローラーが必要です。robotics.PurePursuit オブジェクトを使用してパス追従コントローラーを作成します。

controller = robotics.PurePursuit('Waypoints',path);
controller.DesiredLinearVelocity = 0.4;

10 Hz で実行するように、コントローラーのレートを設定します。

controlRate = robotics.Rate(10);

パスの最後でロボットを停止させる目標点と目標半径を定義します。

goalRadius = 0.1;
robotCurrentLocation = path(1,:);
robotGoal = path(end,:);
distanceToGoal = norm(robotCurrentLocation - robotGoal);

空のマップの定義

robotics.OccupancyGrid を使用して、センサーの読み取り値をキャプチャするための高解像度のマップを定義します。これにより、サイズが 14 m x 13 m、解像度が 20 セル/m のマップが作成されます。

map = robotics.OccupancyGrid(14,13,20);

Figure ウィンドウでマップを可視化します。

figureHandle = figure('Name', 'Map');
axesHandle = axes('Parent', figureHandle);
mapHandle = show(map, 'Parent', axesHandle);
title(axesHandle, 'OccupancyGrid: Update 0');

その後の while ループは、ロボットを駆動しながら環境マップを作成します。以下のステップが実行されます。

  • まず、scanSub サブスクライバーを使用してレーザー スキャン データを受信します。関数 getTransformscan メッセージのタイム スタンプと共に使用して、センサー読み取り時の maprobot_base の座標系間の変換を取得します。

  • 変換からロボットの位置と向きを取得します。ロボットの向きは、ロボットの Z 軸を中心とするヨー回転です。quat2eul を使用して四元数をオイラー角に変換することにより、ヨー回転を取得できます。

  • レーザー スキャン データを前処理します。レーザー光が最大探知距離内でどんな障害物にもヒットしない場合、シミュレーターは NaN 距離を返します。NaN 距離を最大探知距離の値に置き換えます。

  • insertRay メソッドを使用して、レーザー スキャン観測値を占有グリッド map に挿入します。

  • controller オブジェクトを使用して線形速度と角速度のコマンドを計算し、ロボットを駆動します。

  • 更新 50 回ごとにマップを可視化します。

updateCounter = 1;
while( distanceToGoal > goalRadius )
    % Receive a new laser sensor reading.
    scanMsg = receive(scanSub);
    
    % Get robot pose at the time of sensor reading.
    pose = getTransform(tftree, 'map', 'robot_base', scanMsg.Header.Stamp, 'Timeout', 2);
    
    % Convert robot pose to 1x3 vector [x y yaw].
    position = [pose.Transform.Translation.X, pose.Transform.Translation.Y];
    orientation =  quat2eul([pose.Transform.Rotation.W, pose.Transform.Rotation.X, ...
        pose.Transform.Rotation.Y, pose.Transform.Rotation.Z], 'ZYX');
    robotPose = [position, orientation(1)];
    
    % Extract the laser scan.
    scan = lidarScan(scanMsg);
    ranges = scan.Ranges;
    ranges(isnan(ranges)) = sim.LaserSensor.MaxRange;
    modScan = lidarScan(ranges, scan.Angles);
    
    % Insert the laser range observation in the map.
    insertRay(map, robotPose, modScan, sim.LaserSensor.MaxRange);
    
    % Compute the linear and angular velocity of the robot and publish it
    % to drive the robot.
    [v, w] = controller(robotPose);
    velMsg.Linear.X = v;
    velMsg.Angular.Z = w;
    send(velPub, velMsg);
    
    % Visualize the map after every 50th update.
    if ~mod(updateCounter,50)
        mapHandle.CData = occupancyMatrix(map);
        title(axesHandle, ['OccupancyGrid: Update ' num2str(updateCounter)]);
    end
    
    % Update the counter and distance to goal.
    updateCounter = updateCounter+1;
    distanceToGoal = norm(robotPose(1:2) - robotGoal);
    
    % Wait for control rate to ensure 10 Hz rate.
    waitfor(controlRate);
end

すべてのセンサー読み取り値が組み込まれた最終的なマップを表示します。

show(map, 'Parent', axesHandle);
title(axesHandle, 'OccupancyGrid: Final Map');

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

ROS マスターをシャットダウンしてグローバル ノードを削除します。

rosshutdown
Shutting down global node /matlab_global_node_26307 with NodeURI http://bat800808glnxa64:43833/
Shutting down ROS master on http://bat800808glnxa64:36987/.

参考