Multi Robot Lidar Mapping

Hi all,

I want to create a map of an area using multiple robots which have lidar sensors. I have searched and learn how it works lots of matlab example and tried to understand them with changing these examples.

I just want you to give me an/any advice as I am new to SLAM algorithms.

[KSSV]

What data you have?

[robotics]

I have changed "MappingWithKnownPosesDiffDriveExample" code according to my situation as below. I can't find lidar example but range sensor is also okay for me.

There are three different paths my robot can follow on this map. Now I can run this code with different paths successfully. (can be seen attached screenShots)

What I want to do is: robots will set off from the starting point one after the other, and each robot will take different possible paths and do the mapping. Later, the maps of these different roads will be merged. (ps: I have unlimited robots)

I will be grateful for any help you do to reach my goal. Thanks in advance:)

image = imread('threeWay27by26.png'); %choose .png file to use as a map
grayimage = rgb2gray(image);
bwimage = grayimage < 0.5;
refMap = binaryOccupancyMap(bwimage,1);
refFigure = figure('Name','SimpleMap');
show(refMap);

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

diffDrive = differentialDriveKinematics("VehicleInputs","VehicleSpeedHeadingRate");
controller = controllerPurePursuit('DesiredLinearVelocity',2,'MaxAngularVelocity',3);

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

%Below paths needs to be updated according to map which is used
path1 = [1 13; 3 12; 5 14.5; 7 11.5; 11 13; 10.5 20; 14 22; 15 19; 18 20];
path2 = [1 13; 3 12; 5 14.5; 7 11.5; 11 13; 12 12; 13 14; 16 13; 19 14];
path3 = [1 13; 3 12; 5 14.5; 7 11.5; 11 13; 10.5 6; 14 4.5; 16 8; 18 6];

path=path1; %choose the path will be used for mapping (path1 or path2 or path3)

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';

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

function exampleHelperDiffDriveCnt(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