[发明专利]一种基于多水下应答器的惯性/水声组合导航方法

摘要

本发明涉及一种基于多水下应答器的惯性/水声组合导航方法，其技术特点包括：水下应答器布放与位置标定；通过距离变换位置算法计算AUV的绝对位置信息；通过AUV中的惯性导航系统INS/DVL与计算得到的AUV绝对位置信息进行位置组合导航算法来校正自身航位推算误差；布放一个后续水下应答器，利用AUV与当前水下应答器之间的距离组合输出去估计出后续水下应答器位置，AUV再与后续水下应答器进行距离组合导航，并重复执行上述过程。本发明能够充分结合惯性导航、水声导航的二者信息优势，同时考虑到水下应用的实际背景，通过在AUV航行路径上接力式地布放一系列水下应答器，仅利用水声距离信息解决了水下AUV导航设备的校准问题，具有广泛的应用前景。

主权项

1.一种基于多水下应答器的惯性/水声组合导航方法，其特征在于包括以下步骤：步骤1、水下应答器布放与位置标定：当水下AUV运行到某作业区域布放一个水下应答器，利用AUV自身运动信息以及AUV与该水下应答器相互之间的距离信息标定出水下应答器位置；步骤2、通过距离变换位置算法，将AUV与水下应答器之间的相对距离信息转换成AUV相对于应答器之间的三维几何位置，根据标定出的水下应答器位置信息，计算AUV的绝对位置信息；本步骤的具体处理方法为：建立以水下应答器点为原点的东北天直角坐标系，将AUV与水下应答器之间的相对距离信息转换成AUV相对于水下应答器之间的三维几何位置：AUV相对于水下应答器之间的东向距离xe、北向距离xn和天向距离xu，AUV导航系统状态变量方程为f(XK)，三维状态向量为Xk＝[xe xn xu]T，导航系统状态变量方程f(XK)如下：式中：vs为AUV合速度；φ，β分别为AUV航向角、俯仰角；在距离组合导航定位系统中，扩展卡尔曼滤波器k时刻观测方程为Z_k，由状态变量构成的观测方程详细表示如下：式中：di——AUV与水下应答器之间的距离；zi——AUV深度；ni——距离信息di和深度信息zi的量测噪声为零均值的高斯白噪声；采用扩展卡尔曼滤波器处理，状态变量方程f(Xk)的雅克比矩阵Fk，观测方程h(Xk)的雅克比矩阵Hk，扩展卡尔曼滤波器的一步预测方程如下：Φk＝I+Fk·TPk,k‑1＝ΦkPk‑1Φk+Qk‑1扩展卡尔曼滤波器的一步滤波方程为：Pk＝(I‑KkHk)Pk,k‑1(I‑KkHk)T+KkRkKkT式中：Φk为状态转移矩阵；Kk为滤波器增益矩阵；Pk,k‑1，Pk分别为一步预测均方误差、估计均方误差；通过扩展卡尔曼滤波器相应运算过程，将距离信息转换为AUV相对于水下应答器之间的东向距离xe、北向距离xn、天向距离xu，根据事先标定的水下应答器位置信息P0(L0,λ0,h0)，进而得到AUV的绝对位置信息Pc(Lc,λc,hc)：上式中，zi为AUV深度，RE为地球主曲率半径，Lc、λc和hc为AUV纬度、经度和深度信息；步骤3、通过AUV中的惯性导航系统INS/DVL与计算得到的AUV绝对位置信息进行位置组合导航算法来校正自身航位推算误差；步骤4、当AUV即将超出水下应答器作用范围时，布放一个后续水下应答器，利用AUV与当前水下应答器之间的距离组合输出去估计出后续水下应答器位置，当AUV运行区域超出了当前水下应答器的水声作用范围后，AUV再次与后续水下应答器进行距离组合导航，并重复执行上述过程。