具有异常观测处理能力的水下航行器因子图融合导航方法技术

本发明专利技术属于水下自主航行器导航技术领域，特别涉及具有异常观测处理能力的水下航行器因子图融合导航方法，解决了传统算法存在的系统不断重构及无法有效处理异常观测数据的问题。该方法包括：步骤一：建立基于因子图框架的组合导航系统模型；步骤二：得到IMU预积分量的噪声协方差矩阵及带有零偏更新的IMU预积分因子残差向量；步骤三：得到DVL和USBL残差向量及观测噪声协方差矩阵；步骤四：判断DVL或USBL观测值是否正常；若正常，则步骤三得到的观测噪声协方差矩阵即为最终形式；若不正常，计算调节因子修正后得到观测噪声协方差矩阵的最终形式；步骤五：将各残差向量及最终观测噪声协方差矩阵插入到因子图框架中获得导航解。方差矩阵插入到因子图框架中获得导航解。方差矩阵插入到因子图框架中获得导航解。

【技术实现步骤摘要】
具有异常观测处理能力的水下航行器因子图融合导航方法


[0001]本专利技术属于水下自主航行器导航
，特别涉及一种具有异常观测处理能力的水下航行器因子图融合导航方法。

技术介绍

[0002]水下自主航行器(Autonomous Underwater Vehicle,AUV)是一种能够高效完成水下作业的新兴工具，具有活动范围广、体积小、灵活性强等优点。它在海洋资源勘探、海底地形测量和水下救援等应用中发挥着重要作用。特别是在军事领域，可用于战术防御、通信中继、侦察和制导攻击。因此，研制高性能的水下航行器已成为我国建设海洋强国、维护国家安全、实现伟大发展战略目标的迫切任务。然而，随着AUV实用化进程的不断加快，对导航系统的精确性和容错性提出了更高的要求，适用于AUV的高精度导航定位技术将成为未来AUV领域发展的研究重点。
[0003]由于水下环境的特殊性，以卫星导航为代表的无线电导航在水下环境中信号衰减，使其很难应用于水下航行器。因此，如今的AUV主要依靠超短基线系统(USBL)、多普勒测速仪(DVL)或惯性导航系统(INS)等水下传感器来获得载体的状态信息。INS具有完全自主的优点，既不需要外界输入，也不向外界传递信息，可以在各个地球领域中全天候、连续的给载体提供状态信息。然而，INS的导航误差会随着时间积累甚至发散。因此，其长期导航精度较差。DVL基于多普勒效应通过向海底发射并接收超声波信号，按既定模型可以解算出当前载体速度信息。DVL具有反应敏捷、稳定性好、导航精度高等优点。但是，DVL在复杂的水下环境中会受到水温、水深、障碍物各种不可抗因素的干扰作用，导致其精度变差甚至发生异常。USBL是一种典型的基于水声信号传播的声波定位系统。其定位精度已达到相当高的水平，并能适应各种水环境。但是USBL经常存在数据包丢失和连续性差的问题，同时也容易受到各种未知障碍的干扰。考虑到上述三种导航策略的互补性，通常将其组合在一起，建立INS/DVL/USBL组合导航系统，从而获得比单一传感器更准确可靠的导航信息。因此，INS/DVL/USBL组合导航将成为AUV首选的导航方式。
[0004]导航算法是导航系统解算的灵魂。对AUV导航来说，只有提供高精度的导航信息，才能确保AUV正常进行工作任务。目前，INS/DVL/USBL组合导航的信息融合实现通常以基于卡尔曼滤波的方法为主。这些方法大致可以分为两类:集中式处理和分布式处理。但集中式处理的可靠性较差，一旦任何一个传感器出现故障，都会对整个系统造成污染。相比之下，分布式处理具有可靠性高、通信负担低、故障检测和隔离灵活等优点。联邦卡尔曼滤波(FKF)是实现INS/DVL/USBL组合导航的最常用的分布式处理方法。然而，当传感器的有效性发生变化时，对应的子滤波器需要单独隔离，这无疑会导致频繁的系统重构，增加算法的复杂度。此外，FKF还要求所有传感器工作在同一频率。然而，在实际应用中，INS、DVL和USBL总是在不同的频率和不同的时延下工作。虽然已经发展了插值或外推等同步数据处理方法，通过将观测数据统一到同一时间来与FKF结合，但这些方法会导致额外的误差，导致工作在不同输出频率的传感器融合估计精度较差。
[0005]因子图优化算法(FGO)是近年来发展起来的一种信息融合方法，为组合导航异步数据处理提供了一种新思路。在该方法中，系统状态和观测值被定义为未知变量节点和已知因子节点，并对导航状态和观测值之间的概率关系进行了编码。如果在导航系统中添加一个新的传感器观测值，则只需在因子图中嵌入一个新的节点。因此，FGO可以通过简单地增加或减少相关的因子节点来实现对传感器观测信息的充分利用，提供了即插即用的统一导航框架，对于组合导航系统异步数据融合处理具有较强的灵活性和可扩展性。然而，由于水声传播的特殊性，无论是DVL还是USBL都容易受到复杂水下环境的干扰，而现有的FGO没有充分考虑到导航传感器观测异常的问题，导致传感器在复杂环境下性能变化时，导航解决方案的估计精度严重恶化。因此，迫切需要研究基于因子图框架的故障检测与误差补偿技术，以提高FGO用于INS/DVL/USBL组合导航信息融合的鲁棒性。

技术实现思路

[0006]本专利技术的目的是提供一种具有异常观测处理能力的水下航行器因子图融合导航方法，以解决传统AUV组合导航算法中存在的系统不断重构以及无法有效处理异常观测数据的技术问题，确保导航系统的可靠性和容错性。
[0007]本专利技术所采用的技术方案是，具有异常观测处理能力的水下航行器因子图融合导航方法，其特殊之处在于，包括以下步骤：
[0008]步骤一：建立基于因子图框架的INS/DVL/USBL组合导航系统模型，并定义系统的状态空间；
[0009]步骤二：根据IMU离散运动方程和IMU噪声误差传播模型，计算得到IMU预积分量的噪声协方差矩阵及带有零偏更新的IMU预积分因子的残差向量；
[0010]步骤三：根据DVL和USBL观测模型建立观测方程并得到对应的残差向量及观测噪声协方差矩阵；
[0011]步骤四：根据马氏距离的概念，判断DVL或USBL观测值是否正常；若观测值正常，则步骤三得到的所述观测噪声协方差矩阵即为其最终形式；若观测值不正常，则设计并计算调节因子，以对步骤三中相应的所述观测噪声协方差矩阵进行修正，修正后得到相应的所述观测噪声协方差矩阵的最终形式；
[0012]步骤五：将IMU预积分量的噪声协方差矩阵及带有零偏更新的IMU预积分因子的残差向量、DVL的残差向量及最终的观测噪声协方差矩阵、以及USBL的残差向量及最终的观测噪声协方差矩阵设计为因子节点插入到因子图框架中，对因子图中的所有节点进行非线性优化，获得导航解。
[0013]进一步地，步骤一中所述建立基于因子图框架的INS/DVL/USBL组合导航系统模型，具体为：将INS中IMU输出的信息通过预积分的方式处理因子节点，将DVL输出的速度信息表示为速度观测因子，将USBL输出的位置信息表示为位置观测因子，且三者之间采用松耦合方式组合；
[0014]将东
‑
北
‑
天地理坐标系定义为g坐标系，将导航坐标系定义为n系，并且选取东
‑
北
‑
天地理坐标系为导航坐标系，将载体坐标系定义为b系，惯性坐标系定义为i系，步骤一中所述的系统的状态空间如下式(1)所示：
[0015][0016]式(1)中：k为时间节点；为三维位置；为三维速度；是三维姿态；b
gk
和b
ak
分别为陀螺仪和加速度计的零偏。
[0017]进一步地，为了避免当k
‑
1时刻系统状态发生变化时，从k
‑
1时刻到k时刻的积分过程被重复计算，所述步骤二包括以下步骤：
[0018]步骤a1：根据IMU离散运动方程，对IMU进行预积分，并且计算从k
‑
1时刻到k时刻IMU在位置、速度和姿态上的相对运动增量；
[0019]步骤a本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.具有异常观测处理能力的水下航行器因子图融合导航方法，其特征在于，包括以下步骤：步骤一：建立基于因子图框架的INS/DVL/USBL组合导航系统模型，并定义系统的状态空间；步骤二：根据IMU离散运动方程和IMU噪声误差传播模型，计算得到IMU预积分量的噪声协方差矩阵及带有零偏更新的IMU预积分因子的残差向量；步骤三：根据DVL和USBL观测模型建立观测方程并得到对应的残差向量及观测噪声协方差矩阵；步骤四：根据马氏距离的概念，判断DVL或USBL观测值是否正常；若观测值正常，则步骤三得到的所述观测噪声协方差矩阵即为其最终形式；若观测值不正常，则设计并计算调节因子，以对步骤三中相应的所述观测噪声协方差矩阵进行修正，修正后得到相应的所述观测噪声协方差矩阵的最终形式；步骤五：将IMU预积分量的噪声协方差矩阵及带有零偏更新的IMU预积分因子的残差向量、DVL的残差向量及最终的观测噪声协方差矩阵、以及USBL的残差向量及最终的观测噪声协方差矩阵设计为因子节点插入到因子图框架中，对因子图中的所有节点进行非线性优化，获得导航解。2.根据权利要求1所述的具有异常观测处理能力的水下航行器因子图融合导航方法，其特征在于：步骤一中所述建立基于因子图框架的INS/DVL/USBL组合导航系统模型，具体为：将INS中IMU输出的信息通过预积分的方式处理因子节点，将DVL输出的速度信息表示为速度观测因子，将USBL输出的位置信息表示为位置观测因子，且三者之间采用松耦合方式组合；将东
‑
北
‑
天地理坐标系定义为g坐标系，将导航坐标系定义为n系，并且选取东
‑
北
‑
天地理坐标系为导航坐标系，将载体坐标系定义为b系，惯性坐标系定义为i系，步骤一中所述的系统的状态空间如下式(1)所示：式(1)中：k为时间节点；为三维位置；为三维速度；是三维姿态；和分别为陀螺仪和加速度计的零偏。3.根据权利要求2所述的具有异常观测处理能力的水下航行器因子图融合导航方法，其特征在于，所述步骤二包括以下步骤：步骤a1：根据IMU离散运动方程，对IMU进行预积分，并且计算从k
‑
1时刻到k时刻IMU在位置、速度和姿态上的相对运动增量；步骤a2：将计算得到的所述从k
‑
1时刻到k时刻IMU在位置、速度和姿态上的相对运动增量与相对运动增量的估计值作差得到带有固定零偏的IMU预积分因子的残差向量；步骤a3：根据IMU噪声误差传播模型计算得到IMU预积分量的噪声协方差矩阵和雅克比矩阵，以对所述带有固定零偏的IMU预积分因子的残差向量中的陀螺仪和加速度计的零偏进行修正，得到带有零偏更新的IMU预积分因子的残差向量。4.根据权利要求3所述的具有异常观测处理能力的水下航行器因子图融合导航方法，
其特征在于：所述的IMU离散运动方程如下式(2)所示：式(2)中：为b系相对于i系的瞬时角速度在b系下的投影；f
b
为载体在b系下的加速度；n
g
和n
a
分别为陀螺仪和加速度计的零均值高斯白噪声；得到的所述带有固定零偏的IMU预积分因子的残差向量如下式(7)所示：式(7)中：r
Δ～
为位置、速度、姿态的残差；r
b～
表示IMU零偏的残差；Log(
·
)表示向量的对数映射；和分别为IMU在姿态、速度和位置上的相对运动增量；为从b系到n系的姿态旋转矩阵；g
n
是重力加速度；Δt表示相邻两个观测值之间的时间间隔；定义预积分量的误差状态向量如下式(8)所示：式(8)中：和分别为预积分量的位置、速度和姿态误差；和分别为陀螺仪和加速度计的零偏误差；所述的IMU噪声误差传播模型利用误差摄动方法，误差状态的动力学模型如下式(9)所示：式(9)中：ω
t
为噪声矢量；F
t
和G
t
分别为状态转移矩阵和误差转移矩阵；将式(7)中的和用下式(12)所示的一阶展开进行更新，即可得到所述带有零偏更新的IMU预积分因子的残差向量；式(12)如下：式(12)中：表示更新后的零偏；Exp(
·
)表示向量的指数映射；J
k
‑
1,k
表示k
‑
1到k预积分
量的雅可比矩阵；是J...

【专利技术属性】
技术研发人员：高兵兵，董珣，胡高歌，殷海峰，
申请(专利权)人：西北工业大学，
类型：发明
国别省市：