一、概述
上一篇文章我们讲述了怎样搭建一个基本的组合导航系统,它仅仅包括IMU做预测、GPS做观测,而实际中,能够使用的信息远远不止这些,
把多种信息融合在一起,才能得到一个真正稳定可靠的组合导航系统。比如,对于车辆导航来讲,它自身的运动模型导致载体系 x方向和z方
向速度为零(这里按照右前上建立坐标系),零本身就是一种观测;另外,当车上有轮速计时,载体系y向也有了观测。除此以外,有磁力计时,
又能在姿态上提供约束,怎样使用这些信息,是本篇文章重点讨论的问题。更进一步,如果有激光和视觉,那么就可以直接添加姿态观测。
二、轮速计做观测
而实际上并不是这样。还记得我们在推导状态误差模型时所用的方法吗?正确的步骤是先写出观测量和状态量之间不考虑误差时的公式关系,
然后再写出带误差量的公式,用后者减掉前者,得到的才是真正的误差方程; 这种思路一定要谨记,很简单,很有用!
在写观测方程时也是一样的,先写出两种公式如下
发现(4)式和(1)式并不一样,仔细观察会发现,(1)式仅仅考虑了导航系速度误差在载体系上的转换,而(4)式还考虑了因姿态误差引起的导航系速度在载体系的投影误差。
在之前介绍的惯性导航误差模型里,我们选取的状态量为 ,状态方程 和 观测方程的形式为
由于轮速计做观测时,它的模型可以用公式表示为
即只有前向有速度,其中 V_o 为轮速计测量的速度。那么此时就有
因此,此时的观测方程就应该为
感觉看完这些东西,对卡尔曼滤波又明白了许多!
三、车辆运动约束做观测
车辆运动约束用公式可以解释为
即只取Z和H的第1行和第3行即可
四、磁力计做观测
由于地磁在导航系三个轴上的分量是固定的,因此载体姿态变化时,磁力计三轴敏感到的磁通量会发生变化,以此就可以对载体姿态误差形成约束。
假设导航系三轴磁通量分别为
五、激光或视觉做观测
此处的激光、视觉指的是和先验地图匹配的使用方式,即他们提供的直接是 位置和姿态的观测。对于以误差量为状态量的模型,和姿态相关的状态量是失准角,
因此需要给出姿态矩阵到失准角之间的转换关系。假设 是带误差的姿态阵,它就是导航解算得到的矩阵,那么失准角满足下式
因此可以得到
这就相当于得到了失准角 。因此按照原有的状态模型,此时的观测方程可以写为
六、总结与思考
1. 以上几种典型的传感器基本涵盖了主要的观测方式,当有其他传感器时,只需要根据其提供的观测量形式,在上面几种类型里选择与之匹配的即可,区别只是不同的传感器观测噪声不同。
2. 这里只展示了;以误差量为状态量的建模方式,实际上,还有一种是以位置、速度、姿态四元数为状态量进行建模的方法,在低精度器件的导航中,后者反而居多,
这一类模型可参考论文《GPS-aided INS Solution for OpenPilot》,这篇论文里有完整的推导过程,我们就没必要把公式在这里再抄一遍了,麻烦各位自己去看吧。
七、致谢
截止到本篇文章为止,怎样搭建一个基于滤波器的多传感器融合系统就已经介绍完了。由于水平有限,这个系列的文章也写得马马虎虎,感谢各位的包容,也感谢提出的宝贵意见。
文章写作过程中,每次都是泡泡传感器组组长周平负责编辑,在公众号里编辑公式不是一个轻松活,感谢平哥的付出。文章发表前,周兆丰每次都很认真地提出很多宝贵意见,
让我这个不认真的人感到惭愧,在此表示感谢!
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)