MATLAB+ROS Robot仿真swarm多机器人循迹

Matlab 仿真效果图

  图中深绿色和浅绿色为两个机器人，机器人均沿着虚线运行，轨迹分别用与机器人颜色相同的曲线标识。

仿真说明

文件修改自Matlab 2018a版本 Robotics System Toolbox中的例程：Path Following for a Differential Drive Robot
修改后的仿真代码主要包括三个部分：

• RobotModel.m
  单个机器人模型。
• MutilRobotsModel.m
  多机器人模型，调用RobotModel.m文件生成多个机器人。
• pathFollow.m
  主程序，生成多个机器人实例，并控制多个机器人运动。

**注释：**亲测在Matlab 2018a下可以运行，在Matlab 2014a下运行会报错，错误提示如下：

未定义变量 "robotics" 或类 "robotics.BinaryOccupancyGrid"。

出错 MultiMobileRobots (line 144)
                    obj.InternalMap = robotics.BinaryOccupancyGrid(exampleMaps.(mapName));

出错 PathFollowing (line 34)
robots = MultiMobileRobots(robotsNumber);

主程序详解

clc
clear all
path = [2.00    1.00;  % 需要循迹的路径
    1.25    1.75;
    5.25    8.25;
    7.25    8.75;
    11.75   10.75;
    12.00   10.00];

robotGoal = path(end,:);

initialPose1 = [path(1, :) 0]; % 初始化位置
initialPose2 = [path(2, :) 0];
initialPose3 = [path(3, :) 0];

robotRadius = 0.4; % 设置机器人半径
robotsNumber = 2;  % 设置swarm机器人数量
robots = MultiRobotsModel(robotsNumber,'emptyMap'); % 调用多机器人模型创建实例
robots.setPose(1,initialPose1); % 初始化机器人1的位置
robots.setPose(2,initialPose2); % 初始化机器人2的位置
%robots.setPose(3,initialPose3); % 初始化机器人3的位置

plot(path(:,1), path(:,2),'k--d') % 显示需要循迹的路径点
xlim(robots.getMapHandle().XWorldLimits) % 设置坐标系显示的x轴长度
ylim(robots.getMapHandle().YWorldLimits) % 设置坐标系显示的y轴长度

jobStatus = ones(1, robotsNumber); % 机器人当前的状态，1为工作，0为到达目标点
controllers = cell(1, robotsNumber); % 机器人控制器集合
for i=1:robotsNumber
    controller = robotics.PurePursuit; % 控制对象
    controller.Waypoints = path; % 路径点
    controller.DesiredLinearVelocity = 0.3; %最大线速度
    controller.MaxAngularVelocity = 2; % 最大角速度
    controller.LookaheadDistance = 0.5; % 可以检测到的距离
    controllers{i} = controller; 
end

goalRadius = 0.5; % 目标点的半径，在该半径内均为到达目标点
controlRate = robotics.Rate(10); % 机器人控制周期

while(norm(jobStatus)>0)
    for i = 1:robotsNumber
        if jobStatus(i) ~= 0
            currentPose = robots.getRobotPose(i);
            controller = controllers{i};
            % Compute the controller outputs, i.e., the inputs to the robot
   
            [v, omega] = controller(currentPose); % 计算当前位置机器人的运动速度和角度

            % Simulate the robot using the controller outputs.
            drive(robots.Robots(i), v, omega); % 驱动机器人
            
            % Re-compute the distance to the goal
            distanceToGoal = norm(currentPose(1:2) - robotGoal); %计算到达目标点的距离
            
            if distanceToGoal < goalRadius
                jobStatus(i) = 0;
            end
        end
    end
    
    waitfor(controlRate); % 控制机器人的控制周期
    
end

delete(robots) % swarm循迹结束后删除对象

源码

代码请点击下载链接。