一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法
【技术领域】
[0001] 本发明属于水下地磁辅助导航领域，具体涉及到一种基于地磁梯度张量的四元数 卡尔曼滤波姿态估计方法。
【背景技术】
[0002] 姿态测量是现代导航、制导和控制等诸多领域的一个重要问题。目前，载体姿态确 定的方法主要可W分为两大类，矢量确定法和状态估计法。矢量确定法是利用几何关系直 接计算，当参考坐标系中的两个矢量准确已知的情况下，采用矢量匹配方法获得单次观测 的最优解。但很小的水平基准误差就会带来很大的航向误差，无法利用系统动态信息和历 史观测信息，不能通过滤波算法提高姿态估计的精度。另一方面，两个矢量准确已知也是很 难做到的。该种方法在很大程度上受到观测矢量的精度限制，无法克服观测矢量的不确定 性。状态估计法是通过建立动力学或者运动学模型，得到被估计量动态变化模型和测量模 型，采用递推的方法计算估计姿态参数和误差参数。它是一种基于统计特性的方法，能够 有效克服传感器误差项引起的参考矢量不确定性，得到统计意义上状态量的最优估计值， 提高姿态确定的精度。此外该方法可结合载体动态信息和历史观测信息实现递推估计，在 提高测量精度的同时还可同步估计姿态速率信息。常见的状态估计法有扩展卡尔曼滤波 巧K巧和非线性卡尔曼滤波扣KF)、递推四元数估计（REQUEST)、扩展四元数估计巧犯）等， 还有最小模型误差、自适应滤波、预测滤波和鲁棒滤波等用于估计姿态参数。EKF方法是基 于对非线性方程的线性化，当系统具有强非线性时，线性化可能引起大的误差甚至造成滤 波器的不稳定；UKF可W克服EKF的缺点，但对随机变量非线性变换后概率分布的逼近精度 相对较低；REQUEST是一种类EKF方法，它在每一步中应用QUEST算法，可W得到比传统的 EKF更高的精度，但却难于扩展估计其它参量，E犯结合了 REQUEST和EKF的优点，但它同样 不能避免线性化带来的问题。
[0003] 相比于磁场矢量测量，磁场梯度张量测量具有磁矢量测量的优势，但没有磁矢量 测量对地磁场方向敏感的缺点，相比于磁异常测量，磁场梯度张量测量能够测量更多关于 实测地点的信息，容易实现对测量数据的H维反演；磁场梯度张量作为观测量，根据载体 姿态估计的过程方程和观测方程具有强非线性的特性，提出的一种新的四元数卡尔曼滤波 (化ba化re Kalman Filter ;CKF)算法，算法本身内蕴了对四元数单位长度的限制，不再需 要对四元数估计值进行归一化处理；在高维系统中，CKF(化ba化re Kalman Filter)的估计 精度优于UKF。
[0004] 本发明提出的一种基于磁场梯度张量测量的新的姿态测量优化递推估计算法思 路新，未见文献报道。
【发明内容】
[0005] 本发明的目的在于提供一种能够提高估计精度的基于地磁梯度张量的四元数卡 尔曼滤波姿态估计方法。
[0006] 本发明的目的是该样实现的：
[0007] (1)设定初始参数值：
[0008] 由惯性测量单元输出数据确定初始时刻系统状态值和状态协方差P。；
[0009] (2)采集载体运动过程中巧螺及磁强计的输出数据作为量测量；
[0010] (3)测量地理系下地磁梯度张量G。；
[0011] 根据惯性/地磁组合导航系统的指示位置，从预先存储的地磁梯度张量数据库中 提取地理系下地磁梯度张量G。的5个独立分量挺，蘇.，蘇.，括，和挺
[001引 （4)巧帽载体系下地磁梯度张量护；
[0013] 根据步骤（2)中磁强计的输出数据测量载体系下磁场分量hi?hi。，计算载体系下 地磁梯度张量护的5个分量；
[0014]
【主权项】
1. 一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法，其特征在于，包括如下 步骤： (1) 设定初始参数值： 由惯性测量单元输出数据确定初始时刻系统状态值氧和状态协方差P(1; (2) 采集载体运动过程中陀螺及磁强计的输出数据作为量测量； (3) 测量地理系下地磁梯度张量Gn; 根据惯性/地磁组合导航系统的指示位置，从预先存储的地磁梯度张量数据库中提取 地理系下地磁梯度张量G% 5个独立分量gK，和g：； (4) 测量载体系下地磁梯度张量Gb; 根据步骤（2)中磁强计的输出数据测量载体系下磁场分量hi-hm，计算载体系下地磁 梯度张量Gb的5个分量；
"和Ly分别为x,和yb方向上梯度测量基线长； (5) k-1时刻利用四元数对数反对数切换及CKF的时间更新阶段进行状态更新： 过程噪声和观测噪声都是加性的，状态空间形式的离散非线性系统为： Xk - f (x k_j) +Wk_! yk=h(xk) +vk XkeR，PykeRm分别为状态向量和量测向量；f(?)和h(〇分别为系统非线性四 元数状态方程量测方程； 姿态估计系统的非线性四元数状态方程： 单位四元数：
载体姿态的单位四元数为7 = (% 5/ = (%ft92 %载体坐标系中的角速度矢量为
将陀螺测量坐标系与载体质心本体坐标系重合，陀螺角速度输出采用经典模型：
s(0为陀螺输出；P(t)为陀螺漂移；nv(t)和nu(t)分别为随机游走和漂移斜坡噪 声；选取单位四元数q(t)和陀螺漂移P(t)作为系统的状态向量，即X= [q(t)T 0 (t)T]T 代表系统的状态向量； 姿态估计系统的非线性四元数量测方程；选择载体坐标系下的地磁梯度张量的5个独 立分量作为观测量，G6 建立系统的观测方程为
G5PGb分别为地磁梯度张量在地理坐标系n系和载体坐标系b系下的表示，gL4.,,鉻，和么是Gb的5个独立分量，以，心，'，心，和K是GnW5个独立分量，T。 =Kqi，q2,q3)，i= 1，2, 3;j= 1，2, 3 为矩阵Cf的元素，且有
h(X)是与状态有关的非线性函数，观测噪声向量v为协方差为R的零均值白噪声； (5.l)k_l时刻利用四元数对数反对数切换的时间更新； 利用四元数对数反对数切换估计k-1时刻状态四元数q(t)部分的状态预测值和协方 差，采用对数指数变换法计算k_l时刻状态估计值中的四元数q(t)部分的状态预测值 和协方差，算法采用对数变换的Cubature卡尔曼滤波； (5. 1. 1)初始化：q=q0; (5. 1. 2)主循环：对于i= 1，2,…2n，计算Xi=log^qj，令
其中^为CKF中的加权系数； (5. 1. 3)如果|网足够小或超过最大迭代次数，终止循环，输出q，否则，继续循环； (5. 1. 4)利用最后一次循环的\计算协方差矩阵
(5. 2)k-l时亥IJ利用CKF的时间更新； 利用标准CKF算法估计Xuh中除四元数以外参量的均值，即陀螺漂移部分的均值； (5. 3)将步骤（5. 1)利用LTCKF算法得到的四元数部分均值和步骤（5. 2)利用CKF算 法得到的陀螺漂移部分的均值组合在一起构成k时刻的状态，则k时刻状态协方差预 测值Pk|k_A:
其中wk为系统噪声，Qh为系统噪声协方差； (6) 利用地磁梯度张量测量值及CKF的量测更新阶段估计k时刻的状态払4和协方差 f*k|k; 根据步骤5. 3得到的误差协方差PuH按照CKF标准算法确定Cubature点集Xi;k|k_1; 将得到的点集Xi;k|k_^过与状态有关的非线性函数h(Xum)传播Cubature点得到点 集Yi，k|k-1: Yi，k|k-1 一h0^i,k|k-l) 计算k时刻的量测预测值：
计算自相关协方差矩阵Pyy，k|H和互相关协方差矩阵Pxy>k|k-i:
Rk是量测噪声协方差，再根据自相关协方差矩阵Pyy，k|lrt和互相关协方差矩阵Pxy，k|lrl计 算卡尔曼增益Kk:
利用k时刻通过步骤⑵的得到的新的量测值yk对该时刻预测值进行校正，求出 状态估计&1*和状态协方差矩阵Pk|k:
k=k+1，转至步骤（5); (7) 对姿态及陀螺漂移进行校正： 利用状态估计值，确定四元数估计值以及陀螺漂移估计值，获得修正后k时刻姿态及 陀螺漂移； 四元数估计值用心，之和I表示，对k时刻姿态角进行修正：
其中式中，^为载体偏航角估计值，0为俯仰角估计值，f为横滚角估计值，
(8)姿态估计系统的运行时间为N，若k=N输出姿态及陀螺漂移结果，若k〈N，重复步 骤（5)_(7)，直到姿态估计系统运行结束。
【专利摘要】本发明属于水下地磁辅助导航领域，具体涉及到一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法。本发明包括：设定初始参数值；采集载体运动过程中陀螺及磁强计的输出数据作为量测量；测量地理系下地磁梯度张量；测量载体系下地磁梯度张量；进行状态更新；估计k时刻的状态；估计k时刻的状态。本发明提出的一种基于地磁梯度张量的Cubature卡尔曼滤波姿态估计方法对姿态角的估计精度比传统Cubature卡尔曼滤波算法高出数倍，而且通过三轴磁通门磁强计测量获取量测值yk的方法具有价格低廉的潜在优势。
【IPC分类】G01C21-20, G01C21-16, G01C21-08
【公开号】CN104567871
【申请号】CN201510016501
【发明人】黄玉, 武立华, 李德全
【申请人】哈尔滨工程大学
【公开日】2015年4月29日
【申请日】2015年1月12日