2.0 MODEL OF NANO QUAD-ROTORS SUBJECT TO DISTURBANCES

Two main coordinates, some forces, moments and geometrical parameters are introduced first before modelling Nano quad-rotors, as shown in Fig. 1,

Figure 1.

Preliminary knowledge of quad-rotors.

In Fig. 1, O _EE _xE _yE _z represents the inertial coordinate, in which E _x and E _y are on the horizontal plane and E _x is perpendicular to E _y, and E _z is perpendicular to the horizontal determined by right-hand rule. O _BB _xB _yB _z represents the body coordinate whose origin is fixed at the centre of gravity of the quad-rotor. B _x is the normal flight orientation, B _y is positive to starboard in the horizontal plane and B _z is orthogonal to the plane B _xO _BB _y.

The non-linear equations of motion subject to disturbances for Nano quad-rotors^{(Reference Derafa, Ouldali and Benallegue12)} are expressed as:

(1) $$\begin{equation} \left\{\begin{array}{l} {\mathop{x}\limits^{\bullet } =v_{x} +w_{x} } \\[1.5pt] {\mathop{y}\limits^{\bullet } =v_{y} +w_{y} } \\[1.5pt] {\mathop{z}\limits^{\bullet } =v_{z} +w_{z} } \end{array}\right. ,\; \; \; \left\{\begin{array}{l} {\mathop{v_{x} }\limits^{\bullet } =(\cos \phi \sin \theta \cos \psi +\sin \phi \sin \psi )\frac{T_{r} }{m} +\Delta f_{x} } \\[1.5pt] {\mathop{v_{y} }\limits^{\bullet } =(\cos \phi \sin \theta \sin \psi -\sin \phi \cos \psi )\frac{T_{r} }{m} +\Delta f_{y} } \\[3pt] {\mathop{v_{z} }\limits^{\bullet } =-g+\frac{T_{r} }{m} \cos \phi \cos \theta +\Delta f_{z} } \end{array}\right. ,\\ \left\{\begin{array}{l} {\mathop{\phi }\limits^{\bullet } =p+q\sin \phi \tan \theta +r\cos \phi \tan \theta } \\ {\mathop{\theta }\limits^{\bullet } =q\cos \phi -r\sin \phi } \\ {\mathop{\psi }\limits^{\bullet } =q\sin \phi \sec \theta +r\cos \phi \sec \theta } \end{array}\right. , \left\{\begin{array}{l} {\mathop{p}\limits^{\bullet } =\underbrace{\frac{I_{y} -I_{z} }{I_{x} } qr+\Delta f_{p} +\left(\frac{l}{I_{x} } -b_{1} \right)\tau _{1} }_{g_{1} (p,q,r;t)}+b_{1} u_{1} } \\ {\mathop{q}\limits^{\bullet } =\underbrace{\frac{I_{z} -I_{x} }{I_{y} } pr+\Delta f_{q} +\left(\frac{l}{I_{y} } -b_{2} \right)\tau _{2} }_{g_{2} (p,q,r;t)}+b_{2} u_{2} } \\ {\mathop{r}\limits^{\bullet } =\underbrace{\frac{I_{x} -I_{y} }{I_{z} } pq+\Delta f_{r} +\left(\frac{1}{I_{z} } -b_{3} \right)\tau _{3} }_{g_{3} (p,q,r;t)}+b_{3} u_{3} } \end{array}\right. \end{equation}$$

Equation (1) is formed by four equation sets in which the first set represents translational kinematics (position control), the second set represents translational dynamics (velocity control), the third set represents rotational kinematics (Euler angle control) and the fourth set represents rotational dynamics (body rate control). In the above, [x, y, z]^T and [υ _x, υ _y, υ _z]^T ∈ R ³ represent position and velocity vectors in inertial frame, respectively. m and g represent mass and gravitational constant, respectively. [ p, q, r]^T ∈ R ³ is the angular velocity vector under body coordinate system. [ϕ, θ, ψ]^T ∈ R ³ is the Euler angle under inertial coordinate system. T _r is lift and [u ₂, u ₃, u ₄]^T represents virtual inputs (roll force, pitch force and yaw moment) acting on quad-rotors. I _x, I _y, I _z are moments of inertial. w _x, w _y and w _z represent wind velocity in inertial frame along the x, y and z axes, respectively. Δf _x, Δf _y, Δf _z, Δf _p, Δf _q and Δf _r are unmodeled dynamics. b ₁, b ₂ and b ₃ are estimations of l/I _x , l/I _y and 1/I _z, respectively.

3.0 DISTURBANCE OBSERVER DESIGN FOR AN AFFINE NON-LINEAR SYSTEM

3.1 Design procedure

Consider the commonly used first-order affine non-linear system with disturbance, shown as:

(2) $$\Sigma _1 :\;\;\mathop y\limits^ \bullet {\kern 1pt} = \;f(y,t) + b(y,t) \cdot u(t) + d(t) $$

where, f (∗) and b(∗) are unknown, and d(t) includes internal and external disturbances. u(t) and y are measurable variables, representing input and output, respectively.

A discretising system Σ₁ with the sampling time period T yields:

(3) $$\ y(k + 1) = y(k) + T \cdot \left[ {f(y(k),t(k)) + b(y(k),t(k)) \cdot u(k) + d(t(k))} \right] $$

Select a reference model with a discretised formation as:

(4) $$\ y_m (k + 1) = y_m (k) + T \cdot b_m \cdot u(k) $$

where, the gain b _m is given. The rest of the work is to estimate g( y(k), u(k); t(k))=f ( y(k), t(k))+ (b( y(k), t(k)) − b _m)u(k) + d(t(k)) by designing a DO using input and output data.

Taking the notation e(k) = y(k) − y _m (k) and subtracting Equation (4) from (3) yields a new non-linear system:

(5) $$\Sigma _2 :\;\;\Delta e(k + 1) = T \cdot g(y(k),u(k);t(k)) $$

Then at k ^th sampling time point:

(6) $$\Delta e(k) = T \cdot g(y(k - 1),u(k - 1);t(k - 1)) $$

Subtracting Equation (6) from (5) yields:

(7) $$\Delta e(k + 1) = \Delta e(k) + \Delta g(y(k),u(k);t(k)) $$

Reference^{(Reference Hou, Chi and Gao13)} introduces a non-linear discrete system written as:

(8) $$\Sigma _0 :\;\;y(k + 1) = f(y(k), \cdots ,y(k - n_y ),u(k), \cdots ,u(k - n_u )) $$

can be linearized into the following formation by using dynamic linearization (DL) theory:

(9) $$\Delta y(k + 1) = \Phi ^T (k)\Delta H(k) $$

where, u(k) ∈ R and y(k) ∈ R represent input and output of the system, respectively. n _y and n _u are two integers. $f(*):\; R^{n_{y} +n_{u} +2} \mapsto R$ is a non-linear mapping. $\Phi (k)=[\phi _{1} (k),\cdots\phi _{L_{y} } (k),\phi _{L_{y} +1} (k),\cdots ,\phi _{L_{y} +L_{u} } (k)]^{T} \in R^{L_{y} +L_{u} } $ is a time-varying vector called pseudo gradient (PG), and ||Φ(k)|| ≤ b, L _y and L _u(0 ≤ L _y ≤ n _y, 1 ≤ L _u ≤ n _u) are integers called pseudo order (PO). Δy(k) = y(k) − y(k − 1), Δu(k) = u(k) − u(k − 1) and ΔH(k) = H(k) − H(k − 1) = [Δy(k)· · · Δy(k − L _y + 1), Δu(k), · · · Δu(k − L _u + 1)]^T.

Applying DL theory to a linearize system Σ₂ yields:

(10) $$\Sigma _2 :\;\;\Delta e(k + 1) = \Delta e(k) + T \cdot \Phi (k) \cdot \Delta H(k)$$

where, ΔH(k) =[Δ e(k),· · · Δ e(k − L _y + 1), Δ u(k), · · · Δu(k − L _u + 1)]^T.

Thus, finally, by making a comparison with Equation (7) and (10), the disturbance term can be estimated as:

(11) $$\ g(y(k),u(k);t(k)) = \frac{{\Delta e(k)}}{T} + \Phi (k) \cdot \Delta H(k) $$

It can be seen that only Φ(k) needs to be updated online. By using the following cost function:

(12) $$\ J(\underline \Phi (k)) = \left| {y(k) - y(k - 1) - \underline \Phi ^T (k)\Delta H(k - 1)} \right|^2 + \mu \left\| {\underline \Phi (k) - \underline \Phi (k - 1)} \right\|^2 $$

and letting $\frac{{\partial J(\underline \Phi (k))}}{{\partial \underline \Phi (k)}} = 0$ , the PG $\underline \Phi (k)$ can be updated online as:

(13) $$\underline \Phi (k) = \underline \Phi (k - 1) + \frac{{\gamma \Delta H(k)\left[ {y(k) - y(k - 1) - \underline \Phi ^T (k - 1)\Delta H(k - 1)} \right]}}{{\mu + \left\| {\Delta H(k - 1)} \right\|^2 }} $$

where, γ ∈ (0, 2], μ > 0 and $\underline \Phi (k)$ is the estimation of Φ(k) In the next section, $\underline \Xi $ represents the estimation of Ξ .

3.2 Convergence analysis

This part aims to prove the boundedness of the error between Φ(k) and $\underline \Phi (k)$. Subtracting the real value Φ(k) from both sides of Equation (13) and denoting $\underline {\underline \Phi } (k) = \underline \Phi (k) - \Phi (k)$ yields:

(14) $$\underline {\underline \Phi } (k) = \left[ {I - \frac{{\gamma \cdot \Delta H(k - 1) \cdot \Delta H^T (k - 1)}}{{\mu + \left\| {\Delta H(k - 1)} \right\|^2 }}} \right]\underline {\underline \Phi } (k - 1) + \Phi (k - 1) - \Phi (k) $$

where, I is an identity matrix.

Taking the norm of both sides of Equation (14) yields:

(15) \begin{align}\label{GrindEQ_15} \left\| \underline{\underline{\Phi }} (k)\right\| & \le \left\| \left[I-\dfrac{\gamma \cdot \Delta H(k-1)\cdot \Delta H^{T} (k-1)}{\mu +\left\| \Delta H(k-1)\right\| ^{2} } \right]\underline{\underline{\Phi }} (k-1)\right\| +\left\| \Phi (k-1)-\Phi (k)\right\| \nonumber\\[3pt] & \le \left\| \left[I-\dfrac{\gamma \cdot \Delta H(k-1)\cdot \Delta H^{T} (k-1)}{\mu +\left\| \Delta H(k-1)\right\| ^{2} } \right]\underline{\underline{\Phi }} (k-1)\right\| +2b \end{align}

Since

(16) \begin{align}\label{GrindEQ_16} & \left\| \left[I-\frac{\gamma \cdot \Delta H(k-1)\cdot \Delta H^{T} (k-1)}{\mu +\left\| \Delta H(k-1)\right\| ^{2} } \right]\underline{\underline{\Phi }} (k-1)\right\| ^{2} \le \left\| \underline{\underline{\Phi }} (k-1)\right\| ^{2}\nonumber\\[3pt] & \quad +\left[-2+\frac{\gamma \cdot \left\| \Delta H(k-1)\right\| ^{2} }{\mu +\left\| \Delta H(k-1)\right\| ^{2} } \right]\cdot \frac{\gamma \left[\underline{\underline{\Phi }} ^{T} (k-1)\Delta H(k-1)\right]^{2} }{\mu +\left\| \Delta H(k-1)\right\| ^{2} } \end{align}

Based on γ ∈ (0, 2] and μ > 0, the following inequality works:

(17) $$\ - 2 + \frac{{\gamma \cdot \left\| {\Delta H(k - 1)} \right\|^2 }}{{\mu + \left\| {\Delta H(k - 1)} \right\|^2 }} \lt 0 $$

Thus:

(18) $$\left\| {\left[ {I - \frac{{\gamma \cdot \Delta H(k - 1) \cdot \Delta H^T (k - 1)}}{{\mu + \left\| {\Delta H(k - 1)} \right\|^2 }}} \right]\underline {\underline \Phi } (k - 1)} \right\| \le \xi \left\| {\underline {\underline \Phi } (k - 1)} \right\| $$

where, 0 < ξ < 1. Finally, the following relationship works:

(19) \begin{align}\label{GrindEQ_19} & \left\| \underline{\underline{\Phi }} (k)\right\| \le \xi \left\| \underline{\underline{\Phi }} (k-1)\right\| +2b\le \xi ^{2} \left\| \underline{\underline{\Phi }} (k-2)\right\| \nonumber\\[4pt] & \qquad +{} 2\xi b+2b\le \cdots \le \xi ^{k-1} \left\| \underline{\underline{\Phi }} (1)\right\| +\frac{2b(1-\xi ^{k-1} )}{1-\xi } \end{align}

Thus, error convergence of the proposed DO is proved.

4.0 CONTROL SCHEME

Since all states in a Nano quad-rotor system are observable and measurable, the reference model can be selected as:

(20) \begin{equation} \left\{\begin{array}{l} {\mathop{x_{m} }\limits^{\bullet } =v_{xm} } \\[2pt] {\mathop{v_{xm} }\limits^{\bullet } =\underbrace{(\cos \phi \sin \theta \cos \psi +\sin \phi \sin \psi )\frac{T_{r} }{m} }_{\xi _{x} }} \end{array}\right. \!,\; \; \left\{\begin{array}{l} {\mathop{y_{m} }\limits^{\bullet } =v_{ym} } \\[2pt] {\mathop{v_{ym} }\limits^{\bullet } =\underbrace{(\cos \phi \sin \theta \sin \psi -\sin \phi \cos \psi )\frac{T_{r} }{m} }_{\xi _{y} }} \end{array}\right.\! ,\nonumber\end{equation} \begin{equation}\label{GrindEQ_20}\left\{\begin{array}{l} {\mathop{z_{m} }\limits^{\bullet } =v_{zm} } \\[2pt] {\mathop{v_{zm} }\limits^{\bullet } =\underbrace{-g+\frac{T_{r} }{m} \cos \phi \cos \theta }_{\xi _{z} }} \end{array}\right. ,\; \; \left\{\begin{array}{l} {\mathop{p_{m} }\limits^{\bullet } =b_{1} u_{2} } \\[2pt] {\mathop{q_{m} }\limits^{\bullet } =b_{2} u_{3} } \\[3pt] {\mathop{r_{m} }\limits^{\bullet } =b_{3} u_{4} } \end{array}\right. \end{equation}

Thus, the wind velocities and unmodeled dynamics can be observed using the DO proposed as shown in Table 1:

Table 1

Disturbance observers design for Nano quad-rotor control system

Based on the estimations of disturbances, the position control scheme is:

(21) $$\ x\;control:\left\{ {\begin{array}{*{20}c} {\mathop x\limits^ \bullet _c = \omega _1 (x_c - x)} \hfill \\ {v_{xc} = \mathop x\limits^ \bullet _c - \underline w _x } \hfill \\ {\mathop {v_{xc} }\limits^ \bullet = \omega _2 (v_{xc} - v_x )} \hfill \\ {\xi _x = \mathop {v_{xc} }\limits^ \bullet - \Delta \underline f _x } \hfill \\ \end{array}} \right.,\;\;\;y\;control:\left\{ {\begin{array}{*{20}c} {\mathop y\limits^ \bullet _c = \omega _1 (y_c - y)} \hfill \\ {v_{yc} = \mathop y\limits^ \bullet _c - \underline w _y } \hfill \\ {\mathop {v_{yc} }\limits^ \bullet = \omega _2 (v_{yc} - v_y )} \hfill \\ {\xi _y = \mathop {v_{yc} }\limits^ \bullet - \Delta \underline f _y } \hfill \\ \end{array}} \right.,\;\;\;z\;control:\left\{ {\begin{array}{*{20}c} {\mathop z\limits^ \bullet _c = \omega _1 (z_c - z)} \hfill \\ {v_{zc} = \mathop z\limits^ \bullet _c - \underline w _z } \hfill \\ {\mathop {v_{zc} }\limits^ \bullet = \omega _2 (v_{zc} - v_z )} \hfill \\ {\xi _z = \mathop {v_{zc} }\limits^ \bullet - \Delta \underline f _z } \hfill \\ \end{array}} \right.$$

where, x _c, y _c and z _c represent the reference path. υ _xc, υ _yc and υ _zc are command translational velocity. ω ₁ and ω ₂ are controller parameters. By commanding the desired yaw angle, the desired Euler angle and thrust can be solved by following equations:

(22) $$\left\{ {\begin{array}{*{20}c} {\psi _c = Commanded\;\;\;value} \hfill \\ {\phi _c = \arcsin \left( {\frac{{\xi _x \sin \psi _c - \xi _y \cos \psi _c }}{{\sqrt {\xi _x^2 + \xi _y^2 + (\xi _z + g)^2 } }}} \right)} \hfill \\ {\theta _c = \arctan \left( {\frac{{\xi _x \cos \psi _c + \xi _y \sin \psi _c }}{{\xi _z + g}}} \right)} \hfill \\ {T_r = m[\xi _x \left( {\cos \phi _c \sin \theta _c \cos \psi _c + \sin \phi _c \sin \psi _c } \right)} \hfill \\ {\quad \quad + \xi _y (\cos \phi _c \sin \theta _c \sin \psi _c - \sin \phi _c \cos \psi _c ) + (\xi _z + g)\cos \phi _c \cos \theta _c ]} \hfill \\ \end{array}} \right.$$

and the attitude control scheme is:

(23) $$\ Roll\;rate:\left\{ {\begin{array}{*{20}c} {p_c = \omega _3 (\phi _c - \phi )} \hfill \\ {\mathop {p_c }\limits^ \bullet = \omega _4 (p_c - p)} \hfill \\ {u_2 = \frac{1}{{b_1 }}(\mathop {p_c }\limits^ \bullet - \underline g _1 )} \hfill \\ \end{array}} \right.,\;\;\;Pitch\;rate:\;\left\{ {\begin{array}{*{20}c} {q_c = \omega _3 (\theta _c - \theta )} \hfill \\ {\mathop {q_c }\limits^ \bullet = \omega _4 (q_c - q)} \hfill \\ {u_3 = \frac{1}{{b_2 }}(\mathop {q_c }\limits^ \bullet - \underline g _2 )} \hfill \\ \end{array}} \right.,\;\;\;\;Yaw\;rate:\left\{ {\begin{array}{*{20}c} {r_c = \omega _3 (\psi _c - \psi )} \hfill \\ {\mathop {r_c }\limits^ \bullet = \omega _4 (r_c - r)} \hfill \\ {u_4 = \frac{1}{{b_3 }}(\mathop {r_c }\limits^ \bullet - \underline g _3 )} \hfill \\ \end{array}} \right.$$

In above equation, ϕ _c, θ _c and ψ _c are command Euler angle. p _c, q _c and r _c are command body rate. ω ₃ and ω ₄ are controller parameters.

5.0 NUMERICAL VALIDATION

In this part, a numerical simulation was conducted to verify the proposed control scheme. The parameters^{(Reference Li and Chen14)} of the Nano quad-rotor used here are: m = 0.04389kg, l = 0.035m, I _x = I _y = 1.916 × 10⁻⁵ and I _z = 2.781 × 10⁻⁵.

The Dryden model^{(Reference Beard and Mclain11)} is adopted to generate the wind field in which the steady wind velocities along the x, y and z axes in the earth frame are assumed to be 1 m/s. To show that the path following the Nano quad-rotor does not depend on the detailed model information, we assume that the three parameters b ₁, b ₂ and b ₃ perturb within a wide area, in order to show that the Nano quad-rotor does not depend on the detailed model information when it does path following task −30% ~ +0.5m. Meanwhile, an additional mass with the weight 50%m is added on the Nano quad-rotor.

The parameters in above scheme are:

ω ₁ = 2, ω ₂ = 4, ω ₃ = 6 and ω ₄ = 30. The relationships ω ₄ = n ₁ω ₃, ω ₂ = n ₂ω ₁, n _i = 2 ~ 5, i = 1, 2 work according to the response speed (body rate p, q, r > Euler angle ϕ, θ, ψ > translational velocity υ _x, υ _y, υ _z > position x, y, z) of the quad-rotor system.

γ _x= γ _y = γ _z = 1, γ _υx = γ _υy = γ _υz = 1, γ _p = γ _q = γ _r = 1, μ _x = μ _y = μ _z = 1, μ _υx = μ _υy = μ _υz = 1 and μ _p = μ _q = μ _r = 1. They satisfy the conditions γ ∈ (0, 2] and μ > 0 mentioned in Section 3.2.

L _yx = L _yy = L _yz = 0, L _yυx = L _yυy = L _yυz = 0, L _yp = L _yq = L _yr = 0, L _ux = L _uy = L _uz = 1, L _uυx = L _uυy = L _uυz = 1 and L _up = L _uq = L _ur = 1. These parameters control the linearized length of the quad-rotor system. Other values are also applicable.

The reference signal is:

(24) $$\ x_c (t) = \frac{1}{2}\cos t\;\;(m),\;\;y_c (t) = \frac{1}{2}\sin t\;\;(m),\;\;z_c (t) = - 1 - \frac{t}{{10}}\;\;(m),\;\;\psi _c (t) = \frac{\pi }{3}\;\;(rad) $$

Simulation results are shown as in Fig. 2–Fig. 9:

Figure 2.

Effect of 3D path following.

Figure 3.

Virtual inputs.

Figure 4.

Euler angle.

Figure 5.

Estimation of w _x, w _y and w _z.

Figure 6.

Estimation of f _x, f _y and f _z: +50% perturbation.

Figure 7.

Estimation of g ₁, g ₂ and g ₃: +50% perturbation.

Figure 8.

Estimation of f _x, f _y and f _z: −30% perturbation.

Figure 9.

Estimation of g ₁, g ₂ and g ₃: −30% perturbation.

It is clearly shown in Fig. 2–Fig. 4 that the EDI-based method achieves good performance under the wind field without using an accurate model, bringing a satisfactory response time. In Figs. 5–9, the proposed DO accurately estimates not only the disturbances that can be modelled, but also the coloured noise. In the Dryden wind field model, the wind is actually the steady wind adding turbulence (simulated using coloured noise converted from a standard Gaussian white noise signal passing through forming filter); thus, in fact, the disturbance is not time continuous. Since the disturbance estimation in the DO proposed in this paper is based on the differences between plant model and reference model, the characteristics of the disturbance are not strictly assumed.