技术博客
首页 所有文章 标签 分类 关于
技术博客

目录

RAWSim-O-main项目Trae解析

 收录于 未分类
   约 11983 字   预计阅读 24 分钟 
https://bing.ee123.net/img/rand?artid=151859188
目录

RAWSim-O-main项目Trae解析

1.这个项目里面是怎么使用C#来描述MutiAgentPathFinding问题的，用了什么方法，然后是怎么将这些方法和地图结合的，这些地图是节点和边还是栅格地图

我需要了解这个项目中的多智能体路径规划(MAPF)实现方式。首先，我将搜索与MAPF相关的代码，查看项目如何表示地图、节点和边，以及如何实现路径规划算法。

toolName: search_codebase

status: success

query: MultiAgentPathFinding implementation

我需要进一步了解项目中的图结构和路径规划算法的具体实现。我将查看Graph类和相关的数据结构，以及WaypointGraph类，以了解地图表示方式。

toolName: search_codebase

status: success

query: Graph class waypoint structure nodes edges

我需要查看更多关于Agent类和路径规划算法的实现细节，以了解多智能体路径规划的核心算法和数据结构。

toolName: search_codebase

status: success

query: Agent class path planning algorithm

RAWSim-O项目中的多智能体路径规划(MAPF)实现分析

根据对代码的分析，RAWSim-O项目使用了一套完整的多智能体路径规划(Multi-Agent Path Finding, MAPF)系统来协调机器人在仓库环境中的移动。以下是该系统的主要特点和实现方式：

地图表示方式

该项目使用的是基于节点和边的图结构（而非栅格地图）来表示环境：

  1. WaypointGraph类：核心地图表示，管理所有的路径点(Waypoint)，这些路径点按照层级(Tier)组织

    • 每个Waypoint是一个具有x,y坐标的点，代表机器人可以到达的位置
    • Waypoint之间通过路径(Paths)连接，形成图结构
    • 使用QuadTree数据结构进行空间索引，提高查询效率

  2. Graph类：为多智能体路径规划专门设计的轻量级图结构

    • 包含节点(Nodes)和边(Edges)的数组
    • 每个节点有唯一ID和坐标信息(PositionX, PositionY)
    • 边(Edge)包含起点(From)、终点(To)、距离(Distance)和角度(Angle)信息
    • 支持特殊的电梯边(ElevatorEdge)，用于连接不同层级

  3. PathManager.GenerateGraph方法：将WaypointGraph转换为MAPF算法使用的Graph

    • 为每个Waypoint分配唯一ID
    • 创建节点间的边，包括普通边和电梯边
    • 保存节点元数据(NodeInfo)，如队列信息

智能体表示

Agent类用于表示需要规划路径的机器人：

  • 包含当前节点(NextNode)、目标节点(DestinationNode)信息
  • 记录到达下一节点的时间(ArrivalTimeAtNextNode)和朝向(OrientationAtNextNode)
  • 存储路径规划结果(Path)和节点预约信息(Reservations)
  • 支持特殊属性如固定位置(FixedPosition)、休息状态(Resting)等

多智能体路径规划算法

项目实现了多种MAPF算法，所有算法都继承自抽象类PathFinder：

  1. WHCAv/WHCAn方法：基于Windowed Hierarchical Cooperative A*算法

    • 使用时间窗口和预约表(ReservationTable)避免冲突
    • 按优先级顺序规划路径
    • 包含死锁处理机制(DeadlockHandler)

  2. FAR方法：基于Flow Annotation Re-planning算法

    • 跟踪智能体之间的等待关系(_waitFor)
    • 使用最大等待时间(MaximumWaitTime)检测死锁
    • 实现绕行策略处理冲突

  3. PAS方法：Priority-based A*算法

    • 使用优先级机制解决冲突
    • 结合Fibonacci堆优化路径搜索

  4. ODID方法：结合Operator Decomposition和Independence Detection的算法

    • 将智能体分组，减少冲突处理复杂度
    • 使用低级求解器(LowLevelSolver)处理具体路径规划

  5. CBS方法：Conflict-Based Search算法

    • 通过约束树搜索无冲突路径

冲突避免机制

项目使用多种技术避免智能体之间的冲突：

  1. 预约表(ReservationTable)：记录节点在特定时间段的占用情况

    • 使用时间间隔(Interval)表示节点占用
    • 支持添加、移除和检查预约

  2. 死锁处理(DeadlockHandler)：检测和解决死锁情况

    • 当智能体长时间无法移动时触发
    • 提供随机跳跃等策略解决死锁

  3. 优先级机制：为智能体分配优先级，高优先级先规划

    • 可基于距离、任务重要性等因素确定优先级

路径与地图的结合

  1. PathManager类：连接实际仓库环境与MAPF系统

    • 将实际机器人(Bot)转换为Agent对象(getBotAgentDictionary方法)
    • 调用选定的MAPF算法规划路径
    • 将规划结果应用回实际机器人

  2. 队列管理：处理特殊区域如输入/输出站的排队情况

    • 使用QueueManager管理队列
    • 为排队机器人分配合适的目标位置

总结来说，RAWSim-O项目使用了基于节点和边的图结构表示环境，并实现了多种先进的MAPF算法来协调机器人移动，包括WHCAv/WHCAn、FAR、PAS、ODID和CBS等。系统通过预约表、死锁处理和优先级机制避免冲突，并通过PathManager将算法与实际仓库环境无缝集成。

2 现在这个程序中的用的是那种多智能体路径规划算法，他的原理是是怎么样

我需要查看项目中实际使用的多智能体路径规划算法及其原理。我将搜索相关配置和实现代码，找出默认使用的算法以及其工作原理。

toolName: search_codebase

status: success

query: PathManager default path planning method configuration

我需要查看更多关于默认使用的FAR算法的实现细节，以了解其原理。

toolName: search_codebase

status: success

query: FARMethod implementation path planning algorithm

我需要查看更多关于其他路径规划算法的实现，特别是WHCAv和WHCAn算法，以便全面了解项目中使用的多智能体路径规划算法。

toolName: search_codebase

status: success

query: WHCAvMethod WHCAnMethod implementation

项目中使用的多智能体路径规划算法及其原理

根据代码分析，该项目实现了多种多智能体路径规划（Multi-Agent Path Finding, MAPF）算法，默认使用的是FAR（Flow Annotation Re-planning）算法。以下是主要算法及其原理：

1. FAR（Flow Annotation Re-planning）算法

这是项目默认使用的算法，由Ko-Hsin Cindy Wang和Adi Botea于2008年提出。

原理：

  • 基本思想：使用流注释和重规划策略，在智能体移动过程中动态调整路径
  • 核心机制：
    • 维护预约表（Reservation Table）记录节点占用情况
    • 使用等待关系（Wait-For Relation）处理智能体间的依赖
    • 实现死锁检测和处理机制
    • 支持两种避让策略：重新规划路径或选择临近节点

关键特性：

  • 动态避让：当检测到冲突时，智能体会等待或执行避让操作
  • 死锁处理：通过检测等待关系中的循环来识别死锁，并通过随机跳跃打破死锁
  • 最大等待时间：设置智能体最大等待时间（默认30秒），超时后会尝试绕开阻塞节点
  • 避让策略：
    • Es1：尝试突破性操作，有最大尝试次数限制
    • Es2：避免返回到已经避让过的节点

2. WHCAv/WHCAn（Windowed Hierarchical Cooperative A*）算法

这是基于Silver在2006年提出的Cooperative Pathfinding方法的变体。

原理：

  • WHCAv（基于窗口的分层协作A*，向量版本）：

    • 使用时间窗口限制路径规划的时间范围
    • 按优先级顺序为智能体规划路径
    • 在冲突时，高优先级智能体先规划路径，低优先级智能体避让
    • 使用预约表记录已规划路径的时空占用

  • WHCAn（基于窗口的分层协作A*，节点版本）：

    • 与WHCAv类似，但增加了偏置成本（Biased Cost）机制
    • 可以为已经被其他智能体规划使用的节点增加额外成本，引导智能体选择不同路径
    • 支持更精细的冲突解决

关键特性：

  • 时间窗口：限制规划的时间范围（WHCAv默认20秒，WHCAn默认15秒）
  • 分层规划：按照优先级或距离目标的远近顺序规划
  • RRA*：使用反向可恢复A算法（Reverse Resumable A）计算启发式值
  • 预约表：记录节点在特定时间段的占用情况

3. 其他实现的算法

项目还实现了多种其他MAPF算法：

  • CBS（Conflict-Based Search）：由Sharon等人提出，通过构建冲突树寻找最优解
  • OD_ID（Operator Decomposition with Independence Detection）：由Standley提出，结合操作分解和独立检测
  • PAS（Parallel Agent Simulation）：由Erdmann提出，支持并行多智能体路径规划
  • BCP（Biased Cost Pathfinding）：由Geramifard提出，通过偏置成本引导智能体避开冲突
  • Simple：基本的A*实现
  • Dummy：简单的随机跳跃方法，主要用于测试

地图表示与智能体路径规划的结合

项目使用基于节点和边的图结构表示地图：

  1. 图结构：

    • 通过Graph类表示，包含节点和边的集合
    • 节点表示位置点，边表示可行走路径
    • 支持普通边和特殊边（如电梯边）

  2. 路径规划与地图结合：

    • PathManager负责将航点图（WaypointGraph）转换为MAPF算法使用的轻量级图结构
    • 通过GenerateGraph方法创建包含节点和边的图
    • 为每个智能体（Agent）分配起点和目标节点

  3. 预约机制：

    • 使用ReservationTable管理节点和边的时空占用
    • 智能体规划路径时需检查并更新预约表
    • 预约表帮助避免时空冲突

总的来说，项目实现了多种先进的多智能体路径规划算法，默认使用的FAR算法在处理动态环境和冲突避免方面表现良好，适合仓库机器人等实时系统的路径规划需求。

3

我需要查找PathManager类中的GenerateGraph方法来分析图结构生成过程。

toolName: search_codebase

status: success

query: PathManager GenerateGraph implementation

我需要查找路径规划算法的执行流程，特别是PathManager的_reoptimize方法。

toolName: search_codebase

status: success

query: PathManager _reoptimize implementation

我需要查找路径规划算法的核心逻辑，特别是关于路径阻塞、等待步骤和死锁打破的实现。

toolName: search_codebase

status: success

query: FARMethod FindPaths implementation deadlock

我需要查看DeadlockHandler类的实现，以了解死锁检测和处理机制。

toolName: search_codebase

status: success

query: DeadlockHandler implementation

我需要查看FARMethod类中的路径规划算法核心逻辑，特别是路径阻塞、等待步骤和死锁打破的实现。

toolName: search_codebase

status: success

query: FARMethod FindPaths implementation

我需要查看PathManager类中的GenerateGraph方法实现，以了解图结构生成的详细过程。

toolName: search_codebase

status: success

query: PathManager GenerateGraph

我需要查看PathManager类中的_reoptimize方法实现，以了解路径规划算法的执行流程。

toolName: search_codebase

status: success

query: PathManager _reoptimize implementation

我已经收集了足够的信息，可以总结多智能体路径规划的实现流程，包括图结构生成、路径规划算法执行流程和算法核心逻辑。

toolName: todo_write

status: success

Todos updated: 5 items

多智能体路径规划实现流程详细分析

通过对代码的深入分析，我已经梳理出了该系统中多智能体路径规划的完整实现流程和数据流向。下面将结合代码详细说明图结构生成、路径规划算法执行流程以及算法核心逻辑。

一、图结构生成

图结构生成是多智能体路径规划的基础，在PathManager类的GenerateGraph方法中实现：

protected Graph GenerateGraph()
{
    var waypointGraph = Instance.WaypointGraph;

    //Give every way point an unique id
    _waypointIds = new BiDictionary<Waypoint, int>();
    int id = 0;
    foreach (var tier in waypointGraph.GetWayPoints())
        foreach (var waypoint in tier.Value)
            _waypointIds.Add(waypoint, id++);

    //initiate queue managers
    _queueManagers = new Dictionary<Waypoint, QueueManager>();
    foreach (IQueuesOwner queueOwner in Instance.InputStations.Cast<IQueuesOwner>().Union(Instance.OutputStations.Cast<IQueuesOwner>().Union(Instance.Elevators.Cast<IQueuesOwner>())))
        foreach (var queue in queueOwner.Queues)
            _queueManagers.Add(queue.Key, new QueueManager(queue.Key, queue.Value, this));

    //create the lightweight graph
    var graph = new Graph(_waypointIds.Count);

    // Create collection of all node meta information
    _nodeMetaInfo = Instance.Waypoints.ToDictionary(k => k, v => new NodeInfo()
    {
        ID = _waypointIds[v],
        IsQueue = v.QueueManager != null,
        QueueTerminal = v.QueueManager != null ? _waypointIds[v.QueueManager.QueueWaypoint] : -1,
    });
    // Submit node meta info to graph
    foreach (var waypoint in Instance.Waypoints)
        graph.NodeInfo[_waypointIds[waypoint]] = _nodeMetaInfo[waypoint];

    //create edges
    foreach (var tier in waypointGraph.GetWayPoints())
    {
        foreach (var waypointFrom in tier.Value)
        {
            //create Array
            Edge[] outgoingEdges = new Edge[waypointFrom.Paths.Count(w => w.Tier == tier.Key)];
            ElevatorEdge[] outgoingElevatorEdges = new ElevatorEdge[waypointFrom.Paths.Count(w => w.Tier != tier.Key)];

            //fill Array
            int edgeId = 0;
            int elevatorEdgeId = 0;
            foreach (var waypointTo in waypointFrom.Paths)
            {
                //elevator edge
                if (waypointTo.Tier != tier.Key)
                {
                    var elevator = Instance.Elevators.First(e => e.ConnectedPoints.Contains(waypointFrom) && e.ConnectedPoints.Contains(waypointTo));

                    outgoingElevatorEdges[elevatorEdgeId++] = new ElevatorEdge
                    {
                        From = _waypointIds[waypointFrom],
                        To = _waypointIds[waypointTo],
                        Distance = 0,
                        TimeTravel = elevator.GetTiming(waypointFrom, waypointTo),
                        Reference = elevator
                    };
                }
                else
                {
                    //normal edge
                    int angle = Graph.RadToDegree(Math.Atan2(waypointTo.Y - waypointFrom.Y, waypointTo.X - waypointFrom.X));
                    Edge edge = new Edge
                    {
                        From = _waypointIds[waypointFrom],
                        To = _waypointIds[waypointTo],
                        Distance = waypointFrom.GetDistance(waypointTo),
                        Angle = (short)angle,
                        FromNodeInfo = _nodeMetaInfo[waypointFrom],
                        ToNodeInfo = _nodeMetaInfo[waypointTo],
                    };
                    outgoingEdges[edgeId++] = edge;
                }
            }

            //set Array
            graph.Edges[_waypointIds[waypointFrom]] = outgoingEdges;
            graph.PositionX[_waypointIds[waypointFrom]] = waypointFrom.X;
            graph.PositionY[_waypointIds[waypointFrom]] = waypointFrom.Y;
            if (outgoingElevatorEdges.Length > 0)
                graph.ElevatorEdges[_waypointIds[waypointFrom]] = outgoingElevatorEdges;
        }
    }

    return graph;
}

图结构生成过程包括以下关键步骤：

  1. 节点ID分配：为每个航点（Waypoint）分配唯一ID，存储在_waypointIds双向字典中
  2. 队列管理器初始化：为输入站、输出站和电梯的队列创建队列管理器
  3. 轻量级图创建：创建Graph对象，节点数量为航点总数
  4. 节点元信息设置：为每个节点创建NodeInfo对象，包含ID、是否为队列节点、队列终端等信息
  5. 边创建：
    • 普通边：同一层的航点之间的连接，计算距离和角度
    • 电梯边：不同层之间的连接，通过电梯实现，记录时间而非距离
  6. 位置信息设置：记录每个节点的X、Y坐标

这样生成的图结构包含了所有必要的拓扑信息和元数据，为后续的路径规划算法提供基础。

二、路径规划算法执行流程

路径规划算法的执行流程主要在PathManager类的_reoptimize方法中实现：

private void _reoptimize(double currentTime)
{
    //statistics
    _statStart(currentTime);

    // Log timing
    DateTime before = DateTime.Now;

    // Get agents
    Dictionary<BotNormal, Agent> botAgentsDict;
    getBotAgentDictionary(out botAgentsDict, currentTime);
    // Update locks and obstacles
    UpdateLocksAndObstacles();

    //get path => optimize!
    PathFinder.FindPaths(currentTime, botAgentsDict.Values.ToList());

    foreach (var botAgent in botAgentsDict)
        botAgent.Key.Path = botAgent.Value.Path;

    // Calculate time it took to plan the path(s)
    Instance.Observer.TimePathPlanning((DateTime.Now - before).TotalSeconds);

    _statEnd(currentTime);
}

路径规划算法的执行流程包括以下关键步骤：

  1. 触发条件：在PathManager.Update方法中，当满足以下条件时触发路径规划：

    //check if any bot request a re-optimization and reset the flag
var reoptimize = Instance.Bots.Any(b => ((BotNormal)b).RequestReoptimization);

if (reoptimize)
{
    //call re optimization
    _lastCallTimeStamp = currentTime;
    _reoptimize(currentTime);

    //reset flag
    foreach (var bot in Instance.Bots)
        ((BotNormal)b).RequestReoptimization = false;
}

  2. 获取机器人代理：调用getBotAgentDictionary方法，将BotNormal对象转换为路径规划引擎使用的Agent对象

  3. 更新锁和障碍物信息：调用UpdateLocksAndObstacles方法，更新节点的锁定状态和障碍物信息：

    private void UpdateLocksAndObstacles()
{
    // First reset locked / occupied by obstacle info
    foreach (var nodeInfo in _nodeMetaInfo)
    {
        nodeInfo.Value.IsLocked = false;
        nodeInfo.Value.IsObstacle = false;
    }
    // Prepare waypoints blocked within the queues
    IEnumerable<Waypoint> queueBlockedNodes = _queueManagers.SelectMany(m => m.Value.LockedWaypoints.Keys);
    // Prepare waypoints blocked by idling robots
    IEnumerable<Waypoint> idlingAgentsBlockedNodes = Instance.Bots.Cast<BotNormal>().Where(b => b.IsResting()).Select(b => b.CurrentWaypoint);
    // Prepare obstacles by placed pods
    IEnumerable<Waypoint> podPositions = Instance.WaypointGraph.GetPodPositions();
    // Now update with new locked / occupied by obstacle info
    foreach (var lockedWP in queueBlockedNodes.Concat(idlingAgentsBlockedNodes))
        _nodeMetaInfo[lockedWP].IsLocked = true;
    foreach (var obstacleWP in podPositions)
        _nodeMetaInfo[obstacleWP].IsObstacle = true;
}

  4. 调用路径规划算法：调用PathFinder.FindPaths方法，执行具体的路径规划算法

  5. 应用规划结果：将规划结果（Path对象）设置为机器人的路径

  6. 统计和记录：记录路径规划所用时间，更新统计信息

三、算法核心逻辑（以FARMethod为例）

以FARMethod（Flow Annotation Re-planning）为例，分析其核心逻辑，特别是路径阻塞、等待步骤和死锁打破的实现：

public override void FindPaths(double currentTime, List<Agent> agents)
{
    Stopwatch.Restart();

    //Reservation Table
    _reservationTable.Clear();
    var fixedBlockage = AgentInfoExtractor.getStartBlockage(agents, currentTime);

    //panic times initialization
    foreach (var agent in agents)
    {
        if (!_waitTime.ContainsKey(agent.ID))
            _waitTime.Add(agent.ID, double.NegativeInfinity);
        if (!_moveTime.ContainsKey(agent.ID))
            _moveTime.Add(agent.ID, currentTime);
        if (!_es2evadedFrom.ContainsKey(agent.ID))
            _es2evadedFrom.Add(agent.ID, new HashSet<int>());
    }

    //set fixed blockage
    try
    {
        foreach (var agent in agents)
        {
            //all intervals from now to the moment of stop
            // ...
        }
        
        // 为每个机器人规划路径
        foreach (var agent in agents.Where(a => a.RequestReoptimization && !a.FixedPosition && a.NextNode != a.DestinationNode))
        {
            //runtime exceeded
            if (Stopwatch.ElapsedMilliseconds / 1000.0 > RuntimeLimitPerAgent * agents.Count * 0.9 || Stopwatch.ElapsedMilliseconds / 1000.0 > RunTimeLimitOverall)
            {
                Communicator.SignalTimeout();
                return;
            }

            deadlockBreakingManeuver = false;

            //agent is allowed to move?
            if (currentTime < _waitTime[agent.ID])
                continue;

            //remove blocking next node
            _reservationTable.RemoveIntersectionWithTime(agent.NextNode, agent.ArrivalTimeAtNextNode);

            //Create RRA* search if necessary.
            ReverseResumableAStar rraStar;
            if (!_rraStar.TryGetValue(agent.ID, out rraStar) || rraStar == null || rraStar.StartNode != agent.DestinationNode)
            {
                //new search
                rraStar = new ReverseResumableAStar(Graph, agent, agent.Physics, agent.DestinationNode, new HashSet<int>());
                _rraStar[agent.ID] = rraStar;
                _moveTime[agent.ID] = currentTime;
            }

            //already found in RRA*?
            var found = rraStar.Closed.Contains(agent.NextNode);

            //If the search is old, the path may me blocked now
            if (found && rraStar.PathContains(agent.NextNode))
            {
                //new search
                rraStar = new ReverseResumableAStar(Graph, agent, agent.Physics, agent.DestinationNode, new HashSet<int>());
                _rraStar[agent.ID] = rraStar;
                found = false;
            }

            //Is a search processing necessary
            if (!found)
                found = rraStar.Search(agent.NextNode);

            //the search ended with no result => just wait a moment
            if (!found)
            {
                //new search
                _rraStar[agent.ID] = null;

                //still not found? Then wait!
                if (_waitTime[agent.ID] < currentTime - LengthOfAWaitStep * 2f)
                {
                    waitStep(agent, agent.ID, currentTime);
                    continue;
                }
                else
                {
                    deadlockBreakingManeuver = true;
                }
            }

            if (!deadlockBreakingManeuver)
            {
                //set the next step of the path
                nextHopNodes = _getNextHopNodes(currentTime, agent, out reservationOwnerAgentId, out reservationOwnerNodeId);

                //avoid going back
                if (Es2BackEvadingAvoidance && nextHopNodes.Count > 1 && _es2evadedFrom[agent.ID].Contains(nextHopNodes[1]))
                    deadlockBreakingManeuver = true;
                else if (Es2BackEvadingAvoidance)
                    _es2evadedFrom[agent.ID].Clear();

                //found a path => set it
                if (!deadlockBreakingManeuver && nextHopNodes.Count > 1)
                {
                    _setPath(nextHopNodes, agent);
                    _moveTime[agent.ID] = currentTime;
                    continue;
                }
            }

            //deadlock breaking maneuver due to wait time
            if (!deadlockBreakingManeuver)
            {
                deadlockBreakingManeuver = currentTime - _moveTime[agent.ID] > MaximumWaitTime;
            }

            //deadlock breaking maneuver due to wait for relation circle
            if (!deadlockBreakingManeuver)
            {
                //transitive closure of wait for relation
                HashSet<int> waitForSet = new HashSet<int>();
                var waitForID = agent.ID;
                while (!deadlockBreakingManeuver && _waitFor.ContainsKey(waitForID))
                {
                    if (waitForSet.Contains(waitForID))
                        deadlockBreakingManeuver = true;
                    waitForSet.Add(waitForID);
                    waitForID = _waitFor[waitForID];
                }
            }

            // 处理死锁打破
            if (deadlockBreakingManeuver)
            {
                // 根据不同的规避策略处理
                switch (_evadingStrategy)
                {
                    case EvadingStrategy.EvadeToDestination:
                        // 尝试重新规划到目的地的路径
                        // ...
                        break;
                    case EvadingStrategy.EvadeToNextNode:
                        // 尝试找一个可行的下一个节点
                        var foundBreakingManeuverEdge = false;
                        var possibleEdges = new List<Edge>(Graph.Edges[agent.NextNode]);
                        shuffle<Edge>(possibleEdges, Randomizer);
                        foreach (var edge in possibleEdges.Where(e => !e.ToNodeInfo.IsLocked && (agent.CanGoThroughObstacles || !e.ToNodeInfo.IsObstacle) && !_es2evadedFrom[agent.ID].Contains(e.To)))
                        {
                            //create intervals
                            var intervals = _reservationTable.CreateIntervals(currentTime, currentTime, 0, agent.Physics, agent.NextNode, edge.To, true);
                            reservationOwnerNodeId = -1;
                            reservationOwnerAgentId = -1;

                            //check if a reservation is possible
                            if (_reservationTable.IntersectionFree(intervals, out reservationOwnerNodeId, out reservationOwnerAgentId))
                            {
                                foundBreakingManeuverEdge = true;

                                //avoid going back
                                if (this.Es2BackEvadingAvoidance)
                                    _es2evadedFrom[agent.ID].Add(edge.To);

                                //create a path
                                agent.Path.Clear();
                                agent.Path.AddLast(edge.To, true, LengthOfAWaitStep);

                                break;
                            }
                        }

                        if (!foundBreakingManeuverEdge)
                        {
                            //Clear the nodes
                            _es2evadedFrom[agent.ID].Clear();

                            //just wait
                            waitStep(agent, agent.ID, currentTime);
                        }
                        break;
                }
            }

            //deadlock?
            if (UseDeadlockHandler)
                if (_deadlockHandler.IsInDeadlock(agent, currentTime))
                    _deadlockHandler.RandomHop(agent);
        }
    }
    catch (Exception ex)
    {
        // 异常处理
    }
}

算法核心逻辑包括以下关键部分：

1. 路径阻塞处理

当路径被阻塞时，算法会采取以下措施：

  • 预约表管理：使用_reservationTable管理机器人路径预约，避免冲突

  • 路径搜索：使用ReverseResumableAStar（RRA*）算法搜索从目标节点到当前节点的路径

  • 路径验证：检查搜索到的路径是否可行，如果不可行则尝试重新搜索或等待

  • 获取下一跳节点：通过_getNextHopNodes方法获取下一跳节点，同时检查是否有冲突：

    private List<int> _getNextHopNodes(double currentTime, Agent agent, out int reservationOwnerAgentId, out int reservationOwnerNodeId)
{
    var nextHopNodes = _rraStar[agent.ID].NextsNodesUntilTurn(agent.NextNode);
    var startReservation = currentTime;
    reservationOwnerNodeId = -1;
    reservationOwnerAgentId = -1;

    //while the agent could possibly move
    while (nextHopNodes.Count > 1)
    {
        //create intervals
        var intervals = _reservationTable.CreateIntervals(currentTime, startReservation, 0, agent.Physics, nextHopNodes[0], nextHopNodes[nextHopNodes.Count - 1], true);

        //check if a reservation is possible
        if (_reservationTable.IntersectionFree(intervals, out reservationOwnerNodeId, out reservationOwnerAgentId))
        {
            break;
        }
        else
        {
            //delete all nodes from the reserved node to the end
            var deleteFrom = nextHopNodes.IndexOf(reservationOwnerNodeId);
            for (int i = nextHopNodes.Count - 1; i >= deleteFrom; i--)
                nextHopNodes.RemoveAt(i);
        }
    }

    return nextHopNodes;
}

2. 等待步骤处理

当无法找到可行路径或需要等待时，算法会执行等待操作：

private void waitStep(Agent agent, int agentId, double currentTime)
{
    agent.Path.Clear();
    agent.Path.AddLast(agent.NextNode, true, LengthOfAWaitStep);
    _waitTime[agentId] = currentTime + LengthOfAWaitStep;
}

等待步骤的关键点：

  • 清空当前路径
  • 添加一个在当前节点等待的动作
  • 设置等待时间，防止频繁重规划

3. 死锁打破

系统使用多种机制检测和处理死锁：

  1. 等待时间检测：如果机器人在一个位置等待时间过长，触发死锁打破：

    deadlockBreakingManeuver = currentTime - _moveTime[agent.ID] > MaximumWaitTime;

  2. 等待关系环检测：检测机器人之间的等待关系是否形成环：

    //transitive closure of wait for relation
HashSet<int> waitForSet = new HashSet<int>();
var waitForID = agent.ID;
while (!deadlockBreakingManeuver && _waitFor.ContainsKey(waitForID))
{
    if (waitForSet.Contains(waitForID))
        deadlockBreakingManeuver = true;
    waitForSet.Add(waitForID);
    waitForID = _waitFor[waitForID];
}

  3. 死锁处理器：使用DeadlockHandler类处理死锁：

    //deadlock?
if (UseDeadlockHandler)
    if (_deadlockHandler.IsInDeadlock(agent, currentTime))
        _deadlockHandler.RandomHop(agent);

  4. 随机跳跃：DeadlockHandler.RandomHop方法实现了随机跳跃以打破死锁：

    public bool RandomHop(Agent agent, ReservationTable reservationTable = null, double currentTime = 0.0, bool finalReservation = false, bool insertReservation = false)
{
    //try to find a free hop
    var possibleEdges = new List<Edge>(_graph.Edges[agent.NextNode]);
    shuffle<Edge>(possibleEdges);
    foreach (var edge in possibleEdges.Where(e => !e.ToNodeInfo.IsLocked && (agent.CanGoThroughObstacles || !e.ToNodeInfo.IsObstacle)))
    {
        //create intervals
        if (reservationTable != null)
        {
            var intervals = reservationTable.CreateIntervals(currentTime, currentTime, 0, agent.Physics, agent.NextNode, edge.To, finalReservation);

            //check if a reservation is possible
            if (reservationTable.IntersectionFree(intervals))
            {
                if (insertReservation)
                    reservationTable.Add(intervals);

                //create a path
                agent.Path.Clear();
                agent.Path.AddLast(edge.To, true, _rnd.NextDouble() * LengthOfAWaitStep);

                return true;
            }
        }
        else
        {
            //create a path
            agent.Path.Clear();
            agent.Path.AddLast(edge.To, true, _rnd.NextDouble() * LengthOfAWaitStep);

            return true;
        }
    }

    return false;
}

  5. 规避策略：系统实现了两种规避策略：

    • EvadeToDestination：尝试重新规划到目的地的路径
    • EvadeToNextNode：尝试找一个可行的下一个节点

通过这些机制，系统能够有效地处理路径阻塞、等待和死锁情况，确保多个机器人能够高效、无冲突地在仓库环境中移动。

总结

多智能体路径规划系统通过精心设计的图结构生成、路径规划算法执行流程和算法核心逻辑，实现了高效的多机器人协同导航。系统的关键特点包括：

  1. 模块化设计：将路径管理、图结构生成、路径规划算法和死锁处理分离为不同模块
  2. 多种算法支持：支持FAR、CBS、WHCAv*等多种路径规划算法
  3. 预约表机制：使用预约表管理机器人路径，避免冲突
  4. 死锁检测与处理：实现多种死锁检测和处理机制
  5. 适应性规划：根据环境变化动态调整路径规划

这种设计使系统能够在复杂的仓库环境中高效地协调多个机器人的移动，最大限度地减少冲突和死锁情况。

4

我需要查找PathManager类中GenerateGraph方法的实现，了解其输入输出数据结构和执行流程。

toolName: search_codebase

status: success

query: PathManager GenerateGraph implementation

我需要查找路径规划算法的执行流程，特别是PathManager的_reoptimize方法。

toolName: search_codebase

status: success

query: PathManager _reoptimize implementation

我需要查找路径规划算法中关于路径阻塞、等待步骤和死锁打破的核心逻辑实现。

toolName: search_codebase

status: success

query: FARMethod FindPaths implementation deadlock

我需要查看DeadlockHandler类的实现，以了解死锁检测和处理机制。

toolName: search_codebase

status: success

query: DeadlockHandler implementation

我需要查看PathManager.cs中GenerateGraph方法的实现，以了解其输入输出数据结构和执行流程。

toolName: view_files

status: success

filePath: d:\work\github_downlaod_user\merschformann\RAWSim-O-main\RAWSimO.Core\Control\PathManager.cs

我需要继续查看PathManager.cs文件的更多内容，特别是GenerateGraph方法的后续部分和相关方法。

toolName: view_files

status: success

filePath: d:\work\github_downlaod_user\merschformann\RAWSim-O-main\RAWSimO.Core\Control\PathManager.cs

我需要查看Graph类的定义，以了解GenerateGraph方法的输出数据结构。

toolName: search_codebase

status: success

query: Graph class definition MultiAgentPathFinding

我需要查看NodeInfo类的定义，以了解GenerateGraph方法中节点元信息的结构。

toolName: search_codebase

status: success

query: NodeInfo class definition MultiAgentPathFinding

我需要查看Edge类的定义，以了解GenerateGraph方法中创建的边的数据结构。

toolName: search_codebase

status: success

query: Edge class definition MultiAgentPathFinding

我需要查看Graph类的完整定义，以了解GenerateGraph方法的输出数据结构。

toolName: view_files

status: success

filePath: d:\work\github_downlaod_user\merschformann\RAWSim-O-main\RAWSimO.MultiAgentPathFinding\Elements\Graph.cs

PathManager.GenerateGraph() 函数分析

输入输出数据结构

输入

  • 隐式输入：PathManager类中的Instance属性（包含了整个仓库模拟系统的状态）
    • Instance.WaypointGraph：路点图，包含了所有路点及其连接关系
    • Instance.InputStations：输入站点
    • Instance.OutputStations：输出站点
    • Instance.Elevators：电梯
    • Instance.Waypoints：所有路点

输出

  • Graph类型的对象：用于多智能体路径规划的轻量级图结构

    • NodeCount：节点数量
    • PositionX[]和PositionY[]：节点的x和y坐标
    • NodeInfo[]：节点的元信息数组
    • Edges[][]：图的边（二维数组，[nodeID][edgeID]）
    • ElevatorEdges：电梯边的字典

  • 副作用：初始化PathManager类的以下成员变量

    • _waypointIds：路点到ID的双向字典
    • _queueManagers：队列管理器字典
    • _nodeMetaInfo：节点元信息字典

执行流程

开始

为每个路点分配唯一ID

初始化队列管理器

创建轻量级图结构

创建节点元信息集合

将节点元信息提交到图

创建普通边

创建电梯边

返回生成的图

结束

详细执行步骤

  1. 为每个路点分配唯一ID

    _waypointIds = new BiDictionary<Waypoint, int>();
int id = 0;
foreach (var tier in waypointGraph.GetWayPoints())
    foreach (var waypoint in tier.Value)
        _waypointIds.Add(waypoint, id++);
    • 创建一个双向字典，将每个路点映射到一个唯一的整数ID
    • 按层级遍历所有路点，为每个路点分配递增的ID

  2. 初始化队列管理器

    _queueManagers = new Dictionary<Waypoint, QueueManager>();
foreach (IQueuesOwner queueOwner in Instance.InputStations.Cast<IQueuesOwner>().Union(Instance.OutputStations.Cast<IQueuesOwner>().Union(Instance.Elevators.Cast<IQueuesOwner>())))
    foreach (var queue in queueOwner.Queues)
        _queueManagers.Add(queue.Key, new QueueManager(queue.Key, queue.Value, this));
    • 创建队列管理器字典
    • 遍历所有拥有队列的对象（输入站点、输出站点和电梯）
    • 为每个队列创建一个队列管理器

  3. 创建轻量级图结构

    var graph = new Graph(_waypointIds.Count);
    • 创建一个新的Graph对象，节点数量为路点的数量

  4. 创建节点元信息集合

    _nodeMetaInfo = Instance.Waypoints.ToDictionary(k => k, v => new NodeInfo()
{
    ID = _waypointIds[v],
    IsQueue = v.QueueManager != null,
    QueueTerminal = v.QueueManager != null ? _waypointIds[v.QueueManager.QueueWaypoint] : -1,
});
    • 为每个路点创建一个NodeInfo对象
    • 设置节点ID、是否为队列、队列终端节点ID等信息

  5. 将节点元信息提交到图

    foreach (var waypoint in Instance.Waypoints)
    graph.NodeInfo[_waypointIds[waypoint]] = _nodeMetaInfo[waypoint];
    • 将创建的节点元信息添加到图的NodeInfo数组中

  6. 创建普通边和电梯边

    foreach (var tier in waypointGraph.GetWayPoints())
{
    foreach (var waypointFrom in tier.Value)
    {
        //创建数组
        Edge[] outgoingEdges = new Edge[waypointFrom.Paths.Count(w => w.Tier == tier.Key)];
        ElevatorEdge[] outgoingElevatorEdges = new ElevatorEdge[waypointFrom.Paths.Count(w => w.Tier != tier.Key)];

        //填充数组
        int edgeId = 0;
        int elevatorEdgeId = 0;
        foreach (var waypointTo in waypointFrom.Paths)
        {
            //电梯边
            if (waypointTo.Tier != tier.Key)
            {
                var elevator = Instance.Elevators.First(e => e.ConnectedPoints.Contains(waypointFrom) && e.ConnectedPoints.Contains(waypointTo));

                outgoingElevatorEdges[elevatorEdgeId++] = new ElevatorEdge
                {
                    From = _waypointIds[waypointFrom],
                    To = _waypointIds[waypointTo],
                    Distance = 0,
                    TimeTravel = elevator.GetTransportationTime(),
                    Reference = elevator,
                    Angle = 0,
                    FromNodeInfo = _nodeMetaInfo[waypointFrom],
                    ToNodeInfo = _nodeMetaInfo[waypointTo]
                };
            }
            //普通边
            else
            {
                outgoingEdges[edgeId++] = new Edge
                {
                    From = _waypointIds[waypointFrom],
                    To = _waypointIds[waypointTo],
                    Distance = waypointFrom.GetDistance(waypointTo),
                    Angle = Graph.RadToDegree(Math.Atan2(waypointTo.Y - waypointFrom.Y, waypointTo.X - waypointFrom.X)),
                    FromNodeInfo = _nodeMetaInfo[waypointFrom],
                    ToNodeInfo = _nodeMetaInfo[waypointTo]
                };
            }
        }

        //将边添加到图中
        graph.Edges[_waypointIds[waypointFrom]] = outgoingEdges;
        if (outgoingElevatorEdges.Length > 0)
            graph.ElevatorEdges.Add(_waypointIds[waypointFrom], outgoingElevatorEdges);
    }
}
    • 遍历每个层级的所有路点
    • 对于每个起始路点，创建两个数组：普通边数组和电梯边数组
    • 遍历起始路点的所有连接路点
      • 如果目标路点在不同层级，创建电梯边
      • 如果目标路点在同一层级，创建普通边
    • 将创建的边添加到图的Edges数组和ElevatorEdges字典中

  7. 返回生成的图

    return graph;
    • 返回构建完成的Graph对象

总结

GenerateGraph函数的主要作用是将仓库系统中的路点图转换为多智能体路径规划算法所需的轻量级图结构。这个转换过程包括：

  1. 为每个路点分配唯一ID
  2. 初始化队列管理器
  3. 创建轻量级图结构
  4. 设置节点元信息
  5. 创建普通边和电梯边

转换后的图结构包含了节点、边、节点元信息等，为后续的路径规划算法提供了必要的数据结构。

5

我需要查找Instance类的定义，以了解它包含的整个仓库模拟系统的状态。

toolName: search_codebase

status: success

query: Instance class definition RAWSimO.Core

我需要查看Instance类的完整定义，以了解它包含的整个仓库模拟系统的状态。

toolName: view_files

status: success

filePath: d:\work\github_downlaod_user\merschformann\RAWSim-O-main\RAWSimO.Core\InstanceCore.cs

我需要查看Instance类中与WaypointGraph相关的定义，以了解它是如何存储路点图的。

toolName: search_codebase

status: success

query: Instance WaypointGraph definition

我需要查看Instance类中WaypointGraph属性的定义，以了解它是如何在Instance类中声明的。

toolName: search_codebase

status: success

query: Instance WaypointGraph property declaration

我需要查看InstanceCore.cs文件中WaypointGraph属性的声明，以了解它在Instance类中是如何定义的。

toolName: view_files

status: success

filePath: d:\work\github_downlaod_user\merschformann\RAWSim-O-main\RAWSimO.Core\InstanceCore.cs

我需要继续查看InstanceCore.cs文件的更多内容，以找到WaypointGraph属性的声明。

toolName: view_files

status: success

filePath: d:\work\github_downlaod_user\merschformann\RAWSim-O-main\RAWSimO.Core\InstanceCore.cs

看来InstanceCore.cs文件只有105行，我需要查看其他相关文件来找到WaypointGraph属性的声明。根据之前的搜索结果，我将查看Instance类的其他部分文件。

toolName: search_codebase

status: success

query: Instance class WaypointGraph property

我已经找到了Instance类中WaypointGraph属性的定义，它在InstanceMetaInfo.cs文件的第39行。现在我可以总结所有的信息了。

toolName: todo_write

status: success

Todos updated: 3 items

Instance类中的关键属性及其在仓库模拟系统中的作用

通过查看代码，我找到了Instance类中WaypointGraph属性的定义以及相关属性的结构和功能。Instance类是整个仓库模拟系统的核心，包含了所有模拟所需的状态信息。

Instance类的定义

Instance类被定义为部分类(partial class)，其核心字段在InstanceCore.cs中定义，而额外的元信息和属性则在InstanceMetaInfo.cs中定义。

WaypointGraph属性

在InstanceMetaInfo.cs文件中第39行找到了WaypointGraph属性的定义：

/// <summary>
/// The waypoint-graph containing all waypoints.
/// </summary>
public WaypointGraph WaypointGraph = new WaypointGraph();

WaypointGraph类的结构

WaypointGraph类在WaypointGraph.cs中定义，它是仓库中所有路点的容器和管理器，主要包含以下数据结构：

  1. _waypoints: 按层级(Tier)组织的路点集合
  2. _inputStations: 输入站点到路点的映射
  3. _outputStations: 输出站点到路点的映射
  4. _pods: 货架(Pod)到路点的映射
  5. _waypointQuadtree: 用于空间索引的四叉树，按层级组织
  6. _largestEdgeBetweenTwoWaypoints: 每个层级中两个路点之间的最大边长

Instance类中的其他关键属性

Instance类中还包含许多其他重要属性，用于表示整个仓库模拟系统的状态：

  1. Waypoints: 所有路点的列表
  2. InputStations: 所有输入站点的列表
  3. OutputStations: 所有输出站点的列表
  4. Elevators: 所有电梯的列表
  5. Bots: 所有机器人的列表
  6. Pods: 所有货架的列表
  7. Compound: 声明所有物理属性的复合体
  8. ItemDescriptions: 所有可用SKU的列表
  9. ItemBundles: 所有已知的物品包的列表
  10. OrderList: 将传递给物品管理器的订单列表
  11. Semaphores: 所有信号量的列表

路点图的使用

在代码中，WaypointGraph被广泛用于：

  1. 添加新的路点：每当创建新的路点时，都会通过WaypointGraph.Add(waypoint)将其添加到路点图中
  2. 路径规划：在PathManager.GenerateGraph()方法中，使用Instance.WaypointGraph来获取所有路点并转换为多智能体路径规划所需的轻量级图结构
  3. 查询最近的路点：通过GetClosestWaypoint方法查找最接近给定位置的路点
  4. 查询一定距离内的路点：通过GetWaypointsWithinDistance方法查找给定位置周围一定距离内的所有路点

总结

Instance类的WaypointGraph属性是整个仓库模拟系统中路点网络的核心容器，它存储了所有路点及其连接关系，并提供了空间查询功能。Instance类作为整个模拟系统的状态容器，通过其众多属性维护了仓库中的各种元素（机器人、货架、站点、电梯等）及其关系，为路径规划和仓库操作提供了必要的数据基础。

6

我需要查看Waypoints目录下的文件，特别是WaypointGraph.cs和Waypoint.cs，以了解路点和边的存储结构。

toolName: view_files

status: success

filePath: d:\work\github_downlaod_user\merschformann\RAWSim-O-main\RAWSimO.Core\Waypoints\WaypointGraph.cs

现在我需要查看Waypoint.cs文件，以了解单个路点的数据结构和边的存储方式。

toolName: view_files

status: success

filePath: d:\work\github_downlaod_user\merschformann\RAWSim-O-main\RAWSimO.Core\Waypoints\Waypoint.cs

我需要继续查看Waypoint.cs文件的剩余部分，以了解更多关于路点和边的存储结构。

toolName: view_files

status: success

filePath: d:\work\github_downlaod_user\merschformann\RAWSim-O-main\RAWSimO.Core\Waypoints\Waypoint.cs

我需要查看QueueGuard.cs文件，以了解路点之间的边如何被管理和控制。

toolName: view_files

status: success

filePath: d:\work\github_downlaod_user\merschformann\RAWSim-O-main\RAWSimO.Core\Waypoints\QueueGuard.cs

我需要查看QueueSemaphore.cs文件，以了解路点之间的信号量控制机制。

toolName: view_files

status: success

filePath: d:\work\github_downlaod_user\merschformann\RAWSim-O-main\RAWSimO.Core\Waypoints\QueueSemaphore.cs

我需要查看Path.cs文件，以了解路点之间的路径是如何定义和存储的。

toolName: view_files

status: success

filePath: d:\work\github_downlaod_user\merschformann\RAWSim-O-main\RAWSimO.Core\Waypoints\Path.cs

我需要查看Waypoints目录下的所有文件，以了解完整的路点系统结构。

toolName: view_folder

status: success

d:\work\github_downlaod_user\merschformann\RAWSim-O-main\RAWSimO.Core\Waypoints

RAWSimO中的路点和边存储结构分析

在RAWSimO仓库模拟系统中，路点(Waypoint)和边(Path)的存储采用了多层次的数据结构设计，主要通过以下几个核心类实现：

1. 路点图整体结构 (WaypointGraph.cs)

路点图使用分层的哈希集合存储所有路点：

private Dictionary<Tier, HashSet<Waypoint>> _waypoints

这种结构将路点按层(Tier)组织，每层包含一个路点集合。此外，WaypointGraph还维护了：

  • 输入/输出站点映射：Dictionary<InputStation, HashSet<Waypoint>>和Dictionary<OutputStation, HashSet<Waypoint>>
  • 四叉树空间索引：Dictionary<Tier, QuadTree<Waypoint>>，用于快速空间查询
  • 电梯路点映射：Dictionary<Elevator, HashSet<Waypoint>>

2. 单个路点结构 (Waypoint.cs)

每个路点对象存储了：

基本属性

  • 位置信息：X、Y坐标和所属层(Tier)
  • 类型信息：是否为Pod存储位置、是否为工作站位置等

边的存储

路点之间的连接（即边）通过字典存储：

private Dictionary<Waypoint, double> _paths = new Dictionary<Waypoint, double>()

这个字典将目标路点作为键，路径距离作为值，表示从当前路点到目标路点的边。通过Paths属性可以枚举所有连接的路点。

交通控制机制

  • 守卫字典：private Dictionary<Waypoint, QueueGuard> _guards，用于管理路点间的通行控制
  • 机器人跟踪：_botsApproaching和_botsLeaving集合，记录正在接近或离开该路点的机器人

3. 通行控制结构

QueueGuard (QueueGuard.cs)

守卫对象控制路点之间的通行，包含：

  • 源路点和目标路点：From和To属性
  • 入口/出口标识：Entry属性
  • 屏障功能：Barrier属性，决定是否可以阻止进入
  • 关联的信号量：Semaphore属性

QueueSemaphore (QueueSemaphore.cs)

信号量控制特定区域内机器人的数量：

  • 容量限制：Capacity属性，表示区域内允许的最大机器人数
  • 当前请求数：Requests属性，表示区域内当前的机器人数
  • 机器人集合：_bots，记录区域内的所有机器人
  • 守卫集合：_guards，记录所有入口和出口守卫

数据结构总结

  1. 层次化存储：路点按层组织，便于分层管理和查询
  2. 图结构实现：
    • 路点作为图的节点
    • 路点间的路径作为图的边，存储在源路点的字典中
    • 边权重为路径距离
  3. 空间索引：使用四叉树实现快速空间查询
  4. 交通控制：
    • 守卫(QueueGuard)控制路点间的通行
    • 信号量(QueueSemaphore)限制特定区域内的机器人数量

这种设计使系统能够高效地：

  • 存储和查询大量路点
  • 计算最短路径
  • 进行空间查询
  • 实现复杂的交通控制逻辑

整体而言，RAWSimO采用了一种高效且灵活的数据结构来表示仓库中的路点网络和路径，支持复杂的路径规划和交通管理功能。

更新于 2025-09-19
 Python
返回 | 主页