Pixhawk学习6.1——姿态解算

19年12月份写完传感器标定之后停了得有两个半月了，主要是因为年底试验事情比较多，个人也比较心烦，所以就停滞了。过完年疫情严重，在家里隔离办公，整好有心思整理和学习了一些pix的内容，今天一并发出来。

Pixhawk的姿态解算主要有几种方法。互补滤波、基于mahony算法的DCM和扩展卡尔曼滤波（EKF）。互补滤波的内容实在是太多了，而且玩具级的小飞控基本都采用这种简单的方式，主要思路就是将陀螺和加计分别计算得到的姿态值加权求和，算法和思路都比较简单，在此不再赘述。

本节主要讨论一下基于Mahony的DCM算法，主要分为DCM的核心说明、方向余弦矩阵的归一化、陀螺漂移的获取三部分。这部分内容在原先APM中是有的，但是在最新的pixhawk代码中我没有找到。但是并不影响它的重要性，我认为该算法的基础分析特别好，对于我这种惯导专业出身的人来讲思路比较新。

1.DCM核心

DCM，方向余弦矩阵。是由刚体的三维姿态组成的一个坐标转换矩阵。飞机上的惯性测量单元（IMU）一般包括三轴陀螺仪和三轴加速度计。陀螺仪用来测量机体角速度，而加速度计用来测量机体加速度。在这里首先要说明的是，惯性器件测量的物理量都是基于惯性系下的。
一般涉及到惯性坐标系（i）、地理坐标系（导航坐标系）（n）、机体坐标系（b）。
飞机的姿态解算，根本上是要知道机体坐标系相对于导航坐标系的角度，包括航向角（ pesai），俯仰角（ theta）和横滚角（ phai）。而通过两个坐标系之间可以经过一定的旋转顺序（旋转这三个角度）达到重合。姿态解算的核心就是要根据姿态矩阵微分方程，不断更新Cbn 矩阵，从而得到飞机姿态角。姿态矩阵微分方程为：

1.1 该核心方程的来历

定义机体坐标系（前右下），导航坐标系（北东地）。
设S1为导航坐标系，其单位坐标矢量是i，j和k；
设S2为载体坐标系，其单位坐标矢量为i’，j’和k’；
则可表示为（后面这个反对称矩阵用 “欧米伽nbb” 表示，插不进来公式好痛苦啊！！！）：

括号内为矢量点乘。那么当导航系固定，机体系运动时， Cbn的微分可表为（CSDN不能编辑公式实在是太抓狂了！！！！）：

1.2 欧米伽nbb的计算

为机体相对于导航系的角速度在机体系上的分解，并非是机载陀螺直接测到的角速度（后续有惊喜！！！）
该式为以光衢《惯性导航原理》中的内容。
其中， wibb为机载陀螺测到的角速度， wien为地球自传角速度（约15度/小时），wenn 为牵连角速度，即随飞机运动坐标系原点的角速度。

1.3 Cbn或者Cnb的作用（两个矩阵是互为转置，互为逆）

定义，横滚角 phai为右翼向下，左翼向上为正；俯仰角theta 为机头向上为正；航向角 pesai为机头向右偏转为正（顺时针为正）。

同样，在四元数方面，导航坐标系（n）到机体坐标系（b）的三次旋转可表示为：

航向角顺时针为正。

1.4 计算机数值计算

微分方程的数值解法有很多种，欧拉法、龙格库塔法等。一般，在满足性能要求要求的情况下，尽量使用一阶数值积分方法（欧拉法）。

2.方向余弦矩阵的归一化

2.1 造成 不正交的原因

方向预先矩阵满足以下特性：
#Cbn的逆和转置相等， ；
#三个列或者行上的向量是互相垂直的，三个行列向的向量等于1.
在Cbn更新的过程中应该一直满足上述特性。但数值积分会引入数值误差。无法得到与符号积分相同的结果。主要会引入以下两种数值误差：
1、积分误差：数值积分采用有限时间步长和具有优先采样率的数据。如果在步长内旋转角速度有变化，则会引入积分误差；
2、量化误差：计算机模数转换或者计算截断都会存在量化误差。

姿态余弦矩阵的基本特性就是它的正交性，上述误差会破坏这一特性，而如果不做处理，不断积累下去，会导致行列向量不是单位向量，以及列向量不相互垂直。

2.2 归一化的方法

根据姿态余弦矩阵的特性，任意两列或者两行之间的是相互垂直的，那么计算X轴和Y轴的内积（点乘），结果应该是0。如果不垂直，那么该值整好放映出不垂直的程度。

将该误差分配给X和Y，并让其产生朝相互垂直的方向转动，具体为：

可以验证，上式的正交性要好于原来的X和Y。
对于Z轴，强制采用Xmod和Ymod的外积（叉乘）进行正交化。

至此，解决了三个向量正交化的问题。再来解决向量模值为1的问题：
鉴于每行的模值都不会跟1相差太远，可以采用泰勒展开，计算方法如下：

3. 欧米伽nbb的获取

3.1 欧米伽nbb存在的问题

上述已经说明了欧米伽nbb的获取过程。但是在以下文章中：


没有考虑地球自转、牵连角速度等，而是将这些项全部归结到陀螺漂移中。
（上述说的惊喜的地方！！！）
考虑主要有以下原因：
#小无人机的IMU精度有限，这些项相对飞机真实运动角速度来说比较小，被淹没在陀螺的噪声和误差中；
#小无人机的运动速度较小，飞行范围不大，飞行时间较小，因此地球自转、牵连角速度可以忽略。
不同于传统惯导姿态更新，类Pixhawk飞控中的姿态解算除了上述过程外，还要根据外界GPS、磁力计、加计等信息对陀螺漂移进行补偿。而这些传感器的共同有点是，误差不漂移。
补偿原理是：采用参考矢量，与姿态矩阵中的对应参考矢量做外积（叉乘），它代表一个旋转轴和旋转量。需要旋转这个测量的矢量使它变得与这个估计的矢量平行。通过比例积分控制器把它反馈到陀螺仪，定位估计逐渐被迫跟踪参考向量，这样陀螺的漂移就被消除了。

3.2 采用GPS信息获得X轴的参考，校航向（前提是飞机必须要移动，目前看适用于固定翼）

对地GPS的航向矢量和IMU滚转轴（X）在水平面上的投影意味着漂移量。旋转修正是R矩阵X列对地航向矢量叉乘的Z分量。
首先用GPS的对地航向构建参考矢量：

与X列进行叉乘：

取Z分量作为偏航修正量：

该修正量为地理系下的修正量，需要转换到机体系下：

3.3 采用GPS信息获得X轴的参考存在的问题

#GPS必须在动态情况下才有效；
#在有风的情况下飞机有侧滑，而侧滑角会被归结于是陀螺漂移引起的机体航向与GPS航向不重合，最终补偿在了陀螺漂移中，而当飞机调180°头后，该误差会被2倍放大。

3.4 采用加计信息获得Z轴的参考，校水平（前提是飞机的加减速较小）

采用加速度计对重力加速度的测量矢量和IMU偏航轴（Z）的叉乘，来校准水平。
加速度计测量的是机体受到的合力产生的加速度。除了重力以外，主要的还有向心加速度和运动加速度。由于飞机不能一直加速或者一直减速，且一般正加速度和负加速度是成对出现的，因此先不考虑这一部分（想考虑也没办法考虑），而向心加速度可以转动角速度和线速度的叉乘得到，因此，机体系下的重力矢量参考测量值为：

通过计算方向余弦矩阵的第三行和重力参考矢量归一化后值的外积（叉乘），得到水平修正量：

3.5 采用加计信息获得Z轴的参考存在的问题

没有考虑运动加速度的干扰。（靶机弹射阶段的姿态计算）

4.反馈控制器

设置一个反馈控制器，将上述采用外界参考得到的陀螺仪修正量通过控制器叠加在陀螺数据上，达到补偿陀螺漂移的目的（犯懒直接截图）。