SINS设备自适应对准方法、系统及电子设备

技术领域

本发明涉及导航校准领域，尤其是涉及一种SINS设备自适应对准方法、系统及电子设备。

背景技术

近年来，水下导航技术成为海洋资源勘探、开发，海洋工程以及军事作战和精确打击等应用领域中必不可少的关键技术。高精度、高可靠性的水下导航技术在自主式水下航行器(Autonomous Underwater Vehicle，AUV)的发展战略中具有重要地位。

捷联式惯导系统(Strapdown Inertial Navigation System，SINS)由于其自主性强、隐蔽性高、结构简单、体积小及成本低等优点使其在航空、航天、航海等领域受到广泛关注和应用，已成为现代惯性导航技术研究和发展的主流，并成为AUV长航时、长航程、高精度导航的主要手段。初始对准是捷联式惯性导航系统进行导航解算之前的必经阶段，初始对准可分为水平姿态对准和方位(航向)对准。

初始对准主要包括粗对准和精对准，粗对准阶段一般采用解析式的对准方法，如优化对准方法等，此类方法均不具备对野值等非高斯噪声的鲁棒性，即要求外部测速辅助信息不受非高斯噪声污染。因此，在非高斯环境中，采用此类方法进行初始粗对准导致姿态角误差并不一定能满足基于线性卡尔曼滤波(KF)精对准小失准角的要求，这将会引入模型误差，导致基于KF滤波的初始精对准性能下降，甚至误差发散。因此采用非线性模型更能真实、有效地反映误差的传播特性。

相比于扩展卡尔曼滤波(EKF)、粒子滤波(PF)等非线性滤波方法，无迹卡尔曼滤波(UKF)通过选取一定数量的Sigma点，经过Unscented变换(Unscented Transformation，UT)后，可使UKF对非线性系统的后验状态均值和后验状态误差协方差的逼近精度达到二阶以上。相比于EKF，UKF通过Sigma点的确定性采样对非线性系统的概率密度进行近似，具有较强的非线性问题处理能力；相比于PF，UKF的采样点是确定的，其计算量明显较小，且UKF成功避免了PF中的粒子匮乏、衰退问题。因此，UKF在非线性导航问题中的应用，如在SINS非线性初始对准、非线性组合导航中的应用受到了日益关注。

对于GNSS信号受遮蔽或者拒止环境中的动态初始对准，常采用载体系测速设备如多普勒计程仪(DVL)、里程计(Odometer，OD)辅助SINS进行初始对准。SINS/DVL组合导航的过程噪声可以通过惯性传感器误差标定或者其他硬件层面的方法加以解决。SINS/DVL量测噪声会受到多种因素的影响，如海底地形、海洋生物、水下复杂环境致使DVL输出的测速信息易受野值或其他类型非高斯噪声污染，也致使观测量先验信息并不是准确已知的，将会把模型误差引入基于UKF框架下非线性对准方法，进而导致滤波性能降低。

发明内容

有鉴于此，本发明实施方式提供的一种SINS设备自适应对准方法、系统及电子设备，该方法可基于马氏距离算法实现无迹卡尔曼滤波鲁棒化，并对观测量进行重新加权，使得量测噪声协方差阵实现自适应迭代生成，从而实现了在量测噪声具有非高斯、不确定的分布特性条件下自适应地估计量测噪声协方差阵，提升了非线性、非高斯以及不确定条件下的滤波对准性能，提升了在非卫星条件下的惯性自助导航维持能力。

附图说明

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

图1为本发明实施例提供的一种SINS设备自适应对准方法的流程图；

图2为本发明实施例提供的一种SINS设备自适应对准方法中，坐标系的示意图；

图3为本发明实施例提供的一种SINS设备自适应对准方法中步骤S102的流程图；

图4为本发明实施例提供的一种SINS设备自适应对准方法中步骤S103的流程图；

图5为本发明实施例提供的一种SINS设备自适应对准方法中步骤S104的流程图；

图6为本发明实施例提供的一种SINS设备自适应对准方法中步骤S105的流程图；

图7为本发明实施例提供的另一种SINS设备自适应对准方法的流程图；

图8为本发明实施例提供的一种SINS设备自适应对准方法中DVL输出的结果图；

图9为本发明实施例提供的一种SINS设备自适应对准方法中，俯仰角估计误差及局部放大图；

图10为本发明实施例提供的一种SINS设备自适应对准方法中，横滚角估计误差及局部放大图；

图11为本发明实施例提供的一种SINS设备自适应对准方法中，航向角估计误差及局部放大图；

图12为本发明实施例提供的一种SINS设备自适应对准方法中，东向速度估计误差及局部放大图；

图13为本发明实施例提供的一种SINS设备自适应对准方法中，北向速度估计误差及局部放大图；

图14为本发明实施例提供的一种SINS设备自适应对准方法中，位置估计误差及局部放大图；

图15为本发明实施例提供的一种SINS设备自适应对准方法中，量测噪声协方差阵分量曲线图；

图16为本发明实施例提供的一种SINS设备自适应对准系统的结构示意图；

图17为本发明实施例提供的一种电子设备的结构示意图。

图标：

1610-第一对准模块；1620-第二对准模块；1630-第三对准模块；1640-第四对准模块；1650-第五对准模块；

101-处理器；102-存储器；103-总线；104-通信接口。

具体实施方式

为便于对本实施例进行理解，首先对本发明实施例所公开的一种SINS设备自适应对准方法进行详细介绍，如图1所示，该方法包括：

步骤S101，获取SINS设备对应的坐标系和误差参数；其中，坐标系包括：惯性坐标系、地球坐标系、地理坐标系、导航坐标系、载体坐标系；误差参数包括：误差角度、计算误差值、测量误差值。

坐标系是研究SINS问题的基础，该实施例中的坐标系如图2所示，定义惯性坐标系为i系，地球坐标系为e系，地理坐标系为t系，导航坐标系为n系，载体坐标系为b系。

误差角度主要包括欧拉平台误差角α

步骤S102，利用坐标系对应的参数以及误差参数，构建SINS设备对应的状态空间模型以及观测模型，并利用状态空间模型和观测模型构建初始对准模型。

初始化参数获取后，利用坐标系对应的参数以及误差参数分别构建状态空间模型以及观测模型，并利用状态空间模型以及观测模型得到初始对准模型。具体的，如图3所示，包括：

步骤S31，利用坐标系对应的参数以及误差参数，构建SINS设备对应的状态空间模型；

状态空间模型为：

其中，惯性坐标系为i系，地球坐标系为e系，地理坐标系为t系，导航坐标系为n系，载体坐标系为b系；L为纬度；ω

记s

矩阵

角速度向量

(3)式中，矩阵C

SINS在n系下的姿态更新方程为：

(5)式中，

(6)式中，

(7)式中，

定义姿态阵的解算误差ΔC为：

对(8)等式两边同时作微分可得：

结合式(9)和式(10)可得：

将

将式(12)代入式(3)可得SINS非线性姿态误差方程为：

SINS在n系下的速度更新方程为：

式中，g

考虑到实际应用中SINS存在计算和测量误差，式(14)可重新表示为如式(15)的形式。

式(15)中，

式(16)中，δf

式(17)中，δg

由式(15)减式(14)得SINS非线性速度误差方程：

(18)式中，

式中，L为纬度；ω

SINS在n系下的位置更新方程为：

考虑到实际应用中SINS存在计算和测量误差，式(23)可重新表示为如式(24)的形式。

式中，

SINS动基座初始对准非线性状态空间模型如式(26)所示。

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

式中，δL、δλ分别为纬度误差、经度误差；δv

步骤S32，利用坐标系对应的参数以及误差参数，构建SINS设备对应的观测模型。

选取东向速度误差δv

观测模型为：

其中，

式(29)中，[·]

步骤S33，利用状态空间模型和观测模型确定初始对准模型。

初始对准模型确定后，执行后续的对准步骤，具体如下。

步骤S103，根据无迹卡尔曼滤波对初始对准模型进行时间更新和量测更新，得到卡尔曼滤波增益、状态量后验估计结果以及状态误差协方差后验估计结果。

考虑如下离散时间非线性动态对准状态空间模型：

式中，f(·)表示非线性的过程模型函数，f(·)具体推导过程如式(1)-(26)所示；h(·)表示非线性观测模型函数，h(·)具体推导过程如式(28)所示；x

式(31)中，δ

步骤S41，构建无迹卡尔曼滤波，并初始化无迹卡尔曼滤波中包含的Sigma点；

步骤S42，对无迹卡尔曼滤波进行时间更新，利用Sigma点计算已选取的状态量先验估计结果以及状态误差协方差先验估计结果；

步骤S43，对无迹卡尔曼滤波进行量测更新，利用Sigma点计算观测量先验估计结果、量测误差协方差先验估计结果以及状态量观测量互协方差的先验估计结果；

步骤S44，利用状态量的先验估计结果、状态误差协方差先验估计结果、观测量先验估计结果、量测误差协方差先验估计结果以及状态量观测量互协方差的先验估计结果计算得到卡尔曼滤波增益、状态量后验估计结果以及状态误差协方差后验估计结果。

在一种实施方式中，对无迹卡尔曼滤波进行时间更新，利用Sigma点计算已选取的状态量先验估计结果

其中，χ

对无迹卡尔曼滤波进行量测更新，利用Sigma点计算观测量先验估计结果

其中，

利用状态量的先验估计结果

其中，K

对于时间更新过程，首先计算Sigma点χ

计算状态量的先验估计

对于测量更新过程，首先计算Sigma点：

计算观测量的先验估计

计算卡尔曼滤波增益K

式中，P

步骤S104，利用卡尔曼滤波增益、状态量后验估计结果以及状态误差协方差后验估计结果初始化量测噪声协方差阵以及滤波参数，并利用滤波参数对量测噪声协方差阵进行定点迭代。

在水下环境中，由于受到海底地形、海洋生物等因素的影响，DVL的测速噪声具有不确定、非高斯的统计特性，这会导致标准UKF滤波性能下降，甚至滤波发散。在水下AUV导航过程中，SINS/DVL组合导航系统量测噪声会受到多种因素的影响，如工作环境的变化，作业任务类型的变化都会对DVL量测噪声产生影响，系统的量测噪声统计特性准确程度直接影响系统的滤波估计精度。

根据式(41)和式(42)可知，当量测噪声协方差较大时，测量值在状态估计值中所占的比重相对较小，滤波器偏信于状态量先验估计，即SINS解算值；当量测噪声协方差较小时，测量值在状态估计值中所占的比重相对较大，滤波器偏信于测量值，即DVL测量数据。若量测噪声统计特性不准确，状态估计值将会错误分配先验值和测量值比重，从而导致最终滤波精度降低甚至滤波发散。VB估计方法属于多元统计的分析方法，采用数学形式简单的分布近似逼近真实后验分布，通过最小化KL散度，实现近似分布对真实后验概率的最优近似逼近。因此，实现R

考虑到k时刻量测噪声协方差阵R

式中，

根据式(36)～(37)计算Sigma点χ

在一种实施方式中，利用卡尔曼滤波增益、状态量后验估计结果以及状态误差协方差后验估计结果初始化量测噪声协方差阵以及滤波参数，并利用滤波参数对量测噪声协方差阵进行定点迭代的步骤，如图5所示，包括：

步骤S51，利用状态量先验估计结果以及状态误差协方差先验估计结果初始化滤波参数；

步骤S52，利用卡尔曼滤波增益以及滤波参数初始化量测噪声协方差阵；

步骤S53，利用滤波参数对量测噪声协方差阵进行定点迭代，直至迭代次数达到预设的迭代循环次数。

在一种实施方式中，滤波参数的第一参数

利用卡尔曼滤波增益以及滤波参数初始化量测噪声协方差阵R

其中，R

利用滤波参数对量测噪声协方差阵进行定点迭代，直至迭代次数达到预设的迭代循环次数的过程，通过以下算式计算得到：

/>

其中，j＝0:N′-1；N′为迭代循环的次数。

利用定点迭代法自适应估计阵具体过程如下。

初始化：

for j＝0:N′-1

参照式(36)利用

在给定后验PDF概率密度函数q

end

/>

式(56)中，N′为迭代循环的次数。式(36)～(40)，式(45)～(56)即为VBAUKF的量测更新过程。通过分析式(48)可知，当观测量受到野值等非高斯噪声污染时，

步骤S105，根据预设的膨胀因子和标度因子对量测噪声协方差阵进行修正，迭代结束后获取量测噪声协方差阵确定的状态误差结果，并利用状态误差结果对SINS设备进行修正对准。

本方案中，可利用MD算法对UKF进行鲁棒化，通过计算观测量权值ω

步骤S61，利用膨胀因子对量测噪声协方差阵进行修正，得到量测噪声协方差阵对应的修正量测噪声协方差阵

步骤S62，利用标度因子对量测噪声协方差阵进行修正，得到量测噪声协方差阵对应的修正自适应估计结果，并利用修正自适应估计结果进行迭代；

步骤S63，迭代结束后获取量测噪声协方差阵，利用量测噪声协方差阵确定状态误差结果，并利用状态误差结果对SINS设备进行修正对准。

利用膨胀因子对量测噪声协方差阵进行修正，得到量测噪声协方差阵对应的修正量测噪声协方差阵

其中，λ

利用标度因子对量测噪声协方差阵进行修正，得到量测噪声协方差阵对应的修正自适应估计结果，并利用修正自适应估计结果进行迭代的过程，通过以下算式计算得到：

/>

其中，κ

定义k时刻实际观测量的权值为ω

式中，η＝χ

对于

将式(59)带入式(58)，并令

式(60)可以转换为求解以下非线性方程的问题。

通过牛顿迭代法求解式(61)可得λ

式中，

由式(48)～(53)及式(59)可知，当观测异常时，VBRUKF通过引入膨胀因子实现了状态量后验估计的鲁棒化，但并不能保证量测噪声阵自适应估计过程的鲁棒性。由式(48)～(52)可以看出，当观测量受到如野值等非高斯噪声污染时，式(48)计算的

式中，k时刻标度因子κ

由式(64)可知，观测野值会使M

如图7所示的另一种SINS设备自适应对准系统方法，当观测异常时，VBRAUKF通过引入标度因子κ

特别地，在滤波过程中，令κ

下面结合仿真场景对上述SINS设备自适应对准系统方法的技术效果进行描述。试验数据是从一套船载系统中采集得到的，使用的IMU陀螺零偏优于0.02°/h(1σ)，加速度计零偏为5×10-5g(1σ)，数据采集率为200Hz；DVL测速精度为实际速度的5‰，数据更新率为1Hz。试验船同时安装了一个单天线GPS，数据采集率为1Hz。利用GPS输出数据与IMU输出数据进行组合导航，生成参考姿态、速度和位置信息，分别作为实验中的姿态、速度和位置基准。从实测数据中选择时长为900s受野值污染的动态数据用于UKF、VBAUKF、VBRUKF和VBRAUKF初始对准性能验证试验。选取的900s数据包含，陀螺仪和加速度计的原始数据，DVL输出速度数据，对应的姿态、速度和位置基准。其中DVL输出如图8所示。由图8可以看出，DVL输出受到幅值约为40m/s较高强度野值污染。

实际应用中DVL测速信息确实存在野值和非高斯噪声的情况，事实上这种野值或非高斯噪声并不是在每一次应用中都会出现。但是，在实际应用中一旦出现这种问题，如果不能有效地处理，那么在水下导航应用领域可能会导致严重后果。在利用船载实测数据对算法进行验证的时候，对实测数据进行了人为地恶化处理，目的是保证在恶劣环境下相关算法能够有效使用。试验中，设置SINS的初始失准角为[10°；10°；10°]。在实际应用中，量测噪声统计特性先验信息通常是未知、不确定的，故假定初始量测噪声协方差阵。

每隔30s人为地将幅值为40m/s的速度误差引入式(30)，分别利用UKF、VBAUKF、VBRUKF和VBRAUKF进行初始对准试验，姿态对准结果如图9～图11，速度对准误差如图12～图13，位置对准误差如图14。

由图9～图14可以看出，当DVL输出受野值污染时，UKF性能下降很明显，其对准误差曲线呈发散趋势。VBAUKF和VBRUKF姿态和航向对准误差曲线虽呈现收敛趋势，但是其航向角对准误差绝对值超过10°，VBAUKF和VBRAUKF速度和位置对准误差曲线呈发散趋势。VBRAUKF的初始对准性能明显优于UKF、VBAUKF和VBRUKF，且其对测速野值具有很好的抑制作用。在对准最后100s，VBRAUKF俯仰角、横滚角和航向角对准误差分别收敛至0.01°，0.02°和0.4°以内，位置对准误差优于55m，速度对准误差曲线呈现较好收敛性。试验表明：在高强度野值污染条件下，VBRAUKF的初始对准精度优于UKF、VBAUKF和VBRUKF。

图15为分别利用VBAUKF、VBRUKF和VBRAUKF在野值条件下对进行自适应估计得到的量测噪声协方差阵分量曲线。可以看出，VBAUKF和VBRUKF在估计的过程中并不具备鲁棒性，也就是说，异常观测将会导致的估计偏离理想值，进而降低滤波性能，导致滤波发散。同时，这也会导致VBRUKF计算得出膨胀因子也是不准确的，使其无法对异常观测量进行准确辨识。对于VBRAUKF，首先利用MD算法抑制野值的影响，而后根据量测噪声特性自适应地估计量测噪声协方差阵，这会使VBRAUKF对的估计精度优于VBAUKF和VBRUKF，对的解算精度高于VBRUKF，进而使VBRAUKF的对准性能优于VBAUKF和VBRUKF。仿真试验结果验证了VBRAUKF能够在量测噪声具有非高斯、不确定的分布特性条件下自适应地估计量测噪声协方差阵，且利用VBRAUKF进行SINS/DVL动态对准性能优于UKF、VBAUKF和VBRUKF。

通过上述实施例中提到的SINS设备自适应对准系统方法可知，该方法可基于马氏距离算法实现无迹卡尔曼滤波鲁棒化，并对观测量进行重新加权，使得量测噪声协方差阵实现自适应迭代生成，从而实现了在量测噪声具有非高斯、不确定的分布特性条件下自适应地估计量测噪声协方差阵，提升了非线性、非高斯以及不确定条件下的滤波对准性能，提升了在非卫星条件下的惯性自助导航维持能力。

对于前述实施例提供的一种SINS设备自适应对准系统，本发明实施例提供了一种SINS设备自适应对准系统，如图16所示，该系统包括：

第一对准模块1610，用于获取SINS设备对应的坐标系和误差参数；其中，坐标系包括：惯性坐标系、地球坐标系、地理坐标系、导航坐标系、载体坐标系；误差参数包括：误差角度、计算误差值、测量误差值；

第二对准模块1620，用于利用坐标系对应的参数以及误差参数，构建SINS设备对应的状态空间模型以及观测模型，并利用状态空间模型和观测模型构建初始对准模型；

第三对准模块1630，用于根据无迹卡尔曼滤波对初始对准模型进行时间更新和量测更新，得到卡尔曼滤波增益、状态量后验估计结果以及状态误差协方差后验估计结果；

第四对准模块1640，用于利用卡尔曼滤波增益、状态量后验估计结果以及状态误差协方差后验估计结果初始化量测噪声协方差阵R

第五对准模块1650，用于根据预设的膨胀因子λ

本发明实施例所提供的SINS设备自适应对准系统，其实现原理及产生的技术效果和前述SINS设备自适应对准方法实施例相同，为简要描述，装置实施例部分未提及之处，可参考前述方法实施例中相应内容。

本实施例还提供一种电子设备，该电子设备的结构示意图如图17所示，该设备包括处理器101和存储器102；其中，存储器102用于存储一条或多条计算机指令，一条或多条计算机指令被处理器执行，以实现上述SINS设备自适应对准方法。图17所示的电子设备还包括总线103和通信接口104，处理器101、通信接口104和存储器102通过总线103连接。其中，存储器102可能包含高速随机存取存储器(RAM，Random Access Memory)，也可能还包括非不稳定的存储器(non-volatile memory)，例如至少一个磁盘存储器。总线103可以是ISA总线、PCI总线或EISA总线等。所述总线可以分为地址总线、数据总线、控制总线等。为便于表示，图17中仅用一个双向箭头表示，但并不表示仅有一根总线或一种类型的总线。通信接口104用于通过网络接口与至少一个用户终端及其它网络单元连接，将封装好的IPv4报文或IPv4报文通过网络接口发送至用户终端。处理器101可能是一种集成电路芯片，具有信号的处理能力。在实现过程中，上述方法的各步骤可以通过处理器101中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器101可以是通用处理器，包括中央处理器(Central Processing Unit，简称CPU)、网络处理器(Network Processor，简称NP)等；还可以是数字信号处理器(Digital Signal Processor，简称DSP)、专用集成电路(ApplicationSpecific Integrated Circuit，简称ASIC)、现场可编程门阵列(Field-ProgrammableGate Array，简称FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。可以实现或者执行本公开实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。结合本公开实施例所公开的方法的步骤可以直接体现为硬件译码处理器执行完成，或者用译码处理器中的硬件及软件模块组合执行完成。软件模块可以位于随机存储器，闪存、只读存储器，可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器102，处理器101读取存储器102中的信息，结合其硬件完成前述实施例的方法的步骤。

在本申请所提供的几个实施例中，应该理解到，所揭露的系统、设备和方法，可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，又例如，多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些通信接口，设备或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。

所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个处理器可执行的非易失的计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以用软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

最后应说明的是：以上所述实施例，仅为本发明的具体实施方式，用以说明本发明的技术方案，而非对其限制，本发明的保护范围并不局限于此，尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，其依然可以对前述实施例所记载的技术方案进行修改或可轻易想到变化，或者对其中部分技术特征进行等同替换；而这些修改、变化或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的精神和范围，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应所述以权利要求的保护范围为准。