Backstepping反步法控制四旋翼无人机（一）

四旋翼基本参数

四旋翼作为一种可以在空间中自由飞行的无人飞行器，具有6个自由度和4个螺旋桨。其中，4个螺旋桨提供动力，作为四旋翼的动力源；6个自由度分别为：3个位置坐标$x,y,z$，3个角度坐标$\theta$（俯仰），$\phi$（滚转），$\psi$（偏航）。三种角表示如下：

基本假设

为了研究方便，作出以下假设：

1. 四旋翼为刚体；
2. 四旋翼几何中心处于其重心；
3. 只有螺旋桨的拉力$T$和重力$G$作用于四旋翼，且拉力$T$垂直于四旋翼的几何平面，重力$G$指向地心；
4. 四旋翼的转动惯量和质量不变；
5. 呈对角线分布的两个螺旋桨转动方向相同；
6. 忽略空气阻力。

转换矩阵

为研究四旋翼，需要建立坐标系。但随之而来的问题是，应该相对于什么建立坐标系。可以相对四旋翼本身建立坐标系，这种坐标系便于研究四旋翼的俯仰、滚转、偏航角，但显然无法得知四旋翼在具体空间中的位置坐标；亦可以相对地球建立坐标系，该坐标系便于得知四旋翼的位置信息，但其角度信息比较难以求解。

由此可见，在研究四旋翼过程中，势必需要经过两种坐标系的转化。由此得出转换矩阵
$$\begin{array}{rl}{\mathbf{R}}_b^e& =\left[\begin{array}{ccc}\cos \theta \cos \psi & \cos \psi \sin \theta \sin \phi -\sin \psi \cos \phi & \cos \psi \sin \theta \cos \phi +\sin \psi \sin \phi \\ \cos \theta \sin \psi & \sin \psi \sin \theta \sin \phi +\cos \psi \cos \phi & \sin \psi \sin \theta \cos \phi -\cos \psi \sin \phi \\ -\sin \theta & \sin \phi \cos \theta & \cos \phi \cos \theta \end{array}\right]\\ {\mathbf{R}}_e^b& =\left[\begin{array}{ccc}\cos \theta \cos \psi & \cos \theta \sin \psi & -\sin \theta \\ \cos \psi \sin \theta \sin \phi -\sin \psi \cos \phi & \sin \psi \sin \theta \sin \phi +\cos \psi \cos \phi & \sin \phi \cos \theta \\ \cos \psi \sin \theta \cos \phi +\sin \psi \sin \phi & \sin \psi \sin \theta \cos \phi -\cos \psi \sin \phi & \cos \phi \cos \theta \end{array}\right]\end{array}$$
其中上下标的含义解释如下：以${\mathbf{R}}_b^e$为例，指从机体坐标系$b$转为地心坐标系$e$。若某个相对于机体的参数为$A^b$，则它相对于地心的参数为$A^e=A^b\cdot {\mathbf{R}}_b^e$。

一般地，若设机体坐标系$b$下角速度为$\dot{\phi },\dot{\theta },\dot{\psi }$，地心坐标系$e$下角速度为$p,q,r$，则存在如下关系：
$$\left[\begin{array}{c}\dot{\phi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{ccc}1& \tan \theta \sin \phi & \tan \theta \cos \phi \\ 0& \cos \phi & -\sin \phi \\ 0& \frac{\sin \phi }{\cos \theta }& \frac{\cos \phi }{\cos \theta }\end{array}\right]\left[\begin{array}{c}p\\ q\\ r\end{array}\right]\text{\hspace{1em}(1)}$$

基本方程

设每个螺旋桨可产生的向上拉力为$F_i^b$，则总拉力向量为
$${\mathbf{F}}_T^b=\left[\begin{array}{c}0\\ 0\\ \sum _{i=1}^4{F}_i^b\end{array}\right]$$
其中下标$T$表示拉力，$b$表示相对于机体坐标系。
可以看出，拉力向量${\mathbf{F}}_T^b$只有在$z$轴上才不为0。
同时，四旋翼还受到重力
$${\mathbf{G}}^e=\left[\begin{array}{c}0\\ 0\\ -mg\end{array}\right]$$
上标$e$表示相对于地球坐标系。

将上述方程写在地球坐标系$e$下，可以得到
$$\begin{array}{rl}\sum {\mathbf{F}}^{\mathbf{e}}& ={\mathbf{F}}_T^e+{\mathbf{G}}^e\\ & ={\mathbf{R}}_b^e\cdot {\mathbf{F}}_T^b+{\mathbf{G}}^e\\ & =\left[\begin{array}{c}\left(\cos \psi \sin \theta \cos \phi +\sin \psi \sin \phi \right)\sum _{i=1}^4{F}_i^b\\ \left(\sin \psi \sin \theta \cos \phi -\cos \psi \sin \phi \right)\sum _{i=1}^4{F}_i^b\\ \cos \phi \cos \theta \sum _{i=1}^4{F}_i^b-mg\end{array}\right]\end{array}$$

在地球坐标系$e$下，由欧拉方程有
$$\sum {\mathbf{F}}^e=m{\dot{V}}^e$$
即
$$\left[\begin{array}{c}(\cos \psi \sin \theta \cos \phi +\sin \psi \sin \phi )\sum _{i=1}^4{F}_i^b\\ (\sin \psi \sin \theta \cos \phi -\cos \psi \sin \phi )\sum _{i=1}^4{F}_i^b\\ \cos \phi \cos \theta \sum _{i=1}^4{F}_i^b-mg\end{array}\right]=m\left[\begin{array}{c}\ddot{x}\\ \ddot{y}\\ \ddot{z}\end{array}\right]\text{\hspace{1em}(2)}$$
同时，由旋转体的欧拉方程也可以得到
$$\begin{array}{rl}\mathbf{J}\cdot {\dot{\omega }}^b+{\omega }^b\times (\mathbf{J}\cdot {\omega }^b)& =\left[\begin{array}{ccc}{J}_x& 0& 0\\ 0& J_y& 0\\ 0& 0& J_z\end{array}\right]\left[\begin{array}{c}\dot{p}\\ \dot{q}\\ \dot{r}\end{array}\right]+\left[\begin{array}{c}p\\ q\\ r\end{array}\right]\times \left(\left[\begin{array}{ccc}{J}_x& 0& 0\\ 0& J_y& 0\\ 0& 0& J_z\end{array}\right]\left[\begin{array}{c}p\\ q\\ r\end{array}\right]\right)\\ & =\left[\begin{array}{c}{J}_x\dot{p}+\left(J_z-J_y\right)qr\\ J_y\dot{q}+\left(J_x-J_z\right)pr\\ J_z\dot{r}+\left(J_y-J_x\right)pq\end{array}\right]\end{array}$$
同时该欧拉方程又可以表示为
$$\begin{array}{rl}\sum {\mathbf{M}}^b=\tau & =\left[\begin{array}{c}{M}_{Tx}\\ M_{Ty}\\ M_{Tz}\end{array}\right]\\ & =\left[\begin{array}{c}l\left(F_2-F4\right)\\ l\left(F_1-F_3\right)\\ M_{D1}-M_{D2}+M_{D3}-M_{D4}\end{array}\right]\end{array}$$
其中$\tau$为拉力的力矩，$M_{Di}$为第$i$个螺旋桨对$z$轴的力矩。
于是可以有
$$\left[\begin{array}{c}l\left(F_2-F_4\right)\\ l\left(F_1-F_3\right)\\ M_{D1}-M_{D2}+M_{D3}-M_{D4}\end{array}\right]=\left[\begin{array}{c}{J}_x\dot{p}+\left(J_z-J_y\right)qr\\ J_y\dot{q}+\left(J_x-J_z\right)pr\\ J_z\dot{r}+\left(J_y-J_x\right)pq\end{array}\right]$$
化简得
$$\left[\begin{array}{c}{J}_x\dot{p}\\ J_y\dot{q}\\ J_z\dot{r}\end{array}\right]=\left[\begin{array}{c}l\left(F_2-F_4\right)-\left(J_z-J_y\right)qr\\ l\left(F_1-F_3\right)-\left(J_x-J_z\right)pr\\ M_{D1}-M_{D2}+M_{D3}-M_{D4}-\left(J_y-J_x\right)pq\end{array}\right]\text{\hspace{1em}(3)}$$
$(1)(2)(3)$分别为三个描述四旋翼运动学的方程。

线性化

假设四旋翼飞行状况十分平稳，因此三个角$\phi ,\theta ,\psi$的值都很小$(\approx 0)$。这样公式$(1)$可以简化为
$$\left[\begin{array}{c}\dot{\phi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{ccc}1& \tan \theta \sin \phi & \tan \theta \cos \phi \\ 0& \cos \phi & -\sin \phi \\ 0& \frac{\sin \phi }{\cos \theta }& \frac{\cos \phi }{\cos \theta }\end{array}\right]\left[\begin{array}{c}p\\ q\\ r\end{array}\right]\approx \left[\begin{array}{c}p\\ q\\ r\end{array}\right]\text{\hspace{1em}(4)}$$
则公式$(2)(3)(4)$可以合并为
$$\left[\begin{array}{c}\ddot{\phi }\\ \ddot{\theta }\\ \ddot{\psi }\\ \ddot{x}\\ \ddot{y}\\ \ddot{z}\end{array}\right]=\left[\begin{array}{c}\dot{\theta }\dot{\psi }\frac{J_y-J_z}{J_x}+\frac{l}{J_x}\left(F_2-F_4\right)\\ \dot{\phi }\dot{\psi }\frac{J_z-J_x}{J_y}+\frac{l}{J_y}\left(F_1-F_3\right)\\ \dot{\phi }\dot{\theta }\frac{J_x-J_y}{J_z}+\frac{1}{J_z}\left(M_{D1}-M_{D2}+M_{D3}-M_{D4}\right)\\ \frac{1}{m}\left(\cos \psi \sin \theta \cos \phi +\sin \psi \sin \phi \right)\sum _{i=1}^4{F}_i^b\\ \frac{1}{m}\left(\sin \psi \sin \theta \cos \phi -\cos \psi \sin \phi \right)\sum _{i=1}^4{F}_i^b\\ \frac{1}{m}\cos \phi \cos \theta \sum _{i=1}^4{F}_i^b-g\end{array}\right]\text{\hspace{1em}(5)}$$

综合

经过上述推导，四旋翼的运动学方程已经悉数得出，可以合并表示在$(5)$式中。
在下一节分享中，将详细推导如何把反步法运用到四旋翼无人机的建模当中。