[实用新型]一种基于扩展卡尔曼滤波的导航装置

说明书

技术领域

本实用新型适用于高实时性、高精度要求的的导航系统领域，尤其适用于一种利用低成本设备获取高精度结果的基于扩展卡尔曼滤波的导航装置。

背景技术

导航系统的主要作用是提供实时地位置、速率、姿态信息，并且可以使之按照预先设定的路径行走。但是由于外界的诸多干扰使有用信号参杂很多噪声，卡尔曼滤波可以解决此类问题。卡尔曼滤波是一种递归估计，只要获知上一时刻状态的估计值和前状态的观测值就可以计算出当前状态的估计值，因此不需要记录观测或者估计的历史信息。卡尔曼滤波器将位置、速度、姿态角信息融合起来，并且通过预测与更新来滤除噪声，从而使获取的信息更准确，使控制更加精确。

卡尔曼滤波技术的实用新型给数字信号处理带来了一场意义重大的革命。卡尔曼滤波算法虽然很成熟，但是目前算法大多只支持少数状态的滤波处理。由于算法中频繁用到矩阵运算，其中包括矩阵相乘，相加减以及求逆，如果按照普通的运算法则将会消耗极大内存空间，并且执行时间冗长，效率低。所以设计高效的算法是很有必要的。此基于扩展卡尔曼滤波的导航方法及装置是对十一个状态量进行融合滤波，通过研究矩阵运算来提高算法效率，从而降低滤波周期，使之在高频低周期无操作系统的导航硬件中顺利可靠运行。这样可以减少对导航系统硬件的要求，降低成本。

发明内容

本实用新型主要目的是公开一种基于扩展卡尔曼滤波的导航装置，减少系统计算量，节省存储空间从而达到提高效率的目的。

一种基于扩展卡尔曼滤波的导航装置，如图1所示，包括数据采集模块、数据处理系统和数据反馈模块，所述数据采集模块用于采集当前状态观测值，所述信号处理系统用于将当前状态观测值转换为所需姿态角并进一步转换为所需模型矩阵，所述数据反馈模块用于输出当前状态估计值。

所述导航装置还包括扩展卡尔曼滤波器，所述扩展卡尔曼滤波器接收上述信号处理系统的信号，通过扩展卡尔曼滤波，将当前状态观测值修正为估计值，并反馈给数据反馈模块，得到精确导航数据。

所述数据采集模块包括电子罗盘、全球定位系统和惯性测量元件；所述电子罗盘用于测量地磁通量，所述全球定位系统用以测量速度和位置信息。

所述惯性测量元件包括加速度计和陀螺仪；所述加速度计用于测量线加速度，陀螺仪用于测量角速度。

所述扩展卡尔曼滤波器和数据处理系统由ARM7芯片提供；所述ARM7芯片通过SPI接口与惯性导航模块和电子罗盘通讯；所述ARM7芯片通过DEBUG口与GPS模块通讯。

所述数据处理系统由算法实现，将惯性测量元件获取的角速度和线加速度，通过公式(1)转换为横滚角和俯仰角。电子罗盘获取的地磁通量通过公式(2)得到偏航角ψ。再通过公式(3)得到四元数。最后将全球定位系统获得的位置、速度信息，与四元数和重力加速融合，得到状态量。

本实用新型基本原理如下所述，装置运行步骤：

(1)采集数据：由数据采集模块获取当前状态观测值，包括角速度p、q、r；线加速度a_x、a_y、a_z；地磁通量H_x、H_y、H_z；速度和位置信息x、y、z、u、v、w；

(2)预处理数据：将步骤(1)获得角速度、线加速度、地磁通量转换成所需要的姿态角，再进一步转化为四元数；

(3)建立模型：利用步骤(1)、(2)获得数据，对复杂非线性导航系统进行泰勒展开降阶线性化，分别建立状态模型、状态转移模型和观测模型；

(4)扩展卡尔曼滤波：根据卡尔曼滤波基本方程，利用步骤(3)建立的状态量模型和上次状态的估计值来预测当前状态的估计值，作为对当前状态观测值的修正；

(5)输出数据：输出修正后的当前状态的估计值。

传感器元件得到的原始信息是不能直接被赋值给滤波器，必须经过一定的转换得到所需的姿态角(横滚角俯仰角θ、偏航角ψ)。其中横滚角与俯仰角是通过线加速度来转换得到，偏航角是通过地磁通量来转换得到。在得到姿态角后需要通过转换公式将姿态角转化为四元数。转换公式如下所述：

线加速度转角度：