CN1325931C - 无固定参考点的载人潜水器在惯性坐标系中的定位方法 - Google Patents

Abstract

本发明是涉及载人潜水器在海洋中定位技术，具体的说是一种无固定参考点的载人潜水器在惯性坐标系中的定位方法。本发明先确定一个固定坐标，然后确定某时刻的母船和载人潜水器的惯性坐标，载人潜水器这个时刻的位置作为初始值(新的固定坐标系原点)，进行载人潜水器的航迹推算，然后把载人潜水器的每点坐标变换到惯性坐标系坐标。本发明解决了以前因投放固定信标而增加了作业难度、作业时间和作业风险问题。解决了使用固定水声信标，跟踪载人潜水器范围小的问题。

Description

无固定参考点的载人潜水器在惯性坐标系中的定位方法

技术领域

本发明是涉及载人潜水器在海洋中定位技术，具体的说是一种无固定参考点的载人潜水器在惯性坐标系中的定位方法，它在载人潜水器无海底或海面固定参考坐标时，确定载人潜水器在惯性坐标系中的坐标值。

技术背景

载人潜水器在海洋中运动时，不仅需要实时确定载人潜水器和作业母船之间的相对位置，还需要实时确定载人潜水器在惯性坐标系中的坐标。以往确定载人潜水器在惯性坐标系中的坐标，需要在海底或海面投放一个固定位置的声学水声应答信标，首先通过GPS(全球定位系统)接收机和母船上超短基线定位系统的水声接收基阵，确定固定水声应答信标在惯性坐标系中的坐标，然后测量载人潜水器和固定水声应答信标的相对位置，再把载人潜水器与固定水声应答信标之间相对位置的坐标换算成在惯性坐标系中的坐标。

声学水声定位系统主要有三种形式：长基线、短基线、超短基线。

长基线水声定位系统是在海底一定范围内相隔数千米，布置3只以上水声应答器，组成长基线水声接收基阵。在作业母船安装一只水声接收器，在水下载人潜水器上安装一只水声应答信标，测定母船相对于载人潜水器的位置以及载人潜水器与水声信标的相对位置。长基线水声定位系统定位精度高，不受母船移动的影响，监测范围大。但是作业时，在海底水声应答器的布阵过程很复杂，水声应答器在回收的过程中易丢失，损失会很大。

短基线定位系统是在母船上间隔十米以上安装3只水声接收器，组成一个水声接收基阵。在载人潜水器上安装一只水声应答信标，即可在水上监视载人潜水器在水中的位置。在海底投放一只位置固定的水声应答信标，可以确定载人潜水器相对于固定水声应答信标的位置。但是，短基线定位系统在母船上安装3只水声接收器，就要在母船底打三个洞或安装延长支架，对船的航行和维护带来麻烦，所以不是专业的作业母船不能安装短基线定位系统。

超短基线定位系统是在母船上安装一个水声接收基阵。水声接收基阵上有5个接收换能器按圆环型布置，相互间隔1厘米左右。在圆环的中间有一个发射换能器。在载人潜水器上安装一只水声应答信标，水声接收基阵可以安装在船侧弦，而不需改造作业船。同样在载人潜水器上安装一只水声应答信标，在海底投放一只位置固定水声应答信标。通过母船上的GPS和超短基线定位系统，确定母船惯性坐标系中的坐标及载人潜水器相对于母船和固定信标之间的相对位置，就可以确定载人潜水器在惯性坐标系中的坐标。超短基线定位系统在安装操作上都很方便。所以在载人潜水器的定位中，常用超短基线定位声纳。但是超短基线定位系统测量范围小，载人潜水器只能在固定水声信标的附近运动。

发明内容

为了避免投放固定水声信标的麻烦和解决跟踪范围小的问题，本发明提供一种全新的无固定参考点的载人潜水器在惯性坐标系中的定位方法。它通过确定载人潜水器起始点的惯性坐标和对载人潜水器航迹推算，实现大范围的载人潜水器惯性坐标的确定。

为了实现上述目的，本发明的技术方案是：确定固定坐标系(E-XYZ)，其中母船为原点E，EZ指向地心，水平面采用北东坐标系；定义动坐标系(o-xyz)，动坐标系与载人潜水器固联在一起；载人潜水器动坐标系和固定坐标系间的变换可以表示为：

$\stackrel{.}{\mathrm{\η}}=J\left(\mathrm{\η}\right)V---\left(1\right)$

其中： $\mathrm{\η}={[X,Y,Z]}^T,V={[u,v,w]}^T,J\left(\mathrm{\η}\right)=\left[\begin{array}{ccc}\cos \mathrm{\θ}\·\cos \mathrm{\ψ}& -\sin \mathrm{\θ}& 0\\ \sin \mathrm{\θ}\·\cos \mathrm{\ψ}& \cos \mathrm{\θ}& 0\\ 0& 0& 1\end{array}\right],$ X、Y、Z表示载人潜水器在固定坐标系中的坐标，u、v、w表示载人潜水器沿动坐标系三轴的速度，θ表示载人潜水器的航向角，φ表示载人潜水器纵倾角；

将固定坐标转化到惯性坐标系中，用经纬度表示载人潜水器的坐标；载人潜水器在惯性坐标系中的经纬度坐标值为：

其中，(λ₀，L₀)是作业母船起始位置的经纬度坐标，由母船上的GPS接收机接收，X，Y是载人潜水器在固定坐标系中的坐标值，R为地球半径，(λ，L)是载人潜水器在惯性坐标系中的坐标；

以某一时刻载人潜水器在惯性坐标系中的经纬度坐标(λ₁，L₁)作为载人潜水器航迹推算的初始值，以这点为新的固定坐标系原点(0，0)，用载人潜水器的坐标变换式(1)进行航迹推算，求出载人潜水器在新的固定坐标中坐标值(x，y)，再用公式(2)求出载人潜水器在惯性坐标系中的经纬度坐标(λ，L)；

所述某一时刻载人潜水器在惯性坐标系中初始值的经纬度坐标(λ₁，L₁)是通过所述公式(2)计算得到的；

求载人潜水器在惯性坐标系中的经纬度坐标(λ，L)时，公式(2)中(X，Y)取(x，y)，(λ₀，L₀)取(λ₁，L₁)。

本发明工作原理为：先确定一个固定坐标，然后确定某时刻的母船和载人潜水器的惯性坐标，载人潜水器这个时刻的位置作为初始值，进行载人潜水器的航迹推算，然后把载人潜水器的每点坐标变换到惯性坐标系坐标。

本发明具有如下优点：

1.本发明解决了以前因投放固定信标而增加了作业难度、作业时间和作业风险问题。

2.在7000米的海域投放水声信标，很难保证信标进入指定的范围之内，投放完固定水声信标，还要对信标的位置精确测量，这都需要很多时间，要保证天黑之前完成，作业对海况的要求也很高。在载人潜水器作业完后，要成功的回收固定水声信标，由于深海浪高流急，发现和成功回收信标都很困难，如果回收不成功，就要有几十万的损失。本发明不需要投放水下固定信标，避免上述的损失和麻烦。

3.本发明解决了使用固定水声信标，跟踪载人潜水器范围小的问题。

4.水声信标的信号范围有限，而以前的方法(如使用超短基线定位系统和固定信标法)，可以确定的惯性坐标的范围小，可跟踪载人潜水器的活动范围也有限，要求载人潜水器在固定水声信标的信号范围内，才能实现对载人潜水器坐标的确定。采用本发明不用投放固定水声信标，理论上可以无限范围内实时确定载人潜水器的惯性坐标，也避免了丢失水声信标的可能。

5.本发明提出的载人潜水器的定位方法，同样适用于其它的水下机器人，包括自治水下机器人(AUV)和遥控水下机器人(ROV)。

附图说明

图1为本发明固定坐标系和动坐标系示意图。

具体实施方式

下面对本发明作进一步说明。

本发明采用超短基线定位系统确定载人潜水器在惯性坐标系中的坐标，不用在海面或海底投放固定水声信标。具体是：

首先要确定一个固定坐标系(E-XYZ)，原点为E。其中，EZ指向地心，水平面采用北东坐标系，本实施例取母船为固定坐标系的原点E。此外还要定义动坐标系(o-xyz)，动坐标系与载人潜水器固联在一起。如图1所示，原则上，动坐标系原点和坐标轴的方向可以任意选取，为了方便起见，本实施例将ox轴与载人潜水器的主对称轴取向一致，而oy轴与辅助对称轴取向一致，沿动坐标系三轴的速度分别用u，v，w表示，假设载人潜水器的航向角用θ表示，由于载人潜水器可以有纵倾角运动，所以要考虑纵倾角的影响，纵倾角用φ表示，在坐标推算中不考虑载人潜水器横摇的影响，载人潜水器动坐标系和定坐标系间的变换可以表示为：

$\stackrel{.}{\mathrm{\η}}=J\left(\mathrm{\η}\right)V---\left(1\right)$

其中， $\mathrm{\η}={[X,Y,Z]}^T,V={[u,v,w]}^T,J\left(\mathrm{\η}\right)=\left[\begin{array}{ccc}\cos \mathrm{\θ}\·\cos \mathrm{\ψ}& -\sin \mathrm{\θ}& 0\\ \sin \mathrm{\θ}\·\cos \mathrm{\ψ}& \cos \mathrm{\θ}& 0\\ 0& 0& 1\end{array}\right];$

X、Y、Z表示载人潜水器在固定坐标系中的坐标，u、v、w表示载人潜水器沿动坐标系三轴的速度，θ表示载人潜水器的航向角，φ表示载人潜水器纵倾角。

由于载人潜水器运动过程中母船是跟随风浪随机移动的，没有相对固定的参考点，只能计算出每一时刻载人潜水器与母船的相对位置。所以本发明把载人潜水器的固定坐标转化到惯性坐标系中，用经纬度表示载人潜水器的坐标，实现准确的对载人潜水器的航行位置确定和记录。载人潜水器在惯性坐标系中的经纬度坐标值入，L由公式(2)得知：

其中，(λ₀，L₀)是作业母船起始位置的经纬度坐标，X，Y是载人潜水器在固定坐标系中的坐标值，即母船与载人潜水器的相对距离，R为地球半径。(λ，L)是载人潜水器在惯性坐标系中的坐标。

用公式(2)可以由载人潜水器相对作业母船的位置值(载人潜水器在固定坐标系中的坐标)X，Y，求出载人潜水器在惯性坐标系中的经纬度坐标值(λ，L)。

载人潜水器在惯性坐标系中经纬度坐标(λ，L)的推算过程如下：

首先由母船上的GPS接收机接收到母船某一个时刻的经纬度坐标(λ₀，L₀)。母船这一个时刻的位置作为固定坐标系中的原点，由超短基线定位声纳测量载人潜水器在固定坐标系中的坐标值(X，Y)，用公式(2)计算出载人潜水器这个时刻的经纬度坐标(λ₁，L₁)，以这点为新的固定坐标系原点(0，0)，同时作为载人潜水器航迹推算的初始值。在确定了载人潜水器的航迹推算初始值后，开始用载人潜水器的坐标变换式(1)进行航迹推算，求出载人潜水器在新的固定坐标中坐标值(x，y)。再通过公式(2)求出每一个时刻载人潜水器经纬度坐标(λ，L)，这时公式中的坐标(λ₀，L₀)用(λ₁，L₁)代替，(X，Y)用(x，y)代替。

载人潜水器航迹推算的具体过程：本实施例知道K时刻的载人潜水器在固定坐标的位置为x_k，y_k，动坐标系中的速度值为u(k)，v(k)，载人潜水器在固定坐标系中的航向角为θ，纵倾角为φ，位置时延为T_s本实施

例通过公式(1)得公式(3)，推算下一时刻的位置x_k+1，y_k+1：

x_k+1＝x_k+[u(k)cosθ(k)cosψ(k)-v(k)sinθ]Ts

                                                     (3)

y_k+1＝y_k+[u(k)sinθ(k)cosψ(k)+v(k)cosθ]Ts

其中，x_k，y_k为k时刻载人潜水器在固定坐标中的位置估计(k＝0，1，2，3......)，u(k)，v(k)是载人潜水器在水平面的两维运动速度，θ是载人潜水器的航向角，φ是载人潜水器的纵倾角；载人潜水器的经纬度坐标数据每隔T_s秒数据刷新一次。

所以通过公式(3)就可以推算出载人潜水器每时刻在新的固定坐标中的位置值。

用公式(2)可以求出每一时刻载人潜水器在惯性坐标系中的经纬度坐标。这时公式中的坐标(λ₀，L₀)用(λ₁，L₁)代替，(X，Y)用(x_k，y_k)代替。在载人潜水器确定初始值或复位值时，载人潜水器要处于相对静止的状态，以减少由于超短基线和水声通讯等延时产生的误差。母船在接收到某时刻可靠的母船经纬度(λ₀，L₀)和载人潜水器在固定坐标系中的坐标值(X，Y)后，就可以移动了。

本发明提出的载人潜水器导航定位方法，同样适用于其它的水下机器人，包括自治水下机器人(AUV)和遥控水下机器人(ROV)。

Claims (3)

1.一种无固定参考点的载人潜水器在惯性坐标系中的定位方法，其特征在于：确定固定坐标系(E-XYZ)，其中母船为原点E，EZ指向地心，水平面采用北东坐标系；定义动坐标系(o-xyz)，动坐标系与载人潜水器固联在一起；载人潜水器动坐标系和定坐标系间的变换可以表示为：

$\stackrel{.}{\mathrm{\η}}=J\left(\mathrm{\η}\right)V$ (1)

其中：η＝[X，Y，Z]^T，V＝[u，v，w]^T， $J\left(\mathrm{\η}\right)=\left[\begin{array}{ccc}\cos \mathrm{\θ}\·\cos \mathrm{\ψ}& -\sin \mathrm{\θ}& 0\\ \sin \mathrm{\θ}\·\cos \mathrm{\ψ}& \cos \mathrm{\θ}& 0\\ 0& 0& 1\end{array}\right],$ X、Y、Z表示载人潜水器在固定坐标系中的坐标，u、v、w表示载人潜水器沿动坐标系三轴的速度，θ表示载人潜水器的航向角，ψ表示载人潜水器纵倾角；

将固定坐标转化到惯性坐标系中，用经纬度表示载人潜水器的坐标；载人潜水器在惯性坐标系中的经纬度坐标值为：

其中，(λ₀，L₀)是作业母船起始位置的经纬度坐标，由母船上的GPS接收机接收，X，Y是载人潜水器在固定坐标系中的坐标值，R为地球半径，(λ，L)是载人潜水器在惯性坐标系中的坐标；

以某一时刻载人潜水器在惯性坐标系中的经纬度坐标(λ₁，L₁)作为航迹推算的初始值，以这点为新的固定坐标系原点(0，0)，用载人潜水器的坐标变换式(1)进行航迹推算，求出载人潜水器在新的固定坐标中坐标值(x，y)，再用公式(2)求出载人潜水器在惯性坐标系中的经纬度坐标(λ，L)。

2.按照权利要求1所述无固定参考点的载人潜水器在惯性坐标系中的定位方法，其特征在于：所述某一时刻载人潜水器在惯性坐标系中初始值的经纬度坐标(λ₁，L₁)是通过所述公式(2)计算得到的。

3.按照权利要求1所述无固定参考点的载人潜水器在惯性坐标系中的定位方法，其特征在于：求载人潜水器在惯性坐标系中的经纬度坐标(λ，L)时，公式(2)中(X，Y)取(x，y)，(λ₀，L₀)取(λ₁，L₁)。

Images