[发明专利]一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法

摘要

本发明属于水下地磁辅助导航领域，具体涉及到一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法。本发明包括：设定初始参数值；采集载体运动过程中陀螺及磁强计的输出数据作为量测量；测量地理系下地磁梯度张量；测量载体系下地磁梯度张量；进行状态更新；估计k时刻的状态；估计k时刻的状态。本发明提出的一种基于地磁梯度张量的Cubature卡尔曼滤波姿态估计方法对姿态角的估计精度比传统Cubature卡尔曼滤波算法高出数倍，而且通过三轴磁通门磁强计测量获取量测值yk的方法具有价格低廉的潜在优势。

主权项

1.一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法，其特征在于，包括如下步骤：(1)设定初始参数值：由惯性测量单元输出数据确定初始时刻系统状态值和状态协方差P₀；(2)采集载体运动过程中陀螺及磁强计的输出数据作为量测量；(3)测量地理坐标系下地磁梯度张量Gn；根据惯性/地磁组合导航系统的指示位置，从预先存储的地磁梯度张量数据库中提取地理坐标系下地磁梯度张量Gⁿ的5个独立分量和(4)测量载体坐标系下地磁梯度张量Gb；根据步骤(2)中磁强计的输出数据测量载体坐标系下磁场分量h1～h10，计算载体坐标系下地磁梯度张量Gb的5个分量；Lx和Ly分别为xb和yb方向上梯度测量基线长；(5)k‑1时刻利用四元数对数反对数切换及CKF标准算法的时间更新阶段进行状态更新：过程噪声和观测噪声都是加性的，状态空间形式的离散非线性系统为：xk＝f(xk‑1)+wk‑1yk＝h(xk)+vkxk∈Rn和yk∈Rm分别为状态向量和量测向量；f(·)和h(·)分别为系统非线性四元数状态方程量测方程；姿态估计系统的非线性四元数状态方程：单位四元数：载体姿态的单位四元数为载体坐标系中的角速度矢量为其中，表示四元数乘运算；将陀螺测量坐标系与载体质心本体坐标系重合，陀螺角速度输出采用经典模型：ω(t)为陀螺输出；β(t)为陀螺漂移；ηv(t)和ηu(t)分别为随机游走和漂移斜坡噪声；选取单位四元数q(t)和陀螺漂移β(t)作为系统的状态向量，即X＝[q(t)T β(t)T]T代表系统的状态向量；姿态估计系统的非线性四元数量测方程；选择载体坐标系下的地磁梯度张量的5个独立分量作为观测量，建立系统的观测方程为Gⁿ和G^b分别为地磁梯度张量在地理坐标系n系和载体坐标系b系下的表示，和是G^b的5个独立分量，和是Gⁿ的5个独立分量，T_ij＝T_ij(q₀,q₁,q₂,q₃),i＝1,2,3；j＝1,2,3为矩阵的元素，且有h(X)是与状态有关的非线性函数，观测噪声向量v为协方差为R的零均值白噪声；(5.1)k‑1时刻利用四元数对数反对数切换的时间更新；利用四元数对数反对数切换估计k‑1时刻状态四元数q(t)部分的状态预测值和协方差，采用对数指数变换法计算k‑1时刻状态估计值中的四元数q(t)部分的状态预测值和协方差，采用对数变换的Cubature卡尔曼滤波算法；(5.1.1)初始化：q＝q0；(5.1.2)主循环：对于i＝1,2,…2n，计算x_i＝log_q(q_i)，令其中w_i为CKF标准算法中的加权系数；(5.1.3)如果足够小或超过最大迭代次数，终止循环，输出q，否则，继续循环；(5.1.4)利用最后一次循环的x_i计算协方差矩阵(5.2)k‑1时刻利用CKF标准算法的时间更新；利用CKF标准算法估计Xi,k|k‑1中除四元数以外参量的均值，即陀螺漂移部分的均值；(5.3)将步骤(5.1)利用对数变换的Cubature卡尔曼滤波算法得到的四元数部分均值和步骤(5.2)利用CKF标准算法得到的陀螺漂移部分的均值组合在一起构成k时刻的状态则k时刻状态协方差预测值P_k|k‑1为：其中wk为系统噪声，Qk‑1为系统噪声协方差；(6)利用地磁梯度张量测量值及CKF标准算法的量测更新阶段估计k时刻的状态和协方差P_k|k；根据步骤5.3得到的误差协方差Pk|k‑1按照CKF标准算法确定Cubature点集Xi,k|k‑1；将得到的点集Xi,k|k‑1通过与状态有关的非线性函数h(Xi,k|k‑1)传播Cubature点得到点集Yi,k|k‑1：Yi,k|k‑1＝h(Xi,k|k‑1)计算k时刻的量测预测值：计算自相关协方差矩阵Pyy,k|k‑1和互相关协方差矩阵Pxy,k|k‑1：Rk是量测噪声协方差，再根据自相关协方差矩阵Pyy,k|k‑1和互相关协方差矩阵Pxy,k|k‑1计算卡尔曼增益Kk：利用k时刻通过步骤(2)的得到的新的量测值y_k对该时刻预测值进行校正，求出状态估计和状态协方差矩阵P_k|k：k＝k+1，转至步骤(5)；(7)对姿态及陀螺漂移进行校正：利用状态估计值，确定四元数估计值以及陀螺漂移估计值，获得修正后k时刻姿态及陀螺漂移；四元数估计值用和表示，对k时刻姿态角进行修正：其中式中，为载体偏航角估计值，θ为俯仰角估计值，为横滚角估计值，(8)姿态估计系统的运行时间为N,若k＝N输出姿态及陀螺漂移结果，若k

下载完整专利技术内容需要扣除积分，VIP会员可以免费下载。 免登录下载普通用户下载升级VIP会员，免费下载

该专利技术资料仅供研究查看技术是否侵权等信息，商用须获得专利权人授权。该专利全部权利属于哈尔滨工程大学，未经哈尔滨工程大学许可，擅自商用是侵权行为。如果您想购买此专利、获得商业授权和技术合作，请联系【客服】

本文链接：http://www.vipzhuanli.com/patent/201510016501.3/，转载请声明来源钻瓜专利网。