[发明专利]基于四元数-扩展卡尔曼滤波的低成本AHRS航向角补偿方法

说明书

基于四元数‑扩展卡尔曼滤波的低成本AHRS航向角补偿方法，方法包括以下步骤：S1、采集MEMS惯性传感器信息；S2、建立MEMS惯性传感器随机误差模型；S3、利用四元数姿态算法建立运动学方程；S4、根据四元数微分方程和陀螺仪随机误差模型建立姿态测量系统状态方程，利用加速度计和磁力计的输出值构建系统的量测方程；S5、利用扩展卡尔曼滤波获取最佳四元数，解算出AHRS姿态角。利用基于四元数‑扩展卡尔曼滤波的低成本AHRS航向角补偿方法可提升AHRS姿态检测模块的航向角姿态精度，算法结构简单解算速度较快。相较于其他姿态测试方案，本方法采用加速度计和磁力计的三轴测量值作为系统的6维观测量，所得结构可信度更高，有效的提升低成本MEMS AHRS的航向角姿态精度。

技术领域

本发明涉及微惯性导航技术相关领域，具体涉及一种基于四元数-扩展卡尔曼滤波的低成本AHRS航向角补偿方法。

背景技术

低成本的MEMS惯性器件精度差、噪声高，无法满足载体位置、速度导航要求，而常常用于载体姿态测量。其具有体积小、成本低、重量轻,便于集成、携带等优点，既可应用于小型航空飞行器，车辆自主驾驶，人体关节姿态测量等领域，又可应用于军用领域，如低成本制导弹药、小型无人机、雷达系统等，具有广阔的应用前景。但是MEMS惯性器件误差随时间不断累加，短时间内姿态精度迅速恶化，甚至无法应用。

MEMS惯性器件组成的AHRS姿态检测模块由于MEMS惯性器件的固有属性，输出的姿态信息精度普遍较低，尤其是系统关键器件MEMS陀螺仪噪声大、随机漂移误差严重，很大程度影响系统的测量精度，并且会导致航向角严重漂移，输出姿态可信度不高。目前提高MEMSAHRS的精度主要有两种技术途径。一种是从制造工艺和生产原理入手，通过优化结构设计，从根本上提高器件的精度，降低随机漂移带来的影响，但这种方法必然会带来必然会带来整个系统成本的大幅提升，而且测量精度提高有限，研发时间长；另一种方法是通过引入其他传感器辅助参与陀螺仪的姿态计算，采用数据融合策略对各个传感器取长补短，修正陀螺仪随机漂移误差，实现姿态测量系统测量精度和稳定性的提升。

四元数微分方程在解算过程中涉及到积分运算，由于陀螺仪具有常值漂移，在没有其他辅助传感器对陀螺仪数据进行修正的情况下，直接更新计算四元数会产生较大的累积误差，最终造成姿态估计发散。加速度计和磁力计可共同解算出载体的全姿态，对于陀螺仪姿态测量的漂移误差可以起到修正作用。因此，将加速度计和磁力计的三轴测量值作为系统的6维观测量。本发明通过引入加速度计与磁力计校准陀螺仪姿态解算，利用四元数-扩展卡尔曼滤波算法实现AHRS姿态检测模块的高精度俯仰、航向、横滚角的输出。

发明内容

基于上述低成本MEMS AHRS姿态检测模块因为MEMS惯性传感器噪声大、随机漂移误差严重导致的姿态输出精度低的问题，本发明基于四元数-扩展卡尔曼滤波的低成本AHRS航向角补偿方法。

为实现上述目的本发明具体方案如下：

基于四元数-扩展卡尔曼滤波的低成本AHRS航向角补偿方法，所述方法包括以下步骤：

S1、采集MEMS惯性传感器信息；

S2、建立MEMS惯性传感器随机误差模型；

S3、利用四元数姿态算法建立运动学方程；

S4、根据四元数微分方程和陀螺仪随机误差模型建立姿态测量系统状态方程，利用加速度计和磁力计的输出值构建系统的量测方程；

S5、利用扩展卡尔曼滤波获取最佳四元数，解算出AHRS姿态角。

可选地，所述步骤S1中的采集MEMS惯性传感器信息包括：MEMS陀螺仪数据信息、MEMS加速度计数据信息、MEMS磁力计数据信息。