如果这篇博客帮助到你，可以给我充个电加个油哦~
CC BY 4.0 （除特别声明或转载文章外）
一、无人机的坐标系
1、右手定则
- 图b中的大拇指方向为坐标轴方向，四指方向为旋转方向
2、地球固联坐标系与机体坐标系定义
- 右下标e表示Earth，通常将无人机起飞的位置或者地心作为Oe，Ze垂直于地面向下（此时忽略地球的曲率，假设起飞位置是一个平面），下标b表示Body，Xb是机头方向，Zb为飞机对称平面内垂直于Xb轴向下
二、无人机的变换矩阵
1、欧拉角
- 欧拉角的定义：
- （1）将地球固联坐标系绕固定点转动三次即可使它与机体坐标系的三轴指向完全一致。在三次转动中，每次的旋转轴是被转动坐标系的某一坐标轴，每次的转动角度即为欧拉角。这三次转动分别为滚转角、俯仰角和偏航角。
- （2）机体坐标系通常与地球固联坐标系不一致，此时机体坐标系与地面地球固联坐标系之间的夹角就是飞机的姿态角，又称欧拉角:
- 俯仰角 $\theta$ ：机体轴与地平面（水平面)之间的夹角，飞机抬头为正。
- 偏航角（方位角) $\psi$ ：机体轴在水平面上的投影与地轴之间的夹角，以机头右偏为正。
- 滚转角（倾斜角) $\phi$ :飞机对称面绕机体轴转过的角度，右滚为正。
- 欧拉角的变化率与机体角速度的关系：
\(\dot{\theta} =W\ast ^{b}\omega\)
式中 $\dot{\theta }$ 为欧拉角的变化率, ${\theta}$ 为欧拉角，表达式为： \({\theta} =\left [ \phi \quad \theta \quad \psi \right ] ^{T}\) W的表达式为：
$\qquad$ $^{b}\omega$ 为机体角速度，其表达式为： \({^{b}\omega}=\left [\omega_{x_{b} } \quad \omega_{y_{b} } \quad \omega_{z_{b} }\right]^T\)
$\qquad$ 可以看到当 ${\theta}$ 等于 $\pm \pi/2$ 时， $\cos \theta = 0$ ，也就会产生奇异性问题，这个也是欧拉角表示的一种缺陷
2、四元数
- 四元数的定义：
- 四元数一般用向量的形式表示为 $q=[q_{0} \quad q_{v}]^T$ ，其中 $q_{0}$ 是四元数的标量部分， $q_{v}=[q_{1} \quad q_{2} \quad q_{3}]^T$ 是四元数的向量部分，四元数就是由标量和向量组合而成。
- 四元数的复数推广形式可能更好理解： $\mathbf{q}=q_{0}+q_{1} \mathbf{i}+q_{2} \mathbf{j}+q_{3} \mathbf{k} \quad\left(\mathbf{i}^{2}=\mathbf{j}^{2}=\mathbf{k}^{2}=\mathbf{i j} \mathbf{k}=-1\right)$ 。
- 四元数的基本运算法则:
（1）四元数的加、减法：
$\qquad$ （2）四元数的乘法：
\[\mathbf{p} \otimes \mathbf{q} = \begin{bmatrix} p_{0} \\ \mathbf{p_{v}} \end{bmatrix} \otimes \begin{bmatrix} q_{0} \\ \mathbf{q_{v}} \end{bmatrix} = \begin{bmatrix} p_{0} q_{0}-\mathbf{q_{v}}^{\mathrm{T}} \mathbf{p_{v}} \\ \mathbf{p_{v}} \times \mathbf{q_{v}} + p_{0} \mathbf{q_{v}} + q_{0} \mathbf{p}_{\mathrm{v}} \end{bmatrix}\]$\qquad$ $\qquad$ 四元数加减乘的运算性质（ $\mathbf{q}$ , $\mathbf{r}$ , $\mathbf{m}$ 为四元数， $\mathbf{s}$ 为标量， $\mathbf{u}$ , $\mathbf{v}$ 为列向量）：
\[\mathbf{q} \otimes(\mathbf{r}+\mathbf{m}) =\mathbf{q} \otimes \mathbf{r}+\mathbf{q} \otimes \mathbf{m}\] \[\mathbf{q} \otimes \mathbf{r} \otimes \mathbf{m} =(\mathbf{q} \otimes \mathbf{r}) \otimes \mathbf{m}=\mathbf{q} \otimes(\mathbf{r} \otimes \mathbf{m})\] \[s \mathbf{q}=\mathbf{q} s=\begin{bmatrix} s q_{0} \\\ s \mathbf{q_v} \end{bmatrix}\] \[\mathbf{q_u} \otimes \mathbf{q_v}=\begin{bmatrix} 0 \\\ \mathbf{u} \end{bmatrix} \otimes \begin{bmatrix} 0 \\\ \mathbf{v} \end{bmatrix}=\begin{bmatrix} -\mathbf{u^T} \mathbf{v} \\\ \mathbf{u} \times \mathbf{v} \end{bmatrix}\]$\qquad$ （3）四元数的共轭：
\[\mathbf{q}=\begin{bmatrix} q_{0} \\\ \mathbf{q_v} \end{bmatrix} \to \mathbf{q^*}=\begin{bmatrix} q_{0} \\\ \mathbf{-q_v} \end{bmatrix}\]$\qquad$ $\qquad$ 四元数共轭的运算性质：
\[\left ( \mathbf{q}^* \right ) ^*=\mathbf{q}\] \[\left ( \mathbf{p} \otimes \mathbf{q} \right ) ^* = \mathbf{q}^* \otimes \mathbf{p}^*\] \[\left ( \mathbf{p} + \mathbf{q} \right ) ^* = \mathbf{p}^* + \mathbf{q}^*\]$\qquad$ （4）四元数的范数：
\[\begin{aligned} \left\|\mathbf{q}\right\|^{2} & = \left\|\mathbf{q} \otimes \mathbf{q^*}\right\| = \left\|\mathbf{q^*} \otimes \mathbf{q}\right\| \\\ & =q_{0}^{2} + \mathbf{q_v}^T \mathbf{q_v} \\\ & =q_{0}^{2} + q_{1}^{2}+q_{2}^{2}+q_{3}^{2} \end{aligned}\]$\qquad$ $\qquad$ 四元数范数的运算性质：
\[\left\|\mathbf{p} \otimes \mathbf{q}\right\| = \left\|\mathbf{p}\right\|\left\|\mathbf{q}\right\|\] \[\left\| \mathbf{q^*}\right\|= \left\|\mathbf{q}\right\|\]$\qquad$ （5）四元数的逆：
\[\mathbf{q} \otimes \mathbf{q^{-1}} = \begin{bmatrix} 1 \\\ 0_{3\times 1} \end{bmatrix}\]$\qquad$ $\qquad$ 由 $\mathbf{q^*}$ 的定义可知，四元数的逆可以表示为 $\mathbf{q^{-1}}=\frac{\mathbf{q^{*}}}{\left||\mathbf{p}\right||} $
$\qquad$ （6）单位四元数：
$\qquad$ $\qquad$ 当四元数 $\mathbf{q}$ 的范数 $\left||\mathbf{p}\right|| =1 $时，四元数 $\mathbf{q}$ 称为单位四元数。单位四元数有如下性质：当四元数 $\mathbf{p}$ , $\mathbf{q}$ 满足 $\left|| \mathbf{p} \right|| = \left||\mathbf{q}\right|| =1$ 时， 则有
\[\left\|\mathbf{p} \otimes \mathbf{q}\right\|=1\] \[\mathbf{q^{-1}} = \mathbf{q^{*}}\]- 四元数的旋转:
$\qquad$ （1）向量旋转：假如 $\mathbf{q}$ 表示旋转，而 $\mathbf{v_1} \in \mathbb{R}^{3}$ ，那么在旋转 $\mathbf{q}$ 作用下，向量 $\mathbf{v_1}$ 变为向量 $\mathbf{v_1^{‘}}$ 。
\[\begin{bmatrix} 0 \\\ \mathbf{v_1^{'}} \end{bmatrix} = \mathbf{q}\otimes \begin{bmatrix} 0 \\\ \mathbf{v_1} \end{bmatrix}\otimes\mathbf{q^{-1}}\]单位四元数的物理含义是（四元数表示轴角）：
\[\mathbf{q}=\begin{bmatrix} cos\frac{\theta }{2} \\\ \mathbf{v}sin\frac{\theta }{2} \end{bmatrix}\]
$\qquad$ （2）坐标系旋转（将地球固联坐标系转到机体坐标系就是用的坐标系旋转，这个在下面会进行讲解）：
\[\begin{bmatrix} 0 \\\ \mathbf{v_1^{'}} \end{bmatrix} = \mathbf{q^{-1}}\otimes \begin{bmatrix} 0 \\\ \mathbf{v_1} \end{bmatrix}\otimes\mathbf{q}\]
- 四元数变化率与机体角速度的关系:
$\qquad$ 其中：
\[\Delta \mathbf{q} = \left [\cos\frac{\theta}{2} \quad \mathbf{v}^{\mathrm{T}} \sin \frac{\theta}{2} \right ]^{\mathrm{T}}\]