International Journal of Advanced Robotic Systems a Path Tracking Algorithm Using Future Prediction Control with Spike Detection for an Autonomous Vehicle Robot Regular Paper

which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited. Abstract Trajectory tracking is an important aspect of autonomous vehicles. The idea behind trajectory tracking is the ability of the vehicle to follow a predefined path with zero steady state error. The difficulty arises due to the nonlinearity of vehicle dynamics. Therefore, this paper proposes a stable tracking control for an autonomous vehicle. An approach that consists of steering wheel control and lateral control is introduced. This control algorithm is used for a non-holonomic navigation problem, namely tracking a reference trajectory in a closed loop form. A proposed future prediction point control algorithm is used to calculate the vehicle's lateral error in order to improve the performance of the trajectory tracking. A feedback sensor signal from the steering wheel angle and yaw rate sensor is used as feedback information for the controller. The controller consists of a relationship between the future point lateral error, the linear velocity, the heading error and the reference yaw rate. This paper also introduces a spike detection algorithm to track the spike error that occurs during GPS reading. The proposed idea is to take the advantage of the derivative of the steering rate. This paper aims to tackle the lateral error problem by applying the steering control law to the vehicle, and proposes a new path tracking control method by considering the future coordinate of the vehicle and the future estimated lateral error. The effectiveness of the proposed controller is demonstrated by a simulation and a GPS experiment with noisy data. The approach used in this paper is not limited to autonomous vehicles alone since the concept of autonomous vehicle tracking can be used in mobile robot platforms, as the kinematic model of these two platforms is similar.

Abstract Trajectory tracking is an important aspect of autonomous vehicles. The idea behind trajectory tracking is the ability of the vehicle to follow a predefined path with zero steady state error. The difficulty arises due to the nonlinearity of vehicle dynamics. Therefore, this paper proposes a stable tracking control for an autonomous vehicle. An approach that consists of steering wheel control and lateral control is introduced. This control algorithm is used for a non-holonomic navigation problem, namely tracking a reference trajectory in a closed loop form. A proposed future prediction point control algorithm is used to calculate the vehicle's lateral error in order to improve the performance of the trajectory tracking. A feedback sensor signal from the steering wheel angle and yaw rate sensor is used as feedback information for the controller. The controller consists of a relationship between the future point lateral error, the linear velocity, the heading error and the reference yaw rate. This paper also introduces a spike detection algorithm to track the spike error that occurs during GPS reading. The proposed idea is to take the advantage of the derivative of the steering rate. This paper aims to tackle the lateral error problem by applying the steering control law to the vehicle, and proposes a new path tracking control method by considering the future coordinate of the vehicle and the future estimated lateral error. The effectiveness of the proposed controller is demonstrated by a simulation and a GPS experiment with noisy data. The approach used in this paper is not limited to autonomous vehicles alone since the concept of autonomous vehicle tracking can be used in mobile robot platforms, as the kinematic model of these two platforms is similar.

Keywords
Trajectory Tracking, Mobile Robot, Autonomous Vehicle, Yaw Rate Control, Path Following, Steering Control, Spike Control

Introduction
Autonomous vehicles are a rapidly developing field. The application of autonomous vehicles is expected to be widely used in urban areas, industry and airport terminals, etc. An autonomous vehicle is capable of sensing its environment, decision-making and navigating by its self. By using autonomous vehicles, humans can define the destination or path and the rest of the navigation will be taken care of by the system. An advanced control strategy will use the appropriate information to control the vehicle's navigation. The control system must be able to handle the road characteristics -i.e., straight, curvy, rough and varying terrain types. In trajectory tracking, the control strategy is used to track the path and achieve zero steady state error.
The concept of an autonomous vehicle can easily be implemented on a mobile robot platform since the kinematic model of a mobile robot is similar to that of an autonomous vehicle. The term "autonomous vehicle" used in this paper could easily be interpreted as "mobile robot", although there is slight different between them. The autonomous vehicle itself contains a robotics system, such as sensor inputs, signals processing and decisionmaking by the control of the actuators.
Trajectory tracking has been a focus of researchers' attention for years. The main objective is to achieve a fully autonomous vehicle in order to follow a predefined path. One of the popular, pioneering methods was proposed by Kanayama et al. [1], where authors proposed a stable tracking control by controlling the linear velocity and the yaw rate. Kanayama's approach used simplified vehicle kinematics to tackle the trajectory problem. The authors used the Lyapunov method to analyse the stability of the controller. Another control approach was introduced by Fierro et al. [2] and Lee et al. [3], where the backstepping control strategy was used to improve the performance of the trajectory tracking problem. These tracking strategies ignore the dynamics of the vehicle since the focus was on the mobile robot platform. In [4], a fuzzy logic technique was used for vehicle tracking, where the authors introduced two sets of fuzzy logic controls, namely a positioning controller and a following controller. For the autonomous vehicle, the dynamics of the vehicle should be taken into account. This is because the consideration of a dynamic model is more suitable to the actual vehicle system when tested in a real world application.
Most researchers designed the controller based upon a kinematic or dynamic model [5], [6], [7], [8]. In order to facilitate the vehicle motion and dynamic characteristic of the vehicle, the controller should be able to tackle the behaviour of both modes. Therefore, in this paper, the model is developed by combining the dynamic and kinematic motion of the vehicle. The dynamic model is used to estimate the lateral acceleration and yaw rate response of the vehicle, while the kinematic model is used to calculate the lateral and heading direction error when applying the steering wheel. Meanwhile, the kinematic model is used for our reference path generation for vehicle to track the path. This paper aims to tackle the lateral error problem by applying the steering control law to the vehicle and proposes a new path tracking control method by considering the future coordinate of the vehicle and the future estimated lateral error.
In real applications of trajectory tracking, several methods can be used to track the position of the vehicle. Based on [9], [10], [11], [12] the most common sensors used are GPS, electronic compass, inertial measurement unit (IMU), video camera, laser radar, odometer and sonar. However, for outdoor position tracking, most researchers used GPS due to its low cost. In recent developments in the field of autonomous vehicle navigation, the authors in [13] raise a problem related to GPS whereby at least four satellites are required for a less than 20-meter precision error. Therefore, the proposed controller should be robust enough to cope with GPS's uncertainties.
This paper is an extended version of a previous work in [14] where the nonlinear controller for vehicle path tracking was established. In this paper, a spike detection algorithm is introduced to estimate and reduce the overshoot of the vehicle response. The experiment is carried out by capturing GPS data to observe the noise from the GPS reading. This data is used as input for the controller for monitoring purposes.
The paper is organized as follows: in section 2, the concept of the vehicle model is introduced; in section 3, the proposed controller is introduced; and in section 4 the simulation and experimental results of the proposed controller are given.

Vehicle Model
In this research, the development of the vehicle model is based on the two degree of freedom bicycle model, which consists of both the yaw and lateral acceleration [15], [16], [17]. The longitudinal velocity is assumed as constant. The initial acceleration is neglected and the state space of the vehicle model is represented as Equation 1: x Ax Bu where A, B, C and D are defined as: The vehicle's motion depends upon the velocity and the yaw rate velocity. The vehicle kinematic is defined as:  Figure 1 shows the overall control system. A reference generator is used to generate the desired path, which consists of x and y coordinates and a vehicle heading direction, d

Steering Control
θ . An error between the reference path and the prediction vehicle position is calculated before feedback to the nonlinear controller. The nonlinear controller calculates the desired steering wheel angle in order to achieve zero path error. The output from the steering control is fetched as the input of the spike detection algorithm for steering wheel angle adjustments.
The proposed controller is designed based upon the yaw rate tracking, whereby the steering controller will generate a large steering angle when the lateral error is increased in order to turn the vehicle towards the desired path. During cornering, the radius of the arc needs to be maintained in order to navigate properly. This could be described by the velocity and angular rate's relationship to the radius. Thus, the steering wheel angle controller is dependent upon the longitudinal velocity and the angular rate, where v is the vehicle velocity and ψ is the vehicle yaw rate, as given by : and the nonlinear control law for wheel steering is defined as: where f X and f Y are the future prediction point, ef y is the future lateral error, δ is the steering wheel angle, 1 K is the gain and e θ is the vehicle direction error. The controller is used to the generate yaw rate that is required for the vehicle to navigate its position from the predictedvehicle location to the reference path. The first term of Equation 12 acts as a limiter with a range of [ ] 0,1 . The second term acts as the steering wheel ratio based upon the lateral error and the vehicle velocity. The terms ensure that the steering wheel ratio is proportional to the vehicle lateral error in order that it can follow the path with zero steady state error. Vehicle velocity plays an important role in adjusting the ratio of the steering wheel angle. When the velocity increases, the second term ratio of Equation 12 decreases, causing the steering wheel angle output to decrease. The response of the controller is defined by gain 1 K proportional to the lateral error convergence, whereby the gain has a significant impact on the vehicle yaw rate.
An additional saturated function is introduced to control the maximum steering wheel angle. From Equation 12 it has been shown that the lateral error, ef y , is proportionate to the steering angle, δ , and may exceed the maximum vehicle steering wheel angle. Therefore, the steering wheel angle is limited between 4 4 π π δ − ≤ ≤ . The new saturated value is the revised value from the previous research, where the value is obtained by experiment.
The ISO 2631-1 standard was used as the benchmark to determine the comfort level of the vehicle, as shown in Table 1. The overall acceleration defined by the ISO 2631-1 standard is as in [18]

Spike Detection Algorithm
While a lot of control design development disregards the effect of wrong data inputs into the controller, this paper introduces a simple algorithm to check an inaccurate input into the controller by monitoring the output response of the controller. It is often the case during error reading that the spike occurs due to the value of the reading suddenly changing to a non-related value. This can be illustrated when navigating the vehicle using GPS. The GPS reading can suddenly change or be lost for many reasons, such as an inconsistent number of satellites or multipath data errors being received. The data received in this condition is normally inaccurate data and the difference from the previous data is obvious. Because of this, the steering control will generate a large steering angle due to the fluctuation of the GPS data reading, which will cause the incorrect navigation of the vehicle.
To overcome this problem, the authors propose a solution by considering the case where tracking the controller sudden changes and by monitoring the behaviour of the steering control angle derivative generated from the controller. This derivative value can be used as an indicator if the spike occurs by checking the offset of the derivate: The spike detection algorithm is defined as: Where δk-1 is the previous steering angle value at one time step, K2 is the steering tuning gain and offset δ  is the derivate offset limit for spike detection. The simple algorithm can be described by analysing the term used in the algorithm. In Equation 14, the steering rate offset is set in the program to monitor the steering controller output behaviour in a certain region that is allowed to operate. The value of the steering rate offset can easily be achieved by tuning the steering rate during the experimental testing. The allowable maximum steering rate is the steering offset in Equation 14. Over this offset value, the program will indicate that the 'spike' is occurred in the steering output of the controller. This means that the current data received by the controller input is incorrect. Therefore, to reduce the effect of an erroneous controller response, Equation 15 will be used to provide the fix for the steering control output. Equation 15 will be used to reduce the steering controller output by the factor of K2. The value of the steering wheel angle required, δ , is always positive, which is the magnitude value since the negative sign indicates the direction of the steering turning response. The proper value of K2 can be obtained through the tuning process during the experiment so as to get the best steering angle correction required.

Steering Control Response
This section discusses the response of the wheel steering controller to the lateral error and the desired yaw rate. The lateral acceleration of the vehicle associated with controller gain is presented. The chosen of 1 K is dependent upon the initial heading error, the vehicle lateral acceleration and the response of the controller to the path. An appropriate optimized value of gain 1 K is selected:  Table 3. K1 Tuning Response for θe0 = 0, v = 7ms -1 (25kmh -1 ) Table 3 shows the variation of 1 K with the overall acceleration response. Based upon analysis in Table 3, the controller falls under the level a little uncomfortable for a low gain of 1 K based upon the comfort level as in Table 1. Under high 1 K , the response of the vehicle began to fall under uncomfortable. From Table 3, it has been shown that the increases of gain 1 K will result in the level uncomfortable for the driver.
The lateral error response of 1 0 K = can be seen in Figure  3.1. The graph shows that at 1 0 K = the error has not converged on zero. The simulation results show that the lateral error steady state for 1 0 K = is -2.3. At 1 1.00 K = , the response of the vehicle is faster in achieving the desired state. However, from Figure 4, it is observed that chattering does exist in the initial trajectory before the vehicle becomes stable at 1 1.00 K = . This is because the yaw rate generated from the controller is higher at in achieving a fast response as shown in Figure   3.2. From Figure 3.1, on the lateral error response, the variation of 1 K impacts upon the response time of the controller in converging on zero. Hence, the 1 K characteristic is a time-varying gain.   Figure 4, with an initial heading error and where e θ is equal to zero, it can be seen that at 1 0 K = the vehicle was unable to reach the desired path in a steady state since the lateral error was not stabilized at zero. This is due to the second term of the control law in Equation 8 being cancelled by the coefficient zero. Thus, when the vehicle heading direction is the same as the desired heading direction, the first term of the control law will become zero. The second term in the lateral error controller, ef y , does not give any feedback to the steering wheel. As a result, the vehicle follows the desired path in parallel due to there being no error correction. It can be concluded that at a high value of 1 K , the vehicle approaches the desired path faster due to the high yaw rate generated from the steering wheel input. However, this caused the lateral acceleration increases as a result of the fast turning rate to the desired path.

Experiment with GPS data
In order to validate the performance of the nonlinear controller design, a real GPS coordinate was used. A low cost GPS sensor module was used. The reading was taken only when at least six satellites are found by GPS module.
The raw GPS data is converted into the Cartesian coordinate format whereby a local North-East-Down (NED) frame is used and the vehicle starting location was at (0,0) and moving along the path by manual control and capturing the GPS coordinate.
The coordinate transformation from a Earth-Centred Earth-Fixed (ECEF) coordinate to a local NED frame is defined as in [19], whereby: where ned P is a point in the NED coordinate frame, / n e R is the transformation matrix, e P is the position vector in the ECEF frame,  From the figure, it can be seen that the GPS reading is a combination of the GPS position and the noise causing an inconsistent reading from the actual value. This is due to such errors occurring as satellite clock errors, ephemeris errors, ionosphere errors, troposphere errors, receiver errors and multipath errors [20]. These errors contributed to the noisy reading of the sensors. Figure 6 shows the trajectory response of the vehicle. It can be seen that the response is not as smooth as in the ideal case due to the noise error in the position reading. From the response, it is observed that the vehicle is still able to track the reference path with minimal error. The controller is able to avoid sharp turning, although the reference error is large. This is important since sharp turning is caused by the increase of the lateral acceleration of the vehicle, which is not ideal for vehicle navigation.   Figure 7 shows the crosstrack error of the vehicle with its reference path. It is expected that the error at K1 = 0 is not converged on zero because there is no crosstrack error correction. The error for K1 is equal to 0.25, 0.5 and 1.0, which are relatively small.

Analysis of Spike Detection Algorithm
The steering angle rate is observed when the spike occurs. From Figure 8, it can be seen that the spike causes a response of a high steering value. Therefore, it is observed that the spike is correlated with the steering rate response of the controller. Figure 9 shows the steering control response both with and without the spike detection algorithm. The spike signal in the simulation is introduced to simulate uncertainties in the position of the coordinate. The spike is introduced with significant or abrupt changes from the current position reading. This is to simulate the potential errors from the GPS reading, which will affect the controller's response. From the results, it can be seen that the response of the steering control without the spike detection algorithm tends to manoeuvre away from the pre-defined track compared with the steering control with the spike detection algorithm. Therefore, it reduces the lateral acceleration generated from the vehicle, which will result in a smooth response of the vehicle being achieved.

Conclusion
Based upon the simulation carried out, it can be concluded that the proposed nonlinear controller was able to follow the reference path. The vehicle lateral acceleration and path response were observed in order to determine the optimum parameter value of the controller gain. The multiple values of gain are illustrated to present the effect of the gain on the vehicle. The appropriate 1 K have been chosen by observing the characteristics of the controller. By selecting a small value of 1 K , a slow response of the controller is obtained; however, the small value of the lateral acceleration generated caused the comfort level to increase. If the fast response controller is selected, the comfort level of the vehicle is therefore reduced. Furthermore, the spike detection algorithm is introduced. The simulation results show that the algorithm is able to reduce the unwanted coordinates of the vehicle and achieve a smoother response. The experiment is carried out to monitor the noise characteristics of the GPS sensor. From that, the actual GPS data is fetched to the controller to monitor the controller response if the input data is noisy. The proposed controller is proven to be stable by simulation and experiment from the actual GPS data. However, noisy data will affect the performance of the controller as well. For future research, an Extended Kalman Filter will be investigated in order to estimate the GPS coordinate and reduce the noise in the position reading.