固定翼飞机姿态角Backstepping反步法控制

在文章固定翼飞机数学建模入门（姿态角篇）中，笔者已经介绍了固定翼飞机在姿态角$\phi ,\theta ,\psi$通道上的数学建模。本文将以该建模为基础选择控制量，设计控制算法，实现对固定翼飞机姿态角的Backstepping反步法控制。

1. 数学建模

这里不加推导地引用固定翼飞机数学建模入门（姿态角篇）中的结论：
$$\left[\begin{array}{c}\dot{p}\\ \dot{q}\\ \dot{r}\end{array}\right]=\Lambda \cdot \left[\begin{array}{c}\bar{q}Sb{E}_L-J_{zx}pq-J_{z}qr+J_{y}qr\\ \bar{q}S\bar{c}{E}_M-J_{x}pr-J_{xz}{r}^2+J_{zx}{p}^2+J_{z}pr\\ \bar{q}Sb{E}_N-J_{y}pq+J_{x}pq+J_{xz}qr\end{array}\right]\text{\hspace{1em}(1)}$$在飞机各欧拉角不大的情况下，有
$$\left[\begin{array}{c}p\\ q\\ r\end{array}\right]\approx \left[\begin{array}{c}\dot{\phi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]$$因而$(1)$式即$$\left[\begin{array}{c}\ddot{\phi }\\ \ddot{\theta }\\ \ddot{\psi }\end{array}\right]=\Lambda \cdot \left[\begin{array}{c}\bar{q}Sb{E}_L-J_{zx}pq-J_{z}qr+J_{y}qr\\ \bar{q}S\bar{c}{E}_M-J_{x}pr-J_{xz}{r}^2+J_{zx}{p}^2+J_{z}pr\\ \bar{q}Sb{E}_N-J_{y}pq+J_{x}pq+J_{xz}qr\end{array}\right]\text{\hspace{1em}(1)}$$
其中$$\Lambda ={\left(J^b\right)}^{-1}=\left[\begin{array}{ccc}\frac{J_z}{J_x{J}_z-J_{xz}{J}_{zx}}& 0& \frac{J_{xz}}{J_{xz}{J}_{zx}-J_x{J}_z}\\ 0& \frac{1}{J_y}& 0\\ \frac{J_{zx}}{J_{xz}{J}_{zx}-J_x{J}_z}& 0& \frac{J_x}{J_x{J}_z-J_{xz}{J}_{zx}}\end{array}\right]$$且$\bar{q}=\frac{1}{2}\rho V_T^2$为空气动压，$b$为翼展，$\bar{c}$为机翼平均弦长；$E_M,E_L,E_N$分别为滚转、俯仰、偏航力矩系数，各自表达式如下：
$$\begin{array}{rl}{E}_L& =C_{\bar{L}\beta }\cdot \beta +C_{L{\delta }_r}\cdot {\delta }_r+C_{L{\delta }_a}\cdot {\delta }_a+C_{L\bar{p}}\left(\frac{pb}{2V_T}\right)+C_{L\bar{r}}\left(\frac{rb}{2V_T}\right)\\ E_M& =C_{M_0}+C_{M\alpha }\cdot \alpha +C_{M{\delta }_e}\cdot {\delta }_e+C_{M\dot{\alpha }}\left(\frac{\dot{\alpha }\bar{c}}{2V_T}\right)+C_{M\bar{q}}\left(\frac{q\bar{c}}{2V_T}\right)\\ E_N& =C_{N\beta }\cdot \beta +C_{N{\delta }_a}\cdot {\delta }_a+C_{N{\delta }_r}\cdot {\delta }_r+C_{N\bar{p}}\left(\frac{pb}{2V_T}\right)+C_{N\bar{r}}\left(\frac{rb}{2V_T}\right)\end{array}\text{\hspace{1em}(2)}$$其中${\delta }_r,{\delta }_a,{\delta }_e$分别为尾翼方向舵、左右副翼、尾翼升降舵的控制量。

2. 系统解耦

不难发现，在模型$(1)$中，$\phi ,\psi$与$E_L,E_N$相关，$\theta$与$E_M$相关；而由(2)知，$E_L,E_N$均为${\delta }_r,{\delta }_a$的函数，$E_M$为${\delta }_e$的函数。因而
$$\{\begin{array}{l}\begin{array}{rl}\ddot{\phi }& =f_1\left({\delta }_r,{\delta }_a\right)\\ \ddot{\theta }& =f_2\left({\delta }_e\right)\\ \ddot{\psi }& =f_3\left({\delta }_r,{\delta }_a\right)\end{array}\end{array}\text{\hspace{1em}(3)}$$此处出现了控制量耦合的情况，即${\delta }_r,{\delta }_a$会同时作用于$\phi ,\psi$。如果使用Backstepping反步法控制，需要系统中每个通道的控制量为强解耦的。为了规避该问题，需要对系统进行解耦。

这里使用比较泛用的方式解耦。

设状态向量为${\left[\begin{array}{cccccc}\phi & \dot{\phi }& \theta & \dot{\theta }& \psi & \dot{\psi }\end{array}\right]}^T$，控制量为$\vec{U}={\left[\begin{array}{ccc}{\delta }_r& {\delta }_a& {\delta }_e\end{array}\right]}^T$，那么$(1)$可用以下增广矩阵描述
$$\{\begin{array}{l}\begin{array}{rl}{\dot{x}}_1& =\dot{\phi }=x_2\\ {\dot{x}}_2& =\ddot{\phi }=f_1\left({\delta }_r,{\delta }_a\right)\\ {\dot{x}}_3& =\dot{\theta }=x_4\\ {\dot{x}}_4& =\ddot{\theta }=f_2\left({\delta }_e\right)\\ {\dot{x}}_5& =\dot{\psi }=x_6\\ {\dot{x}}_6& =\ddot{\psi }=f_3\left({\delta }_r,{\delta }_a\right)\end{array}\end{array}$$取其中的${\left[\begin{array}{ccc}\dot{\phi }=p& \dot{\theta }=q& \dot{\psi }=r\end{array}\right]}^T$，控制量为$\vec{U}={\left[\begin{array}{ccc}{\delta }_r& {\delta }_a& {\delta }_e\end{array}\right]}^T$，写成向量形式即
$$\dot{\vec{X}}=\vec{F}\left(\vec{X},\vec{U}\right)\text{\hspace{1em}(4)}$$为了解耦，可以将$(4)$写成标准柯西形式：
$$\dot{\vec{X}}=A\vec{X}+B\vec{U}\text{\hspace{1em}(5)}$$其中
$$A=\frac{\partial \dot{\vec{X}}}{\partial x_i},B=\frac{\partial \dot{\vec{X}}}{\partial u_i}\text{\hspace{1em}(6)}$$即
$$A=\left[\begin{array}{ccc}\frac{\partial \dot{\vec{p}}}{\partial p}& \frac{\partial \dot{\vec{p}}}{\partial q}& \frac{\partial \dot{\vec{p}}}{\partial r}\\ \frac{\partial \dot{\vec{q}}}{\partial p}& \frac{\partial \dot{\vec{q}}}{\partial q}& \frac{\partial \dot{\vec{q}}}{\partial r}\\ \frac{\partial \dot{\vec{r}}}{\partial p}& \frac{\partial \dot{\vec{r}}}{\partial q}& \frac{\partial \dot{\vec{r}}}{\partial r}\end{array}\right],B=\left[\begin{array}{ccc}\frac{\partial \dot{\vec{p}}}{\partial {\delta }_r}& \frac{\partial \dot{\vec{p}}}{\partial {\delta }_a}& \frac{\partial \dot{\vec{p}}}{\partial {\delta }_e}\\ \frac{\partial \dot{\vec{q}}}{\partial {\delta }_r}& \frac{\partial \dot{\vec{q}}}{\partial {\delta }_a}& \frac{\partial \dot{\vec{q}}}{\partial {\delta }_e}\\ \frac{\partial \dot{\vec{r}}}{\partial {\delta }_r}& \frac{\partial \dot{\vec{r}}}{\partial {\delta }_a}& \frac{\partial \dot{\vec{r}}}{\partial {\delta }_e}\end{array}\right]$$这里不进行详细推导地给出矩阵$A,B$，各位读者可以自行推导计算：
$$A=\left[\begin{array}{ccc}\frac{\bar{q}{J}_{z}S{b}^2{C}_{L\bar{p}}}{2D_1{V}_T}-\frac{F_{1}q}{D_1}& -\frac{F_{1}p}{D_1}+\frac{F_{1}r}{D_1}& \frac{\left(J_z{C}_{L{\delta }_r}-J_{xz}{C}_{N\bar{r}}\right)S{b}^2\bar{q}}{2D_1{V}_T}+\frac{F_{2}q}{D_1}\\ \frac{2J_{xz}p+\left(J_z-J_x\right)r}{J_y}& \frac{\bar{q}{C}_{M\bar{q}}S{\bar{c}}^2}{2J_y{V}_T}& \frac{\left(J_z-J_x\right)p-2J_{xz}r}{J_y}\\ -\frac{\bar{q}{J}_{xz}{C}_{L\bar{p}}S{b}^2}{2D_1{V}_T}+\frac{F_{3}q}{D_1}& \frac{F_{3}p}{D_1}+\frac{F_{1}r}{D_1}& \frac{\left(J_x{C}_{N\bar{r}}-J_{xz}{C}_{L\bar{r}}\right)\bar{q}S{b}^2}{2D_1{V}_T}+\frac{F_{1}q}{D_1}\end{array}\right]$$
$$B=\left[\begin{array}{ccc}\frac{\bar{q}Sb\left(J_z{C}_{L{\delta }_a}-J_{xz}{C}_{N{\delta }_a}\right)}{D_1}& 0& \frac{\bar{q}Sb\left(J_z{C}_{L{\delta }_r}-J_{xz}{C}_{N{\delta }_r}\right)}{D_1}\\ 0& \frac{\bar{q}{C}_{M{\delta }_e}S\bar{c}}{J_y}& 0\\ \frac{\bar{q}Sb\left(J_x{C}_{N{\delta }_a}-J_{xz}{C}_{L{\delta }_a}\right)}{D_1}& 0& \frac{\bar{q}Sb\left(J_x{C}_{N{\delta }_r}-J_{xz}{C}_{Ldr}\right)}{D_1}\end{array}\right]$$其中
$$\begin{array}{rl}{F}_1& =J_{xz}\left(J_x-J_y+J_z\right)\\ F_2& =J_y{J}_z-J_z^2-J_{xz}^2\\ F_3& =J_x^2-J_x{J}_y+J_{xz}^2\\ D_1& =J_x{J}_z-J_{xz}^2\end{array}$$如此一来，系统就可以写为(5)形式，且已经进行了解耦：
$$\dot{\vec{X}}=A\vec{X}+B\vec{U}=A\left[\begin{array}{c}\phi \\ \theta \\ \psi \end{array}\right]+B\left[\begin{array}{c}{\delta }_r\\ {\delta }_a\\ {\delta }_e\end{array}\right]\text{\hspace{1em}(7)}$$如$(7)$所示，系统解耦后，三个姿态角通道$\phi ,\theta ,\psi$就可以分别使用三个控制量${\delta }_r,{\delta }_a,{\delta }_e$在数学上进行控制。

3. 反步法控制律设计

关于反步法的初步介绍，读者可以参考笔者的另一篇文章Backstepping反步法控制四旋翼无人机（2）。这里仅对固定翼的控制律进行简单推导。

对于$\phi ,\theta ,\psi$中的任一单独通道，都可以把(1)写成如下形式：
$$\{\begin{array}{l}\begin{array}{rl}{x}_1& =x_i\\ x_2& ={\dot{x}}_1=f_i\left(\vec{x},\vec{u}\right)+g_i\left(\vec{x}\right)u_i=f_i+g_i{u}_i\end{array}\end{array}$$即$$\{\begin{array}{l}\begin{array}{rl}{\dot{x}}_1& =x_2\\ {\dot{x}}_2& ={\dot{x}}_1=f_i\left(\vec{x},\vec{u}\right)+g_i\left(\vec{x}\right)u_i=f_i+g_i{u}_i\end{array}\end{array}\text{\hspace{1em}(8)}$$若设$x_1$的期望值为$x_{1d}$，则有状态误差
$$e_1=x_{1d}-x_1$$设李雅普诺夫函数为
$$V=\frac{1}{2}{e}_1^2$$并对其求导
$$\begin{array}{rl}\dot{V}& =e_1{\dot{e}}_1\\ & =e_1\left({\dot{x}}_{1d}-{\dot{x}}_1\right)\\ & =e_1\left({\dot{x}}_{1d}-x_2\right)\end{array}$$为了使得$\dot{V}\le 0$，可以设计一个理想$x_2$
$$x_2^v={\dot{x}}_{1d}+{\alpha }_1{e}_1\left({\alpha }_1>0\right)\text{\hspace{1em}(9)}$$从而
$$\dot{V}=e_1\left({\dot{x}}_{1d}-x_2\right)=e_1\left({\dot{x}}_{1d}-{\dot{x}}_{1d}-{\alpha }_1{e}_1\right)=-\alpha e_1^2\le 0$$显然，$x_2$有自己的表达式$(8)$，因而$(9)$只能称为理想$x_2$，它与实际$x_2$之间依然有误差：
$$e_2=x_2-x_2^v=x_2-{\dot{x}}_{1d}-{\alpha }_1{e}_1\text{\hspace{1em}(10)}$$其导数：
$${\dot{e}}_2={\dot{x}}_2-{\ddot{x}}_{1d}-{\alpha }_1{\dot{e}}_1$$从而重新设计李雅普诺夫函数
$$V=\frac{1}{2}{e}_1^2+\frac{1}{2}{e}_2^2$$求导：
$$\begin{array}{rl}\dot{V}& =e_1{\dot{e}}_1+e_2{\dot{e}}_2\\ & =e_1\left({\dot{x}}_{1d}-x_2\right)+e_2\left({\dot{x}}_2-{\ddot{x}}_{1d}-{\alpha }_1{\dot{e}}_1\right)\end{array}$$代入$(10)$式
$$\begin{array}{rl}\dot{V}& =e_1\left({\dot{x}}_{1d}-x_2\right)+e_2\left({\dot{x}}_2-{\ddot{x}}_{1d}-{\alpha }_1{\dot{e}}_1\right)\\ & =-e_1\left(e_2+{\alpha }_1{e}_1\right)+e_2\left[f_i+g_i{u}_i-{\ddot{x}}_{1d}+{\alpha }_1\left(e_2+{\alpha }_1{e}_1\right)\right]\end{array}\text{\hspace{1em}(11)}$$此时如果设计控制量$u_i$为以下形式：$$u_i=\frac{-f_i+{\ddot{x}}_{1d}-{\alpha }_1\left(e_2+{\alpha }_1{e}_1\right)+e_1-{\alpha }_2{e}_2}{g_i}\left({\alpha }_2>0\right)\text{\hspace{1em}(12)}$$那么，$(11)$就可以变为
$$\dot{V}=-{\alpha }_1{e}_1^2-{\alpha }_2{e}_2^2\le 0$$满足李雅普诺夫稳定性条件。

对于欧拉角$\phi ,\theta ,\psi$分别计算出各自的$f_i,g_i$，代入$(12)$中，即可获得各自的控制量$u_i$。

4. 仿真结果

simulink连接框图如下：

其中“From Eula to pqr”与“From pqr to Eula”两个模块用来做$\phi ,\theta ,\psi$到$p,q,r$之间的转换，此处使用了单位矩阵以满足$\left[\begin{array}{c}p\\ q\\ r\end{array}\right]\approx \left[\begin{array}{c}\dot{\phi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]$。可以自行改写代码，引入系数转换矩阵，以达到更精确的计算。
另外，由于代数环的存在，图中加入了延迟环节。

仿真时间10s，仿真结果如下：

5. 仿真结果分析

图中可以看出，反步法跟踪性能很好，动态过程平稳，没有超调。但该模型对于初值的敏感性很强，图中存在的静差受积分器初值影响，不同的积分器初值会造成不同的静差。如何消除静差是下一步待解决的问题。

6. 参考文献

薛智爽, 刘小芳, 陈洋.基于分层滑模面的固定翼无人机姿态跟踪控制[J].中国科技论文,2021(11):1220-1226.