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

权利要求书

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)。