[发明专利]多关节水下无人潜行器变基线三维空间定位方法

权利要求书

1.多关节水下无人潜行器变基线三维空间定位方法，所采用的设备包括安装在多关节AUV本体上的水听器阵列、布置在海底的单潜标声源；其特征在于：在每个多关节AUV的空间定位周期内，定位系统按照以下的步骤获得多关节水下无人潜行器的空间坐标；具体实施步骤包括：

步骤1，计算水听器至潜标的斜距；

设定海面上大地全局坐标系P(Oxyz)，潜标位置为r₀＝(x₀，y₀，z₀)，设定水听器阵列由头尾两个水听器组成；两个水听器分别为第一水听器(1)和第二水听器(2)，其坐标分别为r₁＝(x₁，y₁，z₁)和r₂＝(x₂，y₂，z₂)；声波从潜标声源r₀传播至第一水听器(1)和第二水听器(2)的时间，由水听器测量得到的水声声压信号与潜标声源的信号进行互相关处理，可得到潜标发射的声波到达两个水听器的时间迟延分别为t₁和t₂，由CTD测得的声速C，由此可得到两个水听器至潜标的斜距分别为R₁和R₂；

步骤2，通过坐标变换计算得到多关节AUV的基线的长度L、俯仰角α和偏航角γ；

水下无人潜行器上两个水听器之间的直线段定义为基线L，通过三轴陀螺仪的输出和AUV关节的旋转角度可得到基线俯仰角α，基线偏航角γ；基线指向海面时，俯仰角α为正，基线指向北方时，偏航角γ为0°，偏东为正；γ∈[-π，π]；

在海面上大地全局坐标系P(Oxyz)中，P₁，P₂，P₃为多关节AUV三节末端的局部坐标系；设P₂基准面，第一节的俯仰角和偏航角为α₁和γ₁，第三节的俯仰角和偏航角为α₃和γ₁；第二节的俯仰、偏航和横摇角是AUV位置在全局坐标系P中的参考坐标，由三轴陀螺仪给出，分别用α_g，γ_g，β_g表示；多关节AUV三节的长度分别为l₁，l₂，l₃，第一水听器(1)与第一节末端的距离d₁，第二水听器(2)与第三节末端的距离d₃；AUV的轴向半径为R，在全局坐标系P中，设局部坐标系P₂的原点o₂的位置为

在P₁坐标系中，第一水听器(1)的位置为在P₂坐标系中，第一水听器(1)和第二水听器(2)的位置分别为和可表示为

在P₃坐标系中，第二水听器(2)的位置为为从P₁坐标系原点到P₂坐标系原点的向量，为从P₃坐标系原点到P₂坐标系原点的向量，可表示为

为从P₁坐标系变换到P₂坐标系的旋转矩阵，为从P₃坐标系变换到P₂坐标系的旋转矩阵，可表示为

在全局坐标系P中，第一水听器(1)和第二水听器(2)的位置分别为^Pr_i，i＝1，2，

是从P₂坐标系原点到P坐标系原点的向量，

是旋转矩阵从P₂坐标系变换到P坐标系，

根据式(7)，第一水听器(1)和第二水听器(2)的位置重新分别表示为^Pr₁＝(^Px₁，^Py₁，^Pz₁)和^Pr₂＝(^Px₂，^Py₂，^Pz₂)；基线的长度L，基线的俯仰角α，基线的偏航角γ，可以计算为：

L＝||^Pr₁-^Pr₂|| (10)

步骤3，根据步骤1和2获得的结果，计算出两个水听器的坐标r₁＝(x₁，y₁，z₁)和r₂＝(x₂，y₂，z₂)；

AUV携带的测深仪测得的第一水听器(1)至潜标与深度差为H₁；根据几何坐标关系可以建立如下几何关系：

求解式(13)和式(14)时，两个水听器的z₁和z₂坐标分别为

为了简洁地表示两个水听器x和y方向的坐标，引入中间变量A，

简化后，可得

上式中的计算结果有两组解：x₁₁和y₁₁为第一水听器(1)的第一解，x₂₁和y₂₁为第二水听器(2)的第一解；x₁₂和y₁₂为第一水听器(1)的第二解，x₂₂和y₂₂为第二水听器(2)的第二解；其中r₁₁＝(x₁₁，y₁₁，z₁)和r₂₁＝(x₂₁，y₂₁，z₂)为第一解，r₁₂＝(x₁₂，y₁₂，z₁)和r₂₂＝(x₂₂，y₂₂，z₂)为第二解；

对于步骤3中求得的两组解，设计多解判别算法求出真解，得到水听器在全局大地坐标系P下的真实坐标；

步骤3中所得两组解具有相同的偏航角和基线长度；但只有一个解是真值，通过确定哪一组解更接近于水听器的预测位置来实现多解判别算法；

定义第一水听器1在前一时刻的位置为r_1prev＝(x_1prev，y_1prev，z_1prev)，第一水听器(1)的预测位置为r′₁＝(x′₁，y′₁，z′₁)，第一水听器(1)在一个定位周期内的预测位移为Δr＝(Δx，Δy，Δz)；由此可以表达第一水听器(1)的预测位置

r′₁＝r_1prev+Δr (18)

第一水听器1在定位周期内的位移可以通过三轴陀螺仪的数据、AUV的关节角度和多普勒计程仪的输出来计算；基线的俯仰角和偏航角α和γ，可以根据AUV三轴陀螺仪和关节角的数据计算，坐标变换在步骤2中给出；多普勒计程仪提供了水下航行器的速度V；这三个参数随时间而变化；第一水听器(1)在定位周期K内的位移可表示为

其中τ为当前时间；然后计算|r₁₁-r′₁|和|r₁₂-r′₁|的长度；式(17)中最接近式(18)所预测位置的解被认为是第一水听器(1)的真实的当前位置；第二水听器(2)真实的当前位置，可以由式(13)确定。

2.如权利要求1所述的多关节水下无人潜行器变基线三维空间定位方法，其特征在于：所采用的AUV是多关节的AUV，该AUV携带三轴陀螺仪、深度计、速度剖面仪(DopplerVelocity Log，DVL)和温盐深度仪(Conductivity-Depth-Temperature，CTD)，头尾两节各安装一个或圆周向多个水听器构成水听器阵列。