CN104459707A - 一种水下拖体航位推算系统初始位置在线获取方法 - Google Patents

Abstract

一种水下拖体航位推算系统初始位置在线获取方法，用于在线确定直线航行状态下深海水下拖体航位推算(Dead Reckoning，DR)系统的初始位置。该方法综合了DR系统和水声定位系统的特点，通过平移初始定位时间段内DR轨迹，得到了在确定运动趋势约束下初始定位时间段内的最优航行轨迹，并依据此轨迹求出DR系统初始位置。首先采集并存储初始定位时间段内水声定位系统、DVL、罗经输出数据，进行数据预处理；然后利用DVL、罗经输出数据进行航位推算，得到DR系统航行轨迹；再平移DR系统轨迹，直至水声定位系统位置数据到该直线距离平方和最小，得到初始定位时间段内的航行轨迹；最后依据该轨迹得出DR系统的初始位置。

Description

一种水下拖体航位推算系统初始位置在线获取方法

技术领域

本发明涉及一种水下拖体航位推算系统初始位置在线获取方法，用于在线确定直线航行状态下深海水下拖体DR系统的初始位置，特别适用于水下拖体直线航行运动过程中在线确定DR系统初始位置。

背景技术

深海拖曳系统可以在近海底条件下进行海底地形地貌勘探和海底视像观察和记录，是调查深海矿产资源最经济有效的手段。基于声学原理的水声定位系统、DVL和基于惯性器件原理的陀螺罗经是水下拖体常用的导航子系统：水声定位系统输出水下拖体位置信息，波动较大；对罗经和DVL输出数据进行航位推算计算也可以得到拖体的位置信息，但是其位置误差随时间积累而发散。由于不同导航方法具有不同优缺点，采用单一导航手段不能得到很好的定位效果。航位推算(Dead Reckoning,DR)/水声定位系统组合定位既能抑制DR系统随着时间增加由误差累积引起的误差发散，又能平滑水声定位系统输出位置的波动。在水声定位系统短时间失效的情况下，可以依靠DR进行导航定位。DR系统的初始位置精度影响DR系统性能，也影响DR/水声组合定位系统导航数据的性能。在对姿态和速度平稳性有较高要求的水下精确测绘等领域，对DR系统的初始位置精度要求更高。

水下导航信息贫乏，通常只能以水声定位系统输出的位置信息作为DR系统的初始位置。水声定位系统是根据声波在水底应答器和载体接收机间传播的时间来确定距离进而进行定位的，受声速误差、航行噪声和载体摇摆等影响，整体数据的优劣程度不一，表现为数据波动较大。所以，需要从一段时间内水声定位系统输出的位置信息中计算提取出DR系统需要的初始位置。

发明内容

本发明的技术解决问题是：克服水声定位系统直接提供DR系统初始位置信息误差波动大的不足，提出一种水下拖体航位推算系统初始位置在线获取方法，该方法综合了DR系统和水声定位系统的特点，通过平移DR航行轨迹，得到了在确定运动趋势约束下的最优轨迹，并依据轨迹求出初始位置。水下运载体高度信息由深度计提供，所以实际应用时本方法只考虑二维平面内的初始位置信息，即经、纬度信息。

本发明的技术解决方案为：水下拖体航位推算系统初始位置在线获取方法，其特点在于通过下列步骤实现：

(1)在线采集并存储初始定位时间段内深海水下拖体的水声定位传感器、罗经和DVL的输出数据，其中初始定位时间根据实际情况确定，通常采用1～2分钟；

(2)对上述数据进行数据预处理；

(3)利用水声定位系统初始位置、罗经和DVL的输出数据进行航位推算，得到DR系统航行轨迹；

(4)平移DR系统航行轨迹，直至水声定位系统位置数据到该直线距离平方和最小，得到了在确定运动趋势约束下的最优航行轨迹；

(5)依据该航行轨迹获得初始位置。

所述步骤(2)中的数据预处理方法为：首先对水声定位系统、罗经和DVL输出数据，应用3σ准则进行去野值运算。然后根据时间标签对上述三种传感器的输出数据进行时间对齐、频率匹配和单位统一。

所述步骤(3)中的DR方法为：通过罗经提供的姿态信息和DVL提供的速度信息可以计算出水下拖体载体系下的速度在导航系下的投影，进而通过积分运算计算得出水下拖体的位置信息，具体计算方法如下：

利用罗经测得的姿态信息，采用方向余弦矩阵可以描述载体系到导航系的转换关系，则有：

$C_b^n=\left[\begin{array}{ccc}\cos \left(\mathrm{\γ}\right)\×\cos \left(\mathrm{\ψ}\right)-\sin \left(\mathrm{\γ}\right)\×\sin \left(\mathrm{\θ}\right)\×\sin \left(\mathrm{\ψ}\right)& \cos \left(\mathrm{\γ}\right)\×\sin \left(\mathrm{\γ}\right)+\sin \left(\mathrm{\γ}\right)\×\sin \left(\mathrm{\θ}\right)\×\cos \left(\mathrm{\ψ}\right)& -\sin \left(\mathrm{\γ}\right)\×\cos \left(\mathrm{\θ}\right)\\ -\cos \left(\mathrm{\θ}\right)\×\sin \left(\mathrm{\ψ}\right)& \cos \left(\mathrm{\θ}\right)\×\cos \left(\mathrm{\ψ}\right)& \sin \left(\mathrm{\θ}\right)\\ \sin \left(\mathrm{\γ}\right)\×\cos \left(\mathrm{\ψ}\right)+\cos \mathrm{\γ})\×\sin \left(\mathrm{\θ}\right)\×\sin \left(\mathrm{\ψ}\right)& \sin \left(\mathrm{\γ}\right)\×\sin \left(\mathrm{\ψ}\right)-\cos \left(\mathrm{\γ}\right)\×\sin \left(\mathrm{\θ}\right)\×\cos \left(\mathrm{\ψ}\right)& \cos \left(\mathrm{\γ}\right)\×\cos \left(\mathrm{\θ}\right)\end{array}\right]$

将DVL测得的载体系下的速度信息投影到导航系下，得到拖体在导航系下东向、北向、天向速度为Vⁿ＝[V_E V_N V_U]^T：

$V^n=\left[\begin{array}{c}{V}_E\\ V_N\\ V_U\end{array}\right]=C_b^n\·V^b=C_b^n\·\left[\begin{array}{c}{V}_x\\ V_y\\ V_z\end{array}\right]$

在导航系下，分别在东向、北向和垂向对相应速度进行积分，就可以得到载体的位置信息，进而得到DR系统航行轨迹，因此，在导航系下，载体的所在位置为：

$\left\{\begin{array}{c}{\mathrm{lat}}_{k+1}={\mathrm{lat}}_k+\frac{V_N\·T}{R_m}\\ {\mathrm{lon}}_{k+1}={\mathrm{lon}}_k+\frac{V_E\·T}{R_n\·\cos \left({\mathrm{lat}}_k\right)}\\ {\mathrm{height}}_{k+1}={\mathrm{height}}_k+V_U\·T\end{array}\right.$

所述步骤(4)中的轨迹平移方法为：首先对上述DR系统航行水平面内航行轨迹进行最小二乘直线拟合，拟合方法如下所示：

DR解算出一组经度、纬度数据分别为：

$\left\{\begin{array}{c}({\mathrm{lat}}_1,1),({\mathrm{lat}}_2,2),\·\·\·,({\mathrm{lat}}_n,n)\\ ({\mathrm{lon}}_1,1),({\mathrm{lon}}_2,2),\·\·\·,({\mathrm{lon}}_n,n)\end{array}\right.$

分别对经度、纬度进行最小二乘拟合，假设最小二乘拟合直线函数为：

$\left\{\begin{array}{c}\mathrm{lat}=a_1\×k+b_1\\ \mathrm{lon}=a_2\×k+b_2\end{array}\right.$

分别计算a₁、a₂、b₁、b₂使得计算得出的经、纬度与实测的经、纬度离差的平方和最小：

$\left\{\begin{array}{c}{a}_1=\frac{n\underset{k=1}{\overset{n}{\mathrm{\Σ}}}{k\×\mathrm{lat}}_k-\underset{k=1}{\overset{n}{\mathrm{\Σ}}}k\×\underset{k=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{lat}}_k}{n\underset{k=1}{\overset{n}{\mathrm{\Σ}}}{k}^2-{\left(\underset{k=1}{\overset{n}{\mathrm{\Σ}}}k\right)}^2}\\ b_1=(\underset{k=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{lat}}_k-a_1\×\underset{k=1}{\overset{n}{\mathrm{\Σ}}}k)/n\end{array}\right.$

$\left\{\begin{array}{c}{a}_2=\frac{n\underset{k=1}{\overset{n}{\mathrm{\Σ}}}{k\×\mathrm{lon}}_k-\underset{k=1}{\overset{n}{\mathrm{\Σ}}}k\×\underset{k=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{lon}}_k}{n\underset{k=1}{\overset{n}{\mathrm{\Σ}}}{k}^2-{\left(\underset{k=1}{\overset{n}{\mathrm{\Σ}}}k\right)}^2}\\ b_2=(\underset{k=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{lon}}_k-a_2\×\underset{k=1}{\overset{n}{\mathrm{\Σ}}}k)/n\end{array}\right.$

将计算得到的a₁、a₂、b₁、b₂带入拟合直线函数中，即可得到拟合直线。

然后在水声定位系统数据和DR系统数据拟合直线距离平方和最小准则下，对DR系统计算出的航行轨迹进行平移，即保持上述拟合直线的斜率，调整其截距，直至水声定位系统位置数据到该直线距离平方和最小的判断准则实现后，提取得到的轨迹即为在统计意义下与真实轨迹最接近的轨迹，该轨迹的末尾点作为后续DR系统的初始位置。

本发明的原理是：首先以水声定位系统USBL数据初始点作为初始位置，利用罗经提供的姿态信息和DVL提供的速度信息，进行航位推算得到水下运载体在初始定位时间段内的航行轨迹。罗经输出的姿态信息在短时间内精度较高，所以进行航位推算得到的位置数据能真实反映水下载体的运动趋势和运动轨迹的形状，此运动趋势与真实运动轨迹相比误差较小。但由于水声定位系统提供的初始位置具有随机性，所以进行航位推算计算的轨迹相对真实轨迹在位置上是有偏移的，但将DR计算得到的轨迹进行一定的平移则会得到与真实轨迹比较接近的运行轨迹。为得到航行轨迹的准确初始位置，本方法在水声定位系统数据和DR系统数据拟合直线距离平方和最小准则下，对航位推算系统计算出的航行轨迹进行平移，直至水声定位系统位置数据到该直线距离平方和最小的判断准则实现后，提取得到的轨迹即为在统计意义下与真实轨迹最接近的航行轨迹。而该轨迹的末尾点即为我们所求的初始位置。

本发明与现有技术相比的优点在于：

(1)综合了水声定位系统、罗经、DVL三种传感器的不同特点，获取更准确的初始位置。水声定位系统在水下环境会产生常值误差和较大随机误差，定位数据波动大，受到声速误差、航行噪声和载体摇摆等影响，所以整体数据的优劣程度不一，可能会出现错误帧或漏帧的情况，直接以水声定位系统的起始点作为初始位置引入较大误差。罗经作为姿态测量装置，可以连续输出精度较高的水下拖体的方向、姿态信息。DVL是根据多普勒效应进行速度检测的传感器，可以获得水下拖体在载体系下的速度信息。

(2)以水声定位系统的起始点为初始位置对采集并存储的初始定位时间段内的罗经、DVL数据进行航位推算，获得水下拖体的运动趋势。罗经输出的姿态信息在短时间内精度较高，所以用罗经和DVL进行航位推算得到的位置数据能真实反映水下载体的运动趋势和运动轨迹的形状，此运动趋势与真实运动轨迹相比误差较小。

(3)在水声定位系统数据和DR系统数据拟合直线距离平方和最小准则下，对航位推算系统计算出的航行轨迹进行平移，直至水声定位系统位置数据到该直线距离平方和最小的判断准则实现后，提取得到的轨迹即为在统计意义下与真实轨迹最接近的航行轨迹，该轨迹的末尾点作为后续DR系统的初始位置。

附图说明

图1为本发明涉及水下拖体航位推算系统初始位置在线获取方法流程图；

图2为本发明涉及航位推算示意图。

具体实施方式

本发明技术解决方案的具体实施步骤如图1所示，航位推算示意图如图2所示，具体实施步骤如下：

1、在线采集并存储初始定位时间段内的深海水下拖体的水声定位传感器、DVL、罗经输出数据。

水下拖体在需要进行导航定位时开始采集并存储水声定位传感器、DVL、罗经输出数据，存储时间即初始定位时间根据实际情况确定，与传感器的输出频率有关，通常采用1～2分钟，为确定初始位置所用。

2、对上述数据进行数据预处理。

首先对水声定位系统、罗经和DVL输出数据，应用3σ准则进行去野值运算。然后根据时间标签对上述三种传感器的输出数据进行时间对齐、频率匹配和单位统一。

3、利用水声定位系统初始位置、DVL和罗经的输出数据进行航位推算，得到DR航行轨迹。

通过罗经提供的姿态信息计算水下拖体对应载体系到导航系的姿态转移矩阵：

$C_b^n=\left[\begin{array}{ccc}\cos \left(\mathrm{\γ}\right)\×\cos \left(\mathrm{\ψ}\right)-\sin \left(\mathrm{\γ}\right)\×\sin \left(\mathrm{\θ}\right)\×\sin \left(\mathrm{\ψ}\right)& \cos \left(\mathrm{\γ}\right)\×\sin \left(\mathrm{\γ}\right)+\sin \left(\mathrm{\γ}\right)\×\sin \left(\mathrm{\θ}\right)\×\cos \left(\mathrm{\ψ}\right)& -\sin \left(\mathrm{\γ}\right)\×\cos \left(\mathrm{\θ}\right)\\ -\cos \left(\mathrm{\θ}\right)\×\sin \left(\mathrm{\ψ}\right)& \cos \left(\mathrm{\θ}\right)\×\cos \left(\mathrm{\ψ}\right)& \sin \left(\mathrm{\θ}\right)\\ \sin \left(\mathrm{\γ}\right)\×\cos \left(\mathrm{\ψ}\right)+\cos \mathrm{\γ})\×\sin \left(\mathrm{\θ}\right)\×\sin \left(\mathrm{\ψ}\right)& \sin \left(\mathrm{\γ}\right)\×\sin \left(\mathrm{\ψ}\right)-\cos \left(\mathrm{\γ}\right)\×\sin \left(\mathrm{\θ}\right)\×\cos \left(\mathrm{\ψ}\right)& \cos \left(\mathrm{\γ}\right)\×\cos \left(\mathrm{\θ}\right)\end{array}\right]---\left(1\right)$

根据姿态转移矩阵和DVL提供的速度信息计算出拖体载体系下的速度在导航系下的投影：

$V^n=\left[\begin{array}{c}{V}_E\\ V_N\\ V_U\end{array}\right]=C_b^n\·V^b=C_b^n\·\left[\begin{array}{c}{V}_x\\ V_y\\ V_z\end{array}\right]---\left(2\right)$

通过积分运算计算得出位置信息：

$\left\{\begin{array}{c}{\mathrm{lat}}_{k+1}={\mathrm{lat}}_k+\frac{V_N\·T}{R_m}\\ {\mathrm{lon}}_{k+1}={\mathrm{lon}}_k+\frac{V_E\·T}{R_n\·\cos \left({\mathrm{lat}}_k\right)}\\ {\mathrm{height}}_{k+1}={\mathrm{height}}_k+V_U\·T\end{array}\right.---\left(3\right)$

4、平移DR轨迹，直至水声定位系统位置数据到该直线距离平方和最小，得到了在确定运动趋势约束下的最优轨迹。

首先对上述DR航行轨迹进行最小二乘直线拟合，拟合方法如下所示：

航位推算解算出一组经度、纬度数据分别为：

$\left\{\begin{array}{c}({\mathrm{lat}}_1,1),({\mathrm{lat}}_2,2),\·\·\·,({\mathrm{lat}}_n,n)\\ ({\mathrm{lon}}_1,1),({\mathrm{lon}}_2,2),\·\·\·,({\mathrm{lon}}_n,n)\end{array}\right.---\left(4\right)$

分别对经度、纬度进行最小二乘拟合，假设最小二乘拟合直线函数为：

$\left\{\begin{array}{c}\mathrm{lat}=a_1\×k+b_1\\ \mathrm{lon}=a_2\×k+b_2\end{array}\right.---\left(5\right)$

分别计算a₁、a₂、b₁、b₂使得计算得出的经、纬度与实测的经、纬度离差的平方和最小：

$\left\{\begin{array}{c}{a}_1=\frac{n\underset{k=1}{\overset{n}{\mathrm{\Σ}}}{k\×\mathrm{lat}}_k-\underset{k=1}{\overset{n}{\mathrm{\Σ}}}k\×\underset{k=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{lat}}_k}{n\underset{k=1}{\overset{n}{\mathrm{\Σ}}}{k}^2-{\left(\underset{k=1}{\overset{n}{\mathrm{\Σ}}}k\right)}^2}\\ b_1=(\underset{k=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{lat}}_k-a_1\×\underset{k=1}{\overset{n}{\mathrm{\Σ}}}k)/n\end{array}\right.---\left(6\right)$

$\left\{\begin{array}{c}{a}_2=\frac{n\underset{k=1}{\overset{n}{\mathrm{\Σ}}}{k\×\mathrm{lon}}_k-\underset{k=1}{\overset{n}{\mathrm{\Σ}}}k\×\underset{k=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{lon}}_k}{n\underset{k=1}{\overset{n}{\mathrm{\Σ}}}{k}^2-{\left(\underset{k=1}{\overset{n}{\mathrm{\Σ}}}k\right)}^2}\\ b_2=(\underset{k=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{lon}}_k-a_2\×\underset{k=1}{\overset{n}{\mathrm{\Σ}}}k)/n\end{array}\right.---\left(7\right)$

将计算得到的a₁、a₂、b₁、b₂带入式(5)，即可得到拟合直线。

然后在水声定位系统数据和DR系统数据拟合直线距离平方和最小准则下，对DR系统计算出的轨迹进行平移，即保持上述拟合直线的斜率，调整其截距，直至水声定位系统位置数据到该直线距离平方和最小的判断准则实现后，提取得到的轨迹即为在统计意义下与真实轨迹最接近的航行轨迹。

5、选取该航行轨迹数据的末尾点作为航位推算系统初始位置。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (4)

1.一种水下拖体航位推算系统初始位置在线获取方法，其特点在于：

(1)在线采集并存储初始定位时间段内深海水下拖体的水声定位传感器、罗经和DVL的输出数据，其中初始定位时间根据实际情况确定，通常采用1～2分钟；

(2)对上述数据进行数据预处理；

(3)利用水声定位系统初始位置、罗经和DVL的输出数据进行航位推算，得到DR系统航行轨迹；

(4)平移DR系统航行轨迹，直至水声定位系统位置数据到该直线距离平方和最小，得到了在确定运动趋势约束下的最优航行轨迹；

(5)依据该航行轨迹获得初始位置。

2.根据权利要求1所述的一种水下拖体航位推算系统初始位置在线获取方法，其特征在于：综合了水声定位系统、罗经、DVL三种传感器的不同特点，获取更准确的初始位置。水声定位系统在水下环境会产生常值误差和较大随机误差，定位数据波动大，受到声速误差、航行噪声和载体摇摆等影响，所以整体数据的优劣程度不一，可能会出现错误帧或漏帧的情况，直接以水声定位系统的起始点作为初始位置引入较大误差。罗经作为姿态测量装置，可以连续输出精度较高的水下拖体的方向、姿态信息。DVL是根据多普勒效应进行速度检测的传感器，可以获得水下拖体在载体系下的速度信息。

3.根据权利要求1所述的一种水下拖体航位推算系统初始位置在线获取方法，其特征在于：以水声定位系统的起始点为初始位置对采集并存储的初始定位时间段内的罗经、DVL数据进行航位推算，获得水下拖体的运动趋势。罗经输出的姿态信息在短时间内精度较高，所以用罗经和DVL进行航位推算得到的位置数据能真实反映水下载体的运动趋势和运动轨迹的形状，此运动趋势与真实运动轨迹相比误差较小。

4.根据权利要求1所述的一种水下拖体航位推算系统初始位置在线获取方法，其特征在于：在水声定位系统数据和DR系统数据拟合直线距离平方和最小准则下，对航位推算系统计算出的航行轨迹进行平移，直至水声定位系统位置数据到该直线距离平方和最小的判断准则实现后，提取得到的轨迹即为在统计意义下与真实轨迹最接近的航行轨迹，该轨迹的末尾点作为后续航位推算的初始位置。