Feasible Path Planning for Autonomous Vehicles

Theobjective of this paper is to find feasible path planning algorithms for nonholonomic vehicles including flatness, polynomial, and symmetric polynomial trajectories subject to the real vehicle dynamical constraints. Performances of these path planning methods are simulated and compared to evaluate the more realistic and smoother generated trajectories. Results show that the symmetric polynomial algorithm provides the smoothest trajectory. Therefore, this algorithm is recommended for the development of an automatic control for autonomous vehicles.


Introduction
Path planning is one of the most important parts of an autonomous vehicle.It deals with, searching a feasible path, taking into consideration the geometry of the vehicle and its surroundings, the kinematic constraints and others that may affect the feasible paths.Study of this paper can be used to develop a real-time control system for autonomous ground vehicles which can track exactly on any feasible paths from the global positioning system (GPS) maps or/and from unmanned aerial vehicle (UAV) images.This system can be also applied to autonomous unmanned ground vehicles (on road or off road) and autoparking and autodriving systems.
The main idea of this paper is a system which can automatically generate an optimal feasible trajectory and then control the vehicle to track exactly on this path from any given starting points to any given destination points from a map subject to the vehicle physical constraints on obstacles, speed, steering angle, and so forth.
From the existed pieces of literature on this research topic, there are still few research papers dealing with optimal trajectory generation subject to real automotive engineering constraints.Basic studies on the flatness, nonholonomic, and nonlinear systems can be read from the recent book of Lévine in [1] where the fundamental motion planning of a vehicle is presented.The flatness and controllability conditions for this nonlinear system are also well investigated and defined.However, in this research, parking simulation of a two-trailer vehicle is also demonstrated but without the constraint of steering angle and steering angular velocity.
The problem of trajectory generation for nonholonomic system is also presented by Dong and Guo in [2] where two trajectory generation methods are proposed.The control inputs are the second-order polynomial equations.By integrating those control inputs, coefficients for those secondorder polynomial equations are found.However this paper is lacking constraint analysis on the vehicle velocity and the steering angle.
The cell mapping techniques and reinforcement learning methods to obtain the optimal motion planning by Gomez et al. in [3] are tested for vehicles considering kinematics, dynamics, and obstacle constraints.This paper shows a simulation of a wheeled mobile robot moving on a path generated by the bang-bang trajectory generation.And the paper does not mention the algorithms used for generating the vehicle trajectory.Several other research papers on optimal trajectories and control of autonomous vehicles can be read in [4][5][6][7]; however, most of those studies are based on the real traffic flow roads and the control algorithms are to perform the maneuver tasks such as lane changing, merging, distance keeping, velocity keeping, stopping, and collision avoidance.
This paper, therefore, focuses on the applicable algorithms to generate feasible trajectories from a starting point to any destination point subject to vehicle constraints.This paper refers to the vehicle sideslip model and estimation by Minh in [8, chapter 8].The nonlinear computational schemes for the nonlinear systems are referred to by Minh and Afzulpurkur in [9].Although the holonomic dynamical trajectories generation is not new, recent research articles have tried to develop those trajectories in different applications.Trajectory for autonomous ground vehicles based on Bezier curves operating under waypoints and corridor constraints can be referred to by Choi et al. in [10] and a real-time trajectory generation for car-like vehicles navigating dynamic environments can be read by Delsart et al. in [11].A new paper of trajectory generation model-based IMM tracking for safe driving in intersection scenario can be read by Zhou et al. in [12] to reduce traffic accidents at intersections.Another paper of dynamic trajectory generation for wheeled robots is referred to by Missura and Behnke in [13] where a dynamic motion of wheeled robots can be determined in real-time onboard.The real vehicle models and conditions for stabilizability of dynamic systems are referred to in [14,15].This paper introduces three (flatness, polynomial, and symmetric) conventional holonomic trajectories generation subject to steering angle constraint for autonomous vehicles.Performances of these three algorithms are compared.Data taken from simulation on the vehicle longitudinal velocity and its steering angular velocity will be used to develop the control algorithms for the vehicle tracking on those trajectories in the next part of this paper.The outline of this paper is as follows.Section 2 describes the kinematic model; Section 3 presents the flatness method; Section 4 produces the trajectory generation subject steering angle constrained violation; Section 5 presents the polynomial method; Section 6 develops the symmetric polynomial method; Section 7 analyses the performance of the three methods; finally conclusion is drawn in Section 8. Simulations in this paper are conducted in the MATLAB Simulink R2009a.

Kinematic Model of a Vehicle
The constraints for this vehicle model are based on the assumption that the wheels are rolling without slipping and the steering angle is simplified as a single wheel in the midpoint of the two frond wheels.Then, a kinematic model of a vehicle can be drawn in Figure 1.
The kinematic model of a forward rear-wheel driving vehicle can be written as where  = [, , , ]  is the system state variables, (, ) are the Cartesian coordinates of the middle point of the rear wheel axis,  is the angle of the vehicle body to the -axis,  is the steering angle,  is the vehicle wheel base,  is the wheel radius, V 1 is the angular velocity of the rear wheel, and V 2 is the angular steering velocity.Given the initial state (0) = [ 0 ,  0 ,  0 ,  0 ]  at time  = 0 and the final state Similarly, the model for a forward front-wheel driving vehicle is presented as The model for a forward rear-wheel driving vehicle in (1) will be used for all calculations and simulations in this paper.From this kinematic model, flatness equations for the vehicle trajectory generation are presented in the next section.

Vehicle Flatness Trajectory Generation
The flatness system is the system of nonlinear differential equations in (1) whose movement curves are in smooth and flat space.From Figure 1, the vehicle angular velocity can be calculated as Transforming from (1), the vehicle body angle is From the derivative of the above trigonometric, , the body angular velocity, θ , is achieved as follows: Therefore,  and  can be directly calculated from variables: ẋ , ẍ , and ẏ , ÿ .And it means that the above system is flat [1].Thus, all state and input variables can be presented by the flat outputs  and .
From ( 5), the boundary conditions for the outputs  and  are The initial state at time,  = 0, to the final state at time,  = , for (), is The initial state for () = (0) is The final state for () = () is From the initial state ( 0 ,  0 ,  0 ,  0 ) at the time  = 0 to the final state (  ,   ,   ,   ), under a real condition that | ẋ ()| ≥  > 0, if it is assumed that = |  −  0 |/2 > 0, the trajectory of () can be written freely as And the trajectory of () can be selected as where and From (10), And, from (5), ) . ( The angular velocity of the vehicle in (1) can be calculated from ( 13) and ( 14).
For derivative of , and, then, and derivative of  is And, then, The absolute vehicle velocity can be calculated from ( 15) to (18) with or with another formula to calculate To calculate θ from (5),  To calculate V 2 () = φ , from (14), if  = arctan(), then, ) . ( Another formula to calculate V 2 () = φ which is derived from ( 5) is Figure 2 shows the trajectory (, ) for the vehicle from the initial position to the final position and its velocity.Figure 3 shows the vehicle body angle, , and the steering angle, , corresponding to their angular velocities, θ and φ .
The maximum steering angle for this movement is  max = 64 ∘ ; it exceeds the physical constraint of steering angle with −45 ∘ ≤  max ≤ 45 ∘ .Therefore, the above trajectory is not feasible.The problem is that the distance from the starting point to the destination point is too short subject to the real vehicle physical constraints.A solution for this problem is to lengthen the travelling distance until it meets the constraint on steering angle.This issue is discussed in the next section.

Trajectory Subject to Steering Angle Constraint
The maximum steering angle of a vehicle depends on its real structure space and design.In this paper, it is assumed that the real structure constraint for the vehicle is (−/4) ≤  ≤ /4 or the maximum steering angle for this vehicle is in a range of −45 ∘ ≤  ≤ 45 ∘ : Simulation in the previous part shows the maximum steering angle,  max = 64 ∘ , exceeding the limitation in (24).Therefore, a new technique for the vehicle trajectory generation subject the constraint in (25) is proposed.
For time  from initial state,  = 0, to the final state,  = , the feasible trajectories () and () subject to conditions in (10) and ( 11) can be generated.During the progress of timing, , recalculate the steering angle, , in ( 14) and then check for the steering constraint in (24).If the constraint in (24) is violated, the distance from the initial position ( 0 ,  0 ) to the final position (  ,   ) is too short for satisfying the steering angle limit.
In this case, it is proposed that to lengthen the distance for  = 1, 2, 3, . . . until the constraint in ( 24) is satisfied and with  being an amplification coefficient,  > 1.
Then the new coordinates are The next simulation is done with the same parameters in the previous part and subject to the constraint in (24) with an amplification coefficient  = 1.1.The constraint in (24) will be satisfied after  = 4 iterations.The new coordinate is   =   = 23.579.
Figure 4 shows the new trajectory and new velocity of the vehicle with the new lengthened distance.The velocity of the vehicle is increased since the final time  is not changed.
Figure 5 shows the new vehicle body angle, , velocity, θ , steering angle, , and velocity, φ , for the new trajectory.The new steering angle has now satisfied the constraint  = 40 ∘ .
Due to the size of this paper, the sideslip of the vehicle model is ignored.In reality, the sideslip of a vehicle depends on the tire stiffness and the cornering velocity.Then the trajectory generation in this study does not depend on the vehicle velocity.In the next part, a new vehicle trajectory generation based on polynomial equations is investigated.

Polynomial Trajectory Generation
For faster generation of a feasible vehicle tracking, a secondorder polynomial for trajectory generation is presented.Equation ( 1) is separated into the following forms:   Then, The vehicle will move from the initial state ( 0 ,  0 ,  0 ,  0 ) at time  = 0 to the final state (  ,   ,   ,   ) at time  =  corresponding from the initial state at ( 1.0 ,  2.0 ,  3.0 ,  4.0 ) to the final state at ( 1. ,  2. ,  3. ,  4. ).
(31)  Simulation of this method is conducted with the same parameters in Section 2 and is shown in Figures 6 and 7 using the following equations: And, from (28), Figure 6 shows the trajectory (, ) for the vehicle from initial point to the final point and the velocity of the vehicle.Figure 7 shows the vehicle body angle, , and the steering angle, , corresponding to the angular velocity, θ and φ , of the vehicle.
As shown in Figure 7, the maximum steering angle for this trajectory generation is  = 41.5736∘ and satisfied the constraint −45 ∘ ≤  ≤ 45 ∘ .Therefore this trajectory generation is better than the flatness method in the previous part.
From Figure 6, it is really not realistic when the vehicle speed is increasing exponentially.It is expected that, when a vehicle is moving from one point to another point, the speed will increase gradually at the starting point and decrease gradually to the destination point.Therefore, in the next part, new symmetric polynomial equations in third order are investigated.

Symmetric Polynomial Trajectory Generation
Since the system is flat and each flat output can be parameterized by a sufficiently smooth polynomial, symmetric thirdorder polynomial equations are tried for this trajectory generation.Because the sideslip is ignored, the vehicle trajectory does not depend on its speed, V 1 , and on the travelling time, .A new time variable, therefore, for this system is applied with a ratio of / for  = 0 ÷ : and the trajectory of  is The constraint on speed is The constraint at starting point  = 0 is The constraint at destination point  =  is    From (38), (39), (41), and (42), for the calculation simplicity, it is assumed that the speed coefficients at the starting and destination points,  0 =   = ; then Similarly, Simulation is conducted with the same parameters in the previous parts and is shown in Figures 8 and 9.The speed coefficients are  0 =   =  = 1.Other parameters are calculated in the following equations: It can be seen from Figure 8 that the trajectory of this symmetric polynomial is more realistic because the velocity increases from the starting point and decreases in the destination point as per the expectation.The maximum steering angle for the symmetric polynomial method is  = 41.1622∘ and satisfied the constraint −45 ∘ ≤  ≤ 45 ∘ .This steering angle is smaller than the steering angle in the second-order polynomial method.In the next part, a comparison of the previous three methods is presented.

Performance Analysis
Performances of the three methods on the trajectory generation and the vehicle velocity are shown in Figures 10 and 11.
It can be seen that the symmetric polynomial generation can produce a more realistic speed and a smoother trajectory since it allows the vehicle gradually to increase the speed at the starting point and reduce the speed at the destination point.
Figure 11 shows the symmetric polynomial method providing the lowest body angle, (), the body angle velocity, θ (), the steering angle, (), and the steering angle velocity, φ ().Therefore, this method is recommended for the development of an automatic control of tracking vehicles.
For the vehicle moving in reverse speeds, as mentioned in (1), the vehicle velocity, V 1 , will change the sign.The speed coefficients in (43) and (44) will be minus values.Simulations for the reverse speeds are conducted with the speed coefficients  0 =   =  = −1.Results of the simulation are shown in Figures 12 and 13.
Since the vehicle is forced to reverse at the starting point with  0 = −1 and at the destination point with   −1, Figure 12 shows the vehicle reversing out at the starting point, going forward to the destination, and then again reversing to the parking space.The vehicle velocity is changing the direction in three times.
Figure 13 shows the body angle, , switching 180 ∘ in two times corresponding to each reverse speed.The maximum steering angle in this reverse speed is  max = 33.7097∘ .This number is much lower than the numbers of the forward speed movements.However the travelling distance is also longer in the reverse speed trajectory.

Conclusion
The paper has presented three methods of trajectory generation for autonomous vehicles subject to constraints.Regarding the real vehicle speed development, the third-order symmetric polynomial trajectory is recommended.Simulations and analyses are also conducted for vehicle moving in forward and in reverse speeds.Results from this study can be used to develop a real-time control system for autodriving and autoparking vehicles.The limitation of this study is the ignorance of the vehicle sideslip due to the cornering velocity.However this error can be eliminated with the feedback control loop and some offset margins of the steering angle constraint.