飞行姿态解算

飞行姿态解算（一）喻时耕宇·4个月前大概六个月前研究的东西，贴出来给大家分享下，不知到有无同样想搞四轴飞控的朋友，可能会感爱好。如果有业内算法工程师能一起讨论就更加好拉。近来又来研究研究有关飞行姿态解算的算法，看看当时没搞懂的四元数算法。重要也是学了网上几位大神的资源，才干小有所得。在这先附上参考某些大神心得的网址：四元数维基百科向量维基百科【Unity技巧】四元数（Quaternion）和旋转四元数和旋转学习笔记—四元数与欧拉角之间的转换四元数和欧拉角之间的转换[小应用]GD32F103+MPU9150四轴飞行器第一步：9DOF姿态融合-GigaDeviceGD32MCU论坛9DOF姿态融合（需要登录）尚有某些没有统计。。忘记地址了。。诸多解释上面帖子里都有了。我想从另一种方向来写这篇文章。先来说说我理解这个东西的通过吧。说起对飞行姿态的解算，其实是相称复杂的一种过程，并不像我们普通认为的那么简朴。首先，一开始（涉及我自己）普通可能有个误区：姿态不就一种倾角嘛（类比斜面）？因此感觉只需规定一种倾斜角而已，感觉挺简朴的。认真一想，发现其实有姿态有2个角度，能够想象一种滑梯再像侧面翻滚一下，这样其实对于空间思维不太好的人已有一点难度了。然后发现还需要再加一种是描述物体本身自旋的姿态。那么最后的结论为，姿态总共有3个轴。那么以我本身的知识来说，如何能够检测出物体姿态的这3个变量呢？总共需要3个传感器。第一步：+加速度传感器。说起测量倾斜角，普通能看见的都使用水泡，铅垂线之类的东西。有传感器使用经验的第一感觉就会觉得使用重力传感器就OK啦。首先说说重力传感器的原理，这里说的重力传感器又叫加速度传感器。加速度计和陀螺仪指南(很具体的介绍)理解了这个东西后来，那么问题来了，仅仅靠加速度计能否完毕角度的测量？答案是在静态状况下能够，在动态状况下不能够。加速度传感器，从这个名字（以及上面的原理）也能够看出，其测不是重力，而是重力引发的类似加速度带来的效应。因此对于其它的加速度同样会有读数（运动状态的变化），特别在震动的时候（震动状态），该传感器会有非常大的数据变化，此时的数据难以反映重力的实际值，因此结论是单靠加速度传感器无法完毕姿态解算。第二步：+陀螺仪既然单靠加速度传感器无法完毕姿态解算，那还需要添加哪些传感器？通过上面资料我们能够找到最少还需要的一种传感器，陀螺仪。陀螺仪测量出的数据为绕各个轴的旋转角速度。通过高等数学的知识能够得出，对角速度进行积分，能够得到旋转角度。把旋转角度加到之前测出的姿态上，会得到一种新的姿态，设为姿态A，通过加速度传感器能够算出来一种姿态B，这样将这两个姿态一定融合，就能够得到一种比较精确的姿态，这也就是之后我们能够进行姿态融合的基础。第三步：+地磁场传感器其实我们还缺一种传感器，地磁场传感器，其实他有个通俗的名字：电子罗盘。说到这，可能有人就已经懂得为什么需要这个传感器了。当加速度传感器完全水平的时候，能够预料，重力传感器无法分辨出在水平面旋转的角度即绕Z轴的旋转无法显示出来，此时只有陀螺仪能够检测。于是又回到第一步时的问题。陀螺仪即使动态十分快速，但由于其工作原理是积分，因此在静态会有累计误差，体现为角度会始终增加或者始终减少。。于是我们会需要一种在水平位置能确认朝向的传感器，这就是如今IMU（惯性导航单元）必备的第三个传感器，地磁场传感器，通过这3个传感器的互相校正，我们终于在大的理论上能够得到比较精确的姿态参数了。总结一下前三步：1、拟定姿态我们需要三个轴上旋转的数据。2、我们需要三个传感器，分别为3轴加速度传感器，3轴陀螺仪，以及3轴地磁场传感器。完毕这三步，能够说在大致姿态解算的框架上我已有了概念，但具体怎么做还是两眼一抹黑啊。于是本着站在巨人的肩膀上做事的原则，我又开始漫长的资料搜索以及筛选。于是有了本文。第四步理解什么叫姿态请各位自带笔和纸，复习或者预习大学高等数学，线性代数，复变函数，等数学知识，听我慢慢回想的学习的过程。。。既然我始终说姿态解算，姿态解算，那究竟什么是姿态。上图，我们想象一种飞机在天空飞行，他可能侧倾，可能仰俯，也可能旋转，这就是我们平时所说的姿态，那么这个姿态抽象出来意味着什么呢？这里是重点啦：姿态，就是一种坐标系与另一种坐标系的转换关系。这个可能比较抽象哈。首先在飞机上建立一种坐标系，X轴为机翼的方向，Y轴为机头的方向，Z轴垂直于飞机，这个坐标系是随着飞机姿态变化而变动的，此时就规定飞机的姿态，就等价于求出这个坐标系，那么如何得到这个坐标系？要得到一种新的坐标系，首先要有一种参考坐标系，普通选用Y轴正向为正北，x正向为正东，z轴垂直于地平面了，给他取个名字，就叫地理坐标系吧。因此此时，我们所说的姿态其实就是飞机坐标系和地理坐标系（是固定的）的一种关系。现在我们终于能够进入第五步啦。第五步：如何表达这种关系。找到了表达这种关系的办法，就能够运用这个办法随意转换这2个坐标系。先上结论吧。有三种关系表达方式：1、欧拉角2、余弦矩阵3、四元数1、欧拉角欧拉角是很直观的，一种物体的旋转，能够分解到三个坐标轴上的旋转。这三个旋转角度就是欧拉角。在惯性系统中普通把这三个角度分别称为roll，pitch，yaw。上个直观的图，很容易理解。用欧拉角来描述物体的旋转不光需要有角度，还需要有旋转次序，普通旋转次序是先yaw后pitch，再roll反映到坐标轴上就是先绕Z轴旋转，再绕X轴旋转，最后绕Y轴旋转。为什么有次序呢？与否能够没次序？如果身边有东西能够转转看，这个问题之后在理论上会进行阐明。需要注意的是yawpitchroll都是对应的固定的参考系也就是上面说的地理坐标系而言，每次新的姿态坐标系都是由地理坐标系通过欧拉角旋转得到的。这样我们就用欧拉角表达了物体的姿态。欧拉角是有诸多优点的。但是也有致命的缺点，那就是GimbalLock（万向节死锁），要理解GimbalLock所说的状况（可能有点难）让我们看个现实中的场景。假如我们有一种望远镜和一种用来放望远镜的三脚架，（我们将）三脚架放在地面上，使支撑望远镜的三脚架的顶部是平行于地平面（参考平面）的，方便使得竖向的旋转轴（记为x轴）是完全地垂直于地平面的。现在，我们就能够将望远镜饶x轴旋转360度，从而观察（以望远镜为中心的）水平包围圈的全部方向。普通将正北朝向方位角度记为0度方位角。第二个坐标轴，即平行于地平面的横向的坐标轴（记为y轴）使得望远镜能够饶着它上下旋转，普通将地平面朝向的仰角记为0度，这样，望远镜能够向上仰+90度指向天顶，或者向下-90度指向脚底。好了，万事俱备。现在，天空中（涉及地面上）的每个点只需要唯一的一对x和y度数就能够拟定。例如x=90度,y=45度指向的点是位于正东方向的半天空上。现在，看看万向节死锁是怎么发生的。一次，我们探测到有一种飞行器贴地飞行，位于望远镜的正东方向（x=90度，y=10度），朝着我们直飞过来，我们跟踪它。飞行器飞行方向是保持x轴角度90度不变，而y向的角度在慢慢增大。随着飞行器的临近，y轴角增加的越来越快且当y向的角度达成90度时（即将超越），忽然它急转弯朝南飞去。这时，我们发现我们不能将望远镜朝向南方（在只绕一种轴旋转的状况下），由于此时y向已经是90度！造成我们失去跟踪目的。这就是万向节死锁！为什么说不能将望远镜朝向南方呢，让我们看看坐标变化，从开始的（x=90度，y=10度）到（x=90度，y=90度），这个过程没有问题，望远镜慢慢转动跟踪飞行器。当飞行器达成（x=90度，y=90度）后，坐标忽然变成（x=180度，y=90度）（由于朝南），x由90突变成180度,因此望远镜需要饶垂直轴向x轴旋转180-90=90度方便追上飞行器,但此时，望远镜已经是平行于x轴，我们懂得饶平行于本身的中轴线的的旋转变化不了朝向，就象拧螺丝同样，螺丝头的指向不变。因此望远镜的指向还是天顶。而后由于飞行器飞远，坐标变成（x=180度，y<90度）时，y向角减小，望远镜只能又转回到正东指向，望'器'兴叹。这阐明用x,y旋转角（又称欧拉角）来定向物体有时并不能按照你想像的那样工作，象上面的例子中从（x=90度，y=10度）到（x=90度，y=90度），按照欧拉角旋转确实能够对的地定向，但从（x=90度，y=90度）到（x=180度，y=90度），再到（x=180度，y<90度）,按照欧拉角旋转后的定向并非对的。我的理解是坐标值的变化和飞行器空间的位置变化一一对应，但是从（x=90度，y=90度）到（x=180度，y=90度），再到（x=180度，y<90度）这个变化，飞行器位置是持续的变化，但坐标值的变化却不是持续的（从90突变到180），其因素在于（x=90度，y=90度）和（x=180度，y=90度）甚至和（x=任意度，y=90度）这些不同的坐标值对应空间同一种位置，这种多个坐标值对应同一种位置的不一致性是造成死锁的本源。上面是2维坐标系中的例子，同样，对于3维的也同样。例如有一种平行于x轴的向量，我们先将它饶y旋转直到它平行于z轴，这时，我们会发现任何饶z的旋转都变化不了向量的方向，即万向节死锁，因此说传统的欧拉角是不能做到全姿态解析的。2.方向余弦矩阵。说方向余弦矩阵之前，先讨论方向余弦。一种向量的方向（姿态）我们能够用他在参考坐标系（地理坐标系）各个轴向的夹角的余弦来表达（及在各个轴的投影）。类似的一种坐标系能够当作是3个向量构成，因此三个向量分别在坐标轴上的投影能够用来表达一种坐标系与参考坐标系的关系。这总共9个方向余弦构成了一种三阶矩阵，其对应方式以下图。第i行、j列的元素表达参考坐标系i轴和姿态坐标系j轴夹角的余弦。事实上方向余弦和欧拉角没有本质区别，由于方向余弦事实上就是用欧拉角表达的。下面附上推倒具体体现式的办法先从二维坐标系转换开始。推广到三轴的单次旋转，我们用矩阵表达为（绕Z轴旋转）：这里要说一下矩阵的含义，C21表达坐标系1到坐标系2的变换矩阵，那么有这样我们能够得到3个变换矩阵分别为单独绕Z轴旋转，绕X轴旋转，绕Y轴旋转。事实上，两坐标系任何复杂的角位置关系都能够看做有限次基本旋转的组合，变换矩阵等于基本旋转拟定的变换矩阵的连乘（线性代数），连乘的基本次序根据基本旋转的次序向右排列。之因此有次序是由于矩阵有“左乘”和“右乘”之分（还是线性代数）。那么我们得到：最后的矩阵就是完整的余弦矩阵。γ、θ、ψ就是欧拉角啦。至此我们解释了为什么欧拉角会有旋转的次序之分。从以上数学计算能够看出不同的旋转次序会带来不同的成果。四元数：四元数要介绍的太多了。。由于他优点有诸多，运用起来也很方便，但是理解起来太抽象了。百度四元数，一开始看到的就是四元数来历，尚有就是四元数的基本计算。对于来历，还是想说一下，四元数（Quaternions）是由威廉·卢云·哈密尔顿(WilliamRowanHamilton,1805-1865）在1843年爱尔兰发现的数学概念（百度百科）。将实数域扩充到复数域，并用复数来表达平面对量，用复数的加、乘运算表达平面对量的合成、伸缩和旋，这就是我们熟知的复数的二维空间含义，因此人们会继续猜想，运用三维复数不就能够体现三维空间的变换了吗，历史上有诸多数学家试图寻找过三维的复数，但后来证明这样的三维复数是不存在的。有关这个结论的证明，我没有查到更明确的版本，据《古今数学思想》中的一种理由，三维空间中的伸缩旋转变换需要四个变量来决定：两个变量决定轴的方向，一种变量决定旋转角度，一种变量决定伸缩比例。这样，只有三个变量的三维复数无法满足这样的规定。但是历史上得到的应当是比这个更强的结论，即使不考虑空间旋转，只从代数角度来说，三维的复数域作为普通复数域的扩张域是不存在的。并且，据《古今数学思想》叙述，即使像哈密尔顿后来引入四元数那样，牺牲乘法交换律，这样的三维复数也得不到。通过某些年的努力之后，Hamilton发现自己被迫应作两个让步，第一种是他的新数包含四个分量，而第二个是他必须牺牲乘法交换律。（《古今数学思想》第三册177页）但是四元数用作旋转的作用明显，简化了运算，并且避免了GimbalLock。四元数是最简朴的超复数，我们不能把四元数简朴的理解为3D空间的矢量，它是4维空间中的的矢量，也是非常不容易想像的。那什么是四元数呢？在维基百科中截取一段：复数是由实数加上虚数单位i构成，其中相似地，四元数都是由实数加上三个元素i、j、k构成，并且它们有以下的关系：每个四元数都是1、i、j和k的线性组合，即是四元数普通可表达为要把两个四元数相加只需将同类的系数加起来就能够，就像复数同样。那么四元数如何表达旋转呢？再截取维基百科上的一段话群旋转主条目：四元数与空间旋转非零四元数的乘法群在R3的实部为零的部分上的共轭作用能够实现转动。单位四元数（绝对值为1的四元数）若实部为cos(t)，它的共轭作用是一种角度为2t的转动，转轴为虚部的方向。提取几个核心词：四元数表达转动，首先其模应当为1，以下这个值应当为1。此时实部，也就是a若等于cos(t)则他的共轭作用为一种2t的转动，转轴方向为向量(b,c,d)的方向。那什么叫共轭作用？事实上就是四元数表达旋转的方式。那么运用四元数代表旋转是如何实现的，在载体系定义的一种矢量rb能够直接运用四元数将其在参考系中表达为rn。首先定义一种四元数rb'，它的虚部等于rb的对应分量，标量分量为零：参考系中的rn’表达为这也就是共轭作用。，其中q=a+bi+cj+dk，q*为q复共轭，q=a-bi-cj-dk及因此有：写成矩阵式：由于都表达旋转，因此这个矩阵理论上应当和余弦矩阵是等效的，从而就能计算欧拉角了。上图显示了四元数和余弦矩阵的关系。这样反求出欧拉角以下图：此时就能够根据四元数反求出欧拉角了。飞行姿态解算（二）喻时耕宇·4个月前继之前研究了某些飞行姿态理论方面的问题后，又找到了之前很流行的一段外国大神写的代码，来分析分析。先贴上代码：#include"AHRS.h"//----------------------------------------------------------------------------------------------------//Definitions#defineKp2.0f#defineKi0.005f#definehalfT0.5f//----------------------------------------------------------------------------------------------------//Variabledefinitionsfloatq0=1,q1=0,q2=0,q3=0;floatexInt=0,eyInt=0,ezInt=0;//----------------------------------------------------------------------------------------------------//FunctionvoidAHRSupdate(floatgx,floatgy,floatgz,floatax,floatay,floataz,floatmx,floatmy,floatmz){floatnorm;floathx,hy,hz,bx,bz;floatvx,vy,vz,wx,wy,wz;floatex,ey,ez;floatq0q0=q0*q0;floatq0q1=q0*q1;floatq0q2=q0*q2;floatq0q3=q0*q3;floatq1q1=q1*q1;floatq1q2=q1*q2;floatq1q3=q1*q3;floatq2q2=q2*q2;floatq2q3=q2*q3;floatq3q3=q3*q3;norm=sqrt(ax*ax+ay*ay+az*az);ax=ax/norm;ay=ay/norm;az=az/norm;norm=sqrt(mx*mx+my*my+mz*mz);mx=mx/norm;my=my/norm;mz=mz/norm;hx=2*mx*(0.5-q2q2-q3q3)+2*my*(q1q2-q0q3)+2*mz*(q1q3+q0q2);hy=2*mx*(q1q2+q0q3)+2*my*(0.5-q1q1-q3q3)+2*mz*(q2q3-q0q1);hz=2*mx*(q1q3-q0q2)+2*my*(q2q3+q0q1)+2*mz*(0.5-q1q1-q2q2);bx=sqrt((hx*hx)+(hy*hy));bz=hz;vx=2*(q1q3-q0q2);vy=2*(q0q1+q2q3);vz=q0q0-q1q1-q2q2+q3q3;wx=2*bx*(0.5-q2q2-q3q3)+2*bz*(q1q3-q0q2);wy=2*bx*(q1q2-q0q3)+2*bz*(q0q1+q2q3);wz=2*bx*(q0q2+q1q3)+2*bz*(0.5-q1q1-q2q2);ex=(ay*vz-az*vy)+(my*wz-mz*wy);ey=(az*vx-ax*vz)+(mz*wx-mx*wz);ez=(ax*vy-ay*vx)+(mx*wy-my*wx);exInt=exInt+ex*Ki;eyInt=eyInt+ey*Ki;ezInt=ezInt+ez*Ki;gx=gx+Kp*ex+exInt;gy=gy+Kp*ey+eyInt;gz=gz+Kp*ez+ezInt;q0=q0+(-q1*gx-q2*gy-q3*gz)*halfT;q1=q1+(q0*gx+q2*gz-q3*gy)*halfT;q2=q2+(q0*gy-q1*gz+q3*gx)*halfT;q3=q3+(q0*gz+q1*gy-q2*gx)*halfT;norm=sqrt(q0*q0+q1*q1+q2*q2+q3*q3);q0=q0/norm;q1=q1/norm;q2=q2/norm;q3=q3/norm;}代码不长，但是数学理论基础却是十分扎实的，必须要要有之前（一）的铺垫。一段段分析代码：#defineKp2.0f#defineKi0.005f#definehalfT0.5ffloatq0=1,q1=0,q2=0,q3=0;floatexInt=0,eyInt=0,ezInt=0;前三行是某些常数的定义，涉及Kp参数，Ki参数，采样时间。后两行是某些变量的声明，q0，q1，q2，q3分别代表四元数的4个参数a，b，c，d，v=q0+q1i+q2j+q3kexInt，eyInt，ezInt代表误差的积分项。这个背面会有阐明。voidAHRSupdate(floatgx,floatgy,floatgz,floatax,floatay,floataz,floatmx,floatmy,floatmz)floatnorm;floathx,hy,hz,bx,bz;floatvx,vy,vz,wx,wy,wz;floatex,ey,ez;来说说这些变量代表的意义，再注意各个参数是相对于哪个参考系的。这是算法理解的重点。先说形参的gxgygz表达陀螺仪读出的数据，分别为绕飞行器参考系x轴，y轴，z轴正方向旋转的角速度。axayaz表达加速度计读出的数据，分别为重力加速度在飞行器参考系x轴，y轴，z轴的分量。mxmymz表达磁场传感器读出的数据，分别为地磁场在飞行器参考系x轴，y轴，z轴的分量。norm是做归一化的中间变量。hx,hy,hz表达将飞行器参考系上的地磁矢量转换到地理坐标系（参考坐标系）后的矢量。bx，bz与上面同样，（注意）区别是bx在算法里为sqrt（hx*hx+hy*hy）这事实上是建立了一种误差函数，bx越靠近hx，则整个预计姿态与电子罗盘测得的姿态越重叠。vx,vy,vz为将原则单位重力转换到飞行器参考系后各个坐标轴上的分量。（重力加速度）wx,wy,wz为将bx与bz又重新转换到飞行器参考系后各个坐标轴上的分量。（地磁场）这个地方是比较难理解的，为什么转换过来又转换回去，背面会说。ex,ey,ez为向量a和向量m，与向量v和向量w的外积的在飞行器参考系的向量。该向量描述了a和m与v和w的偏差程度。floatq0q0=q0*q0;floatq0q1=q0*q1;floatq0q2=q0*q2;floatq0q3=q0*q3;floatq1q1=q1*q1;floatq1q2=q1*q2;floatq1q3=q1*q3;floatq2q2=q2*q2;floatq2q3=q2*q3;floatq3q3=q3*q3;这是些预先计算的数据，方便背面计算使用。norm=sqrt(ax*ax+ay*ay+az*az);ax=ax/norm;ay=ay/norm;az=az/norm;norm=sqrt(mx*mx+my*my+mz*mz);mx=mx/norm;my=my/norm;mz=mz/norm;对加速度传感器和磁场传感器得到的数值进行归一化，方便计算。hx=2*mx*(0.5-q2q2-q3q3)+2*my*(q1q2-q0q3)+2*mz*(q1q3+q0q2);hy=2*mx*(q1q2+q0q3)+2*my*(0.5-q1q1-q3q3)+2*mz*(q2q3-q0q1);hz=2*mx*(q1q3-q0q2)+2*my*(q2q3+q0q1)+2*mz*(0.5-q1q1-q2q2);运用四元数变换坐标系。具体为什么这样变换上一张图，笔记一里有具体过程。上图为从飞行器坐标系转换到地理坐标系的余弦矩阵的四元数体现。bx=sqrt((hx*hx)+(hy*hy));bz=hz;bx在当由磁场得到的飞行器参考系和由全部参数融合得到的飞行器参考系重叠的时候理论上是等于hx的，此时hy应当等于0vx=2*(q1q3-q0q2);vy=2*(q0q1+q2q3);vz=q0q0-q1q1-q2q2+q3q3;反向使用上图余弦矩阵的四元数体现。及把a替代成-a，原理为四元数体现旋转时，a为旋转的角度，因此想旋转回去只需要把a变成-a。(vx,vy,vz)为一原则重力在飞行器参考系上的矢量。wx=2*bx*(0.5-q2q2-q3q3)+2*bz*(q1q3-q0q2);wy=2*bx*(q1q2-q0q3)+2*bz*(q0q1+q2q3);wz=2*bx*(q0q2+q1q3)+2*bz*(0.5-q1q1-q2q2);同理，反向使用余弦矩阵的四元数体现。这里可能大家已经发现，怎么wxwywz是由传感器得到的。ex=(ay*vz-az*vy)+(my*wz-mz*wy);ey=(az*vx-ax*vz)+(mz*wx-mx*wz);ez=(ax*vy-ay*vx)+(mx*wy-my*wx);e为误差向量他是向量a和v的外积加上m和w的外积。exInt=exInt+ex*Ki;eyInt=eyInt+ey*Ki;ezInt=ezInt+ez*Ki;对误差进行积分；gx=gx+Kp*ex+exInt;gy=gy+Kp*ey+eyInt;gz=gz+Kp*ez+ezInt;用误差的积分和误差本身与Kp的乘积的和对角速度进行赔偿。q0=q0+(-q1*gx-q2*gy-q3*gz)*halfT;q1=q1+(q0*gx+q2*gz-q3*gy)*halfT;q2=q2+(q0*gy-q1*gz+q3*gx)*halfT;q3=q3+(q0*gz+q1*gy-q2*gx)*halfT;运用龙格-库格法求出四元数的值，此值由赔偿后的角速度求出。norm=sqrt(q0*q0+q1*q1+q2*q2+q3*q3);q0=q0/norm;q1=q1/norm;q2=q2/norm;q3=q3/norm;对四元数进行归一化。至此算法结束。剩余的事情就是把四元数转化为欧拉角，即可求出我们能够理解的飞行器的姿态yaw，roll，pitch另外稍微分析下这个算法的实现原理。a与v的外积成果为sinθ|a||v|nn为垂直于a，v构成平面的单位向量，方向恪守右手定理。由于都是归一化的，因此，这个成果的大小与θ有关，这个θ为a与v的夹角，由于a与v在halfT时间内的区别应当非常小，因此，此时θ=sinθ，这时候能够说这个外积的每个参数与θ成正比。这个误差是怎么来的？就是由于飞行器在这短时间旋转了一定角度。而从另一种角度，如果单纯从陀螺仪，同样能够得到飞行器旋转的一种角度。这个时候就有个问题？我们选择相信谁的？我们谁也不相信，而是选择按一定比例相信了3个参数，e（误差），g（陀螺仪角速度）和eInt（误差的积分），这样我们就融合了这三个参数，最后得到我们的姿态。PS：此方式在数学上在算法上没有任何问题，但是由于地磁传感器极易受到多种干扰（想想百度地图中的指南针精确度），而此算法又将地磁传感器所批示的方向过分的融入到了姿态当中，造成实际使用中数据会非常不稳定。我也是在此之后才发现。因此之后我使用了另一种融合算法，这个留到下一次再说。飞行姿态解算（三）喻时耕宇·4个月前继之前研究了某些飞行姿态理论方面的问题后，又找到了之前很流行的一段外国大神写的代码，来分析分析。第二篇文章的最后，讲到了文章中的算法在实际使用中有重大缺点。大家都懂得，分析算法理论的时候诸多状况下我们没有考虑太多外界干扰的状况，因素是诸多状况下，传感器的精度以及受到的干扰并不会特别大，而明显的影响到算法。但是在IMU系统中，有点不同。由于地磁场十分微弱，而我们生活中有大量使用电子设备，使得磁场非常的混乱，以至于地磁传感器非常容易受到干扰。由于以上算法把地磁传感器一同加入到姿态的测定中，并基本予以了地磁传感器与加速度传感器同样的加权，造成地磁传感器一旦被干扰，会对姿态产生地球重力忽然被干扰同样的成果。。。对姿态的测量是消灭性的。综上，考虑到磁场的不稳定性，必须对地磁传感器进行降权解决，使得他对姿态的影响变小。于是设计了下列的算法。将磁场传感器的数据在姿态角度中剔除，更新姿态的俯仰角（PITCH）以及横滚角（ROLL）的时候只使用加速度传感器以及陀螺仪(角速度传感器)。只在计算偏航角的（YAW）的时候使用磁场传感器。也就是只使用磁场传感器作为一种电子指南针，定位整个姿态在水平面旋转的角度。这样设计，让磁场传感器只影响姿态中的一种数值，减少了磁场的权重，即使磁场收到干扰，也不会造成姿态骤变，使得四轴坠机。在对YAW进行计算的时候使用了以下函数。eulerAngleRaw.yaw=0.9*(eulerAngleRaw.yaw-gzF*2*halfT)+0.1*angleMagYaw;此处的0.9和0.1是能够变动的但他们相加应当为1，此处为一种最简朴的1阶低通滤波器，增加0.1则是增大截止频率。算法的流程图是这样的：程序流程图新的姿态更新算法是这样的voidAHRSupdate(floatgxf,floatgyf,floatgzf,floataxf,floatayf,floatazf,floatmxf,floatmyf,floatmzf){doublenorm;floatvx,vy,vz;floatex,ey,ez;floathalfT;//采样周期的二分之一//辅助变量，以减少重复操作数floatq0q0=q0*q0;floatq0q1=q0*q1;floatq0q2=q0*q2;floatq0q3=q0*q3;floatq1q1=q1*q1;floatq1q2=q1*q2;floatq1q3=q1*q3;floatq2q2=q2*q2;floatq2q3=q2*q3;floatq3q3=q3*q3;//测量归一化norm=invSqrt(axf*axf+ayf*ayf+azf*azf);axf=axf*norm;//向量a为传感器重力飞行器分量ayf=ayf*norm;azf=azf*norm;//norm=invSqrt(mxf*mxf+myf*myf+mzf*mzf);//向量m为传感器磁场飞行器分量//mxf=mxf*norm;//myf=myf*norm;//mzf=mzf*norm;//计算参考磁通方向//hx=2*mxf*(0.5-q2q2-q3q3)+2*myf*(q1q2-q0q3)+2*mzf*(q1q3+q0q2);//向量h为磁场通过旋转后来参考系分量//hy=2*mxf*(q1q2+q0q3)+2*myf*(0.5-q1q1-q3q3)+2*mzf*(q2q3-q0q1);//hz=2*mxf*(q1q3-q0q2)+2*myf*(q2q3+q0q1)+2*mzf*(0.5-q1q1-q2q2);//bx=1.0f/invSqrt((hx*hx)+(hy*hy));//原则上应当只有X向的分量ex的磁场传感器部分