专利名称：一种基于扩展卡尔曼滤波的导航方法及装置的制作方法
一种基于扩展卡尔曼滤波的导航方法及装置技术领域
本发明适用于高实时性、高精度要求的的导航系统领域，尤其适用于一种利用低成本设备获取高精度结果的基于扩展卡尔曼滤波的导航方法及装置。
背景技术：
导航系统的主要作用是提供实时地位置、速率、姿态信息，并且可以使之按照预先设定的路径行走。但是由于外界的诸多干扰使有用信号参杂很多噪声，卡尔曼滤波可以解决此类问题。卡尔曼滤波是一种递归估计，只要获知上一时刻状态的估计值和前状态的观测值就可以计算出当前状态的估计值，因此不需要记录观测或者估计的历史信息。卡尔曼滤波器将位置、速度、姿态角信息融合起来，并且通过预测与更新来滤除噪声，从而使获取的信息更准确，使控制更加精确。
卡尔曼滤波技术的发明给数字信号处理带来了一场意义重大的革命。卡尔曼滤波算法虽然很成熟，但是目前算法大多只支持少数状态的滤波处理。由于算法中频繁用到矩阵运算，其中包括矩阵相乘，相加减以及求逆，如果按照普通的运算法则将会消耗极大内存空间，并且执行时间冗长，效率低。所以设计高效的算法是很有必要的。此基于扩展卡尔曼滤波的导航方法及装置是对十一个状态量进行融合滤波，通过研究矩阵运算来提高算法效率，从而降低滤波周期，使之在高频低周期无操作系统的导航硬件中顺利可靠运行。这样可以减少对导航系统硬件的要求，降低成本。发明内容
本发明主要目的是公开一种基于扩展卡尔曼滤波的导航方法，对导航方法中滤波过程进行分阶优化以及对矩阵运算进行改进。分析十一阶参数之间的联系与独立性，将滤波系统拆分成两个子系统即一个五阶系统，一个六阶系统。通过研究矩阵的特征，改进通用的算法，减少计算量，节省存储空间从而达到提高效率的目的。
本发明目的还在于公开实现基于上述方法的一种导航装置。
本发明的基于扩展卡尔曼滤波的导航方法通过如下步骤实现
(1)采集数据由数据采集模块获取当前状态观测值，包括角速度P、q、r ；线加速度 、 、 ；地磁通量Hx、Hy、Hz ；速度和位置信息x、y、ζ、u、ν、w ；
(2)预处理数据将步骤(1)获得角速度、线加速度、地磁通量转换成所需要的姿态角，再进一步转化为四元数；
(3)建立模型利用步骤(1)、(2)获得数据，对复杂非线性导航系统进行泰勒展开降阶线性化，分别建立状态模型、状态转移模型和观测模型；
(4)扩展卡尔曼滤波根据卡尔曼滤波基本方程，利用步骤C3)建立的状态量模型和上次状态的估计值来预测当前状态的估计值，作为对当前状态观测值的修正；
(5)输出数据输出修正后的当前状态的估计值。
传感器元件得到的原始信息是不能直接被赋值给滤波器，必须经过一定的转换得到所需的姿态角(横滚角识、俯仰角9、偏航角ψ)。其中横滚角与俯仰角是通过线加速度来转换得到，偏航角是通过地磁通量来转换 得到。在得到姿态角后需要通过转换公式将姿态角转化为四元数。转换公式如下所述线加速度转角度
权利要求
1.一种基于扩展卡尔曼滤波的导航方法，其特征在于包括以下步骤(1)采集数据由数据采集模块获取当前状态观测值，包括角速度P、q、r；线加速度a,、a7、az ；地磁通量 Γρ Ιψ Hx ；速度和位置信息χ、y、ζ、ν, w ；(2)预处理数据将步骤(1)获得角速度、线加速度、地磁通量转换成所需要的姿态角， 再进一步转化为四元数；(3)建立模型利用步骤(1)、(2)获得数据，对复杂非线性导航系统进行泰勒展开降阶线性化，分别建立状态模型、状态转移模型和观测模型；(4)扩展卡尔曼滤波根据卡尔曼滤波基本方程，利用步骤(3)建立的模型和上次状态的估计值来预测当前状态的估计值，作为对当前状态观测值的修正；(5)输出数据输出修正后的当前状态的估计值。
2.根据权利要求1所述的基于扩展卡尔曼滤波的导航方法，其特征在于步骤(2)姿态角包括横滚角-俯仰角沒和偏航角辦，由下式得到
3.根据权利要求1所述的基于扩展卡尔曼滤波的导航方法，其特征在于步骤(3)中所述状态模型以位置、速度、四元数以及重力加速度为参数对当前状态进行修正得到矩阵；所述是状态转移模型由状态模型矩阵的元素分别对位置、速度、四元数以及重力加速度求偏导而得；观测模型是由观测量分别对四元数求偏导而得。
4.根据权利要求1所述的基于扩展卡尔曼滤波的导航方法，其特征在于步骤(4)扩展卡尔曼滤波通过以下方式处理矩阵求逆运算判断主元是否为零是，则采用高斯-约当算法；否，则采用全选主元算法。
5.根据权利要求1所述的基于扩展卡尔曼滤波的导航方法，其特征在于步骤(4)扩展卡尔曼滤波将状态模型和观测模型分为两个子系统，对于具有不同特征的状态量进行分块、按序滤波系统一将速度、位置变量既作为状态向量也作为观测向量；系统二将四元数作为状态向量，将姿态角作为观测向量。
6.根据权利要求1所述的基于扩展卡尔曼滤波的导航方法，如果已知矩阵运算结果为对称矩阵，可以通过下面方式来对算法进行简化(1)初始化结果存放的矩阵，将其下(上)三角元素初始化为零；(2)在计算过程中只计算相应的下(上)三角矩阵元素；(3)将下(上)三角矩阵中的元素对称赋给上(下)三角矩阵。
7.一种基于扩展卡尔曼滤波的导航装置，包括数据采集模块、数据处理系统和数据反馈模块，所述数据采集模块用于采集当前状态观测值，所述信号处理系统用于将当前状态观测值转换为所需姿态角并进一步转换为所需模型矩阵，所述数据反馈模块用于输出当前状态估计值，其特征在于还包括扩展卡尔曼滤波器，所述扩展卡尔曼滤波器接收上述信号处理系统的信号，通过扩展卡尔曼滤波，将当前状态观测值修正为估计值，并反馈给数据反馈模块，得到精确导航数据。
8.根据权利要求7所述的扩展卡尔曼滤波导航装置，其特征在于所述数据采集模块包括电子罗盘、全球定位系统和惯性测量元件；所述电子罗盘用于测量地磁通量，所述全球定位系统用以测量速度和位置信息。
9.根据权利要求8所述的扩展卡尔曼滤波导航装置，其特征在于所述惯性测量元件包括加速度计和陀螺仪；所述加速度计用于测量线加速度，陀螺仪用于测量角速度。
10.根据权利要求9所述的扩展卡尔曼滤波导航装置，其特征在于所述扩展卡尔曼滤波器和数据处理系统由ARM7芯片提供；所述ARM7芯片通过SPI接口与惯性导航模块和电子罗盘通讯；所述ARM7芯片通过DEBUG 口与GPS模块通讯。
全文摘要
本发明公开了一种基于扩展卡尔曼滤波的导航方法及装置。通过对实际导航系统建立模型，模型包括状态模型、状态转移模型和观测模型，对导航方法中滤波过程进行分阶优化以及对矩阵运算进行改进。分析十一阶参数之间的联系与独立性，对矩阵运算进行研究、对系统进行拆分处理。从而达到了运算周期短，减少计算量，节省存储空间从而提高效率的目的,同时也降低了对硬件的要求。
文档编号G01C21/20GK102519463SQ20111041691
公开日2012年6月27日 申请日期2011年12月13日 优先权日2011年12月13日
发明者张铖, 申文斌, 裴海龙 申请人:华南理工大学