CN110057365B - 一种大潜深auv下潜定位方法 - Google Patents

Abstract

本发明涉及水下导航领域，具体涉及一种大潜深AUV下潜定位方法。通过AUV搭载水下通信节点、捷联惯导***和温盐深传感器；构建纯距离误差估计滤波模型；水面母船向AUV发送NED坐标系位置信息及时间信息；AUV记录捷联惯导***定位信息及温盐深传感器信息；通过传输时间和温盐深传感器信息得到AUV到母船的测距信息；构建强跟踪UKF算法；融合水面母船水平定位信息、测距信息和捷联惯导***定位信息和深度信息，跟踪深潜过程中AUV在NED坐标系下的水平定位误差以及载体坐标系下AUV的测速误差，对捷联惯导***校正得到精确AUV下潜定位信息。本发明能够扩展AUV捷联惯导***的水下在线校正手段，有效降低定位误差修正时间成本和设备成本，增强AUV的海洋环境适配能力。

Description

一种大潜深AUV下潜定位方法

技术领域

本发明涉及水下导航领域，具体涉及一种大潜深AUV下潜定位方法。

背景技术

AUV上的捷联惯导***的水下重调问题长久以来备受学界关注。

对于大潜深AUV而言，由于捷联惯导***在下潜阶段无法获得卫星导航定位信息和DVL测速信息的及时修正，不得不在纯惯导模式下工作，致使AUV潜至指定深度时，捷联惯导***产生数百甚至上千米的定位误差。如AUTOSUB 6000配备的Ixsea Oceano PHINS捷联惯导***，在纯惯导模式下的定位精度为0.3m/s，若大潜深AUV以2节速度下潜至6000m深度，捷联惯导***此时将产生至少1800m的定位误差，亟需水下声学定位***予以校正。

目前，常用的水下声学定位***有长基线定位***LBL和超短基线定位***USBL，LBL***虽然精度较高，但基阵的布放和回收都会耗费大量船时；而USBL***虽然安装布设方便，但其水面辅助导航设备不但价格昂贵而且对安装精度要求极高，与这两种声学定位***不同，本发明针对大潜深AUV在深潜过程中的捷联惯导***误差校正问题，提出了一种大潜深AUV下潜定位方法。

发明内容

本发明的目的在于提供一种大潜深AUV下潜定位方法，以扩展AUV捷联惯导***的水下在线校正手段，有效降低捷联惯导***在大潜深AUV布放段定位误差修正的时间成本和设备成本，增强大潜深AUV的海洋环境适配能力。

本发明实施例提供一种大潜深AUV下潜定位方法，包括：

步骤一：通过大潜深AUV搭载水下通信节点、捷联惯导***以及温盐深传感器，得到大潜深AUV的基本信息；

步骤二：构建大潜深AUV的纯距离误差估计滤波模型；

步骤三：根据同步水声通信协议，水面母船向大潜深AUV发送水面母船各时刻在NED坐标系下的水平位置信息以及相应的时间信息；

步骤四：根据大潜深AUV水声通信***接收到的步骤三所述时间信息，大潜深AUV记录此时的捷联惯导***定位信息以及此时的温盐深传感器输出的温度、导电率和深度信息；

步骤五：根据同步水声通信协议，通过传输时间和温盐深传感器信息，大潜深AUV计算得到自身到母船的测距信息；

步骤六：构建强跟踪UKF算法模型；

步骤七：根据强跟踪UKF算法，融合水面母船水平定位信息、大潜深AUV同母船的测距信息和大潜深AUV捷联惯导***定位信息、大潜深AUV深度信息，跟踪深潜过程中大潜深AUV在NED坐标系下的北向和东向水平定位误差以及载体坐标系下AUV的纵向和横向测速误差，对捷联惯导***输出进行校正，得到精确的大潜深AUV下潜定位信息；

本发明还包括这样一些结构特征：

所述步骤一中，所述步骤一方法的具体步骤为：

1)通过大潜深AUV搭载水下通信节点即水下modem，水面母船搭载水面通信节点即水面modem，将水声通信采用同步协议，使水面modem与水下modem时间同步；

2)通过大潜深AUV搭载捷联惯导***，实时输出大潜深AUV在NED坐标系下的定位信息以及在载体坐标系下的速度信息；

3)通过大潜深AUV搭载温盐深传感器，实时输出大潜深AUV潜深信息、大潜深AUV所处位置的温度信息以及大潜深AUV所处位置的海水导电率即盐度信息；

所述步骤二中，所述方法的具体步骤为：

1)设短时间内捷联惯导***的位置误差和速度误差均为常值；

2)建立大潜深AUV的纯距离误差估计滤波模型的离散***方程为：

X_k＝Φ_kX_k-1+w_k

其中，

上式中，

分别为大潜深AUV在NED导航坐标系下惯导***北向和东向的位置误差，

分别为大潜深AUV在载体坐标系下纵向和横向测速误差，T为采样时间，w_k～N(0，Q)，Q为w_k的4×4协方差矩阵，ψ、θ、γ分别为大潜深AUV的艏向、纵倾角和横倾角；

3)将母船同大潜深AUV的测距信息r_k和大潜深AUV温盐深输出的测深信息d_k作为量测信息，设观测噪声噪声为加性白噪声，建立大潜深AUV纯距离误差估计滤波模型的观测方程为：

z_k＝h(X_k)+v_k

其中，

上式中，

为NED导航坐标系下大潜深AUV捷联惯导***北向和东向定位信息，

为NED导航坐标系下水面母船北向和东向定位信息，v_k～N(0，R)，R为v_k的1×1协方差；

所述步骤三中，所述方法的具体步骤为：

1)水面母船采集各时刻卫星导航的定位信息；

2)水面母船生成自身在NED坐标系下的水平定位数据信息；

3)根据同步水声通信协议，水面母船将上述的定位信息和相应母船水声通信***时间发送至大潜深AUV；

所述步骤四中，所述方法的具体步骤为：

1)根据大潜深AUV搭载的水下通信节点即水下modem所采用的串口通信，通过中断方式与大潜深AUV的导航计算机通信；

2)大潜深AUV导航计算机响应水声通信中断后，通过采用串口通信，以轮询方式采集大潜深AUV捷联惯导***的定位信息以及温盐深传感器输出的温度、导电率和深度信息信息；

所述步骤五中，所述方法的具体步骤为：

1)根据同步水声通信协议，大潜深AUV计算得到母船水声通信***时间Ts；

2)根据同步水声通信协议，大潜深AUV计算得到自身水声通信***时间Tr；

3)根据上述结果，大潜深AUV计算得到同步水声通信传输时间T＝Tr-Ts；

4)根据温盐深传感器信息，大潜深AUV计算得到水声传播速度c；

5)根据公式r_k＝(T_r-T_s)c，大潜深AUV计算得到自身同母船的测距信息r_k；

所述步骤六中，所述方法的具体步骤为：

1)设强跟踪UKF的初始状态为：

上式中，0_4×1、0_4×4分别为4维零矢量和4维全零方阵；

2)时间更新：

为状态的估计值，不考虑***协方差矩阵Q，则滤波状态协方差矩阵的时间更新为：

3)根据水面母船水平定位信息以及大潜深AUV捷联惯导***定位信息，计算观测方程参数

和

4)根据观测方程参数

和

基于滤波状态的时间更新

通过确定性观测模型z_k＝h(X_k)预测量测信息为：

上式中，

为状态矢量

的前两项；

5)根据母船和大潜深AUV的测距信息r和大潜深AUV温盐深传感器输出的测深信息d，得到强跟踪UKF的观测信息z_k为：

6)计算量测信息残差为：

7)计算残差协方差矩阵为：

上式中，ρ为遗忘因子，且0＜ρ≤1，通常ρ＝0.95；

8)基于UT变换计算***协方差矩阵Q通过确定性观测模型h的传播Q_zz；根据

和Q，求取Sigma点集{ζ_i}，(i＝1，…，L)；计算Sigma点{ζ_i}通过确定性观测模型h的传播ξ_i＝h(ζ_i)，最后计算Qzz为：

上式中，

为求二阶统计特性时的权系数；

9)计算残差中非正交噪声矢量协方差为：

N_k＝V_k-R-Q_zz；

10)基于UT变换计算k-1时刻状态协方差矩阵P_k，k-1的时间更新通过确定性观测模型h的传播P_zk，k-1；根据

和P_k，k-1求取Sigma点集{χ_i}，(i＝1，…，L)；计算Sigma点{χ_i}通过确定性观测模型h的传播δ_i＝h(χ_i)；根据

计算状态协方差矩阵P_k，k-1的时间更新通过确定性观测模型h的传播P_zk，k-1；

上式中，

为求二阶统计特性时的权系数；

11)计算渐消因子矩阵Λ_k；令M_k＝P_zk，k-1，计算

基于

计算渐消因子矩阵元素λ_i(i＝1，…，4)，构建渐消因子矩阵Λ_k＝diag(λ₁，λ₂，λ₃，λ₄)；

12)状态协方差矩阵的强跟踪时间更新为：

13)以

和

为观测方程参数，基于UT变换计算量测信息预测值；根据

和P_k，k-1求取Sigma点集{ζ_i}，(i＝1，…，L)；计算Sigma点{ζ_i}通过确定性观测模型h的传播ξ_i＝h(ζ_i)；根据

计算

和

上式中，

和

分别为求一、二阶统计特性时的权系数；

14)计算滤波增益为：

15)滤波状态量测更新为：

16)滤波状态协方差矩阵量测更新为：

所述步骤七中，所述方法的具体步骤为：

1)根据强跟踪UKF算法，估计深潜过程中大潜深AUV在NED坐标系下的北向和东向水平定位误差，以及载体坐标系下大潜深AUV的纵向和横向测速误差；

2)根据直接校正方法，补偿大潜深AUV捷联惯导***的定位误差和测速误差，跟踪深潜过程中大潜深AUV在NED坐标系下的北向和东向水平定位误差以及载体坐标系下大潜深AUV的纵向和横向测速误差，对捷联惯导***输出进行校正，得到精确的大潜深AUV下潜定位信息；

相比于现有技术，本发明的有益效果在于：

1.本发明基于强跟踪UKF构建大潜深AUV下潜定位方法，有效克服了母船与AUV间测距信息不确定性大、形式化非线性距离观测模型易产生模型失配等问题；

2.本发明仅仅利用通信声纳的测距信息，即可准确跟踪全海深AUV下潜段的定位误差和速度误差，避免了LBL声学信标的布放与回收的高昂船时成本而且不需要配备辅助USBL定位的高精度航姿与定位设备，避免了导航设备的繁复安装；

3.仿真结果表明，该方法能够有效补偿大潜深AUV下潜过程中捷联惯导***的定位与测速误差，只需要声学测距信息辅助，就可使捷联惯导***在大潜深AUV下潜过程中的东向和北向定位精度达到米级，与卫星导航***定位精度相当。

附图说明

图1为本发明基于强跟踪UKF的大潜深AUV下潜定位信号流示意图；

图2为本发明大潜深AUV与水面母船位置关系图；

图3为本发明定位数据信息示意图；

图4为本发明强跟踪UKF算法流程图；

图5为本发明基于UT变换计算强跟踪UKF渐消因子矩阵流程图；

图6为按照本发明进行定位误差修正的AUV下潜仿真路径示意图；

图7(a)为理论轨迹与纯惯导航迹跟踪轨迹示意图；

图7(b)为理论轨迹与修正后航迹跟踪轨迹示意图；

图8为本发明大潜深AUV纯惯导北向定位误差示意图；

图9为本发明大潜深AUV纯惯导东向定位误差示意图；

图10为本发明大潜深AUV纯惯导xb轴测速误差示意图；

图11为本发明大潜深AUV纯惯导yb轴测速误差示意图；

图12为本发明距离信息辅助的大潜深AUV惯导北向定位误差示意图；

图13为本发明距离信息辅助的大潜深AUV惯导东向定位误差示意图；

图14为本发明距离信息辅助的大潜深AUV惯导xb轴测速误差示意图；

图15为本发明距离信息辅助的大潜深AUV惯导yb轴测速误差示意图。

具体实施方式

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图对本发明做进一步描述：

本发明的技术方案是这样实现的：

具体实施方式一：本实施方式的大潜深AUV下潜定位方法，包括：

步骤一：大潜深AUV搭载水下通信节点(水下modem)、温盐深和捷联惯导***；

步骤二：构建大潜深AUV纯距离误差估计滤波模型；

步骤三：基于同步水声通信，水面母船向AUV发送母船各时刻在北-东-地(NED)导航坐标系下的水平位置信息和相应的时间信息；

步骤四：AUV记录自身水声通信***接收信息时刻的捷联惯导***定位信息和温盐深传感器输出的深度、温度和导电率(获取盐度)信息；

步骤五：基于同步水声通信传输时间和温盐深传感器信息，AUV计算自身到母船的测距信息；

步骤六：构建强跟踪UKF算法；

步骤七：基于强跟踪UKF算法，融合水面母船水平定位信息、AUV同母船的测距信息和AUV捷联惯导***定位信息、AUV深度信息，跟踪深潜过程中AUV在NED导航系下的北向和东向水平定位误差以及载体下AUV的纵向(沿AUV的纵轴x轴)和横向(沿AUV的横轴y轴)测速误差，对捷联惯导***输出进行校正，得到精确的AUV下潜定位信息。

本发明的一个应用情形可以是搭载了水下通信节点(水下modem)、温盐深、捷联惯导***的大潜深AUV，在下潜阶段无法基于卫星导航***、DVL等导航传感器修正捷联惯导***误差，捷联惯导***只能在纯惯导模式下运行，定位误差快速积累。如图1所示，以捷联惯导***的北向和东向定位误差与测速误差为状态变量，构建大潜深AUV纯距离误差估计滤波模型；母船基于同步水声通信***，定时向AUV发送自身在北-东-地(NED)导航坐标系下的水平位置信息和信息发送时刻的时间信息；AUV上搭载的水声modem解算母船信息的同时，触发AUV导航计算机记录母船信息到达时刻的捷联惯导***定位信息、和温盐深传感器输出的深度、温度和导电率(获取盐度)信息；利用同步水声通信传输时间和温盐深传感器信息，AUV计算自身到母船的测距信息；针对大潜深AUV纯距离误差估计滤波模型特点，构建强跟踪UKF算法，基于强跟踪UKF算法，融合水面母船水平定位信息、AUV同母船的测距信息和AUV捷联惯导***定位信息、AUV深度信息，跟踪深潜过程中AUV在NED导航系下的北向和东向水平定位误差以及载体下AUV的纵向(沿AUV的纵轴x轴)和横向(沿AUV的横轴y轴)测速误差，对捷联惯导***输出进行校正，得到精确的AUV下潜定位信息。

具体实施方式二：本实施方式与具体实施方式一不同的是：步骤一具体为：

步骤一一：大潜深AUV搭载水下通信节点(水下modem)，水面母船搭载水面通信节点(水面modem)，水声通信采用同步模式，即水面modem与水下modem时间同步；

步骤一二：大潜深AUV搭载捷联惯导***，实时输出AUV在NED坐标系下的定位信息和载体坐标系下的速度信息；

步骤一三：大潜深AUV搭载温盐深，可实时输出AUV潜深信息、AUV所处位置的温度信息和海水导电率(获取盐度信息)。

本发明的一个应用情形可以是如图2所示，水面母船搭载水面通信节点(水面modem)，大潜深AUV搭载水下通信节点(水下modem)，为便于母船与AUV的通信和观测，AUV采用螺旋下潜方式，大潜深AUV搭载的捷联惯导***可以实时输出AUV在NED坐标系下的定位信息和测速信息和载体坐标系下的速度信息，同时大潜深AUV搭载温盐深，可实时输出AUV潜深信息、AUV所处位置的温度信息和海水导电率(获取盐度信息)。

具体实施方式三：本实施方式与具体实施方式一不同的是：步骤二具体为：

步骤二一：假定短时间内捷联惯导***的位置误差和速度误差均为常值；

步骤二二：建立大潜深AUV纯距离误差估计滤波模型的离散***方程

X_k＝Φ_kX_k-1+w_k

其中

式中

为NED导航坐标系下惯导***的位置误差，

为AUV载体坐标系下，纵向(沿AUV的纵轴x轴)和横向(沿AUV的横轴y轴)测速误差，T为采样时间，w_k～N(0，Q)，Q为w_k的4×4协方差矩阵，ψ、θ、γ分别为AUV的艏向、纵倾和横倾角。

步骤二三：将母船同AUV的测距信息r_k和AUV温盐深输出的测深信息d_k作为量测信息，设观测噪声噪声为加性白噪声，建立大潜深AUV纯距离误差估计滤波模型的观测方程

其中，

为NED导航坐标系下AUV捷联惯导***北向和东向定位信息，

为NED导航坐标系下水面母船北向和东向定位信息，v_k～N(0，R)，R为v_k的1×1协方差。

具体实施方式四：本实施方式与具体实施方式一不同的是：步骤三具体为：

步骤三一：水面母船采集各时刻卫星导航定位信息；

步骤三二：水面母船生成自身在NED导航坐标系下的水平定位数据信息；

步骤三三：基于同步水声通信，水面母船将定位信息和相应母船水声通信***时间发送给AUV。

本发明的一个应用情形可以是大潜深AUV采用北-东-地(NED)导航坐标系，如图2所示，母船基于同步水声通信***定时向大潜深AUV发送的定位数据信息包括命令行、数据包长度、水面母船信息发送历元T_s以及水面母船的东向和北向位置坐标信息。如图3所示，定位数据信息共5个信息字段。其中“20”为定位数据信息的指令码占两个字节；“16”为定位数据信息的字节总数占2个字节；水面母船信息发送历元T_s为定位数据信息发送时刻，其与水面母船的东向和北向位置坐标信息字段均占用4个字节。大潜深AUV根据信息接受时刻T_r与水面母船历元T_s的时间差与水声速度c相乘后得到水声传输距离r_k，即r_k＝(T_r-T_s)c。

具体实施方式五：本实施方式与具体实施方式一不同的是：步骤四具体为：

步骤四一：AUV搭载的水下通信节点(水下modem)采用串口通信，基于中断方式与AUV的导航计算机通信；

步骤四二：AUV导航计算机响应水声通信中断后，采用串口通信，以轮询方式，采集AUV捷联惯导***的定位信息和温盐深传感器输出的深度、温度和导电率(获取盐度)信息；

具体实施方式六：本实施方式与具体实施方式一不同的是：步骤五具体为：

步骤五一：基于同步水声通信协议，AUV解算母船水声通信***时间T_s；

步骤五二：AUV解算自身水声通信***时间Tr；

步骤五三：AUV计算同步水声通信传输时间T＝T_r-T_s；

步骤五四：AUV根据温盐深传感器信息计算水声传播速度c；

步骤五五：AUV按公式计算自身同母船的测距信息r_k

r_k＝(T_r-T_s)c

具体实施方式七：本实施方式与具体实施方式一不同的是：步骤六具体为：

步骤六一：设强跟踪UKF的初始状态为：

其中0_4×1、0_4×4分别为4维零矢量和4维全零方阵。

步骤六二：时间更新：

为状态的估计值，不考虑***协方差矩阵Q，则滤波状态协方差矩阵的时间更新为：

步骤六三：基于水面母船水平定位信息和AUV捷联惯导***定位信息计算观测方程参数

和

步骤六四：采用观测方程参数

和

基于滤波状态的时间更新

通过确定性观测方程z_k＝h(X_k)预测量测信息：

其中

为状态矢量

的前两项；

步骤六五：基于母船同AUV的测距信息r和AUV温盐深输出的测深信息d，获取强跟踪UKF的观测信息z_k；

步骤六六：计算量测信息残差

步骤六七：计算残差协方差矩阵

式中ρ为遗忘因子，且0＜ρ≤1，通常ρ＝0.95。

步骤六八：基于UT变换计算***协方差矩阵Q通过确定性观测模型h的传播Q_zz。首先，根据

和Q，求取Sigma点集{ζ_i}，(i＝1，…，L)；然后，计算Sigma点{ζ_i}通过确定性观测模型h的传播ζ_i＝h(ζ_i)，最后计算Qzz。

式中，

为求二阶统计特性时的权系数。

步骤六九：计算残差中非正交噪声矢量协方差

N_k＝V_k-R-Q_zz

步骤六十：基于UT变换计算k-1时刻状态协方差矩阵P_k，k-1的时间更新通过确定性观测模型h的传播P_zk，k-1。首先根据

和P_k，k-1求取Sigma点集{χ_i}，(i＝1，…，L)；其次计算Sigma点{χ_i}通过确定性观测模型h的传播δ_i＝h(χ_i)；最后根据

计算状态协方差矩阵P_k-1的时间更新通过确定性观测模型h的传播P_zk，k-1，式中，

(i＝1，…，L)，为求二阶统计特性时的权系数；

步骤六十一：计算渐消因子矩阵Λ_k。令M_k＝P_zk，k-1，首先计算

然后基于

计算渐消因子矩阵元素λ_i(i＝1，…，4)，构建渐消因子矩阵Λ_k＝diag(λ₁，λ₂，λ₃，λ₄)；

步骤六十二：状态协方差矩阵的强跟踪时间更新；

步骤六十三：以

和

为观测方程参数，基于UT变换计算量测信息预测值；首先根据

和P_k，k-1求取Sigma点集{ζ_i}，(i＝1，…，L)；其次计算Sigma点{ζ_i}通过确定性观测模型h的传播ξ_i＝h(ζ_i)；最后根据

和

计算

和

其中

和

分别为求一、二阶统计特性时的权系数；

步骤六十四：计算滤波增益

步骤六十五：滤波状态量测更新：

步骤六十六：滤波状态协方差矩阵量测更新：

算法流程图如图4和图5所示，其中图5为图4中基于UT变换计算强跟踪UKF渐消因子矩阵的流程图。

具体实施方式八：本实施方式与具体实施方式一不同的是：步骤七具体为：

步骤七一：采用如图6所示的仿真路径，基于强跟踪UKF算法，估计深潜过程中AUV在NED导航系下的北向和东向水平定位误差，以及载体下AUV的纵向(沿AUV的纵轴x轴)和横向(沿AUV的横轴y轴)测速误差，在没有距离信息辅助的条件下，纯惯导***航迹跟踪路径如图7(a)的“+”曲线所示，如图7(b)中“+”曲线所示所示，校正后的惯导***航迹与理论航迹基本重合，如图8至图11所示分别为纯惯导***的定位误差和测速误差；

步骤七二：基于直接校正方法，补偿AUV捷联惯导***的定位误差和测速误差，跟踪深潜过程中AUV在NED导航系下的北向和东向水平定位误差以及载体下AUV的纵向(沿AUV的纵轴x轴)和横向(沿AUV的横轴y轴)测速误差，对捷联惯导***输出进行校正，得到精确的AUV下潜定位信息，仿真结果如图12至图15所示，由图可见，在声学测距信息辅助下，大潜深AUV测速误差大大降低，定位误差小于10米与卫星导航***定位误差相当。

本发明涉及水下导航领域，具体涉及一种大潜深AUV下潜定位方法。本发明针对大潜深AUV在深潜过程中捷联惯导***定位误差快速积累，校正手段贫乏的问题，提出了一种大潜深AUV下潜定位方法，包括：大潜深AUV搭载水下通信节点(水下modem)、温盐深和捷联惯导***；构建大潜深AUV纯距离误差估计滤波模型；基于同步水声通信，水面母船向AUV发送母船各时刻在北-东-地(NED)导航坐标系下的水平位置信息和相应的时间信息；基于同步水声通信传输时间和温盐深传感器信息，AUV计算自身到母船的测距信息；AUV记录自身水声通信***接收信息时刻的捷联惯导***定位信息和深度信息；基于强跟踪UKF算法，融合水面母船水平定位信息、AUV同母船的测距信息和AUV捷联惯导***定位信息、AUV深度信息，分别跟踪深潜过程中AUV在NED导航系下的北向和东向水平定位误差以及载体下AUV的纵向(沿AUV的纵轴x轴)和横向(沿AUV的横轴y轴)测速误差，对捷联惯导***输出进行校正，得到精确的AUV下潜定位信息。本发明基于纯距离信息，对大潜深AUV在下潜阶段捷联惯导***的定位误差进行修正，实现大潜深AUV的下潜定位。

本发明基于强跟踪UKF构建大潜深AUV下潜定位方法，有效克服了母船与AUV间测距信息不确定性大、形式化非线性距离观测模型易产生模型失配等问题，该专利方法仅仅利用通信声纳的测距信息，即可准确跟踪全海深AUV下潜段的定位误差和速度误差，避免了LBL声学信标的布放与回收的高昂船时成本而且不需要配备辅助USBL定位的高精度航姿与定位设备，避免了导航设备的繁复安装，仿真结果表明，该方法能够有效补偿大潜深AUV下潜过程中捷联惯导***的定位与测速误差，只需要声学测距信息辅助，就可使捷联惯导***在大潜深AUV下潜过程中的东向和北向定位精度达到米级，与卫星导航***定位精度相当。

Claims (6)

1.一种大潜深AUV下潜定位方法，其特征在于，包括：

步骤一：通过大潜深AUV搭载水下通信节点、捷联惯导***以及温盐深传感器，得到大潜深AUV的基本信息；

步骤二：构建大潜深AUV的纯距离误差估计滤波模型；

1)设短时间内捷联惯导***的位置误差和速度误差均为常值；

2)建立大潜深AUV的纯距离误差估计滤波模型的离散***方程为：

X_k＝Φ_kX_k-1+w_k

其中，

上式中，

分别为大潜深AUV在NED导航坐标系下惯导***北向和东向的位置误差，

分别为大潜深AUV在载体坐标系下纵向和横向测速误差，T为采样时间，w_k～N(0,Q)，Q为w_k的4×4协方差矩阵，ψ、θ、γ分别为大潜深AUV的艏向、纵倾角和横倾角；

3)将母船同大潜深AUV的测距信息r_k和大潜深AUV温盐深传感器输出的深度信息d_k作为量测信息，设观测噪声为加性白噪声，建立大潜深AUV纯距离误差估计滤波模型的观测方程为：

z_k＝h(X_k)+v_k

其中，

上式中，

为NED导航坐标系下大潜深AUV捷联惯导***北向和东向定位信息，

为NED导航坐标系下水面母船北向和东向定位信息，v_k～N(0,R)，R为v_k的1×1协方差；

步骤三：根据同步水声通信协议，水面母船向大潜深AUV发送水面母船各时刻在NED坐标系下的水平位置信息以及相应的时间信息；

步骤四：根据大潜深AUV水声通信***接收到的步骤三所述时间信息，大潜深AUV记录此时的捷联惯导***定位信息以及此时的温盐深传感器输出的温度、导电率和深度信息；

步骤五：根据同步水声通信协议，通过传输时间和温盐深传感器信息，大潜深AUV计算得到自身到母船的测距信息；

步骤六：构建强跟踪UKF算法模型；

1)设强跟踪UKF的初始状态为：

上式中，0_4×1、0_4×4分别为4维零矢量和4维全零方阵；

2)时间更新：

为状态的估计值，不考虑***协方差矩阵Q，则滤波状态协方差矩阵的时间更新为：

3)根据水面母船水平定位信息以及大潜深AUV捷联惯导***定位信息，计算观测方程参数

和

4)根据观测方程参数

和

基于滤波状态的时间更新

通过确定性观测模型z_k＝h(X_k)预测量测信息为：

上式中，

为状态矢量

的前两项；

5)根据母船和大潜深AUV的测距信息r和大潜深AUV温盐深传感器输出的深度信息d，得到强跟踪UKF的观测信息z_k为：

6)计算量测信息残差为：

7)计算残差协方差矩阵为：

上式中，ρ为遗忘因子，且0<ρ≤1，ρ＝0.95；

8)基于UT变换计算***协方差矩阵Q通过确定性观测模型h的传播Q_zz；根据

和Q，求取Sigma点集{ζ_i}，(i＝1,…，L)；计算Sigma点{ζ_i}通过确定性观测模型h的传播ξ_i＝h(ζ_i)，最后计算Qzz为：

上式中，

为求二阶统计特性时的权系数；

9)计算残差中非正交噪声矢量协方差为：

N_k＝V_k-R-Q_zz；

10)基于UT变换计算k-1时刻状态协方差矩阵P_k,k-1的时间更新通过确定性观测模型h的传播P_zk,k-1；根据

和P_k,k-1求取Sigma点集{χ_i}，(i＝1,…，L)；计算Sigma点{χ_i}通过确定性观测模型h的传播δ_i＝h(χ_i)；根据

计算状态协方差矩阵P_k,k-1的时间更新通过确定性观测模型h的传播P_zk,k-1；

上式中，

为求二阶统计特性时的权系数；

11)计算渐消因子矩阵Λ_k；令M_k＝P_zk,k-1，计算

基于

计算渐消因子矩阵元素λ_i(i＝1,…，4)，构建渐消因子矩阵Λ_k＝diag(λ₁,λ₂,λ₃,λ₄)；

12)状态协方差矩阵的强跟踪时间更新为：

13)以

和

为观测方程参数，基于UT变换计算量测信息预测值；根据

和P_k,k-1求取Sigma点集{ζ_i}，(i＝1,…，L)；计算Sigma点{ζ_i}通过确定性观测模型h的传播ξ_i＝h(ζ_i)；根据

计算

和

上式中，

和

分别为求一、二阶统计特性时的权系数；

14)计算滤波增益为：

15)滤波状态量测更新为：

16)滤波状态协方差矩阵量测更新为：

步骤七：根据强跟踪UKF算法，融合水面母船水平定位信息、大潜深AUV同母船的测距信息和大潜深AUV捷联惯导***定位信息、大潜深AUV深度信息，跟踪深潜过程中大潜深AUV在NED坐标系下的北向和东向水平定位误差以及载体坐标系下AUV的纵向和横向测速误差，对捷联惯导***输出进行校正，得到精确的大潜深AUV下潜定位信息。

2.根据权利要求1所述的一种大潜深AUV下潜定位方法，其特征在于，所述步骤一中，所述步骤一方法的具体步骤为：

1)通过大潜深AUV搭载水下通信节点即水下modem，水面母船搭载水面通信节点即水面modem，将水声通信采用同步协议，使水面modem与水下modem时间同步；

2)通过大潜深AUV搭载捷联惯导***，实时输出大潜深AUV在NED坐标系下的定位信息以及在载体坐标系下的速度信息；

3)通过大潜深AUV搭载温盐深传感器，实时输出大潜深AUV的深度信息、大潜深AUV所处位置的温度信息以及大潜深AUV所处位置的海水导电率即盐度信息。

3.根据权利要求2所述的一种大潜深AUV下潜定位方法，其特征在于：所述步骤三中，所述方法的具体步骤为：

1)水面母船采集各时刻卫星导航的定位信息；

2)水面母船生成自身在NED坐标系下的水平定位数据信息；

3)根据同步水声通信协议，水面母船将上述的定位信息和相应母船水声通信***时间发送至大潜深AUV。

4.根据权利要求3所述的一种大潜深AUV下潜定位方法，其特征在于：所述步骤四中，所述方法的具体步骤为：

1)根据大潜深AUV搭载的水下通信节点即水下modem所采用的串口通信，通过中断方式与大潜深AUV的导航计算机通信；

2)大潜深AUV导航计算机响应水声通信中断后，通过采用串口通信，以轮询方式采集大潜深AUV捷联惯导***的定位信息以及温盐深传感器输出的温度、导电率和深度信息。

5.根据权利要求4所述的一种大潜深AUV下潜定位方法，其特征在于：所述步骤五中，所述方法的具体步骤为：

1)根据同步水声通信协议，大潜深AUV计算得到母船水声通信***时间Ts；

2)根据同步水声通信协议，大潜深AUV计算得到自身水声通信***时间Tr；

3)根据上述结果，大潜深AUV计算得到同步水声通信传输时间T＝Tr–Ts；

4)根据温盐深传感器信息，大潜深AUV计算得到水声传播速度c；

5)根据公式r_k＝(T_r-T_s)c，大潜深AUV计算得到自身同母船的测距信息r_k。

6.根据权利要求5所述的一种大潜深AUV下潜定位方法，其特征在于：所述步骤七中，所述方法的具体步骤为：

1)根据强跟踪UKF算法，估计深潜过程中大潜深AUV在NED坐标系下的北向和东向水平定位误差，以及载体坐标系下大潜深AUV的纵向和横向测速误差；

2)根据直接校正方法，补偿大潜深AUV捷联惯导***的定位误差和测速误差，跟踪深潜过程中大潜深AUV在NED坐标系下的北向和东向水平定位误差以及载体坐标系下大潜深AUV的纵向和横向测速误差，对捷联惯导***输出进行校正，得到精确的大潜深AUV下潜定位信息。

Images