一种基于9-dof传感器组的姿态检测数据融合方法
【专利摘要】本发明涉及一种基于9-dof传感器组的姿态检测数据融合方法。本发明首先根据陀螺仪的输出估计四元数的值，然后利用高斯-牛顿迭代法将加速度计和磁力计的输出变量转换为观测值，最后计算出四元数输出值。本发明使得卡尔曼滤波过程中的估计值与观测值呈线性关系，简化了滤波器的设计，使得其可以运行在硬件资源紧缺的嵌入式系统中。
【专利说明】一种基于9-dof传感器组的姿态检测数据融合方法
【技术领域】
[0001] 本发明属于惯性导航【技术领域】，尤其涉及通过组合MEMS传感器进行载体姿态检 测的数据融合方法。
【背景技术】
[0002] 姿态检测是惯性导航技术的核心内容。其应用范围已由原来的航空航天和军事领 域，扩展到机器人、海洋探测乃至汽车、消费类电子产品等多个方面。我国在这一【技术领域】 的起步相对较晚，但近年来已经取得较快发展。西安电子科技大学采用加速度计及陀螺仪 组成新型的垂直陀螺，并结合磁通门传感器组成航姿参考系统；中国航空工业第一集团公 司研制了一种组合航向姿态系统。上述成果已在航空航天等高端【技术领域】得到了初步应 用，但因其硬件平台成本高，结构复杂等原因，尚难以在民用领域取得应用。目前在低端市 场，普遍采用的是基于MEMS芯片级的加速度计、陀螺仪、磁强计结合的9-D0F传感器平台， 因其低成本、低功耗、小体积而得以在手机、掌上电脑等手持式设备上应用。
[0003] 在基于MEMS传感器组姿态检测模块的设计中，最重要的环节是传感器数据的融 合。目前，在数据融合算法方面国内外学者已经做了大量的研究，主要有：基于互补滤波的 数据融合方式、基于梯度下降互补滤波的数据融合方式、基于卡尔曼滤波的数据融合方式 等。实践证明卡尔曼滤波确实为一种在精确度、实时性以及有效性上都有突出表现的滤波 方法。但是传统的卡尔曼滤波算法实现相对复杂，对处理器有较高的性能要求。因此，在传 统的卡尔曼滤波算法上改进，使之能有效的搭载在中低端嵌入式硬件平台上，实现兼顾成 本与精度的姿态检测模块，具有重要的实用价值。
【发明内容】
[0004] 本发明的目的是解决姿态检测的数据融合问题，提供了一种基于9-D0F传感器组 的扩展卡尔曼滤波方法。此方法可用计算机语言在嵌入式9-D0F姿态检测系统中实现，能 有效的融合各传感器数据计算出系统姿态四元数。
[0005] 为了实现上述目的，本发明采用如下的技术解决方案： 本发明包括以下步骤： 一、计算先验值。四元数预测方程如下所示：
【权利要求】
1. 一种基于9-dof传感器组的姿态检测数据融合方法，其特征在于该方法包括以下步 骤： 步骤一、计算先验值； 四元数预测方程如下所示：
其中qn代表当前状态下的旋转四元数，F为状态更新矩阵，(Vi代表上一个状态下的旋 转四元数，B代表输入系数矩阵，Um为当前系统输入，Wn_i代表系统噪声矩阵；本系统的 卡尔曼滤波输入值Um为零；因此，方程（1)可以被简化为方程（2)，其中&代表四元数微 分值，S代表采样周期；
等式（2)可以用状态转换矩阵来表示，如下所示
其中ωχ , ω}^Ρωζ分别表示来自三轴陀螺仪的X，y和z轴的角速度分量；采样周期由嵌 入在微控制器中的计时器计算得出；除此之外还需要通过以下等式估算系统的过程噪声协 方差矩阵Q
假设Ε?ω」=0和Ε|ω[ ·，| = 0(￥i共j)并简化等式，得到
其中&,%和〇2分别为X，y和Z轴的角速度噪声方差；最后，利用等式（6)和（7)，计算 出本状态的旋转四元数先验值q.ap,n和协方差矩阵先验值Pap. n,其中Pd为上一状态的协方 差矩阵
步骤二、计算观测值； 利用加速度和磁力计数据，根据以下公式，计算出姿态四元数观测值2η
其中C是增益矩阵是测量噪声；C等同于单位矩阵Η;通过高斯-牛顿迭代法计 算ζη ; 首先，定义如下的旋转矩阵和误差函数
其中d为姿态四元数q的实数部分，a、b、c为虚数部分；g为误差矩阵，yE是地球坐标 系下由三轴加速度分量和三轴磁力分量组成的矩阵，Μ为系数矩阵,ys是传感器坐标系下由 三轴加速度分量和三轴磁力分量组成的矩阵；通过以下等式得到雅克比矩阵
步骤三、计算后验值；首先计算如试（15)所示的差值qre;n，或者可以称作新息
与此同时计算出卡尔曼增益矩阵，其中R为观测噪声协方差矩阵
最终，根据以下两个等式，可以得到此状态的姿态四元数后验值％?与协方差矩阵后 验值Pp_，其中I为单位矩阵
【文档编号】G01C21/16GK104296745SQ201410513253
【公开日】2015年1月21日 申请日期:2014年9月29日 优先权日:2014年9月29日
【发明者】章雪挺, 高祎轩, 刘敬彪 申请人:杭州电子科技大学