An optimal trajectory planning algorithm for autonomous trucks: Architecture, algorithm, and experiment

Safe lane changing of the dynamic industrial park and port scenarios with autonomous trucks involves the problem of planning an effective and smooth trajectory. To solve this problem, we propose a new trajectory planning method based on the Dijkstra algorithm, which combines the Dijkstra algorithm with a cost function model and the Bezier curve. The cost function model is established to filter target trajectories to obtain the optimal target trajectory. The third-order Bezier curve is employed to smooth the optimal target trajectory. Road experiments are carried out with an autonomous truck to illustrate the effectiveness and smoothness of the proposed method. Compared with other conventional methods, the improved method can generate a more effective and smoother trajectory in the truck lane change.


Motivation
In recent years, research on autonomous vehicles has entered a period of vigorous development. More and more autonomous vehicles are tested on real urban roads. Some companies even have begun the trial operation of autonomous vehicles. In the passenger car field, Google Waymo has launched the self-driving taxi business in California, USA. In the commercial vehicle field, TuSimple has commercialized the road freight business in Arizona, USA. Autonomous vehicles use their sensing devices to perceive the surrounding environment, make executable safety decisions through the decision-making subsystem, confirm the effectiveness of the behavior, plan its trajectory, and finally complete autonomous driving.
As a research branch of intelligent robots, many technologies of autonomous vehicles are derived from intelligent robots. The trajectory planning of autonomous driving is equivalent to that of intelligent robots in a special scene. The trajectory planning of the intelligent robot is carried out in three-dimensional space, while the trajectory planning of autonomous driving is similar, but limited to a two-dimensional plane. Therefore, the study of trajectory planning for autonomous driving can refer to current studies on the trajectory planning of intelligent robots. [1][2][3][4][5][6][7][8][9] The problem of trajectory planning for autonomous driving can be regarded as a time-space curve optimization problem in a two-dimensional plane, and solving the optimization problem means solving the problem of trajectory planning for autonomous driving.
At present, there are various methods that can achieve trajectory planning for autonomous vehicles. The common trajectory planning methods for autonomous vehicles are the Dijkstra algorithm combined with the Bezier curve and the A-star algorithm combined with the Bezier curve. The Dijkstra algorithm combined with the Bezier curve is limited by the amount of calculation of the Dijkstra algorithm, which increases the response time of the vehicle. In practice applications, this method requires a larger computing device to calculate all the candidate target trajectories to obtain an optimal target trajectory. The A-star algorithm combined with the Bezier curve is mainly used for the trajectory planning of passenger cars. However, since the size of the truck is larger than the size of the passenger car, the method of trajectory planning may be different. Therefore, it is not an effective method in the trajectory planning of autonomous trucks. In this article, an improved Dijkstra method is proposed for these problems. The main contribution of this article lies in (1) the proposed method uses a different cost function model and makes it different from previous work in that it is especially suitable for autonomous trucks and (2) the experimental studies to verify the proposed method.

Related work
The autonomous vehicle generates the desired path according to various parameters that are fed as input from the sensors and provides a corresponding control signal to the actuators that drive the vehicle to complete the autonomous driving. The vehicle's autonomous driving system is mainly divided into three subsystems: the perception subsystem, the decision-making subsystem, and the actuator subsystem, each of which performs different tasks. The perception subsystem is mainly responsible for environmental perception and positioning. The decision-making subsystem mainly completes the motion planning and trajectory planning of the vehicle, and the actuator subsystem is in charge of the execution of the motion command to help the vehicle complete the driving task. Decisionmaking is an important module for autonomous driving. It determines whether the vehicle can smoothly and accurately complete various driving behaviors. Trajectory planning is a significant research field in the decision-making layer. 10 Trajectory planning mainly implements driving behavior commands through the behavior planning layer to satisfy the movement and dynamic constraints of the vehicle and avoid collisions. The workflow chart of the trajectory planning layer is shown in Figure 1. The blue coating part in Figure 1 is the trajectory planning layer framework. Due to the complexity of the real urban environment, and the non-holonomic vehicle system, it is very difficult to carry out trajectory planning during driving.The trajectory planning layer should generate executable trajectories that correspond to each behavioral result. In fact, trajectory planning is a constrained optimization problem, and the constraints include the vehicle's current state (i.e. planned and initial state) behavior and target vehicle dynamics, constituting the final trajectory optimization. The trajectory that meets these criteria can be used as the final output trajectory.
In the past decade, autonomous driving technology has been developed as a promising technology. Trajectory planning, as a major challenge of autonomous driving technology, 10,11 has also made significant progress. The algorithms related to trajectory planning can be divided into four categories: interpolating-curve-based, graph-searchbased, sampling-based, and numerical-optimizationbased. 12,13 Graph-search-based algorithm extends from algorithms for path planning; the most common graphsearch-based algorithms for trajectory planning are Dijkstra, 14 A-star, 15 and state lattice. 16 The Dijkstra algorithm is suitable for finding the shortest path in both structured and unstructured environments. Its result is continuous but not smooth, and it is computationally intensive. The A-star algorithm reduces the computational cost. The output result is continuous but not smooth. The state lattice algorithm can process multidimensional computation and is suitable for the dynamic environment and local planning, but it takes long time to compute. The most common samplingbased algorithm for trajectory planning is rapidly exploring random tree (RRT). 17 It can be solved quickly in a highdimensional system with convergence and completeness. Its result is continuous but not smooth. The most common interpolating-curve-based algorithms are Clothoid curves, 18 polynomial spirals, 19 Bezier curves, 20 and spline curves. 21 The Clothoid curve algorithm is suitable for local planning. It is easy to implement on high-speed and structured roads, but the integration can be time-consuming. The curvature is continuous but not smooth, depending on waypoints. The polynomial spirals algorithm works without heavy calculation, and its curvature is continuous. But the curvature is more than four orders, of which the key parameters are also not easy to obtain. Sharing similar advantages as the polynomial spirals algorithm when the curve order is not large, the Bezier curve algorithm has lower ductility when the curve order is increased, resulting in the massive calculation. The spline curves algorithm also has worked without much calculation and generates smooth curves, but its output curve may not be the optimal one. The most common numericaloptimization-based algorithm is function optimization algorithms. 22 In the case of considering various constraints, function optimization is easy to implement but has high computational cost and relies heavily on waypoints.

Autonomous truck
Firstly, we introduce an autonomous truck shown in Figure 2, which was modified based on ordinary trucks. This truck's autonomous system consists of an autonomous driving control unit (ADCU), a wire control chassis of the truck, a high-precision inertial measurement unit (IMU), and sensors group, including 16-line LiDARs, a singleline LiDAR, ultrasonic radars, a front-view camera, and a side-view camera. The truck's dimension is 7000Â 2200 Â 4000 mm 3 .
As is shown in Figure 3, the driving system of the truck is mainly composed of the environment perception subsystem, the decision-making subsystem, and the actuator subsystem.The perception subsystem is mainly composed of 16-line LiDARs, a single-line LiDAR, ultrasonic radars, a front-view camera, a side-view camera, and high-precision IMUs. The front-view camera is used for visual target recognition. The side-view camera is used for target recognition, positioning, and parking assist. The ultrasonic radars are used to detect obstacles on the road that may cause a scratch on the vehicle chassis. The high-precision IMU is used to acquire the path planning information and the precise positioning information of the vehicle. The single-line LiDAR is used for obstacle measurement and collision avoidance. High-definition map (HD map) is used for providing the surrounding environment information, road information, intersection information, and making the path plan for the truck.  Figure 4(b), we can see the partial path is composed of many yellow feature points by which the vehicle is positioned. Figure 4(c) shows a case in which the side-view camera helps improve the positioning accuracy. When target feature points are captured, they will be compared with the key feature points stored in the device to obtain an accurate position.
All sensor information is output to the ADCU, and the obstacle information and vehicle position information are obtained through the fusion algorithm. Then the information is output to the decision-making subsystem for truck behavior planning and trajectory planning. According to the output of the behavior planning and trajectory planning, the control signals are transmitted to the actuator subsystem for vehicle control.

Trajectory planning algorithm
Trajectory planning is executed by the decision planning layer to drive the behavior instructions output. Trajectory planning generates a series of trajectory curves from the current state to the next goal state. In the real urban environment, the trajectories need to satisfy the movement and dynamic constraints of the vehicle to avoid collision with obstacles.

Trajectory generation model
In the trajectory planning of autonomous trucks, the graphbased search method is to search the optimal path (state sequence) between the current state of the truck and the next goal state in the graph-based state space.
In this article, the problem of lane-level path planning for trucks on HD maps is abstracted into a shortest path search problem on directed weighted maps. When the HD map of the lane level is obtained, the "scatter points" are carried out in a certain range of lanes, representing the possible driving position of the vehicle.
The trajectory of an autonomous vehicle can be regarded as a set of actions from the initial state X 0 to the goal state X T X 0 ¼ ½x 0 :y 0 ; k 0 ð 1Þ Among them, ðx; yÞ represents the vehicle's position in a two-dimensional coordinate system and k represents the curvature of the vehicle's trajectory. The transformational relation among curvature k, turning radius r, wheelbase L, and front-wheel angle a is expressed as follows 23 As is shown in Figure 5, the candidate trajectory is generated by connecting the sampling points, and the planning area is between the red lines.

Cost function based on vehicle motion constraints
In terms of trajectory planning of autonomous vehicles, the most common techniques are sampling graph search methods, such as the Dijkstra algorithm, A-star algorithm, and its variants. An improved Dijkstra algorithm is proposed in this article, combining the original Dijkstra algorithm (introduced in Appendix) and the cost function. The cost function model for trajectory planning is established as truck motion constraint exists. Considering the vehicle's steering ability when randomly distributing sampling points, not only should the shortest Euclidean distance Distðp i ; p j Þ be considered but also the smoothness of the trajectory Headðp i ; p j Þ is expected to be as small as possible. In addition, considering the initial state constraints of the vehicle, the tangent vector of the starting point and the second sampling point should to be horizontal, so the Headðp 0 ; p 1 Þ should be 0. Based on the above analysis, a more reasonable cost function (see equations (4) to (6)) which is in line with the practical application of autonomous vehicles has been chosen in this article. Figure 6 shows the nodes of the cost function Headðp i ; p j Þ ¼ Normðq ij Þ ð 6Þ Among them, K 1 and K 2 are weights of distance and angle difference, respectively, which can be set according to the actual situation, and ðx i ; y i Þ and ðx j ; y j Þ are coordinates of p i and p j , respectively. Captured by sensors with two different forms, the distance and angle data need to be normalized to unify them under the same reference. In this article, an effective and simple deviation normalization approach is adopted to deal with these two variables (see equations (7) and (8)). Among them, d max and d min represent the largest node distance and the smallest node distance, respectively, and q max and q min represent the largest angle and the smallest angle between nodes, respectively. Norm function calculates the normalized distance d and angle q, respectively NormðqÞ ¼ ðq À q min Þ=ðq max À q min Þ ð 8Þ According to the Dijkstra algorithm, all the nodes are checked successively, with their neighbors added to the list of nodes to be checked. The algorithm stops when it reaches the goal node. Figure 7 shows the flowchart of the proposed algorithm.
Firstly, the data list of sampling points is read. The number of sampling points in the list is n. The starting point and the ending point are marked, respectively. And the number of sampled points is denoted by num ð1 num nÞ.
Then, start from the starting point to find the unmarked point with the lowest cost in the sampling point list to be the next point. And the initial cost is marked as 0. Next, examine all the remaining unsampled nodes in turn and add their adjacent points to the list of points to be checked.   After traversing from the start point to each point, if the cost of point num is smaller than the current minimum, update the minimum, num ¼ num þ 1.
Repeat the above process until reaching the end point.

Smooth trajectory curve
Bezier curve is used to smooth the trajectory connecting the starting and ending points. The Bezier curve only needs a few control points to generate a more complex smooth curve. This method can ensure the clear and simple relationship between the input control points and the generated curve, and the shape and order of the curve can be easily changed. [24][25][26][27][28] Given the position vector of n þ 1 point in space, the coordinate of each point on the Bezier reference curve can be obtained by the following formula T ðtÞ ¼ X n i¼0 P k B k;n ðtÞ t 2 ½0; 1 ð 9Þ Among them, P i represents the control point, T ðtÞ represents trajectory function with time, and t is the time proportional coefficient, n is the order of curve, and B kÀn ðtÞ is the Bernstein basis function Bezier curve has characteristics of the convex hull, symmetry, geometric invariance, quasi-locality, and so on. It guarantees the smoothness, continuity, and controllability of the generated curve. In the third-order Bezier curve, 29 point P 0 is used as the center coordinate of the goal vehicle, points P 1 and P 2 in the middle of the trajectory curve are used as the control points, and the goal point is represented by point P 3 . P 0 , P 1 , P 2 , and P 3 points are illustrated on an example trajectory curve as shown in Figure 8.
Because changing direction in situ can damage the vehicle's tires, it is necessary to let the vehicle follow a smooth trajectory. At the same time, when changing lanes or turning, it is necessary to follow traffic rules and go along a proper road when planning the trajectory. This article plans the vehicle's trajectory points as mentioned earlier. The third-order Bezier curve determined by the four control points can be obtained by the following formula

Road experiments
Road experiments were carried out in a port to evaluate the performance of the proposed algorithm. In the experiment,  Figure 9. This lane change scene is a situation when the truck encounters a static vehicle in front of it during driving, and the truck needs to change lane to avoid the collision. The results of the experiment show that the truck smoothly changes the lane and avoids the obstacle and collision. The results of the experiment include the heading angle, the yaw, and the trajectory curvature of the truck. They are used to evaluate the motion control performance and comfortness of the vehicle.
Since Figure 9 cannot show the lane change process of the truck, we use a schematic diagram to indicate the lane change process of the truck. The schematic diagram of truck lane change is shown in Figure 10.

The trajectory curves
After the experiment, the trajectory curve generated by the truck is analyzed. The trajectory of the candidate sampling points is generated according to the Dijkstra algorithm with the cost function model, the Bezier curve, and the constraints of the vehicle's direction. The heading angle of the truck at the starting point and the ending point is one of the constraints, which should be horizontal and suitable for the scene of the lane change. Through the improved Dijkstra method, the trajectory generated by the vehicle is shown in Figure 11. The polyline in the figure is generated by the Dijkstra algorithm combined with the cost function model, and the smooth curve is obtained by smoothing the polyline through the Bezier curve.
The trajectory generated by the Dijkstra method is shown in Figure 12. The target trajectory shown by the polyline in the figure is generated using the Dijkstra algorithm, and then the smooth curve in the figure is obtained by smoothing the polyline with the Bezier curve.
Similarly, the trajectory planning of the A-star method is considered for better comparison. The A-star algorithm is based on the combination of Dijkstra algorithm and a cost function model, like the improved Dijkstra algorithm. As is shown in Figure 13, the polyline represents the trajectory generated by the A-star algorithm, and the smooth curve represents the trajectory generated by the Bezier curve.   The results of the experiment The trajectory curves of these three methods are shown in Figure 14. The trajectory generated by the improved Dijkstra method proposed in this article is different from that generated by the A-star method and the original Dijkstra method. Since it is not easy to obtain much information through the trajectory curves, the quantitative experiment results are also necessary to be compared.
The heading angle represents the direction of the vehicle. The heading angle of the starting point and the ending point of the vehicle should be the same. The yaw value represents the rate of deflection change around the vertical axis. It has to be noticed that if the yaw value of the truck changes too fast, the sideslip or tail-flick will occur. As is shown in Figure 15, the heading angles and the yaw values of the three methods are calculated, respectively.
The purple curve represents the improved Dijkstra method. The vehicle heading angle increases from 0 to 19:3785 , then decreases to 0 . The starting heading angle is the same as the ending heading angle. The minimum yaw value is 0:8899 rad s À1 , and the maximum yaw is 1 rad s À1 .
The blue curve indicates the Dijkstra method. The vehicle heading angle increases from 0 to 21:8612 , decreases to 0 before the vehicle arrives at the end point, and then overshoots to À11:3099 . The starting heading angle is different from the ending heading angle, which does not meet the requirement of the lane change scene. The minimum yaw value is 0:8613 rad s À1 , and the maximum yaw is 1 rad s À1 .
Similarly, the cyan curve represents the A-star method. The vehicle heading angle increases from 0 to 20:556 , and then also overshoots to À5:7106 . The heading angle of the starting point and the ending point does not meet the actual vehicle orientation. The minimum yaw value is 0:8767 rad s À1 , and the maximum yaw is 1 rad s À1 .To ensure the smoothness of the driving trajectory, it is necessary to judge the curvature of the trajectory. The smaller the curvature of the planning trajectory curve, the larger the    lane radius and the better the ride comfortness. Otherwise, if the curvature of the planning trajectory curve is too large, the vehicle will roll over, which has a great impact on safety.
Furthermore, the curvature of the trajectory at each step should be considered comprehensively. The curvature k at point ðx; yÞ is kðtÞ ¼ x 0 ðtÞy 0 0 ðtÞ À x 0 0 ðtÞy 0 ðtÞ x 0 ðtÞ 2 þ y 0 ðtÞ 2 3=2 As is shown in Figure 16, the curvature of the trajectory is continuous. Among them, the curvature variation range of the improved Dijkstra method is ½À0:0053; 0:0053 m À1 , the curvature variation range of the Dijkstra method is ½À0:0075; 0:0067 m À1 , and the curvature variation range of the A-star method is ½À0:0066; 0:0060 m À1 .
Comparisons are presented in Table 1 to show the performance parameters of the three methods. It can be seen that the maximum heading angle of the improved Dijkstra method is 19:3785 , and it is smaller than the other two methods. The variation range of heading angle is smaller; the deviation of the vehicle is smaller, which is more conducive to the motion control of the vehicle. In addition, it meets the same requirements as the starting point and the ending point. The minimum yaw value of the improved Dijkstra method is larger than the other two methods, the yaw value decreases from 1 rad s À1 to 0:8899 rad s À1 and then returns to 1 rad s À1 . The variation range of the yaw value is smaller, the impact on lateral vehicle control is smaller, and the comfortness is better. Similarly, the curvature of the improved Dijkstra method is the smallest and less than the limit of 0:08m À1 . Therefore, the vehicle motion control performance of this method is more effective.
Through comparisons of the results, it can be concluded that the improved Dijkstra method in this article is more suitable for the current trajectory planning of autonomous trucks than Dijkstra and A-star methods. It can generate an effective and smooth trajectory to help the truck change lane.

Conclusions and future work
This article proposes a new trajectory generation method, which combines the Dijkstra algorithm, the cost function model, and the Bezier curve. The trajectory planning model is used for a real truck experiment. The results of the road experiment demonstrate that the trajectory planning method in this article can realize effective and smooth vehicle lane changing and some basic autonomous functions in simple industrial parks and ports.   However, with the trajectory in complex scenes being too complicated, the real-time performance of this algorithm cannot meet the requirements of the complex road. Therefore, in future work, we will delve into the real-time strategy of RRT and study how to integrate RRT real-time strategy into the Dijkstra algorithm or cost function model. In addition, we hope that the application of the fifth-order Bezier curve into the trajectory algorithm can enable vehicles to be more effective and smoother when changing lanes, which is more similar to the driving method of humans. In view of the problem that the amount of computation caused by the fifth-order Bezier curve is greatly improved, we consider improving the computing capacity of the controller or optimize the Bezier curve.
Authors' note FZ and RX are co-first authors.

Declaration of conflicting interests
The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.