Joint Dispatching and Cooperative Trajectory Planning for Multiple Autonomous Forklifts in a Warehouse: A Search-and-Learning-Based Approach

Abstract

Dispatching and cooperative trajectory planning for multiple autonomous forklifts in a warehouse is a widely applied research topic. The conventional methods in this domain regard dispatching and planning as isolated procedures, which render the overall motion quality of the forklift team imperfect. The dispatching and planning problems should be considered simultaneously to achieve optimal cooperative trajectories. However, this approach renders a large-scale nonconvex problem, which is extremely difficult to solve in real time. A joint dispatching and planning method is proposed to balance solution quality and speed. The proposed method is characterized by its fast runtime, light computational burden, and high solution quality. In particular, the candidate goals of each forklift are enumerated. Each candidate dispatch solution is measured after concrete trajectories are generated via an improved hybrid A* search algorithm, which is incorporated with an artificial neural network to improve the cost evaluation process. The proposed joint dispatching and planning method is computationally cheap, kinematically feasible, avoids collisions with obstacles/forklifts, and finds the global optimum quickly. The presented motion planning strategy demonstrates that the integration of a neural network with the dispatching approach leads to a warehouse filling/emptying mission completion time that is 2% shorter than the most efficient strategy lacking machine-learning integration. Notably, the mission completion times across these strategies vary by approximately 15%.

1. Introduction

The increasing demands in the logistics industry all over the world have driven researchers and engineers to focus on developing intelligent transportation systems aimed at enhancing logistics efficiency [1,2]. One prominent application in this domain is unmanned warehouse systems [3]. As a typical component in an unmanned warehouse, an autonomous forklift transports parcels more efficiently than one driven manually because the former does not induce objective mistakes such as fatigue, anxiety, impatience, or anger [4]. Multiple autonomous forklifts should work together when the delivery burden is heavy [5]. Deploying multiple autonomous forklifts enhances delivery efficiency if the inter-forklift cooperation potential can be maximized. The typical modules that influence delivery efficiency include delivery task dispatch [6], cooperative trajectory planning [7,8], and control [9]. This paper is focused on the dispatching and cooperative trajectory planning schemes.

1.1. Related Work

1.1.1. Dispatching Methods for Multiple Forklifts

A complete multi-forklift delivery planning system consists of two functions: delivery task dispatch and cooperative trajectory planning. Before cooperative trajectory planning, the goal point of each forklift is assigned in the dispatching phase [10]. Weidinger et al. [6] proposed a metaheuristic-based method in which assignment candidates are pruned a priori to facilitate the solution process [11]. A similar idea was proposed by Zhang et al. [12] to dispatch multiple automated guided vehicles (AGVs) in a matrix manufacturing workshop. However, both methods run slowly; thus, they cannot meet the real-time computation demand in a warehouse [6,12]. Lin et al. built a multi-AGV dispatching system via network structure together with simplex decision variables; in this system, an evolutionary algorithm minimizes the completion time of all AGVs in a formulated network optimization problem. However, References [6,12,13] shared a common limitation of assuming a uniform speed for AGVs. Moreover, the inter-vehicle collision avoidance problem is reduced to an oversimplified constraint, ensuring solely nonoverlapping time intervals per stopover.

Furthermore, recent studies have focused on the task integration of one AGV instead of considering how multiple AGVs cooperatively operate within the confined area. Bao et al. [14] proposed a heuristic method based on an auction strategy for a multi-AGV task dispatch scheme considering complex factors (such as pod repositioning). The concerned dispatch scheme is inherently an optimization problem with complex cost terms and constraints facilitated by the proposed auction strategy. Lee et al. [15] proposed a two-stage dispatching method. In particular, the first stage deals coarsely with the delivery efficiency and delivery flow balance by solving a bi-objective optimization problem. The result indicates how the parcels to be picked can be clustered. At stage 2, vehicles are dispatched to complete the clustered missions. Dividing the original scheme into two stages largely reduces the number of dispatch candidates without losing the optimum. Machine-learning-based dispatching methods have also been proposed [16]. The formulated reward functions efficiently simplify the dispatch scheme, particularly when complex factors are considered [17,18]. However, few vehicle kinematics is considered in [14,15], and the dispatching phase is fully separated from the trajectory planning strategy. This approach results in difficulties in maximizing overall delivery efficiency.

1.1.2. Cooperative Trajectory Planning Methods for Multiple Forklifts

Cooperative trajectory planning follows the aforementioned dispatching phase. The prevalent cooperative trajectory planners are based on model predictive control [19], which is highlighted by its fast feature while strictly satisfying safety-related constraints. The artificial potential field method is similarly widely applied in trajectory planning, but it may encounter difficulty finding paths through narrow passages [20].

Ma et al. [21] converted constrained time-varying nonlinear programming problems to general unconstrained optimization problems by properly designing a penalty function. Thereafter, a particle swarm optimization method was employed to plan the motion of multiple robots sequentially in a double warehouse with two elevators. However, the optimization phase, along with other methods based on optimization, can significantly increase the computational burden [22,23,24], which can be reduced by forming model-based paths because warehouses are generally structured.

Yang et al. [1] proposed a strategy in which a time-varying dynamic evaluation function is formed based on a network congestion diffusion model to quantify the degree of road congestion. Hereafter, an improved A* search algorithm and a time window algorithm were combined as a hierarchical planning method to search the idle path and avoid collisions. A path planning framework was designed by Zhou et al. [25] to simultaneously reduce the cost of operation and the path for AGVs in airport parcel loading scenarios. An ant colony optimization method was used to optimize the parcel pickup sequencing by ignoring other moving vehicles, while Dijkstra’s algorithm was employed to determine the shortest route of each AGV. Zacharia et al. developed a joint routing and motion planning method for AGVs that addresses uncertainties in demands and travel times. Their approach combines a scheduler for updating destination resources during navigation and integrates a fuzzy-based genetic algorithm with A* search to handle capacity and distance variations [26]. Nonetheless, the vehicle kinematics considered in [1,25,26] remain oversimplified for industrial use even in the trajectory planning phase.

In other studies, the paths of vehicles were assumed to be predefined, and only the longitude trajectories were investigated. Kneissl et al. [27] formulated a method in which potential collision zones are continuously detected. Moreover, the right-of-way is granted to the first arriving vehicle while all the other vehicles involved stop and wait. Dresner et al. proposed confronting the analogous problem of conflict zones with a reservation-based system; in this system, vehicles request and receive time slots from the intersection while they pass [28]. Similarly, a discrete-event logic, which is comparable with a conventional right-handed bidirectional traffic system, was designed by Guney et al. [29] to handle the priorities of the AGVs in a warehouse dynamically. Thus, the need for computationally demanding heuristic searches is eliminated to ease strategy implementation in real-life industrial applications. Furthermore, Digani et al. [30] proposed an obstacle-free path generation method to deal with local deviations from the predetermined path. In the proposed method, new paths are generated via polar spline curves. However, the aisles in a warehouse cannot be fully exploited when certain traffic laws in [27,28,29,30] are strictly enforced.

1.1.3. Joint Dispatching and Planning Methods for Multiple Forklifts

Most of the existing dispatching studies, e.g., [6,10,11,12,13,14,15,16,17,18], cannot accurately evaluate the candidate choices, possibly preventing the downstream planning module from achieving global optimality [1,21,22,23,24,25,26,27,28,29,30]. Thus, combining the dispatching and planning phases is naturally considered. The multi-agent path finding (MAPF) problem in its classical form is an effective approach for simplifying complex warehouse scenarios and facilitating cooperative solutions for dispatching and planning. In the MAPF problem, time is discretized into steps, allowing vehicles to either move or wait during each step [31]. Consequently, it becomes challenging to plan trajectories for vehicles with varying velocities or based on specific kinematic constraints. To address this limitation, researchers have explored extensions of the MAPF problem to accommodate such complexities. Among those extensions, Zhang et al. [32] designed a joint strategy to deal with an automatic valet parking system, in which a travel-distance-related reward function combined with a deep reinforcement learning technique was used to allocate the target parking spaces. The parking lot was segmented into local regions, and a rule-based right-of-way assignment strategy was applied to solve collisions and deadlocks. A simplified trajectory planning algorithm based on the car-following model [33] served as a tool to solve the trajectories of multiple AGVs when no potential collision was involved. A similar strategy was proposed by Lee et al. [34] for a supply-chain-connected warehouse. In their work, a cloud-based semiautomatic warehouse management system assigns tasks to mobile robots to optimize resource allocation. A robot control system executes an improved A* search algorithm to generate the path of each AGV. Then, potential collisions, named stay-on, head-on, and cross-conflict, are identified and solved by following certain priority-based rules. Redispatchment of the AGV with low priority is triggered as the conflict cannot be prevented by those basic rules.

With regard to joint strategies, the studies above [32,34] can deal with large-scale AGV-based scenarios. However, these studies were concerned with vehicles possessing the simple kinematics of unicycle (differential-drive) robots [35] and generally focused on the construction of maneuverable systems, ignoring the overall optimal solutions concerning warehouse operations. Furthermore, during the trajectory planning phase, they initially planned only the paths and ignored other moving obstacles. Such considerations substantially reduce the risk of collisions and simplify the evaluation of the traveling difficulties pertaining to one potential task relative to the corresponding traveling distance. In other words, it remains unknown the specific trajectory pertaining to each AGV as the jointed strategy is finished, and this trajectory is dependent on local scenes when conflict zones are involved. Thus, the results become unreliable when such strategies are applied to a warehouse of automated forklifts with complex kinematics. As a conclusion of this subsection, it deserves to develop a joint dispatching and planning method to balance the forks’ motion quality and reaction speed in an unmanned warehouse.

1.2. Motivations

This study aims to substantially improve the efficiency of cooperative operations among multiple autonomous forklifts by seamlessly integrating the dispatching and cooperative trajectory planning phases. Our primary objective is to address the limitations of existing dispatching methods, which often overlook low-level forklift kinematic capability. To overcome this challenge, we opt for the implementation of a graph search process in this phase. Moreover, to ensure a robust solution that avoids getting trapped in local optima, we chose to incorporate a machine-learning-based technique. In the trajectory planning phase, we recognize that optimization-based methods are computationally expensive. As such, our secondary objective is to develop an alternative search algorithm that employs a model-based approach. This algorithm is designed to be both velocity-aware and sequentially solvable, striking a balance between accuracy and computational efficiency.

1.3. Contributions

The core contribution of this paper is the proposal of a joint framework, which is promising to reduce the computational burden because all formulations involved are explicitly expressed. Concretely, the dispatching stage can enhance the multi-vehicle task solution quality because it considers the future trajectory pertaining to each forklift. Moreover, the kinematically feasible and safe trajectory of each forklift can be quickly generated through our proposed method at the trajectory planning stage, due to the removal of optimization-based methods.

1.4. Organization

In the rest of this paper, Section 2 formulates the in-warehouse delivery problem. Section 3 provides the score-based dispatching technique, in which ANN is applied to avoid deadlocks in evaluating the cost of each candidate dispatching option. Section 4 introduces the trajectory planning method, namely a model-based velocity-aware hybrid A* search algorithm. Section 5 integrates the two aforementioned methods to develop a joint dispatching and cooperative trajectory planning framework, followed by Section 6, where comparative simulation results are present. Conclusions are drawn in Section 7, finally.

2. Problem Statement

Forklifts are used to deliver goods between fixed picking stations and predetermined shelf areas during delivery tasks in warehouses. The passages are generally designed to be narrow, and they merely allow turning maneuvers with a minimum radius and the passing of only two vehicles. Hence, conflicts arise when multiple forklifts cooperatively operate within a single warehouse.

Within one subtask during the filling of one warehouse, there are two stages: first, the initial pose and the final one should be assigned to one forklift as the dispatching stage; second, the trajectory planning stage generates a trajectory by avoiding collisions with any static or moving objects and satisfying the vehicle kinematics.

2.1. Warehouse Layout

A typical small warehouse layout is schematized in Figure 1. In this warehouse, six separated shelf clusters, denoted as $s_d$ ($d=1,2,\cdots ,6$), are placed and provide areas to store goods. Continuous lines indicate shelf walls, through which forklifts cannot move. Two firewalls, represented by rows of grey squares, are present between shelf cluster $s_1$ and $s_3$ as well as cluster $s_2$ and $s_4$. Meanwhile, four picking stations, marked with slender solid rectangles and $p_r$ ($r=1,2,3,4$), are located in both extremities of the vertical and wide passage in Figure 1. Four forklifts can enter the passage of each row of the shelf clusters, as long as neither stored stacks nor other vehicles block the route.

As presented in Figure 2, each shelf cluster has the capacity to accommodate varying numbers of goods stacks with strategically positioned notches in the arrangement designed to suit forklift kinematics during turns. This aspect will be elaborated on in Section 2.2. Each stack is marked with a number in Figure 2 to represent the order in which a shelf cluster can be filled. The shelf filling state and the vehicle state can be effectively expressed by noting the covered grids when the warehouse is divided by the squares outlined by grey dashed lines in Figure 2. In addition, when one forklift is unloading goods within a shelf cluster, its fork side should point to the stack position (cf. upper left forklift schematic and within shelf cluster $s_1$ in Figure 1). Similarly, when one forklift is picking goods at a picking station, the fork side should point to the station position (cf. upper forklift schematic and picking station $p_1$ in Figure 1).

2.2. Kinematics of a Forklift Vehicle

As reported in Figure 3, a forklift can be described as a front-steering vehicle if the fork part of the vehicle is treated as the rear side. The corresponding kinematic formulas write:

$$\left\{\begin{array}{c}\frac{dx(t)}{dt}=v(t)\cdot \cos \theta (t)\hfill \\ \frac{dy(t)}{dt}=v(t)\cdot \sin \theta (t)\hfill \\ \frac{dv(t)}{dt}=a(t)\hfill \\ \frac{d\theta (t)}{dt}=\frac{v(t)\cdot \tan \varphi (t)}{l}\hfill \\ \frac{d\varphi (t)}{dt}=\omega (t)\hfill \end{array}\right.$$

(1)

where $t$ is time; $P$, located at coordinate ($x$, $y$), indicates the mid-point of the rear wheel axis; and $\theta$, $v$, $a$, $\varphi$, and $\omega$ respectively denote the orientation angle, linear velocity pertaining to point $P$, acceleration, steering angle of the front wheels, and steering rate. Furthermore, $l$ stands for the wheelbase length, $m$ denotes the rear overhang length, $n$ refers to the front overhang length, and $2b$ is the car width. Given that the initial values as well as $\omega \left(t\right)$ and $a\left(t\right)$ are provided, the state variables can be calculated through integration over the dynamic process.

Meanwhile, a few boundaries are imposed on the state profiles over the entire simulation period throughout all dynamic maneuvers:

$$\left\{\begin{array}{c}\left|a(t)\right|\le a_{\max }\\ \left|v(t)\right|\le v_{\max }\\ \left|\varphi (t)\right|\le {\varphi }_{\max }\\ \left|\omega (t)\right|\le {\omega }_{\max }\end{array}\right.$$

(2)

where $a_{max}$, $v_{max}$, ${\varphi }_{max}$, and ${\omega }_{max}$ respectively indicate the upper limits of the corresponding variables.

3. ANN-Combined Score-Based Dispatching Approach

Filling a warehouse in an orderly manner requires several forklifts to perform multiple deliveries and return subtasks. In the current work, subtasks are assigned to different vehicles sequentially. With regard to such subtasks, selecting which vehicle will be used to plan the new trajectory and determining which goal coordinates the forklift is going to should comprise the fundamental initialization. Therefore, a dispatching system is necessary to solve these problems.

The dispatching approach (cf. Algorithm 1) utilizes the planned trajectories $T$ of all vehicles, along with map information $map$, and vector $F$ representing filled stacks for different clusters, as its inputs. The core of this approach lies within a while loop, wherein the potential subtask undergoes iterative updates (with a preset maximum iteration number $ite{r}_{dispatch}$) until it reaches an optimal state, as defined by the proposed method.

Outside of the loop, the function $\mathrm{rank}()$ sorts all vehicles based on $t$, arranging them in ascending order from first to last. This sorting process generates a ranking vector $R$ consisting of four vehicle indices. If $fail=1$, indicating a failure in the trajectory search between $P_i\left(x_i,y_i\right)$ at instant $t_i$ and $P_f\left(x_f,y_f\right)$, the corresponding vehicle is repositioned at the end in $R$ and flagged as having been selected as $n_{current}$. In the loop, the algorithm is divided into two parts. The first one (lines 4 to 13) concerns the selection of the current investigated vehicle with an index of $n_{current}$, and the second part (lines 14 to 30), featuring a scoring system combined with the results of an ANN method, determines the goal coordinate for the current subtask. Notably, Algorithm 1 is applied when the ANN correction system, which is elaborated on in Section 3.2, is enforced.

Algorithm 1: ANN combined score-based dispatching algorithm

$\left[n_{current},P_i,P_f,t_i\right]\leftarrow \mathrm{Dispatch}\left(T,F,fail,P_i,P_f,map\right)$ 1. Initialize $\alpha \leftarrow 0$; 2. $R\leftarrow \mathrm{rank}\left(T,P_i,P_f,fail\right)$; 3. while $iter<ite{r}_{dispatch}$, do 4.  if $\alpha =0$, then 5.   $\left[n_{current},t_i\right]\leftarrow \mathrm{SelectInitialState}\left(R\right)$; 6.  else 7.   if $\mathrm{CheckSelection}\left(R\right)>0$, then 8.    $\left[n_{current},t_i\right]\leftarrow \mathrm{SelectAlteredState}\left(R\right)$; 9.   else 10.    $\left[n_{current},t_i\right]\leftarrow \mathrm{SelectBackupState}\left(R\right)$; 11.   end if 12.  end if 13.  $P_i=\mathrm{SetInitialPose}(T,n_{current},map)$; 14.  if $\mathrm{CheckDeliverTask}\left(P_i\right)$ is true, then 15.   $S_d=\mathrm{PreAstarDeliver}\left(P_i,F,map\right)$; 16.   if $\max \left(S_d\right)>0$, then 17.    $P_f=\mathrm{SetFinalPose}\left(S_d,map\right)$; 18.    return; 19.   else 20.    $\alpha \leftarrow 1$; 21.   end if 22.  else 23.   $S_r=\mathrm{PreAstarReturn}\left(P_i,map\right)$; 24.   if $\max \left(S_r\right)>0$, then 25.    $P_f=\mathrm{SetFinalPose}\left(S_r,map\right)$; 26.    return; 27.   else 28.    $\alpha \leftarrow 1$; 29.   end if 30.  end if 31. end while 32. return;

3.1. Vehicle Selection and Initial Pose of a New Subtask

The function $\mathrm{SelectInitialState}()$ selects the vehicle, ranking the first one as $n_{current}$, and sets $t_i$, which is the initial instant of the trajectory to be planned, as the ending instant of the last trajectory pertaining to $n_{current}$. Failure may occur in the determination of the new trajectory for the fork $n_{current}$ in the new subtask because other forklifts may block the only corresponding route for a considerable time. Under such circumstances, a flag variable $\alpha$ is set to 1, and all vehicles in $R$ are checked to see if they have been selected as $n_{current}$ once for the current subtask by $\mathrm{CheckSelection}()$. The function $\mathrm{SelectAlteredState}()$ is then utilized. In this function, the first motion-finished vehicle is discarded, and the other forklifts are subsequently selected in turn as $n_{current}$ on the basis of the rankings in $R$ until the trajectory can be formed. Furthermore, no trajectory can be successfully planned for all forklifts at certain moments. In this case, $\mathrm{SelectBackupState}()$ is applied, in which the vehicle that ranks last in $R$ is selected, and the $t_i$ of the new trajectory is postponed for a fixed time length of $∆t_i$ relative to the end of the last subtask for vehicle $n_{current}$. The vehicle $n_{current}$ final stopping pose $P_i\left(x_i, y_i,{\theta }_i\right)$ is set as the initial pose of the new subtask by function $\mathrm{SetInitialPose}()$.

3.2. Scoring System

A scoring system is applied to decide the goal coordinate of the new subtask. First, the function $\mathrm{CheckDeliverTask}\left(P_i\right)$ is initially employed to ascertain whether the planned trajectory involves heading to clusters or returning. The functions $\mathrm{PreAstarDeliver}()$ and $\mathrm{PreAstarReturn}()$ are then applied separately depending on whether the goal is a rack cluster or a picking station. In both functions, the grid networks, outlined by light colors in Figure 2, indicate the nodes used to define the location of a vehicle and stacks. The resolution of such nodes is purposely reduced with the aim of lowering computation costs. Given that the nodes are defined, a time dimension involved preliminary A* search algorithm, whose expansion manner is presented in Figure 4, is used to generate preliminary trajectories that link the starting pose $P_i\left(x_i,y_i\right)$ at instant $t_i$ to each potential target coordinate. Five patterns in total for this search algorithm are applied to vaguely indicate the possible maneuvers a vehicle could perform. In particular, manner 5 in Figure 4 expands only in the time dimension, representing the stopping condition of a virtual forklift. The time consumed derived from this algorithm for a virtual vehicle represented by one node is then applied as a parameter to evaluate the difficulty grades of reaching different goal poses. Other vehicles and walls are treated as obstacles during the search. Notably, the orientation angles $\theta$ for the initial and final poses are not required to be determined in such a system. Thus, $\theta$ is not considered a dimension in this search for the sake of calculation simplification.

In the function $\mathrm{PreAstarDeliver}()$, the function $S_{d,0}$ is applied to evaluate the scores pertaining to different target stack locations $s_d$ (cf. Figure 1) when a subtask with one stack as the goal is considered. It writes:·

$$\left\{\begin{array}{c}{S}_{d,0}=C_1{G}_d+J_{d,0}\hfill \\ J_{d,0}=\left\{\begin{array}{c}H+C_2{I}_d+C_3 \mathrm{preliminary} \mathrm{trajectory} \mathrm{is} \mathrm{planned}\hfill \\ \hfill \hspace{1em}\hspace{1em} C_4\hspace{1em}\hspace{1em}\hspace{1em}\mathrm{preliminary} \mathrm{trajectory} \mathrm{planning} \mathrm{is} \mathrm{failed} \mathrm{within} ite{r}_{pre}\hfill \end{array}\right.\hfill \\ H=-\Delta t_{cover}\hfill \\ I_d=\left\{\begin{array}{c}{t}_{find}-t_i+C_5\hspace{1em}{s}_d\in \left\{s_1,s_2\right\}\hfill \\ \hspace{1em}{t}_{find}-t_i\hspace{1em}\hspace{1em}{s}_d\in \left\{s_3,s_4,s_5,s_6\right\}\hfill \end{array}\right.\hfill \end{array}\right.$$

(3)

where $G_d$ denotes the number of stacks to be filled/emptied in the target shelf cluster $s_d$ in order to balance the warehouse filling/emptying mission in different clusters. $J_{d,0}$ stands for the approximate difficulty to reach different goal coordinates in shelf cluster $s_d$. $C_4$ is a negative constant applied when the goal in practice is not reachable. $H$ indicates time length $∆t_{cover}$. $∆t_{cover}$ counts the time units during which other vehicles occupy the shelf entrance node (cf. the grey grid for the upper left forklift in $s_1$ in Figure 1) of the goal pose within a fixed time length $∆t_{entry}$ after the virtual vehicle entering the rack passage. The reason $∆t_{cover}$ is added as a parameter is that, in practice, the availability of the above-mentioned node is critical during the planning of the final trajectory that considers kinematics. $I_d$ is a function of the time length $\left(t_{find}-t_i\right)$ consumed to arrive at the target in this preliminary search. Notably, this function is built to normalize the results for different shelf clusters because shelf clusters $s_1$ and $s_2$ are far from the picking stations, and delivering goods to these locations consumes much time. Furthermore, a negative constant $C_4$ is assigned to $J_{d,0}$ when the target cannot be reached through the search within a predefined maximum iteration number $ite{r}_{pre}$. Meanwhile, $C_1$, $C_2$,…, $C_5$ are calibration parameters. Among these, a substantial weighting coefficient, $C_1$, is allocated to regulate the stacks filled in each shelf cluster; aiming for balance, $C_2$, $C_3$, $C_4$, and $C_5$ are designed to quantitatively assess scores with respect to time considerations. If $S_{d,0}\le 0$ is derived within $ite{r}_{pre}$, the vehicle kinematics-considered trajectory is difficult to find. Thus, the flag variable $\alpha$ is set to 1 in the initialization cycle, where ANN is not enforced, and $n_{current}$ should be reassigned.

Similarly, in the function $\mathrm{PreAstarReturn}()$, only the time consumed with respect to different picking stations $p_r$ (cf. Figure 1) in the A* search is used in the evaluation of $S_r$, which is expressed as:

$$S_r=\left\{\begin{array}{l}{I}_r \mathrm{preliminary} \mathrm{trajectory} \mathrm{is} \mathrm{planned}\\ 0 \mathrm{preliminary} \mathrm{trajectory} \mathrm{planning} \mathrm{is} \mathrm{failed} \mathrm{within} ite{r}_{pre}\end{array}\right.$$

(4)

When ANN is not enforced, the potential targets are initially scored solely by means of Equations (3) and (4). The greater the functions to be evaluated are, the greater the likelihood of subsequent trajectory planning is and the faster the entire warehouse can be filled. In this case, $\arg \max S_{d,0}$ and $\arg \max S_r$ are selected as the goal poses for delivery and return subtasks, respectively.

3.3. ANN Correction Method

The function $\mathrm{PreAstarDeliver}()$ is employed to refine goal score evaluations through a multilayer perceptron (MLP) network, which is elaborated upon as follows.

Figure 5 presents a typical MLP network of ANN with one hidden layer. Mathematically, with the trajectory planning states as known variables, the MLP network of the type reported in Figure 5 can be expanded step by step as follows:

$$\widehat{y}(w,W)=\mathrm{F}({\displaystyle \sum _{j=1}^m{W}_j{\mathrm{h}}_j(w)}+W_0)=\mathrm{F}({\displaystyle \sum _{j=1}^m{W}_j{\mathrm{f}}_j({\displaystyle \sum _{i=1}^n{w}_{ji}{z}_i+w_{j0}})}+W_0)$$

(5)

where $w_{ji}$ and $W_j$ denote the weights assigned to the connection of the neurons. $W_o$ and $w_{j0}$ are linked to the bias, whose values are simply the constant 1.

The ANN correction in the current study is designed for the scoring system for the goal pose determination of delivery subtasks. In the initialization phase of the ANN correction system, excluding the filling balance parameter of $G_d$ in Equation (3), $\left\{J_{1,0},J_{2,0},\cdots ,J_{6,0}\right\}$ respectively denote the base values of the output elements in $J=\left\{J_{1,est},J_{2,est},\cdots ,J_{6,est}\right\}$ (cf. $\hat{\mathrm{y}}$ in Figure 5) for six MLP networks. Among the six elements, the one whose corresponding pose is selected as the target for vehicle kinematics-considered trajectory planning is further fixed based on the corresponding trajectory variables. The values of the other elements remain as unchanged as the results in Equation (3). Suppose that $s_d$ is the shelf cluster investigated within a subtask. Given that the base value $S_{d,0}$ is derived with Equation (3), if $s_d$ that corresponds to $\arg \max S_{d,0}$ is selected as the goal to determine the trajectory, then the trajectory with the goal of $s_d$ will be planned, and the corresponding element in $J$ will be derived with Equation (6) as follows:

$$J_{d,est}=\left\{\begin{array}{cc}\hfill J_{d,0}-C_6(t_f-t_i-\overline{t}+C_7)\hfill & \mathrm{trajectory} \mathrm{is} \mathrm{planned}, \mathrm{and} d\in \left\{1,2\right\}\hfill \\ \hfill J_{d,0}-C_6(t_f-t_i-\overline{t}+C_8)\hfill & \mathrm{trajectory} \mathrm{is} \mathrm{planned}, \mathrm{and} d\in \left\{3,4,5,6\right\}\hfill \\ \hfill C_5\hfill & \mathrm{trajectory} \mathrm{planning} \mathrm{is} \mathrm{failed} \mathrm{within} ite{r}_{pre}\hfill \\ \hfill J_{d,0}\hfill & \mathrm{trajectory} \mathrm{is} \mathrm{not} \mathrm{planned}\hfill \end{array}\right.$$

(6)

where $t_f$ is the ending instant of the trajectory, $\overline{t}$ stands for the average value of the time length pertaining to all previously derived trajectories, $C_6$ is a calibrated constant intended to balance $J_{d,0}$, and the latter solely accounts for time consumed to reach a goal without factoring in the distance covered. $C_7$ and $C_8$ are respectively used to normalize the difference in distances corresponding to various shelf clusters and the expected moving time period of each subtask.

Similar to the parameter $G_d$ in Equation (3), the inputs (cf. $z_1, z_2,\cdots , z_n$ in Figure 5) of the ANN system are the number of stacks filled within each shelf cluster. A vector $U$ with six elements $\{u_1, u_2,\cdots , u_6$}, which correspond to six shelf clusters $s_d$, is used. Suppose that $u_d\in U$ is considered, it can be formulated as

$$u_d=\left\{\begin{array}{cc}\hfill G_d\hfill & \mathrm{delivery} \mathrm{subtask} \mathrm{is} \mathrm{linked}\hfill \\ \hfill -G_d\hfill & \mathrm{return} \mathrm{subtask} \mathrm{or} \mathrm{subtasks} \mathrm{of} \mathrm{both} \mathrm{types} \mathrm{are} \mathrm{linked}\hfill \\ \hfill 0\hfill & \mathrm{no} \mathrm{subtask} \mathrm{is} \mathrm{linked}\hfill \end{array}\right.$$

(7)

The integers other than 0 can represent the filling states of the shelf clusters, which are associated with the currently moving forklifts. The vehicle motion states are also observed. This input of the MLP network vaguely provides information associated with the possible area the vehicles may be located in, given that each shelf cluster should be filled by following certain orders. Furthermore, at least two shelf clusters are not connected to any subtask because only four forklifts in the warehouse are employed. Evidently, these shelf clusters have no impact on the trajectory planning, and this situation is in line with the circumstances, where the investigated shelf group is filled. Thus, 0 is assigned to $u_d$ in this case.

As the number of hidden neurons is set to 12 according to an empirical technique [36], the MLP system used to score shelf cluster $s_d$ can be expressed in the form of Equation (5) as

$${\widehat{J}}_{d,est}(w_d,W_d)=\mathrm{F}({\displaystyle \sum _{j=1}^{12}{W}_{d,j}{\mathrm{h}}_j(w_d)}+W_0)=\mathrm{F}({\displaystyle \sum _{j=1}^{12}{W}_{d,j}{\mathrm{f}}_j({\displaystyle \sum _{i=1}^6{w}_{d,ji}{u}_{d,i}+w_{d,j0}})}+W_0)$$

(8)

The Levenberg-Marquardt method is then used to train the MLP and the values assigned to the elements of $W$ and $w$.

In the following warehouse filling cycles and the final dispatching system, the scoring system described in Equation (3) is replaced by the expression below to determine the optimal goal pose for vehicle $n_{current}$. Equation (9) is the final scoring equation of the discussed dispatching system, wherein $C_9$ and $C_{10}$ act as the calibration constants. These constants are determined via a trial-and-error approach to yield results. The MLP contributes without excessively disrupting performance concerning warehouse filling/emptying time.

$$\left\{\begin{array}{c}{S}_d=C_1{G}_d+J_d\hfill \\ J_d=C_9{J}_{d,0}+C_{10}{\widehat{\mathrm{J}}}_{d,est}(w_d,W_d)\hfill \end{array}\right.$$

(9)

After the initialization cycle of warehouse filling (referring to Figure 6), the training process can continue until $Cy$ cycles are finished. In the following cycles, the MLP has already been established depending on the data pertaining to the previous cycles, and the saved values of $W$ and $w$ are used to estimate the values ${\hat{\mathrm{J}}}_{d,est}()$ via Equation (8). Therefore, the base values of the MLP outputs $J_{d,0}$ are replaced by $J_d$ in Equation (9) when shelf cluster $s_d$ is considered. The expression of the corresponding element in $J$ for the following cycles writes:

$$J_{d,est}=\left\{\begin{array}{cc}\hfill J_d-C_6(t_f-t_i-\overline{t}+C_7)\hfill & \mathrm{trajectory} \mathrm{is} \mathrm{planned}, \mathrm{and} d\in \left\{1,2\right\}\hfill \\ \hfill J_d-C_6(t_f-t_i-\overline{t}+C_8)\hfill & \mathrm{trajectory} \mathrm{is} \mathrm{planned}, \mathrm{and} d\in \left\{3,4,5,6\right\}\hfill \\ \hfill C_5\hfill & \mathrm{trajectory} \mathrm{planning} \mathrm{is} \mathrm{failed} \mathrm{within} ite{r}_{pre}\hfill \\ \hfill J_d\hfill & \mathrm{trajectory} \mathrm{is} \mathrm{not} \mathrm{planned}\hfill \end{array}\right.$$

(10)

3.4. Final Pose Selection

The final pose of each subtask is determined using the $\mathrm{SetFinalPose}()$ function. For subtasks with the purpose of delivery, the destination coordinate $P_f\left(x_f,y_f\right)$ is chosen based on the $\arg \max S_d$ criterion; for return subtasks, the coordinate $P_f\left(x_f,y_f\right)$ corresponding to $\arg \max S_r$ is selected. Finally, the corresponding $\theta$ information should be added to complete the destination pose.

4. Improved Hybrid A* Search Algorithm

Hybrid A* algorithm [37] is an extension of the conventional 2D A* search algorithm due to its consideration of kinematics during node expansions over time. Different from the original hybrid A* search, the velocity variation is considered, and the node expansion manner is determined based on action purposes in the algorithm (cf. Algorithm 2) applied in this study. Furthermore, this improved heuristic method directly determines trajectory details throughout a subtask without an optimization stage.

Algorithm 2 is used to expand a parent node ($P_p,t_p$) in one manner $N_c$ through the improved hybrid A* search with the target of $P_f$. Focusing on the motion of one vehicle, $Lis{t}_{open}$ is used to store the data pertaining to all nodes ($P_{open},t_{open}$), which has been explored and can be further expanded. The information contains the corresponding parent nodes, expansion manner (cf. Section 4.1), time expansion data (cf. Section 4.2), and costs (cf. Section 4.4). On the contrary, the node ($P_{closed},t_{closed}$) cannot be expanded anymore and is stored in $Lis{t}_{closed}$. Furthermore, the function $\mathrm{DetectCollision}()$ is discussed in Section 4.3. In addition, the function $\mathrm{AddNode}()$ is used to add a node with its affiliating data into $Lis{t}_{open}$ or $Lis{t}_{closed}$.

Algorithm 2: Improved hybrid A* search algorithm

$\hspace{1em}\left[\sigma ,Lis{t}_{open}\right] \leftarrow \mathrm{SearchAStar}\left(T,N_c,P_p,P_f,t_p,Lis{t}_{open},Lis{t}_{closed},map\right)$ 1. $\left[t_{high,c},t_{mid,c},t_{zero,c}\right]\leftarrow \mathrm{FixMovingTime}\left(Lis{t}_{open},P_p,t_p,N_c\right)$; 2. $Lis{t}_{open}\leftarrow \mathrm{SetCost}\left(Lis{t}_{open},P_p,t_p,N_c\right)$; 3. $\gamma \leftarrow \mathrm{DetectCollision}\left(T,N_c,P_p,t_p,Lis{t}_{open},map\right)$; 4. if $\left(P_c,t_{zero,c}\right)\in Lis{t}_{closed }$, then 5.  return; 6. end if 7. if $\left(P_c,t_{zero,c}\right)\in Lis{t}_{open}$ and $\gamma =0$, then 8.  if $f_c<f_{pre}$, then 9.   $Lis{t}_{open}\leftarrow \mathrm{ReplaceNode}\left(Lis{t}_{open},P_p,t_p\right)$; 10.  end if 11. else 12.  if $\gamma =0$, then 13.   $Lis{t}_{open}\leftarrow \mathrm{AddNode}\left(Lis{t}_{open},P_c,t_{zero,c}\right)$; 14.   if $P_c=P_f$, then 15.    $\sigma \leftarrow 1$; 16.    return; 17.   end if 18.  else if $\gamma =1$, then 19.   $Lis{t}_{closed}\leftarrow \mathrm{AddNode}\left(Lis{t}_{closed},P_c,t_{zero,c}\right)$; 20.   return; 21.  end if 22. end if 23. return;

4.1. Node Expansion Method

This section elaborates on the various possible expansion manners denoted as $N_{all}$, with each individual possibility represented by $N_c$. The drivable area is initially mapped with the above-discussed grid networks (cf. Figure 2). The dimensions of the grid should be skillfully coupled with the size of the vehicle, with the aim of reducing occupied cells during a certain action and enhancing the utility rate of the space. The case demonstrated in Figure 7 is a well-designed example. In this case, one vehicle covers two grid cells. Thus, on a 2D space domain, the orientation angle $\theta$ involved in forklift movements can be easily described. In addition, through one maneuver, the vehicle body occupies a small number of grids. This can reduce the possibility of interference with other forklifts during motion planning.

As reported in Figure 7a,b, the nodes expanding manners $N_{all}$ in the space domain can be divided into eleven patterns, which are summarized as four categories covering all possible maneuvers a forklift may intend to conduct. These include stopping, going straight (cf. semitransparent solid rectangles in Figure 7a), lane changing (cf. rectangles with diagonal stripes in Figure 7b), and turning (cf. solid rectangles in Figure 7b).

As far as the expanding manners are concerned, when $v$ remains 0, one vehicle stops. As $v$ is other than 0 with steering angle $\varphi =0$, one fork can go or reverse straight. Figure 8 demonstrates one modeled forklift path depicted by consecutive outlines, when the vehicle goes upward and turns to the left from the right lower side to the center left. Through this maneuver, $\varphi$ is varied to gain an identical final location relative to grids as the initial state, despite the change of $\pi /2$ in $\theta$. Under such circumstances, the consecutive motions can be easily established. Meanwhile, the modeled maneuver of reverse turning to the left can also be noted in Figure 8, when the initial and final body outlines are exchanged.

Similar to turning actions, a typical lane-changing maneuver is also modeled and outlined in Figure 9. The forklift goes forward and changes to the left lane, which is shown on the top row in Figure 9. The other possible node-expanding paths of turning and lane changing are modeled by mirroring or rotating the examples reported in Figs. 8 and 9.

The smoothness of the modeled paths can be enhanced by adopting advanced techniques, and the identical heuristic trajectory search rule of this work can also be applied to the newly modeled paths.

4.2. Velocity Planner

The velocity planner determines the node expansion rule in the time domain and the potential node arrival instants. When one maneuver starts or finishes, the vehicle speed solely falls into one of three determined constants ($v_{high}$, $v_{mid}$ and $v_{zero}$), which correspond to high speed, mid speed, and zero speed. Figure 10 reports all possible velocity selections linked to the start and end of the maneuvers. $v_{high}$ can only be selected with the expansion pattern of going straight to fulfill the kinematic constraints given by Equations (1) and (2). The stop maneuver is an expansion of finite value solely on the time domain, it follows that only $v_{zero}$ can be applied to this action.

The velocity level over one maneuver can only be linked to itself or an adjacent one, although all maneuvers can be arbitrarily linked along a trajectory. In other words, during one maneuver, the vehicle speed can maintain or alter among those three velocities, but the direct change between $v_{zero}$ and $v_{high}$ is illegal. For instance, if the vehicle finishes a series of going straight actions with speed $v_{zero}$, its ending velocity of the last second action should then be $v_{mid}$ or $v_{zero}$. Likewise, if turning follows going straight with an original velocity $v_{high}$, the speed of the last going straight maneuver should reduce to $v_{mid}$ to gain an initial velocity $v_{mid}$ for turning.

Following the velocity selection rules, all possible speed variations through one maneuver are shown in Figure 11. It is noteworthy that the link of $v_{high}$→Going straight→$v_{zero}$ and the opposite link are infeasible. Thus, 16 connection choices in total are applicable. Furthermore, velocity is merely an intermediate state used to determine a time dimension expansion, although it is a vital parameter for motion planning. The detailed velocity time history during one maneuver can be modeled by applying varied techniques to obtain the required initial and finishing speeds. In the current work, deceleration or acceleration along one action between the same velocity levels is modeled with identical time lengths, with the aim of simple calculation. The number of time expansion selections (cf. $T_{\mathrm{e}}$ with $e\in \left\{1,2\cdots ,12\right\}$ in

Table 1. Node expansion selections in time domain.

Abbreviation	Description
$T_1$	$v_{zero}$$\to \mathrm{Going} \mathrm{straight}\to v_{zero}$
$T_2$	$v_{zero}$$\to \mathrm{Going} \mathrm{straight}\to v_{mid}$$\mathrm{or} v_{mid}$$\to \mathrm{Going} \mathrm{straight}\to v_{zero}$
$T_3$	$v_{mid}$$\to \mathrm{Going} \mathrm{straight}\to v_{mid}$
$T_4$	$v_{mid}$$\to \mathrm{Going} \mathrm{straight}\to v_{high}$$\mathrm{or} v_{high}$$\to \mathrm{Going} \mathrm{straight}\to v_{mid}$
$T_5$	$v_{high}$$\to \mathrm{Going} \mathrm{straight}\to v_{high}$
$T_6$	$v_{zero}$$\to \mathrm{Turning}\to v_{zero}$
$T_7$	$v_{zero}$$\to \mathrm{Turning}\to v_{mid}$$\mathrm{and} v_{mid}$$\to \mathrm{Turning}\to v_{zero}$
$T_8$	$v_{mid}$$\to \mathrm{Turning}\to v_{mid}$
$T_9$	$v_{zero}$$\to \mathrm{Lane} \mathrm{changing}\to v_{zero}$
$T_{10}$	$v_{zero}$$\to \mathrm{Lane} \mathrm{changing}\to v_{mid}$$\mathrm{or} v_{mid}$$\to \mathrm{Lane} \mathrm{changing}\to v_{zero}$
$T_{11}$	$v_{mid}$$\to \mathrm{Lane} \mathrm{changing}\to v_{mid}$
$T_{12}$	$v_{zero}$$\to \mathrm{Stop}\to v_{zero}$

) thus reduces to 12.

The function $\mathrm{FixMovingTime}()$ determines time expansion data based on

Table 2. Model-based approach for saving time consumption data.

Current Maneuver	$\mathit{\mu }=0$	$\mathit{\mu }=1$
Stop	-	$t_{zero,c}\leftarrow t_{zero,p}+T_{12}$ $t_{mid,c}\leftarrow t_{zero,p}+T_{12}$ $t_{high,c}\leftarrow t_{zero,p}+T_{12}$
Going straight	$\mathbf{if} t_{high,p}=t_{mid,p}$, then $t_{zero,c}\leftarrow t_{mid,p}+T_2$ $t_{mid,c}\leftarrow t_{mid,p}+T_3$ $t_{high,c}\leftarrow t_{mid,p}+T_4$ else $t_{zero,c}\leftarrow t_{mid,p}+T_2$ $t_{mid,c}\leftarrow t_{high,p}+T_4$ $t_{high,c}\leftarrow t_{high,p}+T_5$	$t_{zero,c}\leftarrow t_{zero,p}+T_1$ $t_{mid,c}\leftarrow t_{zero,p}+T_2$ $t_{high,c}\leftarrow t_{zero,p}+T_2$
Turning	$t_{zero,c}\leftarrow t_{mid,p}+T_7$ $t_{mid,c}\leftarrow t_{mid,p}+T_8$ $t_{high,c}\leftarrow t_{mid,p}+T_8$	$t_{zero,c}\leftarrow t_{zero,p}+T_6$ $t_{mid,c}\leftarrow t_{zero,p}+T_7$ $t_{high,c}\leftarrow t_{zero,p}+T_7$
Lane changing	$t_{zero,c}\leftarrow t_{mid,p}+T_{10}$ $t_{mid,c}\leftarrow t_{mid,p}+T_{11}$ $t_{high,c}\leftarrow t_{mid,p}+T_{11}$	$t_{zero,c}\leftarrow t_{zero,p}+T_9$ $t_{mid,c}\leftarrow t_{zero,p}+T_{10}$ $t_{high,c}\leftarrow t_{zero,p}+T_{10}$

, which is elaborated as follows. When planning a trajectory, the maximum feasible velocities are consistently selected for all maneuvers with zero speed supplied to the start and end of the trajectory. With regard to one node expansion, three values of $t_{high,p}$, $t_{mid,p}$ and $t_{zero,p}$ are initially saved in the parent node as the possible start instant of the maneuver, and these respectively correspond to ending velocities of $v_{high}$, $v_{mid}$, and $v_{zero}$, if they exist. In the cases where $v_{high}$ is not reachable, the value stored in $t_{high,p}$ will be the minimum time length during the maneuver, with ending velocity $v_{mid}$. Consequently, $t_{high,p}=t_{mid,p}$. Similar result of $t_{mid,p}=t_{zero,p}$ is obtained when the maximum realizable ending velocity is $v_{zero}$. As the parent node is initialized, values of $t_{high,c}$, $t_{mid,c}$, and $t_{zero,c}$ are derived and saved for the child node with the identical manner for the parent node. Furthermore, the starting velocity of a new expansion may be imposed as $v_{zero}$, combined with the node expansion type of the previous maneuver. A stopping flag $\mu$ is then set as 1. Such a case happens when an expansion of stop occurs or moving direction of a vehicle is reversed. In the rest of the working conditions, the flag $\mu$ remains 0. Notably, a node is of four dimensions, $x$, $y$, $\theta$, and $t$; among them, $t$ is an index used in the graph search, and more than one value stored in one index could complicate the problem. Consequently, only the $v_{zero}$-related time instant $t_{zero}$ is stored as the node index.

Meanwhile, each node stores three ending time instants. Thus, one algorithm should be applied to select the exact ending instant of each node. This algorithm first chooses the minimum time $t_{high}$ of each maneuver over the path. The deceleration phase is then imposed on the last two nodes of the trajectory, and the corresponding $t_{mid}$ and $t_{zero}$ are in turn assigned to these two nodes as the maneuver-finishing time instants. As the above methods are implemented from the start to the end of the initialized trajectory, a few modifications are imposed to fix the node time indices $t$ in certain circumstances. For instance, when $\mu =1$, the maximum achievable ending time $t_p$ for the former maneuver node is determined by selecting the value of $t_{mid,p}$. In this case, the ending velocity of this maneuver turns to be $v_{mid}$. Meanwhile, in the same combination of maneuvers, if a previous maneuver exists and $t_{pp}>t_{mid,pp}$, $t_{mid,pp}$ is assigned to $t_{pp}$, as the prior maneuver cannot achieve a higher velocity.

4.3. Collision Detection Strategy

The pseudocode of the collision detection function $\mathrm{DetectCollision}()$ is recorded in Algorithm 3. A variable $\gamma$ is used as a flag to indicate the validity of the node expansion as well as the type of collisions that may have occurred. When the current expansion $N_c$ is valid, $\gamma$ is set as 0. $\gamma =1$ means that at least the ending pose of the expansion risks colliding, and $\gamma =2$ signifies that the invalidity is only found in the link between the starting and final locations. This algorithm consists of two parts, which separately refer to collisions that occurred with ending and intermediate pose of the vehicle. As regards both circumstances, collisions should be avoided throughout $t\in \left[t_{mid,p},t_{zero,c}\right]$.

Algorithm 3: Collision detection algorithm

$\hspace{1em}\gamma \leftarrow \mathrm{DetectCollision}\left(T,N_c,P_p,t_p,Lis{t}_{open},map\right)$ 1. Initialize $\gamma \leftarrow 0$; 2. $P_c\leftarrow \mathrm{FindEndingPose}\left(P_p,N_c\right)$; 3. $P_{in}\leftarrow \mathrm{FindIntermediatePose}\left(P_p,N_c\right)$; 4. if $\mathrm{CheckStaticCollision}\left(P_c,map\right)$ is true, then 5.  $\gamma \leftarrow 1$; 6.  return; 7. end if 8. $\left[t_{zero,c},t_{mid,p}\right]\leftarrow \mathrm{FixParkingTime}\left(P_p,t_p,Lis{t}_{open},N_c\right)$; 9. $\left[Lis{t}_{cur,1}\right]\leftarrow \mathrm{FindFinalGrids}\left(P_p,N_c\right)$; 10. $\left[Lis{t}_{other}\right]\leftarrow \mathrm{FindObstaclesGrids}\left(T,t_{zero,c},t_{mid,p}\right)$; 11. if $\mathrm{CheckDynamicCollision}\left(Lis{t}_{cur,1},Lis{t}_{other}\right)$ is true, then 12.  $\gamma \leftarrow 1$; 13.  return; 14. end if 15. if $\mathrm{CheckStaticCollision}\left(P_{in},map\right)$ is true, then 16.  $\gamma \leftarrow 2$; 17.  return; 18. end if 19. $\left[Lis{t}_{cur,2}\right]\leftarrow \mathrm{FindIntermediateGrids}\left(P_p,N_c\right)$; 20. if $\mathrm{CheckDynamicCollision}\left(Lis{t}_{cur,2},Lis{t}_{other}\right)$ is true, then 21.  $\gamma \leftarrow 2$; 22.  return; 23. end if 24. return;

Static obstacles correspond to the walls of the shelves and warehouse, and the related collision detection method is expressed on lines 4 to 7 in Algorithm 3. $\mathrm{FindEndingPose}()$ is used to obtain the current pose $P_c\left(x_c,y_c,{\theta }_c\right)$ based on the parent pose $P_p\left(x_p,y_p,{\theta }_p\right)$ and the current expansion manner $N_c$. The function $\mathrm{CheckStaticCollision}()$ is applied to check if collisions with static obstacles exist. This check is first judged by identifying the validity of the final pose. When it comes to maneuvers of turning and lane changing, an additional intermediate pose (cf. thick dashed rectangular in Figure 8 and Figure 9), obtained through $\mathrm{FindIntermediatePose}()$, should be further examined (cf. lines 15 to 18 in Algorithm 3). Focusing on the collision avoidance constraints formulation between the point $Q_j$ ($j=1,\cdots ,{\mathrm{N}}_{\mathrm{obs}}$ and ${\mathrm{N}}_{\mathrm{obs}}$ denotes the obstacle point number) and the current investigated vehicle featuring vertexes $A$, $B$, $C$ and $D$. A collision forms when $Q_j$ enters the rectangle $ABCD$. The restriction that $Q_j$ is located outside of the rectangle $ABCD$ can be formulated by applying a triangle-area-based criterion [38],

$$S_{△P_{i}AB}+S_{△P_{i}BC}+S_{△P_{i}CD}+S_{△P_{i}DA}>S_{\square ABCD}$$

(11)

where $S_{\mathsf{\Delta }}$ indicates the triangle area, and $S_{▭}$ refers to the rectangle area. Applying Equation (11) to every node expansion with respect to every obstacle point, the static obstacle collision judgement is yielded.

Dynamic obstacles in the current study are only forklifts, whose motions have been saved in $T$, and the function $\mathrm{FixParkingTime}()$ is applied to find $t_{mid,p}$ in $Lis{t}_{open}$ and $t_{zero,c}$ by calling Table 2. As reported in Figure 8 and Figure 9, the highlighted grids indicate the approximate area occupied during the movements of turning and lane changing. Similarly, the covered grids pertaining to other maneuvers in the same category can be determined by mirroring or rotating the highlighted grids (cf. Figure 8 and Figure 9). The functions $\mathrm{FindFinalGrids}()$ and $\mathrm{FindIntermediateGrids}()$ are respectively used to record the covered grids of the final pose $P_c$ and the intermediate pose $P_{in}$ for the current node expansion patterns.

Without loss of generality, let us focus on the collision avoidance constraint formulation between the vehicle $n_{current}$ and the vehicle $n_k$ ($n_{current},n_k=1,\cdots ,4$ and $n_{current}\ne n_k$). The function $\mathrm{FindObstaclesGrids}()$ is first utilized to search for the covered grids by the vehicles $n_k$, whose trajectories are not being planned, with respect to time. It is possible that no actions of the forklift $n_k$ have been determined during the period when the current maneuver of vehicle $n_{current}$ could occupy. Under such circumstances, the grids that the vehicle $n_k$ finally parks are treated as the covered ones.

Finally, $\mathrm{CheckDynamicCollision}()$ is used to detect if $Lis{t}_{cur,1}$ or $Lis{t}_{cur,2}$ is going to simultaneously cover the grids already stored in $Lis{t}_{oth}$ over the valid time. Notably, during the collision detection, the current action is virtually regarded as the final maneuver with $v_{zero}$ set as the finishing velocity with $t_{zero,c}$ selected, thus an equal or longer time length of this maneuver in actual operation is considered. This treatment can enhance the safety performance to certain extents.

Notably, referring to Algorithm 3, the types of failure through an expansion have been noticed when one expansion has failed. This provides a tool to distinguish if one child node should be closed or skipped because that failure can be simply due to the intermediate trajectory of the action is interfered, while the corresponding child node could be valid in other situations.

4.4. Trajectory Cost Function

The function $\mathrm{SetCost}()$ is explained in this section. With regard to one node expansion, the cost function $f$ is the sum of two parts, as reported in Equation (12),

$$f=g+C_{11}h$$

(12)

in which $g$ stands for the cumulative cost from the initial pose to the current pose and $h$ indicates the estimated cost from the child node of the current expansion to the target node of the trajectory being defined. $C_{11}$ is a calibrated weighting aimed at achieving a balance between the computational resources used for searching and the resultant trajectory’s quality. In total, it is expected that one forklift complete a delivery or return subtasks subjected to the minimum travelling time of $\left(t_f-t_i\right)$. Meanwhile, times of turning, lane changing, and speed inverse maneuvers should be minimized in order to reduce unnecessary movements, which may decrease the traffic capacity of the passages.

As far as the function of trajectory to the goal is concerned, Equation (13) is applied. We used the Manhattan distance plus another function of ${\theta }_c$. Both the distance and the angle are evaluated in times of certain characteristic dimensions.

$$h=\left|x_g-x_c\right|/\Delta x+\left|y_g-y_c\right|/\Delta y+\left|{\theta }_g-{\theta }_c\right|/(\pi /2)$$

(13)

where $∆x=∆y$, indicating the grid side length; $x_g$, $y_g$, and ${\theta }_g$ correspond to the goal space coordinates; $x_c$, $y_c$, and ${\theta }_c$ refer to the coordinates of the current expansion child node.

The passed trajectory cost function that characterizes the time consumed from the starting pose of the trajectory to the current one writes:

$$\left\{\begin{array}{l}g=t_{zero,c}-t_i+p\\ p=p_{turn}\cdot m_{turn}+p_{lane}\cdot m_{lane}+p_{inv}\cdot m_{inv}\end{array}\right.$$

(14)

where $p_{turn}$, $p_{lane}$, and $p_{inv}$ respectively indicate the predefined penalties pertaining to single time of turning, lane changing, and speed inverse. $m_{turn}$, $m_{lane}$, and $m_{inv}$ denote the cumulative number of corresponding maneuvers from the initial pose to the current child node along the trajectory being defined.

In addition, if one node originally stored in $Lis{t}_{open}$ is reached a second time with a reduced value of $f_c$ during a node expansion compared to the previous saved $f_{pre}$ for the same nodes, the function $\mathrm{ReplaceNode}()$ is used to switch the parent node to the parent node of current expansion, with the aim of reducing calculation time length and optimizing the trajectory being planned.

5. Joint Dispatching and Cooperative Trajectory Planning Framework

The trajectories of forklift vehicles in the warehouse are sequentially determined in the complete cooperative operative algorithm (cf. Algorithm 4). Generally, the dispatching technique is first applied to select the current vehicle index as well as the goal coordinate in the space domain for the current trajectory planning subtask. Subsequently, an improved hybrid A* search algorithm is used to determine all details pertaining to the newly planned trajectory $T$. The above methods are repeated until all stack locations in the warehouse are filled, as the warehouse filling state $F$ is being updated.

The function $\mathrm{CheckFilling}()$ is used to derive the number of unfilled stacks. The function $\mathrm{FindInitialPose}()$ is then applied to find the parent node $\left(P_p,t_p\right)$ for the next expansion with minimum cost $f$ found in $Lis{t}_{open}$ and to simultaneously remove this node from $Lis{t}_{open}$.

A maximum number of iterations $ite{r}_{search}$ in the improved hybrid A* algorithm is induced to break the endless iterations that may derive a trajectory involving an unacceptable waiting period. The improved hybrid A* search algorithm is finished by satisfying any one of the three criteria, which are, respectively, the goal coordinate reached as a new child node, the iteration number exceeding $ite{r}_{search}$, and no node saved in $Lis{t}_{open}$. Among them, only the first criterion indicates the trajectory of the current subtask is successfully planned, and a corresponding flag $\sigma$ is thus set as 1 to declare this success.

The function $\mathrm{GenerateTraj}()$ is employed to generate the trajectory with the newly planned trajectory by backtracking from $P_f$ to $P_i$ with $t_{f,zero}$ as the ending instants. During the backtracking, the maximum realizable velocities of the intermediate nodes are selected based on the manner depicted in Figure 11 and Table 2.

Finally, the function $\mathrm{UpdateFillingState}()$ updates $F$ as the stack located at $P_f\left(x_f,y_f,{\theta }_f\right)$ has been filled.

Algorithm 4: Cooperative operation algorithm

$\hspace{1em}\left[T,F\right]\leftarrow \mathrm{OperateCooperative}\left(T,F,ite{r}_{search},map\right)$ 1. while $\mathrm{CheckFilling}\left(F\right)>0$ 2.  $\left[n_{current},P_i,P_f,t_i\right]\leftarrow \mathrm{Dispatch}\left(T,F,fail,P_i,P_f,map\right)$; 3.  $\sigma \leftarrow 0;$ 4.  $fail\leftarrow 0$; 5.  $t_{high,p}\leftarrow t_i,t_{mid,p}\leftarrow t_i,t_{zero,p}\leftarrow t_i;$ 6.  $Lis{t}_{open}\leftarrow \left(P_i,t_i\right),Lis{t}_{closed}\leftarrow \varnothing$; 7.  while $Lis{t}_{open}\ne \varnothing$ or $iter\le ite{r}_{search}$ or $\sigma \ne 1$, do 8.   $[Lis{t}_{open},P_p,t_p]\leftarrow \mathrm{FindInitialPose}\left(Lis{t}_{open}\right)$; 9.   $Lis{t}_{closed}\leftarrow \mathrm{AddNode}\left(Lis{t}_{closed},P_p,t_p\right)$; 10.   for each $N_c\in N_{all}$, do 11.    $\left[\sigma ,Lis{t}_{open}\right]\leftarrow \mathrm{SearchAStar}\left(T,N_c,P_p,P_f,t_p,Lis{t}_{open},Lis{t}_{closed},map\right)$; 12.   end for 13.  end while 14.  if $\sigma =1$, then 15.   $T\leftarrow \mathrm{GenerateTraj}\left(P_i,P_f,t_i\right)$; 16.   $F\leftarrow \mathrm{UpdateFillingState}\left(T\right)$; 17.  else 18.   $fail\leftarrow 1;$ 19.  end if 20. end while 21. return;

6. Numerical Experiments

Simulations of filling and emptying missions were conducted on the MATLAB 2021b platform, utilizing the parametric settings reported in

Table 3. Parametric settings regarding model and approach.

Parameter	Description	Setting
$n$	Forklift front overhang length	0.3 m
$m$	Forklift rear overhang length	1 m
$l$	Forklift wheelbase	1.5 m
$2b$	Forklift width	1 m
$\left[l{b}_x,u{b}_x\right]$	Horizontal boundaries of map	$\left[-18,18\right]$ m
$\left[l{b}_x,u{b}_y\right]$	Vertical boundaries of map	$\left[-12,12\right]$ m
$reso{l}_{xy}$	Node resolution for search algorithms	2 m
$ite{r}_{pre}$	Maximum iteration in the time dimension involved A* search	500
$ite{r}_{dispatch}$	Maximum iteration of redispatching	10
$ite{r}_{search}$	Maximum iteration in the improved A* search	5000
$\left\{C_1,C_2,\cdots ,C_{11}\right\}$	Calibration parameters	$\left\{6,1.5,80,-40,6,0.5,-4,2,0.5,0.5,3\right\}$
$\left\{T_1,T_2,\cdots ,T_{12}\right\}$	Modeled time lengths of maneuvers	$\left\{4,2,1.25,0.75,0.5,8,5,3,12,8,5,1\right\}$ s
$p_{turn}$	Penalty for turning maneuver	4
$p_{lane}$	Penalty for lane changing maneuver	6
$p_{inv}$	Penalty for speed inverse maneuver	6
$∆t_i$	Time postponed when one trajectory planning is failed	10 s
$∆t_{cover}$	$\mathrm{Time}$ in Equation (3)	20 s
$∆t_{entry}$	Time length after the virtual vehicle entering the rack passage to evaluate $∆t_{cover}$	20 s
$\left\{t_{claim},t_{unload}\right\}$	Time period for picking goods and unloading goods	$\left\{5,5\right\}$ s

. Each warehouse filling or emptying mission began with fully empty or filled initial conditions, respectively. The motion planning for each forklift in the simulation environment solely takes into account obstacles such as warehouse walls and other forklifts.

The simulation results are divided into two parts. The first part focuses on assessing the consistency of the motion planning strategy by presenting three short-time trajectories for multiple forklifts. In the second part, various dispatching strategies were benchmarked using different scores specifically designed for the current warehouse scenario. This evaluation was conducted to determine the performance and efficiency of different dispatching strategies in the given warehouse scenario.

6.1. On the Performance of the Trajectory Planning Technique

Figure 12 reports how forklift 1 returns to a picking station from shelf cluster $s_1$, where it has just unloaded goods. Before the start of the scenario, forklifts 2 and 4 are in the same state of return, and forklift 3 is unloading in shelf cluster $s_4$. When the scene in Figure 12 starts, forklift 1 accelerates and decelerates to reverse to the entrance of shelf cluster $s_1$. At the meantime, forklift 4 turns and heads to the picking station $p_4$, leaving the crossing of the wide passages empty. With the purple square showing the newly filled stack, forklift 3 leaves shelf cluster $s_4$ and enters $s_3$ to finish turning round. When it is inside of $s_3$, forklift 1 goes through the passage between $s_3$ and $s_4$. Subsequently, forklift 1 reverses and turns to the picking station $p_2$, and forklift 3 then enters the wide passages to return to the assigned picking station, which is $p_3$. Finally, forklift 2 is the first vehicle that arrives at the crossing area of wide passages with the goods picked. During the scenario, all trajectories are directly determined by means of the approach elaborated on in Section 4. The least priority is dynamically offered to the newly departed vehicle. For instance, in this scenario, forklift 3 initially has the lowest priority. Thus, it should judge if there is enough time for the vehicle to turn around. Therefore, if the time is limited, vehicle 3 would wait at the entrance of $s_4$, until the passing of forklift 1.

Figure 13 shows how the last stack is filled. Because there is nowhere to be filled hereafter, forklift 3 is parked in one picking station, and forklift 1 heads to picking station $s_4$ and stops. Initially, forklift 4 has just finished one unloading process, then it enters shelf cluster $s_2$ to turn over. After forklift 4 leaves $s_2$, forklift 2 enters the shelf cluster to complete the last delivery subtask. Meanwhile, forklift 4 accelerates, decelerates, and turns to picking station $s_3$ for final parking.

Figure 14 shows a scenario where the trajectory decision for forklift 3 encounters a failure. In this case, forklift 1 reaches shelf $s_4$ to unload goods, while forklift 3 completes its unloading process earlier than forklift 1. Consequently, forklift 3 should be assigned a higher priority to plan the trajectory based on function $\mathrm{rank}()$. However, at this moment, forklift 1 remains stationed at the entrance of shelf $s_4$, obstructing the passage for forklift 3. As a result, a decision failure occurs, leading to an update in the trajectory planning priority stored in $R$. Only after the completion of the trajectory planning for forklift 1 can the trajectory planning for forklift 3 proceed accordingly.

6.2. On the Performance of Dispatching Strategies

Table 4. Benchmark of the different dispatching strategies.

Strategy Name	Filling		Emptying
	Decision Failure Times	End Time (s)	Decision Failure Times	End Time (s)
ANN combined strategy	38	2388.25	27	2597.25
Comprehensive strategy	38	2441.00	21	2658.25
Greedy strategy	135	2768.75	75	3419.00
Traffic jam removing strategy	120	2709.00	51	3117.75
Balance strategy	55	2557.75	35	2660.50

benchmarks five distinct dispatching strategies for both filling and emptying tasks. It is worth noting that these dispatching methods are applicable only to specific coupling scenarios between grids and vehicle maneuvers, as they incorporate a preliminary A* search during the dispatching process.

The approaches differ in the assignment methods pertaining to delivering subtasks. While the decision failure times in Table 4 indicate the number of times $n_{current}$ or $t_i$ is changed without a solid trajectory decided throughout the entire warehouse filling mission, the end time is the finishing timing of the same task. Among the strategies, ANN combined determines the goal through $\arg \max \left(S_d\right)$ (cf. $S_d$ in Equation (9)) when the coefficients of the MLP are determined. Comprehensive strategy applies $S_{d,0}$ in Equation (3), and the target is determined based on $\arg \max \left(S_{d,0}\right)$. The other three strategies only concern a few parameters in Equation (3). Greedy, traffic jam removal, and balance strategies select the goal pose through $\arg \max \left(I_d\right)$, $\arg \max \left(J_{d,0}\right),$ and $\arg \max \left(G_d\right),$ respectively.

As reported in Table 4, the end time of the emptying task is approximately 10% longer than that for filling tasks. However, the occurrence of decision failure times is generally lower. This indicates that during emptying tasks, instances of a forklift obstructing routes and blocking other vehicles for extended periods are rare. Yet, with the current configuration of $ite{r}_{pre}$ and $ite{r}_{search}$, all vehicles might experience longer wait times on average during each subtask.

The ANN combined strategy is the optimal method in terms of end time for both filling and emptying tasks, requiring approximately 2% less time than the comprehensive strategy. However, it does exhibit comparable or even slightly greater decision failure times when compared to the comprehensive strategy. In general, a reduction in the first parameter can reduce the computation burden of the hardware by performing fewer useless calculations in searching a trajectory. The second parameter directly shows the efficiency of the cooperative operation of multiple forklifts. Notably, the ANN approach applied to train the dispatching system only concerns the efficiency of planning a trajectory for the current vehicle. Therefore, it is possible that some stacks in only one shelf cluster are left to be filled by selecting the best solution multiple times. In this case, multiple forklifts have to cooperatively fill one shelf cluster, and the waiting period of each vehicle must increase. Therefore, both the decision failure times and the end time may sharply increase.

In Figure 15, Figure 16, Figure 17 and Figure 18, the sequence in which the warehouse is filled with different methods is reported. The colored blocks plotted in the schematic warehouse stand for the stacks filled during different periods (red for the first quarter of time, blue for the second, green for the third, and cyan for the last). It is observed that the filling of different shelf clusters for the ANN combined strategy (cf. Figure 15) in different quarters of the period is balanced to a certain extent. There are, respectively, 35, 37, 34, and 30 stacks filled during each quarter. Particularly in the last quarter of the period, similar numbers of stacks filled are distributed within all six shelf clusters, and this provides a possibility for a feasible and fast solution to filling the warehouse.

By excluding the MLP of ANN, a comprehensive strategy is also capable of assigning subtasks to each forklift explicitly. Although the performance is slightly worse than the ANN combined one, it does not require the data pertaining to previous cycles. Figure 16 demonstrates the filling sequence for this approach. Similar to that of the ANN combined approach, the filling state of the shelf clusters in the warehouse is relatively balanced throughout the period. A few seconds are added in terms of the end time for this strategy without the fine adjustment of the MLP.

Figure 17 exhibits the filling sequence of the traffic-jam-removing strategy. The difference between this method and the greedy strategy is that the previous one simultaneously considers $H$ in Equation (3). One stack in shelf cluster $s_2$ is filled in the first quarter of the period, and both the decision failure times and the end time pertaining to the traffic jam removal approach reduce compared to the greedy strategy. Thus, a simple additional function of $H$ can slightly further characterize the difficulty of the trajectory planning. Meanwhile, because the filling sequences of these two strategies are similar, the figure pertaining to the greedy method is omitted. As shown in Figure 17, it basically evenly fills shelf clusters $s_4$, $s_5$, and $s_6$ in the first quarter of the period. The majority of stacks in $s_1$ and around a third of stacks in $s_2$ are filled in the last quarter. Considering four vehicles cooperatively delivering goods, violations between trajectories evidently arise, and the times of failure in trajectory decisions and the waiting time can increase markedly.

The filling sequence of the balance strategy is plotted in Figure 18. The filling of different shelf clusters is nearly perfectly balanced. Evidently, this strategy can better arrange the trajectories to fill the last few stacks of the warehouse compared to any other approach. Therefore, the number of stacks filled during the last quarter of the warehouse filling cycle is comparable to that during other periods. In particular, when the 5th, 14th, and 23rd stacks of shelf clusters $s_3$ and $s_4$ (cf. Figure 2) are being filled, the paths between the shelf clusters ($s_1$ and $s_2$) in the left side of the warehouse (cf. Figure 1) and picking stations are cut. Meanwhile, the first delivery vehicle reaching these certain stack locations between shelf clusters $s_3$ and $s_4$ can simultaneously block the routes to certain shelf clusters. This phenomenon also appears in shelf clusters $s_5$ and $s_6$ when the identical stack indexes are considered. Those are the unique conflicts that arose to cause the failure decisions and the lengthening of the end time for the balance strategy.

7. Conclusions

This study presents a joint dispatching and cooperative trajectory planning framework. In this framework, the dispatching method applies a time dimension involved A* search, which is used to score different goal poses during a delivery subtask. ANN is simultaneously implemented to evaluate the difficulties for a forklift to arrive at a goods unloading pose from a picking station. In addition, the stacks to be filled pertaining to different shelf clusters are used as the third parameter to balance the shelf filling states, with the aim of avoiding deadlocks at the final stage of the mission.

As far as the trajectory planning approach within the framework is concerned, a model-based improved A* search algorithm is used to sequentially determine the trajectory of each vehicle without further optimization-based techniques. The node expansion manners are determined on the basis of the purpose of a maneuver, and the speed pertaining to the start and end of a maneuver is divided into stages.

Different dispatching strategies are benchmarked. The ANN combined strategy shows the best performance in warehouse filling efficiency, but the decision failure times of this method are comparable to those of the comprehensive strategy. Meanwhile, it is observed that the balance of the filling state pertaining to shelf clusters is as important as the evaluation of the goal-reaching difficulties. Furthermore, the trajectories of multiple vehicles in a short time are presented. It is shown that although the priority of a vehicle during one subtask is predefined, the vehicles are capable of cooperatively and continuously finding feasible trajectories.

In the future, our joint strategy will be tested in a downsized unmanned warehouse setup. Additionally, we aim to implement a zonal shutdown feature for enhanced safety in case pedestrians enter the area. We also plan to integrate a forklift failure detection mechanism to enable uninterrupted warehouse operations even in the presence of a few malfunctioning forklifts.