Feasible Trajectories Generation for Autonomous Driving Vehicles

Abstract

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.

1. 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.

The body of literature for recent autonomous vehicle paths and trajectory generation is enormous. This paper reviews some update information on the model predictive controls, optimal iterative calculations for real-time control of autonomous ground vehicles, auto-driving, and auto-parking systems. Basic mathematic functions to determine feasible trajectories were presented in [2], in which the authors introduced different mathematical methods for generating optimal paths. The vehicle was controlled and simulated moving on its feasible paths, including forward and reverse velocity, into and out of parking places. However, Authors in [2] lacked the generation of continuing trajectories and online calculations for feasible and optimal trajectories subject to the vehicle’s dynamic constraints.

Vehicle steering dynamic control and simulation are referred to in [3]. In [3], the author provided analyses and simulations of sideslips and the steering angle depending on the vehicle velocity and the environmental limitations. The vehicle steering angle in forward and reverse speeds were also analyzed and simulated. Fundamental parameters of vehicles regarding the position of the gravity center, the stiffness of front and rear wheels, under-steering and over-steering conditions, as well as the sideslip dynamics were also included in the analyses and calculations. Therefore, data from [3] is useful for generating the vehicle trajectories subject to physical constraints.

Regarding the model predictive control for vehicles’ tracking setpoints, reference [4] is considered. The authors in [4] presented a fundamental new robust model of predictive control for uncertain and constrained systems. The tracking setpoints of the robust model predictive control for input-saturated and softened-state constraints were introduced. The softened-state constraints showed higher superiority since, in many cases, the autonomous vehicle might be allowed to violate certain constraints while being subject to heavy penalty conditions. After this, the control system will find it easier to determine the optimal solution and minimize the tracking error.

Recent studies on vehicle trajectory generation and tracking control are referred as following: Reference [5] introduced the problem of tracking trajectories under certain traffic conditions. The design of feasible paths was performed step by step based on the traffic environments. The closed-loop control was determined by a Proportional Integral Derivative (PID) controller. The trajectory generation and the tracking path controller were feasible but lacked optimal calculation and were not subject to vehicle constraints. A new study on vehicle trajectory generation for autonomous systems subject to emergency situations was referred to in [6]. Reference [6] presented an efficient solution to improve the vehicle control subject to emergency situations from the autonomous decision support. However, authors in [6] did not provide mathematic calculations for generating the vehicle optimal trajectory.

Recent research references on vehicle trajectory generation are investigated, most of which presented new mathematic functions for generating online feasible trajectories. The authors in [7] updated the prediction and generation of vehicle trajectory in non-linear programming with consideration of the long short-term memory and generative adversarial networks. The authors in [8] presented trajectory generation for a vehicle changing lanes automatically with nonlinear mathematical functions while taking into account the public traffic laws. The authors in [9] introduced a new method for robust trajectory generation based on neuromorphic research and robotic control. The authors in [10] introduced a new model of predictive control for autonomous vehicle tracking of any given path. The authors in [11] presented a new approach for industrial robot trajectory generation based on motion modular technology. The authors in [12] reviewed recent advances in the generation of trajectory planning methodologies. The authors in [13] described the new method in motion planning for mobile robots with the use of jumping leap search with Bezier curves. The authors in [14] highlighted robust and the efficient tool path generation for curving along low triangular mesh surfaces. The authors in [15] presented research on trajectory generation optimization for robotics and autonomous systems. Finally, the authors References [16,17,18,19,20] presented updated mathematical algorithms and 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.

2. 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, $\varphi$, 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, $\theta$, 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):

$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\theta }\\ \dot{\varphi }\end{array}\right]=\left[\begin{array}{c}cos\theta \\ sin\theta \\ \frac{tan\varphi }{l}\\ 0\end{array}\right]r{v}_1+\left[\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}\right]v_2$$

(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:

$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\theta }\\ \dot{\varphi }\end{array}\right]=-\left[\begin{array}{c}cos\theta \\ sin\theta \\ \frac{tan\varphi }{l}\\ 0\end{array}\right]r{v}_1+\left[\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}\right]v_2$$

(2)

The vehicle driven by the front wheels is formulated in Equation (3):

$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\theta }\\ \dot{\varphi }\end{array}\right]=\left[\begin{array}{c}cos\theta cos\varphi \\ sin\theta cos\varphi \\ \frac{tan\varphi }{l}\\ 0\end{array}\right]r{v}_1+\left[\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}\right]v_2$$

(3)

where ${\left[x,y,\theta ,\varphi \right]}^{\prime }$ 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 $r{v}_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.

3. 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:

$$r{v}_1=\sqrt{{\dot{x}}^2+{\dot{y}}^2}\Rightarrow v_1=\frac{\sqrt{{\dot{x}}^2+{\dot{y}}^2}}{r}$$

(4)

The vehicle body angle, $\theta$, along the x axis can be formulated:

$$\frac{\dot{y}}{\dot{x}}=\frac{r{v}_{1}sin\theta }{r{v}_{1}cos\theta }=tan\theta \Rightarrow \theta =arctan\left(\frac{\dot{y}}{\dot{x}}\right)$$

(5)

Then, the vehicle body angular velocity, $\dot{\theta }$, will always be identical:

$$\begin{gathered} \dot{\theta }=\frac{\ddot{y}\dot{x} - \ddot{x}\dot{y}}{{\dot{x}}^2}\frac{1}{{\left(\frac{\dot{y}}{\dot{x}}\right)}^2+1}=\frac{\ddot{y}\dot{x}-\ddot{x}\dot{y}}{{\dot{x}}^2+{\dot{y}}^2}=\frac{tan\varphi }{l}r{v}_1\Rightarrow \varphi \\ =arctan\left(l\frac{\ddot{y}\dot{x}-\ddot{x}\dot{y}}{r{\left({\dot{x}}^2+{\dot{y}}^2\right)}^{\frac{3}{2}}}\right) \end{gathered}$$

(6)

The vehicle steering angle $\varphi$ and the body angle $\theta$ in Equations (5) and (6) can be formulated directly from $\dot{x}$, $\ddot{x}$, and $\dot{y}$, $\ddot{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 x(t) = x(0):

$$x\left(0\right)=x_0\Rightarrow x\left(T\right)=x_T$$

(7)

and at the starting point of y(t) = y(0)

$$y\left(0\right)=y_0=tan{\theta }_0\Rightarrow \frac{{\partial }^{2}y}{\partial x^2}\left|t=0\right.=\frac{tan{\varphi }_0}{l{cos}^3{\theta }_0}$$

(8)

At the destination point y(t) = y(T)

$$y\left(T\right)=y_T=tan{\theta }_T\Rightarrow \frac{{\partial }^{2}y}{\partial x^2}\left|t=T\right.=\frac{tan{\varphi }_T}{l{cos}^3{\theta }_T}$$

(9)

It is assumed that $\frac{\left|x_T-x_0\right|}{2T}>0$, then the trajectory of x(t) can be chosen by the following Equation:

$$x\left(t\right)=\left(\frac{T-t}{T}\right)x_0+\frac{t}{T}{x}_T+\left|x_T-x_0\right|\frac{t\left(t-T\right)}{2T^2}$$

(10)

Then, y(t) can be chosen as

$$y\left(t\right)=y_0+t{\alpha }_{1}tan{\theta }_0+t^2\frac{{\alpha }_{2}tan{\varphi }_0}{2l{cos}^3{\theta }_0}+t^3{b}_1+t^4{b}_2+t^5{b}_3$$

(11)

where ${\alpha }_1=\frac{2\left(x_T-x_0\right)-\left|x_T-x_0\right|}{2T}$, ${\alpha }_2=\frac{\left|x_T-x_0\right|}{T^2}$, and ${\alpha }_3=\frac{2\left(x_T-x_0\right)+\left|x_T-x_0\right|}{2T}$; and $b={\left[b_1,b_2,b_3\right]}^{\prime }=A^{-1}c$, in which:

$$A=\left[\begin{array}{ccc}{T}^3& T^4& T^5\\ 3T^2& 4T^3& 5T^4\\ 6T& 12T^2& 20T^3\end{array}\right] \mathrm{and} c=\left[\begin{array}{c}{y}_T-y_0-T{\alpha }_{1}tan{\theta }_0-T^2\frac{{\alpha }_{2}tan{\varphi }_0}{2l{cos}^3{\theta }_0}\\ {\alpha }_{3}tan{\theta }_T-{\alpha }_{1}tan{\theta }_0-T\frac{{\alpha }_{2}tan{\varphi }_0}{l{cos}^3{\theta }_0}\\ \frac{{\alpha }_{2}tan{\varphi }_T}{l{cos}^3{\theta }_T}-\frac{{\alpha }_{2}tan{\varphi }_0}{l{cos}^3{\theta }_0}\end{array}\right]$$

Then, the vehicle body angle, $\theta$, is

$$\theta =arctan\left(\frac{2T^2\left({\alpha }_{1}tan{\theta }_0+\frac{{\alpha }_{2}tan{\varphi }_0}{l{cos}^3{\theta }_0}t+3b_1{t}^2+4b_2{t}^3+5b_3{t}^4\right)}{\left(2T\left(x_T-x_0\right)-T\left|x_T-x_0\right|+2\left|x_T-x_0\right|t\right)}\right)$$

(12)

and the vehicle steering angle, $\varphi$, is

$$\begin{array}{c}\varphi =arctan\left(\frac{{\partial }^2\left(y\right)l{cos}^3\theta }{\partial x^2}\right)\hfill \\ =arctan\left(\frac{\left(\frac{{\alpha }_{2}tan{\varphi }_0}{l{cos}^3{\theta }_0}+6b_{1}t+12b_2{t}^2+20b_3{t}^3\right){\left(2T^2\right)}^{2}l{cos}^3\theta }{{\left(2T\left(x_T-x_0\right)-T\left|x_T-x_0\right|+2\left|x_T-x_0\right|t\right)}^2}\right)\hfill \end{array}$$

(13)

The vehicle velocity and acceleration can be re-formulated from (10) and (11) as

$$\dot{y}\left(t\right)=\left({\alpha }_{1}tan{\theta }_0+\frac{{\alpha }_{2}tan{\varphi }_0}{l{cos}^3{\theta }_0}t+3b_1{t}^2+4b_2{t}^3+5b_3{t}^4\right)$$

(14)

and

$$\ddot{y}\left(t\right)=\left(\frac{{\alpha }_{2}tan{\varphi }_0}{l{cos}^3{\theta }_0}+6b_{1}t+12b_2{t}^2+20b_3{t}^3\right)$$

(15)

and for variable x:

$$\dot{x}\left(t\right)=\left(\frac{\left(2T\left(x_T-x_0\right)-T\left|x_T-x_0\right|+2\left|x_T-x_0\right|t\right)}{2T^2}\right)$$

(16)

and

$$\ddot{x}\left(t\right)=\left(\frac{\left|x_T-x_0\right|}{T^2}\right)$$

(17)

The vehicle velocity can be identified from Equations (14)–(17):

$$v_1\left(t\right)=\frac{\sqrt{{\dot{x}}^2+{\dot{y}}^2}}{r}$$

(18)

Or another way to calculate $v_1\left(t\right)=\frac{\dot{x}\left(t\right)cos\theta }{r}+\frac{\dot{y}\left(t\right)sin\theta }{r}$, then:

$$\begin{array}{c}{v}_1\left(t\right)=\left(\frac{\left(2T\left(x_T-x_0\right)-T\left|x_T-x_0\right|+2\left|x_T-x_0\right|t\right)}{2r{T}^2}\right)cos\theta \hfill \\ \hfill +\left(\frac{\left({\alpha }_{1}tan{\theta }_0+\frac{{\alpha }_{2}tan{\varphi }_0}{l{cos}^3{\theta }_0}+3b_1{t}^2+4b_2{t}^3+5b_3{t}^4\right)sin\theta }{r}\right)\end{array}$$

(19)

$\dot{\theta }$ is re-formulated from Equation (6):

$$\dot{\theta }=\frac{\ddot{y}\dot{x}-\ddot{x}\dot{y}}{{\dot{x}}^2+{\dot{y}}^2}=\frac{tan\varphi }{l}r{v}_1$$

(20)

To calculate $v_2\left(t\right)=\dot{\varphi }$, from Equation (13), when $\varphi =arctan(X)$,

$$\begin{gathered} \dot{\varphi }=\dot{X}\left(\frac{1}{1+X^2}\right) \mathrm{with} \\ X=\left(\frac{\left(\frac{{\alpha }_{2}tan{\varphi }_0}{l{cos}^3{\theta }_0}+6b_{1}t+12b_2{t}^2+20b_3{t}^3\right){\left(2T^2\right)}^{2}l{cos}^3\theta }{{\left(2T\left(x_T-x_0\right)-T\left|x_T-x_0\right|+2\left|x_T-x_0\right|t\right)}^2}\right) \end{gathered}$$

(21)

Then, the second input $v_2\left(t\right)=\dot{\varphi }$ is:

$$\dot{\varphi }=\partial \left(l\frac{\ddot{y}\dot{x}-\ddot{x}\dot{y}}{r{\left({\dot{x}}^2+{\dot{y}}^2\right)}^{\frac{3}{2}}}\right)\left(\frac{1}{1+{\left(l\frac{\ddot{y}\dot{x}-\ddot{x}\dot{y}}{r{\left({\dot{x}}^2+{\dot{y}}^2\right)}^{\frac{3}{2}}}\right)}^2}\right)$$

(22)

Simulations for this method were conducted with the above vehicle parameters of $l=2 \mathrm{m}$, $r=0.25 \mathrm{m}$, $x\left(0\right)={\left[0,0,0,0\right]}^{\prime }$, $x\left(T\right)={\left[10,10,0,\frac{\pi }{6}\right]}^{\prime }$, and $T=100$ (T is calculated for 100 discrete time intervals from t = 0$÷T$). The trajectory generation for x, y, $\theta$, $\varphi$, $\dot{\theta }$, and $\dot{\varphi }$ is obtained from Equations (12)–(22) and shown in Figure 2 and Figure 3.

Figure 3 shows the maximum steering angle, ${\varphi }_{MAX}=1.0973$ radians or 62.87° (degrees). This value violated the constraint for the vehicle steering angle ${\varphi }_{MAX}\le {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.

4. Trajectory Generation Subject to Constraints

For a real vehicle, the steering angle is always limited from

$$-\frac{\pi }{4}\le \varphi \le \frac{\pi }{4}$$

(23)

Once the vehicle moves from $\left[x_0,y_0,{\theta }_0,{\varphi }_0\right]$ to $\left[x_T,y_T,{\theta }_T,{\varphi }_T\right]$, it is assumed that the initial conditions of $\left[x_0,y_0,{\theta }_0,{\varphi }_0\right]$ are given and the final conditions $[{\theta }_T,{\varphi }_T]$ cannot be changed. The vehicle violates the constraint $\varphi$ because the distance from $\left[x_0,y_0\right]$ to $\left[x_T,y_T\right]$ is too short. By using iterations from computer programming, it is easy to determine the feasible trajectories with lower values of $\varphi$ and $\dot{\varphi }$ 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 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 $\varphi$ 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=\sqrt{{(x_T-x_0)}^2+{(y_T-y_0)}^2}$ to a new, longer distance with a small amplification coefficient $\rho$:

$$d_{Ni}={\rho }^i{d}_0 \mathrm{with} \rho >1$$

(24)

Several iterations, $i=1,2,3,\dots n$ are required until the steering and other condition constraints are satisfied. Then, the new destination position of the vehicle will be:

$$x_{TN}={\rho }^n\left(x_T-x_0\right), \mathrm{and} y_{TN}={\rho }^n\left(y_T-y_0\right).$$

(25)

The following simulations are conducted with $\rho =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 steering angles are assumed to be limited to 60°, 45°, or 30° according to the vehicle and environment safety requirements. The minimum lengths of corresponding feasible trajectories are 15.0910 m, 32.3490 m, and 52.0983 m corresponding to the new destination points of x(T) = y(T) = 10.000, 23.579, and 34.5227, respectively.

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.

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.

5. 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$.

$$z_1=x, z_2=\frac{tan\varphi }{l{cos}^3\theta }, z_3=tan\theta , \mathrm{and} z_4=y$$

(26)

In this,

$${\dot{z}}_1=\dot{x}$$

(27)

and

$$\begin{gathered} {\dot{z}}_2=\frac{\dot{\varphi }\frac{1}{{cos}^2\varphi }l{cos}^3\theta +\dot{\theta }l3{cos}^2\theta sin\theta tan\varphi }{l^2{cos}^6\theta } \\ =\frac{v_{2}l{cos}^2\theta +3r{v}_{1}cos\theta sin\theta {sin}^2\varphi }{l^2{cos}^5\theta {cos}^2\varphi } \end{gathered}$$

(28)

and

$${\dot{z}}_3=\frac{tan\varphi }{l{cos}^2\theta }r{v}_1$$

(29)

and

$${\dot{z}}_4=\dot{y}=sin\theta r{v}_1$$

(30)

The vehicle travels from the starting point $\left(x_0,y_0,{\theta }_0,{\varphi }_0\right)$ at time $t=0$ to the destination point $\left(x_T,y_T,{\theta }_T,{\varphi }_T\right)$ at time $t=T$ similarly to the system in (26) from the new starting point at $\left(z_{1.0},z_{2.0},z_{3.0},z_{4.0}\right)$ to the new destination point at $\left(z_{1.T},z_{2.T},z_{3.T},z_{4.T}\right)$.

For $0\le t\le T$, the identification of $\left[z_1\left(t\right),z_2\left(t\right),z_3\left(t\right),z_4\left(t\right)\right]$ is:

$$z_1\left(t\right)=z_{1.0}+gt$$

(31)

and

$$z_2\left(t\right)=z_{2.0}+h_{1}t+\frac{1}{2}{h}_2{t}^2+\frac{1}{3}{h}_3{t}^3$$

(32)

and

$$z_3\left(t\right)=z_{3.0}+g{z}_{2.0}t+\frac{1}{2}g{h}_1{t}^2+\frac{1}{6}g{h}_2{t}^3+\frac{1}{12}g{h}_3{t}^4$$

(33)

and

$$z_4\left(t\right)=z_{4.0}+g{z}_{3.0}t+\frac{1}{2}{g}^2{z}_{2.0}{t}^2+\frac{1}{6}{g}^2{h}_1{t}^3+\frac{1}{24}{g}^2{h}_2{t}^4+\frac{1}{60}{g}^2{h}_3{t}^5$$

(34)

in which, $g=\frac{z_{1.T}-z_{1.0}}{T}$ and ${[h_1,h_2,h_3]}^{\prime }=D^{-1}e$, and

$$D=\left[\begin{array}{ccc}T& \frac{1}{2}{T}^2& \frac{1}{3}{T}^3\\ \frac{1}{2}g{T}^2& \frac{1}{6}g{T}^3& \frac{1}{12}g{T}^4\\ \frac{1}{6}{g}^2{T}^3& \frac{1}{24}{g}^2{T}^4& \frac{1}{60}{g}^2{T}^5\end{array}\right] \mathrm{and} e=\left[\begin{array}{c}{z}_{2.T}-z_{2.0}\\ z_{3.T}-z_{3.0}-g{z}_{2.0}T\\ z_{4.T}-z_{4.0}-g{z}_{3.0}T-\frac{1}{2}{g}^2{z}_{2.0}{T}^2\end{array}\right]$$

The new trajectory will be achieved from Equations (35)–(42):

$$\theta =arctan(z_3)$$

(35)

and

$$\varphi =arctan(z_{2}l{cos}^3\theta )$$

(36)

and

$$\dot{x}\left(t\right)={\dot{z}}_1\left(t\right)=g$$

(37)

and

$$\dot{y}\left(t\right)={\dot{z}}_4\left(t\right)=g{z}_{3.0}+g^2{z}_{2.0}t+\frac{1}{2}{g}^2{h}_1{t}^2+\frac{1}{8}{g}^2{h}_2{t}^3+\frac{1}{12}{g}^2{h}_3{t}^4$$

(38)

and

$$v_1\left(t\right)=\frac{\sqrt{{\dot{x}}^2\left(t\right)+{\dot{y}}^2\left(t\right)}}{r}$$

(39)

and

$$\dot{\theta }={\dot{z}}_3{cos}^2\theta =\left(g{z}_{2.0}+g{h}_{1}t+\frac{1}{2}g{h}_2{t}^2+\frac{1}{3}g{h}_3{t}^3\right){cos}^2\theta$$

(40)

and from Equation (27)

$$\dot{\varphi }={\dot{z}}_2\left(t\right)l{cos}^3\theta {cos}^2\varphi -3\dot{\theta }tan\theta sin\varphi cos\varphi$$

(41)

Figure 8 shows the new trajectory of the vehicle and the vehicle velocity.

Figure 9 shows the steering angle, $\varphi$, and the vehicle body angle, $\theta$, corresponding to the angular velocity $\dot{\varphi }$, and $\dot{\theta }$.

As shown in Figure 9, this feasible trajectory requires a maximum steering angle of ${\varphi }_{MAX}=41.5736°$ and satisfied the steering angle constraint, $-45°\le \varphi \le 45°$. Thus, this method is better than the previous method in part 3 with ${\varphi }_{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.

6. 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 is based on the optimal motion planning in [23]. The vehicle moving from the start point to the destination point with the travelling time $t=0÷T$ can be now reformulated based on a symmetric polynomial function:

$$x\left(t\right)=-{(\frac{t}{T}-1)}^3{x}_0+{\left(\frac{t}{T}\right)}^3{x}_T+a_x{\left(\frac{t}{T}\right)}^2(\frac{t}{T}-1)+b_x\frac{t}{T}{(\frac{t}{T}-1)}^2$$

(42)

and the position of y is:

$$y\left(t\right)=-{(\frac{t}{T}-1)}^3{y}_0+{\left(\frac{t}{T}\right)}^3{x}_T+a_y{\left(\frac{t}{T}\right)}^2(\frac{t}{T}-1)+b_y\frac{t}{T}{(\frac{t}{T}-1)}^2$$

(43)

The velocity of $x\left(t\right)$:

$$\begin{gathered} \dot{x}\left(t\right)=-3{(\frac{t}{T}-1)}^2{x}_0+3{\left(\frac{t}{T}\right)}^2{x}_T+a_{x}2\frac{t}{T}(\frac{t}{T}-1)+a_x{\left(\frac{t}{T}\right)}^2+b_x{(\frac{t}{T}-1)}^2 \\ +b_{x}2\frac{t}{T}(\frac{t}{T}-1) \end{gathered}$$

(44)

and the velocity of $y\left(t\right)$:

$$\begin{gathered} \dot{y}\left(t\right)=-3{(\frac{t}{T}-1)}^2{y}_0+3{\left(\frac{t}{T}\right)}^2{y}_T+a_{y}2\frac{t}{T}(\frac{t}{T}-1)+a_y{\left(\frac{t}{T}\right)}^2+b_y{(\frac{t}{T}-1)}^2 \\ +b_{y}2\frac{t}{T}(\frac{t}{T}-1) \end{gathered}$$

(45)

Then, the acceleration of $x\left(t\right)$

$$\begin{gathered} \ddot{x}\left(t\right)=-6(\frac{t}{T}-1)x_0+6\frac{t}{T}{x}_T+a_{x}2(2\frac{t}{T}-1)+a_{x}2\frac{t}{T}+b_{x}2(\frac{t}{T}-1) \\ +b_{x}2(2\frac{t}{T}-1) \end{gathered}$$

(46)

and the acceleration of $y\left(t\right)$

$$\begin{gathered} \ddot{y}\left(t\right)=-6(\frac{t}{T}-1)y_0+6\frac{t}{T}{y}_T+a_{y}2(2\frac{t}{T}-1)+a_{y}2\frac{t}{T}+b_{y}2(\frac{t}{T}-1) \\ +b_{y}2(2\frac{t}{T}-1) \end{gathered}$$

(47)

The vehicle velocity:

$$r{v}_1=\frac{\dot{x}}{cos\theta }=\frac{\dot{y}}{sin\theta }$$

(48)

At the starting point $t=0$:

$$\dot{x}\left(0\right)=k_{0}cos{\theta }_0, \mathrm{and} \dot{y}\left(0\right)=k_{0}sin{\theta }_0$$

(49)

At the destination point $t=T$:

$$\dot{x}\left(T\right)=k_{T}cos{\theta }_T, \mathrm{and} \dot{y}\left(T\right)=k_{T}sin{\theta }_T$$

(50)

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

$$a_x=kcos{\theta }_T-3x_T, \mathrm{and} b_x=kcos{\theta }_0-3x_0$$

(51)

and similarly

$$a_y=ksin{\theta }_T-3y_T, \mathrm{and} b_y=ksin{\theta }_0-3y_0$$

(52)

Simulation of this method is conducted with the same vehicle parameters in previous examples and shown in Figure 10 and Figure 11. The velocity coefficients are set at $k_0=k_T=k=1$. Other parameters regarding the vehicle angular velocity, $v_1\left(t\right)$, the vehicle body angle, $\theta \left(t\right)$, the vehicle body angular velocity, $\dot{\theta }\left(t\right)$, the vehicle steering angle, $\varphi \left(t\right)$, and the vehicle steering angular velocity, $\dot{\varphi }\left(t\right)$ are calculated using the following Equations:

$$v_1=\frac{\sqrt{{\dot{x}}^2+{\dot{y}}^2}}{r}$$

(53)

and

$$\theta =arctan\left(\frac{\dot{y}}{\dot{x}}\right)$$

(54)

and

$$\varphi =arctan\left(l\frac{{cos}^3\theta \ddot{y}}{{(\dot{x})}^2}\right) \mathrm{or} \varphi =arctan\left(l\frac{\ddot{y}\dot{x}-\ddot{x}\dot{y}}{r{\left({\dot{x}}^2+{\dot{y}}^2\right)}^{\frac{3}{2}}}\right)$$

(55)

and

$$\dot{\theta }=\frac{\ddot{y}\dot{x} - \ddot{x}\dot{y}}{{\dot{x}}^2}\frac{1}{{\left(\frac{\dot{y}}{\dot{x}}\right)}^2+1}=\frac{\ddot{y}\dot{x} - \ddot{x}\dot{y}}{{\dot{x}}^2+{\dot{y}}^2}=\frac{tan\varphi }{l}r{v}_1$$

(56)

and

$$\dot{\varphi }=\frac{\partial \left(arctan\left(l\frac{\ddot{y}\dot{x} - \ddot{x}\dot{y}}{r{\left({\dot{x}}^2+{\dot{y}}^2\right)}^{\frac{3}{2}}}\right)\right)}{\partial t}$$

(57)

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 $\varphi =40.1622°$ and is better than the method in part 5 with $\varphi =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.

7. Performances Comparison

The comparison of performances for all three methods is shown in Figure 12 and Figure 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 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 $\varphi$ moving from positive to negative and returning to 0 position. The steering angle, $-45°\le \varphi \le 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 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.

8. 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.