Global optimal path planning of an autonomous vehicle for overtaking a moving obstacle

In this paper, the global optimal path planning of an autonomous vehicle for overtaking a moving obstacle is proposed. In this study, the autonomous vehicle overtakes a moving vehicle by performing a double lane-change maneuver after detecting it in a proper distance ahead. The optimal path of vehicle for performing the lane-change maneuver is generated by a path planning program in which the sum of lateral deviation of the vehicle from a reference path and the rate of steering angle become minimum while the lateral acceleration of vehicle does not exceed a safe limit value. A nonlinear optimal control theory with the lateral vehicle dynamics equations and inequality constraint of lateral acceleration are used to generate the path. The indirect approach for solving the optimal control problem is used by applying the calculus of variation and the Pontryagin’s Minimum Principle to obtain first-order necessary conditions for optimality. The optimal path is generated as a global optimal solution and can be used as the benchmark of the path generated by the local motion planning of autonomous vehicles. A full nonlinear vehicle model in CarSim software is used for path following simulation by importing path data from the MATLAB code. The simulation results show that the generated path for the autonomous vehicle satisfies all vehicle dynamics constraints and hence is a suitable overtaking path for the following vehicle.

Global optimal path planning of an autonomous vehicle for overtaking a moving obstacle

INTRODUCTION
There is a recent trend in the automobile industry towards autonomous vehicles.The autonomous vehicle, also known as driverless vehicle is a kind of vehicle capable of sensing its environment and navigating along a predefined path without driver intervention.Motion planning is one of the important aspects of an autonomous vehicle motion.It deals with the search and execution of collision-free paths by performing specific tasks.Motion planning consists of two stages, path planning and path tracking.The path planning stage involves the search and computation of a collision-free path, taking into account the geometry of vehicle , its surroundings and obstacles, the vehicle kinematic constraints, and sometimes dynamics constraints as well as any other external constraints that may affect the planning of a path (Park et al., 2009).The path tracking stage involves the actual navigation on the planned path considering the kinematic and dynamic constraints of the vehicle (Kim et al., 2011;Mashadi et al., 2014).The optimal path planning method is one of the several approaches in the path planning of autonomous vehicles.In this approach, an optimal and collision-free trajectory for the autonomous vehicle is computed by different kinds of optimization methods so that the autonomous vehicle can move along this path from its initial position to the target location without colliding other vehicles or obstacles in the environment.In addition, the optimal cost function has to be minimized while the quality or inequality constraints are satisfied.The cost functions depend on the problem and may be the time of travel or deviation from a reference path or any other dynamic or kinematic property of the vehicle.Several optimization methods for the path planning problem of robots have been applied in the past (Yahja et al., 2000;Arras et al., 2002;Spenko et al., 2004;Minguez and Montano, 2005;Rashid et al., 2013) and some have recently been used in the autonomous vehicle Latin American Journal of Solids and Structures 11 (2014) 2555-2572 researches (Purwin and D'Andrea, 2006;Park et al., 2009;Yoon et al., 2009;Zhe et al., 2009;Matveev et al., 2011).
There are two optimization approaches which are used for path panning of autonomous vehicles.The local optimal path planning, which needs to predict the future of the system, is used in real time vehicle obstacle avoidance systems.The main objective of obstacle avoidance systems is to avoid obstacles based on the data collected from the onboard sensors.It may be used when the vehicle encounters an unexpected obstacle during following a pre-computed path.Since local path planners run in real time and on onboard computers that usually have very limited computing capabilities, they have to be efficient in terms of both memory utilization and time.Most of the researches on the local obstacle avoidance have been performed using reactive methods based on the sensor data (Spenko et al., 2004;Minguez and Montano, 2005;Garg et al., 2011).Some researches take into account the simple robot dynamics in terms of velocity or turning radius (Yuan et al., 2006).These approaches are computationally efficient, but the vehicle can get stuck in local minima.Some recent researches are accomplished by using advanced algorithms such as Model Predictive Control (MPC) (Borrelli et al., 2005;Park et al., 2009;Yoon et al., 2009;Nanao and Ohtsuka 2010) or Fuzzy Logic (Naranjo et al., 2008).Their efficiency is usually measured by the amount of time they take.The quality of their results depends on the amount of look-ahead distance of the sensors (Isermann et al., 2008).
On the other hand, the main objective of global optimal path planning is to determine appropriate control laws for steering an autonomous vehicle between two pre-described points in the state space.The autonomous vehicle uses the path information to navigate itself efficiently through the environment with the help of a path tracking algorithm.Some approaches have been explored for obtaining the optimal path using search methods such as Genetic Algorithms (GA) (Onieva et al., 2011) or computational methods such as optimal control.Thus, the produced path of the global optimal path planning can be used as a benchmark for the path generated by the local motion planning of the autonomous vehicle.Therefore, designing an autonomous vehicle, consisting of an individual one or a group of cooperating vehicles, is possible when motion planning of vehicles can be done.Thus, path planning is the most fundamental issue for designing an autonomous vehicle.
In this paper, an efficient global path planning algorithm that is capable of finding optimal collision-free paths from a start point to a goal is developed.The Pontryagin's Minimum Principle (PMP) is used to solve the optimal control problem for obtaining the optimal path planning.The paper is structured as follows.Section 2 describes path planning program which is used to generate optimal trajectories for the vehicle.Section 3 discusses the simulation results.Conclusions are presented in section 4.

MODELING OF PATH PLANNING PROGRAM
As shown in Figure 1, the autonomous vehicle and a slow moving car are traveling on the same lane and the autonomous vehicle is following a predefined path according to the road centerline.After detecting the obstacle in a proper distance in front, according to the vehicle and obstacle forward velocities, the autonomous vehicle decides to overtake the obstacle by executing a lane-change maneuver.The optimal path of vehicle for performing the lane-change maneuver is generated by the Latin American Journal of Solids and Structures 11 (2014) 2555-2572 path planning program.Due to the probability of frontal crash in the opposite lane, the autonomous vehicle should move close to the obstacle as much as possible to minimize the sum of lateral deviation of the vehicle relative to the previous path.On the other hand, if the vehicle closes too much to the obstacle, vehicle lateral acceleration during the lane-change maneuver becomes too high.Since in high lateral acceleration maneuvers, autonomous vehicle might become unstable, the path should be obtained such that the vehicle lateral acceleration during the lane-change maneuver does not exceed a safe limit.Since the optimality of path is global the start and end points of vehicle travel during the maneuver are known; so, to define the cost function of the optimal problem, it is assumed that the autonomous vehicle detects the obstacle at distance 0 L in front of itself and after traveling a distance L along the reference path, the vehicle succeeds to complete the lane-change maneuver and overtake the obstacle.As shown in Figure 1, the autonomous vehicle first travels a distance s L along the reference path and then executes the lane-change maneuver in such way that the vehicle final lateral deviation from the reference path becomes H and the lateral acceleration of vehicle doesn't exceed a predefined safe limit.The path is obtained in the inertial reference frame which is coincident on the vehicle reference frame at the moment of decision making for the lane-change maneuver.The origin of vehicle reference frame is located at the center of vehicle front axle.So the cost function of the path planning program is defined as: ( ) where y is the vehicle path deviation from the reference path, f d  is the rate of front wheel steering angle and w is a weighting factor that indicates the relative importance of the corresponding term.First term of equation (1) penalizes the deviation from the reference path along the horizon.The second term of equation (1) penalizes the control input in terms of energy consumption.Path planning of an autonomous vehicle for the lane-change maneuver is a nonlinear optimal control problem including the vehicle dynamics and lateral acceleration inequality constraints.Structures 11 (2014) 2555-2572

Vehicle dynamics modeling
A simplified linear bicycle model (see Figure 2) is used to determine the optimal path.The vehicle degrees of freedom are lateral velocity in direction v y and the yaw rate around the z axis.Governing equations of the bicycle model can be described as follows (Mashadi et al., 2014): where U and v stand for the longitudinal and lateral velocities of vehicle center of gravity (CG), respectively.m is the vehicle mass, z I is the yaw moment of inertia, r and y are the vehicle yaw rate and yaw angle, respectively; f l and r l are the distances from vehicle CG to the front and rear axles.
f C a and r C a denote the cornering stiffnesses of the front and rear tires, respectively.The vehicle motion in the inertial reference frame is described by: where x is the vehicle displacement along the reference path.Although the actual input command to the vehicle dynamics is the steering angle of the front wheels, f d , the input is redefined to be f d  , and f d is set to be a state variable of the system so that the rate of change of f d can be constrained by placing a limit on the system input u .Setting the state vector as and using equations ( 2) and (3), the state space equations for the bicycle model can be written in the following vector form: Latin American Journal of Solids and Structures 11 (2014) 2555-2572 It should be noted that although the vehicle dynamics equation ( 2) is linear, the state equation ( 4) is nonlinear due to the presence of nonlinear terms in equation ( 3), and hence the optimal problem is also nonlinear.
To ensure that the actual vehicle can track the generated path based on this simplified model, the assumptions placed on the vehicle model should be satisfied.The assumptions made during deriving this model imply that the tire lateral force versus tire slip angle is bounded in the linear regime.This leads to a direct constraint on the slip angles to have small values.If the constraints on the tire slip angles were to be used, complicated derivative terms would appear in the nonlinear optimization process.Instead of two individual constraints, the following constraint on the lateral acceleration is used, because it has the same effect on keeping the values of tire slip angles small.
where y a is the lateral acceleration of the vehicle and yc a is the maximum value of vehicle lateral acceleration in which the lateral tire forces are bounded in the linear regime.The desired value of yc a is evaluated in the simulation section.By substituting v from equation (2) into (5) and introducing function ( )  C x , the lateral acceleration constraint is written as: in which yc a is a given limit of the lateral acceleration.The optimal path for the autonomous vehicle should be obtained in such way that the input control of vehicle during tracking does not exceed the saturation limit.Since the control input of vehicle is defined as the steering angle rate, the saturation limit of the control input depends on the steering mechanism of the vehicle presented as: where sat u is the saturation limit of the control input and is considered 60 deg/s in this case study.

Optimal Path planning problem
For obtaining the optimal path, the cost function ( 1) is to be solved together with the vehicle dynamics constraint (4), the lateral acceleration constraint (6), and control input constraint (7).The Pontryagin's Minimum Principle is used for solving the nonlinear optimal control problem.First, cost function ( 1) is rewritten as: where Q denotes positive weighting matrices.Since the vehicle path is straight in the beginning of the time interval and the end of the lane-change maneuver is also straight, the state variables including lateral velocity, yaw rate, heading angle and steering angle of vehicle in the beginning and end of the time interval become zero.So, boundary conditions of the vehicle are defined as follows: The final time f t , is free and should be obtained by solving the optimal control problem.If the obstacle is moving with a constant deceleration ob a , the final distance along the reference path is calculated from: where is initial speed of the obstacle at the moment of decision making for the lane-change maneuver.The Hamiltonian is defined as (Kirk 2004): ( ) where P is the Lagrange multiplier vector for the vehicle dynamics constraint, and c w is the weighting parameter for the inequality constraints on the lateral acceleration.The Heaviside function is defined as: The co-state equations can be presented in the following form: In order that * u be an optimal solution of equation ( 1), the following is a necessary condition (Kirk 2004): In the case that the control input value is smaller than the saturation limit defined in the inequality constraint (7), the first derivative of H with respect to u is equal to zero, so: To insure that the Hamiltonian at the derived control input is minimum, the second derivative test is applied: As the second derivative of the Hamiltonian with respect to u is positive, the control input obtained by equation ( 14) is the minimum point of Hamiltonian and it becomes the solution of the optimal control problem.In the case that the control input is greater than the saturation limit, the control input that minimizes the Hamiltonian is equal to the saturation limit.By Substituting B from equation ( 4) in ( 14) and considering what mentioned before, the optimal control input is obtained as: where 6 ( ) P t is the sixth element of the co-state vector and sgn is the sign function.By applying the calculus of variation method to the cost function subject to the constraint, the following boundary condition between the final state and co-state variables is presented as: All of final states are specified expect the distance along the reference path ( ) x t , so by taking the variation from equation ( 10), the variation of final unspecified state is derived as: ( ) By substituting the equation ( 18) into (17), the following equation is obtained: ( ), ( ), ( ) ( ) 0 The Hamiltonian function at the final time is calculated by replacing the time with f t and substituting equations ( 9) and ( 14) into (11) as the following statement: ( ) Then, by substituting the equation ( 21) into (20), the following boundary condition in the final time is obtained: ( ) By using differential equations ( 4), ( 12), control input ( 16) and boundary conditions ( 9) and ( 22), the two point boundary value nonlinear optimal control problem is presented as: For solving the above two point boundary value problem, the "bvp5c" function in MATLAB software is used (Wang, 2009).Since the time interval should be fixed in the function, the independent variable t can be changed to in equation ( 23) as: ( ) (1) 0 0 0 0 Latin American Journal of Solids and Structures 11 (2014) 2555-2572 Now the problem is set on the fixed interval [0 , 1] .By solving the boundary value problem ( 24) in MATLAB and calculating the state variables in the time interval [0 , ] f t , optimal path of the autonomous vehicle for overtaking the moving obstacle is obtained.

SIMULATION RESULTS
In this section, the numerical solutions of the boundary value problem for planning the optimal path and the simulation of the vehicle dynamics response during tracking the generated path are presented.In simulations throughout the paper, the path generated by the path planning programs is applied to the CarSim vehicle model.CarSim is a software which builds full nonlinear models of actual vehicles when parameters such as dimensions of the vehicle platform, specifications of the engine and tires, and coefficients of the suspensions are given.The CarSim vehicle model has been validated with experimental results of an actual vehicle (Yoon et al., 2009).The parameters characterizing a typical vehicle used for path planning are given in Table 1.Two simulations are prepared in this section.In the first simulation, the autonomous vehicle and the obstacle are moving on a straight road but in the second one, they are moving on a circular road.For every simulation, the boundary value problem equation ( 24) should be solved first with proper lateral acceleration constraint, and then the generated path is imported to CarSim for path tracking simulations to demonstrate the suitability of the generated path.For solving the optimal path planning problem presented in equation ( 24), the acceleration yc a of the autonomous vehicle must be evaluated.To determine a reasonable value for yc a , the bicycle model is compared with the CarSim model in the Simulink environment.By applying an exponentially increasing sinusoidal steering maneuver shown in Figure 3(a) to both models, performed at 100 km/h, a comparison between the bicycle model and the CarSim model is represented in Figure 3.
As illustrated in Figures 3(b) and 3(c), the lateral velocity and yaw rate of the two models agree with each other when the magnitude of the lateral acceleration shown in Figure 3(e) is under 0.3g.For the values of lateral acceleration exceeding 0.3g after 2.5 sec, the lateral velocity and yaw rate of the two models start to deviate.This is due to significantly large tire slip angle generation shown in Figure 3

Obstacle moving on a straight path
For solving the optimal path planning problem in the first simulation, the vehicle and the obstacle speeds are considered constant as 30 m/s and 10 m/s, respectively.The initial distance between the Figure 4(a) shows the lateral deviation from the reference path ( y ) versus the distance along the reference path ( x ).Since the reference path is straight, the plot of variable y versus x implies the path of vehicle in the global coordinate during the overtaking maneuver.The autonomous vehicle first moves forward about 210 m along the reference path and then carries out a lane-change maneuver to change its lane of motion to overtake the moving obstacle.
Figure 4(b) shows the heading angle of vehicle during the maneuver.As it can be seen, the vehicle execute the maneuver with small heading angle which this demonstrate the vehicle stability during the maneuver.The vehicle steering angle is shown in Figure 4(c).Figure 4(d) demonstrates lateral acceleration of the autonomous vehicle for performing the lane-change maneuver.It can be seen that the value of the lateral acceleration does not exceed 0.3g as expected.Also, it is illustrated that the autonomous vehicle deviates to right about 0.3 m just before turning to the left to accomplish the lane-change maneuver.This is due to discontinuity of the control input at the start point of the lane-change section of the path.To resolve this unexpected deviation in the vehicle path, it is needed to define two cost functions.One for the straight section and the other for the lane-change section of the path.The optimal path generated by defining two phase cost function is focused in our further research.
By importing the CarSim model to the Simulink environment, the path generated by the path planning program is tracked by an autonomous vehicle built in the CarSim.As shown in Figure 5

Obstacle moving on a straight path with deceleration
In the second simulation, the autonomous vehicle speed and the obstacle initial speed are the same as the first simulation.From the moment the autonomous vehicle detects the obstacle at 200m ahead, the obstacle starts to decrease its speed with constant deceleration of 0.1g .The initial distance between the vehicle and the obstacle on the straight road is considered as 200 m.optimal path of vehicle for overtaking the obstacle is shown in Figure 7

Obstacle moving on a circular path
In the third simulation, the autonomous vehicle and the obstacle are both moving on a circular road of 500 meter radius.The forward velocities are similar to those of the first simulation.The difference between the two simulations is the lateral acceleration constraint applied for solving the boundary value problem.Since the vehicle moves on the circular road, it already has a steady lateral acceleration which will be increased by the additional lateral acceleration due to executing the lane-change maneuver.So, in solving the boundary value problem (24), the steady lateral acceleration of vehicle moving on the reference road should be subtracted from the desired acceleration for the constraint in the first simulation.The steady lateral acceleration of the autonomous vehicle before beginning of the overtaking maneuver is calculated by: According to the calculated steady lateral acceleration, the desired acceleration for the inequality constraint ( 6) is obtained as: Similar to the first simulation, the path planning program uses the above lateral acceleration constraint for solving the boundary value problem.Lateral deviation of the optimal path from the reference path versus the distance vehicle travels along the reference path is shown in Figure 8(a) and heading angle error of the optimal path from the reference path is shown in Figure 8(b).It can be seen that on the circular road, the autonomous vehicle carries out the lane-change maneuver earlier than when it moves on the straight road due to the lower value of lateral acceleration constraint with respect to the first case.The output of the program is the control input and states x and y for generating the path.Since y demonstrates deviation of the optimal path from the reference path, the variable y should be added to the reference path to generate the target path for the vehicle to follow.By importing the lateral deviation of the optimal path with respect to the reference path into the CarSim, the software solver combines the input with the reference path and generates the target path for the CarSim vehicle model to follow it.Figure 9(a) shows the target optimal path, reference circular path and path tracked by the vehicle during executing the overtaking maneuver.For a clearer view, only the section of overtaking maneuver of the path is shown.Similar to the case of moving on the straight road, the vehicle tracks the regenerated optimal path with small tracking errors.

CONCLUSIONS
This study presents a global optimal path planning of an autonomous vehicle for overtaking a moving obstacle.The path planning program is presented for generating the optimal path of vehicle for performing the lane-change maneuver when it approaches a slow moving vehicle which is considered as an obstacle.It generates the path in such way that the sum of the lateral deviation of vehicle from the reference path and the rate of steering angle become minimum.The vehicle dynamics constraint and the lateral acceleration inequality constraint were used to prepare the nonlinear optimal control problem of path planning program to generate the path by using Pontryagin's Minimum Principle and the optimal path is generated as the global optimal solution.In addition, the CarSim software is used for the path tracking simulation by importing the path data from MATLAB results.The simulation results show that the autonomous vehicle can track the generated path and satisfies all vehicle dynamic constraints and hence is suitable to overtake the leading vehicle.The solution, however, shows that the path has an unexpected deviation from the reference path to the opposite direction when the optimal control problem with only one cost function is used.The solution with two cost functions will be examined in the future.

Figure 1 :
Figure 1: Schematic diagram of the lane-change maneuver.

Figure 3 :
Figure 3: Comparison between the bicycle model and CarSim vehicle model: a) front wheel steering applied to the CarSim and bicycle model (deg), b) lateral velocity (m/s), c) yaw rate (deg/s), d) front tire slip angle (deg) e) lateral acceleration (g) of the CarSim model and bicycle model.

Figure 4 :
Figure 4: Path planning results: a) optimal path of vehicle, b) heading angle (deg), c) steering angle (deg), d) lateral acceleration (g) of vehicle for performing the lane-change maneuver on straight road.
, the autonomous vehicle and the obstacle vehicle model built in the CarSim are connected together through Simulink Environment.For overtaking the obstacle, the autonomous vehicle should perform a double lane-change maneuver.Since the generated path represented in Figure 4(a) is a single lane-change maneuver, the path planning program should combine two generated paths symmetrically with respect to the obstacle for obtaining the path of a double lane-change maneuver.Figure 6(a) shows the optimal path generated by the path planning program and the path tracked by the vehicle.It shows that the vehicle can track the generated path precisely.Figure 6(b) demonstrates the front wheel steering angle during the double lane-change maneuver.As shown in Figure 6(b), the actual steering angle extrema are similar to what obtained by the path planning problem.Figure6(c) also shows the actual lateral acceleration of vehicle tracking the generated path.It can be seen that the value of vehicle lateral acceleration is always smaller than 0.3g.So, the autonomous vehicle is tracking the generated path as its dynamic response is equal to the original bicycle model for which the optimal path is generated.

Figure 5 :
Figure 5: The CarSim vehicle model simulated in the Simulink environment.

Figure 6 :Figure 7 :
Figure 6: Path tracking results from the CarSim: a) optimal and actual paths, b) actual steering angle (deg), c) actual lateral acceleration (g) of vehicle during double lane-change maneuver in the straight road.

Figure 8 :
Figure 8: Path planning results: a) lateral deviation of the optimal path from the reference path versus the distance along the reference path, b) lateral acceleration (g) of vehicle for performing the lane-change maneuver on circular road.

Figure 9 :
Figure 9: Path tracking results: a) target, reference and actual path of vehicle b) actual lateral acceleration of vehicle (g) during the overtaking maneuver on the circular road.

Latin American Journal of Solids and Structures 11 (2014) 2555-2572
t is free in this optimal control problem, its variation, f t d , is nonzero and based on the above equation, the multiplier should be zero.So the following equation is derived:

Table 1 :
Vehicle Parameters for the path planning program.