Robust Path Tracking Control for Autonomous Vehicle Based on a Novel Fault Tolerant Adaptive Model Predictive Control Algorithm

Abstract

Autonomous vehicles are expected to completely change the development model of the transportation industry and bring great convenience to our lives. Autonomous vehicles need to constantly obtain the motion status information with on-board sensors in order to formulate reasonable motion control strategies. Therefore, abnormal sensor readings or vehicle sensor failures can cause devastating consequences and can lead to fatal vehicle accidents. Hence, research on the fault tolerant control method is critical for autonomous vehicles. In this paper, we develop a robust fault tolerant path tracking control algorithm through combining the adaptive model predictive control algorithm for lateral path tracking control, improved weight assignment method for multi-sensor data fusion and fault isolation, and novel federal Kalman filtering approach with two states chi-square detector and residual chi-square detector for detection and identification of sensor fault in autonomous vehicles. Our numerical simulation and experiment demonstrate that the developed approach can detect fault signals and identify their sources with high accuracy and sensitivity. In the double line change path tracking control experiment, when the sensors failure occurs, the proposed method shows better robustness and effectiveness than the traditional methods. It is foreseeable that this research will contribute to the development of safer and more intelligent autonomous driving system, which in turn will promote the industrial development of intelligent transportation system.

1. Introduction

With the development of autonomous driving technology, autonomous vehicles have shown great value in improving vehicle safety, enhancing traffic efficiency, liberating the driver’s hands, etc., which has attracted more and more attention from relevant scholars [1]. One of the most challenging and significant research issues for autonomous vehicle is the smooth and accurate automatic path tracking control, which is important for insuring the driving safety and accurately tracking the reference trajectory [2]. The main goal of the automatic path tracking control system is to guarantee the lateral path following errors with maintaining the lateral driving stability and considering the dynamic constraints of autonomous vehicle.

Many vehicle control algorithms have been presented to obtain active safety and enhance the lateral dynamic stability and path tracking performance. In [3], the authors presented a robust control algorithm of path tracking with considering the problem of delay and data dropout for autonomous vehicle. In [4], the active disturbance rejection control method and differential flatness theory were used for lateral path tracking control of autonomous vehicle. In [5] the disturbances within a finite preview window were augmented as the state vector and a preview steering control strategy was presented for linear augmented error system. However, all these automatic control strategies focus on constant velocity. Due to the inevitable variation of vehicle longitudinal velocity, the previous studies on a fixed constant longitudinal velocity face a significant limitation in real driving conditions.

To track the reference path within limited velocity without collisions, the autonomous vehicle needs to accurately acquire the motion states information and percept the immediate physical traffic environment. Therefore, numerous sensors are adopted for autonomous vehicles, such as camera, inertial measurement unit (IMU), LIDAR, RADAR and GPS (Global Positioning System) integrated navigation system. The effective working conditions of each type of sensors are not the same. For example, the camera cannot work normally when the illumination conditions are insufficient. The detection accuracies of RADAR and LIDAR sensors are easily affected by bad weather such as smoke, rain, and snow. The GPS signal is easily blocked in the environment with tall trees, urban buildings, and tunnels, which can cause signal interruption. Therefore, the sensors of autonomous vehicle may be influenced by illumination, signal outage, poor weather conditions, magnetic interference, and other environmental circumstances. In addition, the sensors age and inherent errors may also result in sensor failure and affecting data availability. All these issues may be the reasons prompting the transmission of faulty information [6,7,8]. The possibility of sensors fault increases with the numbers and types of vehicle sensors, which can result in disruptive consequences and fatal crashes.

The method of sensor redundancy and data fusion can be used to avoid sensor fault. The same motion parameters of an autonomous vehicle can be obtained by multiple different sensors [9]. For instance, the vision system used for vehicle detection can be affected by traffic lights and high-brightness lights of vehicle in nighttime; however, RADAR and LIDAR would not be affected in these cases. Hence, by using the method of sensor redundancy, the impact of sensor fault can be reduced into an acceptable level. However, the reliability of measured parameters after data fusion is still doubtful, if the faulty sensor data are not detected and isolated. Many methods for sensor fault detection have been proposed; however, few of them are focused on autonomous vehicles. Some methods use the distances as the metrics for sensor fault detection, for instance the Mahalabobis distance, affinity propagation clustering, and graph theory [10,11,12,13]. In [14,15], the Kalman filtering method was used for fault detection. With the development of deep learning technology, more and more researchers develop the application of deep learning methods for fault detection, such as convolution neural network [16], recurrent neural network [17], and multilayer neural network models [18,19]. In [20,21], Bayesian networks were used for sensors fault detection of autonomous vehicle. Even though the deep learning neural network can deal with the raw data without manually feature extraction, deep learning-based methods require a large-scale dataset with manual annotations, which is difficult to collect.

The large-scale implementation of autonomous vehicles in transportation systems require the effective sensor fault detection and robust path tracking motion control. One of the significant contributions of this work is to detect the source fault sensor and isolated the fault signal in real-time for autonomous vehicle to enhance the reliability of fused data. The other one is the realization of robust path tracking control when sensor failures occur. Specifically, we develop a fault-tolerant path tracking control approach with combining the model of adaptive model predictive control algorithm and a novel sensor fault detection method based on federal Kalman filtering and chi-square detector. The main contributions of this work can be described as: (1) a single-track 3DOF vehicle dynamics model is established and Taylor expansion is performed for modelling linearization; (2) an adaptive model predictive control algorithm is developed for robust path tracking control; (3) an improved weight assignment method based data fusion method is proposed for multi-sensor data fusion and fault signal isolation; and (4) a novel sensor fault detection approach is proposed using federal Kalman filtering and fault detector with parallel structure for each sensing source, consisting of two state chi-square detector and residual chi-square detector. The simulation and experimental results of double lane changing show the effectiveness of the proposed method in this work.

The rest of this paper is organized as follows. The vehicle dynamic model is established and model linearization process is described in Section 2.1 and Section 2.2. The constraints and objective functions are constructed in Section 2.3, Section 2.4, Section 2.5 and Section 2.6, and the path tracking control algorithm based on model predictive control is designed in Section 2.7. The multi-sensor information fusion algorithm and fault signal detection and isolation algorithm are described in Section 2.8 and Section 2.9, respectively. In Section 3, the simulation and experimental verification using double lane changing path tracking scenario is presented. Finally, the conclusions are given.

2. Methodology

The flowchart of the fault-tolerant model predictive control algorithm (FTAMPC) designed in this paper is shown in Figure 1. Vehicle motion status information can be obtained by GPS combined navigation system, visual odometer system, and LIDAR SLAM system. The multi-sensor signals are fused by the proposed data fusion algorithm and fault signals are detected by the proposed fault detectors based on federal Kalman filtering chi-square detector. The fault signals are isolated in the data fusion process. Finally, the designed adaptive model predictive controller is used for robust path tracking control of autonomous vehicle.

2.1. Modeling and Problem Formulation

The main goal of this work is to enable the vehicle to track the target path quickly and steadily. The vehicle suspension characteristics are relatively small in relation to the research content, so the single-track model is selected. At the same time, the dynamic model established in this paper is mainly used to design the predictive model in the model predictive controller. It needs to be simplified as much as possible while accurately describing the dynamic characteristics of the vehicle. The following idealized assumptions are first proposed when performing dynamic modeling: (1) road fluctuations are ignored, assuming that the vehicle is always driving without vertical motion; (2) suspension motion is ignored; (3) the load movement of the front and rear axles is not considered, and the left and right transfer of the load is ignored; (4) the tire cornering characteristics are considered, but the vertical and horizontal coupling relationships are ignored; and (5) the mechanical effects of steering system are ignored. In this paper, a three-degree-of-freedom vehicle dynamics model is constructed, including longitudinal motion, lateral motion, and yaw (see Figure 2).

The longitudinal force, lateral force, and yaw moment received by the vehicle can be written as:

$$\{\begin{array}{l}m\left(\ddot{x}-\dot{y}\dot{\phi }\right)={\displaystyle \sum F_x}=2F_{lf}\cos {\delta }_f-2F_{cf}\sin {\delta }_f+2F_{lr}\\ m\left(\ddot{y}+\dot{x}\dot{\phi }\right)={\displaystyle \sum F_y}=2F_{lf}\sin {\delta }_f-2F_{cf}\cos {\delta }_f+2F_{cr}\\ I_z\ddot{\phi }={\displaystyle \sum M_z}=2L_f\left(F_{lf}\sin {\delta }_f+F_{cf}\cos {\delta }_f\right)-2L_r{F}_{cr}\end{array}$$

(1)

where $m$ is the vehicle mass; $x$ is the longitudinal position; $\phi$ is the yaw angle; ${\delta }_f$ is the front wheel rotation angle; $y$ is the lateral position; $I_z$ is the $z$-axis moment of inertia; $F_x$ is the total longitudinal force;$F_y$ is the total lateral force; $M_z$ is the total yaw moment; $F_{cf}$ and $F_{cr}$ are the lateral forces on the front and rear tires of the vehicle and are related to the corner stiffness and corner angle of tires; $F_{lf}$ and $F_{lr}$ are the longitudinal force on the front and rear tires of vehicle, which are related to the longitudinal stiffness and slip rate of the tire; $F_{xf}$ and $F_{xr}$ are the forces on the front and rear tires of the vehicle in the $x$ direction; $F_{yf}$ and $F_{yr}$ are the forces on the front and rear tires of the vehicle in the $y$ direction; and $L_f$ and $L_r$ are the distances from the front and rear axis to the center of mass.

Generally, the stable driving vehicle has small variation angle and slip rate. The tire forces involved in this paper are all approximated by a linear function. In addition, there are many trigonometric functions in the vehicle dynamics model described by formulas. Since each angle involved in the dynamics model is in a small angle interval, each trigonometric function can satisfy the following approximate conditions: $\cos \theta \approx 1$, $\sin \theta =0$, $\tan \theta =\theta$. Hence, the tire force of the vehicle can be expressed as:

$$\{\begin{array}{l}{F}_{lf}=C_{lf}{s}_f\\ F_{lr}=C_{lr}{s}_r\\ F_{cf}=C_{cf}\left[{\delta }_f-\left(\dot{y}+L_f\dot{\phi }\right)/\dot{x}\right]\\ F_{cr}=C_{cr}\left(L_r\dot{\phi }-\dot{y}\right)/\dot{x}\end{array}$$

(2)

where $C_{cf}$ and $C_{cr}$ are the lateral stiffness of the front and rear tires; $C_{lf}$ and $C_{lr}$ are the longitudinal stiffness of the front and rear tires; and $s_f$ and $s_r$ are the slip ratio of the front and rear tires.

The non-linear dynamic vehicle model can be written as:

$$\{\begin{array}{l}\ddot{x}=\dot{y}\dot{\phi }+\left(2/m\right)\left\{C_{lf}{s}_f-C_{cf}\left[{\delta }_f-\left(\dot{y}+L_f\dot{\phi }\right)/\dot{x}\right]{\delta }_f+C_{lr}{s}_r\right\}\\ \ddot{y}=-\dot{x}\dot{\phi }+\left(2/m\right)\left\{C_{lf}{s}_f{\delta }_f+C_{cf}\left[{\delta }_f-\left(\dot{y}+L_f\dot{\phi }\right)/\dot{x}\right]-C_{cr}\left(\dot{y}-L_r\dot{\phi }\right)/\dot{x}\right\}\\ \dot{\phi }=\dot{\phi }\\ \ddot{\phi }=\left(2L_f/I_z\right)\left\{C_{lf}{s}_f{\delta }_f+C_{cf}\left[{\delta }_f-\left(\dot{y}+L_f\dot{\phi }\right)/\dot{x}\right]\right\}+\left(2L_r/I_z\right)C_{cr}\left(\dot{y}-L_r\dot{\phi }\right)/\dot{x}\\ \dot{Y}=\dot{x}\sin \phi +\dot{y}\cos \phi \\ \dot{X}=\dot{x}\cos \phi -\dot{y}\sin \phi \end{array}$$

(3)

For convenience, we use ${\xi }_{dyn}={\left[\dot{y},\dot{x},\phi ,\dot{\phi },Y,X\right]}^T$ as the system state quantities and $u_{dyn}={\delta }_f$ as the system control quantity.

2.2. Linearization of Vehicle Dynamics Model

The lateral motion control of autonomous vehicle is to control the front wheel rotation angle for lateral movement, and then realize the path tracking purpose. Therefore, this paper selects path tracking as the ultimate goal of autonomous vehicle lateral control and the tracking accuracy as the main indicator to measure the performance of the control system. Model predictive control can be divided into linear time-varying model predictive control (LMPC) and non-linear model predictive control (NMPC) [22,23]. The linear time-varying model predictive control algorithm uses the linear time-varying model as the predictive model. Compared with the nonlinear model predictive control, the real-time performance of the linear time-varying model predictive control is much better. For the motion control of autonomous vehicles, the real-time performance of the control algorithm is very important; therefore, we use LMPC for lateral motion control of autonomous vehicles in our work.

Firstly, the non-linear dynamic vehicle model needs to be linearized. The state quantity and control quantity of the system satisfy the following relationship:

$${\dot{\xi }}_{dyn}=f\left({\xi }_{dyn},u_{dyn}\right)$$

(4)

Performing the Taylor expansion at any point $\left({\xi }_r,u_r\right)$, retaining the first-order term and ignoring the higher-order terms, we can get:

$$\dot{\xi }=f\left({\xi }_r,u_r\right)+{\frac{\partial f}{\partial f\xi }|}_{\begin{array}{c}\xi ={\xi }_r\\ u=u_r\end{array}}\left(\xi -{\xi }_r\right)+{\frac{\partial f}{\partial u}|}_{\begin{array}{c}\xi ={\xi }_r\\ u=u_r\end{array}}\left(u-u_r\right)$$

(5)

The formula can be transformed into:

$$\dot{\xi }=f\left({\xi }_r,u_r\right)+J_f\left(\xi \right)\left(\xi -{\xi }_r\right)+J_f\left(u\right)\left(u-u_r\right)$$

(6)

where $J_f\left(\xi \right)$ and $J_f\left(u\right)$ are the Jacobian matrixes of $f$ relative to $\xi$ and $u$, respectively.

The linearized system equation can be written as:

$$\begin{array}{l}\dot{\xi }=A\left(t\right)\xi \left(t\right)+B\left(t\right)u\left(t\right)\\ \zeta =C\xi \left(t\right)\end{array}$$

(7)

where $A\left(t\right)=J_f\left(\xi \right)$, $B\left(t\right)=J_f\left(u\right)$, $C={\left(0,0,0,0,1,0\right)}^T$.

After introducing the incremental model, the state space equation is written as:

$$\{\begin{array}{l}\Delta \xi \left(k+1\right)=A\left(k\right)\Delta \xi \left(k\right)+B\left(k\right)\Delta u\left(k\right)\\ \zeta \left(k\right)=C\Delta \xi \left(k\right)\end{array}$$

(8)

2.3. Construct the Constraints

In the general control process, the control quantity limit constraint and the control increment constraint must be considered. The expression of the limit constraint of the control variable is:

$$u_{\min }\left(t+k\right)\le u\left(t+k\right)\le u_{\max }\left(t+k\right),\hspace{1em}k=0,1,\cdots ,N_c-1$$

(9)

where $N_c$ is the control time domain.

The expression of the control incremental constraints is:

$$\Delta u_{\min }\left(t+k\right)\le \Delta u\left(t+k\right)\le \Delta u_{\max }\left(t+k\right),\hspace{1em}k=0,1,\cdots ,N_c-1$$

(10)

We assume that:

$$\{\begin{array}{l}u\left(t+k\right)=u\left(t+k-1\right)+\Delta u\left(t+k\right)\\ U_t=1_{N_c}\otimes u\left(k-1\right)\\ \widehat{A}=A\otimes I_m\end{array}$$

(11)

where $1_{N_c}$ is column vector with rows $N_c$; $I_m$ is m-dimensional identity matrix; $\otimes$ is the Kronecker product; and $u\left(k-1\right)$ is the actual control quantity of the system at the last moment.

The expression of the limit constraint of the controlled variable and the expression of the control incremental constraints can be converted into:

$$\{\begin{array}{l}\Delta U_{\min }\le \Delta U_t\le \Delta U_{\max }\\ U_{\min }\le \widehat{A}\Delta U_t+U_t\le U_{\max }\end{array}$$

(12)

where $U_{\min }$ and $U_{\max }$ represent the minimum and maximum value of the control variable in the control time domain.

2.4. Dynamic Constraint of Tire Cornering

The vehicle may slip due to wet or slippery roads during autonomous driving process, which is extremely risky for vehicle safety. Therefore, it is particularly important to increase vehicle dynamics constraints and reduce the possibility of vehicle sideslip.

The sideslip of the vehicle is closely related to the tire slip angle. On the absolute horizontal road, the tire slip angle $\alpha =0$; when the tire is elastically deformed by lateral force without lateral slip, $\alpha \le {\alpha }_{\max }$; and when the tire is subjected to excessive lateral force, the vehicle slips $\alpha >{\alpha }_{\max }$. It can be concluded that the slip angle of the vehicle tire directly reflects whether the vehicle is slipping, and limiting the tire slip angle limits the risk of sideslip.

Since the established vehicle dynamics space equation does not take the tire slip angle as a state quantity, and cannot directly constrain the tire slip angle, this paper needs to find the relationship between the tire slip angle $\alpha$ and the state quantity $\xi$. The relationship is to restrain the tire slip angle by imposing a specific relationship constraint on the state quantity.

The available tire front and rear wheel angles can be written as [24]:

$$\{\begin{array}{l}{\alpha }_f=\left(\dot{y}+L_f\dot{\phi }\right)/\dot{x}-{\delta }_f\\ {\alpha }_r=\left(\dot{y}-L_r\dot{\phi }\right)/\dot{x}\end{array}$$

(13)

The linearization of the above formula can be written as:

$$\{\begin{array}{l}\alpha =E\xi \left(k,t\right)+F{u}_{dyn}\left(k,t\right)\\ E=\left[\begin{array}{cccccc}\frac{1}{\dot{x}}& -\frac{\dot{y}+L_f\dot{\phi }}{{\dot{x}}^2}& 0& \frac{L_f}{\dot{x}}& 0& 0\\ \frac{1}{\dot{x}}& -\frac{\dot{y}-L_r\dot{\phi }}{{\dot{x}}^2}& 0& -\frac{L_r}{\dot{x}}& 0& 0\end{array}\right]\end{array}$$

(14)

where $\alpha ={\left[{\alpha }_f,{\alpha }_r\right]}^T$ is the tire corner angle matrix, $F={\left[-1,0\right]}^T$, and $E$ is the transform matrix.

The incremental constraint of tire cornering can be described as:

$$\Delta {\alpha }_{\min }\le \Delta \alpha \le \Delta {\alpha }_{\max }$$

(15)

where ${\alpha }_{\min }$ and ${\alpha }_{\max }$ represent the minimum and maximum value of the tire cornering.

2.5. Lateral Acceleration Constraints

The road adhesion coefficient has a great influence on the dynamics of the vehicle. In addition, if the lateral acceleration is too large, the vehicle dynamic, driving comfort and driving safety will be seriously affected. If the constraints of lateral acceleration are too small, the solution of the controller may fail. Hence, the incremental constraint of lateral acceleration can be described as:

$$a_{y,\min }+\epsilon \le a_y\le a_{y,\max }-\epsilon$$

(16)

where $a_{y,\min }$ and $a_{y,\max }$ represent the minimum and maximum value of the lateral acceleration, $\epsilon$ is the relaxation factor, and $a_y=\mu g$ is a quantity proportional to the road adhesion coefficient.

2.6. Construct the Objective Function

This article uses the following objective function [25]:

$$J\left(k\right)={\displaystyle \sum _{i=1}^{N_p}{\Vert \Delta \eta \left(k+i|k\right)\Vert }_Q^2}+{\displaystyle \sum _{i=1}^{N_c-1}{\Vert \Delta u\left(k+i|k\right)\Vert }_R^2}+\rho {\epsilon }^2$$

(17)

where $\rho$ is the weight coefficient; $\epsilon$ is the relaxation factor; $N_p$ is the prediction time domain; $N_c$ is the control time domain; Q is the state weighting matrix; R is the control weighting matrix; $\Delta \eta \left(k+i|k\right)=\eta \left(k+i|k\right)-{\eta }_r\left(k+i|k\right)$ is difference between actual output and reference path; and $\Delta u\left(k+i|k\right)$ is control deviation. Considering the complexity of the vehicle dynamic model and constraints, to avoid the occurrence of no optimal solution, the relaxation factor $\epsilon$ is added to the objective function.

2.7. Adaptive Model Predictive Controller for Vehicle Lateral Motion Control

Based on the above objective function and constraints, the optimization problem of the vehicle lateral motion controller can be described as:

$$\{\begin{array}{l}{\min }_{\Delta U,\epsilon }{\displaystyle \sum _{i=1}^{N_p}{\Vert \Delta \eta \left(t+i|t\right)\Vert }_Q^2}+{\displaystyle \sum _{i=1}^{N_c-1}{\Vert \Delta u\left(t+i|t\right)\Vert }_R^2}+\rho {\epsilon }^2\\ \Delta U_{\min }\le \Delta U\le \Delta U_{\max }\\ \Delta U_{\min }\le A\Delta U+U_f\le \Delta U_{\max }\\ {\alpha }_{\min }\le \Delta \alpha \le \Delta {\alpha }_{\max }\\ a_{y,\min }+\epsilon \le a_y\le a_{y,\max }-\epsilon \\ \epsilon \ge 0\end{array}$$

(18)

Optimize and solve the above formula to get the control increment and relaxation factor sequence in the control time domain:

$$\Delta U_{dyn,t}^{*}={\left[\Delta u_{dyn,t}^{*},\Delta u_{dyn,t+1}^{*},\cdots ,\Delta u_{dyn,t+N_c-1}^{*},\epsilon \right]}^T$$

(19)

The first element of the sequence acts as an actual control increment:

$$u_{dyn}\left(t\right)=u_{dyn}\left(t-1\right)+\Delta u_{dyn,t}^{*}$$

(20)

In the following time domain, the above processes are repeated, and the vehicle tracks the target path cyclically.

2.8. Merging Multi-Sensor Data and Isolating Fault Signals

If we use multiple sensors to measure the same parameter of autonomous vehicle, we can merge the outputs of all sensors using the weight assignment method for data fusion purpose. The result of data fusion can be written as follows:

$$O=WI=\left[w_1,w_2,\cdots ,w_n\right]{\left[i_1,i_2,\cdots ,i_n\right]}^T$$

(21)

where $O$ is the result of data fusion; $W=\left[w_1,w_2,\cdots ,w_n\right]$ is the weight matrix; $I={\left[i_1,i_2,\cdots ,i_n\right]}^T$ is outputs of sensors; $n$ is the number of sensors.

The principle of the method of weight assignment can be written as follows:

$$\{\begin{array}{l}{w}_j=m_j/\left[{\sigma }_j^2{\displaystyle \sum _{i=1}^n\left(m_i/{\sigma }_i^2\right)}\right]\\ {\displaystyle \sum _{i=1}^n{w}_j=1}\end{array}$$

(22)

where ${\sigma }_i$ and ${\sigma }_j$ are the dispersions of the output errors of the ith and jth sensors, $i=1,2,\cdots ,n$; $M=\left[m_1,m_2,\cdots ,m_n\right]$ is the fault detection matrix.

As the true values of vehicle motion states cannot be obtained, the work determines the average value of the different sensors outputs, which is usually used as the true value. However, this method is not suitable for our situation, since different types of sensors in different conditions can give a deliberately low accuracy or even failure; thus, the average value may have a large difference with the true value, which is extremely harmful for vehicle safety.

Assume that at moment k sensor j gives the measured value $T_j\left(k\right)$; then:

$$\{\begin{array}{l}\Delta T_j\left(k\right)=T_j\left(k\right)-{\widehat{T}}_j\left(k\right)\\ \Delta {\overline{T}}_j=\frac{1}{N}{\displaystyle \sum _{k=1}^N\Delta T_j\left(k\right)}\\ {\sigma }_j\left(k\right)=\frac{1}{N}{{\displaystyle \sum _{k=1}^N\left[\Delta T_j\left(k\right)-\Delta {\overline{T}}_j\right]}}^2\end{array}\hspace{1em}\hspace{1em}k=1,2,\cdots ,N$$

(23)

where $\Delta T_j\left(k\right)$ is the measurement error of the jth sensor at moment k; $\Delta {\overline{T}}_j$ is the average value of the jth sensor; ${\sigma }_j\left(k\right)$ is the variance of the output error of the jth sensor at moment k; ${\widehat{T}}_j\left(k\right)$ is prognostic assessment of states information obtained using the Kalman filter; and N is the number measurements from each sensor.

Since the outputs of federal Kalman filters are obtained after unreliable data detection and isolation, we can approximately consider the estimated information as true values of vehicle motion states.

2.9. Fault Signal Detector Design

Figure 3 shows the block diagram of proposed faults signals detector based on federal Kalman filter and chi-square detector.

Every Kalman filter used in our work is standard; we take the sub Kalman_Filter_1 for GPS signal channel as example. The state vector and measurement vector can be written as:

$$\{\begin{array}{l}{X}_{GPS}=\left[\Delta \dot{x},\Delta \dot{y},\Delta \phi ,\Delta \dot{\phi },\Delta X,\Delta Y\right]\\ Y_{GPS}=\Delta \phi \end{array}$$

(24)

The state space and measurement space equations of Kalman Filter from moment k − 1 to moment k can be written as:

$$\{\begin{array}{l}{X}_{GPS,k}=F_{GPS,k}{X}_{GPS,k-1}+w_{GPS,k}\\ Y_{GPS,k}=H_{GPS,k}{X}_{GPS,k}+v_{GPS,k}\end{array}$$

(25)

where $F_{GPS,k}$ is the state transition matrix, $w_{GPS,k}$ is the process noise, $H_{GPS,k}$ is the measurement transition matrix, and $v_{GPS,k}$ is the measurement noise.

The equations of Kalman Filter can be written as:

$$\{\begin{array}{l}{\widehat{X}}_{GPS,k|k-1}=F_{GPS,k}{\widehat{X}}_{GPS,k-1|k-1}\\ P_{GPS,k|k-1}=F_{GPS,k}{P}_{GPS,k-1|k-1}{F}_{GPS,k}^T+Q_{GPS,k}\\ K_{GPS,k}=P_{GPS,k|k-1}{H}_{GPS,k}^T{\left(H_{GPS,k}{P}_{GPS,k|k-1}+R_{GPS,k}^T\right)}^{-1}\\ {\widehat{X}}_{GPS,k|k}={\widehat{X}}_{GPS,k|k-1}+K_{GPS,k}\left(Y_{GPS,k}-H_{GPS,k}{\widehat{X}}_{GPS,k|k-1}\right)\\ P_{GPS,k|k}=\left(I-K_{GPS,k}{H}_{GPS,k}\right)P_{GPS,k|k-1}\end{array}$$

(26)

where $P_{GPS,k|k}$ is systematic covariance matrix, $K_{GPS,k}$ is the gain matrix, $Q_{GPS,k}$ is the process noise covariance matrix, and $R_{GPS,k}^{}$ is the measurement noise covariance matrix.

The state propagator can be written as:

$$\{\begin{array}{l}{\widehat{X}}_{i,k|k-1}=F_{i,k}{\widehat{X}}_{i,k-1|k-1}\\ P_{i,k|k-1}=F_{i,k}{P}_{i,k-1|k-1}{F}_{i,k}^T+Q_{i,k}\end{array},\hspace{1em}\hspace{1em}i=1,2$$

(27)

The ${\chi }^2$ chi-square detector is widely used to detect malfunctions in stochastic dynamic systems [26]. This method can be divided into three types: (1) ${\chi }^2$ detector for residual error, which can be used for sensor fault detection; (2) ${\chi }^2$ detector state with a single state propagator, which can be used for the state transfer error detection and sensor fault detecting with the disadvantage of error accumulation; and (3) ${\chi }^2$ detector with a double state propagators, which can be used for the state transfer error detection without error accumulation.

In this paper, we use the ${\chi }^2$ detector for residual error and the ${\chi }^2$ detector with a double state propagators to form a fault signal detector with a parallel structure, as shown in Figure 4.

To reduce the number of propagators, we designed two double state propagators, the outputs of which are used for each state ${\chi }^2$ detector. The working principle can be described as following: during $t_k$ there is a malfunction and a switch $K_1$ is in position “$L_1$”, switch $K_2$ is in position “$L_2$”, and State_Propagator_1 is “dirty” due to this malfunction, but the output of State_Propagator_2 was obtained using the previous correct state, that is, this output is correct and can be used to correct malfunctions. During $t_{k+1}$, errors in prediction 1 are corrected using the output of Kalman filter. After time $\Delta t$, the $K_1$ switch is in position “$L_2$”, the switch $K_2$ is in position “$L_1$“, and the State_Propagator_2 is used to correct the malfunction of the state.

The error state vectors of state propagators can be defined as:

$$\{\begin{array}{l}{e}_{i,k}=X_k-{\widehat{X}}_{i,k}\\ e_{GPS,k}=X_k-{\widehat{X}}_{GPS,k}\end{array}$$

(28)

where $X_k$ is true state vector, ${\widehat{X}}_{GPS,k}$ is estimation error from Sub_Kalman_Filter_1, and ${\widehat{X}}_{i,k}$ is estimation error from state propagator i. In this paper, we use the following variable to define the detector statistic:

$${\beta }_{GPS,k}=e_{i,k}-e_{GPS,k}={\widehat{X}}_{GPS,k}-{\widehat{X}}_{i,k}$$

(29)

The variance of this variable can be written as:

$$T_{GPS,k}=E\left\{{\beta }_{GPS,k}{\beta }_{GPS,k}^T\right\}=E\left\{e_{GPS,k}{e}_{GPS,k}^T-e_{GPS,k}{e}_{i,k}^T-e_{i,k}{e}_{GPS,k}^T+e_{i,k}{e}_{i,k}^T\right\}=P_{GPS,k}-P_{GPS\_i,k}-P_{i\_GPS,k}+P_{i,k}$$

(30)

where $P_{GPS\_i,k}$ and $P_{i\_GPS,k}$ are the cross covariance between the Sub_Kalman_Filter_1 and the state propagator i.

We set the same initial conditions for the Sub_Kalman_Filter_1 and the state propagator i; then, $P_{GPS\_i,k}=P_{i\_GPS,k}=P_{i,k}$, thus the variance can be written as:

$$T_{GPS,k}=P_{GPS,k}-P_{i,k}$$

(31)

Define the fault detection function:

$${\lambda }_{GPS,k}={\beta }_{GPS}^T{T}_{GPS,k}^{-1}{\beta }_{GPS}^{}$$

(32)

Define the fault detection criteria:

$$\{\begin{array}{l}{\lambda }_{GPS,k}\ge {\epsilon }_{\beta },\hspace{1em}fault\\ {\lambda }_{GPS,k}<{\epsilon }_{\beta },\hspace{1em}no fault\end{array}$$

(33)

where the threshold ${\epsilon }_{\beta }$ is determined by the function of false alarm rate.

Detector ${\chi }^2$ residual error of Sub_Kalman_Filter_1 can be written as:

$$d_{GPS,k}=\Delta {\phi }_{GPS,k}-\Delta {\widehat{\phi }}_{i,k}$$

(34)

Covariance residual error:

$$S_{GPS,k}=H_{GPS,k}{P}_{GPS,k}{H}_{GPS,k}^T+R_{GPS,k}$$

(35)

Define the fault detection function:

$${\gamma }_{GPS,k}=d_{GPS,k}^T{S}_{GPS,k}^{-1}{d}_{GPS,k}^{}$$

(36)

Define the fault detection criteria:

$$\{\begin{array}{l}{\gamma }_{GPS,k}\ge {\epsilon }_d,\hspace{1em}fault\\ {\gamma }_{GPS,k}<{\epsilon }_d,\hspace{1em}no fault\end{array}$$

(37)

where the threshold ${\epsilon }_d$ is determined by the function of false alarm rate.

If the state ${\chi }^2$ detector or the residual ${\chi }^2$ detector detects a fault signal of GPS system, then $m_g=0$, otherwise $m_g=1$. Thus, we can generate a fault detection matrix, which will be used for fault signal isolation in the process of data fusion.

3. Results

3.1. Simulation Verification

To verify the feasibility and effectiveness of the proposed method, we simulated a driving environment of straight lane with two lanes, as shown in Figure 5a. The details about the system and vehicle platform setup for the simulations follows the parameters of our own developed autonomous vehicle, as shown in

Table 1. Configurations of autonomous vehicle.

Hardware	Property
Laser LIDAR: HESAI Pandar 40	Lines: 40; Range: 200 m; Angular resolution: 0.1°; Updating: 20 Hz; Accuracy: ±2 cm
Radar: Delphi ESR	Range: 100 m; Viewing field: ±10 deg; Updating: 20 Hz; Accuracy: ±5 cm, ±0.12 m/s, ±0.5°
Navigation system: NovAtel SPAN-CPT	Accuracy: ±1 cm, ±0.02 m/s, ±0.05° (Pitch/Roll), 0.1° (Azimuth); Updating: 10 Hz
Camera: SY8031	Resolution: 3264 × 2448; FPS: 15;Viewing field: 65° (vertical), 50° (horizontal)
Image processer: NVIDIA JTX 2	CPU: ARM Cortex-A57 (Quad-core, 2 GHz); GPU: Pascal TM (256 core, 1300 MHz); RAM: LPDDR4 (8 G, 1866 MHz, 58.3 GB/s)
Controller: ARK-3520P	CPU: Intel Core i5-6440EQ (Quad-core, 2.8 GHz); RAM: LPDDR4 (32 G, 2133 MHz, 100 GB/s)

. The single lane width is 4 m and the longitudinal driving speed is 15 km/h. The driving condition chosen in this work is double lane changing path control, which is a very typical and common condition for autonomous vehicle.

In Figure 5b, we can see that the curve of the trajectory is relatively smooth, the yaw angle changes within a small range, the longitudinal travel distance is about 190 m, and the initial and ending yaw angles are basically 0 rad.

Figure 6a shows the yaw angle changes detected by vision, GPS, and LIDAR sensing systems and Figure 6b shows the errors between the reference yaw angel and the yaw angles obtained by vision, GPS, and LIDAR sensing systems.

In Figure 6a, we can see that, within the time intervals of 3–5 and 8–10 s, the sensor failure occurs to the GPS sensing system. In Figure 6b, we can see that the yaw angle errors of vision and LIDAR sensing systems maintained in an extremely small range, which are cause by measurement noises. However, since the peak yaw angle error of GPS sensing system reaches 0.07 rad, considering the maximum yaw angle of vehicle in simulation is about 0.1 rad, the sensor fault of GPS channel cannot be ignored and needs to be detected and isolated.

The values of fault detection function based double state chi-square detector and residuals chi-square detector can be seen in Figure 7a,b, respectively.

Based on the results shown in Figure 7, we can see that the proposed method can easily detect the fault signal. In Figure 7a, the peak value of fault detection function $\lambda$ for GPS signal channel by using double state chi-square detector reaches $5\times {10}^{-6}$ and $4\times {10}^{-6}$ at 3 and 5 s, which is the moment when the yaw angle error of GPS signal reaches its peak. However, the peak values of fault detection function $\lambda$ for vision and LIDAR sensing system reach no more than $2\times {10}^{-7}$ and $5\times {10}^{-8}$, respectively. In Figure 7b, the value of fault detection function $\gamma$ for GPS signal channel by using residuals chi-square detector remains in the order of magnitude ${10}^{-7}$, and this value is proportional to the absolute value of the yaw angle error within the time intervals of 3–5 and 8–10 s. However, the order of magnitude of the fault detection function $\gamma$ for vision and LIDAR sensing systems is ${10}^{-8}$. Through the above analysis, we know that, by setting the appropriate threshold, the fault signals can be easily detected when sensor failure occurs.

Figure 8a shows the yaw angle changes detected by vision, GPS, and LIDAR sensing systems and Figure 8b shows the errors between the reference yaw angel and the yaw angles obtained vision, GPS, and LIDAR sensing systems.

In Figure 8, we can see that the lateral position detected by GPS sensing system has obvious fault due to the sensor failure within the time intervals of 3–5 and 8–10 s. The peak lateral position error of GPS sensing system reaches 3.5 m, which is extremely large considering the maximum lateral position of vehicle in simulation is about 4 m and width of road lane is 4 m. Therefore, the sensor fault of GPS channel cannot be ignored and needs to be detected and isolated.

The fault detection results for lateral position in simulation are shown in Figure 9.

In Figure 9, we can see that the detection results of the fault signal are obviously different compared to the normal signals. In Figure 9a, the peak value of fault detection function $\lambda$ for GPS signal channel by using double state chi-square detector reaches $2.5\times {10}^{-3}$, $1.1\times {10}^{-2}$, $1.2\times {10}^{-2}$, and $4.5\times {10}^{-3}$ at 3, 6, 8, and 10 s, which correspond to the moments when the fault signal of GPS sensing system appears and disappears. However, the peak values of fault detection function $\lambda$ for vision and LIDAR sensing system reach no more than $2\times {10}^{-4}$ and $5\times {10}^{-4}$, respectively. In Figure 9b, the value of fault detection function $\gamma$ for GPS signal channel by using residuals chi-square detector remains in the order of magnitude of ${10}^{-3}$ when sensor failure occurs, and this value is proportional to the absolute value of the lateral position error within the time intervals of 3–5 and 8–10 s. However, the order of magnitude of the fault detection functions $\gamma$ for vision and LIDAR sensing systems are ${10}^{-6}$. Hence, the fault lateral position signal can be easily detected in simulation process by setting appropriate thresholds for the fault detection functions.

To confirm the effect of sensor fault to the path tracking control system of autonomous vehicle, the simulation results are compared by using MPC, adaptive MPC (AMPC), fault-tolerant MPC (FTMPC), and fault-tolerant Adaptive MPC FTAMPC algorithms, as shown in Figure 10.

In Figure 10a,b, we can see that the curves of lateral position and lateral position error are divergent, which means that the vehicle is gradually moving away from the reference path, and the accurately path tracking of the autonomous vehicle cannot be realized using the fault sensor information. In Figure 10c,d, we can see that the curves of yaw angle and yaw angle error are irregular and the changes are very dramatic, which also means that the autonomous vehicle cannot track the reference path, and the risk of overturning exists without fault detection and isolation. In addition, because the longitudinal velocity is a constant, FTMPC and FTAMPC reflect almost the same path tracking control performance.

3.2. Experimental Verification

We also conducted a path tracking control experiment using self-developed autonomous vehicle. The details about the sensor system and autonomous vehicle platform setup are shown in Table 1.

The driving condition is double lane changing. Figure 11a shows the autonomous vehicle and the equipped sensing system, including the vision, GPS, and LIDAR sensing systems. The width of lane is about 4 m. Figure 11b shows the vehicle trajectory of double lane changing obtained by GPS sensing system with manual driving mode. We use the record yaw angle and lateral position as reference in the following autonomous path tracking process. As shown in Figure 11b, the trajectory of double lane changing is not perfect; the final yaw angle is not 0 rad and the longitudinal travel distance is about 70 m.

Figure 12a shows the yaw angle changes detected by vision, GPS, and LIDAR sensing systems in the autonomous path tracking process and Figure 12b shows the errors between the reference yaw angel and the measured yaw angles.

In Figure 12a, we can see that the total travel time is about 23 s, and, within the time intervals of 5–10 and 15–20 s, the sensor failure occurs to the GPS sensing system. In Figure 12b, we can see that the yaw angle errors of vision and LIDAR sensing systems are maintained in an extremely small range and the peak of yaw angle error is less than 0.03 rad, which may cause by measurement noises or too few feature points for LIDAR and vision algorithms. However, the peak value of yaw angle error of GPS sensing system reaches 0.22 rad at 7 s; considering the maximum yaw angle of vehicle in the experiment is about 0.3 rad, the sensor fault of GPS channel cannot be ignored and needs to be detected and isolated.

The values of fault detection function for yaw angle in our experiment based on double state chi-square detector and residuals chi-square detector are shown in Figure 13a,b, respectively. Based on the results shown in Figure 13, we can see that the fault signal can be exactly detected by using the proposed method. In Figure 13a, the peak values of fault detection function $\lambda$ for GPS signal channel by using double state chi-square detector reaches $2\times {10}^{-6}$, $2\times {10}^{-6}$, $2\times {10}^{-5}$, and $1\times {10}^{-6}$ at 5, 10, 15, and 20 s, which are the moments when the yaw angle error of GPS signal reaches its peak. However, the peak values of fault detection function $\lambda$ for vision and LIDAR sensing system reach no more than $1\times {10}^{-8}$ and $2\times {10}^{-8}$, respectively. In Figure 13b, the value of fault detection function $\gamma$ for GPS signal channel by using residuals chi-square detector remains in the order of magnitude of ${10}^{-6}$, and this value is proportional to the absolute value of the yaw angle error within the time intervals of 5–10 and 15–20 s. However, the order of magnitude of the fault detection function $\gamma$ for vision and LIDAR sensing systems remain at ${10}^{-7}$. Through the above analysis, we obtained similar results as in the simulation process, which means that the fault signals can be detected by our proposed method with setting the appropriate threshold.

Figure 14a shows the lateral position changes detected by vision, GPS, and LIDAR sensing systems and Figure 14b shows the errors between the reference lateral position and the lateral position obtained by vision, GPS, and LIDAR sensing systems.

In Figure 14a, we can see that due to the sensor failure within the time intervals of 5–10 s and 15–50 s the lateral position detected by GPS sensing system is fault signal. In Figure 14b, we can see that the peak lateral position error of GPS sensing system reaches almost 2.5 m, which is extremely large considering the maximum lateral position of vehicle in simulation is about 4 m and width of road lane is 4 m. Therefore, the sensor fault of GPS channel cannot be ignored and needs to be detected and isolated in the experiment.

Figure 15 shows the curves of fault detection functions for lateral position in experiment based on chi-square detector. In Figure 15a, we can see that the value of fault detection functions $\lambda$ for vision and LIDAR sensing systems by using double state chi-square detector remain in the ranges of $0~5\times {10}^{-7}$ and $0~5\times {10}^{-9}$, respectively. However, the peak value of the fault detection functions $\lambda$ for GPS signal channel reaches $2\times {10}^{-4}$, $3.8\times {10}^{-3}$, $3.7\times {10}^{-3}$, and $5\times {10}^{-4}$ at 5, 10, 15, and 20 s, which correspond with the moments when the fault GPS signal occurs and disappears. In Figure 15b, we can see that there are peaks on the curves of fault detection functions $\gamma$ for vision and LIDAR signal channels by using residuals chi-square detector, which correspond with the lateral position error curves. It is worth noting that the order of magnitude of fault detection functions $\gamma$ for vision and LIDAR signal channels are ${10}^{-6}$. However, the order of magnitude of fault detection functions $\gamma$ for GPS signal channels are ${10}^{-4}$ within the time intervals of 5–10 and 15–20 s, which means we can easily detect the fault lateral position signals by setting the appropriate threshold.

From the above simulation results, we know that directly sending the fault signal to the path tracking controller may seriously affect the driving safety of the autonomous vehicle; therefore, in the experiment, we only compared the performance of FTMPC and FTAMPC, as shown in Figure 16.

In Figure 16a,b, we can see that the path tracking performances of FTMPC and FTAMPC are obviously different, although both algorithms can control the autonomous vehicle to move along the reference path. During the experiment process, the longitudinal velocity of autonomous vehicle is not a constant. The autonomous vehicle accelerates first and then decelerates; under this circumstance, FTAMPC shows better performance. Figure 16c,d shows that, compared with the tracking results of the lateral position, the tracking performance of yaw angle is relatively poor. This is mainly due to the short longitudinal travel distance and large changes of longitudinal velocity and yaw angle, which make it difficult to accurately yaw angle tracking control.

4. Conclusions

In this paper, we focus on a fault tolerant adaptive model predictive control algorithm of autonomous vehicles for robust lateral path tracking control when sensor failures occur. A robust fault tolerant path tracking control algorithm is proposed combining the adaptive model predictive control algorithm for lateral path tracking control, improved weight assignment method for multi-sensor data fusion and fault isolation, and novel federal Kalman filtering approach with two states chi-square detector and residual chi-square detector for detection and identification sensor fault in autonomous vehicles. Simulation and experimental results show that the proposed algorithm can efficiently detect the fault signal. After the fault signal isolation, the reference path can be effectively tracked with the proposed approach in our work. In the further studies, we will explore the possibility of combining the proposed method with reinforcement learning to optimize the fault detection and isolation performance.