一种基于基线几何结构约束的水下航行器定位方法与流程

本发明属于水下声学导航与定位领域，具体涉及一种水下航行器定位方法，同样适用于其它各类水下自主移动目标定位过程。

背景技术：

由于无线电信号在水下传播时极易被吸收而急速衰减，导致陆地、空中目标导航常用的gps、北斗等系统难以直接用于水下航行器精确定位。目前水下航行器定位技术主要可分为三类：(1)惯性/航位推算定位；(2)地球物理学定位；(3)水声定位。惯性/航位推算定位技术的优点在于可不依赖外部信息源进行全天候、全地域自主定位，但是各高精度惯性测量传感器价格高昂、体积较大，误差随着时间积累而趋于发散。而地球物理学定位技术则更多的与惯性/航位推算技术相结合，形成水下组合导航定位系统，虽然可以在一定程度上克服惯性/航位推算定位误差发散问题，但是严重依赖于地形、地磁、深度等先验信息，且易受复杂海洋环境影响。水声定位技术则主要通过测量水下声信号发射器与接收器间的距离及方位，解算估计被定位目标位置信息。常用的水声定位方法包括长基线定位、短基线定位、超短基线定位等。长基线定位系统由布置在水底或水面的位置已知声信标基阵组成，各信标间的基线长度从上百米到几千米不等，类似于gps定位原理，可利用曲面相交法来确定水下航行器相对于基阵的相对位置。短基线及超短基线定位系统将声信标基阵布置于固定载体(如水面舰船、作业平台等)上，信标间的基线长度较短，一般短基线长度不超过几十米，而超短基线长度则更是会小到几厘米，在定位过程中需要测量水下航行器与声信标间的距离和相位差。相比较而言，长基线定位精度最高，可靠性好，但是系统构成复杂，布放、标定及维护信标基阵均需大量的财力物力，而短基线及超短基线定位精度均不如长基线定位系统，好处在于系统构成简单，便于操作。

在当前水下航行器定位过程中，若只采用惯性/航位推算定位法，受限于水下航行器的体积大小与负载能力，其所能携带的惯性传感器定位精度有限，需要水下航行器定时浮出水面，利用gps、北斗等系统进行位置修正，影响任务执行效率；而若只采用水声信标进行定位，则受水下复杂环境的影响，容易出现水声定位信息时延与丢包等现象，造成部分时刻定位失败。此外，多个声信标间的相对精确位置与距离信息未能被充分利用，声信标基线间的几何结构关系对于距离量测误差的约束作用也未被足够重视。

技术实现要素：

为了克服现有技术的不足，本发明提供一种基于基线几何结构约束的水下航行器定位方法，利用水声定位基线间的几何结构关系，构造声信标与水下航行器间距离量测及相关约束条件，并将其应用于水下航行器定位过程中，实现水下航行器自主定位精度的提高。

本发明解决其技术问题所采用的技术方案包括以下步骤：

步骤1，三个具有自定位功能的水声信标以固定时间间隔广播自身位置(xi，yi)，i＝1，2，3，水下航行器利用接受到的水声信号延迟时间δti与区域水下平均声速的乘积，来计算得到各个信标间的3d量测距离利用所携带深度计精确测量水下航行器深度值h，将3d量测距离li转换为平面2d量测距离

步骤2，已知信标位置，计算两两信标间的距离lij，即定位基线长度，j＝1，2，3且i≠j；利用定位基线间的几何结构关系，构造信标与水下航行器当前位置(x，y)间的2d计算距离的等式约束条件s(ri²)＝0与不等式约束条件h(ri)≥j(ri)；

步骤3，在第k-1次定位结果基础上，水下航行器利用惯性传感器所测前向速度vk-1、航向角速度wk-1以及俯仰角速度υk-1，设定各传感器定位采样周期为δt，从而求得第k次预测定位信息包括位置、航向角以及俯仰角，即有

其对应于状态估计与控制输入uk-1＝[vk-1wk-1υk-1]^t的雅可比矩阵和分别为

利用所接收到的三个水面声信标位置(xi，k，yi，k)以及测量得到的相对距离在距离量测误差为εk的基础上，建立水面信标与水下航行器间的相对距离量测方程

其对应于预测信息的雅可比矩阵

选择最大后验估计方法，实现等式约束条件与不等式约束条件h(ri，k)≥j(ri，)与相对距离相融合，并通过求解以下约束优化问题得到水下航行器位置、航向及俯仰估计值，即有

s.t.∑k＝diag(pk/k-1，rk)

h(ri，k)≥j(ri，k)

i＝1，2，3

其中，rk为距离量测误差协方差，而状态估计协方差的预测值pk/k-1则借助于扩展kalman滤波进行递推求解，即

本发明的有益效果是：

(1)实现了水下惯性/航位推算定位方法与水声定位方法相结合，确保了水下航行器长时间有效精确定位；

(2)通过定位基线间的几何结构关系，来构造距离等式约束条件与不等式约束条件，使得声信标间的距离信息在定位过程中得到了有效利用；

(3)借助最大后验估计准则与扩展kalman滤波，使得约束条件有效融入定位求解过程中，实现了水下航行器定位精度的提高。

附图说明

图1是基于真实距离的水声定位结构示意图；

图2是基于实际距离量测的水声定位结构示意图；

图3是x轴定位误差示意图；

图4是y轴定位误差示意图。

具体实施方式

下面结合附图和实施例对本发明进一步说明，本发明包括但不仅限于下述实施例。

本发明的技术方案包括以下步骤：

步骤1：三个具有自定位功能的水声信标以固定时间间隔广播自身位置(xi，yi)(i＝1，2，3)，水下航行器则利用接受到的水声信号延迟时间δti与区域水下平均声速的乘积，来计算得到各个信标间的3d量测距离利用所携带深度计精确测量水下航行器深度值h，将3d量测距离li转换为平面2d量测距离

步骤2：通过已知信标位置计算两两信标间(即定位基线)的距离lij(i，j＝1，2，3且i≠j)，利用定位基线间的几何结构关系，构造信标与水下航行器当前位置(x，y)间的2d计算距离的等式约束条件s(ri²)＝0与不等式约束条件h(ri)≥j(ri)(j(ri)与距离lij和相关)，如下所示；

步骤3：利用最大后验估计准则与扩展kalman滤波相结合，将量测距离等式约束条件s(ri²)＝0以及不等式约束条件h(ri)≥j(ri)有效融入水下航行器自定位过程中，递推求解得到水下航行器航行状态信息。

(1)在第k-1次定位结果基础上，水下航行器利用惯性传感器所测前向速度vk-1、航向角速度wk-1以及俯仰角速度υk-1，设定各传感器定位采样周期为δt，从而可求得第k次预测定位信息其中包括位置、航向角以及俯仰角，即有

其对应于状态估计与控制输入uk-1＝[vk-1wk-1υk-1]^t的雅可比矩阵和分别为

(2)利用所接收到的三个水面声信标位置(xi，k，yi，k)(i＝1，2，3)以及测量得到的相对距离在距离量测误差为εk的基础上，可建立水面信标与水下航行器间的相对距离量测方程，即有

其对应于预测信息的雅可比矩阵则为

(3)选择最大后验估计方法，可实现等式约束条件与不等式约束条件h(ri，k)≥j(ri，)与相对距离相融合，并通过求解以下约束优化问题得到水下航行器位置、航向及俯仰估计值，即有

s.t.∑k＝diag(pk/k-1，rk)

h(ri，k)≥j(ri，k)

i＝1，2，3

其中，rk为距离量测误差协方差。而状态估计协方差的预测值pk/k-1则可借助于扩展kalman滤波进行递推求解，即

在本实施例中，三个水声信标按照一定的几何构型部署于水面上，信标上装有卫星导航与授时设备以及水声通信模块。信标位置通过卫星导航精确可知，信标通过卫星导航授时实现与utc时间同步，并且按照固定时间间隔，利用水声通信模块广播发送自身位置信息。水下航行器利用所携带深度计、多普勒测速仪(dvl)以及惯性测量元件(imu)等，可以实现自主定位，但定位误差随时间发散。此外，水下航行器通过初始卫星导航授时以及所携带原子钟守时模块，实现与utc时间以及水声信标的时间同步，并在此基础上，利用所携带水声通信模块接收水声信标位置信息，并测得与信标间的距离，辅助自身自主定位精度的提高。详细定位过程如下：

步骤1：水声信标与水下航行器间水平距离测量。

(1)在ts时刻，三个水声信标通过卫星导航获得自身位置(xi，yi)(i＝1，2，3)，并通过水声通信模块广播发送位置、时间等信息。水下航行器在tr，i时刻接收到水声信息包后，则在时间同步的基础上，利用信息包发送与接收时刻间的时间差δti，求解得到与水声信标间的3d相对距离，即

式中，为水下平均声速，一般可取近似值

(2)利用所携带深度计可精确测量水下航行器深度值h，并将与信标间的3d量测距离li转换为平面2d量测距离，即

步骤2：通过定位基线几何结构关系构造约束条件。

在第k次定位过程中，水下航行器接收到的水面声信标位置分别为(xi，k，yi，k)(i＝1，2，3)，水下航行器与声信标间的真实距离为ri，k，而利用步骤1所述方法获得的距离量测为受水声信号多径效应、声线弯曲等因素的影响，水声测量距离往往大于真实距离，即从而可构造以下等式与不等式约束条件。

(1)利用接收到的声信标位置信息，可计算得到声信标两两之间的距离为

式中，i，j＝1，2，3且i≠j。若假设水下航行器与声信标连线间的夹角为αij，k，如附图1所示，从而可有以下关系成立

(cosα13，k)²+(cosα12，k)²+(cosα23，k)²＝1+2cosα13，kcosα12，kcosα23，k(4)

通过等效变换，可将上述两式(4)和(5)表示为矩阵与向量相乘的形式，继而可获得以下等式约束条件

式中，

(2)在基于实际量测距离的定位过程中，如附图2所示，以声信标为中心，以到水下航行器的量测距离为半径的三个圆相交，且交点不唯一，使得被定位水下航行器位置被限定于一个很小的范围内，从而可得以下不等式关系式成立。

式中，

对式(7)进行等效变换，可得到以下不等式约束条件

步骤3：利用最大后验估计准则与扩展kalman滤波进行定位解算。

(1)若在第k-1次定位过程中，为估计得到的水下航行器航行状态向量，其中表示水下航行器的实时航向角，表示水下航行器实时俯仰角，表示水下航行器实时位置，而航行深度hk-1一般通过深度计精确测量得到，可不用进行求解。因此，根据水下航行器运动学模型，可递推得到其第k次自定位结果如下

式中，δt为自定位采样周期，uk-1＝[vk-1wk-1υk-1]^t为水下航行器航行控制输入，vk-1、wk-1和υk-1分别为所携带惯性传感器测得的前向速度、航向角速度以及俯仰角速度，控制输入噪声为ωk-1～n(0，qk-1)。

可分别计算运动学方程对应于状态估计与控制输入uk-1的雅可比矩阵和如下：

(2)水下航行器所接收到的三个水面声信标位置分别为(xi，k，yi，k)(i＝1，2，3)，而与之相对应，根据步骤1所计算的2d相对距离则为因此可建立水面信标与水下航行器间的相对距离量测方程如下：

式中，h(ri，k)＝[rl，kr2，kr3，k]^t。若距离量测噪声εk服从高斯分布，则其相应协方差为rk。

可计算距离量测方程对应于预测状态的雅可比矩阵如下：

(3)为了将相对距离量测及其约束条件有效融入水下航行器的定位过程中，可选择通过最大后验估计准则与扩展kalman滤波相结合的方法来设计相应的定位算法。而在最大后验估计准则下，可将水下航行器定位过程看作为，利用所获得的所有距离量测信息z1：k＝(z1，λ，zk)求解使得后验概率密度函数p(xk|z1：k)最大的水下航行器状态估计值将其用数学式表示为

式中，p(zk|xk)为似然函数，p(xk|z1：k-1)为先验概率密度函数。在控制输入噪声ωk-1与距离量测噪声εk服从高斯分布的情况下，则有

从而，根据式(16)和(17)，可将式(15)等效表示为

式中，∑k＝diag(pk/k-1，rk)。pk/k-1为状态估计协方差的预测值，可在计算得到雅可比矩阵以及的基础上，利用扩展kalman滤波方法递推求解为

式中，kk为滤波增益矩阵，可计算为

从而，在考虑基线几何约束的情况下，可将等式约束(6)、不等式约束(8)与(15)相结合，通过求解以下约束优化问题得到水下航行器位置、航向及俯仰估计值，即

注：①状态估计协方差的预测值pk/k-1的求解，可使用其它与扩展kalman滤波类似的方法，如迭代扩展kalman滤波(iekf)等；

②用于预测值pk/k-1更新的雅可比矩阵也可在状态估计值处求得，即

③约束优化问题(22)可选择使用格朗日乘子法、二次逼近法以及二等分法等进行求解。

此外，结合附图3与图4对本发明所公开的水下航行器定位方法进行仿真分析，分析结果如下：

假设水下航行器航行速度为4m/s，测速误差协方差为(0.5m/s)²的高斯白噪声，航向角速度与俯仰角速度测量误差协方差均为(0.001rad/s)²的高斯白噪声，声信标与水下航行器间的距离误差协方差为(4m)²的高斯白噪声加13m～15m的常值误差，距离量测更新周期为10s，定位优化问题可借助于matlab中自带的lsqlin函数求解。

将不考虑约束条件与自定位信息的最小二乘法(无约束ls)、不考虑约束条件的扩展kalman滤波法(无约束ekf)以及本发明所公开定位算法(约束map)等三种水下航行器定位方法进行了对比，附图3与图4分别为水下航行器x轴与y轴定位误差。可以发现，随着水下航行位置的移动，无约束ls算法定位精度变化明显。而相较于常用的无约束ls算法以及无约束ekf算法，通过声信标间几何结构所构造约束条件的有效利用，本发明所公开定位方法(约束map)可以获得更高的定位精度。