A Hierarchical Trajectory Planning Algorithm for Automated Guided Vehicles in Construction Sites

Abstract

Herein, to address the challenges faced by Automatic Guided Vehicles (AGVs) in construction site environments, including heavy vehicle loads, extensive road search areas, and randomly distributed obstacles, this paper presents a hierarchical trajectory planning algorithm that combines coarse planning and precise planning. In the first-level coarse planning, lateral and longitudinal sampling is performed based on road environment constraints. A multi-criteria cost function is designed, taking into account factors such as deviation from the road centerline, shortest path cost, and obstacle collision safety cost. An efficient dynamic programming algorithm is used to obtain the optimal path. Considering nonholonomic constraints of vehicles, eliminating inflection points using improved B-Spline path fitting, and a quadratic programming algorithm is proposed to enhance path smoothness, completing the coarse planning algorithm. In the second-level precise planning, the coarse planning path is used as a reference line, and small-range sampling is conducted based on AGV motion constraints, including lateral displacement and longitudinal velocity. Lateral and longitudinal polynomials are constructed. To address the impact of randomly appearing obstacles on vehicle stability and safety, an evaluation function is designed, considering factors such as jerk and acceleration. The optimal trajectory is determined through collision detection, ensuring both safe obstacle avoidance and AGV smoothness. Experimental results demonstrate the effectiveness of this method in solving the path planning challenges faced by AGVs in construction site environments characterized by heavy vehicle loads, extensive road search areas, and randomly distributed obstacles.

1. Introduction

Automatic Guided Vehicles (AGVs) are autonomous navigation devices driven by planning algorithms [1,2,3,4]. In recent years, in the construction site environment, AGVs have demonstrated their autonomy and have been used to replace manual labor for flexible material transportation tasks, resulting in cost savings and improved work efficiency [5,6,7]. However, the challenges of heavy vehicle loads, extensive road search areas, and randomly appearing obstacles in construction sites have presented new hurdles for AGV applications in this context, prompting researchers to focus more on path planning techniques tailored to construction site environments [8,9].

Path planning technology is a method that plans an optimal path based on given target point positions and road conditions [10,11,12,13]. By reviewing the research on path planning algorithms in new scenarios of road in recent years, two main research approaches have been summarized. Firstly, combining two or more path planning algorithms, leveraging the strengths of each algorithm, to design path planning algorithms tailored to specific scenarios [14,15,16]. For instance, in reference [17], a global path planning algorithm based on an improved A* algorithm and a local path planning algorithm based on an improved artificial potential field algorithm were proposed, achieving theoretical validation for precise obstacle avoidance and rapid navigation of indoor mobile robots. Secondly, by improving a known algorithm and designing heuristic functions, cost functions, etc., tailored to specific scenarios to achieve effective planning results [18,19,20,21]. For example, in reference [22], a Target-Optimized JPS algorithm was introduced, which constructs search direction priorities based on target point direction vectors and also improves the cost function to obtain shorter paths, reducing computation time.

Dynamic programming techniques based on the greedy approach are well-suited for path planning on large, wide roads [23,24,25]. For example, in reference [26], a solution was proposed that uses dynamic programming to inspire continuous state spaces and obtain globally optimal path planning. In reference [27], a static obstacle avoidance algorithm based on a grid map was introduced, generating feasible paths by considering the weight of each obstacle and then optimizing these feasible paths through dynamic programming to find several shortest paths. However, these dynamic programming solutions solely focus on finding the shortest path and do not consider the high load conditions of AGVs, resulting in paths without speed information. On the other hand, Frenet sampling planning decouples the lateral and longitudinal aspects of planning and includes speed information in the planned trajectory [28,29,30]. As shown in reference [31], it uses the global planning path as a reference line and decouples the local trajectory in the Frenet coordinate system both horizontally and vertically. By considering the designed endpoint sampling rules and taking speed planning into account, it finds the optimal trajectory. However, this method has a higher sampling cost in wide road scenarios.

After analyzing the aforementioned works, it was observed that there are currently very few path planning algorithms specifically designed for construction site environments. Existing algorithms struggle to balance high-speed calculation and extensive sampling range. Therefore, this paper proposes a hierarchical trajectory planning method that integrates road environment constraint sampling (coarse planning) and AGV motion constraint sampling (precise planning). This approach leverages the respective strengths of coarse planning and precise planning and tailored to the specific environment of construction sites, designs cost functions.

The rest of this paper is organized as follows. Section 2 introduces the coarse planning method based on road environment constraint sampling. Section 3 discusses the precise planning algorithm based on motion constraint sampling, and its integration with the coarse planning algorithm to form a hierarchical trajectory planning algorithm. Section 4 conducts experimental tests and data analysis to verify the practical effectiveness and feasibility of the algorithm. Section 5 provides an overall summary.

2. Coarse Planning Algorithm

In the construction site environment, the extensive road search area requires the collection of a large amount of data for both lateral and longitudinal sampling, creating a significant computational burden. To address the challenge of enabling AGVs to quickly compute an accurate, safe, and efficient planning path, this section introduces a dynamic programming algorithm based on the greedy approach. By considering safety, shortest path, and other metrics, a multi-objective evaluation function is designed to efficiently obtain the optimal path. Nonholonomic vehicle constraints are also taken into account, with interpolation at the starting and ending points and the use of the B-Spline algorithm to fit the optimal path and eliminate sharp turns. Given the high load of AGVs, which requires road smoothness, the fitted path is further improved through quadratic programming.

2.1. Lateral and Longitudinal Sampling under Road Environment Constraints

The sampling for coarse planning is based on the S-L coordinate system of the road, with road environment constraints taken into account. It considers road boundaries, the size and positions of obstacles, and the road centerline for sampling. Laterally, uniformly distributed sampling points from the road’s centerline towards both sides at a fixed interval of $\Delta l$ until reaching the road boundary L_a. Longitudinally, it uniformly distributes sampling points along the road centerline with a fixed $\Delta s$, setting a fixed sampling length Sa. Sampling points are filtered based on the size and position of obstacles, using the obstacle’s center point as the origin and extending outward by dobs to remove sampling points near the obstacle, improving search efficiency. In scenarios with multiple obstacles, this sampling method significantly reduces computational complexity. Additionally, before planning, if it is determined that there are no available sampling points in the s-direction due to obstacles, it can quickly identify an impassable path and initiate a stopping operation, thus enhancing the safety of the AGV. The specific sampling process is illustrated in the following figure (Figure 1). The blue points represent the sampling points, and the red point represents the target points.

2.2. Selecting the Optimal Path

Traditional dynamic programming algorithms focus solely on finding the shortest path [32]. However, in the construction site environment, we besides considering path length, it is essential to take into account the influence of obstacles in the construction site and keep the path as close to the road centerline as possible. Then we can accurately assess the cost between two nodes. This paper proposes a multi-objective evaluation function to evaluate the cost between nodes. In addition to the cost of the shortest path, we introduce costs related to deviation from the reference line and obstacle collision safety. These three aspects are considered to provide a diversified evaluation of a path. The implementation method is as follows.

Linear interpolation is performed between two points on adjacent s-layers to obtain several interpolated points. The cost of the shortest path is determined by calculating the cumulative distance between all interpolated points on the s-layers, providing the length cost of the planned path. The cost of deviation from the reference line is characterized by the cumulative sum of $l^2$ values of all interpolated points, taking advantage of the properties of the Frenet coordinate system. For obstacle safety cost, a segmented calculation approach is employed. If the distance between the planning point and the obstacle is greater than 2 m, it is considered sufficiently safe, and no cost is calculated. If the distance is less than 0.5 m, it is considered impassable, resulting in an infinite cost. If the distance falls within the range of 0.5 m to 2 m, the cost is represented by the sum of the squared distances between the interpolated points and the obstacle.

Given the coordinates of interpolated points between two points as $\left(s_1,l_1\right),\left(s_2,l_2\right)\dots \left(s_n,l_n\right)$ and the coordinates of obstacles as $\left(s_{obs1},l_{obs1}\right),\left(s_{obs2},l_{obs2}\right)\dots \left(s_{obsn},l_{obsn}\right),$ the cost equation for the path between these two points is calculated as follows:

$$\left\{\begin{array}{l}cos{t}_1={\displaystyle \sum _{i=1}^{n-1}\sqrt{(s_{i+1}-s_i{)}^2+(l_{i+1}-l_i{)}^2}}\\ cos{t}_2={\displaystyle \sum _{i=1}^n{c}_i{l_i}^2}\\ cos{t}_3=\left\{\begin{array}{l}0(\left|di{s}_{obsi}^{}\right|>2)\\ {\displaystyle \sum _{i=1}^n{c}_{i}di{s}_{obsi}^2(2>\left|di{s}_{obsi}^{}\right|>0.5)}\\ \infty (0.5>\left|di{s}_{obsi}^{}\right|)\end{array}\right.\\ c_i=(d{)}^{s_i-s_n},0<d<1\\ di{s}_{obsi}^{}=\sqrt{{(s_{obsi}-s_i)}^2+{(l_{obs}-l_i)}^2}\\ cost=k_{1}cos{t}_1+k_{2}cos{t}_2+k_{3}cos{t}_3\end{array}\right.,$$

(1)

where $cos{t}_1$ represents the cost associated with the shortest path, $cos{t}_2$ represents the cost associated with deviation from the reference line, $cos{t}_3$ represents the cost associated with ensuring safety against collisions with obstacles, and $di{s}_{obsi}$ represents the distance of the ith point from the nearest obstacle. $c_i$ is an increasing function, and we are more concerned about the final operating state of the vehicle. Therefore, the proportion of trajectory points at the beginning of the planning is not as high as near the endpoint.

As shown in the Figure 2a, each sampling point represents a node, and nodes with the same s-coordinate are grouped into the same layer. Starting from the last layer, we traverse all the points sampled in the l-direction for the same s layer, recording the minimum cost value for each point to reach the endpoint after visiting all points in the next layer in the s-direction. The cost equation is as follows:

$$cos{t}_{min}=min\{cos{t}_{p_1}+c_1,cos{t}_{p_2}+c_2,\dots \dots cos{t}_{p_i}+c_i\},$$

(2)

where $cos{t}_{pi}$ represents the cost from the current node to the next-level target point, and $c_i$ represents the minimum cost from the next-level node to the endpoint.

It continues until reaching the first layer in the s-direction, finding the optimal path with the minimum cost for the vehicle to reach point D, as shown in Figure 2b.

2.3. Curve Fitting and Optimization

As shown in Figure 3a below, in order to ensure planning efficiency, the chosen values of $\Delta s$ and $\Delta l$ in dynamic programming cannot be excessively small. This results in relatively large spacing between the output path points generated by the algorithm. Consequently, the planned path often exhibits significant inflection points, lacking the desired smoothness. Thus, the resultant path is typically composed of line segments.

We certainly find it imperative to engage in curve fitting. However, if we exclusively apply curve fitting solely to the sampled points, we overlook the non-holonomic constraints posed by the initial and final velocity directions of the AGV. Consequently, while the fitted curve adeptly mitigates the issue of inflection points, it falls short of ensuring that the vehicle’s velocity direction aligns with the planned trajectory direction upon reaching the endpoint. Alternatively, at the starting point, the vehicle may encounter challenges in tracking the planned curve due to velocity direction issues, as depicted in Figure 3b below.

Utilizing the fitting characteristics of B-Splines, this paper proposes a method that, while adhering to the traditional properties of B-Spline fitting, interpolates the coarse planning path to ensure that the tangent direction at the start point of the planned path aligns with the initial velocity direction of the vehicle, and the tangent direction at the end point matches the terminal velocity direction of the vehicle, as shown in the following Figure 3c.

As depicted in Figure 4, the blue points are selected based on the multi-objective evaluation function, the yellow points are interpolation points, and the red points is the endpoints. Since the starting point of the B-Spline fitting curve lies on the line connecting the first and second sampled points and is tangent to that line, to ensure that the tangent direction at the starting point of the B-Spline fitted curve aligns with the vehicle’s velocity direction, a new virtual starting sample point, denoted as 1.1, is inserted after sample point 1. The insertion is made at a distance of $d_a$. Similarly, to ensure that the tangent direction at the endpoint of the fitted B-Spline curve corresponds to the vehicle’s final velocity direction, a new virtual endpoint sample point, denoted as N.1, is inserted before the last sample point. This insertion is made at a distance of $D_a$ along the extension of the vehicle’s final velocity direction. The B-Spline fitted curve’s endpoint lies on the line connecting the last sample point and the second-to-last sample point and is tangential to that line.

The equation for calculating ${\mathrm{D}}_a$ and ${\mathrm{d}}_a$ are as follows:

$$\left\{\begin{array}{l}{\mathrm{D}}_a=\mathrm{D}/n_1\\ {\mathrm{d}}_a=\mathrm{d}/n_2\end{array}\right.,$$

(3)

where $\mathrm{d}$ represents the distance between sample points 1 and 2, $\mathrm{D}$ represents the distance between sample points n − 1 and n. $n_1$ and $n_2$ are interpolation weighting coefficients.

To ensure the quality of the fitted curve while considering the velocity direction, it is important to choose appropriate values for $n_1$ and $n_2$. Taking the interpolation position of n.1 as an example, as illustrated in Figure 5, different values of $n_2$ can result in significant differences in the fitting performance. Factors such as the alignment of the fitted curve with the original path points, the length of the fitted curve, and its smoothness vary with different values of $n_2$. Therefore, it is necessary to determine the optimal values for $n_1$ and $n_2$ to ensure the best fitting results.

As Section 2.1 involved the filtering of sampling points close to obstacles based on road environment sampling, and Section 2.2 considered obstacle distance as one of the cost functions for optimal path planning, there is no need to consider the obstacle cost function in the small-scale optimization during curve fitting. In contrast, our primary focus is on the smoothness, shortest path, and alignment with sampling points of the planned path. Therefore, considering the original coordinates as $P_{k\_ref}$ and the optimized coordinates as $P_k$ we can improve Equation (2) as follows:

$$\left\{\begin{array}{l}{P}_k=\left[\begin{array}{c}{x}_k\\ y_k\end{array}\right]\\ cos{t}_a=v_1{{\displaystyle \sum _{k=1}^{n-2}‖2P_k-P_{k-1}-P_{k+1}‖}}_2^2\\ cos{t}_b=v_2{{\displaystyle \sum _{k=0}^{n-2}‖P_{k+1}-P_k‖}}_2^2\\ cos{t}_c=v_3{{\displaystyle \sum _{k=0}^{n-1}‖P_k-P_{k\_ref}‖}}_2^2\end{array}\right.,$$

(4)

where $cos{t}_a$ represents the smoothness cost, $cos{t}_b$ represents the length cost, and $cos{t}_c$ represents the deviation cost relative to the original points. $v_1$, $v_2$, and $v_3$ are cost coefficients.

The value ranges for $n_1$ and $n_2$ are defined as follows:

$$\left\{\begin{array}{l}{N}_1=[1,2,3\dots n_z]\\ N_2=[1,2,3\dots n_z]\end{array}\right.,$$

(5)

By traversing $N_1$ and $N_2$ at different values in Equation (5), the cost calculated according to Equation (4) will be obtained. The optimal values for $n_1$ and $n_2$ will be selected based on the minimum cost value. After obtaining the best $n_1$ and $n_2$, the entire path from the coarse planning phase will undergo quadratic programming using the same parameter criteria as defined in Equation (4).

The variable set P representing the entire path is defined as follows:

$$P=\left[x_1{y}_1\dots x_n{y}_n\right],$$

(6)

Based on Equations (4) and (6), the expressions for the smoothness cost $W_1$, length cost $W_2$, and similarity cost $W_3$ can be derived as follows:

$$\left\{\begin{array}{l}{A}_1^T=\left[\begin{array}{cccccc}1& 0& & & & \\ 0& 1& 1& 0& & \\ -2& 0& 0& 1& 1& 0\\ 0& -2& -2& 0& 0& 1\\ 1& 0& 0& -2& -2& 0\\ 0& 1& 1& 0& 0& -2\\ & & 0& 1& 1& 0\\ & & & & 0& 1\\ & & & & & \\ & & & & & \end{array}\begin{array}{cc}& \\ & \\ & \\ & \\ & \\ & \\ & \\ & \\ \mathrm{O}& \\ & \mathrm{O}\end{array}\right],A_2^T=\left[\begin{array}{cccccc}1& 0& & & & \\ 0& 1& & & & \\ -1& 0& 1& 0& & \\ 0& -1& 0& 1& & \\ & & -1& 0& & \\ & & 0& 1& & \\ & & & & \mathrm{O}& \\ & & & & & \mathrm{O}\end{array}\right]\\ R=\left[\begin{array}{c}-2x_{1\mathrm{r}}\\ -2y_{1\mathrm{r}}\\ ⋮\\ -2x_{nr}\\ -2y_{nr}\end{array}\right],A_3^T=\left[\begin{array}{ccccc}1& & & & \\ & 1& & & \\ & & 1& & \\ & & & \mathrm{O}& \\ & & & & 1\end{array}\right],\left\{\begin{array}{l}{W}_1=w_1{P}^T{A}_1^T{A}_{1}P\\ W_2=w_2{P}^T{A}_2^T{\mathrm{A}}_{2}P\\ W_3=w_3{P}^T{A}_3^T{A}_{{}_3}\mathrm{P}+{\mathrm{R}}^{\mathrm{T}}\mathrm{P}\end{array}\right.\end{array}\right.,$$

(7)

With the above derivations, the cost function can be modified as follows:

$${\mathit{cos}\mathit{t}=\mathit{P}}^T(W_1{\mathrm{A}}_1^T{\mathrm{A}}_1+W_2{\mathrm{A}}_2^T{\mathrm{A}}_2+W_3{\mathrm{A}}_3^T{\mathrm{A}}_3)\mathit{P}+W_3{H}^{T}P,$$

(8)

The quadratic programming equation is as follows:

$$x^T(\frac{H}{2})x+f^{T}x,$$

(9)

Combining Equations (7)–(9), we can obtain:

$$\left\{\begin{array}{l}H=2(w_1{A}_1^T{A}_1+w_2{A}_2^T{A}_2+w_3{A}_3^T{A}_3)\\ f^T=w_3{H}^T\end{array}\right.,$$

(10)

To prevent excessive impact on the B-spline fitting due to the quadratic programming, interval tolerances are introduced. The tolerance set B is defined as follows:

$$\left\{\begin{array}{l}\left|P_k-P_{k-ref}\right|<B\\ B=[b_1,b_2\dots b_{n1}\dots b_{n2}\dots b_n]\\ b_1,b_2\dots b_{n1}=b_{n2}\dots b_n=b_{\min }\\ b_{n1+1}\dots b_{n2-1}=b_{\max }\end{array}\right.,$$

(11)

This is performed to prevent the quadratic programming from adversely affecting the fitting of the improved B-spline. With this, Equations (6)–(11) are incorporated into the quadratic programming solver for iterative solution, yielding the optimal coarse planning path.

3. Precise Planning Algorithm

The coarse planning phase is used to obtain a high-quality planning path with relatively low computational resources. However, the planned path does not include velocity information. In construction environments, AGVs often carry heavy loads, and planning without velocity information can be highly dangerous under high-load conditions. Additionally, the B-Spline fitting and quadratic optimization used in coarse planning make minor adjustments to the path points obtained from dynamic planning. Therefore, collision safety detection is still necessary. In this section, we propose a precise planning algorithm based on the results of coarse planning. This algorithm uses the coarse planning results as a reference line and samples in the longitudinal direction based on velocity and in the lateral direction based on lateral displacement. It considers additional factors like jerk to ensure the smoothness of AGV motion under velocity planning. Moreover, it includes collision detection to achieve a safe and high-quality trajectory planning solution. Implementing the precise algorithm on the basis of coarse planning significantly reduces the sampling count, thereby addressing the drawback of high computational load associated with precise planning.

3.1. Construction of Trajectory Polynomials under Motion Constraint Sampling

In the Frenet coordinate system, the planning problem involves a total of three dimensions, namely (s, d, t). This high-dimensional optimization problem can be decoupled into two independent optimization problems in the lateral (d, t) and longitudinal (s, t) directions, which are sampled and processed separately.

In accordance with the hardware specifications, environmental constraints, safety considerations, and other factors related to the AGV, there are boundary constraints on the lateral displacement (d), velocity (v), acceleration (a), jerk (j), and planned curvature (k). The lateral and longitudinal sampling must adhere to the following motion constraints.

$$\left\{\begin{array}{l}d=\left\{\left.d\mid \begin{array}{c}d\in [d_{last}-v_{\max }\Delta t,d_{last}+v_{\max }\Delta t]\\ d\in [-d_{\max },d_{\max }]\end{array}\right\}\right.\\ v=\left\{\left.v\mid \begin{array}{c}v\in [v_{last}-a_{\max }\Delta t,v_{last}+a_{\max }\Delta t]\\ v\in [-v_{\max },v_{\max }]\end{array}\right\}\right.\\ a=\left\{\left.a\mid \begin{array}{c}a\in [a_{last}-j_{\max }\Delta t,a_{last}+j_{\max }\Delta t]\\ a\in [-a_{\max },a_{\max }]\end{array}\right\}\right.\\ j\in [-j_{max},j_{max}]\\ k\in [-k_{max},k_{max}]\\ \Delta t\in [T_{min,}{T}_{max}]\end{array}\right.,$$

(12)

Lateral sampling involves the vehicle starting from the reference line and sampling on both sides of the vehicle at intervals of $\Delta d$ while adhering to the motion constraints defined in Equation (12). This process continues until sampling reaches the road boundary. After completing this lateral sampling, the next step is to sample in the time domain with intervals of $\Delta t$. When the sampling point $D_1$, initial time $t_0$, and planning time $t_1$ are determined, a fifth-degree polynomial is constructed for the relationship between time ($t$) and lateral displacement ($d$) as follows:

$$d(t)=a_0+a_{1}t+a_2{t}^2+a_3{t}^3+a_4{t}^4+a_5{t}^5,$$

(13)

The fifth-degree polynomial for lateral sampling can be solved given the known initial position $D_0=\left[d_0,{\dot{d}}_0,{\ddot{d}}_0\right]$, final position $D_1=\left[d_1,{\dot{d}}_1,{\ddot{d}}_1\right]$ and time instants $t_0$ and $t_1$.

Under the conditions specified in Equation (12), the longitudinal (s) sampling is slightly different. Instead of using $\Delta s$ for longitudinal sampling, it uses a given target velocity $v_t$. Setting $\Delta v$ with $v_t$ as the reference and sampling $n_v$ velocities on both sides to obtain v can be expressed as follows:

$$v=v_t+n△v,n\in [-n_v,n_v],$$

(14)

Subsequently, time $t$ is sampled at intervals of $\Delta t$. Upon completing the sampling of both velocity and time, it is tantamount to having accomplished the sampling of $s$. The advantage of this approach is that, since the sampled values represent velocities, it facilitates the planning of velocity profiles. Consequently, a quartic polynomial concerning velocity and time is constructed as follows.

$$v(t)=b_0+b_{1}t+b_2{t}^2+b_3{t}^3+b_4{t}^4,$$

(15)

Analogous to lateral sampling planning, this quartic polynomial can be resolved, thereby concluding the solution process for the current planning iteration.

Upon the resolution of both lateral and velocity sampling polynomials, an assortment of trajectories is generated through the permutation and combination of each lateral sampling and longitudinal velocity sampling under the same planning timeframe. The series of trajectory clusters generated by sampling are shown in the following figure (Figure 6). The dark blue line represents the centerline of the road, the purple line represents the sampling trajectory, and the green points represent the trajectory points selected for planning.

3.2. Selection of the Optimal Trajectory

The evaluation function for precise planning is set up from three aspects, driving stability, conformity with the reference line, and convergence effect of the desired speed. Paths with potential collision risks are excluded through collision detection, thus obtaining the Optimal Trajectory. Driving stability is characterized by the absolute values of the longitudinal and lateral jerk throughout the vehicle’s planned trajectory, ensuring that high-load AGVs can smoothly operate while avoiding obstacles at construction sites or during acceleration and deceleration. The degree of conformity to the desired path is represented by the absolute value of the d-coordinate in the Frenet frame of the planned path points, aiming to align the AGV as closely as possible with the desired path tracking. The convergence effect on the desired speed is represented by the squared difference between the planned speed and the desired speed, ensuring that the vehicle can converge to the desired speed relatively quickly. The specific calculation equation is as follows:

$$\left\{\begin{array}{l}cos{t}_j={\displaystyle \sum _{i=0}^n(\left|\stackrel{···}{d_i}\right|+\left|\stackrel{···}{s_i}\right|)}\\ cos{t}_d={\displaystyle \sum _{i=0}^n{d}_i}\\ cos{t}_s={(v_{t\arg et}-v_{end})}^2\end{array}\right.,$$

(16)

where $cos{t}_j$ represents the cost of stability, $cos{t}_d$ represents the cost of conformity with the desired path, and $cos{t}_s$ represents the cost of conformity with the desired speed.

The calculation equation is as follows ($c_1$, $c_2$, and $c_3$ as cost coefficients).

$$cost=c_{1}cos{t}_j+c_{2}cos{t}_d+c_{3}cos{t}_s,$$

(17)

During actual testing, it was observed that when facing obstacles at construction sites, the vehicle might frequently encounter situations where it is positioned between two trajectories with similar costs. This leads to frequent replanning of the vehicle’s trajectory, resulting in variability in the trajectory planning and causing instability in the vehicle’s movement, as illustrated in the following Figure 7. Due to similar costs, the AGV may frequently switch optimal trajectories among the following three trajectories.

To address this phenomenon, a new cost function is introduced as follows:

$$cos{t}_l=c_4{\displaystyle \sum _{\mathrm{i}=0}^n{(d_{1\_i}-d_{1\_last\_i})}^2},$$

(18)

where $cos{t}_l$ represents the similarity between the starting point of the current planned path and that of the previous frame’s planned path.

Under the premise that other paths do not significantly differ from the previous frame’s planned path, to ensure vehicular stability, a preference is given to planning results similar to the previous frame’s plan. This approach effectively circumvents the issue of repetitive planning for AGVs under critical cost function scenarios. Consequently, the comprehensive evaluation equation for precise planning is as follows.

$$cost=c_{1}cos{t}_j+c_{2}cos{t}_d+c_{3}cos{t}_s+c_{4}cos{t}_l,$$

(19)

Although the coarse planning algorithm considers obstacle distances and sets a safe distance, to prevent minor adjustments in the planned trajectory due to curve fitting in Section 2.3 and precise planning in Section 3.1, collision detection is still necessary to ensure the safety of the trajectory. All trajectories are sorted from lowest to highest cost according to Equation (18). The obstacle collision detection starts with the trajectory having the lowest cost, and the detection process is illustrated in the following figure (Figure 8). The black rectangle represents obstacles, and the red circle represents the minimum circumscribed circle of obstacles.

By iterating through all points of the trajectory output from the precise planning process, the trajectory point closest to the obstacle is identified, and the minimum distance $di{s}_{save}$ is calculated. Based on the dimensions of the obstacle obtained from the perception module, the minimum circumscribed circle radius $r_{obs}$ of the obstacle is determined. $L_{veh}$ represents the width of the vehicle’s front. A trajectory is deemed safe if it satisfies the following condition; otherwise, the next trajectory is examined. The safety determination condition is as follows:

$$di{s}_{min}>\frac{L_{veh}}{2}+r_{obs}+di{s}_{save},$$

(20)

If the trajectory is found to be free of collisions with obstacles, it is selected and outputted. Otherwise, the next trajectory is examined until a suitable one is identified. If no trajectory meets the safety requirements, the vehicle will stop and trigger an alarm.

3.3. The Hierarchical Trajectory Planning Algorithm Based on Coarse and Precise Planning

Section 2 describes a coarse planning algorithm based on a greedy approach. Although it is computationally simple and efficient, ensuring smooth path processing, this algorithm does not take into account velocity planning. In construction site environments, where obstacles may appear randomly, the absence of velocity planning is considerably unsafe. High-load AGVs must consider the impact of changes in velocity.

The precise planning algorithm described in this chapter, while incorporating velocity planning, still requires a substantial amount of sampling calculations for wide roads and larger environments at construction sites. It necessitates frequent construction and solving of quintic and quartic polynomials. Moreover, the decoupled lateral and longitudinal planning, coupled through time, demands permutation and combination, resulting in a significant computational load.

This paper proposes a hierarchical trajectory planning algorithm for construction sites, and the specific implementation process is as follows:

(1)

According to Section 2.1, under road environmental constraints, starting from the centerline of the road, a smaller Δs and Δl for sampling, remove unnecessary sampling points based on obstacles to improve sampling efficiency.

(2)

According to Section 2.2, consider the cost of path length, obstacle collision safety, and deviation from the reference line to calculate the cost function between nodes in adjacent columns. Then, based on greedy thinking, find the optimal path.

(3)

According to Section 2.3, the optimal path point is fitted and optimized. Using the improved B-Spline for fitting, eliminate inflection points while considering the non-holonomic constraints of the VGA at the starting and ending points. A quadratic programming equation is constructed considering smoothness, shortest path, and similarity with the original path. Then, the coarse planning has been completed as shown in Figure 9.

(4)

According to Section 3.1, horizontal and vertical sampling are carried out based on the motion constraints of AGV, and lateral fifth and horizontal fourth degree polynomials are constructed separately. The reference line of the second layer planning algorithm no longer uses the road centerline as the reference line but uses the result of coarse planning as the reference line. A smaller sampling width L is used to perform small-scale horizontal sampling on the results of coarse planning. Combine horizontal and lateral trajectories at the same sampling time to form trajectory bundles.

(5)

According to Section 3.2, from driving stability, conformity with the reference line, the rate of convergence from the expected speed and the degree of conformity with the previous trajectory is used to calculate the cost for each trajectory. Starting from the trajectory with the lowest cost, collision detection is performed sequentially. Output is the trajectory with the lowest cost and satisfying collision detection. Then, the Precise planning algorithm has been completed as shown in Figure 10 (The blue line represents the sampling boundary, the red line represents the result of coase planning as a reference line).

The hierarchical planning algorithm process is depicted in the Figure 11.

In terms of operational efficiency, coarse planning utilizes the idea of greedy algorithms, reducing the time complexity from $O\left(n^2\right)$ in traditional dynamic programming algorithms to $O\left(n\right).$ Precise planning relies on the rough solutions of coarse planning, with only a number of three to five horizontal samples. Therefore, the time complexity of precise planning is reduced from $O\left(n^2\right)$ to $O\left(n\right)$, resulting in a time complexity of $O\left(n\right)$ for the entire hierarchical planning algorithm. It can meet the demand for algorithm efficiency for the wide road area in the construction site environment.

In terms of safety, the sampling points near obstacles are filtered in Section 2.1. As shown in Equation (1) of Section 2.2, the cost function of coarse planning requires the planned trajectory to be far away from obstacles and set a minimum distance of 0.5 m. The collision detection conducted in Section 3.2 prevents potential safety hazards due to the curve fitting optimization in Section 2.3, ensuring the safety of the planned path.

In terms of trajectory smoothness, the improved B-spline fitting takes into account the non-holonomic constraints of AGV, eliminates trajectory inflection points and smooths and optimizes the path through quadratic programming. And precise planning ultimately obtains a smooth trajectory based on indicators such as lateral and longitudinal jerk.

4. Experimental Validation

In this section, experiments were conducted using an AGV built by Weihai Tiante Intelligent Technology Co., Ltd. (Weihai, China) as the experimental platform, as shown in Figure 12. The exact specifications of the AGV are presented in

Table 1. The exact specifications of the AGV.

Params	Value
AGV Dimensions	2000 × 1000 × 950 mm
AGV Mass	≤800 kg
Max Load	1000 kg
Wheel Specifications	165/55R13
Max Speed	1.5 m/s
Min Turning Radius	1000 mm
Max Climbing Slope	20°
Braking Distance	<20 mm

. Consequently, the velocity planning range should be within 0 to 1.5 (m/s), while the maximum curvature allowed is specified as 1 m⁻¹.

The experiments were carried out in the construction site environment at Guangzhou Baiyun Airport, as shown in the following figure. The operating road within this environment constitutes a temporary or semi-permanent structure. The width of the road is typically ranging from 6 to 10 m or wider. In the experiment, the boxes were used to simulate the obstacles encountered by AGV, as shown in the following figure (Figure 13).

Experiments involved variations in vehicle positions, target point locations, initial velocities, target velocities, and other parameters. These experiments were designed to test and validate the effectiveness of the hierarchical planning proposed in this paper. The stability and reliability of the algorithm was also verified. Representative experimental results were selected for detailed analysis and illustration.

4.1. Experimental System

As shown in Figure 14, Figure 15, Figure 16 and Figure 17. The industrial computer used in this experiment is the NVIDIA AGX Orin 64G. The depth camera (Stereolabs Technologies Inc., San Francisco, CA, USA; Beijing, China) was employed as the obstacle perception sensor, while a laser radar (DJI, Shenzhen, China) and INS570D inertial navigation components (Guangdong Daoyuan Electronic Technology Co., Guangzhou, China) served as localization sensors. These sensors were chosen for experimental testing of the planning algorithm in this paper, under the condition that the perception algorithms meet all the requirements of the experiment.

Detailed information about industrial computers and sensors are shown in the tables below (

Table 2. JETSON AGX ORIN information.

Ai	275TOPS
GPU	2048-core NVIDIA Ampere architecture GPU with 64 Tensor Cores
CPU	12-core Arm๏ Cortex๏. A78AE v8.2 64-bit CPU 3MBL2+6MBL3
OS	ubuntu 20.04

,

Table 3. Livox-MID-70 information.

Wavelength	905 nm
Blind Area	0~0.05 m
FOV	70.4°
Angle Error	<0.1°

,

Table 4. ZED information.

Resolution Ratio	4416 × 1242(2k)@15fps
Depth Range	0.2–20 m
Data Rate	400 Hz
Postural Drift	0.35%; 0.005°/m

and

Table 5. NS570D information.

Accuracy	Position Drift (1 km or 2 min) 0.20%
	Heading Drift (1 min) 0.15°
Position (RMS)	1.5 m, 2 cm + 1 ppm (RTK)
Speed (RMS)	0.03 m/s

).

As shown in Figure 18, the INS570D module, the Livox-MID-70 and the ZED, respectively, communicate with the industrial computer through the RS485 serial port, the Ether Net, and the USB 3.0. The industrial computer runs the perception and planning algorithm, sends the motion commands to the AGV through the Controller Area Network (CAN).

As shown in Figure 19 there are four ZED cameras installed around the AGV, a LiDAR is installed before and after the AGV, and two are installed on each side. The industrial computer and INSS570D are integrated inside the AGV, and the INS570D antenna is placed diagonally in front and behind the AGV.

The sensors used in the perception system in this paper (including ZED, LiDAR, inertial navigation, etc.) were pre calibrated using their official calibration methods and procedures before the experiment. Please refer to the official websites of each sensor for details. During the experiment, a large number of tests have shown that the accuracy of obstacle recognition and positioning can reach the centimeter level.

4.2. AGV Tracking Control for Experiments

The kinematic model of AGV is shown in the Figure 20. AGV is a four-wheel steering model, which regards all movements of AGV to be considered as circular movements around point O’ on the plane of the AGV body. Linear motion and spin are its special limiting motion states. When all components of the AGV, including the wheels, move in a circular motion around a certain point, the angular velocity $\omega$ is the same, and the linear velocity is perpendicular to the center of the circle. We set the wheelbase of the AGV to ${\mathrm{L}}_{agv}$ and the wheel-base to ${\mathrm{H}}_{\mathrm{agv}}$, The speeds of the four drive motors are $V_{LF,}{V}_{RF},V_{LB}$, $V_{RB}.$ The angle of the four steering motors are ${\delta }_{LF,}{\delta }_{RF},{\delta }_{LB}$, ${\delta }_{RB}$. The angle between AGV speed and $V_x$ is δ. The turning radii of the four wheels are $R_{LF,}{R}_{RF},R_{LB}$,$R_{RB}$. The turning radius of AGV is $R$.

The $V$, $R,\delta$ can be obtained according to the following equation after obtaining the $V_{x,}$ $V_y$ and $\omega$.

$$\left\{\begin{array}{l}V=\sqrt{{\mathrm{V}}_x+{\mathrm{V}}_y}\\ \delta =tan(\frac{{\mathrm{V}}_y}{{\mathrm{V}}_x})\\ \mathrm{R}=\frac{V}{\omega }\end{array}\right.$$

(21)

Then, the $R_{LF,}{R}_{RF},R_{LB}$, $R_{RB}$ can be calculated according to the following equation.

$$\left\{\begin{array}{l}{R}_{LF}^{}=\sqrt{{[\frac{{\mathrm{L}}_{\mathrm{agv}}}{2}+Rsin(\delta )]}^2+{[\frac{{\mathrm{H}}_{\mathrm{agv}}}{2}+Rcos(\delta )]}^2}\\ R_{LB}^{}=\sqrt{{[\frac{{\mathrm{L}}_{\mathrm{agv}}}{2}-Rsin(\delta )]}^2+{[\frac{{\mathrm{H}}_{\mathrm{agv}}}{2}+Rcos(\delta )]}^2}\\ R_{RB}=\sqrt{{[-\frac{{\mathrm{H}}_{\mathrm{agv}}}{2}+Rcos(\delta )]}^2+{[\frac{{\mathrm{L}}_{\mathrm{agv}}}{2}-Rsin(\delta )]}^2}\\ R_{RF}=\sqrt{{[-\frac{{\mathrm{H}}_{\mathrm{agv}}}{2}+Rcos(\delta )]}^2+{[\frac{{\mathrm{L}}_{\mathrm{agv}}}{2}+Rsin(\delta )]}^2}\end{array}\right.$$

(22)

Then, $V_{LF,}{V}_{RF},V_{LB}$, $V_{RB}$ can be obtained according to $R_{LF,}{R}_{RF},R_{LB},R_{RB}$.

$$\left\{\begin{array}{l}{V}_{LF}=R_{LF}{\omega }_{}\\ V_{LB}=R_{LB}{\omega }_{}\\ V_{RF}=R_{RF}{\omega }_{}\\ V_{RB}=R_{RB}{\omega }_{}\end{array}\right.$$

(23)

According to the following equation, ${\delta }_{LF,}{\delta }_{RF},{\delta }_{LB}$, ${\delta }_{RB}$ can be obtained

$$\left\{\begin{array}{l}{\delta }_{LF}=\frac{\pi }{2}-\mathit{arctan}(\frac{Rcos(\delta )+\frac{{\mathrm{H}}_{\mathrm{agv}}}{2}}{Rsin(\delta )+\frac{{\mathrm{L}}_{\mathrm{agv}}}{2}})\\ {\delta }_{LB}=\frac{\pi }{2}-\mathit{arctan}(\frac{Rcos(\delta )+\frac{{\mathrm{H}}_{\mathrm{agv}}}{2}}{-Rsin(\delta )+\frac{{\mathrm{L}}_{\mathrm{agv}}}{2}})\\ {\delta }_{RF}=\frac{\pi }{2}-\mathit{arctan}(\frac{Rcos(\delta )-\frac{{\mathrm{H}}_{\mathrm{agv}}}{2}}{Rsin(\delta )+\frac{{\mathrm{L}}_{\mathrm{agv}}}{2}})\\ {\delta }_{RB}=\frac{\pi }{2}-\mathit{arctan}(\frac{Rcos(\delta )-\frac{{\mathrm{H}}_{\mathrm{agv}}}{2}}{-Rsin(\delta )+\frac{{\mathrm{L}}_{\mathrm{agv}}}{2}})\end{array}\right.$$

(24)

At this point, we have achieved the kinematic solution of the AGV, and based on $V_{x,}$ and $\omega$, we can obtain $V_{LF,}{V}_{RF},V_{LB}$, $V_{RB} \mathrm{and} {\delta }_{LF,}{\delta }_{RF},{\delta }_{LB}$, ${\delta }_{RB}$.

After obtaining the optimal trajectory in Section 3.3, the trajectory is composed of a set of points with speed information. By traversing the trajectory points with positioning information, we find the closest trajectory point to AGV, set the preview distance to be $Le{n}_{ahead}$. We find the target point. The angle deviation between AGV and target point is ${\delta }_{err}$.

$${\delta }_{err}=\mathit{arctan}(\frac{y_{tar}-y_{AGV}}{x_{tar}-x_{AGV}})-{\delta }_{AGV},$$

(25)

where $x_{tar}$, $y_{tar}$ are the coordinates of the target point.$x_{AGV}, y_{AGV}$ are the coordinates of AGV, ${\delta }_{AGV}$ is the heading of AGV.

Using PD control to calculate the $\omega$.

$$\omega =k_{p1}{\delta }_{err}+k_{d1}({\delta }_{err}-{\delta }_{err\_last}),$$

(26)

where $k_{p1},$ $k_{d1}$ are the PID parameters, and ${\delta }_{err\_last}$ is the previous angle error.

The target point is equipped with speed information $spee{d}_{target}$, the current speed ($spee{d}_{now}$) of AGV can be obtained through the perceived information, the difference between the two results in a speed deviation is the $spee{d}_{err}$, then the speed control is using an incremental PID algorithm as shown in the following equation:

$$v_x=spee{d}_{now}+k_{p2}spee{d}_{err}+k_{i2}{\displaystyle {\int }_0^{t}spee{d}_{err}dt}+k_{d2}(spee{d}_{err}-spee{d}_{err\_last}),$$

(27)

where $k_{p2},k_{i2},$ $k_{d2}$ are the PID parameters, and $spee{d}_{err\_last}$ is the previous speed error.

According to the Equations (26) and (27), we calculate the $v_x$ and $\omega$, respectively, to control AGV.

The parameters used in the control are shown in the table below (

Table 6. Control parameters.

Params	$\mathit{L}\mathit{e}{\mathit{n}}_{\mathit{a}\mathit{h}\mathit{e}\mathit{a}\mathit{d}}$	${\mathit{k}}_{\mathit{p}1}$	${\mathit{k}}_{\mathit{d}1}$	${\mathit{k}}_{\mathit{p}2}$	${\mathit{k}}_{\mathit{i}2}$	${\mathit{k}}_{\mathit{i}2}$	${\mathbf{H}}_{\mathbf{a}\mathbf{g}\mathbf{v}}$	${\mathbf{L}}_{\mathbf{a}\mathbf{g}\mathbf{v}}$
Value	3 m	0.6	0.1	0.8	0.01	0.1	900 mm	1600 mm

).

4.3. Verification of Selection of Optimal Path in Coarse Planning

In the real road environment at the construction site, sampling was performed based on constraints such as road boundaries, obstacles, and reference lines. Over 30 sets of parallel experiments were conducted with different start and end points to validate the performance of the multi-objective evaluation function in coarse planning. Finally, three representative sets of experiments were selected for presentation. Test 1 is the starting point located on the centerline of the road, and the endpoint is also located on the centerline of the road. Test 2 is the starting point located away from the centerline of the road, and the endpoint is also located away from the centerline of the road. Test 3 starts at the center of the road and ends far away from the center of the road and near obstacles. By setting different conditions, we fully verify the reliability of the planning algorithm. The parameter values during the experiments are listed in

Table 7. Sampling and multi-objective evaluation function parameters.

Params	∆s	∆l	La	Sa	dobs	n	k1	k2	k3	d
Value	1 m	1.5 m	4 m	12 m	0.5 m	10	10	10	1	0.5

.

The experimental results are presented in the following figures (Figure 21, Figure 22 and Figure 23). Blue arrows indicate the current pose of the vehicle, brown rectangles represent obstacles, green squares represent obstacle safety distances, blue dots represent sampling points, red dots represent the output points from coarse planning, and red arrows indicate the vehicle’s target endpoint.

Based on the experimental results, we can obtain the following conclusions.

(1) Sampling based on environmental constraints efficiently and succinctly achieved waypoint sampling. In the experiment, a total of 50 points were sampled to cover a width of 8 m and extended longitudinally by 12 m. Eleven invalid sampling points were filtered out through obstacle filtering (denoted by “×” in the table, as in

Table 8. Evaluation Results of est 1.

Cost	S1	S2	S3	S4	S5	S6	S7	S8	S9	S10
L1	334.3	374.9	∞	×	×	131.9	96.5	64.7	98.8	∞
L2	314.6	286.2	×	×	149.5	114.6	80.3	45.7	35.2	×
L3	303.7	273.1	∞	×	152.2	194.8	×	∞	23.4	11.8
L4	313.7	269.5	236.7	200.6	163.6	283.5	×	×	32.9	34.3
L5	333.2	287.2	254.7	×	×	288.7	187.4	86.3	52.3	96.3

, (S4, L1)), resulting in computational workload savings.

(2) The selection of the optimal path is based on a greedy approach, where the optimal path is selected in a reverse manner. For example, in

Table 9. Evaluation Results of Test 2.

Cost	S1	S2	S3	S4	S5	S6	S7	S8	S9	S10
L1	362.3	402.9	∞	×	×	159.9	124.4	92.7	114.5	∞
L2	342.6	314.2	×	×	177.5	142.6	108.2	73.6	63.2	×
L3	331.7	301.0	∞	×	180.2	222.7	×	∞	51.3	39.8
L4	341.7	297.4	264.7	228.6	191.6	311.4	×	×	60.9	50.1
L5	361.2	315.2	282.7	×	×	316.7	215.4	114.2	80.2	98.4

, under the S2 column, L3 is not the point with the minimum cost, but in conjunction with the choice made by the global path planning, the optimal path includes (S2, L3). Therefore, this point is chosen as the optimal sampling point in that longitudinal section.

(3) Although some sampling points are not within the range of obstacle filtering, due to linear interpolation points connecting adjacent columns of sampling points, if there is always an interpolation point within the obstacle filtering range during the process of reaching the endpoint for all selected paths, the cost of that point is considered infinite, as shown in

Table 10. Evaluation results of test 3.

Cost	S1	S2	S3	S4	S5	S6	S7	S8	S9	S10
L1	585.6	626.7	∞	×	×	383.3	347.8	302.1	202.9	101.7
L2	565.9	537.6	×	×	400.8	365.9	331.6	297.0	∞	×
L3	555.0	524.4	∞	×	403.6	446.1	×	∞	∞	∞
L4	565.0	520.8	488.1	451.9	414.9	∞	×	×	∞	∞
L5	584.6	538.5	506.1	×	×	∞	169.3	∞	∞	∞

(S6, L4).

(4) On the basis of safely avoiding all obstacles and considering the evaluation function set, the final planned path tends to be close to the centerline of the road and ensures the shortest path. The planning results are excellent and align with expectations.

4.4. Curve Fitting and Optimization Experiments

Building upon the foundation of Section 4.2, the coarse planning algorithm-generated path points were improved using B-spline fitting as described in Section 2.3, followed by optimization using quadratic programming. The parameters used are as shown in the table below (

Table 11. Parameters for Curve Fitting and Optimization.

Params	nz	v1	v2	v3	w1	w2	w3	bn1	bn2	bmax	bmin
values	9	500	1	10	400	1	10	7	bn − 7	0.5	0.01

).

Out of the more than 30 sets of experiments conducted, 8 representative sets of experiments were selected. The starting point of tests 4–7 are at the centerline of the road. Their endpoints are on the road centerline, slightly off the road centerline, significantly off the road centerline and close to obstacles, and significantly off the road centerline and away from obstacles. The starting points of tests 8–11 are all far from the centerline of the road, and the endpoint setting rules are the same as tests 4–7. By setting eight different operating conditions, the fitting and optimization effects of curves under different rough solutions were fully tested. The visualization of the experimental results is shown in the following figure (where the red line represents the discrete path point’s output by coarse planning, the yellow line represents the traditional B-spline fitting curve, the green line represents the improved B-spline fitting curve, and the black line represents the final path after quadratic programming optimization).

Taking Figure 24h as an example, as shown in Figure 25, importing the fitting curves into Origin, we can perform a specific analysis of the four types of paths: original path, traditional B-spline, improved B-spline, and the path after quadratic programming optimization. Additionally, we can calculate the cost values $n_1$ and $n_2$ to analyze the algorithm’s effectiveness.

As shown in Figure 26①, B-spline fitting eliminates the sharp corners in the original path caused by the sparse sampling points. At these corners, a smooth curve is used to ensure the smoothness of AGV motion. As depicted in Figure 26②, at both the starting and ending points, the vehicle’s velocity direction is horizontal to the right. However, traditional B-spline fitting paths do not consider velocity direction; instead, they purely fit based on the coordinates of path points. This can lead to difficulties in smoothly tracking the planned path at the starting point and pose inaccuracies at the ending point.

As shown in Figure 27②, the improved B-spline fitting, while eliminating sharp corners, also considers the velocity direction at the starting and ending points. The green curve ultimately ensures that the vehicle reaches the endpoint with a horizontal rightward velocity. The cost values $n_1$ and $n_2$ are calculated as shown in Figure 28, and the best interpolation results are achieved when $n_1$ = 6 and $n_2$ = 7. As depicted in Figure 27①, although the improved B-spline eliminates most of the sharp corners, in areas with steeper corners, the fitted path may still not be smooth enough, and there is room for overall path improvement.

As shown in Figure 29②, under the improved B-spline fitting path with a relatively large value for k1, the quadratic programming prioritizes smoothness. Due to the setting of segmental tolerance, it ensures that the smooth optimization does not disrupt the planned path’s velocity direction at the starting and ending points, as depicted in Figure 29① and ③.

After a series of processing steps, the path output by quadratic programming has been fitted to create a high-quality path that aligns with the original path, ensuring smoothness, shortest-path characteristics, and proximity to the reference line. This path serves as the final result of coarse planning and is sent to precise planning. The remaining seven sets of experiments in Figure 24 all align with the conclusions drawn in the analysis above.

4.5. Validation of Lateral Planning Effectiveness in Precise Planning

As shown in Figure 30, the planning results from Section 4.4 are used as the input reference lines for precise planning, taking test6 as an example. Sampling planning is performed under the motion constraints defined in Section 3.1 to validate the planning effectiveness of the lateral planning in precise planning. The parameters used are as listed in

Table 12. Precise planning parameters.

Params	vmax	amax	jmax	kmax	Tmax	Tmin	∆d	∆v
Value	1.5 m/s	0.2 m/s2	0.25 m/s3	1	8.0 s	3.0 s	0.1 m	0.3 m/s
Params	nv	c1	c3	c4	dissave	Lveh	L
Value	5	0.5	100	80	0.4 m	0.5 m	1 m

.

The experimental process, as described in Section 3.2, involves calculating the optimal path using the evaluation function and then eliminating dangerous trajectories through collision detection. The vehicle is controlled to track the planned trajectory using a pure tracking algorithm. The visualization of the experimental process is shown in the following figure (Figure 31 and Figure 32) (where the pink arrow represents the AGV, and the light red circle represents the danger zone of obstacles).

Compare the reference line output by coarse planning with the planned path in precise planning as shown in Figure 33a, the planned road curvature in Figure 33b, and analyze the changes in trajectory acceleration and jerk during lateral planning as depicted in Figure 33c.

Based on the experimental results, we can obtain the following conclusions.

(1) As shown in Figure 30 and Table 12, compared to coarse planning, the lateral sampling range has been reduced from [−4, 4] to [−0.5, 0.5], with lateral sampling points reduced from 80 to 10 under the condition of $\Delta d$ = 0.1. This significant reduction in the sampling range greatly improves algorithm speed while ensuring efficient, accurate, and safe lateral sampling based on the coarse planning reference line. This greatly enhances both sampling efficiency and quality.

(2) As depicted in Figure 31, although the reference line output by coarse planning is already capable of avoiding all obstacles, precise planning, due to curve fitting, optimization, and the inclusion of a safety margin ($di{s}_{save}$), still performs local obstacle avoidance, as shown in Figure 33a for cases ① and ②. This corresponds to (b), (e), and (f) in Figure 31. As shown in Figure 33b, the lateral deviation is between [−0.17, 0.1].

(3) As demonstrated in Figure 33b, the planned road curvature is within the range [−0.6, 0.87], and as shown in Figure 33c, lateral acceleration ranges from [−0.05, 0.016], while jerk ranges from [−0.06, 0.012]. These values all satisfy the constraints specified in Table 12.

4.6. Validation of Longitudinal Planning in Precise Planning

In the experiments conducted in Section 4.4, the effectiveness of longitudinal planning in precise planning is validated by setting different initial speeds and target speeds. Four sets of speed planning experiments are conducted to evaluate its performance in small-scale acceleration and deceleration as well as large-scale acceleration and deceleration. The results of speed planning are shown in the following figure.

The experimental statistics are presented in the following table.

The experimental results indicate the following:

(1) As shown in Figure 34a–d, for small-scale acceleration and deceleration, precise planning can achieve the target speed within a single planning cycle. However, for large-scale acceleration and deceleration (0 m/s→1.5 m/s and 1.5 m/s→0.5 m/s), as observed in Figure 34f,h, it takes three and two planning cycles, respectively. This is because for significant speed changes, precise planning needs to consider factors like acceleration and jerk costs, so it cannot achieve the target speed within a single planning cycle.

(2) According to

Table 13. Speed planning statistics.

(m/s)	Final Velocity	Max Acc	Max Jerk	Min Jerk
0.5→1	1.00	0.1911	0.1972	−0.1365
1.5→1	1.00	−0.1912	0.1062	−0.1668
0→1.5	1.49	0.1991	0.2180	−0.1308
1.5→0.5	0.50	−0.1944	0.1296	−0.2160

, in all four sets of experiments, the vehicles eventually reach or get close to the target speed. This indicates accurate speed planning, and the acceleration and jerk values meet the requirements in Table 12, demonstrating the effectiveness of sampling constraints based on the motion model.

(3) The maximum acceleration for all four sets of experiments reaches 0.19 m/s², and the acceleration changes gradually under the constraints of jerk. This reflects that speed planning doesn’t blindly pursue reaching the desired speed but considers a balance between speed, longitudinal acceleration, and jerk values. This ensures the AGV’s stability while achieving the desired speed as quickly as possible.

4.7. Comparative Experiments

To validate the proposed method in this paper, it is compared with the traditional Frenet sampling planning algorithm [28,29,30], and the experimental process is shown in the Figure 35 and Figure 36.

As described in Section 3.3, the hierarchical trajectory planning algorithm reduces the time complexity of traditional Frenet sampling planning algorithms from $O\left(n^2\right)$ to $O\left(n\right)$. In the experiment, the hierarchical trajectory planning algorithm samples 80 sets of trajectories each time, while the traditional Frenet sampling planning algorithm samples 1156 sets of trajectories each time, Under the results of coarse planning the precise planning algorithm is applied, and the number of samples is significantly reduced. This provides an effective solution for dealing with the retrieval area of wide roads in the construction site environment.

At the same time, this paper uses the DWA planning algorithm [33] to compare the hierarchical trajectory planning algorithm. The experimental process is shown in Figure 37 and Figure 38.

It can be observed that the DWA planning algorithm is a path planning algorithm that does not consider road boundaries and is used in unknown spaces. The planning results have uncertainty. The hierarchical trajectory planning algorithm can obtain a rough solution path through coarse planning, and the rough solution path is optimized through fitting, which is significantly better than the DWA planning algorithm in terms of smoothness. During the experiment, the DWA planning algorithm may also encounter the problem of local optimal traps, as shown in Figure 39. The hierarchical trajectory planning algorithm uses greedy thinking to find the optimal path from the endpoint in coarse planning, ensuring that the algorithm does not fall into local optimal traps.

Based on the experiments conducted in Section 4.3, Section 4.4, Section 4.5, Section 4.6 and Section 4.7. This paper presents a comparison table of the hierarchical trajectory planning algorithm, dynamic programming algorithm, Frenet sampling algorithm, and DWA algorithm as follows: According to

Table 14. Algorithm comparison.

	Efficiency	Local Trap	Smooth Optimization	Speed Planning
Hierarchical Trajectory	high	×	√	√
Dynamic Programming [32]	high	×	×	×
Frenet Sampling [28,29,30]	low	√	√	√
DWA [33]	high	√	×	×
RRT [34]	high	×	×	×

, although dynamic programming algorithms have high efficiency and no local traps, it cannot smooth paths and does not have speed planning. The disadvantage of Frenet sampling planning is that it frequently solves solutions of fifth and fourth polynomials over a wide range of horizontal and lateral directions, resulting in low running efficiency. DWA planning may fall into local traps, and the RRT algorithm does not have the ability to optimize speed planning and curve smoothing. Through comparison with various algorithms, it was found that the hierarchical trajectory planning algorithm is a trajectory planning algorithm with high operational efficiency, no local traps, smooth trajectory optimization, and has the capable of speed planning. In the table, × represents that the corresponding function has defects, while √ represents that it has the corresponding function.

5. Conclusions

This paper constructs a hierarchical trajectory planning algorithm through the design of two algorithms: coarse planning based on road environment sampling and precise planning based on AGV motion sampling. The hierarchical trajectory planning algorithm ensures efficient algorithm operation while also considering the smoothness of the planned trajectory. It ensures that high-capacity AGVs can output accurate, safe, and smooth planned paths efficiently and stably on wide retrieval roads in a construction site environment.

In the final tests and experiments, we conducted tests based on AGV real vehicles in a real construction site environment. From the actual test results, the dynamic programming algorithm for coarse planning can quickly select the optimal path. The interpolated B-spline fitting method, considering the nonholonomic constraints of the vehicle, ensures the rationality of the path while eliminating the planning turning points. Secondary optimization further ensures the smoothness of the path. Fine planning, based on coarse planning, reduces the lateral sampling range from 8 m to 1 m. In the speed planning process, various acceleration and deceleration scenarios of AGV were tested. The experimental results show that the planned trajectory meets collision detection requirements while achieving the desired speed rapidly and considering trajectory smoothness. In all test experiments, the maximum acceleration was 0.1944 m/s², and the maximum jerk was 0.1972 m/s³, indicating good speed planning performance. This implementation achieves high-quality and efficient trajectory planning.

This research effectively addresses challenges faced by AGVs in construction site environments, such as heavy loads, wide road sampling ranges, and random obstacle distributions. It provides new insights into solving path planning problems in construction site environments. The proposed hierarchical trajectory planning algorithm can be used for single AGV working conditions. It can be further improved while considering the cooperation of multiple vehicles. In the future, we are planning to integrate the leader follower control, game theory and other algorithms for planning and control under multi vehicle working conditions to meet the real requirements of building sites.