平面四旋翼的控制

平面四旋翼模型

动态模型线性化

平面四旋翼的运动方程是非线性的，如下：
$$
\begin{aligned}
&\ddot y=-\frac{u_1}{m}sin(\phi) \\
&\ddot z=-g+\frac{u_1}{m}cos(\phi) \\
&\ddot \phi=-\frac{u_2}{I_{xx}}
\end{aligned}
$$平面四旋翼在悬停平衡态时会有如下配置：
$$
y_0,z_0,\phi_0=0,u_{1.0}=mg,u_{2,0}=0
$$ 接下来我们将模型进行线性化处理。造成模型非线性的原因是模型里存在sin，cos三角函数，当$\phi \rightarrow0$时，$sin\phi \approx\phi$，$cos\phi \approx1$。并且，因为悬停状态时，$u_1$对$\ddot y$没有影响，所以将$u_1=mg$带入，最终得到如下线性模型：
$$
\begin{aligned}
&\ddot y=-g\phi \\
&\ddot z=-g+\frac{u_1}{m} \\
&\ddot \phi=\frac{u_2}{I_{xx}}
\end{aligned}
$$

轨迹跟踪

平面四旋翼的轨迹设计包括了y轴和z轴的位置，速度和加速度：

嵌套控制结构

整个控制系统中包括了位置控制以及姿态控制，位置控制会输出四旋翼所需要的推力$u_1$以及所期望的姿态$\phi,\dot \phi$。姿态控制器得到所期望的姿态后就会输出四旋翼所需要的力矩，过程如下图：

控制方程

3D空间四旋翼的控制

轨迹跟踪

空间四旋翼的轨迹设计包括了x轴，y轴，z轴的位置，速度和加速度，以及yaw的角度，角速度和角加速度：

动态模型线性化

空间四旋翼的运动方程是非线性的，如下：

空间四旋翼在悬停平衡态时会有如下配置：
$$
u_1\sim mg, \theta \sim 0, \phi \sim 0, \psi \sim \psi_0 \\
u_2\sim 0, p\sim 0, q\sim0, r\sim 0
$$接下来我们将模型进行线性化处理：

线性动力方程
R是旋转矩阵，当$\theta \sim 0, \phi \sim 0, \psi \sim \psi_0$时，R经过线性化处理变成了这样：

$$
R=\begin{bmatrix}
c\psi_0-\phi \theta s\psi_0 & -s\psi_0 & \theta c\psi_0+\phi s\psi_0 \\
s\psi_0+\phi \theta c\psi_0 & c\psi_0 & \theta s\psi_0-\phi c\psi_0 \\
-\theta & \phi & 1
\end{bmatrix}
$$并且，因为悬停状态时，$u_1$对$\ddot x, \ddot y$没有影响，所以将$u_1=mg$带入，最终得到如下线性模型：
$$
\begin{aligned}
&\ddot r_1=\ddot x=g(\theta cos\psi+\phi sin\psi) \\
&\ddot r_2=\ddot y=g(\theta sin\psi-\phi cos\psi) \\
&\ddot r_3=\ddot z=\frac{-mg+u_1}{m}
\end{aligned}
$$

角速率
已知角速率有如下公式：
$$
\begin{bmatrix}
p \\ q \\ r
\end{bmatrix} = \begin{bmatrix}
c\theta &0 & -c\phi s\theta \\ 0 & 1 & s\phi \\ s\theta & 0 & c\phi c\theta
\end{bmatrix} \begin{bmatrix}
\dot \phi \\ \dot \theta \\ \dot \psi
\end{bmatrix}
$$化简之后得到：
角动量方程
当$u_2\sim 0, p\sim 0, q\sim0, r\sim 0$时，力矩方程最后一项变为0，如下：