[发明专利]一种基于扩展卡尔曼滤波的导航方法及装置

说明书

技术领域

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

背景技术

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

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

发明内容

本发明主要目的是公开一种基于扩展卡尔曼滤波的导航方法，对导航方法中滤波过程进行分阶优化以及对矩阵运算进行改进。分析十一阶参数之间的联系与独立性，将滤波系统拆分成两个子系统即一个五阶系统，一个六阶系统。通过研究矩阵的特征，改进通用的算法，减少计算量，节省存储空间从而达到提高效率的目的。

本发明目的还在于公开实现基于上述方法的一种导航装置。

本发明的基于扩展卡尔曼滤波的导航方法通过如下步骤实现：

(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)输出数据：输出修正后的当前状态的估计值。

传感器元件得到的原始信息是不能直接被赋值给滤波器，必须经过一定的转换得到所需的姿态角(横滚角俯仰角θ、偏航角ψ)。

其中横滚角与俯仰角是通过线加速度来转换得到，偏航角是通过地磁通量来转换得到。在得到姿态角后需要通过转换公式将姿态角转化为四元数。转换公式如下所述：

线加速度转角度：

g=ax2+ay2+az2]]>

φ＝atan 2(a_y，a_z)                        (1)

θ＝a tan 2(a_x，g)

地磁通量转角度：