CN114777812B - 一种水下组合导航系统行进间对准与姿态估计方法 - Google Patents
一种水下组合导航系统行进间对准与姿态估计方法 Download PDFInfo
- Publication number
- CN114777812B CN114777812B CN202210415114.7A CN202210415114A CN114777812B CN 114777812 B CN114777812 B CN 114777812B CN 202210415114 A CN202210415114 A CN 202210415114A CN 114777812 B CN114777812 B CN 114777812B
- Authority
- CN
- China
- Prior art keywords
- matrix
- error
- navigation
- time
- moment
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 72
- 239000011159 matrix material Substances 0.000 claims abstract description 98
- 238000001914 filtration Methods 0.000 claims description 24
- 238000004364 calculation method Methods 0.000 claims description 14
- 230000001133 acceleration Effects 0.000 claims description 5
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 5
- 125000003275 alpha amino acid group Chemical group 0.000 claims description 4
- 150000001875 compounds Chemical class 0.000 claims description 4
- 238000005070 sampling Methods 0.000 claims description 4
- 238000006243 chemical reaction Methods 0.000 claims description 2
- 238000012937 correction Methods 0.000 claims description 2
- 125000004122 cyclic group Chemical group 0.000 claims description 2
- 239000000203 mixture Substances 0.000 claims description 2
- 230000008569 process Effects 0.000 abstract description 7
- 230000036961 partial effect Effects 0.000 description 12
- 238000012360 testing method Methods 0.000 description 12
- 230000003044 adaptive effect Effects 0.000 description 10
- 230000000694 effects Effects 0.000 description 10
- 238000011160 research Methods 0.000 description 3
- 239000000243 solution Substances 0.000 description 3
- 230000003068 static effect Effects 0.000 description 3
- 230000006978 adaptation Effects 0.000 description 2
- 239000002585 base Substances 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 238000012795 verification Methods 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 239000003637 basic solution Substances 0.000 description 1
- 238000013477 bayesian statistics method Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000010835 comparative analysis Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000011109 contamination Methods 0.000 description 1
- 230000007123 defense Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000002708 enhancing effect Effects 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000002452 interceptive effect Effects 0.000 description 1
- 230000000670 limiting effect Effects 0.000 description 1
- 238000010801 machine learning Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000002829 reductive effect Effects 0.000 description 1
- 238000001228 spectrum Methods 0.000 description 1
- 238000012876 topography Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
Abstract
本发明涉及捷联惯性导航技术领域，具体涉及到一种水下组合导航系统行进间对准与姿态估计方法。在AUV水下组合导航系统进行行进间对准及姿态估计时，本发明先通过马氏距离方法判断观测信息是否被野值污染：如果判定DVL观测信息没有被污染，利用变分贝叶斯方法自适应观测噪声协方差矩阵；反之，如果DVL观测信息被污染，通过计算膨胀因子以衡量被污染程度，将膨胀因子引入变分贝叶斯方法自适应过程，自适应得到观测噪声协方差。最后通过VBRAKF状态估计得到高精度的水下运载体姿态信息以及导航信息。运用本发明方法进行组合导航，系统有着更高的导航精度，更强的可靠性，满足自主、精确水下长航时、长航程的导航定位能力。
Description
技术领域
本发明涉及捷联惯性导航技术领域，具体涉及到一种水下组合导航系统行进间对准与姿态估计方法。
背景技术
自主式水下航行器(AUV)被广泛应用于国家海防建设、海洋资源勘探等军民领域。高精度、高可靠性的自主导航能力是AUV顺利完成任务的前提。捷联式惯性导航系统(SINS)由于结构简单、体积小、隐蔽性强、便于与其他设备集成化设计等优点,在水下定位导航与授时领域受到广泛关注，成为AUV自主导航定位的重要手段。SINS本质上是以牛顿第二定律为基础的积分推算系统，需要对惯性测量单元(IMU)的输出进行积分运算。因此，任何初始误差都会带入到积分运算系统使得误差不断累积，进而影响SINS长航时、远距离的导航定位精度。水下环境对全球导航卫星系统(GNSS)信号具有屏蔽作用，而多普勒计程仪(DVL)测速精度稳定，可为SINS提供实时外部速度辅助融合信息，SINS/DVL组合导航是当前水下导航的主流方案。
初始对准是SINS进行导航解算的前置步骤，高精度的初始对准技术是水下SINS/DVL组合系统长航时、远距离导航定位的重要保证。初始对准作为一种有效减小初始误差以提升导航精度的重要方法，一般分为粗对准和精对准两个阶段：粗对准将运载体的姿态快速地估计在一个较小的范围内，再通过精对准以获得更高的精度；精对准分为静基座精对准和行进间对准两种。静基座精对准要求运载体在起始阶段处于完全静止状态，先前的研究较多且技术已相对成熟。但由于风浪、水流等外部因素影响，水下初始对准难以满足静止条件，因此对水下行进间对准与姿态估计展开研究更具有现实意义。
在水下行进间对准与组合导航姿态估计过程中，外界环境复杂多变，水下生物、地形地质、水流变化等因素都会对DVL的输出速度产生影响，卡尔曼滤波建立的高斯无偏模型无法准确反映水下噪声。水下行进间对准面临的困难主要有：1)水下环境复杂导致高斯噪声模型不再适用；2)水下环境容易导致量测出现野值。
文献1[A.Sage and G.Husa,“Adaptive filtering with unknown priorstatistics,”Joint Automatic Control Conference,vol.7,pp.760-769,1969.]针对卡尔曼滤波中高斯噪声模型不准确的问题提出了Sage-Husa自适应卡尔曼滤波(Sage-Husaadaptive kalman filter，SHAKF)，但是利用SHAKF方法得到的噪声协方差阵的正定性和噪声模型的准确性难以保证，导致滤波效果变差甚至发散；文献2[B.Zhu and H.He,“Integrated navigation for doppler velocity log aided strapdown inertialnavigation 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 kalmanfilter，VBAKF)以应对不准确的噪声模型，但是此算法需要GPS信息且在受到野值影响时滤波性能下降，难以满足水下行进间对准的要求；文献4[X.Lyu,B.Hu,K.Li and L.Chang,“Anadaptive and robust UKF approach based on Gaussian process regression-aidedvariational Bayesian,”IEEE Sensors Journal,vol.21,no.7,pp.9500–9514,2021.]等将基于机器学习的高斯过程回归模型引入到变分贝叶斯自适应估计算法中，从而增强了系统的鲁棒性，但是算法对系统状态维数和样本数量有严格要求；文献5[G.Chang,“RobustKalman 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不能有效自适应噪声模型的变化，这就导致当噪声模型不准确时其性能效果不佳。
基于上述已有研究，现有相关技术的主要缺点是：不能同时满足行进间对准与姿态估计滤波算法的自适应性、鲁棒性与普适性，当存在不准确的噪声模型问题及野值污染问题时(这也是水下环境的面临的一大问题以及技术难点)滤波性能较差。
发明内容
为了综合解决水下组合导航行进间姿态估计中不准确的噪声模型问题以及野值污染问题，本发明提出一种水下组合导航系统行进间对准与姿态估计方法。该方法基于马氏距离的变分贝叶斯鲁棒自适应卡尔曼滤波器(variational bayesian robust adaptivekalman filter，VBRAKF)，来进行AUV的SINS/DVL组合导航系统行进间姿态估计。
在AUV水下组合导航系统进行行进间对准及姿态估计时，本发明先通过马氏距离方法判断观测信息是否被野值污染：如果判定DVL观测信息没有被污染，利用变分贝叶斯方法自适应观测噪声协方差矩阵；反之，如果DVL观测信息被污染，通过计算膨胀因子以衡量被污染程度，将膨胀因子引入变分贝叶斯方法自适应过程，自适应得到观测噪声协方差。最后通过VBRAKF状态估计得到高精度的水下运载体姿态信息以及导航信息。
本发明采用的技术方案为，一种水下组合导航系统行进间对准与姿态估计方法，该方法分为以下步骤：
S1：将自主式水下航行器(AUV)的捷联惯性导航系统(SINS)与多普勒测速仪(DVL)接入导航计算机，将AUV投放至任务区域。当AUV潜入水下，无法接收到GNSS信号时，通过传统方法进行姿态粗对准；
S2：通过导航计算机接收到捷联惯性导航系统中陀螺与加速度计输出的角速度与加速度增量，利用导航计算机进行导航解算：
定义坐标系：选取“东—北—天(E-N-U)”地理坐标系为导航坐标系，记为n系；选取“右—前—上”坐标系为载体坐标系，记为b系；地心惯性坐标系记为i系；地球坐标系记为e系；计算导航坐标系记为n′系。
S2.1执行导航计算机计时器，初始化时刻k＝0
S2.2计时器时间更新k＝k+1
S2.3执行惯性导航解算
通过陀螺输出的采样角增量和加速度计输出的采样比力速度增量解算得到k时刻的SINS姿态姿态矩阵/>速度/>和位置/>(具体解算过程可以参考文献[秦永元.惯性导航.科学出版社,2014.])。
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的表达式为：
的初始值设置为/>01×13表示1×13维的零矩阵，上标T表示矩阵转置；
式(2)中，位置误差为δP＝[δL,δλ]，δL和δλ分别为SINS的纬度误差与经度误差；速度误差δv＝[δvE,δvN]，δvE和δvN分别为东向速度误差和北向速度误差；姿态失准角 分别表示x轴、y轴和z轴方向的姿态失准角(下标x、y、z分别表示x轴、y轴和z轴方向的分量，以下相同)；陀螺漂移误差为/>加速度计零偏误差为
式(1)中，Φk|k-1为从k-1时刻到k时刻的状态一步预测矩阵，计算公式为：
Φk|k-1≈I+F(k-1)Ts (3)
式(3)中，I为13维单位矩阵，Ts为离散化时间间隔(滤波间隔)，F(k-1)为k-1时刻的SINS系统模型矩阵；F(k-1)的表达式为：
其中0i×j为i×j维的零矩阵，Fij为子矩阵，各子矩阵的表达式为：
式(5)-式(14)中，Re为地球半径，vE(k-1)为k-1时刻SINS的东向速度，vN(k-1)为k-1时刻SINS的北向速度，L(k-1)为k-1时刻SINS的纬度，ωie为地球自转角速度，fU(k-1)表示k-1时刻加速度计输出的比力在天向的投影，fN(k-1)表示k-1时刻加速度计输出的比力在北向的投影，fE(k-1)表示k-1时刻加速度计输出的比力在东向的投影，表示k-1时刻导航计算机解算得到的姿态矩阵，/>表示姿态矩阵/>的前两行。
S3.1.2执行VBRAKF状态一步预测均方误差矩阵更新：
式(15)中，Pk-1|k-1称为k-1时刻的状态后验估计均方误差矩阵，Pk|k-1称为从k-1时刻到k时刻的状态一步预测均方误差矩阵，利用式(15)，状态后验估计均方误差矩阵Pk-1|k-1更新成为状态一步预测均方误差矩阵Pk|k-1；
Pk-1|k-1初始值P0|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)中，vn(k)为k时刻SINS真实的速度，δvn(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时刻的东向速度观测噪声VE(k)和北向速度观测噪声VN(k)组成，其表达式为：
设RE(k),RN(k)分别为k时刻的东向、北向速度观测噪声方差，满足如下条件：
E[·]表示求其期望值；以往滤波方法的研究中，当速度观测设备稳定工作时，普遍认为V(k)是高斯白噪声，此时RE(k),RN(k)恒定。但是在水下环境中，由于温度、盐度、密度以及外部环境干扰等的影响，速度观测噪声功率谱不再符合高斯分布，RE(k),RN(k)是时变的。观测噪声V(k)的协方差矩阵为：
观测噪声协方差矩阵R(0)的初始值为：
R(0)＝diag{(0.1m/s)2,(0.1m/s)2} (24)
S4：构建基于马氏距离的VBRAKF鲁棒自适应模块，进行VBRAKF状态估计更新；
S4.1将k时刻观测噪声协方差矩阵R(k)的先验分布选择为逆Wishart分布：
观测噪声协方差矩阵未知且时变，服从多元正态分布。在贝叶斯统计中,通常将逆Wishart分布作为多元正态分布协方差矩阵的共轭先验分布。这是本发明处理未知且时变的观测噪声协方差矩阵的方法，将观测噪声协方差矩阵R(k)的先验分布选择为逆Wishart分布，其表达式为：
p(R(k)|Z(1:(k-1)))＝IW(R(k)；uk|k-1,Uk|k-1) (25)
式(25)中，p(R(k)|Z1:k-1)为观测噪声协方差矩阵R(k)的先验分布，Z(1:(k-1))为[1,k-1]时间段内所有的观测量，p(R(k)|Z(1:(k-1)))具体表示在[1,k-1]内所有的观测量先验已知下，对k时刻观测噪声协方差矩阵R(k)的概率分布作估计；IW(R(k)；uk|k-1,Uk|k-1)称为R(k)的概率分布为逆Wishart分布，uk|k-1称为逆Wishart分布的自由度参数，Uk|k-1称为逆Wishart分布的逆尺度矩阵；
S4.2更新逆Wishart分布的自由度参数与逆尺度矩阵
通过k-1时刻输出的后验估计uk-1|k-1和Uk-1|k-1更新uk|k-1和Uk|k-1：
Uk|k-1＝ρUk-1|k-1 (26)
uk|k-1＝ρ(uk-1|k-1-m-1)+m+1 (27)
式(26)、(27)中ρ表示变分衰减因子。在实践应用中，ρ∈[0.9,1)，依据经验设定ρ＝1-2-4。m为速度观测量的维度，通常取m＝2。
其中，逆Wishart分布Uk-1|k-1的初值U0|0：
U0|0＝(u0|0-m-1)R(0) (28)
初值u0|0＝10；
S4.3构建基于马氏距离的鲁棒化模块，计算k时刻的新息e(k)的马氏距离，并计算膨胀因子ω(k)：
新息表示观测量Z(k)与状态一步预测量/>之间的差，膨胀因子ω(k)用来衡量观测量Z(k)被污染的程度。
S4.3.1计算新息e(k)马氏距离：
式(29)中M(k)为k时刻新息e(k)的马氏距离。马氏距离公式中间部分利用的是上一时刻估计得到的/>近似为k时刻的观测噪声矩阵R(k)，这种近似会牺牲少许算法的自适应能力，但是提升了算法的鲁棒性。
S4.3.2通过S4.3.1得到的马氏距离判断观测量Z(k)是否为野值，计算膨胀因子ω(k)以衡量观测量Z(k)被污染的程度；
具体判断方法为：
选择统计门限函数对于观测量Z(k)，若S4.3.1计算的马氏距离满足M(k)2≤η2，则Z(k)被标记为正常观测量，此时膨胀因子为ω(k)＝1；
如果马氏距离满足M(k)2＞η2，则Z(k)被标记为野值。此时，膨胀因子ω(k)＞1，通过牛顿迭代法求解膨胀因子ω(k)，公式为：
式(30)中，上标(j)表示牛顿迭代次数，牛顿迭代次数上限设置为100，当前时刻观测噪声协方差阵R(k)利用上一时刻估计值近似，即/>膨胀因子迭代的初值ω(k)(0)＝1。每次迭代后的均方误差矩阵
S4.4构建基于马氏距离的变分贝叶斯鲁棒自适应模块，通过变分贝叶斯方法进行固定点迭代，估计观测噪声协方差矩阵及状态向量；
利用式(1)、(15)、(26)、(27)得到的Pk|k-1，Uk|k-1，uk|k-1初始化固定点迭代：uk|k＝uk|k-1+1,i＝0
中上标(0)表示当前固定点迭代次数为0，第i次固定点迭代近似的参数表示形式为/>
变分贝叶斯固定点迭代估计的步骤为：
S4.4.1计算第i次迭代变分贝叶斯估计均方误差矩阵B(k)(i)，公式为：
S4.4.2计算第i+1次迭代逆尺度矩阵近似估计观测噪声协方差矩阵/>
S4.4.3计算第i+1次迭代的滤波增益K(k)(i+1)
S4.4.4计算第i+1次迭代近似的状态后验估计以及状态后验估计均方误差阵
S4.4.5循环执行S4.4.1-S4.4.4，当i＝N-1时，结束迭代；其中，N代表迭代次数；输出k时刻迭代计算完成的状态向量状态后验估计均方误差矩阵/>观测噪声协方差矩阵/>逆尺度矩阵/>
理论上，N取值越大迭代近似的信息越精确。但是N值过大会增加计算负担，影响系统的实时性。综合考虑滤波精度及计算速度，依据工程经验取N＝5。
输出的状态向量是通过VBRAKF得到的k时刻SINS解算导航信息包含的误差，包括k时刻AUV的姿态误差/>位置误差δP(k)，速度误差δv(k)，陀螺漂移误差εb(k)和加速度计零偏误差/>
S5：构建导航误差反馈补偿回路，得到k时刻AUV的姿态、位置、速度在任意时刻的修正值，并进入k+1时刻循环执行S2.2-S4.2，直到导航结束：
S5.1将S3.3.6得到的滤波估计误差值反馈补偿给S2得到导航解算结果：
S5.1.1将k时刻姿态误差转换为姿态误差矩阵/>
其中，简记三角函数 由S4.4得到。
S5.1.2将S5.1.1得到的姿态误差矩阵S4.3得到的速度误差δv(k)与位置误差δP(k)反馈修正给S2.3得到的SINS解算姿态/>位置/>与速度/>具体方法如下：
式(39)中，表示/>的转置矩阵；通过式(39)-(41),即得到了k时刻导航修正值，包括高精度的姿态/>速度/>和位置/>
S5.2进入下一时刻，循环执行S2.2-S5.2，得到k+1时刻的高精度姿态速度/>和位置/>直到导航结束。
本发明具有以下技术效果：
本发明针对水下组合导航系统在行进间对准及姿态估计中，观测噪声未知且时变、观测值易受野值污染时，卡尔曼滤波器性能大大下降的问题，提出了一种水下组合导航系统VBRAKF行进间对准及姿态估计方法。在VBRAKF更新过程中，通过计算新息的马氏距离并计算膨胀因子来增强算法的鲁棒能力，同时通过变分贝叶斯方法实时估计未知且时变的噪声协方差以增强算法的自适应能力。运用本发明方法进行组合导航，系统有着更高的导航精度，更强的可靠性，满足自主、精确水下长航时、长航程的导航定位能力。
附图说明
图1：基于VBRAKF的SINS/DVL组合导航系统结构框图
图2：VBRAKF实施流程
图3：本发明提供的试验船行进轨迹
图4：本发明提供的试验船采集的野值污染情况下的DVL输出
图5：本发明提供的试验船采集的无野值污染情况下的DVL输出
图6：I[0.5° 0.5° 0.5°]条件下的姿态估计俯仰角误差
图7：图6的局部放大图
图8：I[0.5° 0.5° 0.5°]条件下的姿态估计横滚角误差
图9：图8的局部放大图
图10：I[0.5° 0.5° 0.5°]条件下的姿态估计航向角误差
图11：图10的局部放大图
图12：Ⅱ[1° 1° 1°]条件下的姿态估计俯仰角误差
图13：图12的局部放大图
图14：Ⅱ[1° 1° 1°]条件下的姿态估计横滚角误差
图15：图14的局部放大图
图16：Ⅱ[1° 1° 1°]条件下的姿态估计俯仰角误差
图17：图16的局部放大图
图18：Ⅲ[2° 2° 2°]条件下的姿态估计俯仰角误差
图19：图18的局部放大图
图20：Ⅲ[2° 2° 2°]条件下的姿态估计横滚角误差
图21：图20的局部放大图
图22：Ⅲ[2° 2° 2°]条件下的姿态估计航向角误差
图23：图22的局部放大图
图24：俯仰角均方根误差
图25：图24的局部放大图
图26：横滚角均方根误差
图27：图26的局部放大图
图28：航向角均方根误差
图29：图28的局部放大图
具体实施方式
为了使本申请的技术方法和优点更加清晰，下面结合附图对本发明做进一步说明，此处描述的具体实施例仅用以解释本申请，并不用于限定本申请。
图1为本发明所述方法组合导航系统的结构框架；图2为本发明所示方法的实施流程，本发明所述方法包含以下步骤：
S1：将自主式水下航行器(AUV)的捷联惯性导航系统(SINS)与多普勒测速仪(DVL)接入导航计算机，将AUV投放至任务区域。当AUV潜入水下，无法接收到GNSS信号时，通过传统方法进行姿态粗对准；
S2：通过导航计算机接收到捷联惯性导航系统中陀螺与加速度计输出的角速度与加速度增量，利用导航计算机进行导航解算：
S3：构建13维离散化的基于马氏距离的VBRAKF滤波模型与观测模型；
S4：构建基于马氏距离的VBRAKF鲁棒自适应模块，进行VBRAKF状态估计更新；
S5：构建导航误差反馈补偿回路，得到k时刻AUV的姿态、位置、速度在任意时刻的修正值。并进入k+1时刻循环执行S2.2-S4.2，直到导航结束：
本实施例利用船载试验系统验证水下行进间对准与姿态估计算法。船载试验系统的DVL安装于船底，导航效果与自主水下航行器的效果一致。船载试验系统导航设备包括捷联惯导系统，DVL传感器和GNSS接收机，以GNSS/SINS组合导航结果作为参考基准，利用SINS/DVL组合导航验证行进间对准及姿态估计方法的效果。试验船行进轨迹如图3所示，船载试验设备的参数如表1所示。
表1 SINS与DVL的设备参数
为了验证本发明VBRAKF姿态估计方法的有效性，在航行过程中，选取了两段不同时间的600秒数据来进行行进间对准，两段数据的DVL输出分别如图4、图5所示。图4为船载试验系统中DVL的输出，可以看出在多个时刻出现幅值较大的野值。由前面分析可知由于水下环境的复杂性，野值是不可避免且不可预测的，因此水下行进间对准算法的鲁棒化是十分必要的。同时，由于复杂的水下环境，观测噪声协方差矩阵并不是固定的，为了验证所提算法的鲁棒自适应能力，选择另一段不含野值污染的DVL输出，如图5所示，通过人为添加混合高斯分布噪声的方式进行试验验证。
为比较本发明专提出的方法与现有技术的性能，证明其优越性。选取4种方案进行对比分析：三种现有方案KF、RKF、VBAKF以及本发明提出的VBRAKF行进间姿态估计方法。
首先验证水下组合导航系统行进间姿态估计方法的鲁棒性。图4的DVL输出数据段进行了行进间对准以及姿态估计，验证算法的鲁棒性。在实际中，船载试验系统因为一直在执行姿态估计方法，所以姿态是相对准确的。为了验证对准效果，在本次验证中，分别人为设置粗对准结束后水平失准角和方位失准角均为0.5°、1°、2°，即SINS的初始失准角分别为I[0.5° 0.5° 0.5°]、Ⅱ[1° 1° 1°]、Ⅲ[2° 2° 2°]，经过600秒的行进间对准，得到的对准结果如图6至图23所示。
从图6至图23可以看出，KF受观测野值影响明显，当野值出现时，KF估计结果发散。其他三种方法的水平姿态角(俯仰角和横滚角)的滤波精度与收敛速度效果均很理想，且明显好于航向角估计结果，这主要是因为在观测模型中，水平姿态角的可观测性强于航向角。对于航向角而言，当野值出现时，VBAKF虽然可以通过自适应观测噪声协方差阵在一定程度上抑制其影响，但仍有发散趋势，RKF和VBRAKF则具有良好的鲁棒性。计算了不同初始条件下各个方法在对准最后100秒估计误差绝对值的平均值，结果如表2所示。
表2对准估计误差对比
从表2可以看出，KF的姿态估计结果发散，失去了姿态估计的意义。VBRAKF的估计效果明显优于VBAKF，且略优于RKF。试验结果表明了VBRAKF在野值干扰环境下的鲁棒能力。
以下进行水下组合导航系统行进间姿态估计方法的鲁棒自适应性能力评估。在水下环境中，观测量往往会受到各种类型的非高斯噪声污染。评估采用混合高斯分布噪声来模拟非高斯噪声污染。
利用图5的数据进行本发明方法的验证，设置粗对准结束后水平失准角和方位失准角均为1°，即SINS的初始失准角为[1° 1° 1°]。
利用不同姿态估计方法进行50次重复试验的结果如表3所示。
表3姿态估计误差对比
通过图24至图29可以看出，由于水平姿态角(俯仰角和横滚角)的可观测性更强，所以四种滤波方法的滤波结果都在0.1°以下，但是KF的滤波精度和稳定性要明显差于其他方法。对于方位姿态角(航向角)的估计结果，由于受到非高斯噪声的影响，KF的效果极差，已无法满足下步组合导航的要求；RKF虽然能在一定程度上抑制非高斯噪声的污染，但是其效果显然不如VBAKF和VBRAKF。表3反映了对准最后100秒的平均RMSE结果，对于航向角而言，VBRAKF算法的估计结果比KF算法提高了95.61％，比RKF提高了62.22％，比VBAKF提高了14.72％。试验结果验证了VBRAKF行进间对准及姿态估计方法在非高斯环境下具有鲁棒自适应能力。
Claims (6)
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的表达式为：
的初始值设置为/>01×13表示1×13维的零矩阵，上标T表示矩阵转置；
式(2)中，位置误差为δP＝[δL,δλ]，δL和δλ分别为SINS的纬度误差与经度误差；速度误差δv＝[δvE,δvN]，δvE和δvN分别为东向速度误差和北向速度误差；姿态失准角 分别表示x轴、y轴和z轴方向的姿态失准角；陀螺漂移误差为加速度计零偏误差为/>
式(1)中，Φk|k-1为从k-1时刻到k时刻的状态一步预测矩阵，计算公式为：
Φk|k-1≈I+F(k-1)Ts (3)
式(3)中，I为13维单位矩阵，Ts为离散化时间间隔，F(k-1)为k-1时刻的SINS系统模型矩阵；F(k-1)的表达式为：
其中0i×j为i×j维的零矩阵，Fij为子矩阵，各子矩阵的表达式为：
式(5)-式(14)中，Re为地球半径，vE(k-1)为k-1时刻SINS的东向速度，vN(k-1)为k-1时刻SINS的北向速度，L(k-1)为k-1时刻SINS的纬度，ωie为地球自转角速度，fU(k-1)表示k-1时刻加速度计输出的比力在天向的投影，fN(k-1)表示k-1时刻加速度计输出的比力在北向的投影，fE(k-1)表示k-1时刻加速度计输出的比力在东向的投影，表示k-1时刻导航计算机解算得到的姿态矩阵，/>表示姿态矩阵/>的前两行；
S3.1.2执行VBRAKF状态一步预测均方误差矩阵更新：
式(15)中，Pk-1|k-1称为k-1时刻的状态后验估计均方误差矩阵，Pk|k-1称为从k-1时刻到k时刻的状态一步预测均方误差矩阵，利用式(15)，状态后验估计均方误差矩阵Pk-1|k-1更新成为状态一步预测均方误差矩阵Pkk-1；
Pk-1|k-1初始值P0|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)中，vn(k)为k时刻SINS真实的速度，δvn(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时刻的东向速度观测噪声VE(k)和北向速度观测噪声VN(k)组成，其表达式为：
设RE(k),RN(k)分别为k时刻的东向、北向速度观测噪声方差，满足如下条件：
E[·]表示求其期望值；观测噪声V(k)的协方差矩阵为：
观测噪声协方差矩阵R(0)的初始值为：
R(0)＝diag{(0.1m/s)2,(0.1m/s)2} (24)
S4：构建基于马氏距离的VBRAKF鲁棒自适应模块，进行VBRAKF状态估计更新：
S4.1将k时刻观测噪声协方差矩阵R(k)的先验分布选择为逆Wishart分布：
将观测噪声协方差矩阵R(k)的先验分布选择为逆Wishart分布，其表达式为：
p(R(k)|Z(1:(k-1)))＝IW(R(k)；uk|k-1,Uk|k-1) (25)
式(25)中，p(R(k)|Z1:k-1)为观测噪声协方差矩阵R(k)的先验分布，Z(1:(k-1))为[1,k-1]时间段内所有的观测量，p(R(k)|Z(1:(k-1)))具体表示在[1,k-1]内所有的观测量先验已知下，对k时刻观测噪声协方差矩阵R(k)的概率分布作估计；IW(R(k)；uk|k-1,Uk|k-1)称为R(k)的概率分布为逆Wishart分布，uk|k-1称为逆Wishart分布的自由度参数，Uk|k-1称为逆Wishart分布的逆尺度矩阵；
S4.2更新逆Wishart分布的自由度参数与逆尺度矩阵
通过k-1时刻输出的后验估计uk-1|k-1和Uk-1|k-1更新uk|k-1和Uk|k-1：
Uk|k-1＝ρUk-1|k-1 (26)
uk|k-1＝ρ(uk-1|k-1-m-1)+m+1 (27)
式(26)、(27)中ρ表示变分衰减因子；
其中，逆Wishart分布Uk-1|k-1的初值U0|0：
U0|0＝(u0|0-m-1)R(0) (28)
初值u0|0＝10；
S4.3构建基于马氏距离的鲁棒化模块，计算k时刻的新息e(k)的马氏距离，并计算膨胀因子ω(k)；新息表示观测量Z(k)与状态一步预测量/>之间的差，膨胀因子ω(k)用来衡量观测量Z(k)被污染的程度：
S4.3.1计算新息e(k)马氏距离：
式(29)中M(k)为k时刻新息e(k)的马氏距离，马氏距离公式中间部分利用的是上一时刻估计得到的/>近似为k时刻的观测噪声矩阵R(k)；
S4.3.2通过S4.3.1得到的马氏距离判断观测量Z(k)是否为野值，计算膨胀因子ω(k)以衡量观测量Z(k)被污染的程度；
具体判断方法为：
选择统计门限函数对于观测量Z(k)，若S4.3.1计算的马氏距离满足M(k)2≤η2，则Z(k)被标记为正常观测量，此时膨胀因子为ω(k)＝1；
如果马氏距离满足M(k)2＞η2，则Z(k)被标记为野值；此时，膨胀因子ω(k)＞1，通过牛顿迭代法求解膨胀因子ω(k)，公式为：
式(30)中，上标(j)表示牛顿迭代次数，当前时刻观测噪声协方差阵R(k)利用上一时刻估计值近似，即/>膨胀因子迭代的初值ω(k)(0)＝1；每次迭代后的均方误差矩阵：
S4.4构建基于马氏距离的变分贝叶斯鲁棒自适应模块，通过变分贝叶斯方法进行固定点迭代，估计观测噪声协方差矩阵及状态向量：
利用式(1)、(15)、(26)、(27)得到的Pk|k-1，Uk|k-1，uk|k-1初始化固定点迭代：
中上标(0)表示当前固定点迭代次数为0，第i次固定点迭代近似的参数表示形式为/>
变分贝叶斯固定点迭代估计的步骤为：
S4.4.1计算第i次迭代变分贝叶斯估计均方误差矩阵B(k)(i)，公式为：
S4.4.2计算第i+1次迭代逆尺度矩阵近似估计观测噪声协方差矩阵/>
S4.4.3计算第i+1次迭代的滤波增益K(k)(i+1)
S4.4.4计算第i+1次迭代近似的状态后验估计以及状态后验估计均方误差阵/>
S4.4.5循环执行S4.4.1-S4.4.4，当i＝N-1时，结束迭代；其中，N代表迭代次数；输出k时刻迭代计算完成的状态向量状态后验估计均方误差矩阵/>观测噪声协方差矩阵/>逆尺度矩阵/>
输出的状态向量是通过VBRAKF得到的k时刻SINS解算导航信息包含的误差，包括k时刻AUV的姿态误差/>位置误差δP(k)，速度误差δv(k)，陀螺漂移误差εb(k)和加速度计零偏误差/>
S5：构建导航误差反馈补偿回路，得到k时刻AUV的姿态、位置、速度在任意时刻的修正值，并进入k+1时刻循环执行S2.2-S4.2，直到导航结束：
S5.1将S3.3.6得到的滤波估计误差值反馈补偿给S2得到导航解算结果：
S5.1.1将k时刻姿态误差转换为姿态误差矩阵/>
其中，简记三角函数 由S4.4得到；
S5.1.2将S5.1.1得到的姿态误差矩阵S4.3得到的速度误差δv(k)与位置误差δP(k)反馈修正给S2.3得到的SINS解算姿态/>位置/>与速度/>具体方法如下：
式(39)中，表示/>的转置矩阵；通过式(39)-(41),即得到了k时刻导航修正值，包括高精度的姿态/>速度/>和位置/>
S5.2进入下一时刻，循环执行S2.2-S5.2，得到k+1时刻的高精度姿态速度和位置/>直到导航结束。
2.一种根据权利要求1所述水下组合导航系统行进间对准与姿态估计方法，其特征在于：S4.2中，变分衰减因子ρ∈[0.9,1)。
3.一种根据权利要求2所述水下组合导航系统行进间对准与姿态估计方法，其特征在于：S4.2中，变分衰减因子ρ＝1-2-4。
4.一种根据权利要求1所述水下组合导航系统行进间对准与姿态估计方法，其特征在于：S4.2中，速度观测量的维度为m＝2。
5.一种根据权利要求1所述水下组合导航系统行进间对准与姿态估计方法，其特征在于：S4.3.2中，牛顿迭代次数上限设置为100。
6.一种根据权利要求1所述水下组合导航系统行进间对准与姿态估计方法，其特征在于：S4.4.5中，综合考虑滤波精度及计算速度，迭代次数取N＝5。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210415114.7A CN114777812B (zh) | 2022-04-17 | 2022-04-17 | 一种水下组合导航系统行进间对准与姿态估计方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210415114.7A CN114777812B (zh) | 2022-04-17 | 2022-04-17 | 一种水下组合导航系统行进间对准与姿态估计方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114777812A CN114777812A (zh) | 2022-07-22 |
| CN114777812B true CN114777812B (zh) | 2024-04-05 |
Family
ID=82431661
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210415114.7A Active CN114777812B (zh) | 2022-04-17 | 2022-04-17 | 一种水下组合导航系统行进间对准与姿态估计方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114777812B (zh) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116222582B (zh) * | 2023-05-10 | 2023-07-25 | 北京航空航天大学 | 一种基于变分贝叶斯推断多物理场自适应组合导航方法 |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105091907A (zh) * | 2015-07-28 | 2015-11-25 | 东南大学 | Sins/dvl组合中dvl方位安装误差估计方法 |
| CN109443379A (zh) * | 2018-09-28 | 2019-03-08 | 东南大学 | 一种深海潜航器的sins/dvl水下抗晃动对准方法 |
| CN109724599A (zh) * | 2019-03-12 | 2019-05-07 | 哈尔滨工程大学 | 一种抗野值的鲁棒卡尔曼滤波sins/dvl组合导航方法 |
| CN113218421A (zh) * | 2021-05-11 | 2021-08-06 | 中国人民解放军63921部队 | 北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法 |
-
2022
- 2022-04-17 CN CN202210415114.7A patent/CN114777812B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105091907A (zh) * | 2015-07-28 | 2015-11-25 | 东南大学 | Sins/dvl组合中dvl方位安装误差估计方法 |
| CN109443379A (zh) * | 2018-09-28 | 2019-03-08 | 东南大学 | 一种深海潜航器的sins/dvl水下抗晃动对准方法 |
| WO2020062791A1 (zh) * | 2018-09-28 | 2020-04-02 | 东南大学 | 一种深海潜航器的sins/dvl水下抗晃动对准方法 |
| CN109724599A (zh) * | 2019-03-12 | 2019-05-07 | 哈尔滨工程大学 | 一种抗野值的鲁棒卡尔曼滤波sins/dvl组合导航方法 |
| CN113218421A (zh) * | 2021-05-11 | 2021-08-06 | 中国人民解放军63921部队 | 北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法 |
Non-Patent Citations (3)
| Title |
|---|
| Fault-Tolerant Sins/Hsb/Dvl Underwater Integrated Navigation and Position System Based on Variational Bayesian Robust Adaptive Kalman Filter and Adaptive Information Sharing Factor;Shi, Wence等;《SSRN》;20220322;全文 * |
| SINS/DVL/AST水下组合导航中的鲁棒信息融合方法;朱兵;常国宾;何泓洋;许江宁;;国防科技大学学报;20201027(05);全文 * |
| 基于T分布变分贝叶斯滤波的SINS/GPS组合导航;胡淼淼;敬忠良;董鹏;周贵荣;郑智明;;浙江大学学报(工学版);20180823(08);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114777812A (zh) | 2022-07-22 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109211276B (zh) | 基于gpr与改进的srckf的sins初始对准方法 | |
| CN108226980B (zh) | 基于惯性测量单元的差分gnss与ins自适应紧耦合导航方法 | |
| CN109724599B (zh) | 一种抗野值的鲁棒卡尔曼滤波sins/dvl组合导航方法 | |
| CN107525503B (zh) | 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法 | |
| Jiancheng et al. | Study on innovation adaptive EKF for in-flight alignment of airborne POS | |
| CN103913181B (zh) | 一种基于参数辨识的机载分布式pos传递对准方法 | |
| CN109945895B (zh) | 基于渐消平滑变结构滤波的惯性导航初始对准方法 | |
| CN102353378B (zh) | 一种矢量形式信息分配系数的组合导航系统自适应联邦滤波方法 | |
| CN110567455B (zh) | 一种求积更新容积卡尔曼滤波的紧组合导航方法 | |
| CN103822633A (zh) | 一种基于二阶量测更新的低成本姿态估计方法 | |
| Gong et al. | A modified nonlinear two-filter smoothing for high-precision airborne integrated GPS and inertial navigation | |
| CN111380518B (zh) | 一种引入径向速度的sins/usbl紧组合导航定位方法 | |
| CN112146655B (zh) | 一种BeiDou/SINS紧组合导航系统弹性模型设计方法 | |
| Ali et al. | Performance comparison among some nonlinear filters for a low cost SINS/GPS integrated solution | |
| CN109507706B (zh) | 一种gps信号丢失的预测定位方法 | |
| Xue et al. | In-motion alignment algorithm for vehicle carried SINS based on odometer aiding | |
| CN103776449A (zh) | 一种提高鲁棒性的动基座初始对准方法 | |
| CN103674064A (zh) | 捷联惯性导航系统的初始标定方法 | |
| CN108303120B (zh) | 一种机载分布式pos的实时传递对准的方法及装置 | |
| CN110702110A (zh) | 一种基于无迹卡尔曼滤波的舰船升沉运动测量方法 | |
| CN114777812B (zh) | 一种水下组合导航系统行进间对准与姿态估计方法 | |
| Zorina et al. | Enhancement of INS/GNSS integration capabilities for aviation-related applications | |
| CN109974695A (zh) | 基于Krein空间的水面舰艇导航系统的鲁棒自适应滤波方法 | |
| Shi et al. | Attitude estimation of SINS on underwater dynamic base with variational Bayesian robust adaptive Kalman filter | |
| Gong et al. | Airborne earth observation positioning and orientation by SINS/GPS integration using CD RTS smoothing |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |