1. INTRODUCTION
Quadrotors have been widely used in monitoring and operation tasks, such as aerial photography, traffic control, mineral exploitation, power line inspection, pesticide spraying and so on (Gupte et al., Reference Gupte, Mohandas and Conrad2012), thanks to their simplicity of operation and hovering and Vertical Take-Off and Landing (VTOL) capability.
Velocity is important information for quadrotors. Global Positioing System (GPS)/inertial sensor fusion is often used to measure velocity for quadrotors and shows good performance. When quadrotors fly in urban areas or indoor, the GPS signal may be weakened or even lost (Leishman et al., Reference Leishman, McLain and Beard2014a). In these cases, alternative methods are needed.
An optical flow sensor can measure the relative motion between an observer (such as a camera) and the scene (such as the ground). It can provide velocity information for quadrotors. Therefore, the Optical flow/Inertial sensor Fusion (OIF) is a popular method, which has been widely used in the case of no GPS signal (Bristeau et al., Reference Bristeau, Callou, Vissiere and Petit2011). Since the optical flow method is based on the assumption of intensity constancy and gradient constancy (Baker et al., Reference Baker, Scharstein, Lewis, Roth, Black and Szeliski2011), its accuracy is affected by environmental factors and large flight movement which lead to large noises or errors in the optical flow measurements (Sun et al., Reference Sun, Roth and Black2014; Brox and Malik, Reference Brox and Malik2011). With the consideration of the deficiency of the current OIF, a better navigation method is required.
The Model-Aided Inertial sensor Estimator (MA-IE) is a navigation method recently developed for quadrotors (Abeywardena et al., Reference Abeywardena, Kodagoda, Dissanayake and Munasinghe2013a). A quadrotor is an under-actuated system and its attitude is relevant to the translation movement. According to the quadrotor model characteristics, its drag force is proportional to its velocity (Bristeau et al., Reference Bristeau, Martin, Salaun and Petit2009; Martin and Salaun, Reference Martin and Salaun2010). With the help of such characteristics, the MA-IE estimates the velocity directly using the accelerometer outputs instead of integrating them. The MA-IE can, therefore, provide accurate velocity estimation whose error does not increase with time (Crocoll et al., Reference Crocoll, Seibold, Scholz and Trommer2014). Leishman et al. (Reference Leishman, Macdonald, Beard and McLain2014b) designed an MA-IE and showed that better attitude and velocity accuracy can be obtained when the model information is used. Macdonald et al. (Reference Macdonald, Leishman, Beard and McLain2014) showed that the MA-IE can maintain the same accuracy by reducing the position update periodicity to a half, when position measurements are available.
However, the accuracy of the MA-IE significantly depends on the model accuracy. Wind tunnel experiments have illustrated that the drag force of quadrotors is greatly relevant to the airspeed (Kaya and Kutay, Reference Kaya and Kutay2015; Theys et al., Reference Theys, Dimitriadis, Andrianne, Hendrick and Schutter2014). However, there is a no wind assumption for the current MA-IEs (Hanley, Reference Hanley2015). Therefore, although several MA-IEs have been proved to work well under no wind environments, the velocity accuracy will decrease in a windy environment (Baranek and Solc, Reference Baranek and Solc2014). Most wind velocity estimation methods are based on the airspeed sensor (Langelaan et al., Reference Langelaan, Alley and Neidhoefer2011). However, the performances of the methods decrease when used for quadrotors, since the propeller airflow disturbance and low speed flight degrade the accuracy of the airspeed sensor (Arain and Kendoul, Reference Arain and Kendoul2014). When considering the influence of winds, Abeywardena et al. (Reference Abeywardena, Wang, Kodagoda and Dissanayake2013b; Reference Abeywardena, Wang, Dissanayake, Waslander and Kodagoda2014) proposed a Model-Aided Visual/ Inertial sensor Fusion (MA-VIF) scheme. The attitude and position measurements from the Visual Simultaneous Localisation And Mapping (VSLAM) method are fused with the traditional MA-IE. It was proved that the MA-VIF can estimate the wind velocity and provide good Unmanned Aerial Vehicle (UAV) velocity estimates in a windy environment.
In this paper, a novel Model-Aided Optical flow and Inertial sensor Fusion (MA-OIF) is proposed. In the proposed algorithm, three velocity estimates are combined, which are from the optical flow sensor, the inertial sensor and the quadrotor model, respectively. The three estimates will construct a voting strategy to complement one another for improved treatment in various fault cases presented in a single resource. For example, the MA-OIF can either detect the optical flow sensor fault as an improvement over OIF, or cope with the windy environments as an improvement over the MA-IE method.
The rest of the paper is organised as follows. In Section 2, we provide an overview of the quadrotor model and analyse how to estimate the velocity using the model information. In Section 3, the MA-OIF is developed and the observability analysis is done to show that the MA-OIF can work well in a windy environment. A velocity fault-tolerant algorithm based on the MA-OIF is designed in Section 4. The simulation verification is given in Section 5.
2. QUADROTOR DYNAMICS MODEL AND FORCES ESTIMATION
In order to analyse the model-aided navigation scheme for a quadrotor, first the quadrotor model needs to be studied. The quadrotor kinetic equations and the aerodynamic forces are the basis for establishing the filter.
In this paper, the coordinate systems are defined as follows: the inertial coordinate system is the reference coordinate for the inertial components and denoted by i; the earth frame is fixed to the earth and denoted by e; the navigation coordinate system is chosen as the local East-North-Up (ENU) coordinate and denoted by n; the body coordinate system is defined as a right-front-up frame and denoted by b.
Using Newton's second law, the quadrotor dynamics can be expressed as
$$ \dot{\bf V}_{nb}^b = {\bf V}_{nb}^b \times \omega _{nb}^b + {\bf C}_n^b {\bf g}_n + {\bf F}_b/m$$
where
${\bf V}_{nb}^b $
stands for the velocity of the b frame with respect to the n frame resolved in the b frame,
$\omega _{nb}^b $
is the angular rate of the b frame with respect to the n frame resolved in the b frame,
${\bf C}_n^b $
is the coordinate transformation matrix from the n frame to the b frame,
${\bf g}_n$
is the gravitational acceleration vector resolved in the n frame and equals to
$[\matrix{ 0 & 0 & { - g} \cr} ]$
, g is the gravitational acceleration,
${\bf F}_b$
stands for all forces except for gravity resolved in the b frame, m is the mass of the quadrotor.
${\bf C}_n^b $
can be expressed using roll (denoted as φ), pitch (denoted as θ) and yaw (denoted as ψ):
$${\bf C}_n^b = \left[ {\matrix{ {\cos \phi \cos \psi + \sin \phi \sin \psi \sin \theta} & { - \cos \phi \sin \psi + \sin \phi \cos \psi \sin \theta} & { - \sin \phi \cos \theta} \cr {\sin \psi \cos \theta} & {\cos \psi \cos \theta} & {\sin \theta} \cr {\sin \phi \cos \psi - \cos \phi \sin \psi \sin \theta} & { - \sin \phi \sin \psi - \cos \phi \cos \psi \sin \theta} & {\cos \phi \cos \theta} \cr}} \right]$$
The attitude differential equation can be described as
$$\left[ {\matrix{ {\dot \phi} \cr {\dot \theta} \cr {\dot \psi} \cr}} \right] = \left[ {\matrix{ {\sin \phi \tan \theta} & 1 & { - \cos \phi \tan \theta} \cr {\cos \phi} & 0 & {\sin \phi} \cr {\sin \phi /\cos \theta} & 0 & { - \cos \phi /\cos \theta} \cr}} \right]\left[ {\matrix{ {\omega _{bnx}^b} \cr {\omega _{bny}^b} \cr {\omega _{bnz}^b} \cr}} \right]$$
where
$\omega _{bnx}^b $
,
$\omega _{bny}^b $
and
$\omega _{bnz}^b $
are the x-axis, y-axis, and z-axis components of
$\omega _{bn}^b $
, which can be calculated from the gyros’ outputs (Lv et al., Reference Lv, Lai, Liu and Nie2014).
A good introduction of quadrotor aerodynamic forces is given by Artinez (Reference Artinez2007) and Mahony et al. (Reference Mahony, Kumar and Corke2012). It can be seen that the thrust force
${\bf T}$
and the drag force
${\bf H}$
are two main forces of a quadrotor. The thrust is a vertical force acting on all blades. Its components resolved in the b frame is given by
$${\bf T}_b = \left[\matrix{ 0 & 0 & {C_T\omega ^2} \cr} \right] $$
where C T is called the thrust coefficient; it is relevant to the quadrotor's aerodynamic parameters, airspeed, angle of attack and angle of sideslip (Hoffmann et al., Reference Hoffmann, Huang, Waslander and Tomlin2007; Powers et al., Reference Powers, Mellinger, Kushleyev, Kothmann and Kumar2013). Equation (4) means that the thrust only exits in the z-axis of the b frame and is proportional to the square of the propellers’ speed.
The drag force is a horizontal force acting on all blades. It is the resultant of the induced drag, the translational drag, the profile drag, the parasitic drag and the drag introduced by blade flapping (Bangura and Mahony, Reference Bangura and Mahony2012). Its components in the b frame can be approximately expressed as
$${\bf D}_b = \left[\matrix{ {k(V_{nbx}^b - V_{wx}^b )} & {k(V_{nby}^b - V_{wy}^b )} & 0 \cr} \right] $$
where
$V_{nbx}^b $
and
$V_{nby}^b $
are the x-axis and y-axis components of
${\bf V}_{nb}^b $
,
$V_{wx}^b $
and
$V_{wy}^b $
are the x-axis and y-axis components of the wind velocity in the b frame, k is a constant value and known as drag coefficient and can be estimated through experiments (Leishman et al., Reference Leishman, Macdonald, Beard and McLain2014b).
${\bf D}_b$
is a vector composed of the drag force components in the x-axis and y-axis of the b frame, which is proportional to the airspeed. Therefore, if the drag force is known, the x-axis and y-axis components of the airspeed in the b frame can be estimated. It should be noticed that the velocity estimated by the quadrotor model is with respect to the air stream instead of the ground.
The aerodynamic forces can be calculated from accelerometers’ outputs as follows (Abeywardena et al., Reference Abeywardena, Kodagoda, Dissanayake and Munasinghe2013a):
$${\bf F}_b = {\bf f}_bm$$
where
${\bf f}_b$
is the output of accelerometers.
Substituting Equation (6) into Equations (4) and (5)
$${\bf T}_b = \left[\matrix{ 0 & 0 & {\,f_{bz}m}} \right]$$
$$\left\{ {\matrix{ {V_{nbx}^b - V_{wx}^b = f_{bx}m/k} \cr {V_{nby}^b - V_{wy}^b = f_{by}m/k} \cr}} \right.$$
where f
bx
, f
by
and f
bz
are the x-axis, y-axis, z-axis components of
${\bf f}_b$
. When ignoring the wind, Equation (8) means that
$V_{nbx}^b $
and
$V_{nby}^b $
can be directly obtained from accelerometers’ outputs. By contrast, an integral operation is needed in the inertial navigation algorithm. Therefore the errors of
$V_{nbx}^b $
and
$V_{nby}^b $
brought by the accelerometers’ errors increase with time when using the inertial navigation algorithm, while the errors of
$V_{nbx}^b $
and
$V_{nby}^b $
do not diverge when using the quadrotor model constraints.
3. MODEL-AIDED OPTICAL FLOW/ INERTIAL SENSOR FUSION
Several MA-IEs have been developed (Crocoll et al., Reference Crocoll, Seibold, Scholz and Trommer2014; Leishman et al., Reference Leishman, Macdonald, Beard and McLain2014b; Macdonald et al., Reference Macdonald, Leishman, Beard and McLain2014). In the previous MA-IEs, an assumption that the quadrotor is flying under no wind environment is made. Under that assumption, the airspeed estimated by the quadrotor model can be treated as the ground velocity and it is proved that
$V_{nbx}^b $
,
$V_{nby}^b $
, φ and θ can be well estimated. The assumption is reasonable for indoor flight, but unreasonable for outdoor flight. In this section, optical flow measurement will be fused into the MA-IE forming an MA-OIF. It will be shown that the MA-OIF can provide good estimates of UAV and wind velocities.
3.1. MA-OIF Filter Design
The diagram of the proposed MA-OIF filter is shown in Figure 1. φ, θ, ψ,
$V_{nbx}^b $
,
$V_{nby}^b $
,
$V_{nbz}^b $
, h,
$V_{wx}^b $
and
$V_{wy}^b $
are selected as the state variables, where h is the height. Θ is the three dimensional attitude. The gyros’ outputs, z-axis accelerometer output and quadrotor model information are used to predict the states. The optical flow sensor outputs
$\hat V_{nbx}^b $
and
$\hat V_{nby}^b $
, the barometer output
$\hat h$
, the magnetic sensor output
$\hat \psi $
and f
bx
, f
by
are selected as the measurements.
Block diagram of the proposed MA-OIF filter.
Compared with the previous MA-IEs, the proposed MA-OIF filter has two major improvements:
-
1. The previous MA-IEs can only work well in no wind environment. Wind causes navigation error to the algorithm, since wind velocity is not observable. The proposed MA-OIF filter can perform well under a windy environment, thanks to the introduced optical flow sensor.
-
2. In the previous MA-IEs, only two dimensional velocity and two dimensional attitude can be estimated (Crocoll et al., Reference Crocoll, Seibold, Scholz and Trommer2014; Leishman et al., Reference Leishman, McLain and Beard2014a; Macdonald et al., Reference Macdonald, Leishman, Beard and McLain2014), since only inertial sensors’ outputs are used. The MA-OIF can estimate three dimensional velocity and three dimensional attitude. Excepting the inertial sensors, the MA-OIF also uses the outputs of the barometer and the magnetic sensor as measurements.
The wind consists of two components: a static wind and a gust. The static component is assumed to be constant during the flight, while the gust is presumed to vary with time (Waslander and Wang, Reference Waslander and Wang2009). In the designed filter, the gust is modelled as a random walk. The random walk is a low-frequency random signal and can be obtained by integrating a white noise. Although the random walk cannot accurately describe a gust, it is acceptable to the filter since it is used for state prediction and will be corrected in the measurement update process.
The process equations can be established from Equations (3), (4) and (5)
$$\left\{ \matrix{\dot \phi = (\omega _{nby}^b + \delta \omega _y) + (\omega _{nbx}^b + \delta \omega _x)\sin \phi \tan \theta - (\omega _{nbz}^b + \delta \omega _z)\cos \phi \tan \theta \hfill \cr \dot \theta = \cos \phi (\omega _{nbx}^b + \delta \omega _x) + \sin \phi (\omega _{nbz}^b + \delta \omega _z) \hfill \cr \dot \psi = (\omega _{nbx}^b + \delta \omega _x)\sin \phi /\!\cos \theta - (\omega _{nbz}^b + \delta \omega _z)\cos \phi /\!\cos \theta \hfill \cr \dot V_{nbx}^b = g\sin \phi \cos \theta + (k/m)(V_{nbx}^b - V_{wx}^b ) + \delta V_{nbx}^b \hfill \cr \dot V_{nby}^b = - g\sin \theta + (k/m)(V_{nby}^b - V_{wy}^b ) + \delta V_{nby}^b \hfill \cr \dot V_{nbz}^b = - g\cos \phi \cos \theta + f_{bz} + \delta V_{nbz}^b \hfill \cr \dot h = - V_{nbx}^b \cos \theta \sin \phi + V_{nby}^b \sin \theta + V_{nbz}^b \cos \theta \cos \phi \hfill \cr \dot V_{wx}^b = \omega _{wx} \hfill \cr \dot V_{wy}^b = \omega _{wy} \hfill} \right.$$
where ω
wx
and ω
wy
are the driven Gaussian white noises of
$V_{wx}^b $
and
$V_{wy}^b $
.
$\omega _{bnx}^b $
,
$\omega _{bny}^b $
,
$\omega _{bnz}^b $
and f
bz
are treated as the input variables, which can be obtained from the inertial sensors’ outputs. δω
x
, δω
y
and δω
z
stand for the errors of
$\omega _{nbx}^b $
,
$\omega _{nby}^b $
and
$\omega _{nbz}^b $
and are caused by the errors of gyros.
$\delta V_{nbx}^b $
,
$\delta V_{nby}^b $
and
$\delta V_{nbz}^b $
stand for the estimation errors of
${\dot V}_{nbx}^b $
,
${\dot V}_{nby}^b $
and
${\dot V}_{nbz}^b $
are due to the model's inaccuracy. δω
x
, δω
y
, δω
z
,
$\delta V_{nbx}^b $
,
$\delta V_{nby}^b $
,
$\delta V_{nbz}^b $
, ω
wx
and ω
wy
are treated as the process noises. In the process equations, the Coriolis terms are neglected since they are relatively small for autonomous flight (Leishman et al., Reference Leishman, Macdonald, Beard and McLain2014b). f
bx
, f
by
,
$\hat \psi $
,
$\hat h$
,
$\hat V_{nbx}^b $
and
$\hat V_{nby}^b $
are selected as the measurements. The measurement equations can be established as follows
$$\left\{ {\matrix{ {\,f_{bx} = k(V_{nbx}^b - V_{wx}^b )/m + \Delta f_{bx}} \cr {\,f_{by} = k(V_{nby}^b - V_{wy}^b )/m + \Delta f_{by}} \cr {\hat V_{nbx}^b = V_{nbx}^b + \Delta \hat V_{nbx}^b} \cr {\hat V_{nby}^b = V_{nby}^b + \Delta \hat V_{nby}^b} \cr {\hat \psi = \psi + \Delta \hat \psi} \cr {\hat h = h + \Delta \hat h} \cr}} \right.$$
where Δf
bx
, Δf
by
,
$\Delta \hat \psi $
,
$\Delta \hat h$
,
$\Delta \hat V_{nbx}^b $
and
$\Delta \hat V_{nby}^b $
are the noises of f
bx
, f
by
,
$\hat \psi $
,
$\hat h$
,
$\hat V_{nbx}^b $
and
$\hat V_{nby}^b $
and treated as the measurement noises.
3.2. Observability Analysis of the MA-OIF Filter
The Kalman filter is utilised to implement the MA-OIF. Observability is a necessary condition for the Kalman filter to converge. The previous MA-IEs are proved to be observable in a no wind environment but not observable in a windy environment. In this section, the observability of the proposed MA-OIF will be examined.
A rank condition test was proposed to examine whether a nonlinear system has locally weak observability (Kelly and Sukhatme, Reference Kelly and Sukhatme2011). Strictly a locally weak observability is not a sufficient condition for observability. But for a real analytic system such as the MA-OIF, it could be assumed that the locally weak observability is sufficient to examine whether the system is observable (Mirzaei and Roumeliotis, Reference Mirzaei and Roumeliotis2008). In the following paragraph, the rank condition test will be used to analyse the observability of the MA-OIF.
For convenience, the MA-OIF process Equations (9) are linearized about the quadrotor hover state. The linearization does not affect the observability analysis result. Rearranging the MA-OIF process equations results in
$$\eqalign{& \underbrace{{\left[ {\matrix{ {\dot \Theta} \cr { \dot{\bf V}_{nb}^b} \cr {\dot h} \cr { \dot{\bf V}_{wxy}^b} \cr}} \right]}}_{{ \dot{\bf X}}} = \underbrace{{\left[ {\matrix{ {{\bf 0}_{3 \times 3}} & {{\bf 0}_{3 \times 3}} & {{\bf 0}_{3 \times 1}} & {{\bf 0}_{3 \times 2}} \cr {\bf J} & {\displaystyle{k \over m}\Gamma} & {{\bf 0}_{3 \times 1}} & { - \displaystyle{k \over m}\ {\rm Y}} \cr {{\bf 0}_{1 \times 3}} & {\rm P} & 0 & {{\bf 0}_{1 \times 2}} \cr {{\bf 0}_{2 \times 3}} & {{\bf 0}_{2 \times 3}} & {{\bf 0}_{2 \times 1}} & {{\bf 0}_{2 \times 2}} \cr}} \right]}}_{{\bf F}}\underbrace{{\left[ {\matrix{ \Theta \cr {{\bf V}_{nb}^b} \cr h \cr {{\bf V}_{wxy}^b} \cr}} \right]}}_{{\bf X}} \cr & + \underbrace{{\left[ {\matrix{ \Xi & {{\bf 0}_{3 \times 1}} \cr {{\bf 0}_{2 \times 3}} & {{\bf 0}_{2 \times 1}} \cr {{\bf 0}_{1 \times 3}} & 1 \cr {{\bf 0}_{3 \times 3}} & {{\bf 0}_{3 \times 1}} \cr}} \right]}}_{{\bf B}}\underbrace{{\left[ {\matrix{ {\omega _{nb}^b} \cr { - g\cos \phi \cos \theta + f_{bz}} \cr}} \right]}}_{{\bf U}} + \underbrace{{\left[ {\matrix{ \Xi & {{\bf 0}_{3 \times 3}} & {{\bf 0}_{3 \times 2}} \cr {{\bf 0}_{3 \times 3}} & {{\bf I}_{3 \times 3}} & {{\bf 0}_{3 \times 2}} \cr {{\bf 0}_{1 \times 3}} & {{\bf 0}_{1 \times 3}} & {{\bf 0}_{1 \times 2}} \cr {{\bf 0}_{2 \times 3}} & {{\bf 0}_{2 \times 3}} & {{\bf I}_{2 \times 2}} \cr}} \right]}}_{{\bf G}}\underbrace{{\left[ {\matrix{ {\delta \omega _{nb}^b} \cr {\delta {\bf V}_{nb}^b} \cr {\omega _{wxy}} \cr}} \right]}}_{{\bf W}}} $$
where
${\bf I}_{n \times n}$
stands for a n × n identity matrix,
${\bf 0}_{m \times n}$
stands for a m × n zero matrix,
$\Xi $
is the coefficient matrix of Equation (3),
${\bf V}_{wxy}^b $
,
$\delta \omega _{nb}^b $
,
$\delta {\bf V}_{nb}^b $
and ω
wxy
are all the vector forms of their corresponding components and
$${\bf J} = \left[\matrix{g & 0 & 0 \cr 0 & {- g} & 0 \cr 0 & 0 & 0} \right],\;\;\;\Gamma = \left[\matrix{1 & 0 & 0 \cr 0 & 1 & 0 \cr 0 & 0 & 0} \right],\;\;\;{\rm Y} = \left[\matrix{1 & 0 \cr 0 & 1 \cr 0 & 0} \right],\;\;\;{\rm P} = \left[\matrix{- \cos {\it \theta} \sin {\it \phi} \cr \sin {\it \theta} \cr \cos {\it \theta} \cos {\it \phi}} \right]^T.$$
Rearranging the MA-OIF measurement Equations (10) results in
$$\underbrace{{\left[ {\matrix{ {\,f_{bx}} \cr {\,f_{by}} \cr {\hat V_{nbx}^b} \cr {\hat V_{nby}^b} \cr {\hat \psi} \cr {\hat h} \cr}} \right]}}_{{\bf Z}} = \underbrace{{\left[ {\matrix{ {{\bf 0}_{2 \times 3}} & {\displaystyle{k \over m}{\rm Y} ^T} & {{\bf 0}_{2 \times 1}} & { - \displaystyle{k \over m}I_{2 \times 2}} \cr {{\bf 0}_{2 \times 3}} & {{\rm Y} ^T} & {{\bf 0}_{2 \times 1}} & {{\bf 0}_{2 \times 2}} \cr {{\bf e}_{1 \times 3}} & {{\bf 0}_{1 \times 3}} & 0 & {{\bf 0}_{1 \times 2}} \cr {{\bf 0}_{1 \times 3}} & {{\bf 0}_{1 \times 3}} & 1 & {{\bf 0}_{1 \times 2}} \cr}} \right]}}_{{\bf H}}\underbrace{{\left[ {\matrix{ \Theta \cr {{\bf V}_{nb}^b} \cr h \cr {{\bf V}_{wxy}^b} \cr}} \right]}}_{{\bf X}} + \underbrace{{\left[ {\matrix{ {\delta f_{bx}} \cr {\delta f_{by}} \cr {\delta \hat V_{nbx}^b} \cr {\delta \hat V_{nby}^b} \cr {\Delta \hat \psi} \cr {\Delta \hat h} \cr}} \right]}}_{{\bf R}}$$
where
${\bf e}_{1 \times 3} = \left[ {\matrix{ 0 & 0 & 1 \cr}} \right]$
. To facilitate easy calculation, the measurement matrix is divided into four parts:
$${\bf H} = \left[ {\matrix{ {{\bf 0}_{2 \times 3}} & {\displaystyle{k \over m}{\rm Y} ^T} & {{\bf 0}_{2 \times 1}} & { - \displaystyle{k \over m}I_{2 \times 2}} \cr {{\bf 0}_{2 \times 3}} & {{\rm Y} ^T} & {{\bf 0}_{2 \times 1}} & {{\bf 0}_{2 \times 2}} \cr {{\bf e}_{1 \times 3}} & {{\bf 0}_{1 \times 3}} & 0 & {{\bf 0}_{1 \times 2}} \cr {{\bf 0}_{1 \times 3}} & {{\bf 0}_{1 \times 3}} & 1 & {{\bf 0}_{1 \times 2}} \cr}} \right] = \left[ {\matrix{ {{\bf H}_1} \cr {{\bf H}_2} \cr {{\bf H}_3} \cr {{\bf H}_4} \cr}} \right]$$
Then the observability matrix of the MA-OIF can be expressed as
$${\bf O}_{MA - OIF} = \left[ {\matrix{ {{\bf O}_{MA - OIF1}} & {{\bf O}_{MA - OIF2}} & {{\bf O}_{MA - OIF3}} & {{\bf O}_{MA - OIF4}} \cr}} \right]^T$$
${\bf O}_{MA - OIF1}$
,
${\bf O}_{MA - OIF2}$
,
${\bf O}_{MA - OIF3}$
and
${\bf O}_{MA - OIF4}$
are the observability matrices corresponding to
${\bf H}_1$
,
${\bf H}_2$
,
${\bf H}_3$
and
${\bf H}_4$
. It is relatively simple to calculate
${\bf O}_{MA - OIF1}$
,
${\bf O}_{MA - OIF2}$
,
${\bf O}_{MA - OIF3}$
and
${\bf O}_{MA - OIF4}$
rather than calculating
${\bf O}_{MA - OIF}$
directly. Substituting the simplified results into Equation (14) gives:
$$\eqalign{{\bf O}_{MA - OIF} & = \left[\matrix{\left[\matrix{{\bf H}_1^T &\left({\bf H}_1{\bf F} \right)^T} \right]^T \cr \left[\matrix{{\bf H}_2^T &\left({\bf H}_2{\bf F} \right)^T} \right]^T \cr {\bf H}_3 \cr \left[\matrix{{\bf H}_4^T &\left({\bf H}_4{\bf F} \right)^T & \left({\bf H}_4{\bf F}^2 \right)^T} \right]^T} \right] \cr & = \left[\matrix{{\bf 0}_{2 \times 3} &\displaystyle{k \over m} {\rm Y}^T {\bf J} &{\bf 0}_{2 \times 3} &{\rm Y}^T {\bf J} &{\bf e}_{1 \times 3} &{\bf 0}_{1 \times 3} &{\bf 0}_{1 \times 3} &{\rm P}{\bf J} \cr \displaystyle{k \over m} {\rm Y}^T &\displaystyle{k^2 \over m^2} {\rm Y}^T &{\rm Y}^T &\displaystyle{k \over m}{\rm Y}^T &{\bf 0}_{1 \times 3} &{\bf 0}_{1 \times 3} & {\rm P} &\displaystyle{k \over m} {\rm P}\Gamma \cr {\bf 0}_{2 \times 1} &{\bf 0}_{2 \times 1} &{\bf 0}_{2 \times 1} &{\bf 0}_{2 \times 1} & 0 & 1 & 0 & 0 \cr - \displaystyle{k \over m} I_{2 \times 2} & - \displaystyle{k^2 \over m^2} I_{2 \times 2} &{\bf 0}_{2 \times 2} & - \displaystyle{k \over m}I_{2 \times 2} &{\bf 0}_{1 \times 2} &{\bf 0}_{1 \times 2} &{\bf 0}_{1 \times 2} & - \displaystyle{k \over m} {\rm P} {\rm Y}} \right]^T}$$
${\bf O}_{MA - OIF}$
is a 12 × 9 matrix and its rank is 9, which means that
${\bf O}_{MA - OIF}$
is a full column rank matrix and the proposed MA-OIF satisfies the observability rank condition. Therefore, all states are observable and the MA-OIF can provide an accurate velocity estimate.
4. FAULT-TOLERANT MA-OIF
Based on the MA-OIF, a fault-tolerant algorithm is designed. Both the accelerometer outputs (to estimate the drag) and the optical flow sensor outputs are used as measurements in the MA-OIF to improve the velocity observability. However, environmental factors or large flight movement can result in optical flow sensor failures and airframe deformation can result in quadrotor model failures, so a fault-tolerant algorithm is used to increase the velocity reliability. The fault-tolerant MA-OIF is shown in Figure 2.
Block diagram of the fault-tolerant MA-OIF.
The voting algorithm is a straightforward but effective fault detection method (Tabbache et al., Reference Tabbache, Benbouzid, Kheloui and Bourgeot2013). In the designed voter, the quadrotor model is treated as a virtual sensor providing velocity information. The optical flow sensor output
$\tilde{V}\!_{of}$
, the velocity obtained by the quadrotor model
$\tilde{V}_m$
and the velocity derived from accelerometers
$\tilde{V}_a$
enter into the voter. In a real flight, there is a very small probability that two or more sensor failures may occur at the same time. Therefore it is assumed that no more than one sensor is faulty at the same time.
In the voter,
$\tilde{V}_{of}$
,
$\tilde{V}_m$
and
$\tilde{V}_a$
are compared with each other and faults can be detected through the following principles:
-
• If the optical flow sensor has faults,
(16)where
$$\left\{\matrix{\tilde{V}_{of} - \tilde{V}_m \ge \Delta \tilde{V}_{of - m} \cr \tilde{V}_{of} - \tilde{V}_a \ge \Delta \tilde{V}_{of - a} \cr \tilde{V}_m - \tilde{V}_a \lt \Delta \tilde{V}_{m - a}} \right.$$
$\Delta \tilde{V}_{of - m}$
,
$\Delta \tilde{V}_{of - a}$
and
$\Delta \tilde{V}_{m - a}$
are the thresholds, which should be set according to the normal error characteristics of sensors. Sensor error mainly consists of two parts, a constant part and a stochastic part. The constant part is called bias and can be measured through static experiments. According to the three-sigma-rule, the normal stochastic error value should be within a factor of three of its standard deviation. Therefore, the thresholds are set according to the following principle:
(17)where ε vof , ε vm and ε va are the biases of the velocities provided by the optical flow sensor, the vehicle model and the accelerometer respectively, δ vof , δ vm and δ va are the standard deviations of the velocities provided by the optical flow sensor, the vehicle model and the accelerometer respectively.
$$\left\{\matrix{\Delta \tilde{V}_{of - m} \!\!\!\!&=&\!\!\!\! \left \vert {\varepsilon _{vof}} \right\vert + 3\left\vert {\delta _{vof}} \right\vert + \left\vert {\varepsilon _{vm}} \right\vert + 3\left\vert {\delta _{vm}} \right\vert \cr \Delta \tilde{V}_{of - a} \!\!\!\!&=&\!\!\!\!\! \left\vert {\varepsilon _{vof}} \right\vert + 3\left\vert {\delta _{vof}} \right\vert + \left\vert \varepsilon _{va} \right\vert + 3\left\vert {\delta _{va}} \right\vert \cr \Delta \tilde{V}_{m - a} \!\!\!\!&=&\!\!\!\!\!\!\! \left\vert \varepsilon _{vm} \right\vert + 3\left\vert {\delta _{vm}} \right\vert + \left\vert {\varepsilon _{va}} \right\vert + 3\left\vert {\delta _{va}} \right\vert} \right.$$
-
• if the quadrotor model has faults,
(18)
$$\left\{\matrix{\tilde{V}_{of} - \tilde{V}_m \ge \Delta \tilde{V}_{of - m} \cr \tilde{V}_{of} - \tilde{V}_a \lt \Delta \tilde{V}_{of - a} \cr \tilde{V}_m - \tilde{V}_a \ge \Delta \tilde{V}_{m - a}} \right.$$
-
• Since
$\tilde{V}_m$
and
$\tilde{V}_a$
are both obtained from accelerometers, accelerometers’ faults will result in degrading the accuracies of both
$\tilde{V}_m$
and
$\tilde{V}_a$
.
(19)
$$\left\{\matrix{\tilde{V}_{of} - \tilde{V}_m \ge \Delta \tilde{V}_{of - m} \cr \tilde{V}_{of} - \tilde{V}_a \ge \Delta \tilde{V}_{of - a} \cr \tilde{V}_m - \tilde{V}_a \ge \Delta \tilde{V}_{m - a}} \right.$$
It can be seen that three sensors’ faults can all be detected. In the optical flow sensor faulty condition or the quadrotor model faulty condition, velocities can be obtained from the other two sensors. However, in the accelerometer faulty condition, only the velocity from the optical flow sensor can be used and a filter cannot be formed. Therefore, only the optical flow sensor faults and the quadrotor model faults will be considered in the following paragraph.
If the optical flow sensor and the quadrotor model are both healthy, then the MA-OIF is used. If the optical flow sensor or the quadrotor model is faulty, then the faulty sensor will be isolated and the healthy one will be used as a measurement. In faulty conditions, the MA-OIF degrades to the MA-IE or the OIF.
5. SIMULATIONS AND ANALYSIS
Quadrotor simulation software is developed using MATLAB/Simulink to verify the proposed algorithms. The OS4 quadrotor model described by Bouabdallah (Reference Bouabdallah2007) is adopted. As shown in Figure 3, the simulation software includes the quadrotor dynamics and the Guidance, Navigation and Control (GNC) module. The MA-OIF, MA-IE and OIF are all realised for comparison. The true navigation results remain in the control loop for a steady flight path.
Simulation software block diagram.
The simulation conditions are set as follows:
-
• Sensors’ accuracies are set as: gyro
$0\!\cdot\!1^{\degr} /{\rm s}$
, accelerometer
$0\!\cdot\!01\,{\rm m}/{\rm s}^2$
, height supplied by the barometer 1 m, yaw supplied by the magnetic sensor 2°, optical flow sensor
$5\% $
scale error. -
• A
$10\% $
random error is added to the quadrotor drag coefficient k. -
• A
$0\!\cdot\!5\,{\rm m/s}$
threshold is adopted both for the optical flow sensor fault and the quadrotor model fault. -
• The simulated wind consists of a
$1\!\cdot\!5\,{\rm m/s}$
constant wind and a gust implemented by the wind toolbox in Simulink. The amplitude of the gust is set as
$3\!\cdot\!5\,{\rm m/s}$
. The wind is simulated in the n frame. -
• A 60 seconds L-type flight path is set: the quadrotor first hovers for 10 seconds, then flies to the east at
$3\,{\rm m/s}$
for 20 seconds, then another 10 seconds hover, then flies to the north at
$3\,{\rm m/s}$
for 20 seconds.
5.1. Performance Comparison of Different Filters
Initially, a healthy condition was simulated to compare the performances of different filters. Simulations were first carried out under a no wind environment. The velocity and position estimates of the MA-OIF, MA-IE and OIF are shown in Figures 4 and 5 respectively. Attitude and height results are not given since they are not the focus of this paper.
Comparison of true velocity, MA-IE velocity estimates, OIF velocity estimates and MA-OIF velocity estimates under a no-wind environment.
Comparison of true position, MA-IE position estimates, OIF position estimates and MA-OIF position estimates under a no-wind environment.
It can be seen that all three filters can provide good velocity and position estimates. The estimates of the MA-OIF and the OIF are similar to each other, since the optical flow measurement contributes much to both the filters. It can also be noticed that the estimation accuracy of the MA-IE is a little lower than the other two, mainly due to the model uncertainties. The filter performance is directly related to the setting errors. As previously mentioned, some errors were added to the optical flow measurement and the quadrotor model based on a common situation. It appears that the OIF and MA-OIF performs better than the MA-IE under this condition.
The MA-OIF, OIF and MA-IE run under a windy environment and their velocity estimates along with the true velocity are presented in Figure 6. As expected, both the MA-OIF and OIF have good velocity estimation accuracy. In contrast, the velocity estimate accuracy of the MA-IE is poor due to wind.
Comparison of true velocity, MA-IE velocity estimates, OIF velocity estimates and MA-OIF velocity estimates under a windy environment.
Figure 7 presents the position estimates. It can be seen that the estimated path of the MA-OIF and OIF are close to the real one, while the estimated path of the MA-IE diverges greatly. The wind velocity estimates of the MA-OIF are shown in Figure 8. This proves that the MA-OIF is capable of producing accurate wind velocity estimates, which is as predicted by the observability analysis in Section 3. The wind velocity estimation accuracy is related to the accuracy of the optical flow measurement. Since the optical flow sensor has a scale error, the wind velocity estimation accuracy is higher in hover mode (0 s to 10 s and 30 s to 40 s) than in motion mode.
Comparison of true position, MA-IE position estimates, OIF position estimates and MA-OIF position estimates under a windy environment.
Comparison of true wind velocity and MA-OIF wind velocity estimates in the b frame under a windy environment.
The Root Mean Square Errors (RMSE) of different filters are shown in Table 1. It can be seen that the velocity and position estimates of the MA-IE improve greatly when there is no wind. Estimation accuracies of the MA-OIF and OIF under the windy environment and the no wind environment are almost the same.
Velocity and position estimates errors of different fusion structures.
5.2. Optical Flow Sensor Fault Simulation
There are mainly two kinds of faults for an optical flow sensor: large noises and large constant errors. When the ground features are not steady, large noises may occur in the optical flow measurement. A large constant error may be produced when the optical flow sensor runs above a featureless ground. Both conditions were simulated to test the performance of the fault-tolerant MA-OIF. Some large noises were added to the optical flow sensor signal between 20 s to 25 s, as shown in Figure 9. Figure 9 also gives the fault detection results. Since the threshold value for the optical flow sensor output is set as 0·5 m/s in the voter, it can be seen that the noises with amplitude beyond 0·5 m/s can be detected.
Optical flow measurement with large noises and the MA-OIF fault detection result.
Figure 10 presents the velocity estimates of the MA-OIF and OIF. It is clear that the noisy optical flow measurements have little effect on the MA-OIF, even some small noises are not detected. In contrast, the large noises degrade the OIF much, which is because the OIF relies heavily on the optical flow measurements.
Comparison of true velocity, OIF velocity estimates and MA-OIF velocity estimates under windy environment, when large noises exist in the optical flow measurement.
A 2 m/s constant error was added to the optical flow measurements
$\hat V_{nbx}^b $
and
$\hat V_{nby}^b $
between 20 s and 25 s. The velocity estimates of the MA-OIF and OIF along with the detection result are presented in Figure 11. It is clear that the optical flow sensor faults can be well detected. Figure 11 also demonstrates the improvements brought by the fault-tolerant MA-OIF. It is interesting to note that the velocity estimation accuracy of the MA-OIF is better than the MA-IE even when the optical flow sensor is faulty. That is because the wind has already been well estimated when the optical flow sensor fault occurs and this restricts the model divergence during the optical flow sensor outage. It can be noted that the velocity error diverges slowly between 20 s to 25 s but obviously smaller than the velocity error of the MA-IE shown in Figure 6. The wind velocity estimates of the MA-OIF are shown in Figure 12, illustrating that the wind velocity is not observable during optical flow sensor failure.
Comparison of true velocity, OIF velocity estimates and MA-OIF velocity estimates under windy environment, when constant errors exist in the optical flow measurement.
Comparison of true wind velocity and MA-OIF wind velocity estimates in the b frame under windy environment, when constant errors exist in the optical flow measurement.
5.3. Quadrotor Model Fault Simulation
The quadrotor model may change due to airframe deformation, so a model faulty condition was simulated. Since the MA-IE only works well under a no wind environment, the wind speed was set to zero in the simulation. A two times error is added to the drag coefficient between 15 s to 35 s. The velocity estimates of the MA-OIF and MA-IE are presented in Figure 13. It can be seen that the MA-OIF accuracy improves greatly over the MA-IE, since the MA-OIF transforms to the OIF when the model is faulty, while the MA-IE relies heavily on the accurate model. The model fault is well detected between 15 s to 33 s but not detected between 33 s to 35 s. That is because the velocity error introduced by the model is below the threshold value between 33 s to 35 s, since the drag coefficient is a scale factor and the velocity error caused is small when the quadrotor performs small manoeuvres.
Comparison of true velocity, MA-IE velocity estimates and MA-OIF velocity estimates under no wind environment, when errors exist in the quadrotor model.
6. CONCLUSIONS
A fault-tolerant MA-OIF is proposed to improve the velocity estimation accuracy when optical flow sensor faults occur. The proposed MA-OIF is easy to implement on a quadrotor, since it is only a software improvement and no more sensors are needed.
The optical flow sensor faults can be detected and isolated by the fault-tolerant MA-OIF. The velocity estimation accuracy of the MA-OIF improves greatly over the OIF during the optical flow sensor outage, with the help of the quadrotor model. When the optical flow sensor is healthy, the velocity estimation accuracies of the MA-OIF and the OIF are almost the same. Both the algorithms are particularly suitable for the hover mode, since their velocity estimation errors are related to the quadrotor manoeuvres.
The MA-OIF also performs better than the MA-IE. The MA-OIF works well under windy environments, while the MA-IE can only maintain good velocity estimation accuracy under a no-wind environment. Besides, the quadrotor drag model fault can be detected and isolated by the fault-tolerant MA-OIF. The faults may be caused by airframe deformation and greatly affects the MA-IE accuracy.
In our future work, some experiments will be done to test the performance of the fault-tolerant MA-OIF. The threshold value setting method in the voter will also be studied. A weighted threshold value could be used to get a higher fault detection rate.
FINANCIAL SUPPORT
This work was supported by the National Natural Science Foundation of China, (Jizhou Lai, grant number 61174197), (Hugh H.T. Liu, grant number 61428303), (Jianye Liu, grant number 61328301); Jiangsu provincial SixTalent Peaks, (Jizhou Lai, grant number 2015-XXRJ-005); and the Fundamental Research Funds for the Central Universities (Jizhou Lai, grant number NS2014029).
