[发明专利]一种基于EKF的九轴运动传感器的姿态检测方法

权利要求书

1.一种基于EKF的九轴运动传感器的姿态检测方法，其特征在于，包括以下步骤：

步骤1、读取九轴运动传感器的数据，计算初始欧拉角并转换为四元数；

步骤2、初始化状态转移方程的过程噪声协方差矩阵Q以及加速度计的测量噪声协方差矩阵R_a和磁强计的测量噪声协方差矩阵R_m；

步骤3、将陀螺仪的数据与加速度计的数据进行数据融合，先求出状态转移方程，之后计算状态转移方程的雅克比矩阵F_k以及预测值的协方差矩阵P_k；

步骤4、计算加速度的模值K_a与磁场强度的变化量K_m，并根据K_a、K_m的值改变加速度计、磁强计的测量噪声协方差矩阵的权值，之后计算加速度的测量方程h₁以及h₁的雅克比矩阵H₁；

步骤5、计算卡尔曼增益并更新估计的状态和协方差矩阵，将更新后的状态估计四元数转换为姿态角，将得到的姿态角结合磁强计数据计算出偏航角；

步骤6、以偏航角和陀螺仪z轴偏置为状态，将陀螺仪和磁强计数据采用卡尔曼滤波方法进行数据融合，对偏航角做出估计；

步骤7、将两次数据融合的结果结合起来输出最终估计的姿态角并返回步骤3对九轴运动传感器的下一组数据进行估计。

2.根据权利要求1所述的基于EKF的九轴运动传感器的姿态检测方法，其特征在于，步骤1所述读取九轴运动传感器的数据，计算初始欧拉角并转换为四元数，具体为：

由初始的加速度数据和磁场数据，根据如下公式计算出初始的姿态角：

其中a_x，a_y，a_z分别表示加速度计的x，y，z三轴的数据；m_x，m_y，m_z分别表示磁强计的x，y，z三轴的数据；表示平稳状态的x，y，z三轴的磁场数据，pitch₀，roll₀，yaw₀分别表示初始俯仰角、初始翻滚角、初始偏航角；

将得到的初始姿态角转换为四元数：

其中q₀，q₁，q₂，q₃为四元数的四个系数。

3.根据权利要求2所述的基于EKF的九轴运动传感器的姿态检测方法，其特征在于，步骤2所述初始化状态转移方程的过程噪声协方差矩阵Q以及加速度计的测量噪声协方差矩阵R_a和磁强计的测量噪声协方差矩阵R_m，具体如下：

初始化噪声，需要初始化的噪声有陀螺仪噪声ω_n、陀螺仪偏置噪声ω_bn、加速度计测量噪声ω_a和磁强计测量噪声ω_m：

R_a＝[ω_a]

R_m＝[ω_m]。