四元数姿态矩阵地磁融合.doc

_发布笔者关于飞行器导航与定位的文章四元数姿态解算中的地磁计融合解读分类：四旋翼飞行器2014-03-23 14:554451人阅读评论(20)收藏举报四元数姿态解算地磁计融合互补滤波融合算法四旋翼飞行器姿态解算姿态融合算法 笔者最近在做四轴，涉及到地磁计的融合算法，网上大多数是x-IMU的融合代码，但是这段代码对于地磁计的融合说明没有做过多的解释，网上没有相关讨论，仅在阿莫论坛看到一篇相关的代码解释，里面有关于地磁计融合部分的解说，个人觉得说的不是很清楚，虽然是正确的，我这里再补充啰嗦一下。 首先给出x-IMU关于陀螺仪、加速度计、地磁计的融合代码：cppview plaincopyprint?1. voidMahonyAHRSupdate(floatgx,floatgy,floatgz,floatax,floatay,floataz,floatmx,floatmy,floatmz)2. floatrecipNorm;3. floatq0q0,q0q1,q0q2,q0q3,q1q1,q1q2,q1q3,q2q2,q2q3,q3q3;4. floathx,hy,bx,bz;5. floathalfvx,halfvy,halfvz,halfwx,halfwy,halfwz;6. floathalfex,halfey,halfez;7. floatqa,qb,qc;8. 9. /UseIMUalgorithmifmagnetometermeasurementinvalid(avoidsNaNinmagnetometernormalisation)10. if(mx=0.0f)&(my=0.0f)&(mz=0.0f)11. MahonyAHRSupdateIMU(gx,gy,gz,ax,ay,az); 12. return;13. 14. 15. /Computefeedbackonlyifaccelerometermeasurementvalid(avoidsNaNinaccelerometernormalisation)16. if(!(ax=0.0f)&(ay=0.0f)&(az=0.0f)17. 18. /Normaliseaccelerometermeasurement19. recipNorm=invSqrt(ax*ax+ay*ay+az*az);20. ax*=recipNorm;21. ay*=recipNorm;22. az*=recipNorm;23. 24. /Normalisemagnetometermeasurement25. recipNorm=invSqrt(mx*mx+my*my+mz*mz);26. mx*=recipNorm;27. my*=recipNorm;28. mz*=recipNorm;29. 30. /Auxiliaryvariablestoavoidrepeatedarithmetic31. q0q0=q0*q0;32. q0q1=q0*q1;33. q0q2=q0*q2;34. q0q3=q0*q3;35. q1q1=q1*q1;36. q1q2=q1*q2;37. q1q3=q1*q3;38. q2q2=q2*q2;39. q2q3=q2*q3;40. q3q3=q3*q3;41. 42. /ReferencedirectionofEarthsmagneticfield43. hx=2.0f*(mx*(0.5f-q2q2-q3q3)+my*(q1q2-q0q3)+mz*(q1q3+q0q2);44. hy=2.0f*(mx*(q1q2+q0q3)+my*(0.5f-q1q1-q3q3)+mz*(q2q3-q0q1);45. bx=sqrt(hx*hx+hy*hy);46. bz=2.0f*(mx*(q1q3-q0q2)+my*(q2q3+q0q1)+mz*(0.5f-q1q1-q2q2);47. 48. /Estimateddirectionofgravityandmagneticfield49. halfvx=q1q3-q0q2;50. halfvy=q0q1+q2q3;51. halfvz=q0q0-0.5f+q3q3;52. halfwx=bx*(0.5f-q2q2-q3q3)+bz*(q1q3-q0q2);53. halfwy=bx*(q1q2-q0q3)+bz*(q0q1+q2q3);54. halfwz=bx*(q0q2+q1q3)+bz*(0.5f-q1q1-q2q2);55. 56. /Errorissumofcrossproductbetweenestimateddirectionandmeasureddirectionoffieldvectors57. halfex=(ay*halfvz-az*halfvy)+(my*halfwz-mz*halfwy);58. halfey=(az*halfvx-ax*halfvz)+(mz*halfwx-mx*halfwz);59. halfez=(ax*halfvy-ay*halfvx)+(mx*halfwy-my*halfwx);60. 61. /Computeandapplyintegralfeedbackifenabled62. if(twoKi0.0f)63. integralFBx+=twoKi*halfex*(1.0f/sampleFreq);/integralerrorscaledbyKi64. integralFBy+=twoKi*halfey*(1.0f/sampleFreq);65. integralFBz+=twoKi*halfez*(1.0f/sampleFreq);66. gx+=integralFBx;/applyintegralfeedback67. gy+=integralFBy;68. gz+=integralFBz;69. 70. else71. integralFBx=0.0f;/preventintegralwindup72. integralFBy=0.0f;73. integralFBz=0.0f;74. 75. 76. /Applyproportionalfeedback77. gx+=twoKp*halfex;78. gy+=twoKp*halfey;79. gz+=twoKp*halfez;80. 81. 82. /Integraterateofchangeofquaternion83. gx*=(0.5f*(1.0f/sampleFreq);/pre-multiplycommonfactors84. gy*=(0.5f*(1.0f/sampleFreq);85. gz*=(0.5f*(1.0f/sampleFreq);86. qa=q0;87. qb=q1;88. qc=q2;89. q0+=(-qb*gx-qc*gy-q3*gz);90. q1+=(qa*gx+qc*gz-q3*gy);91. q2+=(qa*gy-qb*gz+q3*gx);92. q3+=(qa*gz+qb*gy-qc*gx);93. 94. /Normalisequaternion95. recipNorm=invSqrt(q0*q0+q1*q1+q2*q2+q3*q3);96. q0*=recipNorm;97. q1*=recipNorm;98. q2*=recipNorm;99. q3*=recipNorm;100. 相信有很多人已经理解了加速度计补偿陀螺仪漂移的原理，这部分代码在x-IMU官网上已经给出，大家可以自行下载(http:/www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/)，有能力的可以自行查看Sebastian O.H. Madgwick在2010年4月发表的一篇论文（An efficient orientation filter for inertial and inertial/magneticsensor arrays），可以发现x-IMU官网上的融合代码就是基于此篇论文。 花了一天时间研究地磁融合代码，总算弄明白了其地磁融合的原理。为了让大家理解Madgwick对地磁量的处理方式，我先从加速度计补偿开始说起。 首先，东北天坐标系我们称之为n系（地理坐标系，参考坐标系），载体坐标系我们称之为b系，就是我们飞行器的坐标系。对于四元数法的姿态解算，我们求的就是四元数的值；方向余弦矩阵（用于表示n系和b系的相对关系）中的元素本来应该是三角函数，这里由于我们四元数法，所以矩阵中的元素就成了四元数。所以我们的任务就是求解由四元数构成的方向余弦矩阵nCb（nCb表示从b系到n的转换矩阵，同理，bCn表示从n系到b的转换矩阵，他们的关系是转置）。 显然，上述矩阵是有误差存在的。对于一个确定的向量n，用不同的坐标系表示时，他们所表示的大小和方向一定是相同的。但是由于这两个坐标系的转换矩阵存在误差，那么当一个向量经过这么一个有误差存在的旋转矩阵变换后，在另一个坐标系中肯定和理论值是有偏差的，我们通过这个偏差来修正这个旋转矩阵。我们刚才说了，这个旋转矩阵的元素是四元数，这就是说我们修正的就是四元数，于是乎我们的姿态就这样被修正了，这才是姿态解算的原理。 我这里再重复一遍，因为这是原理部分。我们的姿态解算求的是四元数，我们是通过修正旋转矩阵中的四元数来达到姿态解算的目的，而不要以为通过加速度计和地磁计来修正姿态，加速度计和地磁计只是测量工具和载体，通过这两个器件表征旋转矩阵的误差存在，然后通过算法修正误差，修正四元数，修正姿态。 在n系中，加速度计输出为，经过bCn转换之后到b中的值为；在b系中，加速度计的测量值为，现在和均表示在b系中的竖直向下的向量，由此，我们来做向量积，得到误差，利用这个误差来修正bCn矩阵，于是乎，我们的四元数就在这样一个过程中被修正了。（实际上这种修正方法只把b系和n系的XOY平面重合起来，对于z轴旋转的偏航，加速度计无可奈何，稍后给详细讲解）但是，由于加速度计无法感知z轴上的旋转运动，所以还需要用地磁计来进一步补偿。 我们知道加速度计在静止时测量的是重力加速度，是有大小和方向的；同理，地磁计同样测量的是地球磁场的大小和方向，只不过这个方向不再是竖直向下，而是与x轴（或者y轴）呈一个角度，与z轴呈一个角度。记作，这里我们让x轴对准北边，所以by=0，即。倘若我们知道bx和bz的精确值，那么我们就可以采用和加速度计一样的修正方法来修正。只不过在加速度计中，我们在n系中的参考向量是，变成了地磁计的。如果我们知道bx和bz的精确值，那么我们就可以摆脱掉加速度计的补偿，直接用地磁计和陀螺仪进行姿态解算，但是你看过谁只有陀螺仪和地磁计进行姿态解算吗？没有，因为没人会去测量当地的地磁场相对于东北天坐标的夹角，也就是bx和bz。那么现在怎么办？- 前面已经讲了，我们的姿态解算就是求解旋转矩阵，这个矩阵的作用就是将b系和n正确的转化直到重合。 现在我们假设nCb*旋转矩阵是经过加速度计校正后的矩阵，当某个确定的向量（b系中）经过这个矩阵旋转之后（到n系），这两个坐标系在XOY平面上重合，只是在z轴旋转上会存在一个偏航角的误差。下图表示的是经过nCb*旋转之后的b系和n系的相对关系。可以明显发现加速度计可以把b系通过四元数法从任意角度拉到与n系水平的位置上，这时，只剩下一个偏航角误差。这也是为什么加速度计误差修正偏航的原因。 到这里，就好说了。现在我们反过来从b系推往n系：设地磁计在b系中的输出为，经过nCb*旋转之后得到（n系）。在这个XOY平面上（n系），的投影为bx2,的投影为hx2+hy2。显然，地磁计在XOY平面上（n系）的向量的大小必定相同，所以有bx2= hx2+hy2。而对于bz的处理，我们不做变动，令bz=hz即可。经过这样处理之后的，经过bCn*旋转回转到b系中，得到，这个值再和b系中的地磁计输出做向量积求误差，再次修正bCn*（或者nCb*），得到bCn*（或者nCb*）。这样就完成了一次地磁计的补偿。 将加速度计没能做到的z轴上的旋转修正，通过地磁计在XOY平面上的地磁力相同原理，得到了修正。于是乎，pitch和roll通过加速度计修正，然后在这个基础之上（该地磁计补偿方法必须依靠加速度计修正提供一致的XOY平面，才会有bx2= hx2+hy2等式成立），yaw通过地磁计来补偿，最终得到了没有偏差的实时姿态（也就是由四元数组成的旋转矩阵）。查看评论7楼beanbeanhu2015-04-09 14:11发表回复如何消去芯片在运动中重力加速度影响，而得到真正的运动方向加速度数值呢？Re:Finding_Nemo2015-04-09 19:19发表回复回复beanbeanhu：这个不好处理，从本质上讲有矛盾。姿态解算依靠的就是静置状态下的重力加速度g来修正姿态，但是在运动中会必定会产生额外的线性加速度，这个线性加速度会对姿态求解的结果产生干扰，但是要求三轴线性加速度，必须要利用姿态，因此这是矛盾的。只能在低速运动中近似求得三轴线性加速度。6楼aaaaa11adsafa2015-04-04 23:25发表回复引用举报楼主你好，你用这个磁力计来补偿，会不会因为磁力计受到大的干扰而发生故障啊？我用MPU9150芯片来算，发现有大的动作后，收敛的很慢，就是要慢慢地得到实际角度。这在应用中应该会出现问题吧Re:Finding_Nemo2015-04-05 09:27发表回复引用举报回复aaaaa11adsafa：之所以用磁力计是因为单纯的加速度计和陀螺仪不能修正偏航角yaw，这个角度会慢慢飘走的，因此需要磁力计来修正这个角度。你后面说的那种现象是正常的，因为惯性导航利用的就是惯性，也就是加速度，剧烈晃动导致加速度变化剧烈，会产生额外的线性加速度，与重力加速度计g一起导致了新的外部加速度。姿态解算原理就是利用g，如果有新的加速度计产生，那么姿态解算必然错误。除此之外，剧烈的旋转运动，加速度计是跟不上这个速度的，陀螺可以跟上，但是这两者就产生了差距，时间越长，差距越大。等你停下来的时候姿态就已经飘的不成样子，需要较长时间来收敛到稳定值，因此这种惯性导航算法的前提条件就是低速运动。Re:aaaaa11adsafa2015-04-05 12:31发表回复回复yzhajlydy：嗯。但是上面的代码中不止修正的是YAW吧，另外两个轴也有修正吧？我试过，如果不加入磁力计数据融合，貌似效果还更好，比如翻转过来后，再翻回来，收敛速度更快。Re:Finding_Nemo2015-04-06 09:07发表回复回复aaaaa11adsafa：嗯，因为磁力线方向与三个轴均存在角度，所以他是可以修正3D姿态的，没错。一般融合用6dof即可，yaw单独由磁力计计算，9dof融合中姿态会引入磁场干扰，要去耦。5楼westlovehehe2015-02-24 12:13发表回复最近探讨了这个问题，已经有了新的进展，准备投一篇SCI4楼zhangkaiy12202014-12-27 15:56发表回复总结的很好，谢谢3楼zjrbhy2014-10-28 16:07发表回复IEEE论文名字是什么，能不能学习下Re:Finding_Nemo2014-10-28 18:02发表回复回复zjrbhy：Estimation of IMU and MARG orientation using a gradient descent algorithm2楼zjrbhy2014-10-28 10:29发表回复群主，向你学习，非常厉害1楼zjrbhy2014-10-28 10:26发表回复写的非常好，关于“加速度计补偿陀螺仪漂移的原理” 和Madgwick的那篇论文，版主能不能讲解下，看的不是太懂啊Re:Finding_Nemo2014-10-28 14:25发表回复回复zjrbhy：那个“加速度计补偿陀螺仪漂移”的原理还是用微分方程推过来的，把方程写成关于四元数和四元数导数之后，左边就只剩下陀螺仪了。因为加速度计梯度下降算法已经补偿了姿态的漂移，就不需要再用加速度计去补偿陀螺仪的漂移了，因为姿态的漂移就是陀螺仪的漂移引起的。你看Madwick在2011年IEEE发表的正式论文删除了这部分。大概就是这个意思吧。Re:zjrbhy2014-10-28 17:49发表回复回复yzhajlydy：“加速度计补偿陀螺仪漂移”的原理还是用微分方程推过来的，把方程写成关于四元数和四元数导数之后，左边就只剩下陀螺仪了“是这篇文章的吗”an efficient orientation filter for inertial and inertial/magnetic sensor arrays“没看懂，好像是梯度下降算法Re:Finding_Nemo2014-10-28 18:01发表回复回复zjrbhy：是这篇文章，这是作者自己写的report，没发表的那种，但是很详尽的，正式论文没这个详细。Re:zjrbhy2014-10-28 18:10发表回复回复yzhajlydy：晕，第3.2章好难，完全没看懂，”相信有很多人已经理解了加速度计补偿陀螺仪漂移的原理“版主，你这话太刺激人了Re:Finding_Nemo2014-10-28 18:19发表回复回复zjrbhy：囧，这句话有误解，不是说论文里面的那个陀螺仪bias补偿式子。我只是说用加速度计来补偿姿态而已拉。回复yzhajlydy：用微分方程推过来的，把方程写成关于四元数和四元数导数之后版主，这个推导哪里有啊回复zjrbhy：不懂的加我QQ249442907回复zjrbhy：四元数微分方程两边同时左乘q的共轭四元数q*并且q*=q(-1次幂)所以，假设四元数乘法用表示2q*q的导数=q*qw=q(-1次幂)qw=w把w想成陀螺误差就行您还没有登录,请登录或注册* 以上用户言论只代表其个人观点，不代表CSDN网站的观点或立场核心技术类目全部主题HadoopAWS移动游戏JavaAndroidiOSSwift智能硬件DockerOpenStackVPNSparkERPIE10EclipseCRMJavaScript数据库UbuntuNFCWAPjQueryBIHTML5SpringApache.NETAPIHTMLSDKIISFedoraXMLLBSUnitySplashtopUMLcomponentsWindows MobileRailsQEMUKDECassandraCloudStackFTCcoremailOPhoneCouchBase云计算iOS6RackspaceWeb AppSpringSideMaemoCompuware大数据aptechPerlTornadoRubyHibernateThinkPHPHBasePureSolrAngularCloud FoundryRedisScalaDjangoBootstrap个人资料Finding_Nemoo 访问：33765次o 积分：690o 等级：o 排名：千里之外o 原创：21篇o 转载：3篇o 译文：0篇o 评论：77条文章搜索窗体顶端窗体底端文章分类 四旋翼飞行器(9) 学习笔记(12) 一些好玩的小程序(3)文章存档 2015年05月(1) 2015年04月(2) 2014年12月(1) 2014年09月(1) 2014年04月(1)展开阅读排行 微型四旋翼飞行器的设计与制作(4781) 四元数姿态解算中的地磁计融合解读(4442) 四