四旋翼滑模控制

一、引言

之前介绍了四旋翼建模与pid双环控制器设计，本次打算分享一下四旋翼滑模控制器的设计，参考文献【1】（Jiang X Y , Su C L , Xu Y P , et al. An adaptive backstepping sliding mode method for flight attitude of quadrotor UAVs[J]. Journal of Central South University, 2018, 25(3):616-631…）

二、模型搭建

四旋翼的模型在我之前的博客（四旋翼无人机Matlab建模）已经介绍过，这里不再赘述，四旋翼的模型如下：$$\{\begin{array}{c}\ddot{x}=\frac{u_1}{m}(s\psi s\varphi +c\psi c\varphi s\theta )+d_1\\ \ddot{y}=\frac{u_1}{m}(s\psi s\theta c\varphi -c\psi s\varphi )+d_2\\ \ddot{z}=\frac{u_1}{m}c\theta c\varphi -g+d_3\\ \ddot{\varphi }=\dot{\theta }\dot{\psi }(\frac{I_{yy}-I_{zz}}{I_{xx}})+\frac{u_2}{I_{xx}}+d_4\\ \ddot{\theta }=\dot{\varphi }\dot{\psi }(\frac{I_{zz}-I_{xx}}{I_{yy}})+\frac{u_3}{I_{yy}}+d_5\\ \ddot{\psi }=\dot{\varphi }\dot{\theta }(\frac{I_{xx}-I_{yy}}{I_{zz}})+\frac{u_4}{I_{zz}}+d_6\end{array}(1)$$式中，$d_i,(i=1,2...6)$是系统扰动。
但四旋翼模型(1)经过了部分简化合并，即在力矩平衡方程中将四旋翼螺旋桨的对无人机产生影响归到$d_i$干扰项中，没有进行表示。更常用的四旋翼的动力学模型如下$$\{\begin{array}{c}\ddot{x}=\frac{u_1}{m}(s\psi s\varphi +c\psi c\varphi s\theta )+d_1\\ \ddot{y}=\frac{u_1}{m}(s\psi s\theta c\varphi -c\psi s\varphi )+d_2\\ \ddot{z}=\frac{u_1}{m}c\theta c\varphi -g+d_3\\ \ddot{\varphi }=\dot{\theta }\dot{\psi }(\frac{I_{yy}-I_{zz}}{I_{xx}})+\frac{u_2}{I_{xx}}+d_4-\frac{I_r}{I_{xx}}\dot{\theta }{\omega }_r\\ \ddot{\theta }=\dot{\varphi }\dot{\psi }(\frac{I_{zz}-I_{xx}}{I_{yy}})+\frac{u_3}{I_{yy}}+d_5-\frac{I_r}{I_{yy}}\dot{\varphi }{\omega }_r\\ \ddot{\psi }=\dot{\varphi }\dot{\theta }(\frac{I_{xx}-I_{yy}}{I_{zz}})+\frac{u_4}{I_{zz}}+d_6\end{array}(2)$$式中$${\omega }_r={\omega }_1+{\omega }_3-{\omega }_2-{\omega }_4$$$I_r$是螺旋桨旋转的转动惯量。两个模型都可以使用，如果你想表达的更准确，建议使用模型(2)。下面在simulink搭建模型(2)并为其设计滑模控制器。在建立（2）的simulink模型时，需要注意的点在于怎么获得四旋翼各个旋翼的转速，该转速可以通过$u_1,u_2,u_3,u_4$算得。由四旋翼无人机Matlab建模可知，控制u的表达式如下$$\left[\begin{array}{c}{u}_1\\ u_2\\ u_3\\ u_4\end{array}\right]=\left[\begin{array}{c}{\sum }_{j=1}^4\rho {\omega }_j^2\\ L\rho ({\omega }_4^2-{\omega }_2^2)\\ L\rho ({\omega }_1^2-{\omega }_3^2)\\ {\sum }_{j=1}^4(-1{)}^j\gamma {\omega }_j^2\end{array}\right]=B\left[\begin{array}{c}{\omega }_1^2\\ {\omega }_2^2\\ {\omega }_3^2\\ {\omega }_4^2\end{array}\right](3)$$其中$$B=\left[\begin{array}{cccc}\rho & \rho & \rho & \rho \\ 0& -L\rho & 0& L\rho \\ L\rho & 0& -L\rho & 0\\ -\gamma & \gamma & -\gamma & \gamma \end{array}\right](4)$$然后两边同时乘以$B^{-1}$，就可以得到各旋翼转速的平方，开方后得到各旋翼转速。B矩阵与四旋翼的机体坐标系方向以及控制力/力矩u的定义有关，该模块simulink模型如下：

具体代码如下

function Omegar = fcn(U1, U2, U3, U4)
%rho无人机升力系数 
%L四旋翼臂长
%gama无人机阻力系数
B = [rho rho rho rho;
    0 -L*rho 0 L*rho;
    L*rho 0 -L*rho 0;
    -gama gama -gama gama];
U = [U1 U2 U3 U4]';
BU = B\U;
Omega1 = sqrt(abs(BU(1)));
Omega2 = sqrt(abs(BU(2)));
Omega3 = sqrt(abs(BU(3)));
Omega4 = sqrt(abs(BU(4)));
Omegar  = Omega1 - Omega2 + Omega3 - Omega4;

整个模型如下：

参数选择如下：

参数名称	参数值
无人机质量m	$0.65kg$
重力加速度g	$9.8m/s^2$
无人机绕x轴转动惯量Ixx	$7.5\times 10^{-3}kg/m^2$
无人机绕y轴转动惯量Iyy	$7.5\times 10^{-3}kg/m^2$
无人机绕z轴转动惯量Izz	$1.3\times 10^{-2}kg/m^2$
无人机转子转动惯量Ir	$6\times 10^{-5}kg/m^2$
无人机臂长L	$0.23m$
无人机升力系数rho	$3.1\times 10^{-5}$
无人机阻力系数gama	$7.5\times 10^{-7}$

三、内环控制器设计

模型搭建好了后，开始设计控制器。四旋翼是个欠驱动系统，四个输入控制六个状态，状态之间存在耦合，例如当四旋翼存在俯仰角时，四旋翼的x方向一定会出现位移。因此，常见的处理方式是将四旋翼设计为姿态与位置两个环，外环控制器的到姿态的期望，再让内环控制器完成无人机控制。先设计内环控制器（暂时先不考虑干扰与不确定性）。
以无人机横滚角$\varphi$为例，令$x_1=\varphi ,x_2=\dot{\varphi }$,根据模型（2）,横滚角的子系统可以改写如下：$$\{\begin{array}{ccc}\dot{x_1}& =& x_2\\ \dot{x_2}& =& f(\dot{\theta },\dot{\psi })+\frac{u_2}{I_{xx}}\end{array}(5)$$式中$$f(\dot{\theta },\dot{\psi })=\dot{\theta }\dot{\psi }(\frac{I_{yy}-I_{zz}}{I_{xx}})-\frac{I_r}{I_{xx}}\dot{\theta }{\omega }_r(6)$$定义跟踪误差$$e=x_1-x_d(7)$$式中$x_d$是期望轨迹,定义简单滑模面如下$$s=e+\dot{e}(8)$$对滑模面进行求导$$\dot{s}=\ddot{e}+\dot{e}=\ddot{x_1}-\ddot{x_d}+\dot{x_1}-\dot{x_d}(9)$$由(5)得$$\ddot{x_1}=\dot{x_2}=f(\dot{\theta },\dot{\psi })+\frac{u_2}{I_{xx}}(10)$$将(10)带入(9)$$\dot{s}=f(\dot{\theta },\dot{\psi })+\frac{u_2}{I_{xx}}+\dot{x_1}-\ddot{x_d}-\dot{x_d}(11)$$设计$u_2$使得$\dot{s}=$趋近律，则设计$$u_2=I_{xx}(\ddot{x_d}+\dot{x_d}-f(\dot{\theta },\dot{\psi })-\dot{x_1}-s-sign(s))(12)$$代入式(11)，可以使得$s\dot{s}<0$,控制器设计完成，俯仰角与偏航角设计与横滚角相同，这里不在赘述，直接给结果。
设俯仰角$\theta$的期望跟踪信号为${\theta }_d$,则$$e=\theta -{\theta }_d$$$$\dot{e}=\dot{\theta }-\dot{{\theta }_d}$$$$s=e+\dot{e}$$$$f(\dot{\varphi },\dot{\psi })=\dot{\varphi }\dot{\psi }(\frac{I_{zz}-I_{xx}}{I_{yy}})-\frac{I_r}{I_{yy}}\dot{\varphi }{\omega }_r$$$$u_3=I_{yy}(\ddot{{\theta }_d}-f(\dot{\varphi },\dot{\psi })-\dot{e}-s-sign(s))(13)$$
同理，偏航角按同样方法设计,最终控制律为$$u_4=I_{zz}(\ddot{\psi }-f(\dot{\varphi },\dot{\theta })-\dot{e}-s-sign(s))(14)$$最终完成内环设计，横滚角控制器simulink模型如下：

matlab function中代码如下

function u2 = fcn(fand, dfand, ddfand, fan, dfan, dtheta, dpsi, Omegar, Ixx, Iyy, Izz, Ir)
e = fan -fand;
de = dfan - dfand;
s = e + de;
f = dtheta*dpsi*(Iyy-Izz)/Ixx-Ir*dtheta*Omegar/Ixx;
u2 = Ixx*(ddfand+dfand-f-dfan-s-sign(s));

假设横滚角期望轨迹为sin,跟踪效果如下:

图中黄色的为sin曲线，蓝色的为无人机$\varphi$跟踪情况（无人机俯仰与偏航角控制器可用相同方法测试），从图像可以看到内环控制器设计没有问题，接下来进行外环控制器设计。

四、外环控制器设计

外环控制器控制的为无人机位置，先来讨论比较独立z子系统。
$$\ddot{z}=\frac{u_1}{m}c\theta c\varphi -g(15)$$该子系统就是一个简单二阶系统，定义理想跟踪指令为$z_d$，跟踪误差$$e_z=z-z_d(16)$$滑模面设计为$$s_z=\dot{e_z}+e_z(17)$$则控制律设计如下$$u_1=\frac{m}{c\theta c\varphi }(g+\ddot{z_d}-\dot{e_z}-s_z-sign(s_z))(18)$$
令z跟踪曲线为sin曲线，则在该控制器下无人机z方向跟踪情况如下

黄色为期望曲线，蓝色为跟踪曲线，z方向的控制器设计完成。

水平方向的x，y子系统与$\varphi ,\theta$存在动态耦合，即x,y的变化实际由$\varphi ,\theta$控制，$u_1$只是一个状态量。因此，xy方向的状态方程可以改写如下(忽略干扰):$$\{\begin{array}{ccc}\ddot{x}& =& \frac{u_x{u}_1}{m}\\ \ddot{y}& =& \frac{u_y{u}_1}{m}\end{array}(19)$$$$u_x=s\psi s\varphi +c\psi c\varphi s\theta (20)$$$$u_y=s\psi s\theta c\varphi -c\psi s\varphi (21)$$按照滑模设计方法，可以设计出$u_x,u_y$的值$$u_x=\frac{m}{u_1}(\ddot{x_d}-e_x-s_x-sign(s_x))(22)$$其中$e_x=x-x_d$,$s_x=e_x+\dot{e_x}$,$x_d$为x期望跟踪值
$$u_y=\frac{m}{u_1}(\ddot{y_d}-e_y-s_y-sign(s_y))(23)$$根据(20),(21)可以得到姿态子系统的中间指令信号如下：$$\{\begin{array}{ccc}{\varphi }_d& =& arcsin(u_{x}sin\psi -u_{y}cos\psi )\\ {\theta }_d& =& arcsin(\frac{u_x-sin\varphi sin\psi }{cos\varphi cos\psi })\end{array}(24)$$把(22),(23)代入(24)就可以得到${\varphi }_d,{\theta }_d$
至此，外环控制器设计完成,整体simulink模型如下

仿真simulink文件链接: 四旋翼滑模控制.