一种基于集中滤波的SINS/DVL/USBL的水下紧组合导航方法

一种基于集中滤波的sins/dvl/usbl的水下紧组合导航方法
技术领域
1.本发明涉及水下导航技术领域，具体为一种基于集中滤波的sins/dvl/usbl的水下紧组合导航方法。


背景技术：

2.海洋中蕴含着大量的海洋生物资源，化学资源和丰富的石油、天然气等宝贵矿产资源。水下航行器(autonomous underwater vehicle，auv)作为探测海洋的重要工具，对未来开发海洋资源和拓展海洋军事领域无疑是必不可缺的。水下导航定位技术为auv提供准确的导航信息，对于auv安全而有效地进行海洋科学研究和调查作业至关重要。
3.组合导航就是利用各个导航设备的优点，取长补短，把多个导航设备有效地组合在一起，以此来实现auv的高精度导航，提高容错性和可靠性。sins/dvl组合导航和sins/usbl组合导航是目前较为成熟并广泛应用的水下组合导航方式。然而，sins/dvl组合导航能够抑制速度误差发散，但是dvl提供的高精度速度信息也会受自身测速误差和水下环境影响，最后表现为随时间累积的位置误差；sins/usbl组合导航虽然整体定位精度较高，能够抑制位置误差的发散，但因为要在水下布置声源发射器，有区域距离限制，无法满足auv实现广区域导航定位的需求。故在水下导航中，将sins、dvl和usbl通过组合导航技术进行组合，有助于实现各导航设备取长补短，抑制速度误差和位置误差的发散，提高长周期导航定位精度。
4.申请号为cn202210492631.4(sins/dvl/usbl水下紧组合导航方法及装置、水下载体控制设备)的专利提出了在基于sins/dvl/usbl组合条件下利用dvl的原始波束速度数据和usbl的原始斜距和方位角信息，使用联邦滤波实现紧组合导航方法并解决dvl或usbl信息缺失的问题。但该方法只考虑dvl的波束零偏，没有考虑dvl的其他主要误差和usbl的主要误差，不能实现导航设备之间的相互校正效果，采用的无重置的联邦滤波结构获得的估计也不是全局最优的。针对这些问题，本发明提出的基于集中滤波的sins/dvl/usbl的水下紧组合导航方法充分考虑三个导航设备的误差源影响，采用dvl的的原始波束频移数据和usbl的原始斜距和方位角信息构建基于集中滤波的量测量，采用集中滤波方法能够实现全局最优估计，获得更高定位精度。并且在集中滤波器中自适应地更新调整量测矩阵以解决dvl和usbl信息失效的问题。
5.申请号为cn202211086240.9(一种基于海底油气管道检测的水下多源导航定位方法)的专利提出的集中式sins/dvl/usbl组合导航的信息滤波模型，在观测模型的构建上是采取的松组合的方法实现的，即选取sins/dvl速度观测模型和sins/usbl位置观测模型。而本发明采用的集中滤波的结构是利用紧组合的组合方式实现的，构建基于频移量测的sins/dvl紧组合量测方程和基于相对测量信息的sins/usbl紧组合量测方程，作为集中滤波的量测。采用紧组合的组合方法，整体定位精度更高，鲁棒性也更强。
6.其提出的集中式sins/dvl/usbl组合导航的信息滤波模型，是通过改进ds-ukf算法提高状态估计精度，并用加权one-class svm的离群检测算法解决usbl数据的噪声和跳
点问题。而本发明采用的集中滤波结构中对于dvl和usbl的故障检测是通过卡方检验法实现的，并且在检测出dvl和usbl的故障后自适应改变量测方程维数，更新量测方程，实现更为简单，同时也能有效处理dvl和usbl信息失效带来的精度下降的问题。


技术实现要素：

7.为解决上述技术问题，本发明提出了一种基于集中滤波的sins/dvl/usbl的水下紧组合导航方法，实现三个导航设备之间的相互校正，优势互补，并且提高导航定位精度。
8.为实现上述目的，本发明采取的技术方案是：
9.一种基于集中滤波的sins/dvl/usbl的水下紧组合导航方法，包括如下步骤：
10.步骤1.选择sins/dvl/usbl导航算法的状态变量，构建集中滤波的状态方程；
11.步骤2.根据sins输出的载体速度信息转换计算dvl坐标系下的原始波束频移信息，以及根据载体位置信息和应答器的位置信息转换计算usbl声学基阵坐标系下的测角信息和斜距信息；
12.步骤3.在步骤1和步骤2的基础上，构建基于频移量测的sins/dvl紧组合量测方程和基于相对测量信息的sins/usbl紧组合量测方程，作为集中滤波的量测；
13.步骤4.设计dvl和usbl故障的检测与处理机制，并设计对dvl和usbl故障数据进行隔离，更新步骤3中的集中滤波的量测。
14.作为本发明进一步改进，所述步骤1具体如下：
15.选取传统的15维惯导系统误差作为sins子系统的误差状态量：
[0016][0017]
式中，δvn＝[δv
e δv
n δvu]
t
分别为东、北、天三向的速度误差；φn＝[φ
e φ
n φu]
t
分别为东、北、天三向的姿态误差；δpn＝[δl δλ δh]
t
分别为惯导的纬度误差、经度误差和高度误差；分别为sins加速度计三个轴向的零偏；ε＝[ε
x ε
y εz]
t
分别为sins陀螺仪三个轴向的零漂；
[0018]
选取dvl比例因子误差和dvl安装角误差为dvl子系统的误差状态量：
[0019]
x
dvl
＝[δθ
dx δθ
dy δθ
dz δκd]
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)
[0020]
式中，δθd＝[δθ
dx δθ
dy δθ
dz
]
t
为dvl与sins之间的三个坐标轴向的安装误差角；δκd为dvl的比例因子误差；
[0021]
选取usbl与sins之间的安装误差角、usbl的测角误差、测距误差系数和深度计ps的常值偏置误差作为usbl子系统的误差状态量：
[0022]
x
usbl
＝[δθ
ux δθ
uy δθ
uz δα δβ δκ
u δb
ps
]
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)
[0023]
式中，δθu＝[δθ
ux δθ
uy δθ
uz
]
t
为usbl与sins之间的三个轴向的安装误差角；δα为usbl的方位角测量误差，δβ为usbl的高度角测量误差，δκu为usbl的测距误差系数，δb
ps
为深度计ps的常值偏置误差；
[0024]
构建sins/dvl/usbl集中滤波系统时选取状态变量为：
[0025]
[0026]
建立sins/dvl/usbl紧组合导航集中滤波状态方程为：
[0027][0028]
式中，系统矩阵f
sins
为惯导系统矩阵，f
dvl
＝04×4为dvl子系统的系统矩阵，f
usbl
＝07×7为usbl子系统的系统矩阵，w为相关噪声矩阵。
[0029]
作为本发明进一步改进，所述步骤2具体如下：
[0030]
利用sins测量的导航系的速度信息，计算出四波束方向的频移值为:
[0031][0032]
式中，f0为声波发射频率，c为水中声速，α
dvl
＝70，fd为频移真值，为惯导姿态矩阵，vn为导航坐标系下的速度；
[0033]
根据已知应答器位置p
te
与sins解算的auv载体位置转换到地球坐标系下解算出应答器相对于安装在载体上的应答器的方位角与斜距
[0034][0035]
式中，[α β ρ]
t
为方位角和斜距真值，为方位角和斜距真值，
[0036]
式中，为声学基阵坐标系与载体坐标系之间的姿态转换矩阵，为导航坐标系与地球坐标系之间的姿态转换矩阵，
[0037][0038]
[0039]
应答器在u系下的坐标l，λ，h为sins输出的载体的纬度、经度和高度，rn为卯酉圈主曲率半径，e为第一偏心率。
[0040]
作为本发明进一步改进，所述步骤3具体如下：
[0041]
以sins解算出的频移值与dvl的实际频移差值作为量测值，基于频移量测的sins/dvl紧组合量测方程：
[0042][0043]
观测矩阵为：
[0044][0045]
以sins输出的惯导位置信息转换计算出的方位角、斜距信息与usbl输出的方位角、斜距信息的差值作为量测量：
[0046][0047]
式中，
[0048]
将sins的高度信息和ps的深度信息输出之差作为高度信息量测：
[0049]zps
＝δh-δb
ps
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(11)
[0050]
基于相对测量信息的sins/usbl紧组合量测方程：
[0051][0052]
量测矩阵为：
[0053][0054]
构建的sins/dvl/usbl紧组合导航系统的的量测量可选取为：
[0055][0056]
对应构建的量测方程为
[0057]
z＝hx+v
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(15)
[0058]
式中，量测矩阵为v为量测噪声相关项。
[0059]
作为本发明进一步改进，所述步骤4具体如下：
[0060]
采用χ2检验法来判断dvl和usbl的测量值是否出现异常。
[0061]
在判断dvl存在失效数据，存在野值时，采取数据隔断处理，系统的量测方程简化为：
[0062]
z＝z
s/u
＝h
s/u
x
26
×1+v
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(16)
[0063]
当usbl受环境影响出现野值时，采取数据隔断处理，系统的量测方程简化为：
[0064]
z＝z
s/d
＝h
s/d
x
26
×1+v
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(17)。
[0065]
本技术具有以下效益：
[0066]
本发明采用集中滤波的结构设计sins/dvl/usbl紧组合导航算法，能够实现全局最优估计，获得更高的定位精度；本发明在紧组合导航算法的状态变量的选取中充分考虑各个导航设备的主要误差源，能够实现导航设备之间的相互校正，优势互补；本发明在集中滤波结构中加入了故障检测和处理机制，能够提高集中滤波方法的容错性，解决dvl或usbl数据失效带来的精度下降问题。
附图说明
[0067]
图1是基于集中滤波的水下紧组合导航算法的结构示意图；
[0068]
图2是dvl的原始声波频移数据获取示意图；
[0069]
图3是usbl的原始方位角和斜距数据获取示意图；
[0070]
图4是sins/dvl/usbl紧组合系统集中滤波和联邦滤波水平定位误差对比。
具体实施方式
[0071]
下面结合附图与具体实施方式对本发明作进一步详细描述：
[0072]
如图1所示，本发明具体实施例提供的基于集中滤波的sins/dvl/usbl的水下紧组合导航方法，具体如下；
[0073]
步骤1.选择sins/dvl/usbl导航算法的状态变量，构建集中滤波的状态方程；
[0074]
步骤2.根据sins输出的载体速度信息转换计算dvl坐标系下的原始波束频移信息，以及根据载体位置信息和应答器的位置信息转换计算usbl声学基阵坐标系下的测角信息和斜距信息；
[0075]
步骤3.在步骤1和步骤2的基础上，构建基于频移量测的sins/dvl紧组合量测方程和基于相对测量信息的sins/usbl紧组合量测方程，作为集中滤波的量测；
[0076]
步骤4.设计dvl和usbl故障的检测与处理机制，并设计对dvl和usbl故障数据进行隔离，更新步骤3中的集中滤波的量测。
[0077]
实施步骤1具体如下：
[0078]
选取传统的15维惯导系统误差作为sins子系统的误差状态量：
[0079][0080]
式中，δvn＝[δv
e δv
n δvu]
t
分别为东、北、天三向的速度误差；φn＝[φ
e φ
n φu]
t
分别为东、北、天三向的姿态误差；δpn＝[δl δλ δh]
t
分别为惯导的纬度误差、经度误差和高度误差；分别为sins加速度计三个轴向的零偏；ε＝[ε
x ε
y εz]
t
分别为sins陀螺仪三个轴向的零漂；
[0081]
选取dvl比例因子误差和dvl安装角误差为dvl子系统的误差状态量：
[0082]
x
dvl
＝[δθ
dx δθ
dy δθ
dz δκd]
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)
[0083]
式中，δθd＝[δθ
dx δθ
dy δθ
dz
]
t
为dvl与sins之间的三个坐标轴向的安装误差角；δκd为dvl的比例因子误差；
[0084]
选取usbl与sins之间的安装误差角、usbl的测角误差、测距误差系数和深度计ps的常值偏置误差作为usbl子系统的误差状态量：
[0085]
x
usbl
＝[δθ
ux δθ
uy δθ
uz δα δβ δκ
u δb
ps
]
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)
[0086]
式中，δθu＝[δθ
ux δθ
uy δθ
uz
]
t
为usbl与sins之间的三个轴向的安装误差角；δα为usbl的方位角测量误差，δβ为usbl的高度角测量误差，δκu为usbl的测距误差系数，δb
ps
为深度计ps的常值偏置误差；
[0087]
构建sins/dvl/usbl集中滤波系统时选取状态变量为：
[0088][0089]
建立sins/dvl/usbl紧组合导航集中滤波状态方程为：
[0090][0091]
式中，系统矩阵f
sins
为惯导系统矩阵，f
dvl
＝04×4为dvl子系统的系统矩阵，f
usbl
＝07×7为usbl子系统的系统矩阵，w为相关噪声矩阵。
[0092]
实施步骤2具体如下：
[0093]
利用sins测量的导航系的速度信息，计算出四波束方向的频移值为:
[0094][0095]
式中，f0为声波发射频率，c为水中声速，α
dvl
＝70，fd为频移真值，为惯导姿态矩阵，vn为导航坐标系下的速度；
[0096]
根据已知应答器位置p
te
与sins解算的auv载体位置转换到地球坐标系下解算出应答器相对于安装在载体上的应答器的方位角与斜距
[0097][0098]
式中，[α β ρ]
t
为方位角和斜距真值，为方位角和斜距真值，
[0099]
式中，为声学基阵坐标系与载体坐标系之间的姿态转换矩阵，为导航坐标系与地球坐标系之间的姿态转换矩阵，
[0100][0101][0102]
应答器在u系下的坐标l，λ，h为sins输出的载体的纬度、经度和高度，rn为卯酉圈主曲率半径，e为第一偏心率。
[0103]
实施步骤3具体如下：
[0104]
如图2所示，为dvl换能器四个波束方向的多普勒频移，以sins解算出的频移值与dvl的实际频移差值作为量测值，基于频移量测的sins/dvl紧组合量测方程：
[0105][0106]
观测矩阵为：
[0107][0108]
如图3所示，usbl根据声学基阵上四个水听器获得方位角和斜距，以sins输出的惯导位置信息转换计算出的方位角、斜距信息与usbl输出的方位角、斜距信息的差值作为量测量：
[0109][0110]
式中，
[0111]
将sins的高度信息和ps的深度信息输出之差作为高度信息量测：
[0112]zps
＝δh-δb
ps
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(11)
[0113]
基于相对测量信息的sins/usbl紧组合量测方程：
[0114][0115]
量测矩阵为：
[0116][0117]
构建的sins/dvl/usbl紧组合导航系统的的量测量可选取为：
[0118][0119]
对应构建的量测方程为
[0120]
z＝hx+v
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(15)
[0121]
式中，量测矩阵为v为量测噪声相关项。
[0122]
实施步骤4具体如下：
[0123]
采用χ2检验法来判断dvl和usbl的测量值是否出现异常。
[0124]
取误警率0.005确定阈值td＝14.86，在判断dvl存在失效数据，存在野值时，采取数据隔断处理，系统的量测方程简化为：
[0125]
z＝z
s/u
＝h
s/u
x
26
×1+v
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(16)
[0126]
取误警率0.005确定阈值tu＝12.84，当usbl受环境影响出现野值时，采取数据隔断处理，系统的量测方程简化为：
[0127]
z＝z
s/d
＝h
s/d
x
26
×1+v
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(17)
[0128]
整体流程：
[0129]
在sins/dvl/usbl集中滤波获得导航定位信息的过程中，将选取三个导航设备的主要误差源作为滤波器的状态变量实现校正，构建状态模型；对根据sins输出的导航信息计算出dvl的频移信息和usbl的相对测量信息，进而得到sins/dvl四波束方向频移信息量测和sins/usbl相对位置信息量测，构建量测模型；对dvl和usbl的量测信息进行故障检测，根据故障情况更新集中滤波器的量测模型；最后根据得到的状态误差估计对sins进行校正，得到导航算法计算出的速度、姿态和位置信息。
[0130]
本发明的有益效果通过如下仿真得以验证：
[0131]
设计了auv在水平面以4m/s的速度进行匀速割草机轨迹，仿真时间为2185s，为了确保算法定位精度验证，屏蔽各类误差，设置初始姿态角均为0，无初始速度误差和位置误差和摇摆运动。陀螺仪常值漂移设置为0.02
°
/h，随机游走为加速度计常值漂移设置为50μg，随机游走为更新频率为200hz；dvl比例因子误差为0.5％，安装角误差为[0.3
°ꢀ‑
0.2
°ꢀ
0.6
°
]；usbl方位角误差均为0.1
°
，斜距误差系数为0.5％，安装角误差为[0.5
°ꢀ‑
0.5
°ꢀ
0.5
°
]；ps常值偏置为0.05m，随机偏置为0.01m。仿真结果如图4所示。
[0132]
由图4可以看出，相对于联邦滤波结构，基于集中滤波的sins/dvl/usbl的水下紧组合导航方法的水平定位误差更小，定位精度更高。
[0133]
以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。

技术特征：
1.一种基于集中滤波的sins/dvl/usbl的水下紧组合导航方法，包括如下步骤，其特征在于：步骤1.选择sins/dvl/usbl导航算法的状态变量，构建集中滤波的状态方程；步骤2.根据sins输出的载体速度信息转换计算dvl坐标系下的原始波束频移信息，以及根据载体位置信息和应答器的位置信息转换计算usbl声学基阵坐标系下的测角信息和斜距信息；步骤3.在步骤1和步骤2的基础上，构建基于频移量测的sins/dvl紧组合量测方程和基于相对测量信息的sins/usbl紧组合量测方程，作为集中滤波的量测；步骤4.设计dvl和usbl故障的检测与处理机制，并设计对dvl和usbl故障数据进行隔离，更新步骤3中的集中滤波的量测。2.根据权利要求1所述的一种基于集中滤波的sins/dvl/usbl的水下紧组合导航方法，其特征在于，所述步骤1具体如下：选取传统的15维惯导系统误差作为sins子系统的误差状态量：x
sins
＝[δv
n φ
n δp
n
ꢀ▽ꢀ
ε]
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)式中，δv
n
＝[δv
e δv
n δv
u
]
t
分别为东、北、天三向的速度误差；φ
n
＝[φ
e φ
n φ
u
]
t
分别为东、北、天三向的姿态误差；δp
n
＝[δl δλ δh]
t
分别为惯导的纬度误差、经度误差和高度误差；
▽
＝[
▽
x
ꢀ▽
y
ꢀ▽
z
]
t
分别为sins加速度计三个轴向的零偏；ε＝[ε
x ε
y ε
z
]
t
分别为sins陀螺仪三个轴向的零漂；选取dvl比例因子误差和dvl安装角误差为dvl子系统的误差状态量：x
dvl
＝[δθ
dx δθ
dy δθ
dz δκ
d
]
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)式中，δθ
d
＝[δθ
dx δθ
dy δθ
dz
]
t
为dvl与sins之间的三个坐标轴向的安装误差角；δκ
d
为dvl的比例因子误差；选取usbl与sins之间的安装误差角、usbl的测角误差、测距误差系数和深度计ps的常值偏置误差作为usbl子系统的误差状态量：x
usbl
＝[δθ
ux δθ
uy δθ
uz δα δβ δκ
u δb
ps
]
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)式中，δθ
u
＝[δθ
ux δθ
uy δθ
uz
]
t
为usbl与sins之间的三个轴向的安装误差角；δα为usbl的方位角测量误差，δβ为usbl的高度角测量误差，δκ
u
为usbl的测距误差系数，δb
ps
为深度计ps的常值偏置误差；构建sins/dvl/usbl集中滤波系统时选取状态变量为：建立sins/dvl/usbl紧组合导航集中滤波状态方程为：式中，系统矩阵f
sins
为惯导系统矩阵，f
dvl
＝04×4为dvl子系统的系统矩阵，f
usbl
＝07×7为usbl子系统的系统矩阵，w为相关噪声矩阵。3.根据权利要求1所述的一种基于集中滤波的sins/dvl/usbl的水下紧组合导航方法，
其特征在于，所述步骤2具体如下：利用sins测量的导航系的速度信息，计算出四波束方向的频移值为:式中，f0为声波发射频率，c为水中声速，α
dvl
＝70
°
，f
d
为频移真值，为惯导姿态矩阵，v
n
为导航坐标系下的速度；根据已知应答器位置p
te
与sins解算的auv载体位置转换到地球坐标系下解算出应答器相对于安装在载体上的应答器的方位角与斜距与斜距式中，[α β ρ]
t
为方位角和斜距真值，为方位角和斜距真值，式中，为声学基阵坐标系与载体坐标系之间的姿态转换矩阵，为导航坐标系与地球坐标系之间的姿态转换矩阵，球坐标系之间的姿态转换矩阵，球坐标系之间的姿态转换矩阵，球坐标系之间的姿态转换矩阵，应答器在u系下的坐标l，λ，h为sins输出的载体的纬度、经度和高度，r
n
为卯酉圈主曲率半径，e为第一偏心率。4.根据权利要求1所述的一种基于集中滤波的sins/dvl/usbl的水下紧组合导航方法，其特征在于，所述步骤3具体如下：
以sins解算出的频移值与dvl的实际频移差值作为量测值，基于频移量测的sins/dvl紧组合量测方程：观测矩阵为：以sins输出的惯导位置信息转换计算出的方位角、斜距信息与usbl输出的方位角、斜距信息的差值作为量测量：式中，将sins的高度信息和ps的深度信息输出之差作为高度信息量测：z
ps
＝δh-δb
ps
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(11)基于相对测量信息的sins/usbl紧组合量测方程：量测矩阵为：构建的sins/dvl/usbl紧组合导航系统的的量测量可选取为：对应构建的量测方程为z＝hx+v
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(15)式中，量测矩阵为v为量测噪声相关项。5.根据权利要求1所述的一种基于集中滤波的sins/dvl/usbl的水下紧组合导航方法，其特征在于，所述步骤4具体如下：采用χ2检验法来判断dvl和usbl的测量值是否出现异常。在判断dvl存在失效数据，存在野值时，采取数据隔断处理，系统的量测方程简化为：z＝z
s/u
＝h
s/u
x
26
×1+v
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(16)当usbl受环境影响出现野值时，采取数据隔断处理，系统的量测方程简化为：z＝z
s/d
＝h
s/d
x
26
×1+v
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(17)。

技术总结
一种基于集中滤波的SINS/DVL/USBL的水下紧组合导航方法，方法包括以下步骤：（1）选择SINS/DVL/USBL导航算法的状态变量，构建集中滤波的状态方程；（2）根据SINS输出的载体速度信息转换计算DVL坐标系下的原始波束频移信息，以及根据载体位置信息和应答器的位置信息转换计算USBL声学基阵坐标系下的测角信息和斜距信息；（3）构建基于频移量测的SINS/DVL紧组合量测方程和基于相对测量信息的SINS/USBL紧组合量测方程；（4）设计DVL和USBL故障检测与处理机制，并设计对DVL和USBL故障数据进行隔离。本发明提供了一种基于集中滤波的SINS/DVL/USBL的水下导航算法，能有效解决导航定位过程中DVL或USBL数据失效带来的精度损失，并提高了水下定位精度。提高了水下定位精度。提高了水下定位精度。


技术研发人员：刘锡祥 章彩霞 虞贺斌 黄永江 陈世杰 陶育杰
受保护的技术使用者：东南大学
技术研发日：2023.05.23
技术公布日：2023/8/6