CN104296745A - 一种基于9-dof传感器组的姿态检测数据融合方法 - Google Patents

Abstract

本发明涉及一种基于9-dof传感器组的姿态检测数据融合方法。本发明首先根据陀螺仪的输出估计四元数的值，然后利用高斯-牛顿迭代法将加速度计和磁力计的输出变量转换为观测值，最后计算出四元数输出值。本发明使得卡尔曼滤波过程中的估计值与观测值呈线性关系，简化了滤波器的设计，使得其可以运行在硬件资源紧缺的嵌入式系统中。

Description

一种基于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)

三、计算后验值。首先计算如试(15)所示的差值，或者可以称作新息

                         (15)

与此同时计算出卡尔曼增益矩阵，其中R为观测噪声协方差矩阵

                     (16)

最终，根据以下两个等式，可以得到此状态的姿态四元数后验值与协方差矩阵后验值，其中I为单位矩阵

                      (17)

                      (18)

本发明方法针对卡尔曼滤波算法在嵌入式系统实际应用中出现的采样率不足，状态迭代较慢的问题，提出了一种扩展卡尔曼滤波算法。此方法首先根据陀螺仪的输出估计四元数的值，然后利用高斯-牛顿迭代法将加速度计和磁力计的输出变量转换为观测值，最后计算出四元数输出值。本发明与传统姿态检测数据融合方法相比，具有以下优点：

1.本发明方法使得卡尔曼滤波过程中的估计值与观测值呈线性关系，简化了滤波器的设计，使得其可以运行在硬件资源紧缺的嵌入式系统中。

2.本发明方法适用于所有由三轴陀螺仪、三轴加速度计、三轴磁力计组合而成的9-DOF姿态检测系统，与传感器的种类、型号、结构等硬件无关，与生产厂家无关。

附图说明

图1为数据融合方法的程序流程图。

具体实施方式

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

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

如图1所示，一种基于9-dof传感器组的姿态检测数据融合算法，包括以下步骤：

一、计算先验值。在此阶段，首先需要确定系统的过程噪声协方差矩阵Q。范例：将9-DOF姿态模块放置于准静态环境中60分钟以上，采集陀螺仪输出角速度，采样率为1次/秒，则至少可以获得3600组数据。将采集到的n组数据

代入下式

求出平均值，代入下式

计算出、、，代入下式

得到Q。四元数预测方程如下所示：

                (1)

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

                       (2)

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

        (3)

其中，和分别表示来自三轴陀螺仪的x，y和z轴的角速度分量。采样周期由嵌入在微控制器中的计时器计算得出。可以计算出本状态的旋转四元数先验值和协方差矩阵先验值

                            (4)

              (5)

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

                         (6)

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

         (7)

                  (8)

其中是地球坐标系下由三轴加速度分量和三轴磁力分量组成的矩阵，是传感器坐标系下由三轴加速度分量和三轴磁力分量组成的矩阵。可以通过以下等式得到雅克比矩阵

               (9)

其中

                                         (10)

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

                     (11)

最后得到观测值

                            (12)

三、计算后验值。首先计算如式(15)所示的差值，或者可以称作新息

                         (13)

与此同时计算出卡尔曼增益矩阵

                     (14)

其中测量噪声协方差矩阵R一般取值为。

最终，根据以下两个等式，可以得到此状态的姿态四元数后验值与协方差矩阵后验值

                      (15)

                      (16)

求出的为姿态四元数输出值。

Claims (1)

1.一种基于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)

步骤三、计算后验值；首先计算如试(15)所示的差值，或者可以称作新息

                         (15)

与此同时计算出卡尔曼增益矩阵，其中R为观测噪声协方差矩阵

                     (16)

最终，根据以下两个等式，可以得到此状态的姿态四元数后验值与协方差矩阵后验值，其中I为单位矩阵

                      (17)

                      (18)。