关于无人机四元数解算姿态角解析你知道吗？_bortz 方程

原文链接http://www.elecfans.com/d/705815.html

一、概述

无人机求解姿态角有多种算法，但由于各种算法的自身限制及计算机计算速度的限制，所以我们需要选择一个较佳的求解算法，下面我们先来看看几种求解姿态角的算法：

 

1. 欧拉角法：

欧拉角法（又称三参数法）是欧拉在1776 年提出来的，其原理是动坐标系相对参考坐标系之间的位置关系可以用一组欧拉角来描述。解算欧拉角微分方程只需要解三个微分方程，与其它方法相比，需要求解的方程个数少一些但在用计算机进行数值积分时，要进行超越函数（三角函数）的运算，从而加大了计算的工作量。用此方法求解得到的姿态矩阵永远是正交矩阵。在进行加速度信息的坐标变换时变换后的信息中不存在非正交误差，得到的姿态矩阵不需要进行正交化处理。当载体的纵摇角(俯仰角）为90 °时，将出现奇点，因此该方法不能进行全姿态解算,其使用存在一定的局限。

2. 方向余弦法：

方向余弦法（称九参数法）用矢量的方向余弦来表示姿态矩阵的方法。绕定点转动的两个坐标系之间的关系可以用方向余弦矩阵来表示。方向余弦矩阵是随时间变化的，其变化规律的数学描述就是方向余弦矩阵的微分方程，方向余弦矩阵的即时值就可以通过求解该微分方程而得到。该方法求解姿态矩阵避免了欧拉角法所遇到的奇点问题，可以全姿态工作。但方向余弦矩阵具有九个元素，所以需要解九个微分方程，计算工作量较大，在工程上并不实用。

3. 三角函数法：

三角函数法(又称六参数法)是将绕定点转动的两个坐标系之间的关系用三次转动等效地表示，将三次转动角度的正、余弦函数来表示姿态函数。该方法虽然避免了欧拉角法的缺点，可以全姿态工作，但需要解六个微分方程，计算量也不小，工程上并不实用。

4. Rodrigues参数法：

Rodrigues 数法是法国数学家Rodrigues 在1840 年提出的，该方法所描述的姿态是唯一的，并且具有简洁、直观的优点，其微分方程结构简单，无多余约束，计算效率优于当前广泛使用的四元数法。由于Rodrigues 参数法存在旋转角有奇异值的缺陷 ，因此限制了其在工程上的应用。Schaub 和Junkin 对该方法的缺陷，改进后仍然存在奇异值。但是Rodriguess 参数法仍不失为解算姿态的有效途径。

5. 四元数法：

四元数法 (又称四参数法) 。英国数学家W.R.Haminlton 在1843 年在数学中引入了四元数。但直到20 世纪60 年代末期这种方法还没有得到实际应用，随着空间技术、计算技术SINS 技术的发展，四元数才引起人们的重视。求解四元数微分方程要解四个微分方程。虽然要比解欧拉微分方程多一个方程，但其优越性在于计算量小、精度高、可避免奇异性，该方法是目前研究的重点之一。由于方向余弦法在对载体姿态动力学求解时会产生歪斜、刻度和漂移误差等，然而，SINS 中在进行姿态求解时估计出这些误差是很重要的。与方向余弦法相比，四元数法的优点在于不仅歪斜误差等于零；而且刻度误差的推导很简单，能得出便于进一步分析的解析表达式，而方向余弦法只有在特殊的情况下才能分析和检测到刻度误差，且不能得出通用的结论。通过从不同角度对欧拉角法、方向余弦法和四元数法进行对比。结果表明四元数法具有最佳的性能。

6. 等效旋转矢量法：

由于欧拉角法、方向余弦法、三角函数法和四元数法对于圆锥运动都有原理误差，而Rodrigues 参数法又存在奇异值，因此人们又致力于研究能有效抑制圆锥运动所产生的不可交换性误差的算法。由于刚体的有限转动不是矢量，其转动次序不可交换。无限小转动才是矢量。 1971 年Bortz 提出的转动向量微分方程为计算SINS 的姿态矩阵建立了全面的理论基础。用该方法描述绕定点转动的两个坐标系之问的关系被称为等效旋转矢量法。转动向量的变化率是惯性测量的角速度向量与计算得到的非互易速率向量二者之和，后者是影响SINS 姿态角精度的一个重要因素，因此，在高角速率动态环境中，为防止姿态误差积累，必须对非互易速率向量进行补偿。对非互易向量进行补偿的计算一般称为圆锥补偿算法。要提高算法精度，可以有两种选择，一种是对陀螺输出信号进行高频率采样，使用简单的圆锥算法；另一种是适当降低对陀螺输出信号进行采样的频率，使用复杂但精度高的圆锥算法。

从以上六种算法的介绍中，每个算法求解欧拉角都有其自身的小缺点，但通过综合对比，在四轴上用来解算欧拉角的更好的算法是四元数法，虽然其也有缺点，但其优越性在于计算量小、精度高、可避免奇异性，因此被大多数人所采用。所以下面我们将接收通过如何通过四元数来计算我们所需要的姿态角。

二、基本概念

地理坐标系：

习惯上，我们以正北方(x轴)、正东方(y轴)、垂直指向天空(z轴)建立地理三维直角坐标系(符合右手定则建立)。图形示例如下：

机体坐标系：

习惯上以机体正前方(x轴)、机体正右方(y轴)、机体垂直正上方(z轴)建立机体坐标轴。图形示例如下：

MPU定义坐标系：

令芯片表面朝向自己，将其表面文字转至正确角度，此时，以芯片内部中心为原点，水平向右的为x轴，竖直向上的为y轴，指向自己的为z轴。见下图：

向量点积：

向量的叉乘(也叫向量的外积,定义符号“×”为两向量叉乘符号)：

物理意义：

利用向量的叉积可以创造出一个新的维度，而这个维度是独立于(垂直于)先前这个空间的，因此，在这个新的维度空间可以当成目标运动的参照系。图形示例如下：

旋转向量与旋转矩阵的联系：

处理三维旋转问题时，通常采用旋转矩阵的方式来描述。一个向量乘以旋转矩阵等价于向量以某种方式进行旋转。除了采用旋转矩阵描述外，还可以用旋转向量来描述旋转，旋转向量的长度（模）表示绕轴逆时针旋转的角度（弧度）,下文将会推导旋转矩阵。这里用图片来表示旋转向量。

四元数：

1.为什么会用到四元数

2. 四元数

通过上述了解，我们知道了四元数可以表示三维空间的旋转信息，那接下来我们先来了解下四元数的基本概念及运算法则。

1) 四元数定义：

在数学中四元数定义为

上述关系可以叙述为：相同单位向量进行四元数乘法呈虚单位特性，相异单位向量进行四元数乘法呈两向量叉乘特性，故四元数可以看作一个超复数(超复数是复数在抽象代数中的引申，以高维度呈现。),也可看作四维空间中的一个向量。

2) 四元数的表示形式：

四元数有多种表现形式，有矢量式、复数式、三角式、矩阵式、指数式，下面我们简单看一下他们各自的定义式。

A. 矢量式

B. 复数式

C 三角式

D.矩阵式

3)四元数的大小

数学中用四元数的范数来表示四元数的大小，具体如下：

4) 四元数的运算法则

A. 加法与减法

若有如下两个四元数：

B. 乘法

乘法又分四元数与四元数相乘、四元数与标量相乘，具体如下：

a. 与标量相乘

b. 与四元数相乘

若有如下两个四元数：

C. 除法(称为求逆)

逆的定义：

也即有

三、利用四元数求解姿态变换矩阵

前面我们讲过，四元数可以描述三维空间的旋转信息，也即是我们可以通过四元数求出一个物体相对于一个坐标系旋转之后的坐标信息。

由于人在地面上看运动物体时，是以地理坐标系为参照系的，所以当我们想知道物体发生了怎样的变化时，就得研究物体相对于参照坐标系到物体坐标系转化之间的坐标关系。由于地理坐标系与物体坐标系均为直角坐标系，各个轴之间均为直角，当我们只研究两个坐标系之间的角度变化时，可将物体(等效为刚体)与坐标系固联，也即是，两个坐标系之间的角位置关系可用刚体的转动来表示。

那么物体从一个姿态变化到另一个姿态，可等效为物体绕了某一个轴通过无中间过程的一次旋转完成，但实际物体可能经过了多次中间过程才变化到了我们想要的姿态，我们不去关心中间过程，我们只需找到一种变化关系，通过这种变换关系，可求出物体从地理坐标系变到物体坐标系坐标或者从物体坐标系变到地理坐标系的坐标。

变换关系的推导：

通过上述内容，我们可以知道，当我们研究两个坐标之间的角度变化关系(地理坐标系与物体坐标系)时，取物体坐标系与物体固联，物体坐标系与物体之间角位置始终不变，因此，我们可以以物体(视为刚体)定点转动问题来推导我们想要的变换关系，下面，我们就来推导一下我们想要的变化关系。

由图可知：

所以有

则有

由于未旋转之前，物体坐标系(b系)与地理坐标系重合，则有

对于物体来说，在转动过程中，位置向量与物体的坐标系之间的相对角位置始终不变，即有

所以记D为物体坐标系至地理坐标系的坐标变换矩阵，记为

我们已经知道四元数的范数定义为：

所以可以得出描述物体转动的四元数为规范化四元数。

四、欧拉角推导

我们以MPU规定的坐标系作为公式推导的地理坐标系和起始物体坐标系，将芯片水平放置，以芯片内部中心为原点，水平向右的为x轴，竖直向上的为z轴，指向正前方的为z轴。具体如下：

我们求物体的姿态角(在地面看物体运动)时，物体旋转过后相对于之前的角度变化信息可以等效为物体依次绕三个轴旋转复合得到，我们规定绕z轴旋转称物体的航向角(ψ)、绕y轴旋转称物体的俯仰角(γ)、绕x轴旋转称物体的翻滚角(θ),下面我们先推导物体分别绕三个轴的变换矩阵，最后根据三个变换矩阵复合得到物体相对于地理坐标系的角度信息。具体推导如下：

1. 物体绕z轴旋转

物体绕z轴旋转，可知z轴不变，x、y轴的变换关系如下图所示：

将以上三式写为矩阵形式则得到绕Z轴旋转ɑ角度后的坐标关系，如下所示：

2. 物体绕Y轴旋转

物体绕Y轴旋转，可知Y轴不变，x、z轴的变换关系如下图所示：

将以上三式写为矩阵形式则得到物体绕Y轴旋转β角度后的坐标信息，如下所示：

3. 物体绕x轴旋转

物体绕x轴旋转，可知x轴不变，y、z轴的变换关系如下图所示：

将以上三式写为矩阵形式则得到物体绕x轴旋转γ角度后的坐标信息，如下所示：

注：确定了第一个旋转轴后，该旋转轴变为父级(如z轴)、确定了第二个旋转轴后，第二个旋转轴(y轴)变为另一个轴(x轴)的父级，所以我们计算姿态矩阵时，顺序应为子级(x)(相对于y而言)、子级(y)(相对于z而言)、父级(z)，也就是按上述顺序进行矩阵相乘复合。

对(34)化简得

以上得到了从地理系变换到物体系的姿态角变化矩阵，此前我们已经得到了利用四元数表示的从物体系变换到地理系的姿态变化矩阵，具体矩阵如下：

由(36)(由刚体定点转动推导的无中间过程的姿态矩阵)、(37)(由物体分别绕z、y、x轴旋转复合所得姿态角矩阵)，两式均为物体坐标系变换到地理坐标系姿态矩阵，所以两式矩阵相等，也即是有两矩阵元素一一对应相等，则有

则根据以上五个式子，可求得姿态角如下：

这里需要注意：当我们求姿态角矩阵时，旋转顺序不一样，得到的求角公式是不一样的，求角的时候不能一味的套用公式，使用之前，应先了解一下旋转顺序。

至此，我们已经求出了姿态变换矩阵以及通过构造四元数，用四元数的方法来求得对应的姿态角，但我们现在存在一个问题是，我们不知道四元数具体数值，所以我们要求出真正的姿态角，必须找到求出四元数具体值的办法。

五、四元数参数求解

通过上述推导，若我们知道了q0、q1、q2、q3的具体数值，我们就能求出我们需要的姿态角，我们现在的已知量只有从陀螺仪和加速度计获得的角速度和加速度，那么我们怎么通过已知信息来求解四元数的具体数值呢，下面，我们引入四元数微分，看从中能不能获得有用信息。

1. 四元数微分

由(44)式可以知道，我们通过陀螺仪获得角速度可以求出四元数参数，但我们需要求解一个四元数微分方程，通过求解微分方程，就可以得到我们需要的四元数参数，那么怎么求解一个微分方程呢？、

2. 解微分方程

在解四元数微分方程之前，我们先以一个数学微分方程表达式来找到求其解的方法。

设有一微分方程

六、陀螺仪误差的消除

前已提到，由于角度是由角速度积分得到，而积分时，若从陀螺仪获得的角速度信息存有小的偏差，经过积分之后，就会使误差加大，从而使获得的角度存在偏差，但利用加速度计获得的角度信息不会出现偏差，可是我们也不能直接利用加速度计获得的角度信息，因为加速度计受噪声影响较大，在飞行过程中受振动比陀螺仪明显，短时间内可靠性不高，但积分后的角度信息是可信的，所以我们需要用加速度计获得的角度信息去矫正陀螺仪获得的姿态信息，从而使算出来的角度误差消除。    根据我们的思路，利用加速度计获得的信息去补偿陀螺仪的角速度信息，具体有如下步骤实现：

获取加速度计的值(为物体坐标系下对应的值)，对其归一化(归一化的原因是因为姿态变化矩阵中的四元数是规范四元数，利用陀螺仪去更新的四元数也要归一化，所以加速度计获得的值也需归一化才能是两者对应)。记从加速度计获得的值为ax、ay、az(分别对应x、y、z轴的值)，其归一化方法如下：

2. 获取陀螺仪算出的姿态矩阵中的重力分量(因为加速度计是测得的物体坐标系下的值，所以我们也要提取利用角速度算出的姿态矩阵中的物体坐标系下的重力分量)。重力分量记为Vx、Vy、Vz，具体计算方法如下：

通过四元数计算的从E系(地理坐标系)变换到b系(物体坐标系)姿态矩阵为：

4. 对误差进行积分，从而消除误差，设accex、accey、 accez为x、y、z三轴对应的误差积分结果(对两个重力分量叉乘后的误差进行积分，结果得到角速度值)，ki为积分系数，dt为积分周期时间具体如下(我们程序未用这一步)：

5.互补滤波，将误差输入Pid控制器与本次姿态更新中陀螺仪测得的角速度相加，得到一个修正的角速度值，获得的修正的角速度值去更新四元素，从而获得准确的姿态角信息。设gx、gy、gz为陀螺仪测得的三个轴的角速度及滤波后的角速度修正值，Kp为互补滤波系数，则修正角速度计算方法如下：

至此，我们将利用四元数求解姿态角的算法讲解结束了，但我们讲解顺序是从后到前一步一步推出我们需要的信息，在实际程序编写过程中，我们需要从后往前对程序进行编写，具体步骤我们引用一位网友给出的思维导图，可以根据该思维导图对程序进行编写：

下面我们根据思维导图用程序来一步一步实现如何求解欧拉角：

1. 定义初始四元数的值为q0=1,q1=0,q2=0,q3=0。

2. 读取加速度计值、角速度值，程序定义变量分别为ax、ay、az，gx、gy、gz，将陀螺仪值转为弧度，转换如下：

3.对加速度值进行归一化

4.提取姿态矩阵中的重力分量，我们已经其数学计算公式为

5.求姿态误差，对两向量进行叉乘(定义ex、ey、ez为三个轴误差元素)，数学计算为：

6.对误差积分(定义accex、accey、accez为积分值、ki=0.001为积分系数、dt=0.005为积分周期时间),其程序实现为(目前程序里未使用这一步)：

7.互补滤波，将误差输入PID控制器后与陀螺仪测得的角速度相加，修正角速度值，程序实现如下(Kp为互补滤波系数这里取Kp=0.8，实际值根据需要进行调整)：

10. 计算姿态角，数学公式为

至此，我们就按照思维导图，一步一步实现了用程序语句求解姿态角了。