既知の姿勢を使用する地図作成

ライブスクリプトを開く

この例では、差動駆動型ロボットの距離センサーの読み取り値とロボットの姿勢を使用して環境マップを作成する方法を説明します。rangeSensor オブジェクトを使用してシミュレートされた距離センサー読み取り値から、マップを作成します。differentialDriveKinematics 運動モデルは、速度コマンドに基づいて室内を移動するロボットの駆動をシミュレートします。rangeSensor は、パスを追従しているロボットの姿勢に基づいて、距離の読み取り値を提供します。

参照マップと Figure

exampleMaps から、この例で使用する simpleMap を含む、一連のバイナリ占有グリッド例を読み込みます。

load exampleMaps.mat

simpleMap を分解能 1 で使用して、参照用バイナリ占有マップを作成します。Figure を表示して、その Figure のハンドルを保存します。

refMap = binaryOccupancyMap(simpleMap,1);
refFigure = figure('Name','SimpleMap');
show(refMap);

選択したマップと同じ次元の空のマップを分解能 10 で作成します。Figure を表示して、その Figure のハンドルを保存します。座標軸をマップのサイズにロックします。

[mapdimx,mapdimy] = size(simpleMap);
map = binaryOccupancyMap(mapdimy,mapdimx,10);
mapFigure = figure('Name','Unknown Map');
show(map);

運動モデルとコントローラーの初期化

差動駆動型の運動学的運動モデルを作成します。運動モデルはシミュレートされる差動駆動型ロボットの運動を表します。このモデルは、左右の車輪の速度または線形速度と角速度をロボットの進行方向として取ります。この例では VehicleInputs にビークル速度と向首角速度を使用します。

diffDrive = differentialDriveKinematics("VehicleInputs","VehicleSpeedHeadingRate");

単純追跡コントローラーを作成します。このコントローラーは、シミュレートするロボットが目的のパスを追従するための速度入力を生成します。目的の線形速度を m/s 単位、最大角速度を rad/s 単位でそれぞれ設定します。

controller = controllerPurePursuit('DesiredLinearVelocity',2,'MaxAngularVelocity',3);

距離センサーの設定

最大距離が 10 m のセンサーを作成します。このセンサーは与えられた姿勢とマップに基づいて距離の読み取り値をシミュレートします。この距離センサーとともに参照マップを使用して、未知の環境におけるセンサー読み取り値の収集をシミュレートします。

sensor = rangeSensor;
sensor.Range = [0,10];

計画されたパスの作成

距離センサー読み取り値を収集するために、マップ内の走行パスを作成します。

path = [4 6; 6.5 12.5; 4 22; 12 14; 22 22; 16 12; 20 10; 14 6; 22 3];

参照マップの Figure 上にパスをプロットします。

figure(refFigure);
hold on
plot(path(:,1),path(:,2), 'o-');
hold off

パスを単純追跡コントローラーのウェイポイントとして設定します。

controller.Waypoints = path;

パスの追跡とマップ環境

初期姿勢とパスに基づく最終ゴール位置を設定します。反復を追跡するために、現在の姿勢とインデックスを格納するグローバル変数を作成します。

initPose = [path(1,1) path(1,2), pi/2];
goal = [path(end,1) path(end,2)]';
poses(:,1) = initPose';

提供されている補助関数 exampleHelperDiffDriveCtrl を使用します。補助関数には、パスを通り、距離の読み取り値を取得し、環境を地図作成するためのメインループが含まれています。

関数 exampleHelperDiffDriveControl のワークフローは次のとおりです。

距離センサーと現在の姿勢を使用して参照マップをスキャンします。これにより未知の環境で走行するための正常な距離読み取り値をシミュレートします。
距離の読み取り値を使用してマップを更新します。
単純追跡コントローラーから、次のウェイポイントまで走行するための制御コマンドを取得します。
制御コマンドに基づいてロボットの運動の微分を計算します。
微分に基づいてロボットの姿勢をインクリメントします。

空のマップ内を走行するロボットと、距離センサーによって検出された壁が表示されるはずです。

exampleHelperDiffDriveCtrl(diffDrive,controller,initPose,goal,refMap,map,refFigure,mapFigure,sensor)

Goal position reached

差動駆動型の制御関数

関数 exampleHelperDiffDriveControl のワークフローは次のとおりです。

距離センサーと現在の姿勢を使用して参照マップをスキャンします。これにより未知の環境で走行するための正常な距離読み取り値をシミュレートします。
距離の読み取り値を使用してマップを更新します。
単純追跡コントローラーから、次のウェイポイントまで走行するための制御コマンドを取得します。
制御コマンドに基づいてロボットの運動の微分を計算します。
微分に基づいてロボットの姿勢をインクリメントします。

function exampleHelperDiffDriveControl(diffDrive,ppControl,initPose,goal,map1,map2,fig1,fig2,lidar)
sampleTime = 0.05;             % Sample time [s]
t = 0:sampleTime:100;         % Time array
poses = zeros(3,numel(t));    % Pose matrix
poses(:,1) = initPose';

% Set iteration rate
r = rateControl(1/sampleTime);

% Get the axes from the figures
ax1 = fig1.CurrentAxes;
ax2 = fig2.CurrentAxes;

    for idx = 1:numel(t)
        position = poses(:,idx)';
        currPose = position(1:2);
        
        % End if pathfollowing is vehicle has reached goal position within tolerance of 0.2m
        dist = norm(goal'-currPose);
        if (dist < .2)
            disp("Goal position reached")
            break;
        end
        
        % Update map by taking sensor measurements
        figure(2)
        [ranges, angles] = lidar(position, map1);
        scan = lidarScan(ranges,angles);
        validScan = removeInvalidData(scan,'RangeLimits',[0,lidar.Range(2)]);
        insertRay(map2,position,validScan,lidar.Range(2));
        show(map2);
        
        % Run the Pure Pursuit controller and convert output to wheel speeds
        [vRef,wRef] = ppControl(poses(:,idx));
    
        % Perform forward discrete integration step
        vel = derivative(diffDrive, poses(:,idx), [vRef wRef]);
        poses(:,idx+1) = poses(:,idx) + vel*sampleTime; 
    
    
        % Update visualization
        plotTrvec = [poses(1:2, idx+1); 0];
        plotRot = axang2quat([0 0 1 poses(3, idx+1)]);
        
        % Delete image of the last robot to prevent displaying multiple robots
        if idx > 1
           items = get(ax1, 'Children');
           delete(items(1)); 
        end
    
        %plot robot onto known map
        plotTransforms(plotTrvec', plotRot, 'MeshFilePath', 'groundvehicle.stl', 'View', '2D', 'FrameSize', 1, 'Parent', ax1);
        %plot robot on new map
        plotTransforms(plotTrvec', plotRot, 'MeshFilePath', 'groundvehicle.stl', 'View', '2D', 'FrameSize', 1, 'Parent', ax2);
    
        % waiting to iterate at the proper rate
        waitfor(r);
    end
end