Kinematic constraint multi-agent path planning method

Abstract

A computer-implemented kinematically constrained multi-agent pathfinding method is disclosed. The method includes the following steps: a) defining initial vertices and initial orientations for multiple agents in a graph; b) providing kinematic models containing the corresponding agent's pose and motion constraints for each agent; c) finding a path for each agent to move from its initial position and initial pose to a target position and target pose. The graph represents a shared space, and the initial vertices represent the initial positions of the corresponding agents in the shared space. The paths of the multiple agents are collision-free, and each agent's path follows its pose and motion constraints, traveling along one or more edges in the graph.

Description

Technical Field

[0001] This invention relates to a method for path planning and path search of multiple intelligent agents (such as robots) in a shared space. Background Technology

[0002] Multi-Agent Path Planning (MAPF) is a technique used to find optimal, collision-free paths from a start point to an end point for multiple robots or agents in a shared space. This technique is widely used in automated warehousing, airport management, office robots, and video games to solve the problem of large-scale agent movement and collaboration in enclosed environments. In industrial applications such as robotics and warehouse fulfillment systems, the core challenge lies in calculating collision-free trajectories in enclosed environments for hundreds of agents, while achieving the lowest possible cost (e.g., total distance or time consumption), with each agent assigned a unique start and destination position. Current state-of-the-art MAPF solvers employ centralized computation, providing globally optimal or near-optimal collision-free paths for hundreds of agents.

[0003] However, despite the numerous algorithms available for solving the MAPF problem, traditional solvers fail to consider kinematically constrained agents (such as forklifts). These agents cannot move as flexibly as standard AGVs (Automated Guided Vehicles), a flexibility crucial for critical scenarios like warehouse fulfillment. Furthermore, most traditional MAPF solvers plan paths based on an idealized static world assumption, neglecting uncertainties in the real-world environment, such as agent motion deviations and dynamic environmental changes. This approach can lead to serious failures when deployed in real-world environments: first, imperfections in mechanical control can cause the agent to run faster or slower than planned, resulting in motion deviations; second, unexpected obstacles (i.e., dynamic obstacles) may suddenly appear, blocking the planned path; and third, network fluctuations can cause delays or loss of critical messages (such as new path assignments and emergency stop commands), leading to desynchronization between actual movement and planned scheduling. Summary of the Invention

[0004] Accordingly, one aspect of the present invention provides a computer-implemented kinematically constrained multi-agent path planning method. The method includes the following steps: a) defining initial vertices with given initial poses for multiple agents in a graph; b) providing kinematic models for each agent, the kinematic models including the pose, direction of movement, and motion constraints of the corresponding agent; c) finding a path for each agent such that the agent moves from an initial position with a first given pose to a target position with a second given pose. The graph represents a shared space, and the initial vertices represent the initial positions of the corresponding agents in the shared space. The paths of the multiple agents are collision-free, and each agent's path follows one or more edges in the graph, simultaneously satisfying the agent's pose and motion constraints.

[0005] In some embodiments, for each of the plurality of agents, its attitude change along the path remains within a steering angle threshold.

[0006] In some embodiments, one or more of the aforementioned edges are straight lines or Bézier curves.

[0007] In some embodiments, step c) further includes a step of finding a path based on the total path distance cost, wherein the total cost includes a penalty term when the agent's movement direction is not the preferred direction.

[0008] In some embodiments, step c) further includes a deadlock conflict resolution step, comprising: d) generating a high-level deadlock-based constraint for one of the agents; e) searching for the nearest vertex for the agent by backtracking its path from the agent's vertex; f) designating the nearest vertex as a safe reference point; and g) moving the agent to the safe reference point to resolve the deadlock. The nearest vertex is an intersection point and does not conflict with the paths of the other agents.

[0009] In some embodiments, steps d)-e) are performed separately for the two agents involved in the deadlock. Step c) also includes selecting one agent to continue with steps f) and g) based on a cost comparison of the two agents.

[0010] In some embodiments, the cost of the two agents is calculated based on the number of backtracking steps required to return to their respective nearest vertices.

[0011] In some embodiments, the plurality of intelligent agents are unmanned forklifts, and the preferred direction is the forward movement direction of the forklift.

[0012] In some embodiments, the method further includes the step of controlling multiple intelligent agents via a server on a communication network to enable them to move autonomously along corresponding paths.

[0013] Another aspect of the present invention provides a computer-implemented method for global traffic management during multi-agent path planning. The method includes the following steps: a) for each of the multiple agents whose initial path in the graph would cause a collision, locking the farthest locked vertex on its initial path, and all vertices on the initial path preceding the farthest locked vertex; b) granting each of the multiple agents access rights along the planned path from its initial vertex to the farthest locked vertex; c) controlling the multiple agents, via a server located on a communication network, to autonomously reach their respective farthest locked vertices based on the granted access rights. The graph represents a shared space inhabited by the multiple agents. The farthest locked vertices of the multiple agents do not overlap. Granting access rights to an agent while it has not yet released a locked vertex will prevent other agents from entering those locked vertices.

[0014] In some embodiments, in step c), if the remote locked vertex of one of the multiple agents is the same as its initial vertex, then the agent waits at the initial vertex.

[0015] In some embodiments, step a) further includes: when one of the multiple agents requests a certain vertex faster than all other agents requesting the same vertex, locking that vertex as the furthest locked vertex of that agent in the graph.

[0016] According to another aspect of the invention, a non-transient computer-readable medium is provided, having stored thereon instructions executable by at least one processor to cause the computing device to perform the methods described above or variations thereof.

[0017] According to another aspect of the invention, a computing device is provided, including a memory storing instructions; and one or more processors configured by these instructions for performing the above-described method or variations thereof.

[0018] Embodiments of the present invention therefore provide a kinematic constraint MAPF solver specifically optimized for kinematically constrained agents (e.g., forklifts) operating in complex, path-constrained environments. The solver integrates collision-free control and provides a two-layer solution framework. The first layer (bottom layer) is a kinematic constraint planner that generates conflict-free, forward-priority path solutions for multiple agents under attitude and motion constraints, while possessing deadlock resolution capabilities for constrained paths that surpass those of traditional MAPF solvers. The second layer (top layer) is a collision-free controller that resists agent motion deviations and dynamic environmental changes, enabling dynamic adjustments for safety assurance. Attached Figure Description

[0019] The above and more features of the present invention will be clarified by the following description of preferred embodiments, which are merely illustrative and are described in conjunction with the accompanying drawings, wherein:

[0020] Figure 1 This demonstrates a typical graph structure, which includes multiple agents, their starting and target vertices (or points, nodes), and blocking vertices in the graph.

[0021] Figure 2a This is a diagram illustrating that a forklift is unable to perform a stationary turn on its path.

[0022] Figure 2b It is a schematic diagram showing that the forklift can only follow the G1 continuous path.

[0023] Figure 3a The diagram illustrates the initial and target states of a motion-constrained agent (taking a forklift as an example).

[0024] Figure 3b This demonstrates how to guide an agent to a destination when using a non-kinematically feasible planner. Figure 3a An unacceptable path planned based on the target state.

[0025] Figure 3c This demonstrates how, when using a kinematically feasible planner, the agent can reach... Figure 3a An acceptable path planned based on the target state.

[0026] Figure 4 The structure of an autonomous forklift control system in one embodiment of the present invention is shown.

[0027] Figure 5a The diagram shows the first type of edge – a straight edge.

[0028] Figure 5b The diagram shows the second type of edge – the Bézier curve.

[0029] Figure 6a This example demonstrates the forward movement direction of a forklift.

[0030] Figure 6b This example demonstrates the backward movement direction of a forklift.

[0031] Figure 7a An example of a forklift moving forward is shown, which results in an unacceptable pose along a given edge.

[0032] Figure 7b An example of a forklift moving backwards is shown, which results in... Figure 7a The pose of the given edge shown is acceptable.

[0033] Figure 8 The flowchart illustrates how the high-level collision-free solver and the low-level kinematic feasibility planner work together in a motion constraint MAPF solver according to an embodiment of the present invention.

[0034] Figure 9 It illustrates the relationship between the agent and different edges in the graph, and how the kinematic feasible planner performs path search for the agent.

[0035] Figure 10 An example of the step-by-step execution of a kinematic feasible planner is shown.

[0036] Figure 11 An example of searching for SRPs (Safety Reference Points) based on constraint tree expansion is shown.

[0037] Figure 12a The diagram illustrates an example of two agents initially planning their paths and detecting potential deadlock situations.

[0038] Figure 12b It shows that agent a1 in Figure 12a The process of finding a safe reference point in a scenario and moving to it.

[0039] Figure 12c It shows that agent a2 passes through the deadlock point, and agent a1 returns from the safe reference point.

[0040] Figure 12d This shows that agent a1 returned to the original planned path and arrived at the target vertex.

[0041] Figure 13a An example of a collision caused by the agent's motion deviation is shown.

[0042] Figure 13b This demonstrates how to avoid [something] through a road access permission mechanism. Figure 13a The collision scheme in the example.

[0043] Figure 14 The diagram illustrates an exemplary data processing system that can be used for kinematically constrained multi-agent path planning. Detailed Implementation

[0044] In the appended claims and the foregoing description of the invention, unless the context requires it to be explicitly stated or necessarily implied, the term "comprising" or its variations such as "including" or "containing" are used in an inclusive sense, indicating the presence of the stated feature but not excluding the presence or addition of other features in various embodiments of the invention.

[0045] In this specification and claims, "coupled" or "connected" means a coupling or connection achieved through direct or indirect communication (via one or more communication media), unless otherwise stated. "Direct connection" means that two communication components, nodes, or terminals are connected to each other without the intervention of any intermediate components.

[0046] An exemplary embodiment of the present invention provides a two-layer solver that includes a kinematic constraint MAPF algorithm integrated with a collision-free controller. As will be described in detail below, this two-layer architecture, through a high-level collision-free controller and a low-level kinematic constraint planner, enables the generation of conflict-free and forward-priority paths for multiple agents. It can also resolve deadlock problems in constrained paths and ensure safety. Therefore, the collision avoidance controller and the low-level kinematic constraint planner can generate conflict-free and forward-priority path schemes for multiple agents. This architecture can also resolve deadlock problems in constrained paths and ensure safety during execution. Therefore, the two-layer solver, by directly integrating attitude and orientation constraints into the planning algorithm, can generate kinematically feasible and practical paths. This two-layer solver can achieve reliable and conflict-free navigation in complex environments (such as single-lane roads), thereby enhancing the system's adaptability and robustness. Through the synergy of intelligent planning and centralized scheduling, proactive system-level safety assurance is achieved, effectively eliminating potential risks at the source.

[0047] Before describing exemplary embodiments of the present invention in detail, the standard MAPF solver and kinematic constraints, using a forklift as an example, will be reviewed below. As those skilled in the art know, the MAPF model is a formal problem definition for multi-agent path planning in a shared space, and its core components include the environment, multiple agents, and discrete time steps, such as... Figure 1 As shown. The environment is modeled in graph theory, represented as G = (V, E), where vertices (also called nodes or points) V represent discrete locations in the graph, and edges (also called links or segments) E represent connections between vertices. Figure 1 In the example, Figure 20 consists of a 4×10 grid map containing several blocking vertices 22. The agents (not shown) in this example are k independent robots (such as AGVs, forklifts), each agent a... i Having a unique starting vertex s i and target vertex g i . Figure 1 These starting and target vertices are labeled s1 to s4 and g1 to g4, respectively. A discrete time step is the result of dividing time into several stages, where each time step t... n An agent can choose to move to an adjacent vertex or remain at its current position. For the MAPF solver, a common conflict-free constraint is that at any given time, two or more agents must not meet in a way that causes a collision, or their paths must not intersect. For example, in... Figure 1In this problem, given four agents and their corresponding starting / target vertices, the solver needs to plan conflict-free paths for all agents, ensuring that they avoid collisions throughout their movement along their respective paths. In other words, the solver's goal is to find a path set {π1, π2, ...,πk} that guides each agent from its starting vertex s. i Reach the target vertex s without conflict i Another goal of the solver is to optimize the overall efficiency of the agent's movements (e.g., minimizing the sum of total time or cost).

[0048] However, as mentioned earlier, the standard MAPF solver does not consider kinematically constrained agents. Taking a forklift as an example, its kinematic characteristics differ from those of an ideal agent due to kinematic constraints, which include:

[0049] a) Inability to turn in place – The forklift’s motion is restricted by nonholonomic kinematic constraints, meaning it cannot perform in-place turning (i.e., rotating in place); its motion must follow a G1 continuous path.

[0050] b) Orientation constraints exist—forklifts have inherent front (body) and rear (forks) sections, and can only move strictly along the chassis orientation either forward (body first) or backward (forks first). Furthermore, the start and end points of their path must strictly match the orientation specified by the starting and target positions; and

[0051] c) Forward-priority motion – this is crucial for center of gravity control (rear-mounted fork structure).

[0052] Nonholonomic kinematic constraints are non-integrable restrictions on the velocity of a system. They typically limit the direction of movement to a specific path (e.g., prohibiting lateral movement) but do not restrict the reachable location. Figures 2a-2b The kinematic constraints of the forklift are illustrated. For example... Figure 2a As shown, when moving from vertex s to vertex g, the forklift cannot make a stationary turn at vertex V (e.g., a 1 / 4-turn turn). Instead, the forklift can only move along the continuous path G1, as illustrated in the example below. Figure 2b As shown. As those skilled in the art will know, G1 continuity (tangent continuity) requires that the path direction must change continuously, prohibiting sharp angles or instantaneous changes in direction. Visually, the path is smooth and fluid, with its tangent vector (direction of movement) showing a gradual change.

[0053] Figures 3a-3c Using a forklift as an example, this demonstrates how to plan paths for an agent subject to kinematic constraints. Figure 3a The forklift is shown in its initial state: it is located at the starting vertex S, with an attitude θ = 0. Here, θ represents the forklift's attitude; θ = 0 means the forklift body (excluding the forks) is facing to the right in the diagram. Assume the target vertex is G, and the target attitude is θ = 180 (i.e., the forklift body will face to the left). Figure 3aAs shown, there are different edges between vertices S and G, including the straight line S-P3, the straight line P3-G, the Bézier curve S-P4, and the Bézier curve P4-P3. Those skilled in the art will understand that a straight line is a linear path defined by a start and an end point, while a Bézier curve is a smooth parametric curve defined by a set of control points. Bézier curves are typically constructed in such a way that continuous Bézier curve segments satisfy at least G1 continuity; that is, in practice, Bézier curves are a concrete method for achieving G1 continuous paths.

[0054] Figure 3b This demonstrates an unacceptable path for a forklift planned using a non-kinematic feasible planner, taking the shortest route along lines S-P3 and P3-G. While this path allows the forklift to reach the target vertex G, the target orientation (which will be θ = 0) is deviated from its intended orientation—because the forklift cannot change its orientation while moving along a straight line. On the other hand, Figure 3c The correct path designed based on the kinematic feasible planner is shown: the forklift moves forward and backward along Bézier curve S-P4 and Bézier curve P4-P3 respectively, and its posture will be correct (i.e., θ = 180) when it reaches the target vertex G.

[0055] Figure 4 An embodiment of the invention is illustrated—an autonomous forklift control system based on graph theory programming and considering kinematic constraints—for handling tasks such as picking up and delivering goods in a warehouse without collisions. For simplicity, Figure 4 Only one forklift 32 is shown, but there are clearly multiple forklifts in the system, which is why MAPF planning is needed. Figure 4 The diagram also shows the user's mobile device 30, which acts as the user platform to assign tasks to the forklift 32. The mobile device 30 does not communicate directly with the forklift 32; instead, all instructions and other inputs are transmitted from the mobile device 30 to the two-layer MAPF solver 38 via a gateway (arrow 34). This solver 38 acts as the centralized controller / server for the forklift 32, providing methods for performing kinematic constraint path planning and collision-free control (see below for details). In addition to user input, the forklift 32 provides real-time feedback to the solver 38 on its position, speed, error codes (if any), task status, and other data, enabling the solver 38 to generate a conflict-free and forward-priority path for the forklift 32. After planning, the solver 38 outputs the orientation constraint path and collision-free control instructions to the forklift 32 via the gateway (arrow 36).

[0056] The applicable autonomous forklift models are not limited to this invention. As is known to those skilled in the art, modern autonomous forklifts are equipped with environmental perception and load status monitoring sensors, including 2D / 3D LiDAR and cameras for obstacle detection, pallet detection, and mapping, as well as SLAM (Simultaneous Localization and Mapping) systems for pallet mapping. Furthermore, the forklift includes a local control layer that translates planned actions (e.g., instructions from a centralized controller) into low-level execution commands. Each forklift is associated with a kinematic and dynamic model of the chassis (typically rear-wheel steering or an articulated structure), which is usually provided by the forklift manufacturer. Some autonomous forklifts can be adapted to achieve local navigation and obstacle avoidance. Even in a centralized MAPF architecture, the local layer can handle unexpected obstacles, reducing the burden on the centralized controller. However, because the centralized controller possesses global information on all activity routes and tasks, its central layer can resolve conflicts (such as lane deadlock and congestion) that are difficult for a purely local system to handle. Finally, the forklift is equipped with a wireless communication module (such as Wi-Fi, 5G or industrial radio frequency) to connect to the centralized controller via a communication network, enabling status updates, route downloads and health monitoring.

[0057] like Figure 4 As shown, the solver 38 typically comprises a two-layer structure: a bottom layer 42—acting as a kinematic constraint planner, generating conflict-free and forward-priority paths for multiple forklifts under attitude and motion constraints; and a top layer 40—acting as a collision-free controller to overcome agent motion deviations and dynamic environmental changes, providing a safety-assured adjustment scheme. The bottom layer 42 generates paths by following the motion constraints of each agent, simultaneously satisfying all conflict avoidance constraints, and provides the computed paths to the top layer 40. The top layer 40 acts as a communication bridge between the solver 38 and each forklift 32, managing global traffic resources and providing collision-free control constraints to the bottom layer 42. The bottom layer (42) is suitable for executing four main steps: Step 44 defines the pose and movement direction of the agent relative to the boundary; Step 46 designs a low-level kinematic feasibility planner; Step 48 designs a high-level deadlock-aware conflict resolver; and Step 50 finds a conflict-free path that satisfies the kinematic constraints of each agent. The top layer (40) mainly performs two operations: Step 52 executes a centralized greedy locking mechanism, and Step 54 executes a road access permission mechanism. All of the above steps will be described in detail in subsequent sections.

[0058] First refer to Figures 5a-6b Explain the definition process of the pose and movement direction of the agent and the edge (corresponding to) Figure 4(Step 44 in the diagram). Generally, the pose θ represents the orientation angle of the kinematically constrained agent, while the pose of the edge at the starting node is along the positive x-axis, ranging from -180° to 180°. For the straight lines in the diagram, θ represents the direction from the starting point to the ending point. Figure 5a The diagram illustrates the orientation θ of the straight line P1-P2, which represents the direction from the starting point P1 (x1, y1) to the ending point P2 (x2, y2). For the Bézier curve in the diagram, θ at the starting point indicates the direction from the first control point to the second control point. Figure 5b The orientation θ of the Bézier curve P1-P3 is shown, with the starting point being the direction from the first control point (x1, y1) to the second control point (x2, y2).

[0059] The complete definition of attitude θ is as follows:

[0060] (1)

[0061] On the other hand, the movement direction d is defined as one of two mutually exclusive states: FORWARD indicates that the agent is facing the same direction as the movement direction; BACKWARD indicates that the agent is facing the opposite direction. Figure 6a An example of forward movement is shown, in which the forklift can move from P1 to P2 along the straight line P1-P2, with an attitude of θ. Figure 6b An example of backward movement is shown, in which the forklift can move from P1 to P3 along the Bézier curve P1-P3 while maintaining the attitude θ.

[0062] Next, by reference Figures 7a-7b The process for verifying constrained motion is described (this process also belongs to...) Figure 4 (a component of step 44 in the previous section). In this embodiment, a graph G = (V, E) is used as the mesh graph for path planning. This graph contains vertices V and edges E, and each edge defines the pose and allowed direction of movement. This graph serves the dual function of a topology graph and a physical movement template. Under this framework, the kinematically constrained agent (such as a forklift) must strictly follow predefined boundaries (e.g., P1(x1, y1, 90) → P2(x2, y2, 180), forward), provided that its pose deviation is kept within the steering angle threshold T. This method eliminates the need for real-time kinematic path calculation within the agent itself. Specifically, the definitions are as follows:

[0063] (2)

[0064] Where θ is the agent's orientation; θ' is the agent's orientation after considering the movement direction; θe is the edge's orientation; and T is the turning angle threshold.

[0065] After obtaining θ', compare it with θe. If If the boundary condition is met, then the agent's movement along a specific boundary (with an azimuth angle of θe) is valid and acceptable. Conversely, if the boundary condition is not met, then movement along that specific boundary is invalid and unacceptable. Figure 8 In example a, it is unacceptable when the agent's (planned) movement direction from P1 to P3 is forward, because... and The absolute difference (here, θ, see equation (1)) is much greater than the threshold T. In contrast, Figure 8 b shows an example where the (planned) backward movement direction of the agent from P1 to P3 is acceptable, because and The absolute difference (here, θ - 180, see equation (1)) is less than the threshold T.

[0066] Next, turn Figure 4 Step 46 is described below, which involves designing the low-level kinematic feasible planner contained in the bottom layer 42. The bottom layer 42 employs a two-level hierarchical structure, containing the low-level kinematic feasible planner and the high-level deadlock-aware conflict resolver. The low-level kinematic feasible planner includes an A*-based planner that calculates feasible paths from the oriented starting point to the oriented target for a single kinematically constrained agent, while adhering to constraints inherited from the high-level deadlock-aware conflict resolver, but without providing conflict resolution guarantees. On the other hand, the high-level deadlock-aware conflict resolver employs a conflict resolution algorithm based on the Conflict Search (CBS) framework and integrates deadlock resolution capabilities to efficiently handle multi-agent conflicts. Both the A* algorithm and the CBS algorithm are well-known to those skilled in the art and will not be described further here. Figure 8 This demonstrates an example of a high-level solver and a low-level planner working together: the high-level solver's CBS algorithm constructs a constraint tree and searches for possible conflict-free solutions, which are then passed to the low-level planner for replanning. If the low-level planner cannot find a conflict-free path, the result is returned to the high-level solver, and the solution is deemed invalid. If the low-level planner can find a conflict-free path, the result is returned to the high-level solver, and the solution is deemed valid.

[0067] The low-level planner is built on a hierarchical framework, and its core task is to compute kinematically feasible paths that satisfy strict operational constraints for a single agent. This planner is specifically designed to ensure that kinematically constrained agents (such as forklifts) can navigate from a preset starting vertex (position + attitude) to a target vertex (position + attitude). The generated path must simultaneously satisfy the agent's motion constraints and all conflict avoidance constraints provided by the high-level solver. Compared to standard low-level planners (such as conventional A* planners), this invention achieves a breakthrough by establishing "orientation-aware nodes"—reconstructing the basic search state from simple graph nodes to kinematic state nodes. Specifically, nodes located at the same physical vertex but with different attitudes / movement directions / parent nodes or time steps t are all considered independent states. This ensures accurate tracking of the agent's orientation throughout the path. For example, an orientation-aware node can be represented by {vertex, attitude, movement direction, g(n), h(n), parent node, t}, where g(n) is the cumulative cost from the starting point to the current node n, and h(n) is the estimated cost from node n to the target node. For orientation-aware nodes, the node expansion process is determined by kinematic feasibility. Starting from a given orientation-aware node, the planner only considers transitions through verifiable adjacent vertices connected by edges. This decision must consider both the agent's current pose and the allowed movement directions of the edges (e.g., forward, backward, bidirectional).

[0068] Furthermore, compared to the standard A* algorithm, the ground cost g(n) used for the orientation of the sensing node is a forward-priority ground cost. Specifically, this ground cost g(n) introduces a penalty term when the agent's movement direction is backward. This prompts the planner to generate paths that are more forward-oriented when possible, thereby reducing tire wear and improving the driving stability of kinematically constrained agents such as forklifts. On the other hand, the actual distance heuristic cost h(n) uses a pre-computed lookup table that records the actual shortest path distance (in terms of the number of vertices traversed) for all vertex pairs in the graph, providing a perfect heuristic for the graph structure, making the search efficient and guaranteeing optimality. It should be noted that although the above-mentioned parameter modifications increase the computational complexity compared to the standard A* algorithm, the increase is still within a controllable range. For typical customer map sizes (such as 100m × 100m maps), the computational load is perfectly acceptable under standard server configurations.

[0069] Figure 9This demonstrates how a kinematic feasible planner searches for feasible paths for an agent at an initial vertex. Assume the agent (a forklift) is at the initial vertex S with an attitude θ = 0. To move it to the target vertex P3, there are two paths leading the agent to two different states. These two states are distinguished by the agent's attitude and direction of movement at vertex P3. The first path follows the straight line S-P3, resulting in the agent's state at vertex P3 as: {P3, θ = 0, d =FORWARD, g(n) = 1, h(n) = 1, parent = S, t = 1}. The agent maintains an attitude angle of θ = 0 and a forward direction of movement. The parent node is S, and the required time step is 1 unit. The ground cost g(n) and the actual distance heuristic cost h(n) are both 1. On the other hand, the second path from vertex S to vertex P3 follows two Bézier curves, S-P4 and P4-P3. At the intermediate vertex P4, the movement direction changes from forward to backward, causing a change in attitude. Therefore, in this case, when the agent reaches vertex P3, its state is {P3, θ = 180, d = BACKWARD, g(n) = 3, h(n) = 1, parent = P4, t = 2}. Here, the ground cost g(n) is 3 instead of 2 because, as mentioned earlier, the agent's movement direction changes to backward, resulting in an additional penalty. The parent node before reaching P3 is P4. Since moving along the two Bézier curves requires two time steps, the number of time steps is t = 2.

[0070] Figure 9 This shows that when starting from node S and the agent's pose is θ = 0, only nodes P1, P3, and P4 are reachable. The states of the agent after reaching P1 and P4 are {P1, θ = -90, d = BACKWARD, g(n) = 2, h(n) = 3, parent = S, t = 1} and {P4, θ = 90, d = FORWARD, g(n) = 1, h(n) = 2, parent = S, t = 1}, respectively. However, due to the agent's pose and direction of movement at vertex S, it cannot move from vertex S to vertex P2—this stems from the agent's nonholonomic kinematic constraints.

[0071] See Figure 10 This diagram illustrates the step-by-step execution process of a kinematic feasibility planner through a concrete example. It shows how the planner expands the search range from the initial state (starting vertex and pose) until a target state that satisfies the target vertex and pose is found. Figure 10 The subgraph in the upper right corner shows the agent's initial state at vertex S. The nodes and edges in this graph are connected to... Figure 9similar. Figure 10 The target state of the agent at vertex G is also shown. Specifically, the initial state contains vertex S and pose θ = 0, while the target state contains vertex G and pose θ = 180°. In this example, vertices P1, P2, P3, P4, and G are predefined in the graph, such as an internal plan view of a warehouse environment and known obstacles. Before executing the MAPF method, the user needs to input this graph and its vertices and edges into the solver in the initial stage. As mentioned earlier, the planner maintains orientation awareness by tracking pose / direction / parent node at each node and enforces kinematic constraints in each expansion step. Furthermore, the planner explores feasible paths using a forward-first cost function and systematically explores the state space until the exact target conditions are met.

[0072] like Figure 10 As shown, assume the initial vertex is S with pose θ = 0, and the target node is G. In step 1, node expansion begins with the initial node S, specifically from {S, θ = 0, d = FORWARD, parent = S}. From vertex S, expandable paths include: {P3, θ = 0, d = FORWARD, parent = S}, {P4, θ = 90, d = FORWARD, parent = S}, and {P1, θ = -90, d = BACKWARD, parent = S}. However, node expansion is selected based on priority, i.e., forward movement is preferred over backward movement as described above. Therefore, expansion only proceeds from vertex S to vertices P4 and P3, but not to vertex P1, because the path to P3 requires backward movement. The secondary expansions from vertices P3 and P4 are as follows... Figure 10 Steps 2 and 3 are shown below. In step 2, the path {G, θ = 0, d =FORWARD, parent = P3} extending from P3 can reach the target vertex G. However, at this point, θ = 0, which does not satisfy the target value θ = 180. The other two extension paths from P3 in step 2 (i.e., paths leading to P4 and S) are {P4, θ = -90, d = BACKWARD, parent = P3} and {S, θ = 0, d = BACKWARD, parent = P3}, neither of which can reach the target vertex G. Therefore, when the parent node is S, all paths extended from P3 are infeasible. Therefore, node P3 is skipped, and the search continues.

[0073] Since edge S-P3 was not chosen, edge S-P4 is considered, i.e., the path points to vertex P4 and the source node is S. Now, as shown in step 3, further expansion from P4 either returns to the source node S ({S, θ = 0, d = BACKWARD, parent = P4}); or moves along edge P4-P3 to node P3, the path being {P3, θ = 180, d = BACKWARD, parent = P4}. In this case, expansion along edge P4-P3 is chosen because the cost to reach the target vertex G is lower than that along P4-S. Subsequently, at node P3, a third expansion is considered in step 4, with possible paths including: {G, θ = 180, d = BACKWARD, parent = P3}, {P4, θ = 90, d = FORWARD, parent = P3}, and {S, θ = 180, d = FORWARD, parent = P3}. In step 4, edge P3-G is chosen instead of P3-P4 and P3-S because the latter two cannot reach the target vertex G and do not meet the target pose requirements. Through edge P3-G, the agent can reach the target vertex G with θ = 180°, thus meeting the target requirements. Therefore, the successfully planned path for this agent is along edges S-P4, P4-P3, and P3-G.

[0074] Having described the hierarchical structure and operational steps of the low-level kinematic feasible planner, we now turn to the design of the high-level deadlock-aware conflict resolver, which corresponds to... Figure 4Step 48. The high-level conflict and deadlock resolver works in conjunction with the low-level kinematic feasible planner. Compared to traditional conflict resolvers, the high-level deadlock-aware conflict resolver described here enhances the integrated deadlock resolution mechanism by identifying the SRP strategic location calculated by the low-level planner and safely resolving deadlocks while satisfying kinematic constraints. Specifically, the high-level resolver detects spatial and temporal conflicts (e.g., two agents simultaneously needing the same vertex or exchanging vertices) and identifies these complex deadlocks from the kinematic constraint paths between constraint tree (CT) nodes, rather than simply adding more wait (WAIT) nodes as in traditional techniques. The high-level solver then outputs a high-level objective to the low-level planner, such as "agent a1 must reach the SRP point." The low-level planner receives the high-level objective from the high-level solver and resolves it. Based on the graph structure and an accurate kinematic model of the agent (as mentioned earlier, this model may be provided by the agent manufacturer), the low-level planner calculates a specific feasible safe reference point (SRP). This is not merely a waiting node, but a strategic location with several waiting conditions (such as a nearby avoidance vertex or a passage zone) and includes several waiting operations. After determining the SRP, the low-level planner strictly follows the motion constraints to plan a kinematically feasible path for the agent from the current position to the calculated SRP, and then to the target position. Here, the waiting node refers to a deliberate pause at a specific location, forcing the agent to yield by adjusting the path in a timely manner.

[0075] SRP is a dynamically calculated intermediate point that satisfies higher-level constraints. This point serves as a temporary anchor point, enabling the lower-level motion feasibility planner to decompose the path into two feasible segments—two paths based on the constraints of agents a1 and a2, respectively. Subsequently, the higher-level planner selects the optimal solution based on the cost value, ensuring that the kinematically constrained agent has the correct posture at the anchor point and when reaching the target. Figure 11 The search process of SRP will be explained. First, assume a deadlock conflict exists (a1, a2, P15, t=1), such as... Figure 11 As shown in the top module: At time t=1, both agents a1 and a2 will reach vertex P15 according to their original paths. This deadlock triggers the constraint on agent a1 (a1, P15, t=1), meaning agent a1 cannot reach vertex P15 at time t=1. This constraint also prohibits the generation of simple waiting nodes at the conflict point P15. Similarly, the deadlock triggers the constraint on agent a2 (a2, P15, t=1). Figure 11The lower part illustrates the independent search processes for finding collision-free and deadlock-free solutions for agents a1 and a2, respectively. During the search, the low-level planner backtracks the agent paths to find the nearest vertex that is both an intersection (number of neighbors > 2) and does not conflict with other agent paths. For example, when agent a1 needs to be moved to the SRP (at which point agent a2 does not need to go to any SRP), as follows... Figure 11 As shown in the lower left block, the system selects P8 as the SRP for agent a1 and sets a waiting state at P8. Note... Figure 11 In the diagram, "PK" represents a parking spot, and an asterisk indicates a different location (e.g., the difference between PK2* and PK2). If agent a2 needs to be moved to an SRP (in which case agent a1 does not need to go to any SRP), then... Figure 11 The bottom right block shows: P3 is selected as the SRP for agent a2, and the waiting state of agent a2 is set at P3. The SRP cost of agents a1 and a2 is both 42. The general rule for selecting one of multiple agents to continue is: select the agent with the lowest cost. In this example, since both have the same cost, the selection order is irrelevant; for simplicity, agent a1 is selected here. The number of steps backtracking from the SRP defines the waiting time to reach that point, as shown below. Figure 11 As shown, the waiting time for agents a1 or a2 is 1. When a node is designated as the SRP (Solution Point) for an agent, that agent will move to the SRP, thus breaking the deadlock. The higher-level solver will continue running until all conflicts are resolved. This process corresponds to... Figure 4 Step 50 in the process.

[0076] Figures 12a-12d visually illustrate the narrow-channel deadlock situation caused by the initial predetermined path between two agents a1 and a2, and the process of resolving the deadlock through strategic SRP computation. Figure 12a The initial planned paths s1-g1 and s2-g2 for agents a1 and a2 are shown respectively. Due to the overlap of some nodes on the two paths, a narrow-channel deadlock will occur if the two agents strictly follow the initial paths. This deadlock is detected at time t = 0. Subsequently, a strategic SRP (such as...) is calculated for agent a1 in the backward direction of path s1. Figure 12b As shown). Agent a1 arrives at the SRP at time t = 3 (from which...). Figure 12b It can be seen that SRP is 3 vertices away from vertex s1, thus clearing the obstacle for agent a2 to move along the original path. After agent a2 passes the deadlock position, agent a1 returns to the original path at time t = 5 (e.g., Figure 12c (As shown). Finally, agent a1 arrives at the target node g1 at time t = 16. At this point, the movements of both agents a1 and a2 are complete, as shown. Figure 12d As shown.

[0077] In describing the kinematic constraint planner (i.e. Figure 4 After the bottom layer 42), we now turn to the description of the collision-free controller (i.e. Figure 4 (Top 40 in the text). To address potential collision issues caused by agent movement deviations, dynamic obstacles, and network fluctuations, a dynamic collision-free controller is proposed to manage global traffic resources by assigning "road access rights" to each agent. This mechanism employs a centralized greedy locking strategy, dynamically locking / unlocking nodes based on real-time agent state updates (provided by each agent), thereby ensuring the safety of multi-agent path planning in dense traffic scenarios and improving scheduling efficiency by allowing agents to greedily pre-reserve multiple conflict-free vertices. The centralized greedy locking mechanism follows these rules: 1) At least two available nodes must exist—otherwise, the agent enters a paused state (waiting state); 2) A greedy expansion mechanism exists to lock additional available vertices along the channel; 3) Stop at intersections—the locking operation terminates when it reaches multiple neighboring vertices (nodes directly connected to more than two other vertices in the topology graph), ensuring fairness in road access rights.

[0078] Figure 13a This demonstrates the potential collisions that can occur when a centralized greedy locking mechanism is lacking. Specifically, assume agent a1's initial planned path is ABCDE, and agent a2's initial planned path is FDCBG. Since both paths pass through nodes D, C, and B, the initial plan involves a2 performing a waiting operation at the starting node F (waiting for 3 time steps). This approach would have avoided a collision between a1 and a2. However, in actual movement, suppose a1's turning operation causes its movement speed to be lower than expected, resulting in both agents arriving at node D simultaneously, thus causing a collision.

[0079] In order to solve Figure 13a To address the potential collision issues shown, a road access permission mechanism (such as...) was designed. Figure 13b (As shown). Road access rights represent the farthest vertex successfully locked by the centralized greedy locking mechanism, serving as a communication bridge between the central controller and individual agents. The central controller informs agents of the currently safe boundaries to traverse by sending road access rights. Agents can only move to the farthest vertex with access rights on the planned path. When the first agent is on the planned path, its acquired access rights will prevent other agents from entering that path. If new road access rights are lacking, agents must remain stationary to avoid potentially dangerous areas. This mechanism ensures strict global traffic safety—each node is accessible only to a single agent. Because no two agents will acquire the same farthest vertex, all farthest vertices are non-overlapping.

[0080] like Figure 13bAs shown, the conflict-free controller greedily locks the farthest vertex for agents a1 and a2. For agent a1, nodes A, B, C, D, and E are all locked, and the conflict-free controller grants agent a1 the right-of-way E. For agent a2, only node F is locked (it is the same as agent a2's initial vertex), and the conflict-free controller grants agent a2 the right-of-way F. The reason why node D (as the intersection of the paths of a1 and a2) is locked only by a1 and not by a2 is that in this example, a1 requested node D earlier than a2, so following the first-come, first-served principle, node D is locked to a1. After agents a1 and a2 have successfully locked the farthest vertex, agent a2 cannot obtain two available nodes at the same time (see the aforementioned "minimum two-node rule") and therefore must remain at node F. When agent a2 remains at node F, agent a1, even if it moves slower and arrives at node D later than a2, can still proceed along its path ABCDE—since a2 can only reach node F at most, a collision will not occur. In summary, the conflict-free controller analyzes task and agent information to find a deadlock-free kinematic constraint path that satisfies the precise target posture and dynamically assigns road access permissions to each agent.

[0081] Various method embodiments of the present invention can be implemented through systems implemented in hardware and / or software. For example, Figure 14 A data processing system 100 is illustrated in some embodiments of the present invention. The data processing system 100 can be used to perform the above-described method of integrating kinematic constraint MAPF with collision-free control; more generally, the system can be used to perform or assist in performing one or more method embodiments of the present invention.

[0082] Data processing system 100 typically includes adapter components required to receive, store, and execute corresponding computer instructions, data, commands, and / or code. Its core components are processor 102 and memory (storage unit) 104. Processor 102 may include one or more of the following: CPU, MCU, GPU, logic circuit, Raspberry Pi chip, digital signal processor (DSP), application-specific integrated circuit (ASIC), field-programmable gate array (FPGA), or any other digital or analog circuit / circuit configured to interpret and / or execute program instructions and / or process signals and / or information and / or data. Memory 104 may include one or more volatile memories (such as RAM, DRAM, SRAM, etc.), one or more non-volatile memories (such as ROM, PROM, EPROM, EEPROM, FRAM, MRAM, FLASH, SSD, NAND, NVDIMM, etc.), or any combination thereof. Appropriate computer instructions, commands, codes, information, and / or data may be stored in memory 104. Computer instructions used to perform or facilitate the performance of embodiments of the methods of the present invention may be stored in memory 104. The processor 102 and the memory (storage device) 104 may be integrated or separated (and connected via an operative connection).

[0083] Optionally, the data processing system 100 may also include one or more input devices 106. Examples of such input devices 106 include: keyboards, mice, styluses, image scanners, microphones, haptic / touch input devices (e.g., touchscreens), image / video input devices (e.g., cameras), etc. Input devices 106 may be used to receive user input. Optionally, the data processing system 100 may also include one or more output devices 108. Examples of such output devices 108 include: displays (e.g., monitors, screens, projectors, etc.), speakers, headphones, earphones, printers, additive manufacturing equipment (e.g., 3D printers), etc. Displays may include liquid crystal displays, LED / OLED displays, or other suitable display devices, which may or may not have touch sensing capabilities. Output devices 108 (e.g., displays) may be used to present three-dimensional medical images, raw slide images, reconstructed slide images, residual slide images, etc. The data processing system 100 may also include one or more disk drives 112, which may include one or more of the following: solid-state drives, hard disk drives, optical drives, flash drives, tape drives, etc. The data processing system 100 may be equipped with a suitable operating system, such as one installed on disk drive 112 or memory 104. Memory 104 and disk drive 112 may be controlled and operated by processor 102. The data processing system 100 may optionally be equipped with communication device 110 for use with other computing devices (such as servers, personal computers, terminals, tablets, mobile phones, smartwatches, IoT devices, or other wireless computing devices). Communication device 110 may include one or more of the following components: modem, network interface card (NIC), integrated network interface, NFC transceiver, ZigBee transceiver, Wi-Fi transceiver, Bluetooth® transceiver, RF transceiver, cellular (2G, 3G, 4G, 5G), Transceivers (5G and above, etc.) include optical ports, infrared ports, USB connections, or other wired / wireless communication interfaces. The transceiver can be implemented by a single device or multiple devices (integrated transceivers, standalone transceivers, etc.). The communication link can transmit commands, instructions, information, and / or data via wired or wireless means. In one example, processor 302 and memory 104 (optionally including input device 306, output device 308, communication device 310, and disk drive 312 (if present)) are directly or indirectly interconnected via buses, peripheral component interconnects (PCI, such as PCI Express), universal serial bus (USB), fiber optic bus, or other buses. 312 (if present) is directly or indirectly interconnected via buses, peripheral component interconnects (PCI) (such as PCI Express), universal serial bus (USB), fiber optic bus, or other similar bus structures. In one embodiment, at least some components can be connected wirelessly, for example, via networks such as the Internet or cloud computing networks.

[0084] Those skilled in the art will understand that Figure 14The data processing system 100 shown is merely an example; in other embodiments, the system 100 may have different configurations (e.g., including additional components, reducing the number of components, etc.).

[0085] While not strictly necessary, one or more embodiments described with reference to the accompanying drawings can be used by developers as application programming interfaces (APIs) or libraries, and can also be integrated into other software applications, such as terminal or computer operating systems, and portable computing device operating systems. In one or more embodiments, since program modules contain routines, programs, objects, components, and data files that assist in performing specific functions, those skilled in the art will understand that the functionality of a software application can be distributed across multiple routines, objects, and / or components to achieve the same functionality desired by this specification.

[0086] It should also be understood that when the methods and systems of the present invention are implemented entirely or partially by a computing system, any suitable computing system architecture may be employed. This includes stand-alone computers, network computers, and dedicated or non-dedicated hardware devices. When using the terms "computing system" and "computing device," these terms are intended to include (but are not limited to) any reasonable configuration of computer or information processing hardware capable of performing the described functions.

[0087] Exemplary embodiments have been fully described above. Although the description relates to specific embodiments, those skilled in the art will understand that the invention can be modified in detail without changing the core principles. Therefore, the invention should not be construed as being limited to the embodiments described herein.

[0088] While the embodiments have been described in detail with reference to the accompanying drawings and the foregoing description, it should be understood that they are illustrative rather than restrictive, and the examples shown are merely exemplary embodiments and do not limit the scope of the invention in any way. It should be understood that any feature described herein can be used in combination with any embodiment. Exemplary embodiments are not mutually exclusive, nor do they exclude other embodiments not mentioned herein. Therefore, the present invention also provides embodiments that include one or more combinations of the illustrative embodiments described above. Modifications and variations to the content of the present invention, as long as they do not depart from its spirit and scope, are all within the scope of implementation and should therefore be limited only by the appended claims.

[0089] For example, although the embodiments of the present invention use a forklift as an example of a kinematically constrained agent, those skilled in the art should understand that other types of kinematically constrained agents (such as general-purpose robots) can also be applied to the described kinematically constrained multi-agent path planning method. Regarding the centralized vertex locking method used during multi-agent path planning in the embodiments of the present invention, the agent can be kinematically constrained or unconstrained (e.g., it can freely turn and move to any posture and direction).

[0090] Furthermore, in the kinematic constraint planner described in the exemplary embodiments of the present invention, the two-level planning is implemented using a modified A* algorithm and a conflict basis search (CBS) algorithm, respectively. However, those skilled in the art should understand that the planner is not limited to implementations based on these two specific algorithms. Any suitable algorithm applicable to conflict resolution and path planning can be used to construct a kinematic constraint planner.

Claims

1. A computer-implemented kinematically constrained multi-agent path planning method, comprising: a) Define the starting vertices in the graph for each of the multiple agents, each with a given initial pose; This diagram represents a shared space, and the starting vertex represents the initial position of the corresponding agent in the shared space. b) Provide kinematic models for each of the plurality of agents; the kinematic models include the pose, direction of movement, and motion constraints of the corresponding agents; and c) Find a path for each of the plurality of agents such that the agent moves from an initial position with the first given posture to a target position with the second given posture; The paths of the plurality of agents do not collide with each other; each of the plurality of agents follows one or more edges in the graph, while satisfying the pose and motion constraints of the agent.

2. The method of claim 1, wherein for each of the plurality of agents, its attitude change along the path remains within a turning angle threshold.

3. The method according to claim 1, wherein one or more sides are straight lines or Bézier curves.

4. The method of claim 1, wherein step c) further comprises determining the path based on the total path distance cost, the total cost including a penalty term when the agent's movement direction is not the preferred direction.

5. The method of claim 1, wherein step c) further comprises the following steps for resolving deadlock conflicts: d) Generate high-level constraints based on deadlock for one of the plurality of intelligent agents; e) Search for the nearest vertex for the agent by backtracking the agent's path from its current vertex; the nearest vertex is an intersection point and does not conflict with the paths of other agents among the plurality of agents; f) Designate the nearest vertex as a safe reference point; as well as g) Move the agent to a safe reference point to resolve the deadlock.

6. The method of claim 5, wherein steps d)-e) are performed for the two agents associated with the deadlock; step c) further comprises selecting one agent to continue performing steps f) and g) based on a cost comparison of the two agents.

7. The method of claim 6, wherein the cost of the two agents is calculated based on the number of backtracking steps required for them to return to their respective nearest vertices.

8. The method according to claim 4, wherein the plurality of intelligent agents are unmanned forklifts, and the preferred direction is the forward movement direction of the forklift.

9. The method of claim 1 further includes controlling the plurality of intelligent agents to move autonomously along their respective paths via a server on a communication network.

10. A computer-implemented method for global traffic management in a multi-agent path planning process, comprising: a) For each of the multiple agents whose initial path in the graph would cause a collision, lock the farthest locked vertex on the initial path, as well as all vertices on the initial path before the farthest locked vertex; the graph represents a shared space in which the multiple agents reside. The furthest locked vertices of the plurality of intelligent agents do not overlap with each other; b) Grant each of the plurality of agents access to the planned path from its initial vertex to the furthest locked vertex; This access permission prohibits any other agent among the plurality of agents from entering locked vertices if the agent has not released the locked vertices; c) Control the multiple intelligent agents via a server located on the communication network, enabling them to autonomously reach their respective furthest locked vertices based on the granted access permissions.

11. The method of claim 10, wherein in step c), if the remote locked vertex of one of the plurality of agents is the same as its initial vertex, then the agent waits at the initial vertex.

12. The method of claim 10, wherein step a) further comprises: When one of the multiple agents requests a certain vertex faster than all other agents requesting the same vertex, that vertex is locked as the furthest locked vertex of that agent in the graph.

13. A non-transient computer-readable medium storing instructions executable by at least one processor to cause a computing device to perform the kinematically constrained multi-agent path planning method according to claim 1.

Images