Feasible Trajectories Generation for Autonomous Driving Vehicles

This study presents smooth and fast feasible trajectory generation for autonomous driving vehicles subject to the vehicle physical constraints on the vehicle power, speed, acceleration as well as the hard limitations of the vehicle steering angle and the steering angular speed. This is due to the fact the vehicle speed and the vehicle steering angle are always in a strict relationship for safety purposes, depending on the real vehicle driving constraints, the environmental conditions, and the surrounding obstacles. Three different methods of the position quintic polynomial, speed quartic polynomial, and symmetric polynomial function for generating the vehicle trajectories are presented and illustrated with simulations. The optimal trajectory is selected according to three criteria: Smoother curve, smaller tracking error, and shorter distance. The outcomes of this paper can be used for generating online trajectories for autonomous driving vehicles and auto-parking systems.


Introduction
Path and trajectory planning and generation are the most important parts of navigating in autonomous vehicles and mobile robots. In an autonomous driving system, a feasible path is referred to as the geometric construction, and the global plan is used to map out the curve moving from point A to point B while taking into account the avoidance of obstacles or overcoming complex mazes. Meanwhile, the trajectory is a piece in a path, or trajectories of the path, and are designed locally including specified velocities in each position. All trajectories must be joined smoothly, or a trajectory must be designed in account to connect the two adjunction trajectories smoothly and continuously. This paper provides some discrete mathematic polynomial methods to map out feasible trajectories from a given starting position to a given destination position subject to the vehicle's physical and environmental limitations.
The main aim of this paper is to develop a real-time trajectory planning for an autonomous vehicle based on the global path given from the GPS positioning and LIDAR detecting systems. The generation of feasible and the optimal trajectories is based on the online calculation of the vehicle's updated information such as the position, velocity, and steering angular speed, etc. In this paper, we assumed that the vehicle steering wheel is designed based on the vehicle dynamics in [1]. A typical passenger vehicle is selected with a steering ratio of 15.0 and a maximum turn on the right and left of the steering wheel of +/−675 degrees. Therefore, the front wheel angles will be limited to +/−45 degrees. From now on, the vehicle steering angle is considered as the front wheel angle and has a physical limitation of +/−45 degrees. references on vehicle trajectory generation and advanced autonomous vehicle control techniques are referred to in [21][22][23][24][25][26][27][28][29][30][31].
Online optimal trajectory generation subject to a vehicle's physical constraints and environmental limitations remain a challenge. This paper proposes new methods of trajectory generations subject to those constraints. The optimal trajectory is selected based on three criteria: The shortest distance, the smoothest curve, and the smallest tracking error. The outline of this paper is as follows: Section 2 outlines the conventional kinematic diagram and control model for the ground vehicle; Section 3 introduces a basic method for trajectory generation; Section 4 presents the selection of the optimal trajectory subject to the vehicle constraints and environmental limitations; Section 5 introduces a new method for quartic polynomial trajectory generation; Section 6 presents a new method for symmetric polynomial function; Section 7 reviews the performances comparison of all three methods; and Section 8 includes conclusions and recommendations.

Vehicle Kinematic Model
A typical passenger vehicle can be modelled as a simple draw of four wheels, where two front wheels are connected to the steering system, φ, and rotate in a maximum angle range of +/−45 • (degrees) to the vehicle body. The distance between the center of the front wheels and the center of the rear wheels is called the vehicle wheelbase (l). In our model, we select the wheelbase length of l = 2 m. The wheel radius of four wheels is assumed to be r = 0.25 m. The vehicle position (x and y) is identified at the center of the rear wheels. The vehicle body angle, θ, is also identified along the axe x. It is assumed that the vehicle position is always identical to the GPS and LIDAR systems with exact x and y coordinates. The vehicle body angle θ and the steering angle ψ are assumed to also always be identical to the 3D sensors. The kinematic model of a vehicle is shown in Figure 1. information technologies applied for mapping out the feasible paths of autonomous vehicles. Updated references on vehicle trajectory generation and advanced autonomous vehicle control techniques are referred to in [21][22][23][24][25][26][27][28][29][30][31].
Online optimal trajectory generation subject to a vehicle's physical constraints and environmental limitations remain a challenge. This paper proposes new methods of trajectory generations subject to those constraints. The optimal trajectory is selected based on three criteria: The shortest distance, the smoothest curve, and the smallest tracking error. The outline of this paper is as follows: Section 2 outlines the conventional kinematic diagram and control model for the ground vehicle; Section 3 introduces a basic method for trajectory generation; Section 4 presents the selection of the optimal trajectory subject to the vehicle constraints and environmental limitations; Section 5 introduces a new method for quartic polynomial trajectory generation; Section 6 presents a new method for symmetric polynomial function; Section 7 reviews the performances comparison of all three methods; and Section 8 includes conclusions and recommendations.

Vehicle Kinematic Model
A typical passenger vehicle can be modelled as a simple draw of four wheels, where two front wheels are connected to the steering system, , and rotate in a maximum angle range of +/−45° (degrees) to the vehicle body. The distance between the center of the front wheels and the center of the rear wheels is called the vehicle wheelbase (l). In our model, we select the wheelbase length of l = 2 m. The wheel radius of four wheels is assumed to be r = 0.25 m. The vehicle position (x and y) is identified at the center of the rear wheels. The vehicle body angle, , is also identified along the axe x. It is assumed that the vehicle position is always identical to the GPS and LIDAR systems with exact x and y coordinates. The vehicle body angle and the steering angle are assumed to also always be identical to the 3D sensors. The kinematic model of a vehicle is shown in Figure 1. The vehicle moves forward and reverses and is controlled by the engine angular and steering angular velocity. A vehicle driven by the rear wheels and moving forward is presented in Equation (1): The vehicle moves forward and reverses and is controlled by the engine angular and steering angular velocity. A vehicle driven by the rear wheels and moving forward is presented in Equation (1): The vehicle driven by the rear wheels and moving in reverse is obtained in Equation (2), where the vehicle speed is assigned in a negative sign: The vehicle driven by the front wheels is formulated in Equation (3): where [x, y, θ, φ] are the state and output variables. The two control inputs are v 1 and v 2 , where v 1 is the angular speed of the driven wheels, therefore rv 1 is the vehicle speed (km/h); and v 2 is the steering angular speed in revolutions per minute (rpm). The two models in Equations (1) and (2) are mainly used to calculate and simulate the vehicle performances. The next part presents a basic method for trajectory generation.

Position Quintic Polynomial for Trajectory Generation
A basic position quintic polynomial for trajectory generation can be achieved from the nonlinear vehicle movement curve along the x and y axes on the flat system as referred to in [21]. Then, the control input v 1 can be identified as: The vehicle body angle, θ, along the x axis can be formulated: Then, the vehicle body angular velocity, . θ, will always be identical: The vehicle steering angle φ and the body angle θ in Equations (5) and (6)  y. Similarly, all state variables and inputs can be represented by the output coordinates x and y.
It is assumed that the vehicle moves from the starting point at time t = 0 to the destination point at t = T for x(0), y(0) and x(T), y(T) from t = 0 ÷T at the starting point of and at the starting point of y(t) = y(0) At the destination point y(t) = y(T) It is assumed that > 0, then the trajectory of x(t) can be chosen by the following Equation: Then, y(t) can be chosen as where Then, the vehicle body angle, θ, is and the vehicle steering angle, φ, is The vehicle velocity and acceleration can be re-formulated from (10) and (11) as and ..
and for variable x: . and ..
The vehicle velocity can be identified from Equations (14)- (17): Or another way to calculate v 1 (t) = .
φ is: Simulations for this method were conducted with the above vehicle parameters of l = 2 m, r = 0.25 m, x(0) = [0, 0, 0, 0] , x(T) = 10, 10, 0, π 6 , and T = 100 (T is calculated for 100 discrete time intervals from t = 0÷T). The trajectory generation for x, y, θ, φ, Or another way to calculate 1 ( ) =̇( ) +̇( ) , then: ̇ is re-formulated from Equation (6): To calculate 2 ( ) =̇, from Equation (13), when = ( ), Then, the second input 2 ( ) =̇ is: Simulations for this method were conducted with the above vehicle parameters of      Figure 3 shows the maximum steering angle, = 1.0973 radians or 62.87° (degrees). This value violated the constraint for the vehicle steering angle ≤ 45°. This trajectory, therefore, is infeasible because the distance from x°, y° to x T and y T is too short, causing the steering angle to be too large. The next part presents the method to achieve feasible and optimal trajectories subject to vehicle constraints and environmental limitations.

Trajectory Generation Subject to Constraints
For a real vehicle, the steering angle is always limited from Once the vehicle moves from [ 0 , 0 , 0 , 0 ] to [ , , , ], it is assumed that the initial conditions of [ 0 , 0 , 0 , 0 ] are given and the final conditions [ , ] cannot be changed. The vehicle violates the constraint because the distance from [ 0 , 0 ] to [ , ] is too short. By using iterations from computer programming, it is easy to determine the feasible trajectories with lower values of and ̇ or the vehicle will move on smoother and safer curves.
As mentioned in [1], the front wheel angle always has hard constraints. At a low vehicle velocity of less than 16 km/h, the vehicle can be driven with the maximum steering angle. However, in higher speeds, the steering angles allowed will be reduced from +/−45 to +/−12 degrees when the vehicle velocity reaches 40 km/h. At a vehicle speed higher 67 km/h, the steering angle is restricted to less than only +/−3.5 degrees as shown in Figure  4.   • . This trajectory, therefore, is infeasible because the distance from x • , y • to x T and y T is too short, causing the steering angle to be too large. The next part presents the method to achieve feasible and optimal trajectories subject to vehicle constraints and environmental limitations.

Trajectory Generation Subject to Constraints
For a real vehicle, the steering angle is always limited from Once the vehicle moves from [x 0 , y 0 , θ 0 , φ 0 ] to [x T , y T , θ T , φ T ], it is assumed that the initial conditions of [x 0 , y 0 , θ 0 , φ 0 ] are given and the final conditions [θ T , φ T ] cannot be changed. The vehicle violates the constraint φ because the distance from [x 0 , y 0 ] to [x T , y T ] is too short. By using iterations from computer programming, it is easy to determine the feasible trajectories with lower values of φ and . φ or the vehicle will move on smoother and safer curves.
As mentioned in [1], the front wheel angle always has hard constraints. At a low vehicle velocity of less than 16 km/h, the vehicle can be driven with the maximum steering angle. However, in higher speeds, the steering angles allowed will be reduced from +/−45 to +/−12 degrees when the vehicle velocity reaches 40 km/h. At a vehicle speed higher 67 km/h, the steering angle is restricted to less than only +/−3.5 degrees as shown in Figure 4.
The vehicle speed always has to be controlled in a strict function of the steering angle due to safety purposes, therefore this paper proposes a new method for generating the vehicle's feasible trajectories subject to the hard steering angular constraints and the environmental conditions as follows:  The vehicle speed always has to be controlled in a strict function of the steering angle due to safety purposes, therefore this paper proposes a new method for generating the vehicle's feasible trajectories subject to the hard steering angular constraints and the environmental conditions as follows: The feasible trajectories x(t) and y(t) from starting points x(0) and y(0) to the destination point x(T) and y(T) for t = 0 ÷ T subject to Equation (23) and other constraints in [1] generated from Equations (10) and (11) can be modified by adding iterations to lengthen the trajectory distance. The programming calculation for each feasible trajectory must be verified online and then the steering angle and other variables must be re-calculated to satisfy the objective constraints. If certain constraints are violated, the distance from the starting point to the destination point is too short to generate a safe and feasible trajectory. In this case, we must lengthen the trajectory distance, 0 = √( − 0 ) 2 + ( − 0 ) 2 to a new, longer distance with a small amplification coefficient : Several iterations, = 1,2,3, . . . are required until the steering and other condition constraints are satisfied. Then, the new destination position of the vehicle will be: The following simulations are conducted with = 1.1, and the vehicle parameters are the same as in previous simulations. By adding constraint (23) and other vehicle dynamic constraints, feasible trajectories are obtained in Figure 5  The feasible trajectories x(t) and y(t) from starting points x(0) and y(0) to the destination point x(T) and y(T) for t = 0 ÷ T subject to Equation (23) and other constraints in [1] generated from Equations (10) and (11) can be modified by adding iterations to lengthen the trajectory distance. The programming calculation for each feasible trajectory must be verified online and then the steering angle φ and other variables must be re-calculated to satisfy the objective constraints. If certain constraints are violated, the distance from the starting point to the destination point is too short to generate a safe and feasible trajectory.
In this case, we must lengthen the trajectory distance, d 0 = (x T − x 0 ) 2 + (y T − y 0 ) 2 to a new, longer distance with a small amplification coefficient ρ: Several iterations, i = 1, 2, 3, . . . n are required until the steering and other condition constraints are satisfied. Then, the new destination position of the vehicle will be: The following simulations are conducted with ρ = 1.1, and the vehicle parameters are the same as in previous simulations. By adding constraint (23) and other vehicle dynamic constraints, feasible trajectories are obtained in Figure 5 Figure 6 shows the steering angles during the time that the vehicle is moving along different trajectories with different maximum steering angles. The lower and smoother steering angles will lead to safer trajectories. According to the environmental limitations, the optimal trajectory can be selected based on the safest steering angle, the shortest distance, and the smallest tracking error, etc.  Figure 6 shows the steering angles during the time that the vehicle is moving different trajectories with different maximum steering angles. The lower and sm steering angles will lead to safer trajectories. According to the environmental limit the optimal trajectory can be selected based on the safest steering angle, the short tance, and the smallest tracking error, etc. Once we have calculated and selected an optimal feasible trajectory subject straints, we can again determine the steering angle by the vehicle velocity moving this trajectory. Figure 7 shows the graphic of the vehicle steering angle vs. the velocity subject to the hard constraint for the steering angle less than 45°. In this ca vehicle moves along this trajectory with the maximum steering angle of 42.8547°. T destination of the vehicle is = = 23.559 and this feasible trajectory is foun four interactions with = 4 trials in (24) and (25).   Figure 6 shows the steering angles during the time that the vehicle is moving different trajectories with different maximum steering angles. The lower and sm steering angles will lead to safer trajectories. According to the environmental limit the optimal trajectory can be selected based on the safest steering angle, the shorte tance, and the smallest tracking error, etc. Once we have calculated and selected an optimal feasible trajectory subject straints, we can again determine the steering angle by the vehicle velocity moving this trajectory. Figure 7 shows the graphic of the vehicle steering angle vs. the velocity subject to the hard constraint for the steering angle less than 45°. In this ca vehicle moves along this trajectory with the maximum steering angle of 42.8547°. Th destination of the vehicle is = = 23.559 and this feasible trajectory is foun four interactions with = 4 trials in (24) and (25). Once we have calculated and selected an optimal feasible trajectory subject to constraints, we can again determine the steering angle by the vehicle velocity moving along this trajectory. Figure 7 shows the graphic of the vehicle steering angle vs. the vehicle velocity subject to the hard constraint for the steering angle less than 45 • . In this case, the vehicle moves along this trajectory with the maximum steering angle of 42.8547 • . The new destination of the vehicle is x TN = y TN = 23.559 and this feasible trajectory is found after four interactions with n = 4 trials in (24) and (25).
The next part proposes a new quartic polynomial trajectory generation.
The next part proposes a new quartic polynomial trajectory generation.

Speed Quartic Polynomial Trajectory Generation
From reference [22] of trajectory generation for a nonholonomic system, for and smoother feasible trajectory generation, we propose a quartic function for gene the vehicle feasible trajectory: The vehicle dynamics in Equation (1) can be separated into four variables, 1 , and 4 .

Speed Quartic Polynomial Trajectory Generation
From reference [22] of trajectory generation for a nonholonomic system, for faster and smoother feasible trajectory generation, we propose a quartic function for generating the vehicle feasible trajectory: The vehicle dynamics in Equation (1) can be separated into four variables, z 1 , z 2 , z 3 , and z 4 . For 0 ≤ t ≤ T, the identification of [z 1 (t), z 2 (t), z 3 (t), z 4 (t)] is: and and z 3 (t) = z 3.0 + gz 2.0 t + 1 2 and The new trajectory will be achieved from Equations (35)-(42): and and . x and .
and v 1 (t) = . x and from Equation (27 Figure 9 shows the steering angle, , and the vehicle body angle, , corresponding to the angular velocity ̇, and ̇.    Figure 9 shows the steering angle, , and the vehicle body angle, , corresponding to the angular velocity ̇, and ̇. As shown in Figure 9, this feasible trajectory requires a maximum steering angle of = 41.5736° and satisfied the steering angle constraint, − 45°≤ ≤ 45°. Thus, this method is better than the previous method in part 3 with = 62.870° that violated the vehicle steering angle constraint. However, in this method, the vehicle velocity is increasing and not in a smooth curve. In reality, the speed of vehicle must be controlled under smooth changes. The next part proposes another method, which can generate smoother velocity changes.

Symmetric Polynomial Trajectory Generation
This part presents a new method of symmetric trajectory generation for smoother, safer, and easier to control for autonomous vehicle tracking on its trajectory. This method As shown in Figure 9, this feasible trajectory requires a maximum steering angle of φ MAX = 41.5736 • and satisfied the steering angle constraint, −45 • ≤ φ ≤ 45 • . Thus, this method is better than the previous method in part 3 with φ MAX = 62.870 • that violated the vehicle steering angle constraint. However, in this method, the vehicle velocity is increasing and not in a smooth curve. In reality, the speed of vehicle must be controlled under smooth changes. The next part proposes another method, which can generate smoother velocity changes.

Symmetric Polynomial Trajectory Generation
The vehicle velocity: From Equations (49) and (50), for simplicity, we assumed that the coefficients of velocity at the start and destination points, k 0 = k T = k, then and similarly a y = ksinθ T − 3y T , and b y = ksinθ 0 − 3y 0 (52) Simulation of this method is conducted with the same vehicle parameters in previous examples and shown in Figures 10 and 11. The velocity coefficients are set at k 0 = k T = k = 1. Other parameters regarding the vehicle angular velocity, v 1 (t), the vehicle body angle, θ(t), the vehicle body angular velocity, . θ(t), the vehicle steering angle, φ(t), and the vehicle steering angular velocity, . φ(t) are calculated using the following Equations: and θ = arctan The results of this method were observed to be better than previous methods: The vehicle trajectory is smoother. The vehicle speed also increases smoothly from the starting point and reaches the maximum speed in the middle of the trajectory. Then, the speed is reduced as the vehicle moves to the destination point, as is shown in Figure 10. This method appears to be more realistic and controls the vehicle tracking in its trajectory more easily. The body and steering angle and the angular velocity are shown in Figure 11. As shown in Figure 11, the maximum steering angle for this method is = 40.1622° and is better than the method in part 5 with = 41.5736°. Therefore, the trajectory in Figure 10 is smoother and safer than the previous methods. The next part presents the performance comparisons of all three methods.

Performances Comparison
The comparison of performances for all three methods is shown in Figures 12 and 13 for trajectory curves, velocities, the steering angle/velocity, and the body angle/velocity. The results of this method were observed to be better than previous methods: The vehicle trajectory is smoother. The vehicle speed also increases smoothly from the starting point and reaches the maximum speed in the middle of the trajectory. Then, the speed is reduced as the vehicle moves to the destination point, as is shown in Figure 10.
This method appears to be more realistic and controls the vehicle tracking in its trajectory more easily. The body and steering angle and the angular velocity are shown in Figure 11.
As shown in Figure 11, the maximum steering angle for this method is φ = 40.1622 • and is better than the method in part 5 with φ = 41.5736 • . Therefore, the trajectory in Figure 10 is smoother and safer than the previous methods. The next part presents the performance comparisons of all three methods.

Performances Comparison
The comparison of performances for all three methods is shown in Figures 12 and 13 for trajectory curves, velocities, the steering angle/velocity, and the body angle/velocity. As shown in Figure 11, the maximum steering angle for this method is = 40.1622° and is better than the method in part 5 with = 41.5736°. Therefore, the trajectory in Figure 10 is smoother and safer than the previous methods. The next part presents the performance comparisons of all three methods.

Performances Comparison
The comparison of performances for all three methods is shown in Figures 12 and 13 for trajectory curves, velocities, the steering angle/velocity, and the body angle/velocity.  Figure 12 shows the feasible trajectory of the symmetric polynomial method is the smoothest, and the velocity of this method is also symmetrical, increasing from 0 and reducing to 0. Therefore, this method is useful in the application of auto-parking systems. Figure 13 shows the comparison of the body and steering angles and angular velocity. This symmetric method also provides the smoothest curve of the body and steering  Figure 12 shows the feasible trajectory of the symmetric polynomial method is the smoothest, and the velocity of this method is also symmetrical, increasing from 0 and reducing to 0. Therefore, this method is useful in the application of auto-parking systems. Figure 13 shows the comparison of the body and steering angles and angular velocity. This symmetric method also provides the smoothest curve of the body and steering angles and velocity.
A vehicle's feasible trajectories are the continuous segments connected to each other along the vehicle's feasible path. All trajectories are required to join each other smoothly and continuously. The control of vehicle tracking along its feasible trajectories is performed by the control of the vehicle velocity and the steering angle. The following are the simulations of a vehicle's auto-parking performance with the symmetric polynomial trajectory generated from a starting point of x(0), y(0) to a destination point of x(−6), y(3). Figure 14 shows the vehicle moving in reverse and the steering angle φ moving from positive to negative and returning to 0 position. The steering angle, −45 • ≤ φ ≤ 45 • , satisfied the constraint as per the vehicle dynamic moving in reverse in Equation (2). Simulations for the vehicle moving in reverse speeds are also displayed in Figure 15 As mentioned in Equation (2), the reverse speed is a negative sign. Then the velocity co efficients in Equations (51) and (52) must be negative values. Simulations for the vehicle in reverse speeds are conducted with the symmetric polynomial method. The velocity co efficients are assigned for 0 = = = − 1. Simulations for the vehicle moving in reverse speeds are also displayed in Figure 15. As mentioned in Equation (2), the reverse speed is a negative sign. Then the velocity coefficients in Equations (51) and (52) must be negative values. Simulations for the vehicle in reverse speeds are conducted with the symmetric polynomial method. The velocity coefficients are assigned for k 0 = k T = k = −1.
Since this simulation assigns the vehicle reversing at the start, for k 0 = −1, and at the destination, for k T − 1, Figure 15 shows the vehicle reversing at the starting point then moving forward to the destination, then reversing to the destination. The vehicle velocities change the sign three times accordingly.
All three methods of feasible trajectory generation subject to vehicle constraints and environmental limitations are presented and illustrated with simulations. The next part outlines the conclusions and recommendations drawn from this paper.
Simulations for the vehicle moving in reverse speeds are also displayed in Figure 15. As mentioned in Equation (2), the reverse speed is a negative sign. Then the velocity coefficients in Equations (51) and (52) must be negative values. Simulations for the vehicle in reverse speeds are conducted with the symmetric polynomial method. The velocity coefficients are assigned for 0 = = = − 1. Since this simulation assigns the vehicle reversing at the start, for 0 = − 1, and at the destination, for − 1, Figure 15 shows the vehicle reversing at the starting point then moving forward to the destination, then reversing to the destination. The vehicle velocities change the sign three times accordingly.
All three methods of feasible trajectory generation subject to vehicle constraints and environmental limitations are presented and illustrated with simulations. The next part outlines the conclusions and recommendations drawn from this paper.

Conclusions
This paper presents three methods of the position quintic polynomial, the speed quartic polynomial, and the symmetric polynomial function for vehicle trajectory generation subject to a vehicle's physical and environment constraints regarding the real vehicle parameters and the real vehicle dynamics. The feasible trajectory subject to constraints can be achieved online to smoothen the steering angle and the vehicle speed. The optimal trajectory is selected based on the shortest distance, the lowest steering angle, and the minimal tracking error. Simulations for each method are presented and compared. Results show that the symmetric polynomial trajectory generation produces a smoother curve and symmetric vehicle velocity. Therefore, this method can be applied to the design of vehicle auto-parking systems. Due to the size of this paper, the vehicle tracking errors are still not presented. The next study will concentrate on control methods to minimize vehicle tracking errors along their trajectories in the fastest time subject to the vehicle's physical constraints and environmental limitations.