无人机中的姿态描述与部分分析力学

前言

开新坑了，这期的学习记录主要是关于无人机飞控前置知识的学习笔记，主要包括姿态描述与动力学建模两大部分内容。

从牛顿力学到分析力学，确实是一项伟大的跨越。
四元数的超维映射，比较难以理解

这期数学推导有点多，但笔者尝试在学习完这部分的内容后立马尝试推导四旋翼无人机的动力学建模，发现仍然难以推导，只能后续填坑了。

姿态描述：定位基础

定位姿态存在四类描述方式：变换矩阵、旋转矩阵、欧拉角、四元数

旋转矩阵与欧拉角

旋转矩阵是以矩阵形式描述物体在三维空间中的旋转变换，欧拉角则是通过三个角度来描述旋转，即众人熟悉的Roll（滚转）、Pitch（俯仰）、Yaw（偏航）。旋转矩阵可以通过欧拉角很简单的转换，反之同理。

旋转矩阵与欧拉角的转换

首先如下表定义物理量的含义。

符号	物理含义 (欧拉角速度)
$[{\phi}, {\theta}, {\psi}]^T$	欧拉角向量
${\phi}$	滚转角
${\theta}$	俯仰角
${\psi}$	偏航角
$\text{R}_{angle}$	下标角对应的旋转矩阵

推导的旋转矩阵式为：
$$R_{\phi} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\phi & -\sin\phi \\ 0 & \sin\phi & \cos\phi \end{bmatrix}$$
$$R_{\theta} = \begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \end{bmatrix}$$
$$R_{\psi} = \begin{bmatrix} \cos\psi & -\sin\psi & 0 \\ \sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{bmatrix}$$

当你在使用欧拉角/旋转矩阵描述姿态时，你要注意：

对于一刚体运动物体而言，需要规定旋转顺序，即绕哪个轴旋转，旋转顺序不同，最终姿态截然不同。
通常，在飞控领域约定俗成的欧拉角旋转顺序为Z-Y-X序，即先绕Z轴旋转Yaw，再绕Y轴旋转Pitch，最后绕X轴旋转Roll。
正如同矩阵乘法不满足交换律一样，欧拉角的旋转顺序也不满足交换律。
我们以一个例子展示旋转顺序对于最终姿态的影响。

对于IMU中的Gyro陀螺仪数据，以欧拉角旋转顺序为Z-Y-X序，我们可以由其数据得到：
$$
\begin{bmatrix}
\omega_x \\
\omega_y \\
\omega_z
\end{bmatrix}
= \omega_{\text{roll}} + \omega_{\text{pitch}} + \omega_{\text{yaw}}
$$

可得

$$
\begin{bmatrix}
\omega_x \\
\omega_y \\
\omega_z
\end{bmatrix} =
\begin{bmatrix}
\dot{\phi} \\
0 \\
0
\end{bmatrix} +
R_x(\phi)
\begin{bmatrix}
0 \\
\dot{\theta} \\
0
\end{bmatrix} +
R_x(\phi)R_y(\theta)
\begin{bmatrix}
0 \\
0 \\
\dot{\psi}
\end{bmatrix}
$$

带入旋转矩阵的表达式，可以得到下式

$$
\begin{bmatrix}
\omega_x \\
\omega_y \\
\omega_z
\end{bmatrix} =
\begin{bmatrix}
\dot{\phi} \\
0 \\
0
\end{bmatrix} +
\begin{bmatrix}
0 \\
\dot{\theta} \cos \phi \\
-\dot{\theta} \sin \phi
\end{bmatrix} +
\begin{bmatrix}
-\dot{\psi} \sin \theta \\
\dot{\psi} \sin \phi \cos \theta \\
\dot{\psi} \cos \phi \cos \theta
\end{bmatrix}
$$

即化为3x3矩阵并求逆，可得到依据陀螺仪数据得到欧拉角速率的关系式

$$
\begin{bmatrix}
\dot{\phi} \\
\dot{\theta} \\
\dot{\psi}
\end{bmatrix} =
\begin{bmatrix}
1 & \sin \phi \tan \theta & \cos \phi \tan \theta \\
0 & \cos \phi & -\sin \phi \\
0 & \sin \phi \sec \theta & \cos \phi \sec \theta
\end{bmatrix}
\begin{bmatrix}
\omega_x \\
\omega_y \\
\omega_z
\end{bmatrix}
$$
若欧拉角旋转顺序为X-Y-Z序，则第二个式子项会发生变化，结果也会完全不一样。

四元数

欧拉角先天存在万向节死锁问题，即当俯仰角为±90°时，滚转角和偏航角会发生耦合，导致姿态描述不唯一。为了解决这个问题，四元数被引入到飞控领域。

四元数在三维复数的基础上引入一个新虚数单位$k$
$$z = a + bi + cj\qquad(i^2=j^2=-1)$$
$$q = a + bi + cj + dk\quad(i^2=j^2=k^2=-1)$$
$i，j，k$为两两正交的单位向量

四元数的性质与运算

四元数的范数
$$||q|| = \sqrt{a^2 + b^2 + c^2 + d^2}$$
四元数的共轭
$$q^* = a - bi - cj - dk$$
四元数同样存在三角表示和指数表示
- 已知$a^2+||q||^2=1$，引入新向量$u = \dfrac{q}{||q||}$
- 则四元数可三角表示为$$q = \cos\theta + u\sin\theta$$
- 对于四元数存在重要的含义：对于将一次旋转操作表示为四元数$q_1$（尤其注意代表纯旋转的四元数一定是单位四元数）时，需要对被旋转的向量$v$进行夹乘四元数运算，旋转后的向量$v’$可表示为：
  $$v’ = q_1 v q_1^*$$
- 这一夹乘的特性可被简要概括为，旋转角度叠加，拉伸畸变抵消，此即左乘以四元数，右乘以其逆的核心含义，又因为旋转角度叠加，所以旋转操作转化为四元数时通常写如下式子，以直观得知转动角度为$\theta$
  $$q = \cos\frac{\theta}{2} + u\sin\frac{\theta}{2}$$
- 四元数的指数表示为：
  $$q = e^{u\frac{\theta}{2}}$$
四元数的乘法和逆
- 四元数的乘法不满足交换律
- 四元数的逆初始定义为