四轴飞行的姿态估计

leixiaofeng

【1】数学平台
这里所说的姿态,是用来描述一个刚体的固连坐标系和参考坐标系之间的角位置关系,有一些数学表示方法,很常见的就是欧拉角,也就是pitch,yaw,roll三个转角,转来转去很容易把人转糊涂,所以我们还是看一下另外一种

想象一下有两根筷子,它们的一端相连,并且两根筷子可以绕这一点旋转,呈一定角度,比如30度,那么现要让他们重合,很简单,把他们转到一起就行了,我相信没有人会先pitch XX度,然后yaw XX度 然后roll XX度,大家都会直接

把它们转到一起,一次就可以搞定的事情,干嘛分三步?

然后,更加理想化,固定其中一只筷子,旋转另外一只,那么很明显我们需要旋转30度,而旋转轴呢? 两根筷子确定一个平面A,沿它们的焦点做平面A的垂线,这就是旋转轴. 于是这就是一次旋转的姿态描述方式,爱尔兰数学家哈密

顿发明了一种叫做[四元数]的东西可以用来描述这种一次旋转,关于[四元数]的相关知识,可以看这个
四元数ourdev_643759VIYSS5.pdf(文件大小:94K) (原文件名:Shoemake-quattut.pdf)

我们要解决四旋翼和地球的相对姿态问题,地球太大了,换一个小一点的概念,将四轴放在水平地面上,四轴上固定着一个坐标系r,地面上固定着一个坐标系R,用四元数Q来描述r和R的角位置关系.于是现在有了基本的数学平台。

【2】传感器
再来说说测量的问题
1.陀螺仪,可以测量角速度,具有高动态特性,但是它是一个间接测量器件,它测量的是角度的导数,角速度,显然我们要将角速度对时间积分才能得到角度
如果这个世界是理想的,美好的,那我们的问题到此就解决了,不过很遗憾,现实是残酷的,误差的引入,使得积分出现了问题
假设陀螺仪固定不动,理想角速度值是0dps(degree per second)，但是有一个偏置0.1dps加在上面,于是测量出来是0.1dps,积分一秒之后,得到的角度是0.1度,1分钟之后是6度,还能忍受,一小时之后是360度,转了一圈,也就是

说,陀螺仪在短时间内有很大的参考价值。

2.加速度计,可以测量加速度,包括重力加速度,于是在静止或匀速运动的时候,加速度计仅仅测量的是重力加速度,而重力加速度与刚才所说的R坐标系是固连的,通过这种关系,可以得到加速度计所在平面 与 地面 的角度关系.

但是加速度计若是绕着重力加速度的轴转动,则测量值不会改变,也就是说无法感知这种水平旋转。

3.磁传感器,可以测量磁场,在没有其他磁场的情况下,仅仅测量的是地球的磁场,而地磁也是和R坐标系固连的,通过这种关系,可以得到平面A和地平面的关系
(平面A：和磁场方向垂直的平面)，同样的，若是沿着磁场方向的轴旋转，测量值不会改变，无法感知这种旋转。

综合考虑，加速度计和磁传感器都是极易受外部干扰的传感器，都只能得到2维的角度关系，但是测量值随时间的变化相对较小，结合加速度计和磁传感器可以得到3维的角度关系。陀螺仪可以积分得到三维的角度关系，动态

性能好，受外部干扰小，但测量值随时间变化比较大。可以看出，它们优缺点互补，结合起来才能有好的效果。

【3】数据融合
现在有了三个传感器，都能在一定程度上测量角度关系，但是究竟相信谁？
根据刚才的分析，应该是在短时间内更加相信陀螺仪，隔三差五的问问加速度计和磁传感器，我角度飘了多少了？
有一点必须非常明确，陀螺仪才是主角，加速度计和磁传感器仅仅是跑龙套的

关于数据融合，也有好多方法，这里采用一种最为形象，贴切的，简单易懂的，[互补滤波]，可以先通过这个简单的单轴融合的例子了解，来自CROSSBOW
CROSSBOWourdev_643801SBWOGM.pdf(文件大小:112K) (原文件名:Combine Angular Rate Sensors with

Accelerometers.pdf)


(原文件名:融合流程(CROSSBOW).JPG)

通过上图可以看出,陀螺仪在融合后的角度基础上积分,然后与加速度计测量的角度融合,然后又提供给下一次积分,看起来更像一个反馈系统.


【3】全姿态融合
上面的例子仅仅是单轴的融合，理解了单轴就可以结合四元数，得到3维的姿态融合方法了

(原文件名:姿态融合.JPG)

图中的符号解释：Bg是加速度计测得的值,扩充为四元数,Bg=[0,Bgx,Bgy,Bgz],上标B代表是在体坐标系(Body Frame)下的测量值
                Bh是磁传感器计测得的值,扩充为四元数,Bh=[0,Bhx,Bhy,Bhz]
                Eh是参考坐标系下的固有磁场,地磁场,上标E代表是在参考坐标系(earth Frame)下的值
                Eg是参考坐标系下的固有加速度,重力加速度
                p,q,r是陀螺仪测得的角速度
                [a,b,c,d]是姿态四元数Q

(1)关于用加速度计和磁传感器求姿态
假设现在已经得到正确的姿态四元数Q,那么可以利用四元数旋转将参考坐标系和体坐标系下的向量互相转换,将Eh和Eg转换到体坐标系下(BEh=Q×Eh×Q* , BEg=Q×Eg×Q*,BEh,BEg是参考坐标系的Eh,Eg由Q旋转到体坐标系下)
如果这个世界是理想的,美好的,那么Bg=BEg,Bh=BEh,也就是 { Q×Eh×Q* - Bh = 0} , { Q×Eg×Q* - Bg = 0}这两个方程成立,联立这两个方程就可以解得姿态四元数Q,不过很遗憾,现实是残酷的,由于各种误差的存在,这个

方程组无解,因此我们只能找到最优解,找最优解的最小方差问题有许多方法可以采用,如[梯度下降][高斯牛顿],图中是用的高斯牛顿法,由于高斯牛顿法含有矩阵求逆的计算,计算量超大,处理器不是DSP或FPGA的话就算了,关于
[梯度下降]和[高斯牛顿],可以在google,wiki百科搜索.

(2)陀螺仪积分求姿态
这个在网上搜索,陀螺仪,四元数微分方程 等等就可以找到了,秦永元[惯性导航]书上也有详细的

下图是一个很老的全姿态互补滤波的流程,不过很巧妙,

加速度计和磁传感器经过高斯牛顿迭代得到姿态误差速率,陀螺仪直接通过四元数微分方程得到姿态四元数速率,两个加起来积分得到姿态四元数

(原文件名:全部流程.JPG)