A Path Planning and Tracking Control Algorithm for Multi-Autonomous Mobile Robot Systems Based on Spatiotemporal Conflicts and Nonholonomic Constraints

Abstract

This paper proposes a path planning and tracking control algorithm for multi-autonomous mobile robot (multi-AMR) systems that addresses the challenges posed by AMRs with a kinematic model. The proposed path planning algorithm is divided into two layers. The upper layer of the algorithm detects spatiotemporal conflicts between the paths of any two AMRs using a spatiotemporal conflict detection tree and the Separating Axis Theorem. The lower layer of the algorithm takes into account the kinematic model of the AMRs, ensuring that the generated paths satisfy the nonholonomic constraints. Furthermore, the lower layer introduces weighted adjustments to the heuristic evaluation, significantly improving the efficiency of the planning process. The proposed tracking control algorithm accounts for the kinematic model of AMRs and various constraints, achieving precise path tracking through model predictive control. The simulation results demonstrate that the proposed path planning and tracking control algorithm can efficiently generate conflict-free paths and achieve precise tracking control that satisfies the nonholonomic constraints of multi-AMR systems.

1. Introduction

Ongoing technological advancements have led to a notable shift towards automation within the manufacturing industry [1]. This transition has seen a growing deployment of Autonomous Mobile Robots (AMRs) in industrial production and warehousing logistics systems [2]. The significance of path planning for AMRs cannot be understated, as these paths directly influence the efficiency and safety of the robots’ movements. Consequently, a critical challenge arises: devising effective and feasible paths for each AMR operating within a multi-AMR system that comply with their kinematic models while ensuring conflict avoidance throughout the entire path [3].

In the field of single AMR path planning, researchers have proposed a variety of algorithms, such as the Dijkstra algorithm, Greedy Best-First Search algorithm and A* algorithm, among others [4,5,6,7,8,9]. Multi-AMR systems, which feature several AMRs collaborating within the same scenario, exhibit greater complexity and challenges compared to single AMR systems. Various path planning algorithms have been devised for these systems, encompassing priority-based cooperative algorithms, AI-based algorithms, and conflict-search algorithms. The WHCA* algorithm enhances HCA* by implementing a windowed search strategy, which plans paths near the current place to reduce computational load. It also dynamically adjusts AMRs’ priorities based on window conditions, improving adaptability to environmental changes and interactions among AMRs [10,11,12]. However, these algorithms require maintaining communication among all AMRs for conflict detection. The PBS algorithm stratifies multi-AMR path planning into priority groups, allowing sequential planning where lower-priority AMRs adjust their paths based on those of higher-priority AMRs to prevent collisions [13]. In the realm of AI-based algorithms, the PRIMAL algorithm employs a distributed architecture that merges deep reinforcement learning with imitation learning for multi-AMR path planning [14]. Additionally, PRIMAL2 has the lifelong planning capability [15]. The MADDPG algorithm and the MAPPO algorithm cooperatively trains behaviors of other AMRs in continuous spaces, enabling collaborative decision-making among multi-AMR [16,17]. However, these algorithms require substantial amounts of data and computational resources for training and generally have lower interpretability. In conflict-search-based algorithms, the Conflict-Based Search (CBS) algorithm is a multi-layered strategy for multi-AMR path planning where the lower layer independently plans a path for each AMR, and the upper layer assigns spatiotemporal constraints by detecting conflicts among the lower layer’s path [18,19,20].

The AMR path tracking control method based on kinematic models can accurately and effectively design control strategies that meet the motion constraints of AMRs, improving the performance metrics of the system in completing operational tasks. Currently, various algorithms such as Proportional-Integral-Derivative (PID) control, Fuzzy Logic Control (FLC), and Model Predictive Control (MPC) have been developed. PID is a classic control algorithm that achieves precise control of the system by adjusting three parameters: proportional, integral, and derivative, but their performance is limited when dealing with nonlinear systems and multi-constraint problems [21]. FLC, based on fuzzy set theory, controls the system through fuzzy rules. FLC does not require precise mathematical models, and has good robustness and adaptability, but designing and adjusting fuzzy rules is relatively complex, and achieving precise control can be challenging [22]. MPC, as an advanced control strategy, has gained widespread attention for its significant advantages in handling multi-constraint optimization problems [23]. MPC can optimize the current control input by predicting the system’s behavior over a future time horizon in each control cycle, thus achieving precise control of complex systems. It can handle nonlinear systems and effectively cope with uncertainties and disturbances both inside and outside the system. Additionally, MPC excels in considering system dynamics and input–output constraints, ensuring safety and stability while maximizing system performance.

Addressing the path planning issues in multi-AMR systems, we enhance the CBS algorithm by proposing a multi-AMR path planning algorithm based on spatiotemporal conflicts and nonholonomic constraints. The algorithm employs a conflict detection tree integrated with the Separating Axis Theorem for precise collision detection, and efficiently plans feasible paths based on kinematic modeling for AMRs in a continuous space. Furthermore, we achieve precise tracking control of the planned paths based on the kinematic model of AMRs through the model predictive control algorithm. Our main contributions are summarized as follows:

• This paper develops a conflict detection tree integrated with the Separating Axis Theorem. It enables precise spatiotemporal conflict detection for each AMR’s path. This reduces the need for replanning and improves the efficiency of the algorithm.
• This paper introduces a path planning algorithm that incorporates kinematic models to plan feasible paths for each AMR. In the path planning process, factors required for actual AMR control are considered, addressing the issue of conflicts between AMRs caused by significant discrepancies between planning and control in multi-AMR systems. Moreover, building on this foundation, the algorithm’s heuristic function is weighted to further enhance planning efficiency by biasing the paths towards the goal.
• This paper takes the planning results of multi-AMR systems as the desired paths and, through the model predictive control algorithm, considers the specific kinematic models of the AMRs to achieve precise path tracking control.

2. Preliminaries and Problem Setting

In reality, AMRs need to consider their orientation and kinematic models to complete movements. The differences in orientation and kinematic models of AMRs also result in varying amounts of time and space required during path planning and tracking control. For example, Ackermann steering AMRs cannot turn in place and require additional space to turn. Therefore, the problem we need to solve is to achieve path planning and tracking control for multi-AMRs based on kinematic models.

2.1. Kinematic Model

This paper, we consider the orientation and kinematic model of Ackermann steering AMR to plan path. And, we achieve precise path tracking control based on this model.

The kinematic model of the Ackermann steering AMR as shown in Figure 1. The center of the AMR’s rear axle in the global coordinate system $\left(XOY\right)$ is $(x_b,y_b)$. The yaw angle is $\theta$. The AMR’s steering center is $O^{\prime }$, and the front wheel steering angle is $\Phi$. The linear velocity v is controlled by the AMR’s rear wheels. In discrete time, the kinematic model of the Ackermann steering AMR in the global coordinate system is:

$$\begin{array}{c}\hfill x_{b,t+1}=x_{b,t}+v_{t}T\cos \left({\theta }_t\right)\end{array}$$

(1)

$$\begin{array}{c}\hfill y_{b,t+1}=y_{b,t}+v_{t}T\sin \left({\theta }_t\right)\end{array}$$

(2)

$$\begin{array}{c}\hfill {\theta }_{t+1}={\theta }_t+{\displaystyle \frac{v_{t}T\tan \left({\Phi }_t\right)}{L}}\end{array}$$

(3)

where L is the distance from the AMR’s front axle to its rear axle, and T is the time step. The coordinates of the AMR’s center position $(x,y)$ are:

$$\begin{array}{c}\hfill x_{t+1}=x_{b,t+1}+LT\cos \left({\theta }_{t+1}\right)/2\end{array}$$

(4)

$$\begin{array}{c}\hfill y_{t+1}=y_{b,t+1}+LT\sin \left({\theta }_{t+1}\right)/2\end{array}$$

(5)

Figure 1. The kinematic model of Ackermann steering AMR.

2.2. Problem Setting

In this paper, we define an enclosed warehouse space C, containing static obstacles O. We consider that there are some AMRs within this enclosed area. For ${\mathrm{AMR}}_i$, in the global coordinate system, the center position coordinates are $(x_i,y_i)$, the yaw angle is ${\theta }_i$, the start coordinate is $S_i=(x_{si},y_{si},{\theta }_{si})$, the goal coordinate is $G_i=(x_{gi},y_{gi},{\theta }_{gi})$, and in discrete time, the path is $P_i=[p_i^1,p_i^2,\dots ,p_i^t]$. Let $p_i^t$ represent the position coordinates and yaw angle of ${\mathrm{AMR}}_i$ at time t. $Space\left(p_i^t\right)$ denotes the space occupied by ${\mathrm{AMR}}_i$ at that moment. $Space\left(O\right)$ represents the space occupied by static obstacles. As for the Ackermann steering AMR, define the front wheel steering angle as ${\Phi }_i$, the linear velocity as $v_i$, the change in front wheel steering angle as ${\delta }_i$, and the change in linear velocity as $a_i$.

If a planned path P and a tracking control meet the following conditions, it is considered a feasible path:

• The planned path satisfies the nonholonomic constraints of the AMR;
• The AMR does not collide with any static obstacles while operating along the planned path, i.e., $Space\left(p_i^t\right)\cap Space\left(O\right)=⌀$;
• Any two AMRs operating on their respective planned path will not collide with each other, i.e., $Space\left(p_i^t\right)\cap Space\left(p_j^t\right)=⌀,\forall i\ne j$.
• The control inputs and their rate of change in the path tracking control algorithm meet the constraint conditions.

Considering the reasonableness of the problem, the following assumptions are made:

• The start and goal coordinates of different AMRs vary, and neither the start nor the goal positions collide with any static obstacles, i.e., $S_i\ne G_i,Space\left(S_i\right)\cap Space\left(O\right)=⌀,$ $Space\left(G_i\right)\cap Space\left(O\right)=⌀$;
• The start and goal positions of any two AMRs do not collide, i.e., $Space\left(S_i\right)\cap Space\left(S_j\right)=⌀,$ $Space\left(G_i\right)\cap Space\left(G_j\right)=⌀,\forall i\ne j$;
• The initial position and orientation of the path tracking control algorithm are the same as the desired position and orientation, but the initial speed is zero.

As shown in Figure 2, we improved the CBS algorithm by considering the kinematic model of AMRs, enabling path planning for a multi-AMR system. Additionally, we designed a model predictive control algorithm based on the kinematic model of AMRs to achieve precise tracking control. The goal is to plan conflict-free, feasible paths for all AMRs by considering their nonholonomic constraints and achieve precise tracking control.

Figure 2. A path planning and tracking control algorithm for Multi-AMR Systems based on spatiotemporal conflicts and nonholonomic constraints. The dashed rectangle represents the initial position of the AMR, while the filled rectangle represents the position of the AMR at a certain moment in time.

3. Path Planning Algorithm for Multi-AMR Systems

We enhanced the traditional CBS algorithm for the path planning of multi-AMR systems. At the lower layer, we employ an algorithm that considers spatiotemporal and nonholonomic constraints to plan feasible a path for an individual AMR. At the upper layer, a conflict detection tree integrated with the Separating Axis Theorem is used to precisely detect spatiotemporal conflicts in the planned paths of AMRs. The upper layer detects spatiotemporal conflicts and uses them as constraints for the lower layer algorithm, which is then called again to replan. This process is repeated until there are no conflicts between the paths. At this point, the algorithm successfully plans conflict-free, feasible paths that satisfy the nonholonomic constraints for all AMRs.

3.1. Spatiotemporal Conflict Detection Algorithm

Generally, AMR conflict detection uses a minimum bounding circle. However, as shown in Figure 3, using a minimum bounding circle introduces excessive redundant space around the AMR. This excess can mistakenly identify nonconflicting AMRs as conflicting. Consequently, this increases the frequency of replanning and reduces the algorithm’s efficiency. Oriented Bounding Box (OBB) is a rectangular bounding box that adjusts according to the rectangle’s orientation. Since the actual shape of an AMR closely resembles a rectangle, this paper chooses the OBB to approximate the space occupied by an AMR.

Figure 3. Conflict situations of AMRs under two types of bounding boxes.

This paper employs the Separating Axis Theorem (SAT) for conflict detection with OBB [24]. By checking if the projections of two OBBs on any set of axes perpendicular to their edges overlap, it can be determined whether they are in conflict. Each OBB has vertices defined as $D_1=(-rl,rw)$, $D_2=(rl,rw)$, $D_3=(rl,-rw)$, $D_4=(-rl,-rw)$. The formula for calculating the actual coordinates of each vertex is

$$\begin{array}{c}\left[\begin{array}{c}{X}^{\prime }\\ Y^{\prime }\end{array}\right]=\left[\begin{array}{c}x\\ y\end{array}\right]+\left[\begin{array}{cc}\cos \left(\theta \right)& -\sin \left(\theta \right)\\ \sin \left(\theta \right)& \cos \left(\theta \right)\end{array}\right]\left[\begin{array}{c}X\\ Y\end{array}\right]\hfill \end{array}$$

(6)

where $(x,y)$ represents the center coordinates of the OBB, which is the position of the AMR, and $\theta$ is the rotation angle of the OBB, corresponding to the yaw angle of the AMR. X and Y represent the coordinates of the vertices of the OBB in the local coordinate system. This equation converts the coordinates in the local coordinate system to the global coordinate system.

Then, calculate the normal vector ${\overrightarrow{F}}_{mn}=(F_{mnx},F_{mny})$ from the vector ${\overrightarrow{D}}_{mn}=D_m-D_n$ formed by adjacent vertices $D_m=(D_{mx},D_{my})$ and $D_n=(D_{nx},D_{ny})$, where $mn$ belongs to the set $\{12,23,34,41\}$. The formula is

$$\begin{array}{cc}\hfill & \left[\begin{array}{c}{F}_{mnx}\\ F_{mny}\end{array}\right]={\displaystyle \frac{1}{\parallel D_m-D_n\parallel }}\left[\begin{array}{c}{D}_{ny}-D_{my}\\ D_{mx}-D_{nx}\end{array}\right]\hfill \end{array}$$

(7)

For ${\mathrm{AMR}}_i$ and ${\mathrm{AMR}}_j$, their OBBs are denoted as ${\mathrm{OBB}}_i$ and ${\mathrm{OBB}}_j$, respectively. Therefore, we define the sets ${\overrightarrow{F}}_{imn}$ and ${\overrightarrow{F}}_{jmn}$ to contain all the normal vectors of ${\mathrm{OBB}}_i$ and ${\mathrm{OBB}}_j$, respectively. The vector $\overrightarrow{F}$ is any vector from the sets ${\overrightarrow{F}}_{imn}$ and ${\overrightarrow{F}}_{jmn}$. The result of the projection $Pr$ is

$$\begin{array}{c}\hfill Pr=D_x{F}_x+D_y{F}_y\end{array}$$

(8)

where $D_x$ represents the magnitude of the vector formed by vertex D and the origin in the x-direction, and $D_y$ represents the magnitude of the vector formed by vertex D and the origin in the y-direction. $F_x$ represents the magnitude of the normal vector $\overrightarrow{F}$ in the x direction, and $F_y$ represents the magnitude of the normal vector $\overrightarrow{F}$ in the y direction. Select any one of the normal vector $\overrightarrow{F}$ from the two OBBs that require conflict detection. Project the vectors formed by all vertices D of ${\mathrm{OBB}}_i$ and the origin, and the vectors formed by all vertices D of ${\mathrm{OBB}}_j$ and the origin onto this normal vector $\overrightarrow{F}$, and take the minimum and maximum values of the projection positions to obtain the projection intervals $Int{v}_i$ and $Int{v}_j$. If there exists such a normal vector $\overrightarrow{F}$, where $Int{v}_i\cap Int{v}_j=⌀$, it indicates that the two OBBs are not in conflict. Otherwise, it indicates that the two OBBs are in conflict.

3.2. Path Planning Algorithm Based on AMR Kinematic Model

In this paper, we consider all the AMRs are the Ackermann steering AMRs. According to the kinematic model described in Figure 1, the motion modes of the Ackermann steering AMR studied in this paper are primarily categorized into seven types, as shown in Figure 4.

Figure 4. The motion modes of the Ackermann steering AMR.

The Hybrid A* algorithm can plan paths based on the positions and orientations of points A and B, and ensures that the planned path adheres to the nonholonomic constraints of the AMR, as shown in Figure 5. Therefore, this paper modifies the Hybrid A* algorithm to plan feasible paths for Ackermann steering AMRs.

Figure 5. The comparison of path planning between Hybrid A* algorithm based on kinematic models and traditional A* algorithm. A is the starting point of the path planning, and B is the endpoint of the path planning.

In this paper, we address the multi-AMR path planning problem by considering existing spatiotemporal constraints $D_o$, extracted from the upper layer conflict detection tree, during AMR path planning. Consequently, by considering both spatial and temporal dimensions, the Hybrid A* algorithm is modified into a Spatiotemporal Hybrid A* algorithm. The nodes store information which includes $(p,cost,t)$, where p represents the position and yaw angle of the AMR in global coordinates, $cost$ is the cost, and t is the time. In the Spatiotemporal Hybrid A* algorithm, the AMR extends from a node at time $t-1$ to a node at time t based on nonholonomic constraints, and performs interpolation between these two nodes for conflict detection. This node expansion is feasible, provided that the AMR’s path between the two nodes avoids conflicts with any static obstacles and sidesteps any existing spatiotemporal constraints $D_o$ extracted from the upper layer conflict detection tree. The constraint $D_o$ consists of multiple nodes, each in the form of $(t_d,x_d,y_d,ya{w}_d,hh,hw)$, where $t_d$ represents the start time of the constraint, which lasts for 0.1 s. $x_d$, $y_d$, and $ya{w}_d$ represent the central position and orientation angle of the constraint, while $hh$ and $hw$ represent the length and width of the constraint, respectively.

Additionally, by incorporating a temporal dimension, the Spatiotemporal Hybrid A* algorithm produces more expanded nodes and has a slower planning speed. Therefore, we increase the weight of heuristic estimated costs, the algorithm is more oriented towards searching in the direction of the goal point, thus reducing the search space and accelerating the search speed. Based on this, the Spatiotemporal Hybrid A* algorithm is modified into a Weighted Spatiotemporal Hybrid A* algorithm. In this paper, the pseudocode for the path planning algorithm based on AMR kinematic model we propose is shown in Algorithm 1.

Algorithm 1 Path planning algorithm based on AMR kinematic model

1: AMR type ← kinematic model  2: p ← x,y,$\theta$  3: Root.state ← p, angle  4: Root.time ← 0  5: Root.cost ← g(Root)+ $\lambda$h(Root)  6: Open ← {Root}  7: while Open $\ne ⌀$ do  8:    N $\leftarrow {\min }_{cost}{\mathrm{N}}^{\prime },\forall {\mathrm{N}}^{\prime }\in \mathrm{Open}$  9:    if N.state near goal then 10:       ${\mathrm{N}}_{goal},{\mathrm{path}}_{goal}\leftarrow$ find path using the reeds shepp algorithm 11:       if ${\mathrm{path}}_{goal}$ is no conflict then 12:         solution path ← backtraching search from ${\mathrm{N}}_{goal}$ 13:         return solution path 14:       end if 15:  end if 16:  State set ← extending nodes from N according to kinematic model 17:  for state in State set do 18:      ${\mathrm{N}}^{\prime }\leftarrow$ new node 19:      ${\mathrm{N}}^{\prime }$.state ← state 20:      ${\mathrm{N}}^{\prime }$.time ← N.time+1 21:      if from N to ${\mathrm{N}}^{\prime }$ is no conflict then 22:         Update cost(${\mathrm{N}}^{\prime }$) 23:         if ${\mathrm{N}}^{\prime }$ not in Open then 24:           Open ← Open ∪$\left\{{\mathrm{N}}^{\prime }\right\}$ 25:         else if ${\mathrm{N}}^{\prime }$.cost<(Open $\cap {\mathrm{N}}^{\prime }$).cost then 26:           Update ${\mathrm{N}}^{\prime }$.cost 27:         end if 28:      end if 29:    end for 30: end while

The calculation formula for the cost function $cost\left(n\right)$ at node n is:

$$\begin{array}{c}\hfill cost\left(n\right)=g\left(n\right)+\lambda h\left(n\right)\end{array}$$

(9)

where $h\left(n\right)$ is the heuristic cost at the node, $\lambda$ is a constant weighting factor for the heuristic cost, and $g\left(n\right)$ is the actual cost at the current node.

To align the planned paths with practical scenario requirements, we change the components of the cost function. The node information is $(p,cost,t,\Phi )$, where $\Phi$ is the front wheel steering angle of the AMR. The calculation formula for $g\left(n\right)$ is

$$\begin{array}{c}\hfill g\left(n\right)=\alpha g_1+\beta g_2+\gamma g_3+g_4\end{array}$$

(10)

$$\begin{array}{c}\hfill g_1=T\end{array}$$

(11)

$$\begin{array}{c}\hfill g_2=T·\Phi \end{array}$$

(12)

$$\begin{array}{c}\hfill g_3=T·C_{\Phi }\end{array}$$

(13)

where $g_1$ represents the cost of each time step T. $g_2$ represents the cost of the front wheel steering angle $\Phi$, where a larger steering angle results in a higher cost. $g_3$ represents the cost resulting from changes in the front wheel steering angle, where a greater difference $C_{\Phi }$ between the steering angle of the current step and the previous step leads to a higher cost. $g\left(n\right)$ represents the actual cost of the current node. To ensure that the AMR moves in a straight line as much as possible, reducing the number of turns, changes in direction, and reversals, weights are applied to $g_1$, $g_2$, and $g_3$, increasing the costs associated with turning, changing direction, and reversing. $\alpha$ is the weight for reversing, $\beta$ is the weight for turning, and $\gamma$ is the weight for changing direction. The values of $\alpha$, $\beta$, and $\gamma$ are all greater than 1. $g_4$ represents the cumulative cost generated by the historical actions prior to the current step.

4. Path Tracking Control Algorithm for Multi-AMR Based on Model Predictive Control

In this section, the path planned by the algorithm in the previous section is used as the desired path. By considering the kinematic model of Ackermann steering AMR, the tracking control is implemented based on the MPC algorithm. This ensures that, in a multi-AMR scenario, each AMR can accurately follow the path to complete its task.

For Ackermann steering AMR, for its kinematic models, the nonlinear equation is linearized using a first-order Taylor expansion at point $p_r=(x_r,y_r,{\theta }_r)$, $▵x=x-x_r$, $▵y=y-y_r$, $▵\theta =\theta -{\theta }_r$, $▵v=v-v_r$ and $▵\Phi =\Phi -{\Phi }_r$. The linearized formula is

$$\begin{array}{c}\hfill \left[\begin{array}{c}\dot{▵x_t}\\ \dot{▵y_t}\\ \dot{▵{\theta }_t}\end{array}\right]=\left[\begin{array}{ccc}0& 0& -v_{r,t}sin\left({\theta }_{r,t}\right)\\ 0& 0& v_{r,t}cos\left({\theta }_{r,t}\right)\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}▵x_t\\ ▵y_t\\ ▵{\theta }_t\end{array}\right]+\left[\begin{array}{cc}\cos \left({\theta }_{r,t}\right)& 0\\ \sin \left({\theta }_{r,t}\right)& 0\\ {\displaystyle \frac{\tan \left({\Phi }_{r,t}\right)}{L}}& {\displaystyle \frac{v_{r,t}}{L{\cos }^2\left({\Phi }_{r,t}\right)}}\end{array}\right]\left[\begin{array}{c}▵v_t\\ ▵{\Phi }_t\end{array}\right]\end{array}$$

(14)

After discretization using the forward Euler method, the formula is

$$\begin{array}{c}\hfill \left[\begin{array}{c}▵x_{t+1}\\ ▵y_{t+1}\\ ▵{\theta }_{t+1}\end{array}\right]=\left[\begin{array}{ccc}1& 0& -T{v}_{r,t}sin\left({\theta }_{r,t}\right)\\ 0& 1& T{v}_{r,t}cos\left({\theta }_{r,t}\right)\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}▵x_t\\ ▵y_t\\ ▵{\theta }_t\end{array}\right]+\left[\begin{array}{cc}T\cos \left({\theta }_{r,t}\right)& 0\\ T\sin \left({\theta }_{r,t}\right)& 0\\ {\displaystyle \frac{T\tan \left({\Phi }_{r,t}\right)}{L}}& {\displaystyle \frac{T{v}_{r,t}}{L{\cos }^2\left({\Phi }_{r,t}\right)}}\end{array}\right]\left[\begin{array}{c}▵v_t\\ ▵{\Phi }_t\end{array}\right]\end{array}$$

(15)

At this point, the simplified form of the above formula is

$$\begin{array}{c}\hfill {\tilde{\mathbf{X}}}_{t+1}=\mathbf{A}{\tilde{\mathbf{X}}}_t+\mathbf{C}{\tilde{\mathbf{U}}}_t\end{array}$$

(16)

where state vector ${\tilde{\mathbf{X}}}_{t+1}=\left[\begin{array}{c}▵x_{t+1}\\ ▵y_{t+1}\\ ▵{\theta }_{t+1}\end{array}\right]$, state matrix $\mathbf{A}=\left[\begin{array}{ccc}1& 0& -T{v}_r\sin \left({\theta }_r\right)\\ 0& 1& T{v}_r\cos \left({\theta }_r\right)\\ 0& 0& 1\end{array}\right]$, input matrix $\mathbf{C}=\left[\begin{array}{cc}T\cos \left({\theta }_r\right)& 0\\ T\sin \left({\theta }_r\right)& 0\\ {\displaystyle \frac{T\tan \left({\Phi }_r\right)}{L}}& {\displaystyle \frac{T{v}_r}{L{\cos }^2\left({\Phi }_r\right)}}\end{array}\right]$, input vector ${\tilde{\mathbf{U}}}_t=\left[\begin{array}{c}▵v_t\\ ▵{\Phi }_t\end{array}\right]$.

We have simplified the linearized discrete formulas of the Ackermann steering AMR. Then, we will design the predictive formulas.

We define the change in the input vector as $▵{\mathbf{U}}_t={\tilde{\mathbf{U}}}_t-{\tilde{\mathbf{U}}}_{t-1}$, and combining the state vector and control vector results in ${\mathbf{\xi }}_t=\left[\begin{array}{c}{\tilde{\mathbf{X}}}_t\\ {\tilde{\mathbf{U}}}_{t-1}\end{array}\right]$. And, we merge the state matrix and input matrix to construct a new state matrix $\tilde{\mathbf{A}}=\left[\begin{array}{cc}\mathbf{A}& \mathbf{C}\\ {\mathbf{O}}_{m\times n}& {\mathbf{I}}_m\end{array}\right]$ and a new input matrix $\tilde{\mathbf{C}}=\left[\begin{array}{c}\mathbf{C}\\ {\mathbf{I}}_m\end{array}\right]$, where ${\mathbf{O}}_{m\times n}$ is the zero matrix, ${\mathbf{I}}_m$ is the identity matrix, m is the length of the control vector, and n is the length of the state vector. Then, we obtain the predictive formula and decompose the state vector and control vector,

$$\begin{array}{c}\hfill {\mathbf{\xi }}_{t+1}=\tilde{\mathbf{A}}{\mathbf{\xi }}_t+\tilde{\mathbf{C}}▵{\mathbf{U}}_t\end{array}$$

(17)

$$\begin{array}{c}\hfill {\mathbf{\tau }}_{t+1}=\tilde{\mathbf{D}}{\mathbf{\xi }}_{t+1}\end{array}$$

(18)

where $\tilde{\mathbf{D}}=\left[\begin{array}{cc}{\mathbf{I}}_n& 0\end{array}\right]$.

Finally, we construct the cost function and the constraints. For Ackermann steering AMR, the formula is

$$\begin{array}{cc}\hfill & J=\sum _{t=0}^N\left(\parallel {\mathbf{\tau }}_{t+1}{\parallel }_Q^2+{\parallel ▵{\mathbf{U}}_t\parallel }_R^2\right)\hfill \\ \hfill \mathrm{s}.\mathrm{t}.& {\Phi }_{min}\le {\Phi }_t\le {\Phi }_{max},\hfill & \hfill v_{min}\le v_t\le v_{max}\\ \hfill & {\delta }_{min}\le {\delta }_t\le {\delta }_{max},\hfill & \hfill a_{min}\le a_t\le a_{max}\end{array}$$

(19)

In the cost function of the algorithm, $\mathbf{\tau }$ represents the state error, $\mathbf{U}$ denotes the change in control input, Q is the weighting matrix for the state error, and R is the weighting matrix for the change in control input. N represents the prediction horizon, and ${\Phi }_t$, $v_t$, ${\delta }_t$, and $a_t$ represent the front wheel steering angle, velocity, front wheel steering angle change, and acceleration, respectively. Each of these variables is subject to upper and lower bound constraints. By minimizing the cost function within the existing constraints and over a finite prediction horizon, a balance between the state error and control input is achieved.

By solving the cost function, the optimized control input is obtained, which then controls the AMR’s movement. This optimization process is repeated based on the latest state, forming a closed-loop control to achieve the desired path tracking.

5. Simulation

In this paper, we use Python for simulation experiments. To verify the effectiveness of the algorithm, we conducts tests in two different scenarios: a without obstacle scenario and a random obstacle scenario, with the number of AMRs in each scenario being 5, 10, and 15, respectively. And the sizes of the scenario maps are 50 × 50, 80 × 80, and 100 × 100 m, respectively. In the random obstacle scenario, the number of obstacles are 10, 20, and 40, respectively.

We use the Priority-Based Search algorithm (PBS), the Spatiotemporal Hybrid A* Conflict-Based Search algorithm (STH-CBS), and the Weighted Spatiotemporal Hybrid A* Conflict-Based Search algorithm (WSTH-CBS) to solve the multi-AMR path planning problem. Further, we define the distance between the start and goal points of each AMR is not less than half of the map’s length. To test the improvement in planning efficiency of the algorithm proposed in this paper compared to the PBS algorithm, we conduct five tests for each type of scenario map. The average of these test results represents the actual performance of the algorithm, as shown in Table 1.

Table 1. The comparison of planning times for different algorithms in Multi-AMR systems.

Scenario Type	Map Size	Algorithm	Time (s)
		PBS	4.814
	50 × 50	STH-CBS	2.730
		WSTH-CBS	2.451
		PBS	21.675
Without obstacle	80 × 80	STH-CBS	20.655
		WSTH-CBS	18.972
		PBS	110.666
	100 × 100	STH-CBS	55.915
		WSTH-CBS	52.088
		PBS	5.393
	50 × 50	STH-CBS	4.571
		WSTH-CBS	4.340
		PBS	56.487
Random obstacle	80 × 80	STH-CBS	31.448
		WSTH-CBS	22.964
		PBS	232.450
	100 × 100	STH-CBS	82.548
		WSTH-CBS	62.908

According to test results, the STH-CBS and WSTH-CBS algorithms, which are improvements based on CBS, have higher planning efficiency than the Priority-Based Search (PBS) algorithm. Additionally, as the number of AMRs planned increases and the scenarios become more complex, the PBS algorithm faces an overload of spatiotemporal constraints for later planned AMRs, leading to fewer feasible paths and thus lower planning efficiency compared to the other two algorithms.

Figure 6 shows the paths of AMRs planned using the WSTH-CBS algorithm in a 100 × 100 random obstacle scenario. Figure 7 captures the positions at different moments from Figure 6, where Figure 7a shows the positions and paths during the AMRs’ operation, and Figure 7b shows the final positions and motion paths of the AMRs. The black areas represent the static obstacles, and the rectangles represent the Ackermann steering AMRs. Dashed rectangles and squares represent the starting positions of the AMRs, and filled rectangles and squares of the same color represent the current positions of the AMRs. The dashed line between the starting and current positions represent the motion paths of the AMRs.

Figure 6. Spatiotemporal paths of AMRs in a 100 × 100 random obstacle scenario.

Figure 7. Positions of AMRs at different times in a 100 × 100 random obstacle scenario. The dashed rectangle represents the initial position of the AMR, while the filled rectangle represents the position of the AMR at a certain moment in time.

Based on the paths of all AMRs obtained in Figure 7b, we perform tracking control using the MPC algorithm. In Figure 7b, AMR4 and AMR13 are selected as the example. Figure 8 shows the tracking results achieved through MPC control. Specifically, Figure 8a presents the tracking result for AMR4, and Figure 8b presents the result for AMR13. The MPC algorithm parameters are shown in Table 2. The blue curve represents the desired path obtained from the path planning algorithm, while the red curve represents the actual path achieved through MPC control. It can be observed that the red curve almost completely overlaps with the blue curve, demonstrating the effectiveness of the proposed planning and control algorithms.

Figure 8. The path tracking control results based on the model predictive algorithm.

Table 2. MPC algorithm parameter.

Parameter	Value
Prediction horizon	30
Front wheel steering angle constraint (rad)	$-0.6\le \Phi \le 0.6$
Velocity constraint (m/s)	$-1\le v\le 1$
Front wheel steering angle change constraint (rad/s)	$0.6\le \delta \le 0.6$
Acceleration constraint (m/s2)	$-2\le a\le 2$

To evaluate the performance of the STH-CBS and WSTH-CBS algorithms, further simulation experiments are conducted in the warehouse scenario. The sizes of the warehouse scenario are 50 × 50 and 100 × 100. In this paper, we gradually increase the number of AMRs and compare the performance of the algorithms in terms of planning time and average path length. The planning time is the total time required for all AMRs to find feasible paths, where the less time reflects the higher algorithm efficiency. The average path length is the mean value of the path lengths traveled by each AMR, where a smaller average reflects a better overall planning solution. The comparison results of the algorithms are illustrated in Figure 9, where the horizontal axis represents the number of AMRs, the left vertical axis represents the planning time of the algorithms, and the right vertical axis represents the average path length planned by the algorithms. The orange and blue lines represent the planning times of the STH-CBS and WSTH-CBS algorithms, respectively. The green and red lines, respectively, represent the average path lengths planned by the STH-CBS and WSTH-CBS algorithms.

Figure 9. The comparison of the planning times and the average path lengths for different algorithms in warehouse scenarios.

In the 50 × 50 warehouse scenario, the number of AMRs starts at two and increases by one each time, until it reaches seven, as shown in Figure 9a. In six simulation experiments, the average planning time for the STH-CBS algorithm is 32.29 s, and the average path length is 44.67 m; for the WSTH-CBS algorithm, the average planning time is 7.28 s, and the average path length is 47.13 m. Compared to the STH-CBS algorithm, the WSTH-CBS algorithm experiences a 5.51% increase in average path length, but the average planning time decreases by 77.45%.

In the 100 × 100 warehouse scenario, the number of AMRs starts at five and increases by two each time, until it reaches fifteen, as shown in Figure 9b. In six simulation experiments, the average planning time for the STH-CBS algorithm is 78.34 s, and the average path length is 95.28 m; for the WSTH-CBS algorithm, the average planning time is 22.95 s, and the average path length is 97.73 m. Compared to the STH-CBS algorithm, the WSTH-CBS algorithm experiences a 2.57% increase in average path length, but the average planning time decreases by 70.7%.

Based on the results of simulation experiments conducted on different sizes of warehouse scenario maps, the average path length planned by the WSTH-CBS algorithm is slightly longer than that of the STH-CBS algorithm. However, the WSTH-CBS algorithm shows a significant improvement in path planning efficiency.

To test the performance of the proposed algorithm in a warehouse environment, additional simulations were conducted on a 50 × 50 m scenario map with seven AMRs. An additional path planning algorithm, combining Bezier curves and the CBS algorithm (B-CBS), was introduced as a comparison method. This algorithm first uses the CBS algorithm to plan paths for each AMR on a discrete grid map, then smooths the paths using Bezier curves, and finally controls the AMRs to move simultaneously along the smoothed paths. The path planning success rate of this algorithm was then compared with that of the proposed algorithm. The path planning success rate is defined as the probability that all AMRs successfully avoid each other from the start to the goal. A higher success rate indicates better algorithm performance.

As shown by the data in Table 3, the B-CBS algorithm requires curve smoothing to meet control requirements, which results in significant deviations between the planned path and the tracking control trajectory, thereby reducing the success rate. In contrast, the proposed algorithm, which is based on the kinematic model of the AMR, enables precise tracking control, achieving a consistent success rate of 100%, thus demonstrating the effectiveness of the proposed algorithm.

Table 3. Comparison of success rates in a 50 × 50 warehouse environment.

Test Number	Algorithm	Success Rate
	B-CBS	71.43%
Test 1	STH-CBS	100%
	WSTH-CBS	100%
	B-CBS	85.71%
Test 2	STH-CBS	100%
	WSTH-CBS	100%
	B-CBS	71.43%
Test 3	STH-CBS	100%
	WSTH-CBS	100%

Figure 10 shows the paths of AMRs planned using the WSTH-CBS algorithm in a 50 × 50 warehouse scenario. Figure 11 captures the positions at different moments from Figure 10, where Figure 11a shows the positions and paths during the AMRs’ operation, and Figure 11b shows the final positions and motion paths of the AMRs. The rectangles represent the Ackermann steering AMRs. As shown in Figure 11a, the blue AMR changes its direction by turning to the left side to avoid a conflict with the orange AMR. At the same times, the orange AMR changes its direction by turning to the right side to avoid a conflict with the blue AMR.

Figure 10. Spatiotemporal paths of AMRs in a 50 × 50 warehouse scenario.

Figure 11. Positions of AMRs at different times in a 50 × 50 warehouse scenario. The dashed rectangle represents the initial position of the AMR, while the filled rectangle represents the position of the AMR at a certain moment in time.

Based on the paths of all AMRs obtained in Figure 11b, we perform path tracking control using the MPC algorithm. For the Ackermann steering AMRs, we designed an MPC algorithm based on their models to perform path tracking control. Figure 12 shows the results of the path tracking. The blue curve represents the desired path obtained from the path planning algorithm, while the red curve represents the actual path achieved through the MPC algorithm. The black boxes highlight the paths of some AMRs during turning. By examining the magnified view of these sections, it can be observed that the red curve almost completely overlaps with the blue curve, with only minor control deviations during some turns.

Figure 12. Path tracking control results of AMRs in a 50 × 50 warehouse scenario.

The reason for the control deviations during turning is that, while controlling through the MPC algorithm, we considered the constraints on the upper and lower limits of the front wheel steering angle for the Ackermann steering AMR. Despite these constraints, the deviations between the actual path and the desired path are minimal. This demonstrates the effectiveness of the proposed planning and control algorithms in multi-AMR systems.

6. Conclusions

In this paper, we presented a novel path planning and tracking control algorithm for multi-AMR systems, specifically addressing the complexities introduced by kinematic models among AMRs. Our approach featured a two-layer path planning algorithm. The upper layer effectively identified spatiotemporal conflicts between the planned paths of any two AMRs using a spatiotemporal conflict detection tree combined with the Separating Axis Theorem. The lower layer of the algorithm takes into account the kinematic models of the AMRs, ensuring that the generated paths satisfy the nonholonomic constraints of each robot type. Furthermore, this layer enhanced heuristic evaluations with weighted adjustments, markedly improving planning efficiency. In addition, we developed a tracking control algorithm utilizing MPC. This algorithm precisely managed path tracking by considering the kinematic models and multiple constraints of the AMRs. Simulation results confirmed that our proposed approach successfully generated conflict-free paths while maintaining precise tracking control, fully adhering to the nonholonomic constraints of multi-AMR systems.

Looking ahead, the algorithm proposed in this paper can be further extended to differential drive and omnidirectional AMRs by adjusting the kinematic model. For differential drive AMRs, the primary difference in the kinematic model lies in its use of wheel speed differences to achieve turning. Therefore, the kinematic equations in the current path planning algorithm need to be modified by replacing the front wheel steering angle and linear velocity control of the Ackermann AMR with the speed control of the left and right wheels of the differential drive AMR, to accurately describe the motion characteristics of the differential drive. For omnidirectional AMRs, since they have the ability to move freely in any direction, their kinematic model is relatively simple, and does not require complex steering control. It is even possible to directly control the translational velocities in the x and y axes independently from the angular velocity, further optimizing the efficiency and flexibility of path generation. With these adjustments, the proposed algorithm can adapt to the kinematic models of different types of AMRs, expanding its range of applications and providing efficient path planning and control solutions for multi-robot systems in various real-world scenarios.