CN115096302A - 捷联惯性基导航系统信息滤波鲁棒对准方法、系统及终端 - Google Patents

Abstract

本发明属于SINS初始对准技术领域，公开了一种捷联惯性基导航系统信息滤波鲁棒对准方法、系统及终端，进行捷联惯性基导航系统初始对准模型的构建；进行信息卡尔曼滤波算法的时间更新和量测更新；计算观测量与观测新息之间的马氏距离，计算观测新息对应权值；基于马氏距离算法，引入膨胀因子λ_k，对估计出的R阵进行修正；利用修正后的R阵进行IKF算法量测更新即为鲁棒IKF的量测更新。本发明提出的RIKF方法不仅适用于水下载体系速度辅助惯性基导航系统初始对准中，且还将适用于全球卫星导航系统信号可用性差或遮蔽条件下，载体系速度辅助惯性基导航系统初始对准，并可进一步推广应用至组合导航领域。

Description

捷联惯性基导航系统信息滤波鲁棒对准方法、系统及终端

技术领域

本发明属于捷联式惯性导航系统(Strapdown Inertial Navigation System，SINS)初始对准技术领域，尤其涉及一种捷联惯性基导航系统信息滤波鲁棒对准方法、系统及终端。

背景技术

目前，捷联式惯性导航系统(Strapdown Inertial Navigation System，SINS) 由于其自主性强、隐蔽性高、结构简单、体积小及成本低等优点，成为自主式水下航行器(Autonomous Underwater Vehicle，AUV)长航时、长航程、高精度导航的主要手段。SINS在导航过程中不向外界发射电磁波或其他信号，不易受外界干扰，它具有隐蔽性高、抗干扰能力强的独特优势。因此，SINS已经成为 AUV自主式导航的主流装置。目前来看，AUV尚未具备真正意义上的自主式远程航行能力，其主要原因是导航的精度问题，也就是自主惯性导航系统(Inertial Navigation System，INS)(简称“惯导系统”或“惯导”)的漂移误差积累问题。目前乃至很长一段时间，全航程仍需要天基定位导航系统对INS进行定期校准，而高频无线电信号随水深衰减极快，校准过程中需要AUV上浮至水面或接近水面位置，增大了自身暴露的概率，限制了其长航时隐蔽航行的能力。

初始对准是SINS进行导航解算之前的必经阶段，初始对准的精度对SINS 解算精度有着至关重要的影响，同时运载体的快速反应能力在很大程度上取决于对准的时间。水下SINS初始对准需要外部导航信息进行辅助，目前常用的水下辅助导航手段有：声学定位及海洋物理场匹配导航等。海洋物理场匹配现阶段仍然处于理论研究阶段，海洋物理场的建设还不能满足实际应用的需求。水下声学定位系统的布设成本高，尤其是在远海、深海布设的难度更加大。多普勒计程仪(Doppler velocity log，DVL)利用声脉冲发射信号与散射回波信号之间的多普勒频移信息，得到运载体相对水层或水底的速度，其测速误差不会随时间累积，AUV在大深度、广水域航行过程中，DVL可为SINS提供可靠的外速度辅助信息，SINS与DVL组成的惯性基导航系统可实现全天候、完全自主式的导航。在水下动基座初始对准中，外界环境是复杂多变的，DVL测速容易受到非高斯噪声污染，造成基于标准卡尔曼滤波(Kalman Filter，KF)框架的初始对准方法性能变差甚至发散。

信息卡尔曼滤波(Information Kalman Filter，IKF)是标准KF的一个变种。标准KF利用状态误差协方差矩阵来表示高斯分布，IKF则是采用信息矩阵(状态误差协方差矩阵的逆)来表示高斯分布，这两种表示形式互为对偶。相比于 KF，IKF的优势在于不需要准确已知状态量的初始信息，量测更新效率更高。由于IKF也是基于KF框架下的滤波方法，在实际应用中也需对量测噪声作高斯分布假设。因此，当观测量受到野值等非高斯噪声污染时，IKF滤波性能将会降低甚至发散。

通过上述分析，现有技术存在的问题及缺陷为：

(1)现有水下辅助导航技术中，海洋物理场匹配现阶段仍然处于理论研究阶段，海洋物理场的建设还不能满足实际应用的需求。

(2)现有的水下辅助导航技术中，水下声学定位系统的布设成本高，尤其是针对远海、深海的布设难度更加大。

(3)在水下动基座初始对准中，状态量的先验信息并不是始终准确已知，且外界环境复杂多变，DVL测速易受到非高斯噪声污染，造成基于KF框架的初始对准方法性能变差甚至发散。

发明内容

针对现有技术存在的问题，本发明提供了一种捷联惯性基导航系统信息滤波鲁棒对准方法、系统及终端，尤其涉及一种载体系速度辅助捷联惯性基导航系统信息滤波鲁棒对准方法、系统、介质、设备及终端。

本发明是这样实现的，一种捷联惯性基导航系统信息滤波(IKF)鲁棒对准方法，所述捷联惯性基导航系统信息滤波鲁棒对准方法包括：

基于马氏距离算法，引入膨胀因子λ，当观测量受到非高斯噪声污染时，对量测噪声协方差阵R进行膨胀得到

并利用

替代R进行IKF量测更新，得到鲁棒IKF。

进一步，所述捷联惯性基导航系统信息滤波鲁棒对准方法包括以下步骤：

步骤一，进行捷联惯性基导航系统初始对准模型的构建；

步骤二，进行IKF算法的时间更新；

步骤三，计算观测量与观测新息之间的马氏距离，计算观测新息对应权值；

步骤四，基于马氏距离算法，引入膨胀因子λ_k，对量测噪声协方差阵R阵进行修正；

步骤五，进行鲁棒IKF(Robust IKF，RIKF)量测更新：利用修正后的R阵进行IKF算法量测更新；

步骤六，利用RIKF估计出状态误差对SINS导航解算误差进行修正。

进一步，所述步骤一中的捷联惯性基导航系统初始对准模型的构建包括：

(1)初始对准状态方程

选取的状态量不考虑高度通道的速度和位置信息，则选取状态量为：

式中，δL、δζ分别为纬度误差、经度误差；δv_E、δv_N分别为东向速度误差、北向速度误差；α＝[α_x；α_y；α_z]为欧拉平台误差角；

为陀螺仪常值漂移；

为加速度计零偏。

基于SINS/DVL组合的捷联惯性基导航系统初始对准对应的状态方程为：

式中，w_SINS～N(0,Q_SINS)为系统噪声，Q_SINS为系统噪声协方差阵。由状态方程建立的状态转移矩阵F_SINS为：

式中，F为7×7矩阵，F中的非零元素如下：

F_1,4＝1/R_e，F_2,1＝(v_E/R_e)tanLsecL，F_2,3＝secL/R_e，F_3,3＝(v_N/R_e)tanL；

F_3,4＝2ω_iesinL+(v_E/R_e)tanL，F_3,6＝-f_U，F_3,7＝f_N；

F_4,3＝-[2ω_iesinL+(v_E/R_e)tanL]，F_4,5＝f_U；

F_4,7＝-f_E，F_5,4＝-1/R_e，F_5,6＝ω_iesinL+(v_E/R_e)tanL，F_5,7＝-[ω_iecosL+(v_E/R_e)]；

F_6,1＝-ω_iesinL，F_6,3＝1/R_e，F_6,5＝-[ω_iesinL+(v_E/R_e)tanL]，F_6,7＝-v_N/R_e；

F_7,1＝ω_iecosL+(v_E/R_e)(secL)²，F_7,3＝tanL/R_e，F_7,5＝ω_iecosL+v_E/R_e；

F_7,6＝v_N/R_e；

式中，

G为7×6矩阵，表达形式如下：

式中，

表示姿态矩阵

的前两行。

(2)初始对准量测方程

DVL与SINS组合，选取速度误差作为观测量，选取SINS测得的导航系下速度

与DVL测得的载体系下速度v^b在n系上投影之差作为观测量，则基于 SINS/DVL组合的捷联惯性基导航系统初始对准对应的量测方程为：

式中，H_v为量测矩阵，量测噪声V_v～N(0,R_v)，R_v为量测噪声阵；在线性小失准角条件下，

若选取东向速度误差δv_E和北向速度误差δv_N作为观测量，则有：

在实际应用中，用

代替

得量测矩阵H_v为：

式中，

表示矩阵

的前两行。

进一步，所述步骤二中的IKF算法的时间更新包括：

；

式中，F_k-1为k-1时刻状态转移矩阵，H_k为k时刻量测噪声阵，

为状态量的先验估计，P_k|k-1为状态误差协方差阵的先验估计，IP＝P^-1为信息矩阵。

进一步，所述步骤三中的计算观测量与观测新息之间的马氏距离，计算观测新息对应权值包括：

选择k时刻的观测量

与观测量的先验估计

之间的马氏距离作为评判指标，则k时刻评判指标θ_k的定义如下式所示：

式中，

为马氏距离；

为量测误差协方差阵的先验估计；μ_k为观测新息向量；对于真实的观测量

若评判指标θ_k满足

则

将被标记为正常观测量；反之，若评判指标θ_k满足

则

将被标记为异常观测量。

进一步，所述步骤四中的基于马氏距离算法，引入膨胀因子λ_k，对R阵进行修正包括：

通过引入膨胀因子λ_k用于膨胀量测噪声协方差阵R_k：

将噪声协方差阵表达式带入k时刻评判指标θ_k的表达式，得：

将新的k时刻评判指标θ_k的表达式转化为求解λ_k的非线性问题，则：

式中，λ_k通过牛顿迭代法求解，则λ_k(i+1)与λ_k(i)的关系表示为：

式中，

且λ_k(i)初始值为λ_k(0)＝1；当评判指标满足

时，迭代终止。

所述步骤五中的利用修正后的R阵进行IKF算法量测更新包括：

将概率参数α设置为0.99，鲁棒滤波方法的效率为99％，2自由度卡方分布值为

利用

代替R_k进行标准IKF滤波得到RIKF算法，k时刻RIKF 算法量测更新方程如下：

式中，k＝1，2，3...。

本发明的另一目的在于提供一种应用所述的捷联惯性基导航系统信息滤波鲁棒对准方法的捷联惯性基导航系统信息滤波鲁棒对准系统，所述捷联惯性基导航系统信息滤波鲁棒对准系统包括：

初始对准模型构建模块，用于进行捷联惯性基导航系统初始对准模型构建；

时间量测更新模块，用于进行IKF算法的时间更新；

马氏距离计算模块，用于计算观测量与观测新息之间的马氏距离，计算观测新息对应权值；

R阵修正模块，用于基于马氏距离算法，引入膨胀因子λ_k，对R阵进行修正；

RIKF量测更新模块，用于利用修正后的R阵进行IKF算法量测更新；

误差修正模块，用于利用RIKF估计出状态误差对SINS导航解算误差进行修正。

本发明的另一目的在于提供一种计算机设备，所述计算机设备包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行如下步骤：

基于马氏距离算法，引入膨胀因子λ，当观测量受到非高斯噪声污染时，对量测噪声协方差阵R进行膨胀得到

并利用

替代R进行IKF量测更新，得到鲁棒IKF(Robust IKF，RIKF)。

本发明的另一目的在于提供一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，使得所述处理器执行如下步骤：

基于马氏距离算法，引入膨胀因子λ，当观测量受到非高斯噪声污染时，对量测噪声协方差阵R进行膨胀得到

并利用

替代R进行IKF量测更新，得到鲁棒IKF(RIKF)。

本发明的另一目的在于提供一种信息数据处理终端，所述信息数据处理终端用于实现所述的捷联惯性基导航系统信息滤波鲁棒对准系统。

结合上述的技术方案和解决的技术问题，请从以下几方面分析本发明所要保护的技术方案所具备的优点及积极效果为：

第一、针对上述现有技术存在的技术问题以及解决该问题的难度，紧密结合本发明的所要保护的技术方案以及研发过程中结果和数据等，详细、深刻地分析本发明技术方案如何解决的技术问题，解决问题之后带来的一些具备创造性的技术效果。具体描述如下：

本发明针对现有物理场匹配辅助导航手段不成熟和声学导航手段布设较为复杂，成本较高等问题，利用多普勒计程仪(DVL)提供测速信息辅助SINS进行初始对准或组合导航，SINS和DVL组成的捷联惯性基导航系统可实现自主式水下航行器(AUV)全天候、全水域的初始对准或组合导航。但是由于水下环境的复杂性(水流速、水底地形的变化等因素)，DVL测速信息容易受到野值等非高斯噪声的污染，同时状态量的先验信息并不始终是准确已知的。本发明基于马氏距离(Mahalanobis Distance，MD)算法引入膨胀因子λ，当观测量受到非高斯噪声污染时，对量测噪声协方差阵R进行膨胀得到

并利用

替代R进行IKF量测更新，即得到鲁棒IKF(RIKF)。

本发明提出的RIKF方法不仅适用于水下载体系速度辅助惯性基导航系统初始对准中，而且还将适用于全球卫星导航系统(Global satellite navigation system，GNSS)信号可用性差或遮蔽条件下，载体系速度辅助惯性基导航系统初始对准，并可进一步推广应用至组合导航领域。

第二，把技术方案看做一个整体或者从产品的角度，本发明所要保护的技术方案具备的技术效果和优点，具体描述如下：

根据试验结果，在水下非高斯环境中，相比于KF和IKF，本发明提供的 RIKF可在确保水下动态对准鲁棒性的同时，有效提高对准精度，还能够有效克服初始对准过程中状态量先验信息不可知及观测量受不同类型非高斯噪声污染时滤波性能下降的问题。

第三，作为本发明的权利要求的创造性辅助证据，还体现在以下几个重要方面：

(1)本发明的技术方案解决了人们一直渴望解决、但始终未能获得成功的技术难题：对于水面以上有GNSS信号辅助下的初始对准，由于有导航坐标系 (n系)下的位置和速度作为观测信息，其研究相对成熟。在水下AUV动态初始对准中，外界环境是复杂多变的，如水底地形复杂变化以及水流速的变化等均会影响DVL对速度的测量，使得量测噪声容易受到非高斯噪声的污染，从而量测噪声统计特性具有非高斯的特点。这会导致基于标准卡尔曼滤波框架的IKF 滤波性能变差甚至发散。为了在水下非高斯环境中进一步精确地获取SINS的姿态和航向信息，本发明在无GNSS辅助条件下提出基于鲁棒IKF的水下动态对准方法。发明方法不仅可以有效克服外部复杂环境引起的非高斯有色噪声，而且还可以在状态量先验信息未知或不准确情况下实现较高精度初始对准，有效克服了在工程应用中，利用卡尔曼滤波实现初始对准需要假设噪声特性符合高斯分布特性以及需要获取较为准确的状态误差协方差阵P初始值P₀的技术难题。

(2)本发明的技术方案克服了技术偏见：信息卡尔曼滤波(IKF)由于其表示全局不确定性更加简单的独特优势，被广泛应用于目标跟踪、协同导航、 SLAM、精密测量等领域，发明人通过中国知网进行检索，并未查询到将IKF 应用于惯性基导航系统初始对准中。究其原因，一方面由于初始对准对速度要求较高，即要求初始对准时间尽可能短(精对准一般要求600～900s内完成)，IKF 在进行滤波更新时，需要对状态误差协方差阵P进行求逆，降低了滤波时间更新过程中预测的效率；另一方面，在初始对准过程中，一般P的初始值选取较为依赖经验值。但在实用中，状态量的先验信息并不是始终准确已知，如粗对准得到的初始姿态角并不一定能够满足基于KF滤波的小失准角要求，此时若选取经验值将会导致滤波性能降低，甚至发散。IKF并不需要准确已知P的初始值， IKF允许将P的初始值设置为无穷大，这样初始对准将不再受限于准确已知P的初始值。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对本发明实施例中所需要使用的附图做简单的介绍，显而易见地，下面所描述的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下还可以根据这些附图获得其他的附图。

图1是本发明实施例提供的捷联惯性基导航系统信息滤波鲁棒对准方法的流程图；

图2是本发明实施例提供的DVL输出示意图；其中：

图2(a)是本发明实施例提供的DVL输出(高斯噪声污染)示意图；

图2(b)是本发明实施例提供的DVL输出(野值污染)示意图；

图3是本发明实施例提供的不同方法姿态角估计误差的RMSE(50次Monte Carlo试验)示意图；其中：

图3(a)是本发明实施例提供的不同方法俯仰角估计误差的RMSE示意图；

图3(b)是本发明实施例提供的不同方法横滚角估计误差的RMSE示意图；

图3(c)是本发明实施例提供的不同方法航向角估计误差的RMSE示意图；

图4是本发明实施例提供的不同方法姿态角估计误差的TRMSE(最后100s) 示意图；其中：

图4(a)是本发明实施例提供的不同方法俯仰角估计误差的TRMSE示意图；

图4(b)是本发明实施例提供的不同方法俯仰角估计误差的TRMSE示意图；

图4(c)是本发明实施例提供的不同方法航向角估计误差的TRMSE示意图；

图5是本发明实施例提供的不同方法姿态角估计误差的TRMSE(最后100s) 示意图；其中：

图5(a)是本发明实施例提供的不同方法俯仰角估计误差的TRMSE示意图；

图5(b)是本发明实施例提供的不同方法横滚角估计误差的TRMSE示意图；

图5(c)是本发明实施例提供的不同方法航向角估计误差的TRMSE示意图；

图6是本发明实施例提供的不同方法姿态角估计误差示意图；其中：

图6(a)是本发明实施例提供的不同方法的俯仰角对准误差示意图；

图6(b)是本发明实施例提供的不同方法的横滚角对准误差示意图；

图6(c)是本发明实施例提供的不同方法的航向角对准误差示意图；

图7是本发明实施例提供的捷联惯性基导航系统信息滤波鲁棒对准系统的结构框图；

图中：1、初始对准模型构建模块；2、时间量测更新模块；3、马氏距离计算模块；4、R阵修正模块；5、RIKF量测更新模块；6、误差修正模块。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

针对现有技术存在的问题，本发明提供了一种捷联惯性基导航系统信息滤波鲁棒对准方法、系统及终端，下面结合附图对本发明作详细的描述。

一、解释说明实施例。为了使本领域技术人员充分了解本发明如何具体实现，该部分是对权利要求技术方案进行展开说明的解释说明实施例。

本发明基于马氏距离(Mahalanobis Distance，MD)算法引入膨胀因子λ，当观测量受到非高斯噪声污染时，对量测噪声协方差阵R进行膨胀得到

并利用

替代R进行IKF量测更新，即得到RIKF。利用RIKF进行初始对准流程如图1所示。

如图1所示，本发明实施例提供的捷联惯性基导航系统信息滤波鲁棒对准方法包括以下步骤：

S101，进行捷联惯性基导航系统初始对准模型的构建；

S102，进行IKF算法的时间更新；

S103，计算观测量与观测新息之间的马氏距离，计算观测新息对应权值；

S104，基于马氏距离算法，引入膨胀因子λ_k，对估计出的R阵进行修正；

S105，进行RIKF量测更新：利用修正后的R阵进行IKF算法量测更新；

S106，利用RIKF估计出状态误差对SINS导航解算误差进行修正。

本发明实施例提供的捷联惯性基导航系统信息滤波鲁棒对准方法具体包括：

一、捷联惯性基导航系统初始对准模型

1.1初始对准状态方程

由于SINS的高度通道是独立、发散的，其高度通道信息可借助外部传感器如气压计或水压计等准确获得。因此，选取的状态量不考虑高度通道的速度和位置信息。选取状态量为：

式中，δL、δζ分别为纬度误差、经度误差；δv_E、δv_N分别为东向速度误差、北向速度误差；α＝[α_x；α_y；α_z]为欧拉平台误差角；

为陀螺仪常值漂移；

为加速度计零偏。

基于SINS/DVL组合的捷联惯性基导航系统初始对准对应的状态方程如式 (2)所示。

式中，w_SINS～N(0,Q_SINS)为系统噪声，Q_SINS为系统噪声协方差阵。由式(2) 建立的状态转移矩阵F_SINS为：

式中，F为7×7矩阵，F中的非零元素如下：

F_1,4＝1/R_e，F_2,1＝(v_E/R_e)tanLsecL，F_2,3＝secL/R_e，F_3,3＝(v_N/R_e)tanL

F_3,4＝2ω_iesinL+(v_E/R_e)tanL，F_3,6＝-f_U，F_3,7＝f_N

F_4,3＝-[2ω_iesinL+(v_E/R_e)tanL]，F_4,5＝f_U

F_4,7＝-f_E，F_5,4＝-1/R_e，F_5,6＝ω_iesinL+(v_E/R_e)tanL，F_5,7＝-[ω_iecosL+(v_E/R_e)]

F_6,1＝-ω_iesinL，F_6,3＝1/R_e，F_6,5＝-[ω_iesinL+(v_E/R_e)tanL]，F_6,7＝-v_N/R_e

F_7,1＝ω_iecosL+(v_E/R_e)(secL)²，F_7,3＝tanL/R_e，F_7,5＝ω_iecosL+v_E/R_e

F_7,6＝v_N/R_e (4)

式(4)中，

G为7×6矩阵，其具体形式如下：

式中，

表示姿态矩阵

的前两行。

1.2初始对准量测方程

DVL与SINS组合，一般选取速度误差作为观测量，即选取SINS测得的导航系(n系)下速度

与DVL测得的载体系(b系)下速度v^b在n系上投影之差作为观测量。基于SINS/DVL组合的捷联惯性基导航系统初始对准对应的量测方程如式(6)所示。

式中，H_v为量测矩阵，量测噪声V_v～N(0,R_v)，R_v为量测噪声阵。在线性小失准角条件下，

若选取东向速度误差δv_E和北向速度误差δv_N作为观测量，则有：

在实际应用中，用

代替

可得量测矩阵H_v为：

式中，

表示矩阵

的前两行。

二、鲁棒信息滤波(RIKF)算法基本理论

图1为基于RIKF算法的捷联式惯性基导航系统初始对准流程。

IKF时间更新和量测更新方程如下。

(a)时间更新方程

(b)量测更新方程

式中，F_k-1为k-1时刻状态转移矩阵，其表达式如式(4)所示。H_k为k时刻量测噪声阵，其表达式如式(7)所示。

为状态量的先验估计，P_k|k-1为状态误差协方差阵的先验估计，IP＝P^-1为信息矩阵。

为了实现标准IKF的鲁棒化，选择k时刻的观测量

与观测量的先验估计

之间的马氏距离作为评判指标，则k时刻评判指标θ_k的定义如式(14)所示。

式中，

为马氏距离；

为量测误差协方差阵的先验估计；μ_k为观测新息向量。对于真实的观测量

若其评判指标θ_k满足

则

将被标记为正常观测量；反之，若其评判指标θ_k满足

则

将被标记为异常观测量，此时通过引入膨胀因子λ_k用以膨胀量测噪声协方差阵R_k，即：

将式(15)带入式(14)可得：

式(16)可以转化为求解λ_k的非线性问题，如式(17)所示。

式中，λ_k可以通过牛顿迭代法求解。因此，λ_k(i+1)与λ_k(i)的关系可表示为：

式中，

且λ_k(i)初始值为λ_k(0)＝1。当评判指标满足

时，迭代终止。本发明将概率参数α设置为0.99，即鲁棒滤波方法的效率为99％，2自由度卡方分布值为

利用

代替R_k进行标准IKF滤波即可得到RIKF算法。k(k＝1，2，3...)时刻RIKF算法量测更新方程如下：

如图7所示，本发明实施例提供的捷联惯性基导航系统信息滤波鲁棒对准系统包括：

初始对准模型构建模块1，用于进行捷联惯性基导航系统初始对准模型构建；

时间量测更新模块2，用于进行IKF算法的时间更新；

马氏距离计算模块3，用于计算观测量与观测新息之间的马氏距离，计算观测新息对应权值；

R阵修正模块4，用于基于马氏距离算法，引入膨胀因子λ_k，对估计出的R 阵进行修正；

RIKF量测更新模块5，用于利用修正后的R阵进行IKF算法量测更新；

误差修正模块6，用于利用RIKF估计出状态误差对SINS导航解算误差进行修正。

二、应用实施例。为了证明本发明的技术方案的创造性和技术价值，该部分是对权利要求技术方案进行具体产品上或相关技术上的应用实施例。

本发明的技术方案主要应用于水下AUV，国内AUV技术的研究工作从上个世纪80年代开始。1979年，自提出了水下航行器研究的计划以来，国内研究水下航行器的单位越来越多。经过这么多年的研究发展，水下航行器已经得到大力发展并且有了一系列的成果。

某自动化所于1994年成功研制了国内第一款AUV——“探索者”号，实现了从有缆到无缆的技术飞跃。该型号AUV成功进行了多个航次的海上试验，成功下潜到1000米，并达到当时国际先进水平。1992年，中科院沈阳自动化所与中船702所、中科院声学所以及俄罗斯海洋技术问题研究所合作立项开始研制 6000米级的AUV“CR-01”，此型号于1995年研制成功并进行海试，中国也成为当时世界上少数几个有能力研制6000米级AUV的国家。在CR-01的基础上， CR-02于1997年研制成功，此型号解决了复杂地形下的导航与控制关键技术，其导航系统采用INS/DVL/LBL组合导航的方案，将定位误差控制在十米以内。近年来，某自动化所在AUV领域更是取得了一系列辉煌的成果，某自动化所构建了两个AUV的技术体系，分别为“潜龙”系列和“探索”系列。其中，“潜龙”系列AUV研制的主要用途是深海资源勘探，“探索”系列的主要用途是海洋科学研究。“潜龙”系列包括“潜龙一号”到“潜龙四号”四款主要型号。“潜龙一号”重1500公斤，最大下潜深度达6000米，这个深度可以覆盖全球 97％的海域面积，续航能力长达30小时。“潜龙二号”与“潜龙三号”最大下潜深度为4500米。潜龙系列AUV的导航系统包括惯导，DVL与水下声学定位设备，“潜龙一号”支持基于超短基线和基于长基线两种声学定位与惯导组合导航方案，“潜龙一号”利用INS/LBL组合导航的绝对定位精度优于2.63米。“潜龙二号”于2016年进行了复杂海底环境下的自主航行作业，单次下潜时间达到32小时13分钟，其航向精度达到±1°，导航精度达到0.5％。“探索”系列包括“探索100”、“探索1000”、“探索4500”等。“探索100”在国家 863计划支持下研制的小型自主式AUV，重要只有47公斤，最大下潜深度100 米。“探索1000”是一种最大下潜深度800米的AUV，用于获取敏感黑潮海域的水文观测数据。

综上所述，AUV是海洋探测、海洋科学研究、海洋环境保障、海洋生物保护、海洋军事侦察的重要工具，也是水下定位导航授时(Position,Navigation and Timing,PNT)系统建设用户端的重要组成，AUV的技术也较为成熟，惯性导航系统(INS)和多普勒测速仪(DVL)组成的惯性基导航系统已成为AUV主流导航手段，本发明方法可实现对现有AUV产品在初始对准过程中对环境的适应能力和快速启动能力上的提升。

三、实施例相关效果的证据。本发明实施例在研发或者使用过程中取得了一些积极效果，和现有技术相比的确具备很大的优势，下面内容结合试验过程的数据、图表等进行描述。

基于船载实测数据对RIKF进行验证。实验数据是从一套船载实验系统中采集得到的，实验系统使用的惯性测量单元(IMU)和多普勒计程仪(DVL)主要性能指标分别如表1和表2所示。由表1和表2可以看出，IMU数据更新率 (200Hz)明显大于DVL的数据更新率(1Hz)。

表1 IMU主要性能指标

表2 DVL性能指标

试验船上同时安装了一个单天线的GPS接收机，输出速度和位置信息，其数据更新率为1Hz。利用GPS输出数据与IMU输出数据进行组合导航，生成参考姿态、速度和位置信息，分别作为实验中的姿态、速度和位置基准。船载实验在长江内进行，实验过程设计为：当实验系统开机时，试验船保持系泊状态，时长大约15min；然后试验船驶出，运动大约6.4h。记录整个运动过程中的IMU 和DVL输出的原始数据，GPS输出的速度和位置数据。从实测数据中选择2组时长为900s的动态数据(一组受高斯噪声污染，另一组受野值污染)用于卡尔曼滤波(KF)、信息卡尔曼滤波(IKF)和鲁棒信息卡尔曼滤波(RIKF)初始对准性能试验验证。选取的两组900s数据包含：陀螺仪和加速度计的原始数据， DVL输出的速度数据，对应的姿态、速度和位置基准。其中，DVL的输出如图 2所示。由图2可以看出，在实际应用中DVL输出会受到较高强度的野值污染。

在开展试验工作之前，对船载实测数据进行了分析，发现在实际应用中DVL 测速信息确实存在野值和非高斯噪声的情况，事实上这种野值或非高斯噪声并不是在每一次应用中都会出现。但是，在实际应用中一旦出现这种问题，如果不能有效地处理，那么在军事应用领域可能会导致严重后果。出于这样的考虑，在利用实测数据(图2(a))对算法进行验证的时候，对实测数据进行了人为地恶化处理，目的是保证在恶劣环境下相关算法能够有效使用。

在试验中，设置SINS的初始失准角为[0.5°；0.5°；1°]，初始量测噪声协方差阵 R₀＝diag([0.1²,0.1²])m²/s²，初始状态误差协方差阵为：

式中pi为圆周率。

1、混合高斯分布污染情形

基于维纳近似定理，任何非高斯噪声分布可用已知概率密度的高斯噪声分布的有限和来表示或充分近似。假设观测噪声的实际概率分布如式(22)所示。

ρ_actual＝(1-α)N(0,R_c)+αN(0,R_p) (22)

式中，干扰因子α满足0≤α≤0.1；R_c＝R₀为DVL输出速度数据的量测噪声协方差阵，R_p＝κR₀(κ为比例系数)为具有较大标准偏差的干扰噪声协方差阵。当干扰因子α偏离0时，式(22)的分布又称为厚尾分布。

试验中，人为地将式(22)引入图2(a)。为充分验证RIKF应用于SINS初始对准中的可行性和有效性，结合实际应用，考虑如下两类混合高斯分布噪声污染情形：1)干扰噪声幅值固定，比例变化；2)干扰噪声比例固定，幅值变化。

(1)干扰噪声幅值固定，即设置R_p＝100R₀，α依次设置为0，0.02，0.04， 0.06，0.08，0.1，分别利用KF，IKF，RIKF进行50次Monte Carlo仿真试验。

为评判滤波性能，选取姿态误差的均方根误差(Root mean-square errors，RMSE)和均方根误差的时间平均(Time-averaged RMSE，TRMSE)作为评判滤波性能的指标。定义姿态误差的RMSE，TRMSE分别如式(23)-(24)所示。

式中，符号“attitude”表示姿态角，M为Monte Carlo仿真试验总次数，试验中M＝50。在初对准仿真试验中，计算时间800s到900s之间的TRMSE，即T₁＝800s，T₂＝900s。利用不同方法得到进行初始对准得到的姿态估计误差的RMSE (α＝0.1)如图3所示。利用不同方法进行初始对准得到的姿态估计误差的 TRMSE如图4所示。

由图3可看出，KF和IKF对非高斯噪声无鲁棒性，在利用KF和IKF进行初始对准的过程中，当DVL输出受非高斯噪声污染时，IKF对准误差曲线和KF 航向角对准误差曲线呈现发散趋势。由试验结果可知，KF对水平姿态角对准误差的RMSE收敛于0.05°以内，对航向角对准误差的RMSE收敛于1°以内； IKF对水平姿态角对准误差的RMSE分别收敛于0.2°和0.5°以内，对航向角对准误差的RMSE收敛于1°以内；RIKF对水平姿态角和航向角的对准误差的RMSE分别收敛于0.05°和0.5°以内。RIKF初始对准性能明显优于KF和IKF，这是因为当干扰噪声出现时，RIKF利用MD算法对量测噪声阵进行膨胀，进而实现对异常观测新息的抑制，使得RIKF在DVL输出受非高斯噪声污染时具有更强的对准鲁棒性。

图4可以看出，当干扰因子α为0时，即量测噪声不受干扰噪声的污染时， KF和IKF的姿态估计误差的TRMSE低于其他情况(α为0.02、0.04、0.06、0.08 和0.1)姿态估计误差的TRMSE。这也说明KF和IKF在α为0(即无干扰噪声) 的条件下具有较好的滤波性能。然而，当α增加，即量测噪声受到干扰噪声的污染时，KF和IKF对姿态角估计精度明显下降，相比于KF和IKF，RIKF具有更高初始对准精度和稳定性。试验结果表明，当DVL输出受到非高斯噪声污染时， RIKF能够有效抑制混合高斯分布噪声对初始对准结果的影响，将RIKF应用于捷联式惯性基导航系统初始对准中是可行、有效的。

(b)设置α＝0.1，κ依次设置为50，100，150，200，250，300，分别利用 KF，IKF，RIKF进行50次Monte Carlo仿真试验。利用不同方法进行初始对准得到的姿态估计误差的TRMSE如图5所示。

由图5可以看出，相比于KF和IKF，RIKF具有更高的初始对准精度。这也充分说明当干扰噪声比例增加时，RIKF具有更强的鲁棒性。试验结果表明，RIKF能够有效地抑制不同幅值、不同比例干扰噪声对初始对准结果的影响。在非高斯复杂条件下，将RIKF应用于SINS初始对准中是可行、有效的。

2、野值污染情形

由图2(b)可看出，在实际应用中，DVL输出确会受到高强度野值污染。利用实测数据(图2(b))对算法进行验证，分别利用KF，IKF，RIKF进行初始对准试验，结果如图6所示。

如图6所示，当DVL测速信息受野值污染的情形下，KF和IKF对准精度下降很明显，这也说明了KF和IKF对速度野值没有鲁棒性。在外部辅助信息受到野值污染的条件下，RIKF的对准性能明显优于KF和IKF。这是因为：RIKF 首先利用MD算法将速度野值准确地辨识出来，并通过如式(19)～(21)的量测更新过程将速度野值对初始对准的影响消除掉，从而相比于KF和IKF实现对准性能的提升。通过对准试验及图6可知，KF和IKF在900s时刻航向角对准误差分别为0.9434°和0.7179°，且航向角误差曲线并不收敛；RIKF在900s 时刻航向角对准误差为0.1242°，且航向角误差曲线在400s时刻以后稳定收敛到0.25°以内。相比于KF和IKF，RIKF不仅具有更高的对准精度，而且其对准误差曲线更加稳定，收敛性更好。

根据试验结果可得出结论：在水下非高斯环境中，相比于KF和IKF，RIKF 可在确保水下动态对准鲁棒性的同时，有效提高对准精度。RIKF能够有效克服初始对准过程中观测量受不同类型非高斯噪声污染时滤波性能下降的问题。

本发明提出的RIKF方法不仅适用于水下载体系速度辅助惯性基导航系统初始对准中，而且还将适用于全球卫星导航系统(Global satellite navigation system，GNSS)信号可用性差或遮蔽条件下，载体系速度辅助惯性基导航系统初始对准，并可进一步推广应用至组合导航领域。

应当注意，本发明的实施方式可以通过硬件、软件或者软件和硬件的结合来实现。硬件部分可以利用专用逻辑来实现；软件部分可以存储在存储器中，由适当的指令执行系统，例如微处理器或者专用设计硬件来执行。本领域的普通技术人员可以理解上述的设备和方法可以使用计算机可执行指令和/或包含在处理器控制代码中来实现，例如在诸如磁盘、CD或DVD-ROM的载体介质、诸如只读存储器(固件)的可编程的存储器或者诸如光学或电子信号载体的数据载体上提供了这样的代码。本发明的设备及其模块可以由诸如超大规模集成电路或门阵列、诸如逻辑芯片、晶体管等的半导体、或者诸如现场可编程门阵列、可编程逻辑设备等的可编程硬件设备的硬件电路实现，也可以用由各种类型的处理器执行的软件实现，也可以由上述硬件电路和软件的结合例如固件来实现。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，都应涵盖在本发明的保护范围之内。

Claims (10)

1.一种捷联惯性基导航系统信息滤波鲁棒对准方法，其特征在于，所述捷联惯性基导航系统信息滤波鲁棒对准方法包括：

基于马氏距离算法，引入膨胀因子λ当观测量受到非高斯噪声污染时，对量测噪声协方差阵R进行膨胀得到

并利用

替代R进行IKF量测更新，得到鲁棒RIKF。

2.如权利要求1所述捷联惯性基导航系统信息滤波鲁棒对准方法，其特征在于，所述捷联惯性基导航系统信息滤波鲁棒对准方法包括以下步骤：

步骤一，进行捷联惯性基导航系统初始对准模型的构建；

步骤二，进行IKF算法的时间更新；

步骤三，计算观测量与观测新息之间的马氏距离，计算观测新息对应权值；

步骤四，基于马氏距离算法，引入膨胀因子λ_k，对估计出的R阵进行修正；

步骤五，进行RIKF量测更新：利用修正后的R阵进行IKF算法量测更新；

步骤六，利用RIKF估计出状态误差对SINS导航解算误差进行修正。

3.如权利要求2所述捷联惯性基导航系统信息滤波鲁棒对准方法，其特征在于，所述步骤一中的捷联惯性基导航系统初始对准模型的构建包括：

(1)初始对准状态方程

选取的状态量不考虑高度通道的速度和位置信息，则选取状态量为：

式中，δL、δζ分别为纬度误差、经度误差；δv_E、δv_N分别为东向速度误差、北向速度误差；α＝[α_x；α_y；α_z]为欧拉平台误差角；

为陀螺仪常值漂移；

为加速度计零偏；

基于SINS/DVL组合的捷联惯性基导航系统初始对准对应的状态方程为：

式中，w_SINS～N(0,Q_SINS)为系统噪声，Q_SINS为系统噪声协方差阵；由状态方程建立的状态转移矩阵F_SINS为：

式中，F为7×7矩阵，F中的非零元素如下：

F_1,4＝1/R_e，F_2,1＝(v_E/R_e)tan L sec L，F_2,3＝sec L/R_e，F_3,3＝(v_N/R_e)tan L；

F_3,4＝2ω_iesin L+(v_E/R_e)tan L，F_3,6＝-f_U，F_3,7＝f_N；

F_4,3＝-[2ω_iesin L+(v_E/R_e)tan L]，F_4,5＝f_U；

F_4,7＝-f_E，F_5,4＝-1/R_e，F_5,6＝ω_iesin L+(v_E/R_e)tan L，F_5,7＝-[ω_iecos L+(v_E/R_e)]；

F_6,1＝-ω_iesin L，F_6,3＝1/R_e，F_6,5＝-[ω_iesin L+(v_E/R_e)tan L]，F_6,7＝-v_N/R_e；

F_7,1＝ω_iecosL+(v_E/R_e)(sec L)²，F_7,3＝tanL/R_e，F_7,5＝ω_iecosL+v_E/R_e；

F_7,6＝v_N/R_e；

式中，

G为7×6矩阵，表达形式如下：

式中，

表示姿态矩阵

的前两行；

(2)初始对准量测方程

DVL与SINS组合，选取速度误差作为观测量，选取SINS测得的导航系下速度

与DVL测得的载体系下速度v^b在n系上投影之差作为观测量，则基于SINS/DVL组合的捷联惯性基导航系统初始对准对应的量测方程为：

式中，H_v为量测矩阵，量测噪声V_v～N(0,R_v)，R_v为量测噪声阵；在线性小失准角条件下，

若选取东向速度误差δv_E和北向速度误差δv_N作为观测量，则有：

在实际应用中，用

代替

得量测矩阵H_v为：

式中，

表示矩阵

的前两行。

4.如权利要求2所述捷联惯性基导航系统信息滤波鲁棒对准方法，其特征在于，所述步骤二中的IKF算法的时间更新包括：

式中，F_k-1为k-1时刻状态转移矩阵，H_k为k时刻量测噪声阵，

为状态量的先验估计，P_k|k-1为状态误差协方差阵的先验估计，IP＝P^-1为信息矩阵。

5.如权利要求2所述捷联惯性基导航系统信息滤波鲁棒对准方法，其特征在于，所述步骤三中的计算观测量与观测新息之间的马氏距离，计算观测新息对应权值包括：

选择k时刻的观测量

与观测量的先验估计

之间的马氏距离作为评判指标，则k时刻评判指标θ_k的定义如下式所示：

式中，

为马氏距离；

为量测误差协方差阵的先验估计；μ_k为观测新息向量；对于真实的观测量

若评判指标θ_k满足

则

将被标记为正常观测量；反之，若评判指标θ_k满足

则

将被标记为异常观测量。

6.如权利要求2所述捷联惯性基导航系统信息滤波鲁棒对准方法，其特征在于，所述步骤四中的基于马氏距离算法，引入膨胀因子λ_k，对估计出的R阵进行修正包括：

通过引入膨胀因子λ_k用于膨胀量测噪声协方差阵R_k：

将噪声协方差阵表达式带入k时刻评判指标θ_k的表达式，得：

将新的k时刻评判指标θ_k的表达式转化为求解λ_k的非线性问题，则：

式中，λ_k通过牛顿迭代法求解，则λ_k(i+1)与λ_k(i)的关系表示为：

式中，

且λ_k(i)初始值为λ_k(0)＝1；当评判指标满足

时，迭代终止；

所述步骤五中的利用修正后的R阵进行IKF算法量测更新包括：

将概率参数α设置为0.99，鲁棒滤波方法的效率为99％，2自由度卡方分布值为

利用

代替R_k进行标准IKF滤波得到RIKF算法，k时刻RIKF算法量测更新方程如下：

式中，k＝1，2，3...。

7.一种应用如权利要求1～6任意一项所述捷联惯性基导航系统信息滤波鲁棒对准方法的捷联惯性基导航系统信息滤波鲁棒对准系统，其特征在于，所述捷联惯性基导航系统信息滤波鲁棒对准系统包括：

初始对准模型构建模块，用于进行捷联惯性基导航系统初始对准模型构建；

时间量测更新模块，用于进行IKF算法的时间更新和量测更新；

马氏距离计算模块，用于计算观测量与观测新息之间的马氏距离，计算观测新息对应权值；

R阵修正模块，用于基于马氏距离算法，引入膨胀因子λ_k，对估计出的R阵进行修正；

RIKF量测更新模块，用于利用修正后的R阵进行IKF算法量测更新；

误差修正模块，用于利用RIKF估计出状态误差对SINS导航解算误差进行修正。

8.一种计算机设备，其特征在于，所述计算机设备包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行如下步骤：

基于马氏距离算法，引入膨胀因子λ，当观测量受到非高斯噪声污染时，对量测噪声协方差阵R进行膨胀得到

并利用

替代R进行IKF量测更新，得到鲁棒IKF。

9.一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，使得所述处理器执行如下步骤：

基于马氏距离算法，引入膨胀因子λ，当观测量受到非高斯噪声污染时，对量测噪声协方差阵R进行膨胀得到

并利用

替代R进行IKF量测更新，得到鲁棒IKF。

10.一种信息数据处理终端，其特征在于，所述信息数据处理终端用于实现如权利要求7所述捷联惯性基导航系统信息滤波鲁棒对准系统。

Images