A_Satr算法在多机器人路径规划中的应用与实现

1. 项目概述：多机器人导航与A_Satr算法的结合

在自动化仓储、智能工厂等场景中，多机器人协同作业的需求日益增长。传统单机器人导航算法在面对多机器人系统时，往往会出现路径冲突、效率低下等问题。A_Satr算法作为一种改进的路径规划方法，通过结合A*算法的启发式搜索和动态权重调整机制，能够有效解决多机器人在网格地图环境中的导航问题。

这个项目使用Matlab实现了基于A_Satr算法的多机器人导航模拟，主要解决以下几个核心问题：

1. 多机器人路径规划的冲突避免
2. 动态环境下的实时路径调整
3. 系统整体效率的优化

提示：A_Satr算法名称中的"Satr"实际上是"Star"的变体写法，表示这是一种基于A*算法的改进版本。

2. 核心算法解析：A_Satr的工作原理

2.1 基础A*算法的回顾

A*算法作为经典的路径规划算法，其核心在于评估函数f(n)=g(n)+h(n)的设计：

• g(n)：从起点到当前节点n的实际代价
• h(n)：从当前节点n到目标点的预估代价（启发式函数）

在标准网格地图中，常用的启发式函数有：

1. 曼哈顿距离：适用于只能四方向移动的场景
2. 欧几里得距离：适用于可八方向移动的场景
3. 对角线距离：结合前两者的折中方案

2.2 A_Satr算法的改进点

A_Satr算法在标准A*基础上引入了三个关键改进：

1. 动态权重机制：

   matlab复制% 动态权重计算公式示例
   w = w_base + k*(1 - d/d_max);
   % 其中：
   % w_base - 基础权重
   % k - 调节系数
   % d - 当前点到目标点的距离
   % d_max - 最大可能距离
   

2. 冲突预测与规避：

   • 通过时空地图预测机器人未来位置
   • 提前调整路径避免冲突

3. 多目标优化：

   • 不仅考虑路径长度
   • 同时优化能耗、时间均衡等因素

2.3 算法性能对比

我们通过实验对比了不同算法在相同场景下的表现：

3. 系统实现细节

3.1 网格地图建模

在Matlab中，我们使用二维矩阵表示网格地图：

matlab复制% 地图表示示例
map = zeros(100,100); % 100x100网格
map(20:30, 40:60) = 1; % 障碍物设置为1

关键参数包括：

• 网格分辨率：通常0.1m-0.5m/格
• 障碍物膨胀半径：考虑机器人物理尺寸
• 动态障碍物标记：实时更新地图

3.2 多机器人调度架构

系统采用集中式调度架构：

1. 中央控制器维护全局地图
2. 每个机器人作为独立客户端
3. 通信协议使用TCP/IP

matlab复制% 机器人数据结构示例
robot = struct(...
    'id', 1,...
    'position', [10,10],...
    'goal', [90,90],...
    'path', [],...
    'status', 'idle'...
);

3.3 路径规划实现流程

完整的路径规划流程包括：

1. 初始化环境地图
2. 设置起点和终点
3. 运行A_Satr算法
4. 路径平滑处理
5. 冲突检测与解决

核心算法函数实现：

matlab复制function [path, cost] = A_Satr(map, start, goal)
    % 初始化开放列表和关闭列表
    openList = PriorityQueue();
    closedList = false(size(map));
    
    % 设置初始节点
    startNode = struct('pos', start, 'g', 0, 'h', heuristic(start, goal), 'parent', []);
    openList.push(startNode, startNode.g + startNode.h);
    
    % 主循环
    while ~openList.isEmpty()
        currentNode = openList.pop();
        
        % 检查是否到达目标
        if isequal(currentNode.pos, goal)
            path = reconstructPath(currentNode);
            cost = currentNode.g;
            return;
        end
        
        % 生成相邻节点
        neighbors = getNeighbors(currentNode.pos, map);
        for i = 1:length(neighbors)
            neighborPos = neighbors(i,:);
            
            % 跳过障碍物和已关闭节点
            if map(neighborPos(1), neighborPos(2)) == 1 || closedList(neighborPos(1), neighborPos(2))
                continue;
            end
            
            % 计算新g值
            tentative_g = currentNode.g + distance(currentNode.pos, neighborPos);
            
            % 创建新节点
            neighborNode = struct(...
                'pos', neighborPos,...
                'g', tentative_g,...
                'h', dynamicHeuristic(neighborPos, goal),...
                'parent', currentNode...
            );
            
            % 添加到开放列表
            openList.push(neighborNode, neighborNode.g + neighborNode.h);
        end
        
        % 当前节点加入关闭列表
        closedList(currentNode.pos(1), currentNode.pos(2)) = true;
    end
    
    % 未找到路径
    path = [];
    cost = inf;
end

4. 关键技术与优化策略

4.1 动态启发式函数设计

A_Satr的核心创新在于动态调整启发式函数的权重：

matlab复制function h = dynamicHeuristic(pos, goal)
    base_h = norm(pos - goal); % 欧几里得距离
    d = base_h;
    d_max = norm(size(map) - [1,1]); % 地图对角线距离
    
    % 动态权重计算
    w = 1.5 + 0.5*(1 - d/d_max); % 权重在1.5-2.0之间变化
    
    h = w * base_h;
end

这种设计使得：

• 远离目标时：加大启发式权重，加快搜索速度
• 接近目标时：减小权重，提高路径质量

4.2 冲突解决策略

多机器人系统中常见的冲突类型及解决方案：

1. 节点冲突：多个机器人同时到达同一网格

   • 解决方案：优先级调度+等待策略

2. 边冲突：机器人在相邻网格相向移动

   • 解决方案：路径重规划或速度调整

3. 死锁：多个机器人互相阻塞

   • 解决方案：引入死锁检测和解除机制

冲突检测算法实现：

matlab复制function conflicts = detectConflicts(robots, timestep)
    conflicts = [];
    positions = zeros(length(robots), 2);
    
    % 预测未来位置
    for i = 1:length(robots)
        if timestep <= length(robots(i).path)
            positions(i,:) = robots(i).path(timestep,:);
        else
            positions(i,:) = robots(i).goal;
        end
    end
    
    % 检查节点冲突
    for i = 1:length(robots)
        for j = i+1:length(robots)
            if isequal(positions(i,:), positions(j,:))
                conflicts = [conflicts; struct('type','node','robots',[i,j],'time',timestep)];
            end
        end
    end
    
    % 检查边冲突(简化版)
    % ...
end

4.3 路径平滑处理

原始网格路径往往存在锯齿状问题，我们采用B样条曲线进行平滑：

matlab复制function smoothPath = smoothPath(originalPath)
    % 转换为参数化表示
    t = linspace(0, 1, length(originalPath));
    xx = originalPath(:,1);
    yy = originalPath(:,2);
    
    % 三次B样条拟合
    sp_x = spapi(3, t, xx);
    sp_y = spapi(3, t, yy);
    
    % 重采样
    new_t = linspace(0, 1, 3*length(originalPath));
    smoothPath = [fnval(sp_x, new_t)', fnval(sp_y, new_t)'];
end

5. 实验与性能分析

5.1 测试环境配置

我们搭建了三种典型测试场景：

1. 简单场景：10x10网格，2个机器人
2. 中等场景：50x50网格，5个机器人
3. 复杂场景：100x100网格，10个机器人

硬件配置：

• CPU: Intel i7-10750H
• 内存: 16GB DDR4
• MATLAB版本: R2021a

5.2 性能指标对比

在不同场景下的算法表现：

5.3 可视化分析

Matlab可视化界面展示了以下关键信息：

1. 网格地图与障碍物分布
2. 各机器人实时位置和规划路径
3. 冲突预警显示
4. 性能指标实时监控

可视化核心代码：

matlab复制function updateVisualization(map, robots)
    clf;
    
    % 绘制地图
    imagesc(map);
    colormap([1 1 1; 0 0 0]); % 白色可通行，黑色障碍物
    hold on;
    
    % 绘制机器人
    colors = lines(length(robots));
    for i = 1:length(robots)
        % 当前位置
        plot(robots(i).position(2), robots(i).position(1), 'o', ...
            'MarkerSize', 10, 'MarkerFaceColor', colors(i,:));
        
        % 路径
        if ~isempty(robots(i).path)
            plot(robots(i).path(:,2), robots(i).path(:,1), '-', ...
                'Color', colors(i,:), 'LineWidth', 2);
        end
        
        % 目标点
        plot(robots(i).goal(2), robots(i).goal(1), 'x', ...
            'Color', colors(i,:), 'MarkerSize', 15);
    end
    
    axis equal;
    grid on;
    title(sprintf('Multi-robot Navigation - Time: %.1fs', toc));
    drawnow;
end

6. 工程实践中的挑战与解决方案

6.1 实时性优化

在大规模场景中，算法实时性面临挑战。我们采用以下优化策略：

1. 局部重新规划：

   • 只对受影响区域重新计算
   • 保持大部分路径不变

2. 并行计算：

   matlab复制% 使用parfor并行计算各机器人路径
   parfor i = 1:numRobots
       robots(i).path = A_Satr(map, robots(i).position, robots(i).goal);
   end
   

3. 近似算法：

   • 设定最大计算时间限制
   • 时间到达时返回当前最优解

6.2 动态障碍物处理

实际环境中障碍物可能动态变化，系统需要：

1. 实时更新地图信息
2. 区分静态和动态障碍物
3. 预测动态障碍物运动轨迹

动态障碍物处理流程：

matlab复制function handleDynamicObstacles(robots, dynamicObstacles)
    % 预测障碍物位置
    predictedPositions = predictObstaclePositions(dynamicObstacles);
    
    % 更新地图
    for i = 1:length(robots)
        robotMap = robots(i).localMap;
        for j = 1:size(predictedPositions,1)
            robotMap(predictedPositions(j,1), predictedPositions(j,2)) = 1;
        end
        
        % 检查当前路径是否安全
        if ~isPathSafe(robots(i).path, predictedPositions)
            % 触发重新规划
            robots(i).path = A_Satr(robotMap, robots(i).position, robots(i).goal);
        end
    end
end

6.3 系统稳定性保障

为确保系统长期稳定运行，我们实现了：

1. 心跳检测机制
2. 异常恢复流程
3. 降级策略
   • 通信中断时切换为局部决策
   • 计算超时返回简化路径

7. 应用场景扩展

7.1 仓储物流应用

在智能仓储中，系统可以实现：

• 多AGV协同搬运
• 订单批次优化
• 充电调度管理

典型参数设置：

• 网格分辨率：0.2m
• 最大机器人速度：1.5m/s
• 安全距离：0.5m

7.2 智能巡检系统

用于设备巡检时需要考虑：

• 巡检点优先级
• 异常处理流程
• 电池续航约束

巡检路径规划特点：

matlab复制% 巡检点排序算法示例
function orderedPoints = sortInspectionPoints(points, startPoint)
    % 使用旅行商问题(TSP)近似算法
    remainingPoints = points;
    currentPoint = startPoint;
    orderedPoints = [];
    
    while ~isempty(remainingPoints)
        % 找到最近点
        distances = arrayfun(@(p)norm(p.position-currentPoint), remainingPoints);
        [~, idx] = min(distances);
        
        % 添加到有序列表
        orderedPoints = [orderedPoints; remainingPoints(idx)];
        currentPoint = remainingPoints(idx).position;
        
        % 移除已选点
        remainingPoints(idx) = [];
    end
end

7.3 其他潜在应用领域

1. 医疗服务机器人
2. 智能农业作业
3. 灾难救援机器人
4. 室内服务机器人

8. 项目完整代码结构

核心代码文件组织如下：

code复制/project_root
│── /algorithms
│   ├── A_Satr.m            # 核心算法实现
│   ├── dynamicHeuristic.m  # 动态启发式函数
│   └── pathSmoothing.m     # 路径平滑处理
│── /simulation
│   ├── mapGenerator.m      # 地图生成
│   ├── robotController.m   # 机器人控制
│   └── conflictSolver.m    # 冲突解决
│── /utils
│   ├── visualization.m     # 可视化工具
│   ├── priorityQueue.m     # 优先队列实现
│   └── metrics.m           # 性能评估
│── main.m                  # 主程序入口
│── config.m                # 参数配置

主程序流程示例：

matlab复制% main.m
clear; clc; close all;

% 初始化配置
config;

% 创建地图
map = mapGenerator('medium');

% 初始化机器人
robots = initializeRobots(5, map);

% 主循环
for step = 1:MAX_STEPS
    % 更新机器人状态
    updateRobotPositions;
    
    % 检测并解决冲突
    detectAndResolveConflicts;
    
    % 处理动态障碍物
    handleDynamicObstacles;
    
    % 可视化
    updateVisualization(map, robots);
    
    % 检查任务完成情况
    if all([robots.status] == "finished")
        break;
    end
    
    pause(0.1); % 控制仿真速度
end

% 输出性能报告
generatePerformanceReport;

9. 实际部署注意事项

9.1 参数调优建议

关键参数及其影响：

1. 启发式权重范围：影响搜索速度与路径质量平衡
   • 建议值：1.2-2.0
2. 冲突预测时域：决定提前多少步预测冲突
   • 建议值：3-5个时间步长
3. 路径重规划阈值：障碍物距离触发重新规划
   • 建议值：2-3个网格距离

9.2 硬件配置考量

1. 计算单元选择：
   • 小型系统：嵌入式PC
   • 大型系统：工业服务器
2. 通信方案：
   • WiFi：适用于普通环境
   • 工业无线：高可靠性要求场景
3. 传感器配置：
   • 激光雷达：精确定位
   • 视觉系统：辅助识别

9.3 安全机制设计

必须实现的安全功能：

1. 急停按钮硬件回路
2. 速度限制策略
3. 异常状态检测
4. 安全区域设置

10. 常见问题排查指南

10.1 算法相关问题

问题1：路径规划时间过长

• 可能原因：
  1. 地图分辨率过高
  2. 启发式函数效率低
  3. 障碍物过于复杂
• 解决方案：
  1. 降低地图分辨率
  2. 优化启发式函数
  3. 使用分层规划策略

问题2：频繁路径重规划

• 可能原因：
  1. 动态障碍物过多
  2. 冲突检测过于敏感
• 解决方案：
  1. 调整重规划阈值
  2. 优化障碍物预测算法

10.2 实现相关问题

问题3：Matlab运行内存不足

• 解决方案：

matlab复制% 1. 清除不必要变量
clear unnecessaryVars;

% 2. 使用稀疏矩阵存储地图
map = sparse(map);

% 3. 增加Java堆内存
java.lang.Runtime.getRuntime.maxMemory
java.lang.Runtime.getRuntime.totalMemory

问题4：可视化卡顿

• 优化建议：
  1. 降低刷新频率
  2. 简化绘图元素
  3. 使用硬件加速

10.3 多机器人协同问题

问题5：死锁情况处理

• 典型场景：十字路口四机器人互相阻挡
• 解决方案：
  1. 优先级重新分配
  2. 引入临时等待区
  3. 部分机器人后退策略

问题6：通信延迟影响

• 缓解措施：
  1. 预测-校正机制
  2. 本地备份决策
  3. 心跳超时检测

11. 项目进阶方向

11.1 算法层面改进

1. 机器学习增强：

   • 使用强化学习优化启发式函数
   • 基于历史数据预测冲突热点

2. 混合算法：

   • 结合RRT的快速探索特性
   • 集成Dijkstra的精确性

11.2 系统架构扩展

1. 分布式架构：

   • 部分决策下放至机器人端
   • 共识算法解决局部冲突

2. 云边协同：

   • 云端全局规划
   • 边缘端实时控制

11.3 实际部署优化

1. 数字孪生系统：

   • 虚拟与现实同步
   • 提前仿真验证

2. 能耗优化：

   • 路径与速度联合优化
   • 充电调度策略

12. 学习资源推荐

12.1 理论基础

1. 必读论文：

   • 《A*算法的现代变体研究》
   • 《多机器人系统路径规划综述》

2. 经典教材：

   • 《Principles of Robot Motion》
   • 《Introduction to Autonomous Mobile Robots》

12.2 Matlab相关

1. 官方文档：

   • Robotics System Toolbox
   • Parallel Computing Toolbox

2. 实用技巧：

   matlab复制% 性能分析工具使用示例
   profile on
   % 运行待分析代码
   A_Satr(map, start, goal);
   profile viewer
   

12.3 开源项目参考

1. ROS导航栈：gmapping、amcl等包
2. MATLAB中央文件交换：搜索路径规划相关提交

13. 项目完整实现要点

13.1 核心算法完整实现

A_Satr算法的完整Matlab实现需要考虑以下关键点：

1. 优先队列实现：

matlab复制classdef PriorityQueue < handle
    properties
        elements = [];
        priorities = [];
    end
    
    methods
        function push(obj, element, priority)
            % 插入新元素
            obj.elements = [obj.elements; element];
            obj.priorities = [obj.priorities; priority];
            
            % 保持优先级排序
            [obj.priorities, idx] = sort(obj.priorities);
            obj.elements = obj.elements(idx);
        end
        
        function element = pop(obj)
            if ~isempty(obj.elements)
                element = obj.elements(1);
                obj.elements(1) = [];
                obj.priorities(1) = [];
            else
                element = [];
            end
        end
        
        function result = isEmpty(obj)
            result = isempty(obj.elements);
        end
    end
end

2. 完整的A_Satr实现：

matlab复制function [path, cost] = A_Satr(map, start, goal, varargin)
    % 参数解析
    p = inputParser;
    addParameter(p, 'heuristic', 'euclidean');
    addParameter(p, 'weight_range', [1.2 2.0]);
    parse(p, varargin{:});
    
    % 初始化数据结构
    openSet = PriorityQueue();
    closedSet = false(size(map));
    gScore = inf(size(map));
    fScore = inf(size(map));
    cameFrom = cell(size(map));
    
    % 初始节点设置
    gScore(start(1), start(2)) = 0;
    fScore(start(1), start(2)) = dynamicHeuristic(start, goal, p.Results.weight_range);
    openSet.push(start, fScore(start(1), start(2)));
    
    % 主循环
    while ~openSet.isEmpty()
        current = openSet.pop();
        
        % 到达目标
        if isequal(current, goal)
            path = reconstructPath(cameFrom, current);
            cost = gScore(current(1), current(2));
            return;
        end
        
        % 生成邻居
        neighbors = getNeighbors(current, map);
        for i = 1:size(neighbors,1)
            neighbor = neighbors(i,:);
            
            % 跳过障碍物和关闭列表
            if map(neighbor(1), neighbor(2)) == 1 || closedSet(neighbor(1), neighbor(2))
                continue;
            end
            
            % 计算临时g值
            tentative_gScore = gScore(current(1), current(2)) + ...
                distance(current, neighbor);
            
            % 发现更好路径
            if tentative_gScore < gScore(neighbor(1), neighbor(2))
                cameFrom{neighbor(1), neighbor(2)} = current;
                gScore(neighbor(1), neighbor(2)) = tentative_gScore;
                fScore(neighbor(1), neighbor(2)) = gScore(neighbor(1), neighbor(2)) + ...
                    dynamicHeuristic(neighbor, goal, p.Results.weight_range);
                
                % 添加到开放集
                openSet.push(neighbor, fScore(neighbor(1), neighbor(2)));
            end
        end
        
        % 当前节点加入关闭集
        closedSet(current(1), current(2)) = true;
    end
    
    % 未找到路径
    path = [];
    cost = inf;
end

13.2 多机器人调度核心逻辑

集中式调度器的关键实现：

matlab复制classdef CentralScheduler < handle
    properties
        robots = [];
        map = [];
        dynamicObstacles = [];
        timeStep = 0;
    end
    
    methods
        function obj = CentralScheduler(map, numRobots)
            obj.map = map;
            obj.initializeRobots(numRobots);
        end
        
        function initializeRobots(obj, numRobots)
            % 随机初始化机器人位置和目标
            for i = 1:numRobots
                freeSpaces = find(obj.map == 0);
                startIdx = freeSpaces(randi(length(freeSpaces)));
                goalIdx = freeSpaces(randi(length(freeSpaces)));
                
                [startX, startY] = ind2sub(size(obj.map), startIdx);
                [goalX, goalY] = ind2sub(size(obj.map), goalIdx);
                
                obj.robots = [obj.robots; struct(...
                    'id', i,...
                    'position', [startX, startY],...
                    'goal', [goalX, goalY],...
                    'path', [],...
                    'status', 'idle',...
                    'priority', randi(3) % 1-3优先级
                )];
            end
        end
        
        function update(obj)
            obj.timeStep = obj.timeStep + 1;
            
            % 阶段1：更新机器人位置
            obj.updatePositions();
            
            % 阶段2：检测并解决冲突
            obj.resolveConflicts();
            
            % 阶段3：规划新路径
            obj.planPaths();
        end
        
        function updatePositions(obj)
            for i = 1:length(obj.robots)
                if ~isempty(obj.robots(i).path) && obj.robots(i).status == "moving"
                    % 移动到下一路径点
                    nextPos = obj.robots(i).path(1,:);
                    obj.robots(i).position = nextPos;
                    obj.robots(i).path(1,:) = [];
                    
                    % 检查是否到达目标
                    if isequal(nextPos, obj.robots(i).goal)
                        obj.robots(i).status = "finished";
                    end
                end
            end
        end
        
        function resolveConflicts(obj)
            % 简化的冲突检测与解决
            positions = vertcat(obj.robots.position);
            [uniquePositions, ~, ic] = unique(positions, 'rows');
            
            % 查找重复位置
            counts = accumarray(ic,1);
            conflictPositions = uniquePositions(counts > 1,:);
            
            % 解决冲突
            for posIdx = 1:size(conflictPositions,1)
                conflictPos = conflictPositions(posIdx,:);
                robotIndices = find(ismember(positions, conflictPos, 'rows'));
                
                % 按优先级排序
                [~, order] = sort([obj.robots(robotIndices).priority], 'descend');
                
                % 高优先级机器人继续移动，其他等待
                for i = 2:length(robotIndices)
                    obj.robots(robotIndices(order(i))).status = "waiting";
                end
            end
        end
        
        function planPaths(obj)
            % 并行计算各机器人路径
            parfor i = 1:length(obj.robots)
                if obj.robots(i).status ~= "finished"
                    % 获取当前地图状态(包括其他机器人作为动态障碍物)
                    otherRobots = obj.robots([1:i-1 i+1:end]);
                    dynamicObstacles = vertcat(otherRobots.position);
                    
                    % 创建临时地图
                    tempMap = obj.map;
                    for j = 1:size(dynamicObstacles,1)
                        tempMap(dynamicObstacles(j,1), dynamicObstacles(j,2)) = 1;
                    end
                    
                    % 路径规划
                    [path, ~] = A_Satr(tempMap, obj.robots(i).position, obj.robots(i).goal);
                    
                    % 更新路径
                    if ~isempty(path)
                        obj.robots(i).path = path;
                        obj.robots(i).status = "moving";
                    end
                end
            end
        end
    end
end

13.3 完整仿真流程示例

从初始化到运行的主流程：

matlab复制% 主仿真脚本
clear; clc; close all;

% 1. 创建地图
mapSize = [50, 50];
map = zeros(mapSize);

% 添加障碍物(随机生成)
numObstacles = 100;
for i = 1:numObstacles
    x = randi(mapSize(1));
    y = randi(mapSize(2));
    map(x,y) = 1;
end

% 2. 创建调度器
numRobots = 5;
scheduler = CentralScheduler(map, numRobots);

% 3. 设置可视化
figure;
h = imagesc(map);
colormap([1 1 1; 0 0 0]);
hold on;

% 绘制初始状态
robotHandles = gobjects(numRobots,1);
pathHandles = gobjects(numRobots,1);
colors = lines(numRobots);
for i = 1:numRobots
    robotHandles(i) = plot(scheduler.robots(i).position(2), scheduler.robots(i).position(1), ...
        'o', 'MarkerSize', 10, 'MarkerFaceColor', colors(i,:));
    pathHandles(i) = plot(NaN, NaN, '-', 'Color', colors(i,:));
end

% 4. 主循环
maxSteps = 200;
for step = 1:maxSteps
    % 更新调度器
    scheduler.update();
    
    % 更新可视化
    for i = 1:numRobots
        % 更新机器人位置
        set(robotHandles(i), 'XData', scheduler.robots(i).position(2), ...
            'YData', scheduler.robots(i).position(1));
        
        % 更新路径显示
        if ~isempty(scheduler.robots(i).path)
            set(pathHandles(i), 'XData', scheduler.robots(i).path(:,2), ...
                'YData', scheduler.robots(i).path(:,1));
        end
    end
    
    % 检查是否所有机器人完成任务
    if all([scheduler.robots.status] == "finished")
        disp('所有机器人完成任务！');
        break;
    end
    
    % 控制仿真速度
    pause(0.1);
    drawnow;
end

% 5. 性能分析
fprintf('仿真完成，总步数: %d\n', step);
fprintf('平均路径长度: %.2f\n', mean(arrayfun(@(r)size(r.path,1), scheduler.robots)));