四元数、欧拉角和方向余弦的定义及关系

最新推荐文章于 2023-05-07 01:27:45 发布

wust_fly_high

最新推荐文章于 2023-05-07 01:27:45 发布

阅读量2.1w

点赞数 28

分类专栏：四旋翼姿态解算文章标签：四旋翼四元数欧拉角方向余弦矩阵四旋翼姿态角

本文链接：https://blog.csdn.net/wkdwl/article/details/52106646

版权

四旋翼姿态解算专栏收录该内容

4 篇文章 5 订阅

订阅专栏

目前，描述两个坐标系之间关系的常用方法主要有欧拉角法、方向余弦矩阵法和四元数法。因此要弄懂这三种方法的定义及关系，我们必须先从坐标系转化开始了解。下面以四旋翼为例，定义两个坐标系。导航坐标系(参考坐标系)n，选取东北天右手直角坐标系作为导航坐标系n、载体坐标系(机体坐标系)b，选取右手直角坐标系定义：四轴向右为X正方向，向前为Y轴正方向，向上为Z轴正方向。

欧拉角：欧拉角是一种常用的描述方位的方法，是由欧拉提出的。基本思想就是将两个坐标系的变换分解为绕三个不同的坐标轴的三次连续转动组成的序列。欧拉角的旋转规定为连续两次旋转，必须绕着不同的转动轴旋转，所以一共有12种旋转顺规。这里我们选用Z-Y-X的旋转顺规来描述b系与n系的关系。Z-Y-X顺规就是指。绕Z轴旋转偏航角(YAW) $\psi$ ，绕Y轴旋转横滚角(ROLL) $\phi$ ，绕X轴旋转俯仰角(PITCH) $\theta$ 。

先由简单的2维XY坐标系计算，如下图，载体坐标系b和导航坐标系n初始重合，绕X轴逆时针旋转 $\theta$ 角。向量OA在导航坐标系中的坐标为(Y,Z)，在载体坐标系中的坐标为(Y',Z')。则可以通过计算得出两个坐标的关系。

如上图，做出辅助线1可得 $\begin{pmatrix} Y=Y'\cos \theta -Z'\sin \theta \\ Z=Y'\sin \theta +Z'\cos \theta \end{pmatrix}$ 写成矩阵运算为 $\begin{bmatrix} Y\\ Z\end{bmatrix}=\begin{bmatrix} \cos \theta&-\sin \theta \\ \sin \theta &\cos \theta \end{bmatrix}\begin{bmatrix} Y'\\ Z' \end{bmatrix}$ ,

推广至三维坐标系中则为 $\begin{bmatrix} X\\ Y\\ Z \end{bmatrix}=\begin{bmatrix} 1 &0 &0 \\ 0&\cos \theta &-\sin \theta \\ 0&\sin \theta &\cos \theta \end{bmatrix}\begin{bmatrix} X'\\ Y'\\ Z' \end{bmatrix}$ ，

同理得绕Y轴旋转的坐标关系 $\begin{bmatrix} X\\ Y\\ Z \end{bmatrix}=\begin{bmatrix} \cos \phi &0 &\sin \phi \\ 0&1 &0 \\ -\sin \phi &0 &\cos \phi \end{bmatrix}\begin{bmatrix} X'\\ Y'\\ Z' \end{bmatrix}$ ,

绕Z轴旋转的坐标关系为 $\begin{bmatrix} X\\ Y\\ Z \end{bmatrix}=\begin{bmatrix} \cos\psi &-\sin \psi &0 \\ \sin \psi &\cos \psi &0 \\ 0& 0 & 1 \end{bmatrix}\begin{bmatrix} X'\\ Y'\\ Z' \end{bmatrix}$

由此得到经过三个欧拉角转动后，导航坐标系下的向量 $\begin{bmatrix} X^{b}\\ Y^{b}\\ Z^{b} \end{bmatrix}$ 旋转后的与其对应的载体坐标系下的向量 $\begin{bmatrix} X^{n}\\ Y^{n}\\ Z^{n} \end{bmatrix}$ 之间的关系为，按Z-Y-X的顺规得到

$\begin{bmatrix} X^{b}\\ Y^{b}\\ Z^{b} \end{bmatrix}=\begin{bmatrix} 1 &0 &0 \\ 0& \cos \theta &-\sin \theta \\ 0& \sin \theta & \cos \theta \end{bmatrix}\begin{bmatrix} \cos \phi &0 & \sin \phi \\ 0& 1 & 0\\ -\sin \phi &0 &\cos \phi \end{bmatrix}\begin{bmatrix} \cos \psi &-\sin \psi &0 \\ \sin \psi & \cos \psi & 0\\ 0& 0 & 1 \end{bmatrix}\begin{bmatrix} X^{n}\\ Y^{n}\\ Z^{n} \end{bmatrix}$

即为 $\begin{bmatrix} X^{b}\\ Y^{b}\\ Z^{b} \end{bmatrix}=R_{n}^{b}\begin{bmatrix} X^{n}\\ Y^{n}\\ Z^{n} \end{bmatrix}$ ，其中 $R_{n}^{b}$ 即为坐标系n到坐标系b的变换矩阵，我们平常定义的欧拉角形式的方向余弦矩阵为 $R_{b}^{n}$ ， $R_{b}^{n}=\left ( R_{n}^{b} \right )^{T}$

$R_{b}^{n}=\begin{bmatrix} \cos \phi \cos \psi &\sin \theta \sin \phi \cos \psi -\cos \theta \sin \psi & \cos \theta \sin \phi \cos \psi +\sin \theta \sin \psi \\ \cos \phi \sin \psi &\sin \theta \sin \phi \sin \psi+\cos \theta \cos \psi &\cos \theta \sin \phi \sin \psi -\sin \theta \cos \psi \\ -\sin \phi &\sin \theta \cos \phi & \cos \theta \cos \phi \end{bmatrix}$

四元数：四元数是由四个元构成的数， $Q\left ( q_{0},q_{1},q_{2},q_{3} \right )=q_{0}+q_{1}\vec{i}+q_{2}\vec{j}+q_{3}\vec{k}$ ，其中， $q_{0},q_{1},q_{2},q_{3}$ 是实数， $\vec{i},\vec{j},\vec{k}$ 既是互相正交的单位向量，又是虚单位 $\sqrt{-1}$ 。且满足以下四元数乘法关系：

${\left.\begin{matrix} \vec{i}\bigotimes \vec{i}=-1&\vec{j}\bigotimes \vec{j}=-1 &\vec{k}\bigotimes \vec{k}=-1 \\ \vec{ i}\bigotimes \vec{j}=\vec{k}&\vec{j}\bigotimes \vec{k}=\vec{i} &\vec{k}\bigotimes \vec{i}=\vec{j} \\ \vec{ j}\bigotimes \vec{i}=-\vec{k}&\vec{k}\bigotimes \vec{j} =-\vec{i} &\vec{i} \bigotimes \vec{k}=-\vec{j} \end{matrix}\right\}}$

其中 $\bigotimes$ 表示四元数乘法，具体展开为

$Q(q_{0},q_{1},q_{2},q_{3})\bigotimes P(p_{0},p_{1},p_{2},p_{3})=(q_{0}+q_{1}\vec{i}+q_{2}\vec{j}+q_{3}\vec{k})\bigotimes (p_{0}+p_{1}\vec{i}+p_{2}\vec{j}+p_{3}\vec{k})=(q_{0}p_{0}-q_{1}p_{1}-q_{2}p_{2}-q_{3}p_{3})+(q_{0}p_{1}+q_{1}p_{0}+q_{2}p_{3}-q_{3}p_{2})\vec{i}+(q_{0}p_{2}+q_{2}p_{0}+q_{3}p_{1}-q_{1}p_{3})\vec{j}+(q_{0}p_{3}+q_{3}p_{0}+q_{1}p_{2}-q_{2}p_{1})\vec{k}=r_{0}+r_{1}\vec{i}+r_{2}\vec{j}+r_{3}\vec{k}$

写成矩阵形式为

$\begin{bmatrix} r_{0}\\ r_{1}\\ r_{2}\\ r_{3} \end{bmatrix}=\begin{bmatrix} \q_{0} &-q_{1} &-q_{2} &-q_{3} \\ q_{1} &q_{0} &-q_{3} &q_{2} \\ q_{2} &q_{3} & q_{0} & -q_{1}\\ q_{3}& -q_{2} &q_{1} &q_{0} \end{bmatrix}\begin{bmatrix} p_{0}\\ p_{1}\\ p_{2}\\ p_{3} \end{bmatrix}=M(Q)P$ 或者

$\begin{bmatrix} r_{0}\\ r_{1}\\ r_{2}\\ r_{3} \end{bmatrix}=\begin{bmatrix} \ p_{0} &-p_{1} &-p_{2} &-p_{3} \\ p_{1} &p_{0} &-p_{3} &p_{2} \\ p_{2} &p_{3} & p_{0} & -p_{1}\\ p_{3}& -p_{2} &p_{1} &p_{0} \end{bmatrix}\begin{bmatrix} q_{0}\\ q_{1}\\ q_{2}\\ q_{3} \end{bmatrix}=M'(P)Q$

其中 $M(Q)$ 的构成形式为第一列是四元数 $Q$ 本身，第一行是 $Q$ 的共轭四元数 $Q^{\ast }$ 的转置，剩下的部分是一个3行3列的矩阵 $V_{Q}$ 称为 $M(Q)$ 的核，是由四元数构成的反对称矩阵

$V_{Q}=\begin{bmatrix} q_{0} &-q_{3} &q_{2} \\ q_{3}& q_{0} &-q_{1} \\ -q_{2}&q_{1} &q_{0} \end{bmatrix}$

同理得到 $M'(P)$ 的核 $V_{P}$

$V_{P}=\begin{bmatrix} p_{0} &-p_{3} &p_{2} \\ p_{3} &p_{0} &-p_{1} \\ -p_{2} &p_{1} &p_{0} \end{bmatrix}$

定义四元数 $q_{n}^{b}$ 用来表示导航坐标系n到在台坐标系的旋转变换，那么有

$r^{n}=q_{b}^{n}\bigotimes r^{b}\bigotimes q_{b}^{n\ast }=q_{b}^{n}\bigotimes(r^{b}\bigotimes q_{b}^{n\ast })=q_{b}^{n}\bigotimes\left [ M( q_{b}^{n\ast }) r^{b}\right ]=M(q_{b}^{n})M( q_{b}^{n\ast }) r^{b}$

$M(q_{b}^{n})M( q_{b}^{n\ast }) =\begin{bmatrix} q_{0} &-q_{1} &-q_{2} &-q_{3} \\ q_{1} &q_{0} &-q_{3} &q_{2} \\ q_{2} &q_{3} &q_{0} &-q_{1} \\ q_{3} &-q_{2} &q_{1} &q_{0} \end{bmatrix}\begin{bmatrix} q_{0} &q_{1} &q_{2} &q_{3} \\ -q_{1} &q_{0} &-q_{3} &q_{2} \\ -q_{2} &q_{3} &q_{0} &-q_{1} \\ -q_{3} &-q_{2} &q_{1} &q_{0} \end{bmatrix}$

即为

$\begin{bmatrix} 0\\ r_{x}^{n}\\ r_{y}^{n}\\ r_{z}^{n} \end{bmatrix}=\begin{bmatrix} q_{0} ^{2}+q_{1} ^{2}+q_{2} ^{2}+q_{3} ^{2}&0 &0 &0 \\ 0 &q_{0} ^{2}+q_{1} ^{2}-q_{2} ^{2}-q_{3} ^{2} &2(q_{1}q_{2}-q_{0}q_{3}) &2(q_{0}q_{2}+q_{1}q_{3}) \\ 0 &2(q_{0}q_{3}+q_{1}q_{2}) &q_{0} ^{2}-q_{1} ^{2}+q_{2} ^{2}-q_{3} ^{2} &2(q_{2}q_{3}-q_{0}q_{1}) \\ 0 &2(q_{1}q_{3}-q_{0}q_{2}) &2(q_{0}q_{1}+q_{2}q_{3}) &q_{0} ^{2}-q_{1} ^{2}-q_{2} ^{2}+q_{3} ^{2} \end{bmatrix}\begin{bmatrix} 0\\ r_{x}^{b}\\ r_{y}^{b}\\ r_{z}^{b} \end{bmatrix}$

其中 $C_{b}^{n}=\begin{bmatrix} q_{0} ^{2}+q_{1} ^{2}-q_{2} ^{2}-q_{3} ^{2} &2(q_{1}q_{2}-q_{0}q_{3}) &2(q_{0}q_{2}+q_{1}q_{3}) \\ 2(q_{0}q_{3}+q_{1}q_{2}) &q_{0} ^{2}-q_{1} ^{2}+q_{2} ^{2}-q_{3} ^{2} &2(q_{2}q_{3}-q_{0}q_{1}) \\ 2(q_{1}q_{3}-q_{0}q_{2}) &2(q_{0}q_{1}+q_{2}q_{3}) &q_{0} ^{2}-q_{1} ^{2}-q_{2} ^{2}+q_{3} ^{2} \end{bmatrix}$

简化为 $r^{n}=C_{b}^{n}r^{b}$

其中 $C_{b}^{n}$ 为通常定义的四元数旋转矩阵为载体坐标系b到导航坐标系n的旋转矩阵 $C_{b}^{n}$ 。

四元数欧拉角、方向余弦矩阵之间的关系之间的关系，已知方向余弦矩阵和四元数旋转矩阵后，可以通过方向余弦矩阵的第一列和第三行元素计算姿态欧拉角如下

$\begin{bmatrix} \theta \\ \phi \\ \psi \end{bmatrix}=\begin{bmatrix} atan 2(2q_{2}q_{3}+2q_{0}q_{1},q_{3}^{2}-q_{2}^{2}-q_{1}^{2}+q_{0}^{2})\\ -asin(2q_{1}q_{3}-2q_{0}q_{2}) \\ atan 2(2q_{1}q_{2}+2q_{0}q_{3},q_{1}^{2}+q_{0}^{2}-q_{3}^{2}-q_{2}^{2}) \end{bmatrix}$

到此介绍完毕，欢迎大家互相交流指正。

wust_fly_high

关注

28
点赞
踩
89

收藏

觉得还不错? 一键收藏
10
评论
四元数、欧拉角和方向余弦的定义及关系

目前，描述两个坐标系之间关系的常用方法主要有欧拉角法、方向余弦矩阵法和四元数法。因此要弄懂这三种方法的定义及关系，我们必须先从坐标系转化开始了解。下面以四旋翼为例，定义两个坐标系。导航坐标系(参考坐标系)n，选取东北天右手直角坐标系作为导航坐标系n、载体坐标系(机体坐标系)b，选取右手直角坐标系定义：四轴向右为X正方向，向前为Y轴正方向，向上为Z轴正方向。欧拉角：欧拉角是一种常用的描述方位
复制链接

扫一扫