滑模控制在四旋翼上的应用举例（内含公式手推、simulink框图连接、代码编写）

本文以Zongcheng Ma, Jinfu Feng, Jian Yang的论文《Research on vertical air-water trans-media control of Hybrid Unmanned Aerial Underwater Vehicles based on adaptive sliding mode dynamical sruface control》这篇文章为例，将文章中的结果进行部分复现，展示应用在四旋翼上的滑模控制。具体算法实施过程中可能会用到一部分自适应思想，但不影响理解，也欢迎大家参考笔者关于自适应的文章基于反步法backstepping的自适应控制简介，进一步了解自适应的思想。

1. 论文背景

该论文建立了如下场景：一四旋翼飞行器从固定高度开始下降，穿越水面，并进入到水下一定位置的过程。在入水过程中四旋翼受力复杂，因此需要较好的控制律进行控制。

本文只对高度通道$z$和姿态通道$\phi ,\theta ,\psi$设计控制律，目的是在复现该论文过程中，更好地体会滑模控制的思想。由于四旋翼各个通道的耦合性，姿态控制需要得到较好结果后才能进行高度控制，因此在实际应用中往往把姿态控制当做内环，位置控制作为外环，设计复合控制器或级联控制器，这也是本文针对姿态控制和高度通知均设计控制律的原因。

2. 数学建模

设各变量为
$$\begin{array}{rl}{\eta }_1& ={\left[\begin{array}{ccc}x& y& z\end{array}\right]}^T\\ {\eta }_2& ={\left[\begin{array}{ccc}\phi & \theta & \psi \end{array}\right]}^T\\ V& ={\left[\begin{array}{ccc}u& v& w\end{array}\right]}^T\\ \omega & ={\left[\begin{array}{ccc}p& q& r\end{array}\right]}^T\end{array}$$其中${\eta }_1,{\eta }_2$为地球坐标系下的物理量，$V,\omega$为机体坐标系下的物理量。（关于地球坐标系和机体坐标系可以参考文章Backstepping反步法控制四旋翼无人机（一））。

列出欧拉方程
$$\{\begin{array}{l}m{\ddot{\eta }}_1=F\\ J\dot{\omega }+\omega \times J\omega =M\end{array}$$
对于入水过程，该论文设计了一个临界区间$H$，当无人机高度位于区间内时，作用于其上的浮力$f(z)$、拉力系数$C_0(z)$、总质量（包括机身质量和附加质量）$m_i(z)$、总转动惯量$J_i(z)$、均拥有不同形式：
$$\begin{gathered} \begin{array}{r}f(z)=\{\begin{array}{l}0,z>\frac{H}{2}\\ \left(\frac{H}{2}-z\right)m_{0}g,-\frac{H}{2}\le z\le \frac{H}{2}\\ m_{0}g,z<-\frac{H}{2}\end{array}\\ C_0(z)=\{\begin{array}{l}0,z>\frac{H}{2}\\ \left(\frac{H}{2}-z\right)c_0,-\frac{H}{2}\le z\le \frac{H}{2}\\ c_0,z<-\frac{H}{2}\end{array}\\ m_i(z)=\{\begin{array}{l}{m}_0,z>\frac{H}{2}\\ \left(\frac{H}{2}-z\right)\left(m_0+{\lambda }_{ii}\right),-\frac{H}{2}\le z\le \frac{H}{2}\\ m_0+{\lambda }_{ii},z<-\frac{H}{2}\end{array}\end{array} \\ {J}_i(z)=\{\begin{array}{l}{J}_{ii},z>\frac{H}{2}\\ \left(\frac{H}{2}-z\right)\left(J_{ii}+{\lambda }_{ii}\right),-\frac{H}{2}\le z\le \frac{H}{2}\\ J_{ii}+{\lambda }_{ii},z<-\frac{H}{2}\end{array} \end{gathered}$$

那么总的运动公式可以表示为
$$\{\begin{array}{l}{m}_3{\ddot{z}}_1=U_1\cos \phi \cos \theta -m_{0}g+f(z)-C_0\dot{z}+d_f+d_k+d_l\\ J{\ddot{\eta }}_2=\Gamma -C{\dot{\eta }}_2+d_{\Gamma }\end{array}\text{\hspace{1em}(0)}$$其中：
$m_3=m_0+\Delta$，第二项$\Delta$为附加质量和未建模的干扰项的和；
$\Gamma ={\left[\begin{array}{ccc}{U}_2& U_3& U4\end{array}\right]}^T$为控制向量；
$C=l\times diag\left(K_p,M_q,N_r\right)$为科里奥利力；
$d_f,d_k,d_{\Gamma }$分别为浮力线性化的误差、阻力线性化的误差、外部干扰。

3. 姿态控制器设计

设跟踪误差为
$$\begin{array}{rl}{e}_{{\eta }_2}& ={\eta }_2-{\eta }_{2d}\\ & =\left[\begin{array}{c}\phi \\ \theta \\ \psi \end{array}\right]-\left[\begin{array}{c}{\phi }_d\\ {\theta }_d\\ {\psi }_d\end{array}\right]\end{array}$$求二阶导
$${\ddot{e}}_{{\eta }_2}={\ddot{\eta }}_2-{\ddot{\eta }}_{2d}=J^{-1}\left(\Gamma -C{\dot{\eta }}_2+d_{\Gamma }\right)-{\ddot{\eta }}_{2d}$$
设计滑模面为
$$s_1={\dot{e}}_{{\eta }_2}+{\lambda }_1{e}_{{\eta }_2}\text{\hspace{1em}(1)}$$此时如果把控制量设计为
$$\{\begin{array}{l}\Gamma =\hat{J}\bar{\Gamma }+C{\dot{\eta }}_2-{\hat{d}}_{\Gamma }\\ \bar{\Gamma }={\ddot{\eta }}_{2d}-{\lambda }_1{\dot{e}}_{{\eta }_2}-c_1{s}_1\end{array}\text{\hspace{1em}(3)}$$亦即
$$\{\begin{array}{l}\Gamma =\hat{J}\bar{\Gamma }+C{\dot{\eta }}_2-{\hat{d}}_{\Gamma }\\ {\lambda }_1{\dot{e}}_{{\eta }_2}={\ddot{\eta }}_{2d}-c_1{s}_1-\bar{\Gamma }\end{array}\text{\hspace{1em}(4)}$$
则对(1)求导，并代入(4)有
$$\begin{array}{rl}{\dot{s}}_1& ={\ddot{e}}_{{\eta }_2}+{\lambda }_1{\dot{e}}_{{\eta }_2}\\ & =J^{-1}\left(\Gamma -C{\dot{\eta }}_2+d_{\Gamma }\right)-{\ddot{\eta }}_{2d}+{\lambda }_1{\dot{e}}_{{\eta }_2}\\ & =J^{-1}\left(\Gamma -C{\dot{\eta }}_2+d_{\Gamma }\right)-{\ddot{\eta }}_{2d}+{\ddot{\eta }}_{2d}-c_1{s}_1-\bar{\Gamma }\\ & =J^{-1}\left[\left(\hat{J}\bar{\Gamma }+C{\dot{\eta }}_2-{\hat{d}}_{\Gamma }\right)-C{\dot{\eta }}_2+d_{\Gamma }\right]-c_1{s}_1-\bar{\Gamma }\\ & =J^{-1}\left(\hat{J}\bar{\Gamma }-{\hat{d}}_{\Gamma }+d_{\Gamma }\right)-c_1{s}_1-\bar{\Gamma }\end{array}\text{\hspace{1em}(5)}$$
下面设计自适应律：
$$\{\begin{array}{l}{\dot{\hat{d}}}_{\Gamma }={\gamma }_1{s}_1\\ \dot{\hat{J}}=-{\gamma }_2\bar{\Gamma }{s}_1^T\end{array}\text{\hspace{1em}(6)}$$并设计李雅普诺夫函数为以下形式
$$V_1=\frac{1}{2}{s}_1^T{s}_1+\frac{1}{2{\gamma }_1}{\tilde{d}}_{\Gamma }^T{J}^{-1}{\tilde{d}}_{\Gamma }+\frac{1}{2{\gamma }_2}tr\left({\tilde{J}}^T{J}^{-1}\tilde{J}\right)\text{\hspace{1em}(7)}$$其中
$${\tilde{d}}_{\Gamma }=d_{\Gamma }-{\hat{d}}_{\Gamma },\tilde{J}=J-\hat{J}\text{\hspace{1em}(8)}$$对李雅普诺夫函数求导：
$${\dot{V}}_1=s_1^T{\dot{s}}_1+\frac{1}{{\gamma }_1}{\tilde{d}}_{\Gamma }^T{J}^{-1}{\dot{\tilde{d}}}_{\Gamma }+\frac{1}{{\gamma }_2}tr\left({\tilde{J}}^T{J}^{-1}\dot{\tilde{J}}\right)\text{\hspace{1em}(9)}$$设$d_{\Gamma }$和$J$变化缓慢，那么由(8)
$$\begin{array}{rl}\dot{{\tilde{d}}_{\Gamma }}& ={\dot{d}}_{\Gamma }-{\dot{\hat{d}}}_{\Gamma }=-{\dot{\hat{d}}}_{\Gamma }\\ \dot{\tilde{J}}& =\dot{J}-\dot{\hat{J}}=-\dot{\hat{J}}\end{array}\text{\hspace{1em}(10)}$$把(10)代入(9)，可得到下式(11)
$$\begin{array}{rl}{\dot{V}}_1& =s_1^T{\dot{s}}_1+\frac{1}{{\gamma }_1}{\tilde{d}}_{\Gamma }^T{J}^{-1}{\dot{\tilde{d}}}_{\Gamma }+\frac{1}{{\gamma }_2}tr\left({\tilde{J}}^T{J}^{-1}\dot{\tilde{J}}\right)\\ & =s_1^T{\dot{s}}_1-\frac{1}{{\gamma }_1}{\tilde{d}}_{\Gamma }^T{J}^{-1}{\dot{\hat{d}}}_{\Gamma }-\frac{1}{{\gamma }_2}tr\left({\tilde{J}}^T{J}^{-1}\dot{\hat{J}}\right)\\ & =s_1^T\left[J^{-1}\left(\hat{J}\bar{\Gamma }-{\hat{d}}_{\Gamma }+d_{\Gamma }\right)-c_1{s}_1-\bar{\Gamma }\right]-\frac{1}{{\gamma }_1}{\tilde{d}}_{\Gamma }^T{J}^{-1}{\dot{\hat{d}}}_{\Gamma }-\frac{1}{{\gamma }_2}tr\left({\tilde{J}}^T{J}^{-1}\dot{\hat{J}}\right)\\ & =-c_1{s}_1^T{s}_1+s_1^T{J}^{-1}\hat{J}\bar{\Gamma }+\left(-s_1^T{J}^{-1}{\hat{d}}_{\Gamma }+s_1^T{J}^{-1}{d}_{\Gamma }\right)-s_1^T\bar{\Gamma }-\frac{1}{{\gamma }_1}{\tilde{d}}_{\Gamma }^T{J}^{-1}{\dot{\hat{d}}}_{\Gamma }-\frac{1}{{\gamma }_2}tr\left({\tilde{J}}^T{J}^{-1}\dot{\hat{J}}\right)\\ & =-c_1{s}_1^T{s}_1+s_1^T{J}^{-1}\hat{J}\bar{\Gamma }+s_1^T{J}^{-1}{\tilde{d}}_{\Gamma }-s_1^T\bar{\Gamma }-\frac{1}{{\gamma }_1}{\tilde{d}}_{\Gamma }^T{J}^{-1}{\dot{\hat{d}}}_{\Gamma }-\frac{1}{{\gamma }_2}tr\left({\tilde{J}}^T{J}^{-1}\dot{\hat{J}}\right)\\ & =-c_1{s}_1^T{s}_1+s_1^T{J}^{-1}\hat{J}\bar{\Gamma }+s_1^T{J}^{-1}{\tilde{d}}_{\Gamma }-\left(s_1^T{J}^{-1}J\bar{\Gamma }\right)-\frac{1}{{\gamma }_1}{\tilde{d}}_{\Gamma }^T{J}^{-1}{\dot{\hat{d}}}_{\Gamma }-\frac{1}{{\gamma }_2}tr\left({\tilde{J}}^T{J}^{-1}\dot{\hat{J}}\right)\\ & =-c_1{s}_1^T{s}_1+s_1^T{J}^{-1}\hat{J}\bar{\Gamma }-\frac{1}{{\gamma }_1}{\tilde{d}}_{\Gamma }^T{J}^{-1}{\dot{\hat{d}}}_{\Gamma }-\frac{1}{{\gamma }_2}tr\left({\tilde{J}}^T{J}^{-1}\dot{\hat{J}}\right)-s_1^T{J}^{-1}\left(J-\hat{J}\right)\bar{\Gamma }\\ & =-c_1{s}_1^T{s}_1+s_1^T{J}^{-1}\hat{J}\bar{\Gamma }-\frac{1}{{\gamma }_1}{\tilde{d}}_{\Gamma }^T{J}^{-1}{\dot{\hat{d}}}_{\Gamma }-\frac{1}{{\gamma }_2}tr\left({\tilde{J}}^T{J}^{-1}\dot{\hat{J}}\right)-s_1^T{J}^{-1}\tilde{J}\bar{\Gamma }\end{array}$$下面引入3个性质：
对于${\tilde{d}}_{\Gamma }$与${\tilde{d}}_{\Gamma }^T$有
$${\tilde{d}}_{\Gamma }^T{J}^{-1}{\dot{\hat{d}}}_{\Gamma }={\dot{\hat{d}}}_{\Gamma }^T{J}^{-1}{\tilde{d}}_{\Gamma }$$且
$$s_1^T{J}^{-1}\hat{J}\bar{\Gamma }=tr\left(J^{-1}\tilde{J}\bar{\Gamma }{s}_1^T\right)$$且
$$tr\left({\tilde{J}}^T{J}^{-1}\dot{\hat{J}}\right)=tr\left(J^{-1}\tilde{J}\dot{\hat{J}}\right)$$则代入(1)有
$$\begin{array}{rl}{\dot{V}}_1& =-c_1{s}_1^T{s}_1+s_1^T{J}^{-1}\hat{J}\bar{\Gamma }-\frac{1}{{\gamma }_1}{\tilde{d}}_{\Gamma }^T{J}^{-1}{\dot{\hat{d}}}_{\Gamma }-\frac{1}{{\gamma }_2}tr\left({\tilde{J}}^T{J}^{-1}\dot{\hat{J}}\right)-s_1^T{J}^{-1}\tilde{J}\bar{\Gamma }\\ & =-c_1{s}_1^T{s}_1+s_1^T{J}^{-1}\hat{J}\bar{\Gamma }-\frac{1}{{\gamma }_1}{\dot{\hat{d}}}_{\Gamma }^T{J}^{-1}{\tilde{d}}_{\Gamma }-\frac{1}{{\gamma }_2}tr\left(J^{-1}\tilde{J}\dot{\hat{J}}\right)-tr\left(J^{-1}\tilde{J}\bar{\Gamma }{s}_1^T\right)\\ & =-c_1{s}_1^T+\left(s_1^T{J}^{-1}-\frac{1}{{\gamma }_1}{\dot{\hat{d}}}_{\Gamma }^T{J}^{-1}\right){\tilde{d}}_{\Gamma }-tr\left(J^{-1}\tilde{J}\bar{\Gamma }{s}_1^T+\frac{1}{{\gamma }_2}{J}^{-1}\tilde{J}\dot{\hat{J}}\right)\\ & =-c_1{s}_1^T+\left(s_1^T{J}^{-1}-\frac{1}{{\gamma }_1}{\dot{\hat{d}}}_{\Gamma }^T{J}^{-1}\right){\tilde{d}}_{\Gamma }-tr\left[J^{-1}\tilde{J}\left(\bar{\Gamma }{s}_1^T+\frac{1}{{\gamma }_2}\dot{\hat{J}}\right)\right]\\ & =-c_1{s}_1^T+\left(s_1^T{J}^{-1}-\frac{1}{{\gamma }_1}{\dot{\hat{d}}}_{\Gamma }^T{J}^{-1}\right){\tilde{d}}_{\Gamma }-\left(\bar{\Gamma }{s}_1^T+\frac{1}{{\gamma }_2}\dot{\hat{J}}\right)J^{-1}\tilde{J}\end{array}$$再代入自适应律(6)
$$\begin{array}{rl}{\dot{V}}_1& =-c_1{s}_1^T+\left(s_1^T{J}^{-1}-\frac{1}{{\gamma }_1}{\dot{\hat{d}}}_{\Gamma }^T{J}^{-1}\right){\tilde{d}}_{\Gamma }-\left(\bar{\Gamma }{s}_1^T+\frac{1}{{\gamma }_2}\dot{\hat{J}}\right)J^{-1}\tilde{J}\\ & =-c_1{s}_1^T+\left[s_1^T{J}^{-1}-\frac{1}{{\gamma }_1}\left({\gamma }_1{s}_1^T\right)J^{-1}\right]{\tilde{d}}_{\Gamma }-\left[\bar{\Gamma }{s}_1^T-\frac{1}{{\gamma }_2}\left({\gamma }_2\bar{\Gamma }{s}_1^T\right)\right]J^{-1}\tilde{J}\\ & =-c_1{s}_1^T{s}_1\le 0\end{array}$$即满足李雅普诺夫稳定性。

4. 高度控制器

设计滑模面为
$$s_2={\dot{e}}_z+{\lambda }_2{e}_z$$其中$e_z=z-z_d$。
对其求导
$${\dot{s}}_2={\ddot{e}}_z+{\lambda }_2{\dot{e}}_z=\ddot{z}-{\ddot{z}}_d+{\lambda }_2{\dot{e}}_z$$
又因为
$$m_3{\ddot{z}}_1=U_1\cos \phi \cos \theta -m_{0}g+f(z)-C_0\dot{z}+\rho$$因此
$$\ddot{z}=\frac{U_1}{m_3}\cos \phi \cos \theta -\frac{m_0}{m_3}g+\frac{f(z)}{m_3}-\frac{C_0}{m_3}\dot{z}+\frac{\rho }{m_3}$$
因此，
$${\dot{s}}_2=\frac{U_1}{m_3}\cos \phi \cos \theta -\frac{m_0}{m_3}g+\frac{f(z)}{m_3}-\frac{C_0}{m_3}\dot{z}+\frac{\rho }{m_3}-{\ddot{z}}_d+{\lambda }_2{\dot{e}}_z$$
参考公式(0)，可以设计控制量$U_1$为
$$U_1=\frac{{\hat{m}}_{3}L+m_{0}g-f(z)+C_0\dot{z}-\hat{\rho }}{\cos \phi \cos \theta }\text{\hspace{1em}(12)}$$并且其中
$$L=-\tau s_2-k\cdot sgn\left(s_2\right)+{\ddot{z}}_d-{\lambda }_2{\dot{e}}_z\text{\hspace{1em}(13)}$$$\rho$为干扰。
设计自适应律
$$\{\begin{array}{l}{\dot{\hat{m}}}_3=-k_{1}L{s}_2\\ \dot{\hat{\rho }}=k_2{s}_2\end{array}\text{\hspace{1em}(14)}$$
以及李雅普诺夫函数
$$V_2=\frac{1}{2}{s}_2^2+\frac{1}{2m_3{k}_1}{\tilde{m}}_3^2+\frac{1}{2m_3{k}_2}{\tilde{\rho }}^2\text{\hspace{1em}(15)}$$求导有
$${\dot{V}}_2=s_2{\dot{s}}_2+\frac{1}{m_3{k}_1}{\tilde{m}}_3{\dot{\tilde{m}}}_3+\frac{1}{m_3{k}_2}\tilde{\rho }\dot{\tilde{\rho }}\text{\hspace{1em}(16)}$$同样认为$m_3,\rho$变化缓慢，参考(10)有
$${\dot{\tilde{m}}}_3=-{\dot{\hat{m}}}_3,\dot{\tilde{\rho }}=-\dot{\hat{\rho }}\text{\hspace{1em}(17)}$$那么有式(18)
$$\begin{array}{rl}{\dot{V}}_2& =s_2{\dot{s}}_2+\frac{1}{m_3{k}_1}{\tilde{m}}_3{\dot{\tilde{m}}}_3+\frac{1}{m_3{k}_2}\tilde{\rho }\dot{\tilde{\rho }}\\ & =s_2\left(\frac{U_1}{m_3}\cos \phi \cos \theta -\frac{m_0}{m_3}g+\frac{f(z)}{m_3}-\frac{C_0}{m_3}\dot{z}+\frac{\rho }{m_3}-{\ddot{z}}_d+{\lambda }_2{\dot{e}}_z\right)-\frac{1}{m_3{k}_1}{\tilde{m}}_3{\dot{\hat{m}}}_3-\frac{1}{m_3{k}_2}\tilde{\rho }\dot{\hat{\rho }}\end{array}$$再把式(12)$U_1$的表达式代入，并考虑自适应律(14)
$$\begin{array}{rl}{\dot{V}}_2& =s_2\left(\frac{{\hat{m}}_3}{m_3}L+\frac{\tilde{\rho }}{m_3}-{\ddot{z}}_d+{\lambda }_2{\dot{e}}_z\right)+\frac{1}{m_3{k}_1}{\tilde{m}}_3{k}_{1}L{s}_2-\frac{1}{m_3{k}_2}\tilde{\rho }{k}_2{s}_2\\ & =\frac{\tilde{\rho }}{m_3}{s}_2-{\ddot{z}}_d{s}_2+{\lambda }_2{s}_2{\dot{e}}_z+\frac{{\hat{m}}_3}{m_3}L{s}_2-\frac{1}{m_3}\tilde{\rho }{s}_2+\frac{{\hat{m}}_3}{m_3}L{s}_2\end{array}$$代入式(13)
$$\begin{array}{rl}{\dot{V}}_2& =-{\ddot{z}}_d{s}_2+{\lambda }_2{s}_2{\dot{e}}_z+s_2\left(-\tau s_2-k\cdot sgn\left(s_2\right)+{\ddot{z}}_d-{\lambda }_2{\dot{e}}_z\right)\\ & =-{\ddot{z}}_d{s}_2+{\lambda }_2{s}_2{\dot{e}}_z-\tau s_2^2-k|s_2|+s_2{\ddot{z}}_d-{\lambda }_2{s}_2{\dot{e}}_z\\ & =-\tau s_2^2-k|s_2|\le 0\end{array}$$亦满足李雅普诺夫稳定性！

5. simulink仿真

搭建simulink框图如下

其中各个模块内部如下：

1. s1模块
   
2. s2模块
   
3. J_hat, dgamma_hat模块
   
4. dgamma模块
   
5. m3_hat, rho_hat模块
   
6. Attitude Control模块
   
7. Altitude Control模块
   
8. dot_eta2模块
   
9. MathModel模块
   

6. 仿真结果

仿真结果如下。

1. 姿态通道
   
2. 高度通道
   
3. 控制量
   
4. 估计转动惯量$J_i$与估计质量${\hat{m}}_3$
   
5. 高度误差
   
6. 估计扰动$d_{\Gamma }$
   

7. 结论

可以看出：

1. 自适应滑模控制可以很好地控制四旋翼飞行稳定性。
2. 该方法并不能很好估计扰动$d_{\Gamma }$、总质量$m_3$、转动惯量$J_i$，估计值与真实值有较大差别。但是估计值均为有界值，且能维持飞行稳定。
3. 由于控制量$U_1$表达式中的$L$含有符号项$sgn$，因此当调节过程末尾曲线趋于稳定时，真实值会在期望值附近上下摆动，引发$sgn$符号在正负之间来回切换，该切换高频且振荡，因此$U_1$为一高频振荡的信号。这也是滑模控制的一个缺点，当实际硬件无法承受这种高频振荡时，往往会无法满足控制需求，进而引发硬件宕机或工作失稳。