CN110057365A - 一种大潜深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-₁+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 (8)

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

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

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

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

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

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

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

步骤七：根据强跟踪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)建立大潜深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协方差。

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

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

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

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

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

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

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

6.根据权利要求5所述的一种大潜深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。

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

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)滤波状态协方差矩阵量测更新为：

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

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

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