6.2.1 Nested Saturation with Integral Part Controller – NSIP

The following controller asymptotically stabilizes system (6.6):

$u_{\mathit{NSIP}}=-{\sigma }_{b_{n^{\prime }}}(z_{n^{\prime }}+{\sigma }_{b_{n^{\prime }-1}}(z_{n^{\prime }-1}+\cdots +{\sigma }_{b_1}(z_1))),$

where

$\begin{array}{ccc}\hfill z_i& \hfill =\hfill & z_{i+1}+\sum _{j=1}^{n^{\prime }-i}\left(\begin{array}{c}\hfill n^{\prime }-i-1\hfill \\ \hfill j-1\hfill \end{array}\right)y_{n^{\prime }-j}\forall i\in [1,n^{\prime }-1],\hfill \\ \hfill z_{n^{\prime }}& \hfill =\hfill & y_{n^{\prime }}.\hfill \end{array}$

Notice that $\left(\begin{array}{c}\hfill ⁎\hfill \\ \hfill ⁎\hfill \end{array}\right)$ is the binomial coefficient.

6.2.2 Separated Saturation with Integral Part Controller – SSIP

The following controller stabilizes system (6.6) globally asymptotically:

$u=-{\sigma }_{b_{n^{\prime }}}(z_{n^{\prime }})-{\sigma }_{b_{n^{\prime }-1}}(z_{n^{\prime }-1})-\cdots -{\sigma }_{b_1}(z_1),$

where

$z_i=\sum _{j=0}^{n^{\prime }-i}\left(\begin{array}{c}\hfill n^{\prime }-i\hfill \\ \hfill j\hfill \end{array}\right)y_{n^{\prime }-j},\forall i\in [1,n^{\prime }].$

Stability analysis is similar to that proposed for the NSIP algorithm, the main difference is the saturation constraints required to perform the Lyapunov analysis and defined as

$b_i⩾b_{i-1}+b_{i-2}+\cdots +b_1.$

6.2.3 Saturated State Feedback with Integral Part Controller – SSFIP

The following controller can be seen as a PID algorithm with bounded states. The algorithm has the form

$u_{\mathit{SSFIP}}=-\sum _{j=1}^n{\sigma }_j(k_j{x}_j)+{\sigma }_{\int b_1}\left(\underset{0}{\overset{t}{\int }}{x}_1\right),$

where $x_i$ denotes the state i and $k_j>0$ is the constant gain controller. This control law stabilizes system (6.5) globally asymptotically. For the stability analysis, we rewrite (6.12) in the following form:

$u_{\mathit{SSFIP}}=-\sum _{i=1}^{n^{\prime }}{\sigma }_{b_i}(k_i{y}_i).$

Rewrite (6.13) for $i=n^{\prime }$ as

$u_{\mathit{SSFIP}}=-{\sigma }_{b_{n^{\prime }}}(k_{n^{\prime }}{y}_{n^{\prime }})-{\xi }_{n^{\prime }}$

with ${\xi }_n^{\prime }=\sum _{j=1}^{n^{\prime }-1}{\sigma }_{b_j}(k_j{y}_j)$ and define

$\begin{array}{ccc}\hfill z_{n^{\prime }}& \hfill =\hfill & y_{n^{\prime }},\hfill \\ \hfill z_i& \hfill =\hfill & k_{i+1}{y}_i+z_{i+1},\forall i\in [1,n^{\prime }-1].\hfill \end{array}$

Propose $V_{n^{\prime }}=\frac{1}{2}{z}_{n^{\prime }}^2$, and then ${\dot{V}}_{n^{\prime }}=-y_{n^{\prime }}({\sigma }_{b_{n^{\prime }}}(k_{n^{\prime }}{y}_{n^{\prime }})+{\xi }_{n^{\prime }})$. Assuming that $b_{n^{\prime }}>b_{{\xi }_{n^{\prime }}}$, we get $|k_{n^{\prime }}{y}_{n^{\prime }}|>b_{{\xi }_{n^{\prime }}}$ which implies that ${\dot{V}}_{n^{\prime }}<0$. Thus, there exist a time $T_1$ such that $\forall t>T_1$, $|y_{n^{\prime }}|⩽\frac{b_{{\xi }_{n^{\prime }}}}{k_{n^{\prime }}}$ and, as a consequence, $u_{\mathit{SSFIP}}=-k_{n^{\prime }}{y}_{n^{\prime }}-{\xi }_{n^{\prime }}$.

It is assumed that for a given l, (6.13) is true and $|y_l|$ is bounded $\forall l\in [i,n^{\prime }]$ and $i\ne 1$. Now, we prove that (6.13) is also true for $i-1$. Take $V_{i-1}=\frac{1}{2}{z}_{i-1}^2$, then ${\dot{V}}_{i-1}=z_{i-1}{\dot{z}}_{i-1}$. We have that $z_{i-1}-z_{n^{\prime }}=\sum _{j=i-1}^{n^{\prime }-1}{k}_{j+1}{y}_j$, and then $z_{i-1}=\sum _{j=i-1}^{n^{\prime }-1}{k}_{j+1}{y}_j+y_{n^{\prime }}$. Therefore ${\dot{z}}_{i-1}=-{\xi }_i$.

The above implies

${\dot{V}}_{i-1}=-(k_i{y}_{i-1}+\sum _{j=i}^{n^{\prime }-1}{k}_{j+1}{y}_j+y_{n^{\prime }})({\sigma }_{b_{i-1}}(k_{i-1}{y}_{i-1})+{\xi }_{i-1}).$

Assuming $b_{i-1}>b_{{\xi }_{i-1}}$ gives $|y_{i-1}|>\frac{b_{{\xi }_{i-1}}}{k_{i-1}}$, hence $\text{sgn}({\sigma }_{b_{i-1}}(k_{i-1}{y}_{i-1})+{\xi }_{i-1})=\text{sgn}(y_{i-1})$. Notice that $(\sum _{j=i}^{n^{\prime }-1}{k}_{j+1}{y}_j+y_{n^{\prime }})=z_i$ and $z_i$ is bounded. Thus, if $|k_i{y}_{i-1}|>b_{z_i}$, then $\text{sgn}(k_i{y}_{i-1}+z_i)=\text{sgn}(y_{i-1})$. Therefore ${\dot{V}}_{i-1}<0$ and $\exists T_{n^{\prime }-i+2}>T_{n^{\prime }-i+1}$ such that $\forall t>T_{n^{\prime }-i+2}$,

$\begin{array}{ccc}\hfill \left|y_{i-1}\right|& \hfill <\hfill & \frac{b_{{\xi }_{i-1}}}{k_{i-1}},\forall i\in [2,n^{\prime }],\hfill \\ \hfill \left|y_1\right|& \hfill <\hfill & \frac{b_1}{k_1}.\hfill \end{array}$

Since it was proved that (6.13) is true for $i-1$, using the recurrence theorem, the same is also true $\forall i\in [1,n^{\prime }]$. This means that for $i=1$ there exists a time $T_{n^{\prime }}$ such that

$u=-\sum _{i=1}^{n^{\prime }}{k}_i{y}_i,\forall t>T_{n^{\prime }}.$

The previous equations hold if the following conditions are satisfied: $\frac{b_{{\xi }_i}}{k_i}>\frac{b_{z_{i+1}}}{k_{i+1}}$, $\forall i\in [2,n^{\prime }-1]$; $\frac{b_1}{k_1}>\frac{b_{z_2}}{k_2}$, $b_i>b_{{\xi }_i}$, $\forall i\in [2,n^{\prime }]$; $b_{x_{1_i}}=\sum _{j=2}^i{b}_{j-1}$, $b_{z_i}=\sum _{j=i}^{n^{\prime }-1}\frac{k_{j+1}{b}_{{\xi }_j}}{k_j}+\frac{b_{{\xi }_{n^{\prime }}}}{k_{n^{\prime }}}$.

Observe that for $t>T_{n^{\prime }}$ the closed-loop system is $\dot{\mathbf{y}}(t)=\overline{A}\mathbf{y}(t)$ and, as $\overline{A}$ is Hurwitz, stability is proved.

The previous stability analysis demonstrated the convergence of the states to zero, $y_i\to 0$, implying $x_i\to 0$. Nevertheless, we can consider system (6.6) as an error system such that $y_1=\underset{0}{\overset{t}{\int }}{e}_1,y_2=e_1,\dots ,y_{n^{\prime }}=e_n$, with $e_1=x_1-x_{1_d},\dots ,e_n=x_n-x_{n_d}$; and then this implies that $e_i\to 0$ and $x_1\to x_{1_d},\dots ,x_n\to x_{n_d}$. The previous holds for $x_{i_d}$ constant or bounded $|x_{i_d}|⩽b_i$.

More details about the previous controllers can be found in [2].

6.2.4 Validation in a Quadcopter Vehicle

The control algorithms were validated in simulations using the simplified quadcopter model described in Sect. 5.1.5 of Chap. 5. To develop the algorithms, the following hypotheses were used:

• $m,I_j=1$

• quasi-stationary movements

• no external and unknown disturbances present

Notice from the equations in the simplified model that the altitude and the attitude can be represented by two integrators in cascade as

$\ddot{z}=u_z,\ddot{\psi }=u_{\psi },$

where z is the altitude of the aerial vehicle, ψ the yaw angle, $u_{\psi }$ defines the control input for the yaw angle, and $u_z=u\cos (\theta )\cos (\varphi )-g$, where $\theta ,\varphi$ denote the pitch and roll angles, g expresses the gravitational acceleration, and u represents the main control input.

Due to chapter-length limitation, we only use the SSFIP algorithm; similar results can be obtained using the NSIP and SSIP algorithms. Therefore the control scheme to stabilize the previous system takes the form

$u_{\gamma }:=-{\sigma }_{\dot{\gamma }}\left(k_{3_{\gamma }}\dot{\gamma }\right)-{\sigma }_{\gamma }\left(k_{2_{\gamma }}\gamma \right)-{\sigma }_{e_{\gamma }}\left(k_{1_{\gamma }}\underset{0}{\overset{t}{\int }}{e}_{\gamma }dt\right),\gamma :z,\psi .$

Once the altitude and the yaw movement are stabilized, the next step is to stabilize the longitudinal and lateral dynamics. They are typically coupled and both have high impact on the complete vehicle dynamics. Thus their classical simple representation is denoted by four integrators in cascade in the following form

$\begin{array}{cc}\hfill \ddot{s}& \approx \tan (\rho ),\hfill \\ \hfill \ddot{\rho }& \approx u_{\rho },\hfill \end{array}$

where s defines the state x or y, and ρ represents the angle θ or ϕ, respectively. Notice that the controller needs to be developed for a linear system. Then linearizing the above, it follows that $\ddot{s}\approx \rho$. Using the proposed methodology, the controller SSFIP for the previous system is

$u_{\rho }:=-{\sigma }_{\dot{\rho }}(k_{5_{\rho }}\dot{\rho })-{\sigma }_{\rho }(k_{4_{\rho }}\rho )-{\sigma }_{\dot{s}}(k_{3_{\rho }}\dot{s})-{\sigma }_s(k_{2_{\rho }}s)-{\sigma }_{e_s}\left(k_{1_{\rho }}\underset{0}{\overset{t}{\int }}{e}_{s}dt\right)$

with $e_s=s-s_{\mathit{ref}}$ and $s_{\mathit{ref}}$ being the desired position. Remark that for the longitudinal model $\ddot{x}\approx -\tan (\theta )$, and consequently for the longitudinal case, the last three terms of the above expression are positive.

6.2.4.1 Simulation Results

The simulation objective is to accurately follow a desired trajectory in the presence of uncertainties (white noise) in the states $x,y$, and z. The following graphs introduce good behavior when applying the SSFIP control to the quadcopter system, see Figs. 6.4–6.7. Notice the good performance of the closed-loop system, the boundedness of the states, and the smallness of control responses. Observe also that only the θ and ϕ angles are affected by the uncertainties; this fact is normal because the noise in the x or y axis produces uncertainties in the longitudinal or lateral dynamics. The desired altitude is $z_d=20\text{ m}$, and in the horizontal plane the desired trajectory is to follow a square trajectory with 6 m on each side. The initial conditions were zero for all the states except for the yaw angle which was $\psi (0)=10$ grad.

6.3.1 Quadcopter IAB Algorithm

To stabilize the quadcopter, let us define the position error as follows:

$e_{\xi }=\xi -{\xi }_{\mathit{ref}}⟹{\dot{e}}_{\xi }=\dot{\xi }-{\dot{\xi }}_{\mathit{ref}}=v-{\dot{\xi }}_{\mathit{ref}}.$

Now propose a positive definite function to design a virtual velocity $v^v$ that will ensure the convergence to the desired position⁵

$V_{Lr}=\frac{1}{2}({\mathbb{K}}_{Ir}{\chi }_1,{\chi }_1)+\frac{1}{2}(e_{\xi },e_{\xi })$

where $(\cdot ,\cdot )$ represents the inner vector product, ${\mathbb{K}}_{Ir}$ is a positive constant matrix, and ${\chi }_1={\int }_0^t{e}_{\xi }d\tau$. Differentiating $V_{Lr}$ results in ${\dot{V}}_{Lr}=({\mathbb{K}}_{Ir}{\chi }_1,e_{\xi })+(e_{\xi },\dot{e_{\xi }})$, and taking the virtual velocity as $v^v={\dot{\xi }}_{\mathit{ref}}-{\mathbb{K}}_{Ir}{\chi }_1-{\mathbb{K}}_r{e}_{\xi }$, with ${\mathbb{K}}_r$ a positive constant matrix, gives

${\dot{V}}_{Lr}{|}_{v=v^v}=-({\mathbb{K}}_r{e}_{\xi },e_{\xi })⩽0\forall t⩾0.$

Define the velocity error as

$e_v=v-v^v⟹\dot{e_v}=\dot{v}-{\dot{v}}^v=\frac{1}{m}(\mathbf{u}+{\delta }_u)-{\dot{v}}^v$

with $\mathbf{u}=R\hat{F}+f_g$. Now a positive definite function is proposed to ensure convergence of the position error to zero

$V_{Lv}=V_{Lr}+\frac{1}{2}(e_v,e_v)⟹{\dot{V}}_{Lv}={\dot{V}}_{Lr}+(e_v,\dot{e_v}).$

From the velocity error definition, it follows that $v=v^v+e_v$, and then the above can be expressed as ${\dot{V}}_{Lv}=-({\mathbb{K}}_r{e}_{\xi },e_{\xi })+(e_{\xi },e_v)+(e_v,{\dot{e}}_v)$, by choosing

$\mathbf{u}=-{\hat{\delta }}_u+m({\dot{v}}^v-e_{\xi }-{\mathbb{K}}_v{e}_v)$

with ${\mathbb{K}}_v$ denoting a positive constant matrix and ${\hat{\delta }}_u$ the estimated value of ${\delta }_u$. Rewriting ${\dot{V}}_{Lv}$, it follows that

${\dot{V}}_{Lv}=-({\mathbb{K}}_r{e}_{\xi },e_{\xi })-({\mathbb{K}}_v{e}_v,e_v)+\frac{1}{m}(e_v,e_{{\delta }_u})$

with $e_{{\delta }_u}={\delta }_u-{\hat{\delta }}_u$. Suppose that ${\delta }_u$ is constant, thus ${\dot{e}}_{{\delta }_u}=-{\dot{\hat{\delta }}}_u$. Consider the following candidate Lyapunov function $V_{Lu}=V_{Lv}+\frac{1}{2}({{\mathrm{\Gamma }}_1}^{-1}{e}_{{\delta }_u},e_{{\delta }_u})$, with ${\mathrm{\Gamma }}_1$ being a positive diagonal constant matrix. Then

${\dot{V}}_{Lu}=-({\mathbb{K}}_r{e}_{\xi },e_{\xi })-({\mathbb{K}}_v{e}_v,e_v)+\frac{1}{m}(e_v,e_{{\delta }_u})+({{\mathrm{\Gamma }}_1}^{-1}{e}_{{\delta }_u},-{\dot{\hat{\delta }}}_u).$

Now propose ${\dot{\hat{\delta }}}_u=\frac{1}{m}{\mathrm{\Gamma }}_1{e}_v$ as the desired dynamics for ${\hat{\delta }}_u$. Thus

${\dot{V}}_{Lu}=-({\mathbb{K}}_r{e}_{\xi },e_{\xi })-({\mathbb{K}}_v{e}_v,e_v)⩽0\forall t⩾0.$

There exists a relationship between u, the main force $\hat{F}$, and the quadcopter attitude η so that it is possible to find $(\eta ,\hat{F})$ from $\mathbf{u}={[u_x,u_y,u_z]}^T$. Here the z–y–x Euler convention is used. Consequently, the rotation matrix, R, has the following form:

$\mathbb{R}=\left(\begin{array}{ccc}\hfill c_{\psi }{c}_{\theta }\hfill & \hfill -s_{\psi }{c}_{\varphi }+c_{\psi }{s}_{\theta }{s}_{\varphi }\hfill & \hfill s_{\psi }{s}_{\varphi }+c_{\psi }{s}_{\theta }{c}_{\varphi }\hfill \\ \hfill s_{\psi }{c}_{\theta }\hfill & \hfill c_{\psi }{c}_{\varphi }+s_{\psi }{s}_{\theta }{s}_{\varphi }\hfill & \hfill -c_{\psi }{s}_{\varphi }+s_{\psi }{s}_{\theta }{c}_{\varphi }\hfill \\ \hfill -s_{\theta }\hfill & \hfill c_{\theta }{s}_{\varphi }\hfill & \hfill c_{\theta }{c}_{\varphi }\hfill \end{array}\right),$

where the c and s stand for the cosine and sine functions, respectively. From (6.19) and (6.23), the following relations for $({\eta }_{\mathit{ref}},{\hat{F}}_{\mathit{ref}})$ can be obtained:

${\theta }_{\mathit{ref}}=\arctan \left(\frac{u_y{s}_{\psi }+u_x{c}_{\psi }}{u_z+mg}\right),$

${\varphi }_{\mathit{ref}}=\arctan (c_{{\theta }_{\mathit{ref}}}\cdot \frac{u_x{s}_{\psi }-u_y{c}_{\psi }}{u_z+mg}),$

$f_{\mathit{ref}}=\frac{u_z+mg}{c_{{\theta }_{\mathit{ref}}}\cdot c_{{\varphi }_{\mathit{ref}}}}.$

${\psi }_{\mathit{ref}}$ is assigned arbitrarily, and the main thrust is taken as ${\hat{F}}_{\mathit{ref}}={[0,0,f_{\mathit{ref}}]}^T$. Let us define the error of Euler angles as

$e_{\eta }=\eta -{\eta }_{\mathit{ref}}⟹\dot{e_{\eta }}=\dot{\eta }-{\dot{\eta }}_{\mathit{ref}}=W_{\eta }^{-1}\mathrm{\Omega }-{\dot{\eta }}_{\mathit{ref}}.$

Propose the next positive definite function $V_{L\eta }=\frac{1}{2}({\mathbb{K}}_{I\eta }{\chi }_2,{\chi }_2)+\frac{1}{2}(e_{\eta },e_{\eta })$, where ${\mathbb{K}}_{I\eta }$ is a positive constant matrix and ${\chi }_2={\int }_0^t{e}_{\eta }d\tau$. Thus, ${\dot{V}}_{L\eta }=({\mathbb{K}}_{I\eta }{\chi }_2,e_{\eta })+(e_{\eta },{\dot{e}}_{\eta })$. Define the virtual angular velocity as ${\mathrm{\Omega }}^v=W_{\eta }({\dot{\eta }}_{\mathit{ref}}-{\mathbb{K}}_{I\eta }{\chi }_2-{\mathbb{K}}_{\eta }{e}_{\eta })$, with ${\mathbb{K}}_{\eta }$ being a positive constant matrix. This implies

$V_{L\eta }{|}_{\mathrm{\Omega }={\mathrm{\Omega }}^v}=-({\mathbb{K}}_{\eta }{e}_{\eta },e_{\eta })⩽0\forall t⩾0.$

Define the angular velocity error as $e_{\mathrm{\Omega }}=\mathrm{\Omega }-{\mathrm{\Omega }}^v⟹{\dot{e}}_{\mathrm{\Omega }}=\dot{\mathrm{\Omega }}-{\dot{\mathrm{\Omega }}}^v$. Remember that $\mathrm{\Omega }={\mathrm{\Omega }}^v+e_{\mathrm{\Omega }}$, then $\dot{\mathrm{\Omega }}={\mathrm{I}}^{-1}(\hat{\tau }-\mathrm{\Omega }\times \mathrm{I}\mathrm{\Omega }+{\delta }_{\tau })$. Propose the following positive definite function, $V_{L\mathrm{\Omega }}=V_{L\eta }+\frac{1}{2}(e_{\mathrm{\Omega }},e_{\mathrm{\Omega }})$, then get