Fault Detection of Multi Wheeled Robot Consensus Based on EKF

1. Multi-Wheeled Robot Consensus

1.1. Graph Theory

1.1.1. Notation

We denote the set of real numbers as $\mathbb{R}$. The notation ${\mathbb{R}}^n$ represents the real vector space with n dimensions, while ${\mathbb{R}}^{n\times n}$ denotes the set of $n\times n$ matrices. A diagonal matrix of size $n\times n$, with diagonal elements $q_1,q_2,...,q_n$, is denoted as $diag(q_1,q_2,...,q_n)$. The identity matrix is represented by $I\in {\mathbb{R}}^{n\times n}$. The Kronecker product of matrices is denoted by ⊗. Additionally, we utilize $x={(x_1,...,x_n)}^T$ to signify the vector in ${\mathbb{R}}^n$. T in exponential means the transposition.

1.1.2. Graph Theory

Understanding fundamental concepts of graph theory is indispensable for exploring the dynamics of multi-agent systems [15]. Consider a multi-agent system comprising n mobile robots, interconnected through a communication network. The interaction model among agents can be represented by delineating the communication topology in the form of a graph

Consider ${\mathcal{G}}_t=(\Lambda ,{\rm Y})$ an oriented graph, where $\Lambda =[{\Lambda }_1,{\Lambda }_2,...,{\Lambda }_n]$ represents the set of nodes (vertices), Each node i signifie the $i^{th}$ agent, and

$${\rm Y}=(i,j)\in \Lambda \times \Lambda ,j\in N_i$$

represents the collection of edges (arrows) linking the nodes, where $N_i$ denotes the set of node indexes connected to the ith node. In the notation $(i;j)$, it signifies a connection between nodes i and j, with j being in $N_i$, although i may not be in $N_j$. Therefore, $(i;j)$ and $(j;i)$ do not necessarily denote the same edge. These node connections are consolidated into a matrix $M_d=\left[a_{ij}\right]$, known as the adjacency matrix or connection matrix, with dimensions $N*N$ . The elements within the matrix indicate whether pairs of vertices are adjacent in the graph or not.

$$a_{k}j=\left\{\begin{array}{cc}1\hfill & \mathrm{i}\mathrm{f} \mathrm{k} \mathrm{i}\mathrm{s} \mathrm{connect} \mathrm{to} \mathrm{j}\hfill \\ 0\hfill & \mathrm{i}\mathrm{f} \mathrm{n}\mathrm{o}\mathrm{t}.\hfill \end{array}\right.$$

The degree matrix Q is a diagonal matrix of size $N\times N$, providing information regarding the number of edges connected to each node. With

$$Q_{ij}=\left\{\begin{array}{cc}degree\left({\Delta }_i\right)\hfill & \mathrm{i}\mathrm{f} i=j\hfill \\ 0\hfill & \mathrm{i}\mathrm{f} \mathrm{n}\mathrm{o}\mathrm{t}.\hfill \end{array}\right.$$

We define the Laplacian matrix $H=\left[l_{ij}\right]$ by:

$$H=Q-M$$

The components within this matrix are as follows::

$$h_{kj}=\left\{\begin{array}{cc}-a_{kj}\hfill & \mathrm{i}\mathrm{f} k\ne j\hfill \\ \sum _{j=1}^N{a}_{kj}\hfill & \mathrm{i}\mathrm{f} k=j.\hfill \end{array}\right.$$

1.1.3. Closed Loop System

Consider a collection of $N=3$ agents of significant scale, wherein the conduct of each agent is delineated by a non-linear controlled model.

The Figure 1 shows the master-slave configuration considered in this paper so the matrices are as follow.

$A=\left[\begin{array}{ccc}0& 0& 0\\ 1& 0& 0\\ 0& 1& 0\end{array}\right]$ , $D=\left[\begin{array}{ccc}0& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$ and $L=\left[\begin{array}{ccc}0& 0& 0\\ 1& -1& 0\\ 0& 1& -1\end{array}\right]$

Remember also the writing of the protocol:

$$\begin{array}{c}\hfill u_i=-P\left(X_i\right)\left(\sum _{j=1}^N{a}_{ij}(X_i-X_j)\right)\end{array}$$

(1)

which $P\left(X_i\right)\in {\Re }^{n\times m}$, $a_{ij}$ are the adjacent elements linked to $\mathcal{G}$

1.2. Wheeled Mobile Robot Modeling

An unicycle is a robot powered by two independent wheels. It is placed on a plane surface where $(0,\overrightarrow{x},\overrightarrow{y})$ an inertial reference. The kinematic model illustrated in Figure 2 and the continuous state-space representation is given as follows:

$$\left\{\begin{array}{c}\dot{x}\left(t\right)=vcos\left(\theta \left(t\right)\right)\hfill \\ \dot{y}\left(t\right)=vsin\left(\theta \left(t\right)\right)\hfill \\ \dot{\theta }\left(t\right)=\omega \left(t\right)\hfill \\ \dot{v}\left(t\right)=a\left(t\right)\hfill \end{array}\right.$$

(2)

Where x and y are the position along the x-axes and the y-axes respectively, $v\in \Re$ denotes the linear speed associated with the center of mass, $\theta$ is the orientation angle, $\omega$ is the angular speed of the center of mass , a is the acceleration and u represents the input vector $u={\left[\begin{array}{cc}\omega & a\end{array}\right]}^T$. Then, the generalised robot model is given by:

$$\left\{\begin{array}{cc}\hfill \dot{X}\left(t\right)& =f(X,u,t)\hfill \\ \hfill Y\left(t\right)& =CX\left(t\right)\hfill \end{array}\right.$$

(3)

With $X={\left[\begin{array}{cccc}x& y& \theta & v\end{array}\right]}^T$ and The observation matrix $C=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right]$ it provides the system observability condition.

We propose to calculate a regulator which makes it possible to describe an equation of a sinusoidal trajectory.

$$\begin{array}{cc}\hfill x_d\left(t\right)& =t\hfill \\ \hfill y_d\left(t\right)& =sin\left(t\right)\hfill \end{array}$$

(4)

For this, we use a method by linearizing looping. The command will therefore be in the end as follow:

$$\begin{array}{ccc}\hfill \left(\begin{array}{c}{u}_1\\ u_2\end{array}\right)=& {\left(\begin{array}{cc}-vsin\left(\theta \right)& cos\left(\theta \right)\\ vcos\left(\theta \right)& sin\left(\theta \right)\end{array}\right)}^{-1}\hfill & \hfill \left(\begin{array}{c}(x_d-x)+2(\dot{x}-vcos\left(\theta \right))+\ddot{x_d}\\ (y_d-y)+2(\dot{y}-vsin\left(\theta \right))+\ddot{y_d}\end{array}\right)\end{array}$$

(5)

If we define the error vector,

$$e=(e_x,e_y)$$

the dynamics of the error is written as follow:

$$\left(\begin{array}{c}{e}_x+2\dot{e_x}+\ddot{e_x}\\ e_y+2\dot{e_y}+\ddot{e_y}\end{array}\right)=\left(\begin{array}{c}0\\ 0\end{array}\right)$$

which is stable and converges rapidly to 0. According to Euler’s approximation, we will discretize the state model $\left(1\right)$ by a sampling time $T_e$ as follows:

$$\left\{\begin{array}{c}{x}_{k+1}=x_k+T_e{v}_{k}cos\left({\theta }_k\right)\hfill \\ y_{k+1}=y_k+T_e{v}_{k}sin\left({\theta }_k\right)\hfill \\ {\theta }_{k+1}={\theta }_k+T_e{\omega }_k\hfill \\ v_{k+1}=v_k+T_e{a}_k\hfill \end{array}\right.$$

(6)

1.3. Simulations

NOTATION:

m : The robot-master

$s1$: The first robot-slave

$s1$: The second robot-slave

$X_d={\left[\begin{array}{cccc}{x}_d& y_d& {\theta }_d& v_d\end{array}\right]}^T$: State vector of the desired trajectory

$X_m={\left[\begin{array}{cccc}{x}_m& y_m& {\theta }_m& v_m\end{array}\right]}^T$: State vector of the master robot

$X_{s1}={\left[\begin{array}{cccc}{x}_{s1}& y_{s2}& {\theta }_{s1}& v_{s1}\end{array}\right]}^T$: State vector of the first slave robot

$X_{s2}={\left[\begin{array}{cccc}{x}_{s2}& y_{s2}& {\theta }_{s2}& v_{s2}\end{array}\right]}^T$: State vector of the second slave robot

$u_m={\left[\begin{array}{cc}{u}_{ml}& u_{mr}\end{array}\right]}^T$: The left and right controller of the robot-master

$u_{s1}={\left[\begin{array}{cc}{u}_{s1l}& u_{s1r}\end{array}\right]}^T$: The left and right controller of the robot-slave

$u_{s2}={\left[\begin{array}{cc}{u}_{s2l}& u_{s2r}\end{array}\right]}^T$: The left and right controller of the robot-slave

Consider the desired trajectory in Figure 9.

Figure 3. The desired trajectory.

Figure 3. The desired trajectory.

We observe that the robot-master accurately follows this trajectory, which is also replicated by the two robot slaves, as depicted in Figure 4, Figure 5 and Figure 6.

Figure 4. The robot-master’s trajectory

Figure 4. The robot-master’s trajectory

Figure 5. The first robot-slave’s trajectory

Figure 5. The first robot-slave’s trajectory

Figure 6. The second robot-slave’s trajectory

Figure 6. The second robot-slave’s trajectory

Figure 7. The coordinate x of the desired trajectory, the master and the slaves

Figure 7. The coordinate x of the desired trajectory, the master and the slaves

Figure 8. The coordinate y of the desired trajectory, the master and the slaves

Figure 8. The coordinate y of the desired trajectory, the master and the slaves

Figure 9. The control of the robot-master

Figure 9. The control of the robot-master

Figure 10. The control of the first robot-slave

Figure 10. The control of the first robot-slave

Figure 11. The control of the second robot-slave

Figure 11. The control of the second robot-slave

2. Fault Detection Based on the Extended Kalman Filter

2.1. Extended Kalman Filter and Fault Diagnosis

2.1.1. the Extended Kalman Filter (EKF)

Within the extended Kalman filter framework, the nonlinearities inherent in the system dynamics are approximated using a linearized rendition of the nonlinear model based on the most recent state estimate. Let’s consider the system characterized by the following state equations:

$$\left\{\begin{array}{c}X(k+1)=f(X,u,k)+\rho \left(k\right)\hfill \\ Y(k+1)=h(X,u,k)+\gamma \left(k\right)\hfill \end{array}\right.$$

(7)

$\rho \left(k\right)$ and $\gamma \left(k\right)$ denote the process and observation noises, assumed to have zero mean with covariances ${\Psi }_{{\rho }_k}$ and ${\Psi }_{{\gamma }_k}$ respectively. Function f, defined as non-linear according to Equation $\left(7\right)$, is employed to compute the predicted state from the previous estimate, as is function h for predicting the measurement from the predicted state. However, due to the nonlinearity of f and h, a Jacobian matrix of partial derivatives is computed since these functions cannot be directly applied to the covariance.

Hypothesis:

As stated in [16], when the Extended Kalman Filter (EKF) is applied to a system and placed in a canonical observation form, it gains properties associated with global convergence.

• The pair of the matrices $(A,C)$ is detectable, that means there is no unstable mode and unobservable in the system
• the signals $\rho \left(k\right)$ and $\gamma \left(k\right)$ are central Gaussian white noise of Density Power Spectral (DSP) covariance ${\Psi }_{{\rho }_k}$ and ${\Psi }_{{\gamma }_k}$ respectively means

$$\left\{\begin{array}{cc}\mathbb{E}\left[\gamma \left(i\right)\gamma {\left(j\right)}^T\right]={\Psi }_{{\rho }_k},\hfill & \mathrm{if}i=j\hfill \\ \mathbb{E}\left[\rho \left(i\right)\rho {\left(i\right)}^T\right]={\Psi }_{{\gamma }_k},\hfill & \mathrm{if}i=j\hfill \\ \mathbb{E}\left[\gamma \left(i\right)\rho {\left(j\right)}^T\right]=0,\hfill & \forall i,j.\hfill \end{array}\right.$$

(8)

Where $\mathbb{E}[..]$ represents the mathematical expectation. The latter equation illustrates the stochastic independence of the noises $\rho$ and $\gamma$. This assumption is introduced to simplify the subsequent calculations but is not obligatory.

Correction:

• Updated state estimate :

  $${\widehat{x}}_{k/k}={\widehat{x}}_{k/k-1}+{\mathbb{K}}_k{\widehat{y}}_k$$

  (9)
• Updated covariance estimate :

  $${\Psi }_{k/k}=(I-{\mathbb{K}}_k{C}_k){\Psi }_{k/k-1}$$

  (10)
• measurement residual :

  $${\tilde{y}}_k=y_k-C_k{\widehat{x}}_{k/k-1}$$

  (11)
• Innovation covariance:

  $${\mathbb{Z}}_k=C_k{\Psi }_{k/k-1}{C_k}^T+{\Psi }_{{\gamma }_k}$$

  (12)
• Near-optimal Kalman gain:

  $${\mathbb{K}}_k={\Psi }_{k/k-1}{C_k}^T{{\mathbb{Z}}_k}^{-1}$$

  (13)

Prediction:

• Predicted state estimate :

  $${\widehat{x}}_{k+1/k}=A_k{\widehat{x}}_{k/k}+u_k$$

  (14)
• Predicted covariance :

  $${\Psi }_{k+1/k}=A_k{\Psi }_{k/k}{A_k}^T+{\Psi }_{{\rho }_k}$$

  (15)

where $A_k$ is the linearized matrix of the function f which is written as follows :

$$A_k=\frac{\delta f}{\delta X}{|}_{{\widehat{x}}_{k/k}}$$

Remark: Occasionally, numerical issues can cause the covariance of innovation $Z_k$ to lose its positive definite nature. In such instances, it is advisable to substitute the equation for covariance estimation with:

$${\Psi }_{k/k}=\sqrt{(I-K_k{C}_k){\Psi }_{k/k-1}{\Psi }_{k/k-1}^T{(I-K_k{C}_k)}^T}$$

The replacement will always ensure a positive definite outcome, even if the matrix ${\Psi }_{k/k-1}$ is not. Consequently, the Kalman equations will exhibit greater stability, as any minor deviations in the positive definite nature of the covariance matrices will be corrected in the subsequent iteration. According to $\left(14\right)$ :

$A_k=$$\left[\begin{array}{cccc}1& 0& -T_{e}vsin\left(\theta \right)& cos\left(\theta \right)\\ 0& 1& T_{e}vcos\left(\theta \right)& sin\left(\theta \right)\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$ Suppose the signals ${\gamma }_k$ and ${\rho }_k$ are Gaussian white with a unit covariance matrix, that is to say:

${\Psi }_{\gamma }=$$\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$${\Psi }_{\rho }=$$\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$ also we take the sample time $Te=0.01$.

2.1.2. Fault Diagnosis Steps

Technically, a fault diagnosis system usually goes through three basic steps: a residual generation, a residue evaluation module, and a decision logic [17].

Step 1: Residual generation

A standard residual generator enables the construction of a fault indicator vector or residuals vector, denoted as r, based on the measured input and output quantities of the system. Under normal conditions without faults in the monitored system, the residual’s nominal value theoretically reaches zero once the transient effects from initial conditions have dissipated. However, it deviates notably from zero in the presence of specific faults.

Step 2:Residue evaluation module

The residue evaluation module constitutes the detection phase, where suitable methods are employed to determine whether the system is operating normally or not, i.e., if the given residue deviates from zero [17]. Detecting a fault typically involves comparing the residues to a predetermined detection threshold $t_{th}$. This threshold represents a crucial aspect of residual-based FDI methods, defined as the boundary value for the deviation of a residue from zero. Thus, the process of fault detection proceeds as follows:

$$r=\left\{\begin{array}{cc}1\hfill & if,r>|t_{th}|\hfill \\ 0\hfill & if,r<|t_{th}|.\hfill \end{array}\right.$$

Excessively high thresholds may result in failing to detect a fault (missed alarm), while overly low thresholds may lead to false alarms, detecting faults in healthy conditions [18]. There exist two categories of thresholds [8]: the first being a constant threshold, and the second an adaptive one. The adaptive threshold is employed to accommodate inevitable parameter uncertainty, disturbances, and noise encountered in practical applications.

Step 3 :decision

This step involves examining the outcome of evaluating a set of residues and, based on the pattern of activated and non-activated tests, it generates a determination regarding the faulty component within the monitored system.

2.2. Closed Loop System

The closed loop system is given in Simulink/Matlab by the functional diagram in Figure 12.

The robot is controlled in order to follow a desired trajectory given by $P_d={[x_d,y_d]}^T$ as shown in Figure 13.

2.3. Simulation Results

First of all, we notice that the robot master follows the desired trajectory , as shown in Figure 14.

The fault scenario addressed involves an offset applied to the x measurement at the 15-second mark. Subsequently, we apply our Extended Kalman Filter (EKF), and the resulting robot trajectory is illustrated in Figure 15.

It is observed that there is a slight deviation attributable to the applied fault, yet the robot continues to track the same trajectory consistently.

Figure 15. Trajectory of the mobile robot when fault applied at t=15s

Figure 15. Trajectory of the mobile robot when fault applied at t=15s

Figure 16. Coordinates x and y of the mobile robot when fault applied at t=15s

Figure 16. Coordinates x and y of the mobile robot when fault applied at t=15s

Furthermore, upon introducing a fault (considered as an intermittent fault, such as a switch) to a parameter at time $t=15s$, a minor peak is observed, after which the robot promptly returns to its intended trajectory. Hence, it can be concluded that the Extended Kalman Filter effectively identifies the fault introduced into the system.

3. Fault Detection in Multi-Robot System

In the preceding section, the system (consisting of a robot-master and slaves) executes its trajectory within an ideal environment. However, real-world environments lack such ideal conditions. Thus, we aim to introduce a fault and observe how the system responds to the challenge. It’s observed that the robot-master adheres to its trajectory, as depicted in Figure 3. Subsequently, we induce an offset fault in the master robot’s x measurement at 15 seconds, as illustrated in Figure 18.

Figure 17. Multi-robot system closed loop

Figure 17. Multi-robot system closed loop

Figure 18. The fault applied in time t=15s

Figure 18. The fault applied in time t=15s

The responses of both the master and the slaves are depicted in the subsequent figures (Figure 19, Figure 20 and Figure 21). It is observed that the trajectory of the robots experiences a slight disturbance at a specific time, yet it promptly returns to its intended path without deviating from the desired trajectory.

The Figure 22 and Figure 23 show the x and y measurement of the desired trajectory, the master and the slaves respectively.

Furthermore, the simulations of the controllers for both the master and the slaves are illustrated in Figure 24, Figure 25 and Figure 26 respectively. It is observed that the robot-master adheres to the desired trajectory until time $t=15s$, at which point it deviates slightly. However, after a few seconds, it resumes following the trajectory initially set.

Meanwhile, our attention is directed towards the behavior of the robot-slaves, which entails two cases for investigation: firstly, when the robot-slave operates without fault, and secondly, when the robot-slave is subjected to a fault injection. Simulations depicted in Figure 20 and Figure 21 reveal the impact on the functionality of the slave robot. Notably, a minor peak is observed in the slave responses (refer to Figure 22 and Figure 23), following which it swiftly resumes its trajectory.

3.1. Fault Diagnosis

To utilize the Extended Kalman Filter (EKF) for fault detection in a wheeled mobile robot, the following procedural steps are advised:

1- System Modeling: Develop a comprehensive model that characterizes the robot’s behavior under both normal operating conditions and various fault scenarios.

2- State Estimation: Implement the EKF to estimate the current state of the robot utilizing sensor measurements.

3- Fault Models: Define specific fault models detailing how each fault impacts the robot’s state and sensor measurements.

4- Residual Calculation: Calculate residuals by computing the disparity between actual measurements and estimated measurements, based on the fault models delineated in the preceding step.

5- Fault Detection: Employ statistical tests or threshold-based techniques on the residuals to identify the occurrence of faults.

6- Fault Diagnosis: Initiate the fault diagnosis phase subsequent to the detection of faults.

For establishing thresholds, the system should first undergo simulation under fault-free conditions, followed by simulations with faults. The fault-free simulations aid in determining thresholds through the application of the three-sigma method.

$$\left\{\begin{array}{c}-r=-3\sigma \hfill \\ +r=+3\sigma \hfill \end{array}\right.$$

where $\sigma$ denotes the standard deviation of residual r. As mentioned in the previous section, the fault is said detected when the residual exceeds the threshold more than $|t_{th}|$.

Based on EKF, so in our case we are going to calculate the difference between the values of the robot’s state vector and the values of the estimated state vector $X_e$. So four residuals are generated for each robot.

$$\left\{\begin{array}{c}{r}_x=x-x_e\hfill \\ r_y=y-y_e\hfill \\ r_{\theta }=\theta -{\theta }_e\hfill \\ r_v=v-v_e\hfill \end{array}\right.$$

where the indexe e means the estimated value. Each residual is compared to the threshold.

Table 1 summarises the signatures of the residuals under fault applied in the robot-master and the slaves robots respectively.The different residuals signatures allow the fault isolation. We notice that when the fault appears in the robot master the other two robots (slaves ) are affected. In other side, if we insert the fault in the robot slaves the master is not affected so we conclude that fault affects only the following robot so we can say that the fault propagates in one direction only.As shown in figures (Figure 27, Figure 28 and Figure 29) fault appears at time $t=15s$ and is detected and the detection delay does not effect on the system performance.

4. Conclusion

The multi-robots system, such as master-slave robots, helps to perform several tasks at the same time which improves the performance of the process. But in case of failure, the diagnostic steps are very important to predict and then solve the problem. So that an extended kalman filter is adopted in order to produce the signatures of the residuals between the multi-robot system and the EKF to detect fault which appear during functioning of the system. The previous simulations show the effectiveness of the proposed methods and how the EKF has managed to achieve the objectives that we set from the beginning.