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

说明书

本发明公开了一种基于EKF的九轴运动传感器姿态检测方法，该方法为：读取九轴运动传感器中的数据，计算初始欧拉角并转换成初始姿态四元数；初始化状态转移噪声矩阵以及传感器的观测噪声的矩阵；将陀螺仪数据和加速度计数据采用EKF方法进行数据融合，在将加速度计的数据归一化之前先计算加速度的模，并根据此模值对噪声矩阵进行调节，估计出目标物体的姿态四元数并转化为姿态角；将陀螺仪数据和磁强计数据结合之前估计出的姿态角采用卡尔曼滤波法进行数据融合单独求出偏航角，求解过程中根据磁强计的数据的变化量对噪声矩阵进行调节，从而改变卡尔曼增益；将两次融合的结果结合即为输出的姿态角。本发明具有快速、准确、抗干扰能力强等优点。

技术领域

本发明涉及惯性测量元件(IMU)的姿态解算技术领域，特别是一种基于EKF的九轴运动传感器的姿态检测方法。

背景技术

在姿态检测技术中，微机电系统(MEMS)惯性传感器起着举足轻重的作用。像常见的加速度计、陀螺仪和磁强计等都属于MEMS惯性传感器，这三种传感器分别可以测取加速度、角速度和磁感应强度，通过对采集到的数据进行处理可以求得我们所需的关于位置、速度和姿态角等的信息。

随着对MEMS器件要求的提高，出现了多传感器集成的芯片，九轴运动传感器便是将加速度计、陀螺仪和磁强计三种传感器集成在一起的一种传感器。通过九轴运动传感器采集运动目标的加速度、角速度和磁感应强度等数据，再通过一些算法进行处理便能够得到目标的姿态角。而同样的数据采用不同的算法计算出来的结果也会有所偏差，所以如何设计更好的算法使求解的过程更快、求解的结果更准确便成了大家的研究目标。

首先便是姿态的表示方法，常用的表示方法有方向余弦矩阵、欧拉角和四元数。方向余弦矩阵的优点是易于在两个坐标系之间进行矢量转换，缺点是如果通过方向余弦矩阵微分方程来计算姿态矩阵的话，计算量很大，每一次姿态更新都要求方向余弦矩阵的9个元素。欧拉角表示的优点是物理含义明确、描述姿态直观，但是欧拉角微分方程有奇点问题，无法求解所有的姿态，所以一般不使用欧拉角微分方程来进行姿态解算。但是由于欧拉角描述姿态的方式对人来说非常清晰直观，所以还是经常使用该方法表达姿态，只是会通过其他姿态表示方法来进行姿态解算，然后转化到欧拉角。这个姿态表示方法就是四元数，使用四元数进行姿态解算不会出现欧拉角那样的奇点问题，可以求解所有姿态，并且只有四个参数，相比于方向余弦矩阵微分方程计算量要小得多，非常适合用在微控制器中。但四元数的表示方式对人来说没有欧拉角那么直观，所以经常使用四元数来进行姿态解算，之后转换为欧拉角方便人的观察。

常见的姿态解算算法一般分为两大类：一类是互补滤波算法，一类是卡尔曼滤波算法。顾名思义，互补滤波算法就是使用各种传感器的测量值来相互补充从而获得更好的姿态解算结果。而卡尔曼滤波算法类中应用最广泛的是扩展卡尔曼滤波算法，扩展卡尔曼滤波技术可根据包含高斯噪声的传感器测量数据递归估算系统状态，非常适用于零偏和噪声较大的低成本陀螺和加速度计组成的航姿参考系统(AHRS)中。扩展卡尔曼滤波是标准卡尔曼滤波在非线性情形下的一种扩展形式，它是一种高效率的递归滤波器。扩展卡尔曼滤波的基本思想是利用泰勒级数展开将非线性系统线性化，然后采用卡尔曼滤波框架对信号进行滤波，因此它是一种次优滤波。一般来说，扩展卡尔曼滤波在对非线性函数做泰勒展开时，只取到一阶导和二阶导，而由于二阶导的计算复杂性，更多的实际应用只取到一阶导，同样也能有较好的结果。取一阶导时，状态转移方程和观测方程就近似为线性方程，高斯分布的变量经过线性变换之后仍然是高斯分布，这样就能够延用标准卡尔曼滤波的框架。

使用九轴运动传感器进行姿态解算时，正常情况下，加速计和陀螺仪融合即可得到比较稳定的滚转角和俯仰角，再加上磁强计主要是为了得到偏航角，但是磁强计不止对偏航角有影响，对其他两个角度的更新也有影响，当磁强计受到干扰时不止偏航角会不准确，其他两个角度也会受牵连变得不准确。

发明内容

本发明的目的在于提供一种抗干扰能力强的基于EKF的九轴运动传感器的姿态检测方法，通过对九轴运动传感器的数据进行处理，能够快速地得出准确的姿态角。