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

摘要

本发明涉及一种基于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)。