[发明专利]一种基于9-dof传感器组的姿态检测数据融合方法

说明书

技术领域

本发明属于惯性导航技术领域，尤其涉及通过组合MEMS传感器进行载体姿态检测的数据融合方法。

背景技术

姿态检测是惯性导航技术的核心内容。其应用范围已由原来的航空航天和军事领域，扩展到机器人、海洋探测乃至汽车、消费类电子产品等多个方面。我国在这一技术领域的起步相对较晚，但近年来已经取得较快发展。西安电子科技大学采用加速度计及陀螺仪组成新型的垂直陀螺，并结合磁通门传感器组成航姿参考系统；中国航空工业第一集团公司研制了一种组合航向姿态系统。上述成果已在航空航天等高端技术领域得到了初步应用，但因其硬件平台成本高，结构复杂等原因，尚难以在民用领域取得应用。目前在低端市场，普遍采用的是基于MEMS芯片级的加速度计、陀螺仪、磁强计结合的9-DOF传感器平台，因其低成本、低功耗、小体积而得以在手机、掌上电脑等手持式设备上应用。

在基于MEMS传感器组姿态检测模块的设计中，最重要的环节是传感器数据的融合。目前，在数据融合算法方面国内外学者已经做了大量的研究，主要有：基于互补滤波的数据融合方式、基于梯度下降互补滤波的数据融合方式、基于卡尔曼滤波的数据融合方式等。实践证明卡尔曼滤波确实为一种在精确度、实时性以及有效性上都有突出表现的滤波方法。但是传统的卡尔曼滤波算法实现相对复杂，对处理器有较高的性能要求。因此，在传统的卡尔曼滤波算法上改进，使之能有效的搭载在中低端嵌入式硬件平台上，实现兼顾成本与精度的姿态检测模块，具有重要的实用价值。

发明内容

本发明的目的是解决姿态检测的数据融合问题，提供了一种基于9-DOF传感器组的扩展卡尔曼滤波方法。此方法可用计算机语言在嵌入式9-DOF姿态检测系统中实现，能有效的融合各传感器数据计算出系统姿态四元数。

为了实现上述目的，本发明采用如下的技术解决方案：

本发明包括以下步骤：

一、计算先验值。四元数预测方程如下所示：

                (1)

其中q_n代表当前状态下的旋转四元数，F为状态更新矩阵，q_n-1代表上一个状态下的旋转四元数，B代表输入系数矩阵，u_n-1为当前系统输入，代表系统噪声矩阵。本系统的卡尔曼滤波输入值u_n-1为零。因此，方程(1)可以被简化为方程(2)，其中代表四元数微分值，δ代表采样周期。

                       (2)

等式(2)可以用状态转换矩阵来表示，如下所示

        (3)

其中，和分别表示来自三轴陀螺仪的x，y和z轴的角速度分量。采样周期由嵌入在微控制器中的计时器计算得出。除此之外还需要通过以下等式估算系统的过程噪声协方差矩阵Q

                 (4)

假设  和  并简化等式，得到

     (5)

其中，和分别为x，y和z轴的角速度噪声方差。最后，利用等式(6)和(7)，可以计算出本状态的旋转四元数先验值和协方差矩阵先验值，其中为上一状态的协方差矩阵

                            (6)

              (7)

二、计算观测值。利用加速度和磁力计数据，根据以下公式，可以计算出姿态四元数观测值

                         (8)

其中C 是增益矩阵，是测量噪声。在本文中C等同于单位矩阵H。通过高斯-牛顿迭代法计算。首先，定义如下的旋转矩阵和误差函数

         (9)

                  (10)

其中d为姿态四元数q的实数部分，a、b、c为虚数部分。为误差矩阵， 是地球坐标系下由三轴加速度分量和三轴磁力分量组成的矩阵，M为系数矩阵，是传感器坐标系下由三轴加速度分量和三轴磁力分量组成的矩阵。可以通过以下等式得到雅克比矩阵

               (11)

其中

                                         (12)

于是可以得到如下所示的迭代方程

                     (13)

最后得到观测值

                            (14)