A path-tracking algorithm using predictive Stanley lateral controller

Path tracking is one of the most important aspects of autonomous vehicles. The current research focuses on designing path-tracking controllers taking into account the stability of the yaw and the nonholonomic constraints of the vehicle. In most cases, the lateral controller design relies on identifying a path reference point, the one with the shortest distance to the vehicle giving the current state of the vehicle. That restricts the controller’s ability to handle sudden changes of the trajectory heading angle. The present article proposes a new approach that imitates human behavior while driving. It is based on a discrete prediction model that anticipates the future states of the vehicle, allowing the use of the control algorithm in future predicted states augmented with the current controller output. The performance of the proposed approach is verified through several simulations on V-REP simulator with different types of maneuvers (double lane change, hook road, S road, and curved road) and a wide range of velocities. Predictive Stanley controller was used compared to the original Stanley controller. The obtained results of the proposed control approach show the advantage and the performance of the technique in terms of minimizing the lateral error and ensuring yaw stability by an average of 53% and 22%, respectively.


Introduction
Research in autonomous systems has shown promising results in substituting redundant efforts done by human beings with personal assistants at homes or factories. One of the fields that attract those interested in artificial intelligence is self-driving vehicles as notable results have been manifested, such as predicting the intent of pedestrians or even other vehicles surrounding the operating vehicle. Other efforts have been exerted for providing high-resolution images using simple segmentation maps or even producing hard maneuvers through obstacles at very high speed using different driving strategies and lateral control algorithms.
The potential benefits of autonomous vehicles (AVs) include, but not limited to (1) increasing highway safety due to the elimination of human error in driving as AVs will not be subjected to system failures and abuse, (2) better use of the traveling time for productive work or leisure, (3) independent mobility for the old, the disabled, and other mobility-constrained population segments, (4) reduction in fuel consumption and emissions due to smoother acceleration/deceleration characteristics and improving traffic flow (5) increasing road capacity and reducing congestion. Moreover, the implementation of AVs will represent a step change in how vehicles operate on the transportation network. 1 Lateral steering control has been introduced as one of the most important parts in autonomous vehicles. The purpose of lateral control is to perform automatic steering of the vehicle to maintain the navigation of the vehicle along the optimized trajectory despite changes in road conditions, curvatures, and other disturbances. Furthermore, a new control approach is developed, which imitates human behavior while driving on the basis of a discrete prediction model for anticipating the future states of the vehicle. With this in mind, the use of the control algorithm in future predicted states augmented with the current controller output will be allowed.
This would require a system that provides accurate vehicle positioning, which is the main feedback of the lateral controller. Added to that it provides a free collision trajectory taking the vehicle dynamics and passengers' comfort into consideration. A fusion algorithm is carried out to determine the vehicle position based on various types of sensors "GPS, IMU, LIDAR, wheel encoders, and a stereo camera" and for motion planning. The time elastic band (TEB) local planner is used to provide the controller with the free collision trajectory.
The article is structured as follows: the first section depicts the discretization of the kinematic bicycle model to be used as a predictive model and the predictive Stanley (PS) controller design. The second section provides a description of the experiments carried out to test the proposed methodologies and the evaluation metrics used in addition to the results obtained. Finally, the third section sums up the conclusions and future work.

Related work
The path-tracking problem in self-driving vehicles is one of the most difficult challenges because of the mobility constraints, sudden changes in the reference paths (severe maneuvers), and high vehicle's speeds.
Previous studies investigated various approaches, for instance, the proportional-integral-derivative (PID) control method. 2 It has a simple structure and can be easily implemented without the need for extensive computational resources. Unfortunately, this method cannot be automatically adapted to the environment or have an acceptable accuracy at a wide range of maneuvers or high vehicle's speeds. For that reason, adaptive methods have been introduced for their ability to adapt to different types of environments. Zhao et al. 3 proposed an adaptive PID controller to track predefined paths based on the lateral error (E), which is a function in the lateral and longitudinal velocity and the heading (q).
Another type of lateral controllers is the geometric path-tracking controller. Such type of controllers tracks a reference path using only the vehicle kinematics and ignoring the dynamic forces on the vehicle and assumes the no-slip conditions holding on the wheels. One of the most important geometric controllers is the pure pursuit controller, 4-6 which is the first method for estimating the steering necessary to maintain the vehicle on the road. 7 Coulter 8 clarified the implementation issue of pure pursuit, so that the pure pursuit has been widely used in outdoor applications. 9 Ollero et al. 10 introduced a supervisory controller (fuzzy controller) for the online tuning of the pure pursuit parameters, which have the ability to automatically tune the look-ahead distance based on path characteristics, velocity, and tracking errors. Shan et al. 11 introduced a new method for pursuit technique called clothoid fitting (CF) pursuit, which replaces the circles employed in pure pursuit with a clothoid C curve, which reduces the fitting error. Moreover, they used a fuzzy system to determine the suitable look-ahead distance corresponding to the path's curvature.
Stanley controller 12 is another type of the geometrical path-tracking controllers, which helped Stanford to win the DARPA challenge in 2006. Moreover, it shows a good performance in the previous studies 13,14 as it considers both the lateral error and the heading one. Amer et al. 15 introduced an adaptive Stanley controller with a fuzzy supervisory controller to adapt the controller gains automatically. That is based on prior database knowledge collected using particle swarm optimization depending on the vehicle velocity and the heading error. The controller shows a good performance at high speeds but it is not tested yet on highly severe maneuvers. There are other types of kinematic controllers, such as follow-the-carrot 16 and other geometrical controllers. 17,18 In summary, most of the previously mentioned methods attempted to develop a supervisory controller to make the basic controllers' parameters adaptive as in the literature 10,15 or reformulate the error calculation method as in the literature. 11 Moreover, Zakaria et al. 19 propose a pathtracking method using future prediction control, whereas the lateral error is calculated based on only one future point without taking the current state of the vehicle into consideration. However, the main problem is that all of those methods calculated the error based on one reference point as usually the one with the shortest distance to the vehicle. That restricts the controller's ability to handle sudden changes of the trajectory heading angle at high speeds. The article in hand proposes a new approach that imitates human behavior while driving as it aims to reduce the error along the path not only at one reference point as in the literature 19 on the basis of a discrete prediction model. Such a model anticipates the future states of the vehicle. With this in mind, the use of the control algorithm in future predicted states augmented with the current controller output will be allowed. PS controller is used and compared to the basic Stanley (BS) controller developed in the literature. 12

Methodology
The BS controller requires two essential inputs to be able to compute both the lateral error and the heading error, namely the reference path and the vehicle's current position relative to the global frame.
The proposed control approach introduces a third input, which is a developed array of future vehicle states. 8 Consequently, the controller will be able to calculate the required steady-state value of the steering angle for each state (row) in the array to be augmented with the value of the basic controller. Consequently, the controller is allowed to deal with the sudden changes in the heading angle of the trajectory ( traj ).

Basic Stanley controller
The proposed control approach uses the Stanley controller as the basic controller. Equation (1) illustrates that it is divided into three regions: saturated low region, saturated high region, and nominal region, as mentioned in the literature 12 where is the heading of the vehicle ( V ) with respect to the heading of the trajectory ( traj ) at the point of the shortest distance to the vehicle position, eðtÞ is the lateral error, and K is the controller gain.
Prediction model The prediction model is an important component in the implementation of this control approach as it anticipates the future vehicle states [X f , Y f , q] each time step. X f , Y f , and q are the vehicle global frontal position in both direction and the heading angle, respectively.

1) Kinematic bicycle model
The control approach is implemented on a four-wheeled vehicle. Two incremental encoders are mounted on the frontal (driven) wheels to measure the vehicle longitudinal velocity (V). An absolute encoder is mounted on the steering column to measure the steering angle. Given the previous information, the vehicle prediction model is simplified to a kinematic bicycle model for simplicity and reduction of computational time as in the literature 20 and is shown in Figure 1, assuming no-slip condition. d and L are the steering angle measured by the absolute encoder and the wheelbase 2) Prediction model discretization The modeling process is used to determine the future states of the vehicle in each time step and is developed based on the prior knowledge of the current vehicle state [X f , Y f , q, V, d]. Therefore, it is important to obtain the discrete form of the previously developed model because the proposed control approach is highly dependent on that.
The following equations describe the calculation of the future states of the vehicle based on the discrete prediction model, assuming constant velocity and steering angle at each time step. Thus, [X f o , Y f o , q o ] are the prior knowledge of the vehicle current state, while N denotes the number of time steps 2 f1, 2, 3, . . . g For each time step, the above equations (5) to (7) are used to create an array of vehicle future states by which the controller will be implemented separately on each state (row) in the created array

Predictive Stanley lateral control
As shown in Figure 2, the proposed control approach mainly depends on the prediction model as it determines the future states of the vehicle, which is used as the feedback for each BS controller. Consequently, the controller is allowed to deal with the sudden changes in the heading angle of the trajectory because of the ability of the proposed control approach to determine the error in earlier positions of the vehicle. Added to that, the suitable steady-state steering angle is calculated by augmenting the output of the controller at each state to eliminate the error along the path not only at the reference point.
Equation (9) illustrates the controller after adding the part of the steering angle, which is calculated based on the future predicted states. K i is the weight, which represents how each controller contributes in determining the final value of the steering angle

Experimental work and results
To validate the proposed methodology, multiple experiments have been performed in simulation on different types of maneuvers (double lane change, hook road, S road, and curved road) and velocities. This section discusses the experiments and tests carried out and the used evaluation criteria.

Evaluation metrics
For the lateral control system, the new control approach or the proposed controller is compared to the BS controller through different types of maneuvers and velocities. The main criteria that used to evaluate and compare the controllers' performance are the root mean square (RMS) value of the lateral error (e RMS ), which is defined as the shortest distance between the vehicle and the nearest point on the path, as shown in Figure 4. In addition the RMS of the heading error ( RMS ). The RMS of the yaw rate (r RMS ) quantifies the yaw stability, and the RMS of the change in

Setup
The proposed controller is developed on MATLAB with Ode3 (Bogacki-Shampine) as MATLAB solver and a fixed time step of 0.01 s. Furthermore, V-REP simulator is used for vehicle modeling, which acts as the system plant or process. Our controller publishes the control action to the V-REP simulator from MATLAB through ROS. 21 It also subscribes to the vehicle current state, which is published by V-REP simulator, as shown in Figure 5. Figure 6 shows the simulation environment and the vehicle model (Golf Cart 14-seater). Tables 1 to 3 present the advantage of the proposed controller over the original Stanley controller. That was tested on different types of maneuvers and vehicle speeds. However, this improvement costs more computational time, while it is still within the acceptable range.
The performance is measured by calculating the RMS of the lateral error, the RMS of the heading error, the RMS of the yaw rate, and the RMS of the change in the control action using equations (10) to (13) Table 1 presents the advantage of the PS controller over the BS controller on different types of maneuvers at 5 m/s. Table 2 shows the advantage of the PS controller over the BS controller on different types of maneuvers at 10 m/s. Table 3 shows the advantage of the PS controller over the BS controller on different types of maneuvers at 15 m/s. Equations (5) to (7) and (9) describe both the proposed controller and the prediction model. It is clear that the main parameters which affect the performance of the proposed method are the contribution of the main controller or the controller which depends on the current state of the vehicle (K o ). The contribution of all other controllers depends on the future vehicle states (K 1!N ) and the time step of the prediction model (D). Such parameters were tuned for each     maneuver and vehicle speed using trial and error method to reduce the total lateral error.
1) Curved road: Table 4 presents the controller selected parameters for the curved road at different vehicle speeds. Figure 7 shows the advantage of the proposed controller over the original Stanley controller using the controller parameters in Table 4 at 5 m/s. Figure 8 shows the advantage of the proposed controller over the original Stanley controller using the controller parameters in Table 4 at 10 m/s. Figure 9 shows the advantage of the proposed controller over the original Stanley controller using the controller parameters in Table 4 at 15 m/s.               2) Hook road: Table 5 presents the controller selected parameters for the hook road at different vehicle speeds. Figure 10 shows the advantage of the proposed controller over the original Stanley controller using the controller parameters in Table 5 at 5 m/s. Figure 11 shows the advantage of the proposed controller over the original Stanley controller using the controller parameters in Table 5 at 10 m/s. Figure 12 shows the advantage of the proposed controller over the original Stanley controller using the controller parameters in Table 5 at 15 m/s. Table 6 presents the controller selected parameters for DLC maneuver at different vehicle speeds. Figure 13 shows the advantage of the proposed controller over the original Stanley controller using the controller parameters in Table 6 at 5 m/s. Figure 14 shows the advantage of the proposed controller over the original Stanley controller using the controller parameters in Table 6 at 10 m/s. Figure 15 shows the advantage of the proposed controller over the original Stanley controller using the controller parameters in Table 6 at 15 m/s. Table 7 presents the controller selected parameters for the S road at different vehicle speeds. Figure 16 shows the advantage of the proposed controller over the original Stanley controller using the controller parameters in Table 7 at 5 m/s. Figure 17 shows the advantage of the proposed controller over the original Stanley controller using the controller parameters in Table 7 at 10 m/s. Figure 18 shows the advantage of the proposed controller over the original Stanley controller using the controller parameters in Table 7 at 15 m/s. Figure 19 shows the vehicle heading angle versus time in both cases on the double lane change maneuver at 15 m/ s. Moreover, the RMS of the yaw rate is calculated in Tables 1 to 3 to quantify the instability of the yaw. Figure 20 shows the steering angle versus time in both cases, on the double lane change maneuver at 15 m/s. Moreover, the RMS of the change in the control action is calculated in Tables 1 to 3 to quantify the instability of both controllers.

Conclusion and future work
In this article, a study on PS lateral controller for a golf cart (14 seater) is presented.
A nonlinear discrete bicycle model is used as a prediction model, anticipating the future states of the vehicle. Such a model allows the controller to deal with severe maneuvers at high vehicle speeds.
The predictive control approach uses the BS control instead of optimizers that solve optimization problems each time step resulting in extensive computational time. The simulation results are presented on V-REP simulator, showing the ability of the proposed controller to navigate through different trajectories and velocities. Compared to the original Stanley controller, this resulted in an improvement of an average of 53% in terms of the RMS of the lateral error, 14% for the RMS of the heading error, 22% for the RMS of the yaw rate, and 32% for the RMS of the change in the control action. Moreover, the results illustrated that the proposed controller performance is significantly higher than the BS controller at severe maneuvers and high vehicle speeds. For future work, the vehicle model can be extended to a dynamic bicycle model for a more accurate prediction of the vehicle's future states. Moreover, a supervisory controller can be added to perform an online tuning for the introduced approach parameters (K o , K 1!N , and D) to maintain the proposed controller performance at different maneuvers and vehicle speeds. Finally, several experiments ought to be performed on different realworld maneuvers and under different environmental conditions.