CN104197939A - 基于水下信息网络的多参考节点水下航行器组合导航方法 - Google Patents

Abstract

本发明公开了一种基于水下信息网络的多参考节点水下航行器组合导航方法的实现。该方法首先在海水中或者海底部署多个参考网络节点，节点之间通过水声modem进行通信。该方法利用位置已知的多个水下网络节点作为参考节点，通过测定待测目标与多个参考节点之间的距离，从而解算出水下航行器的位置信息，实现水下航行器的定位，然后利用计算所得的位置信息来修正水下航行器配置的捷联惯导/多普勒测速导航系统的累积误差，从而实现高精度的组合导航。本发明可以用来定时修正该导航系统的累积误差，通过预先设定好修正周期，从而在保证水下航行器隐蔽性以及自主性的基础上，实现水下航行器的长时间高精度组合导航，具有较高的实际应用价值。

Description

基于水下信息网络的多参考节点水下航行器组合导航方法

技术领域

本发明是关于导航定位技术，尤其是关于一种基于水下信息网络的多参考节点水下航行器组合导航系统以及导航定位方法。

背景技术

海洋可以说是人类生存和可持续发展的重要领域，对于海洋的利用以及开发已经成为决定国家兴衰的基本因素之一。在海洋的利用以及开发等领域中，由于无线电波在水下衰减比较严重，无法满足水下通信的要求，从而以声波为载体的水声通信技术得到了极其广泛的发展以及应用。

近几十年来，水下信息网络作为无线传感器网络技术应用的一大分支，以水声通信和网络技术为基础，在水声技术的研究领域中进入到全新的发展阶段。水下信息网络，就是部署在水下环境中，以声波作为信息载体、水声信道作为通信信道的无线传感器网络，是陆地无线传感器网络向水下应用的延伸。在水下部署传感器网络节点，一方面可以检测水下环境，进行水下信息的收集，另一方面可以把已知位置的传感器网络节点作为参考节点，用来对该海域中的水下航行器等进行定位，从而辅助水下航行器实现高精度导航。

要使水下航行器完成一些预定的任务以及使命，自然离不开水下导航技术。和陆地导航以及空中导航相比，水下导航具有工作时间长、隐蔽性要求高、环境复杂以及信息源少等特点，因此相对于陆地导航以及空中导航，水下导航的研究和应用具有更大的难度。

目前，将水下信息网络和目标定位技术结合在一起，是水声定位技术的新兴研究方向。对检测区域的目标进行定位是水下信息网络众多应用的一个关键技术，也是其他应用的基础。

本发明中，把多个已知位置信息的水下信息网络节点作为参考节点，用来对该海域中的水下航行器进行定位，并利用定位结果来定时修正水下航行器SINS/DVL导航系统的累积误差，从而实现水下航行器长时间的高精度自主导航。

利用水下信息网络节点来实现监测海域的目标定位，同时与水下航行器自身配置的SINS/DVL导航系统相结合，从而修正SINS/DVL导航系统的累积误差，实现高精度的组合导航，这在水下导航定位研究中，有着长足的意义。

发明内容

本发明的技术解决问题是：克服现有技术的不足，提供一种长时间的高精度自主组合导航方法，该方法能够大大提高长时间航行的水下航行器的导航精度，并具有自主性。

本发明为实现上述发明目的采用如下技术方案：

一种基于水下信息网络的多参考节点水下航行器组合导航方法，包括如下步骤：

A，水下航行器在水下航行全程中通过自身装配的SINS/DVL导航系统进行导航。SINS/DVL导航系统可以对水下航行器进行短时间的高精度导航定位，但是随着时间的推移，存在一定的累积误差。对于长时间航行的水下航行器，SINS/DVL导航系统存在一定的导航偏差，需要定时引入其他定位系统进行辅助导航定位，从而修正此导航系统的导航累积误差，实现长时间的高精度自主导航。

其中，SINS/DVL导航系统原理为：

综合SINS系统与DVL系统的误差模型，建立SINS/DVL系统的状态方程以及量测方程，利用卡尔曼滤波进行该系统的最优估计，从而实现SINS/DVL导航系统的导航。

B，由于长时间航行之后，SINS/DVL导航系统存在一定的累积误差，故需要定时对其进行辅助定位，利用辅助定位结果修正导航累积误差，定位过程如下：

水下航行器航行过程中，每隔一段时间，通过水下航行器上的测距平台，与多个参考网络节点之间完成通信测距，从而获得时延差，算出距离信息，利用这多个距离信息，解算出水下航行器的位置信息，实现水下航行器的定位。

步骤B中的一种通信测距的方法，本发明中采用双向通信测距的方法完成参考点与待测点之间的水声测距，具体步骤为：

首先，待测点水声modem产生由信息码和同步头信号组成的定位脉冲，经调制后，向水下参考点(位置已知)发射此定位脉冲信号，此发送信号中包含发送时间t1，当水下参考点收到该信号时，进行同步解调，并利用同步过程得到信号传输时延差τ1，并把τ1和收到的信号一起重新调制，变频回发给待测节点作为应答，此回发信号中还包含回发时间t3和参考点的深度信息，待测点换能器收到此回发信号时，再进行同步解调，得到此次的传输时延差τ2，同时解调得到τ1，在通信测距过程中，信号传输时延差τ1、τ2，通过LFM信号求相关检测求得。

设信号发射端与接收端的时间误差为Δτ，发送节点与接收节点之间的单向时延差为τ。由信号处理过程，可得如下时间关系：

$\left\{\begin{array}{c}{\mathrm{\τ}}_1=\mathrm{\τ}+\mathrm{\Δ\τ}\\ {\mathrm{\τ}}_2=\mathrm{\τ}-\mathrm{\Δ\τ}\end{array}\right.---\left(1\right)$

由上式可得：

$\left\{\begin{array}{c}\mathrm{\τ}=\frac{{\mathrm{\τ}}_1+{\mathrm{\τ}}_2}{2}\\ \mathrm{\Δ\τ}=\frac{{\mathrm{\τ}}_1-{\mathrm{\τ}}_2}{2}\end{array}\right.---\left(2\right)$

再将得到的时延差τ，乘以该水域的等效平均声速，得到两点之间的距离，完成测距。

步骤B中的定位原理，具体步骤为：

假设有n个已知的参考点位置信息(n>3)，参考点采用统一的时钟系统。故待测目标与参考网络节点之间的距离可以表示为：

d_i＝cτ_i    (3)

i＝1,2,…,n

其中，d_i表示待测目标与第i个参考点之间的距离，c为声波在水中的等效平均传播速度，τ_i为待测目标与第i个参考点之间的时延差，通过双向通信测距测得。又有：

d_i ²＝(x_i-x)²+(y_i-y)²+(z_i-z)²    (4)

其中，(x_i,y_i,z_i)为第i个参考点的坐标，为已知量，(x,y,z)为待测目标的坐标，z坐标值通过深度传感器测得。将式(3)代入式(4)中，同时展开，得：

$\left\{\begin{array}{c}{c}^2{\mathrm{\τ}}_1^2={(x_1-x)}^2+{(y_1-y)}^2+{(z_1-z)}^2\\ c^2{\mathrm{\τ}}_2^2={(x_2-x)}^2+{(y_2-y)}^2+{(z_2-z)}^2\\ c^2{\mathrm{\τ}}_3^2={(x_3-x)}^2+{(y_3-y)}^2+{(z_3-z)}^2\\ .\\ .\\ .\\ c^2{\mathrm{\τ}}_n^2={(x_n-x)}^2+{\left(y_{n-}y\right)}^2+{(z_n-z)}^2\end{array}\right.---\left(5\right)$

将式(5)括号展开后，并用式(5)中的后面n-1项分别减去第一项，整理之后，可以得到如下线性方程组：

$\left\{\begin{array}{c}2(x_2-x_1)x+2(y_2-y_1)y=x_2^2-x_1^2+y_2^2-y_1^2+{(z_2-z)}^2-{(z_1-z)}^2-c^2({\mathrm{\τ}}_2^2-{\mathrm{\τ}}_1^2)\\ 2(x_3-x_1)x+2(y_3-y_1)y=x_3^2-x_1^2+y_3^2-y_1^2+{(z_3-z)}^2-{(z_1-z)}^2-c^2({\mathrm{\τ}}_3^2-{\mathrm{\τ}}_1^2)\\ .\\ .\\ .\\ 2(x_n-x_1)x+2(y_n-y_1)y=x_n^2-x_1^2+y_n^2-y_1^2+{(z_n-z)}^2-{(z_1-z)}^2-c^2({\mathrm{\τ}}_n^2-{\mathrm{\τ}}_1^2)\end{array}\right.---\left(6\right)$

整理，得如下表达式：

$\left[\begin{array}{c}2(x_2-x_1)2(y_2-y_1)\\ 2(x_3-x_1)2(y_3-y_1)\\ .\\ .\\ .\\ 2(x_n-x_1)2(y_n-y_1)\end{array}\right]\left[\begin{array}{c}x\\ y\end{array}\right]=\left[\begin{array}{c}{x}_2^2-x_1^2+y_2^2-y_1^2+{(z_2-z)}^2-{(z_1-z)}^2-c^2({\mathrm{\τ}}_2^2-{\mathrm{\τ}}_1^2)\\ x_3^2-x_1^2+y_3^2-y_1^2+{(z_3-z)}^2-{(z_1-z)}^2-c^2({\mathrm{\τ}}_3^2-{\mathrm{\τ}}_1^2)\\ .\\ .\\ .\\ x_n^2-x_1^2+y_n^2-y_1^2+{(z_n-z)}^2-{(z_1-z)}^2-c^2({\mathrm{\τ}}_n^2-{\mathrm{\τ}}_1^2)\end{array}\right]---\left(7\right)$

将其写成矩阵形式：

CX＝D    (8)

其中：

X＝[x y]^T    (9)

$C=\left[\begin{array}{c}2(x_2-x_1)2(y_2-y_1)\\ 2(x_3-x_1)2(y_3-y_1)\\ .\\ .\\ .\\ 2(x_n-x_1)2(y_n-y_1)\end{array}\right]---\left(10\right)$

$D=\left[\begin{array}{c}{x}_2^2-x_1^2+y_2^2-y_1^2+{(z_2-z)}^2-{(z_1-z)}^2-c^2({\mathrm{\τ}}_2^2-{\mathrm{\τ}}_1^2)\\ x_3^2-x_1^2+y_3^2-y_1^2+{(z_3-z)}^2-{(z_1-z)}^2-c^2({\mathrm{\τ}}_3^2-{\mathrm{\τ}}_1^2)\\ .\\ .\\ .\\ x_n^2-x_1^2+y_n^2-y_1^2+{(z_n-z)}^2-{(z_1-z)}^2-c^2({\mathrm{\τ}}_n^2-{\mathrm{\τ}}_1^2)\end{array}\right]---\left(11\right)$

因为方程组CX＝D中，C中的变量为已知信息，可以认为不存在误差扰动，而D中的时延差变量τ需要通过测量得到，存在误差扰动，满足最小二乘的前提条件，故可以利用最小二乘法，得到方程组的解为：

X＝(C^TC)^-1C^TD    (12)

通过以上公式，可以由一组实测时延差，以及参考点的位置信息，从而求得待测点的位置信息，实现定位。

C，定时对水下航行器进行定位运算，利用定位所得的位置信息来修正水下航行器配置的SINS/DVL导航系统的累积误差，从而实现高精度的组合导航。

利用SINS/DVL导航系统进行导航，长时间航行之后累积误差增大，定时对水下航行器进行定位运算，利用定位过程求得的待测点的位置信息对SINS/DVL导航系统的累积误差进行修正，从而实现高精度的自主组合导航。

与现有技术相比，本发明具有如下优点及显著效果：

对于目前常用的SINS/DVL导航系统，可以对水下航行器进行短时间的高精度导航定位，但是随着时间的推移，存在一定的累积误差。对于长时间航行的水下航行器，SINS/DVL导航系统存在一定的导航偏差，需要定时引入其他定位系统进行辅助定位，从而修正此系统导航累积误差，这样才能保证水下航行器的长时间高精度导航。本发明提出的一种基于水下信息网络的多参考节点水下航行器组合导航方法，在SINS/DVL导航的基础上，定时利用多个水下信息网络参考节点，通过测定待测目标与多个参考节点之间的距离来进行定位，利用定位结果修正SINS/DVL导航系统的累积误差，从而在保证水下航行器隐蔽性的基础上，实现水下航行器的长时间高精度自主组合导航。本发明易实现，具有较高的实际应用价值。

附图说明

图1是真实轨迹与SINS/DVL解算轨迹仿真图；

图2是本发明的多节点定位示意图；

图3是本发明的双向通信测距示意图；

图4是LFM信号的相关检测包络输出；

图5是组合导航修正轨迹对比图。

具体实施方案

下面结合附图对本发明的技术方案做进一步详细说明。

如图1所示，真实轨迹与SINS/DVL解算轨迹仿真图。仿真参数为：水下航行器导航坐标系初始坐标为(0,0,20)，初始速度为3m/s。初始角度为：航向角40度，俯仰角0度，横滚角0度。陀螺仪和加速度计每隔1s进行一次采样，DVL每隔60s进行一次速度修正，总共航行时间为4h。惯导仪器与DVL的仿真误差参数为：陀螺仪常值漂移误差0.1°/h，陀螺仪常值噪声误差0.05°/h，加速度计零偏误差10^-4g，加速度计噪声误差10^-5g，DVL偏移误差0.05m/s，DVL刻度系数误差0.01。

水下航行器通过自身装配的SINS/DVL导航系统进行导航。通过仿真图可以看出，SINS/DVL导航系统可以对水下航行器进行短时间的高精度导航定位，但是随着时间的推移，存在一定的累积误差。对于长时间航行的水下航行器，SINS/DVL导航系统存在一定的导航偏差，需要定时引入其他定位系统进行辅助导航定位，从而修正此导航系统的导航累积误差，实现长时间的高精度自主导航。

如图2所示，本发明的多节点定位示意图。首先在海水中或者海底部署多个参考网络节点，参考节点之间一般距离几千米，节点之间通过水声modem进行通信。通过测定待测目标与多个参考节点之间的距离，从而解算出水下航行器的位置信息，实现水下航行器的定位。。本发明以4个参考网络节点为例，来进行定位示意图的展示以及组合导航的仿真。

如图3所示，本发明的双向通信测距示意图。在定位过程中，需要测量待测点与参考点之间的距离，本发明利用双向通信测距来测得待测点与参考点之间的距离。具体过程为：

首先，待测点水声modem产生由信息码和同步头信号组成的定位脉冲，经调制后，向水下参考点(位置已知)发射此定位脉冲信号，此发送信号中包含发送时间t₁，当水下参考点收到该信号时，进行同步解调，并利用同步过程得到信应答，此回发信号中还包含回发时间t3和参考点的深度信息，待测号传输时延差τ1，并把τ1和收到的信号一起重新调制，变频回发给待测节点作为点换能器收到此回发信号时，再进行同步解调，得到此次的传输时延差τ2，同时解调得到τ1。

设信号发射端与接收端的时间误差为Δτ，发送节点与接收节点之间的单向时延差为τ。由信号处理过程，可得如下时间关系：

$\left\{\begin{array}{c}{\mathrm{\τ}}_1=\mathrm{\τ}+\mathrm{\Δ\τ}\\ {\mathrm{\τ}}_2=\mathrm{\τ}-\mathrm{\Δ\τ}\end{array}\right.---\left(1\right)$

由上式可得：

$\left\{\begin{array}{c}\mathrm{\τ}=\frac{{\mathrm{\τ}}_1+{\mathrm{\τ}}_2}{2}\\ \mathrm{\Δ\τ}=\frac{{\mathrm{\τ}}_1-{\mathrm{\τ}}_2}{2}\end{array}\right.---\left(2\right)$

再将得到的时延差τ，乘以该水域的等效平均声速，得到两点之间的距离，完成测距。

如图4所示，LFM信号的相关检测包络输出。LFM信号的仿真参数为：f₀＝4kHz，f_s＝20kHz，B＝5kHz，T＝400ms，采样点数N＝8000，接收信号信噪比为15dB，传输信道为水声多径信道。在通信测距过程中，信号传输时延差τ₁、τ₂，通过LFM信号求相关检测求得。

如图5所示，组合导航修正轨迹对比图。利用以上信息，在某一时刻对水下航行器进行定位，定位原理具体步骤为：

假设有n个已知的参考点位置信息(n>3)，参考点采用统一的时钟系统。故待测目标与参考网络节点之间的距离可以表示为：

d_i＝cτ_i    (3)

i＝1,2,…,n

其中，d_i表示待测目标与第i个参考点之间的距离，c为声波在水中的等效平均传播速度，τ_i为待测目标与第i个参考点之间的时延差，通过双向通信测距测得。又有：

d_i ²＝(x_i-x)²+(y_i-y)²+(z_i-z)²    (4)

其中，(x_i,y_i,z_i)为第i个参考点的坐标，为已知量，(x,y,z)为待测目标的坐标，z坐标值通过深度传感器测得。将式(3)代入式(4)中，同时展开，得：

$\left\{\begin{array}{c}{c}^2{\mathrm{\τ}}_1^2={(x_1-x)}^2+{(y_1-y)}^2+{(z_1-z)}^2\\ c^2{\mathrm{\τ}}_2^2={(x_2-x)}^2+{(y_2-y)}^2+{(z_2-z)}^2\\ c^2{\mathrm{\τ}}_3^2={(x_3-x)}^2+{(y_3-y)}^2+{(z_3-z)}^2\\ .\\ .\\ .\\ c^2{\mathrm{\τ}}_n^2={(x_n-x)}^2+{\left(y_{n-}y\right)}^2+{(z_n-z)}^2\end{array}\right.---\left(5\right)$

将式(5)括号展开后，并用式(5)中的后面n-1项分别减去第一项，整理之后，可以得到如下线性方程组：

$\left\{\begin{array}{c}2(x_2-x_1)x+2(y_2-y_1)y=x_2^2-x_1^2+y_2^2-y_1^2+{(z_2-z)}^2-{(z_1-z)}^2-c^2({\mathrm{\τ}}_2^2-{\mathrm{\τ}}_1^2)\\ 2(x_3-x_1)x+2(y_3-y_1)y=x_3^2-x_1^2+y_3^2-y_1^2+{(z_3-z)}^2-{(z_1-z)}^2-c^2({\mathrm{\τ}}_3^2-{\mathrm{\τ}}_1^2)\\ .\\ .\\ .\\ 2(x_n-x_1)x+2(y_n-y_1)y=x_n^2-x_1^2+y_n^2-y_1^2+{(z_n-z)}^2-{(z_1-z)}^2-c^2({\mathrm{\τ}}_n^2-{\mathrm{\τ}}_1^2)\end{array}\right.---\left(6\right)$

整理，得如下表达式：

$\left[\begin{array}{c}2(x_2-x_1)2(y_2-y_1)\\ 2(x_3-x_1)2(y_3-y_1)\\ .\\ .\\ .\\ 2(x_n-x_1)2(y_n-y_1)\end{array}\right]\left[\begin{array}{c}x\\ y\end{array}\right]=\left[\begin{array}{c}{x}_2^2-x_1^2+y_2^2-y_1^2+{(z_2-z)}^2-{(z_1-z)}^2-c^2({\mathrm{\τ}}_2^2-{\mathrm{\τ}}_1^2)\\ x_3^2-x_1^2+y_3^2-y_1^2+{(z_3-z)}^2-{(z_1-z)}^2-c^2({\mathrm{\τ}}_3^2-{\mathrm{\τ}}_1^2)\\ .\\ .\\ .\\ x_n^2-x_1^2+y_n^2-y_1^2+{(z_n-z)}^2-{(z_1-z)}^2-c^2({\mathrm{\τ}}_n^2-{\mathrm{\τ}}_1^2)\end{array}\right]---\left(7\right)$

将其写成矩阵形式：

CX＝D    (8)

其中：

X＝[x y]^T    (9)

$C=\left[\begin{array}{c}2(x_2-x_1)2(y_2-y_1)\\ 2(x_3-x_1)2(y_3-y_1)\\ .\\ .\\ .\\ 2(x_n-x_1)2(y_n-y_1)\end{array}\right]---\left(10\right)$

$D=\left[\begin{array}{c}{x}_2^2-x_1^2+y_2^2-y_1^2+{(z_2-z)}^2-{(z_1-z)}^2-c^2({\mathrm{\τ}}_2^2-{\mathrm{\τ}}_1^2)\\ x_3^2-x_1^2+y_3^2-y_1^2+{(z_3-z)}^2-{(z_1-z)}^2-c^2({\mathrm{\τ}}_3^2-{\mathrm{\τ}}_1^2)\\ .\\ .\\ .\\ x_n^2-x_1^2+y_n^2-y_1^2+{(z_n-z)}^2-{(z_1-z)}^2-c^2({\mathrm{\τ}}_n^2-{\mathrm{\τ}}_1^2)\end{array}\right]---\left(11\right)$

因为方程组CX＝D中，C中的变量为已知信息，可以认为不存在误差扰动，而D中的时延差变量τ需要通过测量得到，存在误差扰动，满足最小二乘的前提条件，故可以利用最小二乘法，得到方程组的解为：

X＝(C^TC)^-1C^TD    (12)

通过以上公式，可以由一组实测时延差，以及参考点的位置信息，从而求得待测点的位置信息，实现定位。

水下航行器航行长时间航行之后累积误差增大，定时对水下航行器进行定位，定位过程完成之后，利用定位过程求得的待测点的位置信息，对SINS/DVL导航系统的累积误差进行修正，从而实现高精度的自主组合导航。

图5中的仿真参数为：SINS/DVL导航参数与图1参数相同，总航行时间为4h，每隔1.5h进行一次定时误差修正。选用4个参考网络节点，坐标为：(5000,10000,40)，(15000,14000,40)，(15000,2000,40)，(25000,8000,40)，假设定位过程中定位的时候只考虑测时误差和声线弯曲。通过图5的仿真可以看出：本发明研究的基于水下信息网络的多参考节点水下航行器组合导航技术，可以用来定时修正SINS/DVL导航系统的累积误差，从而在保证水下航行器隐蔽性以及自主性的基础上，实现水下航行器的长时间高精度组合导航。

Claims (5)

1.一种基于水下信息网络的多参考节点水下航行器组合导航方法，其包括如下步骤：

A，水下航行器在水下航行全程中通过自身装配的捷联惯导/多普勒测速SINS/DVL导航系统进行粗导航；

B，定时对所述水下航行器进行辅助定位，所述定位过程如下：

水下航行器航行过程中，每隔一段时间，通过水下航行器上的测距平台，与多个参考网络节点之间完成通信测距，从而获得时延差，算出距离信息，利用这多个距离信息，解算出水下航行器的位置信息，实现水下航行器的定位；

C，定时对水下航行器进行定位运算，利用定位所得的位置信息来修正水下航行器配置的SINS/DVL导航系统的累积误差，从而实现高精度的组合导航。

2.根据权利要求1所述的一种基于水下信息网络的多参考节点水下航行器组合导航方法，其特征在于：

所述步骤A中的SINS/DVL导航系统是综合SINS系统与DVL系统的误差模型，建立SINS/DVL系统的状态方程以及量测方程，利用卡尔曼滤波进行该系统的最优估计，从而实现SINS/DVL导航系统的粗导航。

3.根据权利要求1所述的一种基于水下信息网络的多参考节点水下航行器组合导航方法，其特征在于：

所述步骤B中的通信测距的具体步骤包括：

待测点水声modem产生由信息码和同步头信号组成的定位脉冲，经调制后，向水下参考点发射此定位脉冲信号，其中所述水下参考点的位置已知，此发送信号中包含发送时间t₁；

当水下参考点收到该信号时，进行同步解调，并利用同步过程得到信号传输时延差τ₁，并把τ₁和收到的信号一起重新调制，变频回发给待测节点作为应答，此回发信号中还包含回发时间t₃和参考点的深度信息；

待测点换能器收到此回发信号时，再进行同步解调，得到此次的传输时延差τ₂，同时解调得到τ₁；

在通信测距过程中，信号传输时延差τ₁、τ₂，通过LFM信号求相关检测求得；

设信号发射端与接收端的时间误差为Δτ，发送节点与接收节点之间的单向时延差为τ，通过信号处理过程，可得如下时间关系：

$\left\{\begin{array}{c}{\mathrm{\τ}}_1=\mathrm{\τ}+\mathrm{\Δ\τ}\\ {\mathrm{\τ}}_2=\mathrm{\τ}-\mathrm{\Δ\τ}\end{array}\right.---\left(1\right)$

由上式可得：

$\left\{\begin{array}{c}\mathrm{\τ}=\frac{{\mathrm{\τ}}_1+{\mathrm{\τ}}_2}{2}\\ \mathrm{\Δ\τ}=\frac{{\mathrm{\τ}}_1-{\mathrm{\τ}}_2}{2}\end{array}\right.---\left(2\right)$

再将得到的时延差τ，乘以该水域的等效平均声速，得到两点之间的距离，完成测距。

4.根据权利要求1所述的一种基于水下信息网络的多参考节点水下航行器组合导航方法，其特征在于：

所述步骤B中的定位过程，具体包括以下步骤：

假设有n个已知的参考点位置信息，其中n>3，参考点采用统一的时钟系统，故待测目标与参考网络节点之间的距离可以表示为：

d_i＝cτ_i    (3)

i＝1,2,…,n

其中，d_i表示待测目标与第i个参考点之间的距离，c为声波在水中的等效平均传播速度，τ_i为待测目标与第i个参考点之间的时延差，通过双向通信测距测得，又有：

d_i ²＝(x_i-x)²+(y_i-y)²+(z_i-z)²    (4)

其中，(x_i,y_i,z_i)为第i个参考点的坐标，为已知量，(x,y,z)为待测目标的坐标，z坐标值通过深度传感器测得，将式(3)代入式(4)中，同时展开，得：

$\left\{\begin{array}{c}{c}^2{\mathrm{\τ}}_1^2={(x_1-x)}^2+{(y_1-y)}^2+{(z_1-z)}^2\\ c^2{\mathrm{\τ}}_2^2={(x_2-x)}^2+{(y_2-y)}^2+{(z_2-z)}^2\\ c^2{\mathrm{\τ}}_3^2={(x_3-x)}^2+{(y_3-y)}^2+{(z_3-z)}^2\\ .\\ .\\ .\\ c^2{\mathrm{\τ}}_n^2={(x_n-x)}^2+{\left(y_{n-}y\right)}^2+{(z_n-z)}^2\end{array}\right.---\left(5\right)$

将式(5)括号展开后，并用式(5)中的后面n-1项分别减去第一项，整理之后，可以得到如下线性方程组：

$\left\{\begin{array}{c}2(x_2-x_1)x+2(y_2-y_1)y=x_2^2-x_1^2+y_2^2-y_1^2+{(z_2-z)}^2-{(z_1-z)}^2-c^2({\mathrm{\τ}}_2^2-{\mathrm{\τ}}_1^2)\\ 2(x_3-x_1)x+2(y_3-y_1)y=x_3^2-x_1^2+y_3^2-y_1^2+{(z_3-z)}^2-{(z_1-z)}^2-c^2({\mathrm{\τ}}_3^2-{\mathrm{\τ}}_1^2)\\ .\\ .\\ .\\ 2(x_n-x_1)x+2(y_n-y_1)y=x_n^2-x_1^2+y_n^2-y_1^2+{(z_n-z)}^2-{(z_1-z)}^2-c^2({\mathrm{\τ}}_n^2-{\mathrm{\τ}}_1^2)\end{array}\right.---\left(6\right)$

整理，得如下表达式：

$\left[\begin{array}{c}2(x_2-x_1)2(y_2-y_1)\\ 2(x_3-x_1)2(y_3-y_1)\\ .\\ .\\ .\\ 2(x_n-x_1)2(y_n-y_1)\end{array}\right]\left[\begin{array}{c}x\\ y\end{array}\right]=\left[\begin{array}{c}{x}_2^2-x_1^2+y_2^2-y_1^2+{(z_2-z)}^2-{(z_1-z)}^2-c^2({\mathrm{\τ}}_2^2-{\mathrm{\τ}}_1^2)\\ x_3^2-x_1^2+y_3^2-y_1^2+{(z_3-z)}^2-{(z_1-z)}^2-c^2({\mathrm{\τ}}_3^2-{\mathrm{\τ}}_1^2)\\ .\\ .\\ .\\ x_n^2-x_1^2+y_n^2-y_1^2+{(z_n-z)}^2-{(z_1-z)}^2-c^2({\mathrm{\τ}}_n^2-{\mathrm{\τ}}_1^2)\end{array}\right]---\left(7\right)$

将其写成矩阵形式：

CX＝D    (8)

其中：

X＝[x y]^T    (9)

$C=\left[\begin{array}{c}2(x_2-x_1)2(y_2-y_1)\\ 2(x_3-x_1)2(y_3-y_1)\\ .\\ .\\ .\\ 2(x_n-x_1)2(y_n-y_1)\end{array}\right]---\left(10\right)$

$D=\left[\begin{array}{c}{x}_2^2-x_1^2+y_2^2-y_1^2+{(z_2-z)}^2-{(z_1-z)}^2-c^2({\mathrm{\τ}}_2^2-{\mathrm{\τ}}_1^2)\\ x_3^2-x_1^2+y_3^2-y_1^2+{(z_3-z)}^2-{(z_1-z)}^2-c^2({\mathrm{\τ}}_3^2-{\mathrm{\τ}}_1^2)\\ .\\ .\\ .\\ x_n^2-x_1^2+y_n^2-y_1^2+{(z_n-z)}^2-{(z_1-z)}^2-c^2({\mathrm{\τ}}_n^2-{\mathrm{\τ}}_1^2)\end{array}\right]---\left(11\right)$

因为方程组CX＝D中，C中的变量为已知信息，可以认为不存在误差扰动，而D中的时延差变量τ需要通过测量得到，存在误差扰动，满足最小二乘的前提条件，故可以利用最小二乘法，得到方程组的解为：

X＝(C^TC)^-1C^TD    (12)

通过以上公式，可以由一组实测时延差，以及参考点的位置信息，从而求得待测点的位置信息，实现定位。

5.根据权利要求1所述的一种基于水下信息网络的多参考节点水下航行器组合导航方法，其特征在于：

步骤C中定时修正SINS/DVL导航系统的累积误差，具体包括以下步骤：

利用SINS/DVL导航系统进行导航，长时间航行之后累积误差增大，定时对水下航行器进行定位运算，利用定位过程求得的待测点的位置信息对SINS/DVL导航系统的累积误差进行修正，从而实现高精度的自主组合导航。