姿态结算相关-----姿态的表示和传感器

这两天忙着整理资料给学弟们培训，又把姿态解算这部分拾了起来，其实更主要的原因是我有一门课的期末作业就是姿态解算。

先列出这些对我有很大帮助的博文,这几位博主也是我做四轴时的几位网上导师啊！

梯度下降法推导与解读：http://www.tuicool.com/articles/JvEZFz

                                            http://blog.sina.com.cn/s/blog_81f1e268010181v3.html

                                            http://www.openedv.com/posts/list/49740.htm

 磁力计校准：http://blog.exbot.net/archives/1753

                         http://www.amobbs.com/thread-5483833-1-1.html

                         http://www.sensorsmag.com/sensors/motion-velocity-displacement/compensating-tilt-hard-iron-and-soft-iron-effects-6475

  卡尔曼滤波理解：http://www.geek-workshop.com/thread-15257-1-1.html

                                  http://blog.csdn.net/lizilpl/article/details/45268471

mpu9150姿态解算：http://bbs.21ic.com/icview-605405-1-2.html

祭拜完各位先贤，我也说一说我对姿态解算的理解吧，已经是至少第三遍看这部分内容了，每次都有新的收获，这或许就是书读百遍其义自见吧！

我们说飞行器做的动作要让人们看上去是正确的，其实就是地理坐标系(N)和机体坐标系(B)相互换转的一个过程(以下简称N2B, B2N)。一开始我们假设B系与N系是重合的，然后B系开始旋转，旋转呢有很多种旋转方法，

一是可以每次分别绕自己的一个坐标轴旋转(这就是内在旋转啦)，每次旋转都会得到一个旋转算子，假设绕Z轴旋转得到的是CZ(对应下图第一个矩阵)，绕Y轴旋转的得到的是CY(对应下图第二个矩阵)，绕X轴旋转得到的是CX(第三个矩阵)，

依此左乘这些旋转算子C=CX*CY*CZ，这样最终得到的B系就可以用这个大C来表示了。这就是方向余弦矩阵啦！注意，考虑到矩阵乘法不满足交换律，不同的旋转顺序会得到不同的方向余弦矩阵。下图是Z-X-Y顺序的方向余弦矩阵

二是可以绕空间中一个固定的轴来旋转，这个轴不必是自己的坐标轴，可以是任意一个单位向量，由于四元数可以表示绕一个刚体定点转动的过程(见下图)，所以我们借助四元数来表示B系的旋转过程,由于主动量是机体坐标系B，我们假设一开始时有个向量R在两个重合的坐标系中，所以对于R来说B系的整个旋转过程可以表示为R‘ = q-1 * R *q.  (q-1是指q的逆，这里不对四元数的定义和基本运算作介绍). 根据四元数的乘法，我们用R的旋转公式导出了四元数表征的一个矩阵，这个矩阵是唯一的，不像方向余弦矩阵那样可能会因为旋转顺序不同而不同，因为我就是绕着一个固定的矢量旋转的啊！

四元数表征的旋转矩阵，---可以与方向余弦矩阵对应起来哦！

注意，我们说到现在，所有的操作都是基于一开始B和N重合，然后B系从N系旋转的一个过程，所以我们推导得到的这两个公式都是N2B的，这也是我们所关心的转换方向，只有N2B矩阵正确的表示了旋转的过程，此时才能说我们飞行器所做的动作在地理坐标系中看上去是正确的。所以我们关心的是N2B，毕竟内在旋转定义的欧拉角是在机体坐标系B中的。

好了，说完了姿态的表示，我们明白了其实就是这个转换矩阵，那么如何利用机体上的这些传感器得到这个正确的转换矩阵呢？这就是我们算法要做的事了。

现在手头上有陀螺仪、加速计、磁力计这三个传感器(要想定高和定位还要用气压计和GPS)，那么这三个传感器是用来干嘛的呢？

陀螺仪输出的是三轴角速度，既然有了角速度，那我们直接积分不就得到了三轴角度了吗？然而现实总是残酷的，光用陀螺积分会发现出现积分漂移，短时间内还好(一两分钟)不会飘多少，放在那时间一长就飘的找不到北了。。。

那咋整呢？对了，手上还有个加速计，这个加速计测得是啥，加速度？非也非也，大家不要被他的名字给骗了，准确的说他测的是惯性力，在匀速和静止状态下他的三轴输出其实就是重力加速度的分量。这样一来，在静止和匀速(低速运动)时我们直接用加速计也可以算得各轴旋转角度。好了结合陀螺仪的动态性能和加速计的静态性能，我们是不是可以较为准确的结算到三轴角度了？非也非也，原因就出在这个重力加速度上。我们说加速计是不区分外力加速度和重力加速度的，因为在静止时它Z轴就输出一个g，做自由落体时它Z轴反而输出0，所以说加速计无法正确的感知Z轴的运动，这样一来Z轴的角度算出来就会有问题(表现出来就是Z轴没有被误差校准，还是随着角速度积分产生的漂移飘阿飘)。不过，事物都是两面性的，既然这个重力加速度g干扰了我加速计的工作，它同时也为我们带来一个基准，因为重力加速度g是地理坐标系下的，而且他是一个定量，我们暂且将它表示为G={0,0,1}.这就为我们从地理坐标系N正确的转换到机体坐标系B提供了一个基准。

刚刚提到了加速度传感器没能校正我的Z轴角度，那咋整呢？手上还有个磁力计，也叫电子罗盘。磁力计输出的是三轴磁场强度(是你机体坐标系B中的磁场强度)，那我先来看看地球上的磁场到底是怎么一回事

                                         

地球的磁场就张这么个鬼样子，不解释了。当我们站在北半球(比如我现在南京)，我脚下的磁场矢量的方向实际上是跟地面有一定角度的。将这个磁场矢量做三轴分解，对水平方向的两个分量来说，他们的矢量和总是指向磁北的。那么我们不妨假设X轴就指向磁北，这时我们就引入了地球磁场的一个矢量记作H={hx, 0, hz}.这也是地理坐标系中的哦，所以也可以当作我们矫正误差的一个基准。再来看看我们机体坐标系B中的磁力计，其实只要由于罗盘保持水平，只需要用磁力计水平方向两轴（通常为X轴和Y轴）的检测数据就可以计算出航向角(当前方向和磁北的夹角)。那不就妥了，现在我们有了基准H，又能计算出Z轴的偏航角，不用加速计也能直接校准陀螺了啊！这句话从理论角度出发是没毛病，但是我们做工科的，不能天天纸上谈兵！刚刚提到了"只要罗盘保持水平"，现实中哪能真的一直让飞机处于绝对水平的状态。更重的问题是，磁力计在倾斜时算出的偏航角也是不正确的，这就需要给它做一个倾角补偿。不单单如此，磁力计磁力计，顾名思义，肯定会受周围磁场的影响，没做保护措施的话，飞机上的电机线圈产生的磁场和电池电流产生的磁场都会影响到磁力计的输出。所以，从倾角补偿的目的出发，加速计还是不能丢的。

ok，现在从传感器得到了机体坐标系下的原始数据，消除误差的基准也都有了(为什么要这两个基准呢，因为转换矩阵是从N2B的，你一直用B系中的数据自己校正自己的坐标系那叫什么事啊)，我们说得到正确的转换矩阵就是让它的误差逼近0.那我们用什么方法来让这个误差逼近0呢？这就引出了解算的几种算法，根据方法的不同我们可以将其分为以下几类：

互补滤波法：将误差做一个PI控制，反馈给陀螺，以此达到校正陀螺的目的

梯度下降法：取误差函数导数最大处(我在推导过程中没看见有任何使导数最大成立的条件，所以一般意义上的梯度下降法只是求个梯度，并没有让它最大)，此处的梯度肯定最大，那我们沿着这个梯度反方向最终可以得到误差趋近于零

卡尔曼滤波：准确的说我们介绍的是扩展卡尔曼滤波。思想很简单，就是让我的这一时刻的真值跟我的估计值误差的方差最小，而我这一时刻的估计值又跟测量值和先验估计有关，这就引出了一个迭代估计的过程。从而达到误差最优估计。