CN106767793A - 一种基于sins/usbl紧组合的auv水下导航定位方法 - Google Patents

Abstract

本发明公开一种基于SINS/USBL紧组合的AUV水下导航定位方法，由捷联惯性导航系统SINS和超短基线系统USBL组成，超短基线系统由安装在AUV上的水听器接收基阵和布放在海底的声源组成，由声波到达基阵中心水听器的传播时间计算出斜距，通过频域加权互相关得到声波到达基阵坐标轴上两个水听器的时延差，从而计算出到达距离差，USBL输出的斜距和到达距离差与SINS计算出的斜距和到达距离差进行滤波，滤波输出再反馈校正SINS。本发明解决SINS长时间位置误差积累的问题，USBL采用斜距和到达距离差与SINS计算的斜距和到达距离差进行紧组合，避免USBL直接解算位置带来的坐标转换误差和基阵偏移误差，提高AUV的定位精度。

Description

一种基于SINS/USBL紧组合的AUV水下导航定位方法

技术领域

本发明属于水下导航技术，具体涉及一种基于SINS/USBL紧组合的AUV水下导航定位方法。

背景技术

海洋已经成为了21世纪乃至以后各国激烈角逐的主要战场，而AUV(AutonomousUnderwater Vehicle)无疑是未来开发海洋资源和拓展海洋军事领域的必不可缺的工具。AUV在民用领域的应用主要体现在可以进行海洋环境的考察、海底矿产、生物资源的勘探、海事救援、海洋考古及海底光缆或其他工程项目的建设和维护等领域；在军事领域的应用，归结起来大体分为五个方面：水雷对抗、情报搜集、海洋环境监测、通信中继及其他军事领域的应用。正是由于AUV如此广泛的应用，它被称为现代海军的“力量倍增器”，引起各国极大地重视。

水下高精度导航和定位技术是解决水下作业的前提和关键，为保证AUV能够顺利的完成水下的相关任务，要求AUV导航系统具有水下长期自主的导航定位和返航能力。目前已有的定位技术中，惯性导航系统(SINS)因其隐蔽性强和自主性在水下航行器中得到广泛应用，虽然惯导技术本身已日渐成熟，但其误差会随着时间积累，因此，伴随惯导技术发展的同时，世界各国都一直在探索惯导系统的校准方法。LBL(Long Base Line，长基线)水声定位系统通常是由基线长度为几千米的海底应答器阵和被定为载体上的问答机组成，利用水下目标与海底阵元之间的距离信息来求解目标位置，但是水下布放、回收、校准基阵比较麻烦，且数据更新频率较低，作业复杂。超短基线只有一个尺寸很小的基阵安装在载体上，无需布放基阵，使用比较灵活和方便。

对超短基线声学定位系统研究较早的是挪威的Kongsberg Simrad公司，该公司于1997年推出了世界领先水平的高精度长程超短基线定位系统—HiPAP35O，作用距离可达3000米，距离测量精度优于20cm，随后推出HiPAP500，作用水深达4000m，测距精度优于20cm；新近推出的HiPAPP700，作用水深达10000米，测距精度优于50cm，也是世界上唯一工作水深上万米的长程超短基线定位系统。另外，法国OCEANO Technologies公司推出的Posidonia 6000长程超短基线定位系统，工作水深6000m，最大作用距离8000m，在6000m水深300度开角范围内，测距精度为0.5％，询问频率为8-14kHZ，应答频率为14-18kHz，该系统已经成功推向市场。由于国内对AUV技术开始研究的时间较晚，与欧美等发达国家相比，在很多方面都存在着不足。但随着不断的投入也取得了很大的进展。国内具有代表性的水声定位系统是哈尔冰工程大学水声工程学院研制出了四种基于声学定位的系统：深水重潜装潜水员超短基线定位系统、“探索者”号水下机器人超短基线定位系统，灭雷具配套水声跟踪定位装置、长程超短基线定位系统。

超短基线系统主要通过测距和侧向能力计算出AUV相对于声源的相对位置，再利用姿态传感器测得姿态角，通过坐标变换得到AUV的绝对地理坐标，由该位置来修正惯导的位置误差，即采用松组合导航方式。但是，由于超短基线系统在直接解算位置时，会引入坐标转换误差和基阵偏移误差，使得超短基线定位精度不高，无法保证组合导航系统的定位精度。

发明内容

发明目的：本发明的目的在于解决现有技术中存在的不足，提供一种基于SINS/USBL紧组合的AUV水下导航定位方法。

技术方案：本发明所述的一种基于SINS/USBL紧组合的AUV水下导航定位方法，其特征在于：通过将捷联惯性导航系统SINS和超短基线系统USBL组合，进而利用卡尔曼滤波方法完成组合导航，其具体步骤如下：

(1)超短基线由安装在AUV上的5个十字形水听器基阵和布放在海底的声源组成，水听器0安装在基阵中心原点，水听器1和水听器3安装在X轴上且距离为d₁₃，水听器2和水听器4安装在Y轴上且距离为d₂₄，水听器基阵接收声源的信号，通过同步时钟得到声波到达水听器基阵中心水听器0的传播时间，通过频域加权互相关分别得到声波到达水听器基阵X和Y坐标轴上两个水听器的时延差；

(2)根据得到的传播时间和时延差，计算得到USBL的斜距与到达距离差；

(3)根据步骤(3)所得位置信息P_SINS和海底声源位置信息，计算出SINS的斜距与到达距离差；

(4)根据位置信息P_SINS和海底声源位置信息，计算得到SINS的斜距与到达距离差；

(5)将USBL的斜距和到达距离差与SINS的斜距和到达距离差位置进行卡尔曼滤波，滤波输出校正SINS位置输出。

2、根据权利要求1所述的基于SINS/USBL紧组合的AUV水下导航定位方法，其特征在于，所述步骤(1)中频域加权互相关获取时延差通的具体步骤为：

(11)设定海底声源的发射信号为x(t),t为时间，第i个水听器接收到的信号为x_i＝a_ix(t-τ_i)+n_i(t)，第j个水听器接收到的信号为x_j＝a_jx(t-τ_j)+n_j(t)，其中a_i,a_j为声信号在水中传播的衰减系数，n_i(t),n_j(t)为噪声信号，τ_i,τ_j为传播时间，i，j取值组合为(1，3)和(2,4)；

(12)x_i和x_j的互相关函数为其中，τ＝τ_i-τ_j表示两个声波的到达时间差，T表示观测时间；

(13)根据维纳-辛钦定理，互功率谱和互相关函数之间关系为：G_ij(ω)是x_i和x_j之间的互功率谱，ω为频率；

(14)选择频域加权函数对信号进行加权滤波，以此来抑制噪声的影响，再利用傅里叶逆变换到时域得到广义互相关函数，即：

(5)找出相关峰函数图像最大值对应的横坐标，对应的横坐标值为时延差τ。

3、根据权利要求1所述的基于SINS/USBL紧组合的AUV水下导航定位方法，其特征在于，所述步骤(2)的具体步骤如下：

(21)声学基阵由安装在基阵中心和坐标轴上的5个十字形水听器基阵组成，海底声源地理坐标已知；

(22)由声波到达基阵中心水听器0的传播时间t，可得USBL斜距为R_USBL＝ct，c为水中声速；

(23)由时延差τ计算USBL到达距离差为△R_USBL13＝cτ₁₃，△R_USBL24＝cτ₂₄，τ₁₃为声源信号到X轴水听器1和水听器3之间的时延差，τ₂₄为声源信号到Y轴水听器2和水听器4之间的时延差。

4、根据权利要求1所述的基于SINS/USBL紧组合的AUV水下导航定位方法，其特征在于，所述步骤(4)的具体步骤为：

(41)假设水听器基阵安装位置所在的坐标系为载体坐标系，已知水下声源的绝对地理坐标参考位置是P_re(X_re Y_re Z_re)，水听器1到水听器4在载体坐标系下的位置分别是

(42)由捷联惯性导航系统解算的水下航行器位置为P_SINS(X_SINS Y_SINS Z_SINS)；

由坐标转换关系可得水听器1在导航坐标系下的位置为P_SINS1(X_SINS1 Y_SINS1 Z_SINS1)；

其中，C_ij为载体坐标系到导航坐标系的变换矩阵中的元素；

同理可得其他水听器在导航坐标系下的位置分别为：

P_SINS2(X_SINS2 Y_SINS2 Z_SINS2)；

P_SINS3(X_SINS3 Y_SINS3 Z_SINS3)；

P_SINS4(X_SINS4 Y_SINS4 Z_SINS4)；

(43)根据变换的位置信息和海底声源位置信息，可得SINS的斜距与到达距离差为：

有益效果：本发明通过超短基线系统测量斜距和到达距离差，与SINS计算的斜距与到达距离差构建紧组合模型，两者以一种相对信息测量量进行组合，没有用超短基线系统直接解算位置于SINS进行松组合导航，避免了超短基线系统直接解算带来的坐标转换误差和基阵偏移误差，提高了定位精度，弥补了SINS长时间航行误差积累的问题，同时避免了无线电定位系统和长基线定位系统的使用，保证了水下高精度导航的要求。

附图说明

图1为本发明的整体流程图；

图2为本发明中组合导航系统结构示意图；

图3为本发明中USBL定位原理示意图；

图4为本发明中SINS计算斜距与到达距离差示意图。

具体实施方式

下面对本发明技术方案进行详细说明，但是本发明的保护范围不局限于所述实施例。

如图1所示，本发明由安装在AUV上的捷联惯性系统系统SINS和超短基线系统USBL组成。超短基线由安装在AUV上的5个十字形水听器基阵和布放在海底的声源组成，水听器基阵用于接收声源的信号，并测量声源发出的声波到达基阵中心水听器的传播时间，通过频域加权互相关得到声波到达基阵坐标轴上两个水听器的时延差。

本发明的具体方法为：

(1)超短基线由安装在AUV上的5个十字形水听器基阵和布放在海底的声源组成，水听器基阵用于接收声源的信号，测量声波到达基阵中心水听器的传播时间，通过频域加权互相关得到声波到达基阵坐标轴上两个水听器的时延差；

(2)根据得到的传播时间和时延差，计算出USBL的斜距与到达距离差；

(3)捷联惯性导航系统SINS通过捷联解算得到AUV的位置信息P_SINS；

(4)根据位置信息P_SINS和海底声源位置信息，计算出SINS的斜距与到达距离差；

(5)USBL输出斜距和到达距离差与SINS计算出的斜距和到达距离差位置进行卡尔曼滤波，滤波输出校正SINS位置输出。

一、上述步骤(1)中时延差根据频域加权互相关获得，具体实现方式如下：

(1)海底声源的发射信号为x(t),第i个水听器接收到的信号为x_i＝a_ix(t-τ_i)+n_i(t)，第j个水听器接收到的信号为x_j＝a_jx(t-τ_j)+n_j(t)，其中a_i,a_j为声信号在水中传播的衰减系数，n_i(t),n_j(t)为噪声信号，τ_i,τ_j为传播时间；

(2)x_i和x_j的互相关函数为其中，τ＝τ_i-τ_j表示到达时间差，T表示观测时间；

(3)根据维纳-辛钦定理可知，互功率谱和互相关函数之间关系为：G_ij(ω)是x_i和x_j之间的互功率谱；

(4)选择频域加权函数对信号进行加权滤波，以此来抑制噪声的影响，再傅里叶逆变换到时域得到广义互相关函数，即：

(5)找出相关峰函数图像最大值对应的横坐标，对应的横坐标值为时延差τ。

二、步骤(2)中根据传播时间和时延差，计算USBL斜距与到达距离差具体实现方式如下：

(1)超短基线系统定位原理如图2所示，声学基阵由安装在基阵中心和坐标轴上的5个十字形水听器基阵组成，海底声源地理坐标已知；

(2)由声波到达基阵中心水听器的传播时间t，可得USBL斜距为R_USBL＝ct，c为水中声速；

(3)由频域加权互相关得到声源到达坐标轴两个水听器之间的时延差，可得USBL到达距离差为△R_USBL13＝cτ₁₃，△R_USBL24＝cτ₂₄，τ₁₃为声源信号到达X轴水听器1和水听器3之间的时延差，τ₂₄为声源信号到达Y轴水听器2和水听器4之间的时延差。

三、步骤(3)中捷联惯性导航系统SINS通过捷联结算得到AUV的位置信息P_SINS，具体实现方式如下：

(1)计算姿态矩阵和姿态角。采用四元数法计算姿态矩阵，根据欧拉定理,动坐标系相对参考坐标系的方位等效于动坐标系绕某个等效转轴转动一个角度θ，如果用u表示等效转轴方向的单位矢量，则动坐标系的方位完全由u和θ两个参数来确定。

用u和θ可构造一个四元数：

对上式求导并化简可得四元数微分方程：

式中

根据毕卡逼近法求解四元数微分方程得：

式中

式中

令地球坐标系相对惯性坐标系的自转角速度为ω_ie，(其值为15.04088°/h)，L表示当地纬度,λ表示当地经度，则

ω_ie ⁿ：地球坐标系相对惯性坐标系的自转角速度在地理坐标系中的矢量，为：

ω_ie ^b：地球坐标系相对惯性坐标系的自转角速度在载体坐标系中的矢量，为：

式中的姿态矩阵在载体静止时，由初始角度决定；当载体相对地理坐标系转动时，姿态矩阵跟着变化，由四元数即时修正后求得(下同)。

ω_en ⁿ:地理坐标相对地球坐标系转动角速度在地理坐标系中的矢量，为：

V_E、V_N分别为载体运动的东向和北向速度；

R_N为参考椭球体子午面内的曲率半径，R_N＝R_e(1-2e+3e sin²L)；

R_E为垂直子午面的法线平面内的曲率半径，R_E＝R_e(1+e sin²L)；

其中R_e为参考椭球体的长轴半径；e为椭球的椭圆度。

又因为,则

ω_en ^b:地理坐标相对地球坐标系转动角速度在载体坐标系中的矢量，为：

ω_ib ^b：陀螺输出角速度，记为

ω_nb ^b：载体坐标系相对地理坐标系的转动角速度在载体坐标系中的矢量，记为

则可得

ω_nb ^b＝ω_ib ^b-ω_ie ^b-ω_en ^b

四、元数即时修正后，根据下式可由四元数的元实时更新姿态矩阵

从姿态阵中即可提取实时姿态角

(2)速度计算

得到的载体坐标系中的比力矢量为f^b，则地理坐标系中有：

式中的方向余弦矩阵在载体静止时，由初始角度决定；当载体相对地理坐标系转动时，方向余弦矩阵跟着变化，由四元数即时修正后求得。

载体在惯导系内的比力方程为：

写成分量形式有：

式中：fⁿ为载体加速度在导航坐标系上的投影，fⁿ＝[f_E f_N f_U]^T；Vⁿ表示船体在导航坐标系中的速度矢量，Vⁿ＝[V_E V_N V_U]^T；gⁿ为重力加速度矢量，gⁿ＝[0 0 -g]^T。积分上式，即可求得运载体在导航坐标系上的各个速度分量V_E、V_N、V_U。

(3)位置解算

得到经纬度的微分方程可表示如下：

式中，h为高度。

积分上式的经纬度的更新公式即可得到经纬度：

则得到位置P(λ,L,h)。

步骤(4)中根据位置信息P_SINS和海底声源位置信息，计算出SINS的斜距与到达距离差，具体实现方式如下：

(1)图3为SINS计算斜距与到达距离差示意图，假设水听器基阵安装位置所在的坐标系为载体坐标系，已知水下声源的绝对地理坐标参考位置为P_re(X_re Y_re Z_re)，水听器1到水听器4在载体坐标系下的位置分别为

(2)由捷联惯性导航系统解算的水下航行器位置为P_SINS(X_SINS Y_SINS Z_SINS)，由坐标转换关系可得水听器1在导航坐标系下的位置为P_SINS1(X_SINS1 Y_SINS1 Z_SINS1)，其中， C_ij为载体坐标系到导航坐标系的变换矩阵中的元素，同理可得其他水听器在导航坐标系下的位置分别为P_SINS2(X_SINS2 Y_SINS2 Z_SINS2)，P_SINS3(X_SINS3 Y_SINS3 Z_SINS3)，P_SINS4(X_SINS4 Y_SINS4 Z_SINS4)

(3)根据变换的位置信息和海底声源位置信息，可得SINS的斜距与到达距离差为：

五、步骤(5)中USBL输出斜距和到达距离差与SINS计算出的斜距和到达距离差位置进行卡尔曼滤波，滤波输出校正SINS位置输出,具体实现方式如下：

(1)建立的USBL斜距与到达距离差误差模型及组合导航系统状态方程。受水下噪声、混响、声传播多途效应、多普勒效应、声速非线性分布等因素的影响，以及时延差测量的误差影响，将会引起斜距与到达距离差的测量误差，为简化模型，可将斜距误差和到达距离差误差用一阶马尔科夫过程表示，则斜距误差可表示为：

式中，R为距离真值，δR为距离测量误差，τ_δR，ω_δR分别是一阶马尔科夫过程的相关时间和驱动白噪声，υ_R为高斯白噪声。

到达距离差误差可表示为：

式中，△R为到达距离差真值，δ△R为到达距离差测量误差，τ_δ△R，ω_δ△R分别是一阶马尔科夫过程的相关时间和驱动白噪声，υ_△R为高斯白噪声。

组合导航系统的状态方程描述为：

式中，X_SINS、X_USBL分别为捷联惯性导航系统和超短基线定位系统的状态变量，F_SINS、F_USBL分别为捷联惯性导航系统和超短基线定位系统的状态转移矩阵，W_SINS、W_USBL分别为捷联惯性导航系统和超短基线定位系统的系统噪声。

根据捷联惯性导航系统长期工作时的误差特点，选择位置误差、速度误差、姿态误差、陀螺漂移和加速度计零偏作为状态量

其中，δV_EδV_NδV_U分别是捷联惯性导航系统东向、北向、天向的速度误差，φ_Eφ_Nφ_U分别是捷联惯性导航系统东向、北向、天向的失准角，δLδλδh分别是捷联惯性导航系统纬度、经度、高度误差，为捷联惯性导航系统加速度计三个轴向的偏置误差，ε_bxε_byε_bz是捷联惯性导航系统陀螺的三个轴向漂移。F_SINS可由捷联惯性导航系统误差方程确定。

其中：

F₄₆＝-ω_iesin L

C_ij为姿态转移矩阵的元素

选择超短基线定位系统的斜距误差和到达距离差误差作为状态变量：

X_USBL＝[δR_USBL δ△R_USBL13 δ△R_USBL24]^T

式中，δR_USBL为超短基线系统的斜距误差，δ△R_USBL13、δ△R_USBL24为声源信号到达两个水听器之间的距离差。

(2)组合导航系统量测方程

Z＝HX+V

观测量为USBL输出斜距和到达距离差与SINS计算的斜距和到达距离差的差值。

由USBL斜距与到达距离差误差模型可知，

R_USBL＝R+δR_USBL+υ_R

△R_USBL13＝△R₁₃+δ△R_USBL13+υ_△R13

△R_USBL24＝△R₂₄+δ△R_USBL24+υ_△R24

由SINS计算的斜距与到达距离差为：

于是观测量为：

将上式进行泰勒线性化得：

R_SINS-R_USBL＝a₁δx+a₂δy+a₃δz-δR_USBL-υ_R

△R_SINS13-△R_USBL13＝b₁δx+b₂δy+b₃δz-δ△R_USBL13-υ_△R13

△R_SINS24-△R_USBL24＝c₁δx+c₂δy+c₃δz-δ△R_USBL24-υ_△R24

式中，

其中，

将上式中的δx,δy,δz用δL,δλ,δh表示为：

可得观测矩阵H＝[0_3×6 H′_3×3 0_3×6 -E_3×3]

式中，

H′_3×3中元素如下：

h₁₁＝-(R_N+h)sin L cosλa₁-(R_N+h)sin L sinλa₂+[R_N(1-e²)+h]cos La₃

h₂₁＝-(R_N+h)sin L cosλb₁-(R_N+h)sin L sinλb₂+[R_N(1-e²)+h]cos Lb₃

h₃₁＝-(R_N+h)sin L cosλc₁-(R_N+h)sin L sinλc₂+[R_N(1-e²)+h]cos Lc₃

h₁₂＝-(R_N+h)cos L sinλa₁-(R_N+h)cos L cosλa₂

h₂₂＝-(R_N+h)cos L sinλb₁-(R_N+h)cos L cosλb₂

h₃₂＝-(R_N+h)cos L sinλc₁-(R_N+h)cos L cosλc₂

h₁₃＝cos L cosλa₁+cos L sinλa₂+sin La₃

h₂₃＝cos L cosλb₁+cos L sinλb₂+sin Lb₃

h₃₃＝cos L cosλc₁+cos L sinλc₂+sin Lc₃

(3)系统状态方程及量测方程的离散化

Z_k＝H_kX_k+V_k

式中，X_k为k时刻的状态向量，也就是被估计矢量；Z_k为k时刻的测量序列；W_k-1为k-1时刻的系统噪声；V_k为k时刻的测量噪声序列；Φ_k,k-1为k-1时刻到k时刻的一步状态转移矩阵；Γ_k-1是系统噪声输入矩阵，H_k为k时刻的测量矩阵，

(4)利用标准卡尔曼滤波方程计算状态的最优估计

状态一步预测向量

状态估值计算

X_k＝X_k/k-1+K_k(Z_k-H_kX_k/k-1)

滤波增益

K_k＝P_k/k-1H_k ^T(H_kP_k/k-1H_k ^T+R_k)^-1

一步预测均方误差矩阵

估计均方误差方程

(5)校正SINS

利用当前的误差最优估计可以立即校正SINS每次的由测量数据得到的状态量。

位置校正可以通过惯导系统对参数的估计值与估计误差简单相减来修正：

式中，X_c是校正后的状态量。

Claims (4)

1.一种基于SINS/USBL紧组合的AUV水下导航定位方法，其特征在于：通过将捷联惯性导航系统SINS和超短基线系统USBL组合，进而利用卡尔曼滤波方法完成组合导航，其具体步骤如下：

(1)超短基线由安装在AUV上的5个十字形水听器基阵和布放在海底的声源组成，水听器0安装在基阵中心原点，水听器1和水听器3安装在X轴上且距离为d₁₃，水听器2和水听器4安装在Y轴上且距离为d₂₄，水听器基阵接收声源的信号，通过同步时钟得到声波到达水听器基阵中心水听器0的传播时间，通过频域加权互相关分别得到声波到达水听器基阵X和Y坐标轴上两个水听器的时延差；

(2)根据得到的传播时间和时延差，计算得到USBL的斜距与到达距离差；

(3)捷联惯性导航系统SINS通过捷联解算得到自主水下航行器AUV的位置信息P_SINS；

(4)根据步骤(3)所得位置信息P_SINS和海底声源位置信息，计算出SINS的斜距与到达距离差；

(5)将USBL的斜距和到达距离差与SINS的斜距和到达距离差位置进行卡尔曼滤波，滤波输出校正SINS位置输出。

2.根据权利要求1所述的基于SINS/USBL紧组合的AUV水下导航定位方法，其特征在于，所述步骤(1)中频域加权互相关获取时延差通的具体步骤为：

(11)设定海底声源的发射信号为x(t),t为时间，第i个水听器接收到的信号为x_i＝a_ix(t-τ_i)+n_i(t)，第j个水听器接收到的信号为x_j＝a_jx(t-τ_j)+n_j(t)，其中a_i,a_j为声信号在水中传播的衰减系数，n_i(t),n_j(t)为噪声信号，τ_i,τ_j为传播时间，i，j取值组合为(1，3)和(2,4)；

(12)x_i和x_j的互相关函数为其中，τ＝τ_i-τ_j表示两个声波的到达时间差，T表示观测时间；

(13)根据维纳-辛钦定理，互功率谱和互相关函数之间关系为：ω为频率，G_ij(ω)是x_i和x_j之间的互功率谱；

(14)选择频域加权函数对信号进行加权滤波，以此来抑制噪声的影响，再通过傅里叶逆变换到时域得到广义互相关函数，即：

(5)找出相关峰函数图像最大值对应的横坐标，对应的横坐标值为时延差τ。

3.根据权利要求1所述的基于SINS/USBL紧组合的AUV水下导航定位方法，其特征在于，所述步骤(2)的具体步骤如下：

(21)声学基阵由安装在基阵中心和坐标轴上的5个十字形水听器基阵组成，海底声源地理坐标已知；

(22)由声波到达基阵中心水听器0的传播时间t，可得USBL斜距为R_USBL＝ct，c为水中声速；

(23)由时延差τ计算USBL到达距离差为△R_USBL13＝cτ₁₃，△R_USBL24＝cτ₂₄，τ₁₃为声源信号到X轴水听器1和水听器3之间的时延差，τ₂₄为声源信号到Y轴水听器2和水听器4之间的时延差。

4.根据权利要求1所述的基于SINS/USBL紧组合的AUV水下导航定位方法，其特征在于，所述步骤(4)的具体步骤为：

(41)假设水听器基阵安装位置所在的坐标系为载体坐标系，已知水下声源的绝对地理坐标参考位置是P_re(X_re Y_re Z_re)，水听器1到水听器4在载体坐标系下的位置分别是

(42)由捷联惯性导航系统解算的水下航行器位置为P_SINS(X_SINS Y_SINS Z_SINS)；

由坐标转换关系可得水听器1在导航坐标系下的位置为P_SINS1(X_SINS1 Y_SINS1 Z_SINS1)；

其中，C_ij为载体坐标系到导航坐标系的变换矩阵中的元素；

同理可得其他水听器在导航坐标系下的位置分别为：

P_SINS2(X_SINS2 Y_SINS2 Z_SINS2)；

P_SINS3(X_SINS3 Y_SINS3 Z_SINS3)；

P_SINS4(X_SINS4 Y_SINS4 Z_SINS4)；

(43)根据变换的位置信息和海底声源位置信息，可得SINS的斜距与到达距离差为：

$R_{SINS}=\sqrt{{(X_{SINS}-X_{re})}^2+{(Y_{SINS}-Y_{re})}^2+{(Z_{SINS}-Z_{re})}^2}$

$\begin{array}{c}{\mathrm{\ΔR}}_{SINS13}=\sqrt{{(X_{SINS1}-X_{re})}^2+{(X_{SINS1}-X_{re})}^2+{(Z_{SINS1}-Z_{re})}^2}\\ -\sqrt{{(X_{SINS3}-X_{re})}^2+{(Y_{SINS3}-Y_{re})}^2+{(Z_{SINS3}-Z_{re})}^2}\end{array}$

$\begin{array}{c}{\mathrm{\ΔR}}_{SINS24}=\sqrt{{(X_{SINS2}-X_{re})}^2+{(X_{SINS2}-X_{re})}^2+{(Z_{SINS2}-Z_{re})}^2}\\ -\sqrt{{(X_{SINS4}-X_{re})}^2+{(Y_{SINS4}-Y_{re})}^2+{(Z_{SINS4}-Z_{re})}^2}\end{array}.$