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

我会带着你远行 2022-01-26 00:25 521阅读 0赞

### 文章目录 ###

*  Matlab 仿真效果图
 *  仿真说明
 *  主程序详解
 *  源码

# Matlab 仿真效果图 #

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

# 仿真说明 #

文件修改自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循迹结束后删除对象

# 源码 #

代码请点击[下载链接。][Link 1]

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70]: https://img-blog.csdnimg.cn/20190528165314179.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 1]: /images/20220126/b5838193ab824d5ebb8488a1498fa560.png
[Link 1]: https://download.csdn.net/download/qq_16775293/11212162