一种水下组合导航系统行进间对准与姿态估计方法技术方案

本发明专利技术涉及捷联惯性导航技术领域，具体涉及到一种水下组合导航系统行进间对准与姿态估计方法。在AUV水下组合导航系统进行行进间对准及姿态估计时，本发明专利技术先通过马氏距离方法判断观测信息是否被野值污染：如果判定DVL观测信息没有被污染，利用变分贝叶斯方法自适应观测噪声协方差矩阵；反之，如果DVL观测信息被污染，通过计算膨胀因子以衡量被污染程度，将膨胀因子引入变分贝叶斯方法自适应过程，自适应得到观测噪声协方差。最后通过VBRAKF状态估计得到高精度的水下运载体姿态信息以及导航信息。运用本发明专利技术方法进行组合导航，系统有着更高的导航精度，更强的可靠性，满足自主、精确水下长航时、长航程的导航定位能力。长航程的导航定位能力。长航程的导航定位能力。

A method of alignment and attitude estimation for Underwater Integrated Navigation System

【技术实现步骤摘要】
一种水下组合导航系统行进间对准与姿态估计方法


[0001]本专利技术涉及捷联惯性导航
，具体涉及到一种水下组合导航系统行进间对准与姿态估计方法。

技术介绍

[0002]自主式水下航行器(AUV)被广泛应用于国家海防建设、海洋资源勘探等军民领域。高精度、高可靠性的自主导航能力是AUV顺利完成任务的前提。捷联式惯性导航系统(SINS)由于结构简单、体积小、隐蔽性强、便于与其他设备集成化设计等优点,在水下定位导航与授时领域受到广泛关注，成为AUV自主导航定位的重要手段。SINS本质上是以牛顿第二定律为基础的积分推算系统，需要对惯性测量单元(IMU)的输出进行积分运算。因此，任何初始误差都会带入到积分运算系统使得误差不断累积，进而影响SINS长航时、远距离的导航定位精度。水下环境对全球导航卫星系统(GNSS)信号具有屏蔽作用，而多普勒计程仪(DVL)测速精度稳定，可为SINS提供实时外部速度辅助融合信息，SINS/DVL组合导航是当前水下导航的主流方案。
[0003]初始对准是SINS进行导航解算的前置步骤，高精度的初始对准技术是水下SINS/DVL组合系统长航时、远距离导航定位的重要保证。初始对准作为一种有效减小初始误差以提升导航精度的重要方法，一般分为粗对准和精对准两个阶段：粗对准将运载体的姿态快速地估计在一个较小的范围内，再通过精对准以获得更高的精度；精对准分为静基座精对准和行进间对准两种。静基座精对准要求运载体在起始阶段处于完全静止状态，先前的研究较多且技术已相对成熟。但由于风浪、水流等外部因素影响，水下初始对准难以满足静止条件，因此对水下行进间对准与姿态估计展开研究更具有现实意义。
[0004]在水下行进间对准与组合导航姿态估计过程中，外界环境复杂多变，水下生物、地形地质、水流变化等因素都会对DVL的输出速度产生影响，卡尔曼滤波建立的高斯无偏模型无法准确反映水下噪声。水下行进间对准面临的困难主要有：1)水下环境复杂导致高斯噪声模型不再适用；2)水下环境容易导致量测出现野值。
[0005]文献1[A.Sage and G.Husa,“Adaptive filtering with unknown prior statistics,”Joint Automatic Control Conference,vol.7,pp.760
‑
769,1969.]针对卡尔曼滤波中高斯噪声模型不准确的问题提出了Sage
‑
Husa自适应卡尔曼滤波(Sage
‑
Husa adaptive kalman filter，SHAKF)，但是利用SHAKF方法得到的噪声协方差阵的正定性和噪声模型的准确性难以保证，导致滤波效果变差甚至发散；文献2[B.Zhu and H.He,“Integrated navigation for doppler velocity log aided strapdown inertial navigation system based on robust IMM algorithm,”Optik,vol.217,p.164871,2020.]提出了交互多模型(interacting multiple model，IMM)方法以匹配不同的噪声模型，但是此算法需要大量先验信息来建立模型库，计算量庞大，实时性较差，制约了其在工程实践中的应用；文献3[沈忱,徐定杰,沈锋,蔡佳楠.GPS/INS组合导航的变分贝叶斯自适应卡尔曼滤波[J].哈尔滨工业大学学报,2014,46(05):59
‑
65.]在GPS/INS组合导航系统中
提出了一种变分贝叶斯自适应卡尔曼滤波(variational bayesian adaptive kalman filter，VBAKF)以应对不准确的噪声模型，但是此算法需要GPS信息且在受到野值影响时滤波性能下降，难以满足水下行进间对准的要求；文献4[X.Lyu,B.Hu,K.Li and L.Chang,“An adaptive and robust UKF approach based on Gaussian process regression
‑
aided variational Bayesian,”IEEE Sensors Journal,vol.21,no.7,pp.9500
–
9514,2021.]等将基于机器学习的高斯过程回归模型引入到变分贝叶斯自适应估计算法中，从而增强了系统的鲁棒性，但是算法对系统状态维数和样本数量有严格要求；文献5[G.Chang,“Robust Kalman filtering based on Mahalanobis distance as outlier judging criterion,”Journal of Geodesy,vol.88,no.4,pp.391
–
401,2014.]提出了一种基于马氏距离(mahalanobis diatance,MD)的鲁棒卡尔曼滤波方法(robust kalman filter，RKF)，能够有效地剔除观测数据中野值的影响，但是RKF不能有效自适应噪声模型的变化，这就导致当噪声模型不准确时其性能效果不佳。
[0006]基于上述已有研究，现有相关技术的主要缺点是：不能同时满足行进间对准与姿态估计滤波算法的自适应性、鲁棒性与普适性，当存在不准确的噪声模型问题及野值污染问题时(这也是水下环境的面临的一大问题以及技术难点)滤波性能较差。

技术实现思路

[0007]为了综合解决水下组合导航行进间姿态估计中不准确的噪声模型问题以及野值污染问题，本专利技术提出一种水下组合导航系统行进间对准与姿态估计方法。该方法基于马氏距离的变分贝叶斯鲁棒自适应卡尔曼滤波器(variational bayesian robust adaptive kalman filter，VBRAKF)，来进行AUV的SINS/DVL组合导航系统行进间姿态估计。
[0008]在AUV水下组合导航系统进行行进间对准及姿态估计时，本专利技术先通过马氏距离方法判断观测信息是否被野值污染：如果判定DVL观测信息没有被污染，利用变分贝叶斯方法自适应观测噪声协方差矩阵；反之，如果DVL观测信息被污染，通过计算膨胀因子以衡量被污染程度，将膨胀因子引入变分贝叶斯方法自适应过程，自适应得到观测噪声协方差。最后通过VBRAKF状态估计得到高精度的水下运载体姿态信息以及导航信息。
[0009]本专利技术采用的技术方案为，一种水下组合导航系统行进间对准与姿态估计方法，该方法分为以下步骤：
[0010]S1：将自主式水下航行器(AUV)的捷联惯性导航系统(SINS)与多普勒测速仪(DVL)接入导航计算机，将AUV投放至任务区域。当AUV潜入水下，无法接收到GNSS信号时，通过传统方法进行姿态粗对准；
[0011]S2：通过导航计算机接收到捷联惯性导航系统中陀螺与加速度计输出的角速度与加速度增量，利用导航计算机进行本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种水下组合导航系统行进间对准与姿态估计方法，其特征在于，该方法分为以下步骤：S1：将自主式水下航行器AUV的捷联惯性导航系统SINS与多普勒测速仪DVL接入导航计算机，将AUV投放至任务区域；当AUV潜入水下，无法接收到GNSS信号时，通过传统方法进行姿态粗对准；S2：通过导航计算机接收到捷联惯性导航系统中陀螺与加速度计输出的角速度与加速度增量，利用导航计算机进行导航解算：定义坐标系：选取“东—北—天”地理坐标系为导航坐标系，记为n系；选取“右—前—上”坐标系为载体坐标系，记为b系；地心惯性坐标系记为i系；地球坐标系记为e系；计算导航坐标系记为n
′
系；S2.1执行导航计算机计时器，初始化时刻k＝0；S2.2计时器时间更新k＝k+1；S2.3执行惯性导航解算通过陀螺输出的采样角增量和加速度计输出的采样比力速度增量解算得到k时刻的SINS姿态姿态矩阵速度和位置S3：构建13维离散化的基于马氏距离的VBRAKF滤波模型与观测模型，具体步骤如下：S3.1构建组合导航系统13维基于马氏距离的VBRAKF的滤波模型：S3.1.1执行VBRAKF状态一步预测更新为：式(1)中，表示k
‑
1时刻输出的状态向量X的后验估计，表示状态向量X从k
‑
1时刻至k时刻的一步预测；X表示包含SINS姿态误差速度误差δv，位置误差δP和陀螺漂移误差ε
b
、加速度零偏误差的13维状态向量，X的表达式为：的13维状态向量，X的表达式为：的初始值设置为01×
13
表示1
×
13维的零矩阵，上标T表示矩阵转置；式(2)中，位置误差为δP＝[δL,δλ]，δL和δλ分别为SINS的纬度误差与经度误差；速度误差δv＝[δv
E
,δv
N
]，δv
E
和δv
N
分别为东向速度误差和北向速度误差；姿态失准角分别为东向速度误差和北向速度误差；姿态失准角分别表示x轴、y轴和z轴方向的姿态失准角；陀螺漂移误差为加速度计零偏误差为式(1)中，Φ
k|k
‑1为从k
‑
1时刻到k时刻的状态一步预测矩阵，计算公式为：Φ
k|k
‑1≈I+F(k
‑
1)T
s
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)式(3)中，I为13维单位矩阵，T
s
为离散化时间间隔，F(k
‑
1)为k
‑
1时刻的SINS系统模型矩阵；F(k
‑
1)的表达式为：
其中0
i
×
j
为i
×
j维的零矩阵，F
ij
为子矩阵，各子矩阵的表达式为：为子矩阵，各子矩阵的表达式为：为子矩阵，各子矩阵的表达式为：为子矩阵，各子矩阵的表达式为：为子矩阵，各子矩阵的表达式为：为子矩阵，各子矩阵的表达式为：为子矩阵，各子矩阵的表达式为：为子矩阵，各子矩阵的表达式为：为子矩阵，各子矩阵的表达式为：为子矩阵，各子矩阵的表达式为：式(5)
‑
式(14)中，R
e
为地球半径，v
E
(k
‑
1)为k
‑
1时刻SINS的东向速度，v
N
(k
‑
1)为k
‑
1时刻SINS的北向速度，L(k
‑
1)为k
‑
1时刻SINS的纬度，ω
ie
为地球自转角速度，f
U
(k
‑
1)表示k
‑
1时刻加速度计输出的比力在天向的投影，f
N
(k
‑
1)表示k
‑
1时刻加速度计输出的比力在北向的投影，f
E
(k
‑
1)表示k
‑
1时刻加速度计输出的比力在东向的投影，表示k
‑
1时刻导航计算机解算得到的姿态矩阵，表示姿态矩阵的前两行；S3.1.2执行VBRAKF状态一步预测均方误差矩阵更新：式(15)中，P
k
‑
1|k
‑1称为k
‑
1时刻的状态后验估计均方误差矩阵，P
k|k
‑1称为从k
‑
1时刻到k
时刻的状态一步预测均方误差矩阵，利用式(15)，状态后验估计均方误差矩阵P
k
‑
1|k
‑1更新成为状态一步预测均方误差矩阵P
kk
‑1；P
k
‑
1|k
‑1初始值P
0|0
为：其中diag表示矩阵对角线元素，μg为加速度计零偏误差单位，1μg＝10
‑6g≈9.8
×
10
‑6m/s2，13
×
13矩阵其他元素为0；Q(k
‑
1)称为k
‑
1时刻的系统噪声矩阵，是一个13
×
13维的对角矩阵，由SINS陀螺和加速度计的随机误差决定；由于SINS误差特性稳定，因此系统噪声矩阵是恒定的，即Q(k
‑
1)＝Q(0)，初始值为：S3.2将导航计算机SINS解算速度减去DVL输出的观测速度在计算导航系下的投影速度作为k时刻的观测量Z(k)，构建VBRAKF观测模型：式(18)中，为k时刻DVL输出的载体坐标系下的速度；式(18)中第二至第六行是根据观测量Z(k)构建观测模型，为k时刻SINS真实的姿态矩阵，为k时刻的理想导航坐标系至计算导航坐标系间的姿态矩阵，即k时刻的姿态误差矩阵的逆矩阵，的逆矩阵，表示k时刻的姿态误差，
×
为叉乘符号，表示的反对称矩阵，其表达式为：式(18)中，v
n
(k)为k时刻SINS真实的速度，δv
n
(k)为k时刻SINS真实速度误差；Z(k)＝H(k)X(k)+V(k)为组合导航系统的观测模型，H(k)为k时刻的观测矩阵，其表达式为：式(20)中，I2×2为2
×
2的单位矩阵，02×2为2
×
2的零矩阵；实际式(20)计算过程中，用代替V(k)为k时刻的观测噪声矩阵，由k时刻的东向速度观测噪声V
E
(k)和北向速度观测噪声V
N
(k)组成，其表达式为：
设R
E
(k),R
N
(k)分别为k时刻的东向、北向速度观测噪声方差，满足如下条件：E[
·
]表示求其期望值；观测噪声V(...

【专利技术属性】
技术研发人员：李鼎，于旭东，史文策，何泓洋，罗晖，吴苗，许江宁，
申请(专利权)人：中国人民解放军国防科技大学，
类型：发明
国别省市：