欧拉角与姿态解算

爱尔兰的楠小楠

已于 2023-12-29 01:31:36 修改

阅读量1.9k

点赞数 20

分类专栏：旋翼无人机开发文章标签：算法无人机

于 2023-12-29 00:15:11 首次发布

本文链接：https://blog.csdn.net/m0_73745340/article/details/135280506

版权

旋翼无人机开发专栏收录该内容

3 篇文章 6 订阅

订阅专栏

引言

本篇博客主要是介绍描述空间姿态的方法以及姿态的基本解算过程

一、基本概念及公式介绍

常用的姿态表示方法有欧拉角、方向余弦矩阵、四元数这几种。

欧拉角表示方法采用 $(\phi ,\theta ,\psi )$ 来表示飞行器的姿态，其中为滚转角，为俯仰角和为航向角，表示飞行器首先航向偏转角度，再俯仰角度，然后机体滚转角度得到的姿态。

方向余弦矩阵通过机体坐标和地面坐标的转换矩阵（DCM, Directional Cosine Matrix）来表示机体的姿态。

四元数通过四个元数来表示飞行器全方位的姿态，它的特点是表征方式简洁，并且没有奇异点。

机体坐标系

1.欧拉角->方向余弦矩阵

${{C}_{b}^{g}}=\left [{\begin{matrix}\cos{\theta }cos\psi &\sin{\phi }sin\theta cos\psi -cos\phi sin\psi &cos\phi sin\theta cos\psi +sin\phi sin\psi \\cos\theta sin\psi &sin\phi sin\theta sin\psi +cos\phi cos\psi &cos\phi sin\theta sin\psi -sin\phi cos\psi \\-sin\theta &sin\phi cos\theta &cos\phi cos\theta \end{matrix}}\right ]$

其中， ${{C}_{b}^{g}}$ 表示从机体坐标转换到地面坐标的变换矩阵， ${{C}_{g}^{b}}$ 为地面坐标到机体坐标的转换矩阵， ${{C}_{g}^{b}}=({{C}_{b}^{g}}{{)}^{T}}$ 为 ${{C}_{b}^{g}}$ 的转置

2.欧拉角->四元数

$\begin{matrix}{{q}_{0}}&=\pm \left ({\cos{\frac{\phi }{2}}cos\frac{\theta }{2}cos\frac{\psi }{2}+sin\frac{\phi }{2}sin\frac{\theta }{2}sin\frac{\psi }{2}}\right )\\{{q}_{1}}&=\pm \left ({sin\frac{\phi }{2}cos\frac{\theta }{2}cos\frac{\psi }{2}-cos\frac{\phi }{2}sin\frac{\theta }{2}sin\frac{\psi }{2}}\right )\end{matrix}$

$\begin{matrix}{{q}_{2}}=\pm \left ({cos\frac{\phi }{2}sin\frac{\theta }{2}cos\frac{\psi }{2}+sin\frac{\phi }{2}cos\frac{\theta }{2}sin\frac{\psi }{2}}\right )\\{{q}_{3}}=\pm \left ({cos\frac{\phi }{2}cos\frac{\theta }{2}sin\frac{\psi }{2}-sin\frac{\phi }{2}sin\frac{\theta }{2}cos\frac{\psi }{2}}\right )\end{matrix}$

3.方向余弦矩阵->欧拉角

通过前面的欧拉角到方向余弦矩阵的变换公式，可以逆变换得到从发现余弦矩阵到欧拉角的变换，方便起见记 $C={{C}_{b}^{g}}$ ，则

$\begin{matrix}&\phi =\arctan{(}{{C}_{32}}/{{C}_{33}})\\&\theta =-\arcsin{(}{{C}_{31}})\\&\psi =arctan({{C}_{21}}/{{C}_{11}})\end{matrix}$

其中， ${{C}_{ij}}$ 表示方向余弦矩阵的第 $i$ 行第 $j$ 列元素。

4.四元数->方向余弦矩阵

$\left .{{{C}_{b}^{g}}=\left [{\begin{matrix}{{q}_{0}^{2}}+{{q}_{1}^{2}}-{{q}_{2}^{2}}-{{q}_{3}^{2}}&2({{q}_{1}}{{q}_{2}}-{{q}_{0}}{{q}_{3}})&2({{q}_{1}}{{q}_{3}}+{{q}_{0}}{{q}_{2}})\\2({{q}_{1}}{{q}_{2}}+{{q}_{0}}{{q}_{3}})&{{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}_{2}}{{q}_{3}}+{{q}_{0}}{{q}_{1}})&{{q}_{0}^{2}}-{{q}_{1}^{2}}-{{q}_{2}^{2}}+{{q}_{3}^{2}}\end{matrix}}\right .}\right ]$

5.方向余弦矩阵->四元数

根据四元数定义

${{q}_{0}^{2}}+{{q}_{1}^{2}}+{{q}_{2}^{2}}+{{q}_{3}^{2}}=1$

由四元数和方向余弦矩阵转换矩阵可得

$\begin{matrix}4{{q}_{0}^{2}}=1+{{C}_{11}}+{{C}_{22}}+{{C}_{33}}\\\\4{{q}_{1}^{2}}=1+{{C}_{11}}-{{C}_{22}}-{{C}_{33}}\\\\4{{q}_{2}^{2}}=1-{{C}_{11}}+{{C}_{22}}-{{C}_{33}}\\\\4{{q}_{3}^{2}}=1-{{C}_{11}}-{{C}_{22}}+{{C}_{33}}\end{matrix}$

$\begin{matrix}{{q}_{0}}=\pm \frac{1}{2}\sqrt{1+{{C}_{11}}+{{C}_{22}}+{{C}_{33}}}\\\\{{q}_{1}}=\pm \frac{1}{2}\sqrt{1+{{C}_{11}}-{{C}_{22}}-{{C}_{33}}}\\\\{{q}_{2}}=\pm \frac{1}{2}\sqrt{1-{{C}_{11}}+{{C}_{22}}-{{C}_{33}}}\\\\{{q}_{3}}=\pm \frac{1}{2}\sqrt{1-{{C}_{11}}-{{C}_{22}}+{{C}_{33}}}\end{matrix}$

6.四元数->欧拉角

利用上面的方向余弦矩阵与欧拉角和四元数的转换关系得到

$\begin{matrix}\phi =atan2(2({{q}_{2}}{{q}_{3}}+{{q}_{0}}{{q}_{1}}),{{q}_{0}^{2}}-{{q}_{1}^{2}}-{{q}_{2}^{2}}+{{q}_{3}^{2}})\\\theta =-asin(2({{q}_{1}}{{q}_{3}}-{{q}_{0}}{{q}_{2}}))\\\psi =atan2(2({{q}_{1}}{{q}_{2}}+{{q}_{0}}{{q}_{3}}),{{q}_{0}^{2}}+{{q}_{1}^{2}}-{{q}_{2}^{2}}-{{q}_{3}^{2}})\end{matrix}$

其中 $atan2(y,x)=atan(y/x)$

7.欧拉角、四元数、方向余弦矩阵、姿态表示方法的特点

姿态表示方法里面，欧拉角是比较直观的方法，用三个角度来衡量机体坐标和地面坐标之间的姿态倾转，在小角度时，基本上姿态角就等于倾角，但是它的缺点是非线性特点，在俯仰角接近90度的时候，欧拉角变量会呈现较大的非线性，在90度具有奇异点，不利于计算，它的特点是意义直观，所以欧拉角通常用于向用户表示姿态，而在内部计算和程序实现中，则通常采用四元数或方向余弦矩阵方法。

四元数和方向余弦矩阵的表示方法，都没有奇异点的问题，能够性能一致的表示360度全方位的姿态，在计算过程中，直接对四元数或者方向余弦矩阵数值进行迭代，相比之下四元数的表示更加简洁，计算量更小，而方向余弦矩阵则多一些冗余度和计算值，但是在无人机涉及的坐标转换计算方面更加便利直接，两者都有应用。

二、基本解算过程

        接下来，以一个通俗的方式进行具体的讲解
        在刚入门无人机开始的时候，会迷惑于一些概念，比如姿态角，欧拉角，旋转矩阵，四元数等等。这其中最容易让人搞混的就是姿态角和欧拉了。网上许多文章也是混用，导致许多人在阅读时会产生疑惑，姿态角是否就是欧拉角。
        首先，我们先定义一下接下来要使用的坐标系，分别是世界坐标系和机体坐标系。世界坐标系采用东北天机体坐标系，机体坐标系采用右前上的定义方式。

机体坐标系

姿态角，顾名思义就是描述飞机姿态的三个角度，分别是偏航角，俯仰角，横滚角。偏航角，为飞机机体坐标系Y轴在水平面上的投影与世界坐标系Y轴之间的角度。俯仰角，为飞机机体坐标系，Y轴与水平面之间的角度横滚角。横滚角，为飞机机体坐标系Z轴和Y轴所在铅锤面之间的角度。
而欧拉角其实是一种描述旋转的方式。刚刚提到的旋转矩阵，四元数与其是并列的概念，他们都是用来描述旋转的方法。欧拉角是这几种描述旋转的方法中比较直观，比较符合人类思维的一种方式。

欧拉角 $(\theta \phi \psi)$

四元数 $(q_0~q_1~q_2~q_3)$

旋转矩阵 $\begin{pmatrix}R_{11}&R_{12}&R_{13}\\R_{21}&R_{22}&R_{23}\\R_{31}&R_{32}&R_{33}\end{pmatrix}$

欧拉角怎么描述旋转的呢？其实它是相当于把原始姿态到目标姿态这个转化的过程分解，围绕X轴绕Y轴绕Z轴的三次旋转，同时旋转的顺序是很重要的。

假设我们用YXZ的顺序和XYZ的顺序来进行旋转，即使每次旋转的角度都一样。两种顺序的最终姿态也是不一样的。

需要注意的是，这里欧拉角旋转所绕着轴都是飞机机体坐标系的轴也就是内旋，也常称为动态欧拉角与之相对的就是绕世界坐标系的静态欧拉角外旋不管是动态，欧拉角还是静态，欧拉角实际上最终是可以等效的，只是用动态欧拉角来讲解会显得更加直观一点。在确定了内外旋的情况下，根据旋转顺序的不同，一共还有6种。

XYZ XZY YXZ

YZX ZXY ZYX

        那么这和姿态角有什么关系呢？其实真相已经呼之欲出了。这6种欧拉角中的其中一种绕XYZ三轴旋转的角度刚好与姿态角的定义相吻合，只要我们采用这种顺序的欧拉角，它和姿态角就是对应的关系。
        根据我们东北天右前上的坐标定义方式，按照ZXY顺序旋转的欧拉角刚好与姿态角相吻合。
        好了，说了这么多，其实也只是辨明了姿态角和欧拉角之间的关系。那么这和姿态解算有什么关系呢？一切的计算最终都要落到数学中。接下来我们来用公式的方式来重新观察一下刚刚所讨论的旋转。

先从最简单的二维旋转开始,假设在二维XY坐标系中存在一个向量，如果将坐标系逆时针旋转一定的角度，那么该向量在新坐标系中的坐标应该是什么，通过变量代换，我们可以简单的得到该向量在新坐标系下的表达式。

$\begin{matrix}x'&=x\cos{\psi }+y\sin{\psi }\\y'&=-x\sin{\psi }+y\cos{\psi }\end{matrix}$

如果是矩阵的形式表达的话就是这样：

$\left .{\left ({\begin{matrix}x'\\y'\end{matrix}}\right .}\right )=\left ({\begin{matrix}\cos{\psi }&\sin{\psi }\\-\sin{\psi }&\cos{\psi }\end{matrix}}\right )\left ({\begin{matrix}x\\y\end{matrix}}\right )$

这是二维的情况：

二维坐标系旋转

实际上我们可以将刚刚的旋转看作是三维坐标系下绕Z轴所做的旋转。这样的话我们就可以得到绕Z轴的旋转矩阵。

${{R}_{z}}(\psi )=\left .{\left ({\begin{matrix}\cos{\psi }&\sin{\psi }&0\\-\sin{\psi }&\cos{\psi }&0\\0&0&1\end{matrix}}\right .}\right )$

同样的道理，我们可以分别得到绕Y轴和绕X轴的旋转矩阵。

$\left .{{{R}_{y}}(\theta )=\left ({\begin{matrix}\cos{\theta }&0&-\sin{\theta }\\0&1&0\\\sin{\theta }&0&\cos{\theta }\end{matrix}}\right .}\right )$

$\left .{{{R}_{x}}(\phi )=\left ({\begin{matrix}1&0&0\\0&\cos{\phi }&\sin{\phi }\\0&-\sin{\phi }&\cos{\phi }\end{matrix}}\right .}\right )$

有了这三个轴，单独的旋转矩阵，我们可以将其组合成一个整体的旋转矩阵，将三个旋转矩阵按照ZXY的顺序右乘。

${{R}_{zxy}}={{R}_{z}}{{R}_{x}}{{R}_{y}}$

$=\left ({\begin{matrix}&\cos{\psi }\cos{\theta }+\sin{\theta }\sin{\phi }\sin{\psi }&\cos{\phi }\sin{\psi }&\cos{\theta }\sin{\phi }\sin{\psi }-\sin{\theta }\cos{\psi }\\&-\cos{\theta }\sin{\psi }+\cos{\psi }\sin{\phi }\sin{\theta }&\cos{\phi }\cos{\psi }&\cos{\psi }\cos{\theta }\sin{\phi }+\sin{\theta }\sin{\psi }\\&\cos{\phi }\sin{\theta }&-\sin{\phi }&\cos{\theta }\cos{\phi }\end{matrix}}\right )$

这个旋转矩阵也叫方向余弦矩阵，或者叫欧拉旋转矩阵。通过这个旋转矩阵，我们可以很方便的求出坐标系一中的某个向量，在坐标系二中的坐标。
例如，加速度计可以测量三个轴的加速度，当加速度计，水平静置在地面的时候，它的测量的三轴加速度显然是

$\left .{\left ({\begin{matrix}0\\0\\g\end{matrix}}\right .}\right )$

可以将其理解为重力加速度在世界坐标系下的坐标。
当飞机飞在空中，有一定的姿态角之后，我们又可以测出此时的三重加速度，分别是

$\left .{\left ({\begin{matrix}{{A}_{x}}\\{{A}_{y}}\\{{A}_{z}}\end{matrix}}\right .}\right )$

显然这个就是重力加速度在机体坐标系下的坐标。因此我们有了世界坐标系以下的重力加速度矢量，通过旋转矩阵可以得到重力加速度，在机体坐标系下的分量。

$\left .{\left ({\begin{matrix}{{A}_{x}}\\{{A}_{y}}\\{{A}_{z}}\end{matrix}}\right .}\right )={{R}_{zxy}}\left ({\begin{matrix}0\\0\\g\end{matrix}}\right )$

$\left .{\left ({\begin{matrix}{{A}_{x}}\\{{A}_{y}}\\{{A}_{z}}\end{matrix}}\right .}\right )=\left ({\begin{matrix}\cos{\theta }\sin{\phi }\sin{\psi }-\cos{\psi }\sin{\theta }\\\cos{\psi }\cos{\theta }\sin{\phi }+\sin{\theta }\sin{\psi }\\\cos{\theta }\cos{\phi }\end{matrix}}\right )g$

同时，由于重力加速度总是与水平面垂直，因此实际上凭借重力加速度，我们是无法感知偏航角的变化的。仅有加速度计的情况下，偏航角转了跟没转一样。
既然如此，我们就可以干脆令天偏航角为0，实际上也就是把绕Z轴旋转的那个矩阵给去掉，于是又有

$\left .{\left ({\begin{matrix}{{A}_{x}}\\{{A}_{y}}\\{{A}_{z}}\end{matrix}}\right .}\right )=\left ({\begin{matrix}-\sin{\theta }\\\cos{\theta }\sin{\phi }\\\cos{\theta }\cos{\phi }\end{matrix}}\right )g$

这个方程就简单许多了，我们只需构造一下反正切就可以求出俯仰角和横滚角了。

总结一下，欧拉角是一种描述旋转的方法。特定顺序下的欧拉角与姿态角相吻合，求出此时的欧拉角即可得到姿态角。在东北天右前上的坐标系下特定顺序指的就是ZXY。利用加速度计和旋转矩阵可以计算得到俯仰角与横滚角。不过，由于加速度计对噪声敏感，因此实际飞行中仅凭加速度计来获取姿态很不可靠的。一般我们还会加上陀螺仪来与之融合。

爱尔兰的楠小楠

关注

20
点赞
踩
38

收藏

觉得还不错? 一键收藏
打赏
1
评论
欧拉角与姿态解算

常用的姿态表示方法有欧拉角、方向余弦矩阵、四元数这几种。欧拉角表示方法采用来表示飞行器的姿态，其中为滚转角，为俯仰角和为航向角，表示飞行器首先航向偏转角度，再俯仰角度，然后机体滚转角度得到的姿态。方向余弦矩阵通过机体坐标和地面坐标的转换矩阵（DCM, Directional Cosine Matrix）来表示机体的姿态。四元数通过四个元数来表示飞行器全方位的姿态，它的特点是表征方式简洁，并且没有奇异点。机体坐标系1.欧拉角->方向余弦矩阵其中，
复制链接

扫一扫