本发明涉及一种基于多水下应答器的惯性/水声组合导航方法，其技术特点包括：水下应答器布放与位置标定；通过距离变换位置算法计算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)如下：式中：
在距离组合导航定位系统中，扩展卡尔曼滤波器k时刻观测方程为Zk，由状态变量构成的观测方程 详细表示如下：式中：
ni——距离信息di和深度信息zi的量测噪声为零均值的高斯白噪声；
采用扩展卡尔曼滤波器处理，状态变量方程f(Xk)的雅克比矩阵Fk，观测方程h(Xk)的雅克比矩阵Hk，扩展卡尔曼滤波器的一步预测方程如下：Φk＝I+Fk·T
Pk＝(I-KkHk)Pk,k-1(I-KkHk)T+KkRkKkT式中：
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再次与后续水下应答器进行距离组合导航，并重复执行上述过程。
2.根据权利要求1所述的一种基于多水下应答器的惯性/水声组合导航方法，其特征在于：所述标定水下应答器位置的方法为：建立直角坐标系下AUV到水下应答器绝对位置X(x0,y0,z0)校准的观测方程：式中：i＝1,2,...,n表示测量次数，Fi为第i次AUV与水下应答器之间的相对距离，(xi,yi,zi)表示AUV导航系统第i次测量点位置；然后利用泰勒级数展开法和最小二乘法迭代求解上述方程，将所得结果X(x0,y0,z0)利用直角坐标至球面坐标转换公式，完成水下应答器位置P0(L0,λ0,h0)的标定。
3.根据权利要求1所述的一种基于多水下应答器的惯性/水声组合导航方法，其特征在于：所述步骤3的具体方法为：通过距离信息变换位置算法实时更新得到的AUV位置，与AUV中导航设备INS/DVL实时输出的位置信息做差后进入位置组合导航kalman滤波器，对系统姿态、速度和位置输出值进行实时补偿，同时反馈修正导航设备内部状态信息。
一种基于多水下应答器的惯性/水声组合导航方法
技术领域
[0001] 本发明属于惯性导航与水声定位技术领域，尤其是一种基于多水下应答器的惯性/水声组合导航方法。
背景技术
[0002] 光学捷联惯导技术成熟解决了海上无人装备对惯导系统小型化以及高可靠性的问题，其通过水下自主式航行器(AUV)自身惯性导航系统(INS)/多普勒测速仪(DVL)以及压力传感器等获取航向、速度以及深度信息，完成航位推算导航。但是，导航设备定位误差随时间累积的问题依然存在，导致AUV不得不依靠上浮水面利用GPS来完成校准，既浪费了能源又增加了暴露的几率。
[0003] 在无线电波、光波受限的水下空间，水声信号成为水下信息最有效的传播载体，水声导航具有多目标、双向定位的技术特点。为满足水下AUV高精度、长时间、隐蔽性的导航需求，如何将惯性与水声进行结合并实现精确导航功能上，是目前迫切需要解决的问题。
发明内容
[0004] 本发明的目的在于克服现有设计的不足，提供一种设计合理、精度高且性能稳定的基于多水下应答器的惯性/水声组合导航方法。
[0005] 本发明解决其技术问题是采取以下技术方案实现的：
[0006] 一种基于多水下应答器的惯性/水声组合导航方法，包括以下步骤：
[0007] 步骤1、水下应答器布放与位置标定：当水下AUV运行到某作业区域布放一个水下应答器，利用AUV自身运动信息以及AUV与该水下应答器相互之间的距离信息标定出水下应答器位置；
[0008] 步骤2、通过距离变换位置算法，将AUV与水下应答器之间的相对距离信息转换成AUV相对于应答器之间的三维几何位置，根据标定出的水下应答器位置信息，计算AUV的绝对位置信息；
[0009] 步骤3、通过AUV中的惯性导航系统INS/DVL与计算得到的AUV绝对位置信息进行位置组合导航算法来校正自身航位推算误差；
[0010] 步骤4、当AUV即将超出水下应答器作用范围时，布放一个后续水下应答器，利用AUV与当前水下应答器之间的距离组合输出去估计出后续水下应答器位置，当AUV运行区域超出了当前水下应答器的水声作用范围后，AUV再次与后续水下应答器进行距离组合导航，并重复执行上述过程。
[0011] 所述步骤1标定水下应答器位置的方法为：
[0012] 建立直角坐标系下AUV到水下应答器绝对位置X(x0,y0,z0)校准的观测方程：
[0013]
[0014] 式中：i＝1,2,...,n表示测量次数，(xi,yi,zi)表示AUV导航系统第i次测量点位置；然后利用泰勒级数展开法和最小二乘法迭代求解上述方程，将所得结果X(x0,y0,z0)利用直角坐标至球面坐标转换公式，完成水下应答器位置P0(L0,λ0,h0)的标定。
[0015] 所述步骤2的具体处理方法为：
[0016] 建立以水下应答器点为原点的东北天直角坐标系，将AUV与水下应答器之间的相对距离信息转换成AUV相对于水下应答器之间的三维几何位置：东向距离xe、北向距离xn和天向距离xu，定义AUV导航系统状态变量方程f(XK)为三维状态向量Xk＝[xe xn xu]T，导航系统动态方程f(XK)如下：
[0017]
[0018] 式中：
[0019] xe，xn，xu分别为AUV相对于水下应答器之间的东向、北向和天向距离；
[0020] vs为AUV合速度；
[0021] φ，β分别为AUV航向角、俯仰角；
[0022] 在距离组合导航定位系统中，扩展卡尔曼滤波器k时刻观测方程为Zk，由状态变量构成的观测方程h(Xk)＝[d z]T详细表示如下：
[0023]
[0024] 式中：
[0025] di——AUV与水下应答器之间的距离；
[0026] zi——AUV深度；
[0027] ni——距离信息di和深度信息zi的量测噪声为零均值的高斯白噪声；
[0028] 采用扩展卡尔曼滤波器处理，状态方程f(Xk)的雅克比矩阵Fk，观测方程h(Xk)的雅克比矩阵Hk，扩展卡尔曼滤波器的一步预测方程如下：
[0029]
[0030] Φk＝I+Fk·T
[0031] Pk,k-1＝ΦkPk-1Φk+Qk-1
[0032] 扩展卡尔曼滤波器的一步滤波方程为：
[0033]
[0034]
[0035] Pk＝(I-KkHk)Pk,k-1(I-KkHk)T+KkRkKkT
[0036] 式中：
[0037] Φk为状态转移矩阵；
[0038] Kk为滤波器增益矩阵；
[0039] Pk,k-1，Pk分别为一步预测均方误差、估计均方误差；
[0040] 通过扩展卡尔曼滤波器相应运算过程，将距离信息转换为AUV与水下应答器之间的相对东向距离xe、相对北向距离xn、相对天向距离xu，根据事先标定的水下应答器位置信息P0(L0,λ0,h0)，进而得到AUV的绝对位置信息Pc(Lc,λc,hc)：
[0041]
[0042] 上式中，xe为AUV与水下应答器之间的相对东向距离，xn为AUV与水下应答器之间的相对北向距离，zi为AUV深度，RM和RN为地球主曲率半径，Lc、λc和hc为AUV纬度、经度和深度信息。
[0043] 所述步骤3的具体方法为：通过距离信息变换位置算法实时更新得到的AUV位置，与AUV中导航设备INS/DVL实时输出的位置信息做差后进入位置组合导航kalman滤波器，对系统姿态、速度和位置输出值进行实时补偿，同时反馈修正导航设备内部状态信息。
[0044] 所述步骤4的具体方法为：当AUV即将超出水下应答器作用范围时，布放一个后续水下应答器，利用AUV与当前水下应答器之间的距离组合输出去估计出后续水下应答器位置，当AUV运行区域超出了当前水下应答器的水声作用范围后，AUV再次与后续水下应答器进行距离组合导航，并重复执行上述过程。
[0045] 本发明的优点和积极效果是：
[0046] 1、本发明在AUV航行路径上通过接力式地布放一系列水下应答器b1、b2、……、bn，不断地与当前水下应答器进行距离组合导航同时估计另一个水下应答器位置，仅利用声学测距手段，可实现抑制自身INS/DVL航位推算误差发散的位置校准功能，始终保证水下AUV位置误差有界，采用本发明的AUV全程定位精度优于20m，远远高于INS/DVL航位推算条件下随时间积累约为50m/h的定位误差，可有效抑制INS/DVL组合定位误差的发散趋势，保证了导航设备输出满足精度指标要求，为解决AUV导航设备水下校准提供了可能。
[0047] 2、本发明能够充分结合惯性导航、水声导航的二者信息优势，同时考虑到水下应用的实际背景，通过在AUV航行路径上接力式地布放一系列水下应答器，仅利用水声距离信息解决了水下AUV导航设备的校准问题，具有广泛的应用前景。
附图说明
[0048] 图1为本发明的处理流程图；
[0049] 图2为本发明的位置组合导航算法原理框图；
[0050] 图3为本发明的导航方法原理框图。
具体实施方式
[0051] 以下结合附图对本发明实施例做进一步详述：
[0052] 一种基于多水下应答器的惯性/水声组合导航方法，如图1所示，当水下AUV运行到某作业区域布放一个水下应答器b1，首先利用AUV自身运动信息以及AUV与水下应答器b1相互之间的距离信息估计出水下应答器b1的位置，然后进行AUV中的惯性导航系统INS/DVL与水下应答器之间的位置组合导航算法来校正自身航位推算误差；当AUV即将超出水下应答器b1作用范围时布放水下应答器b2，利用AUV与水下应答器b1之间的距离组合输出去估计出水下应答器b2位置，当AUV运行区域超出了水下应答器b1的水声作用范围后，AUV再次与水下应答器b2进行距离组合导航，上述过程依次类推。由此可以看出，在AUV航行路径上通过接力式地布放一系列水下应答器b1、b2、……、bn，不断地与当前水下应答器进行距离组合导航同时估计另一个水下应答器位置，仅利用声学测距手段，可实现抑制自身INS/DVL航位推算误差发散的位置校准功能，始终保证水下AUV位置误差有界。
[0053] 本发明包括以下步骤：
[0054] 步骤1、水下应答器布放与位置标定：当水下AUV运行到某作业区域布放一个水下应答器b1，利用AUV自身运动信息以及AUV与水下应答器b1相互之间的距离信息标定出水下应答器b1的位置。
[0055] 水下导航系统一般采用自身的惯性导航系统INS/DVL组合方式进行航位推算，当AUV运行到某作业区域，布放水下应答器，水下应答器沉入水底经触发后以等间隔时间发出声信息，提供自身与AUV相互间的距离信息。水下应答器布放后，AUV进行机动航行，建立直角坐标系下AUV到水下应答器绝对位置X(x0,y0,z0)校准的观测方程：
[0056]
[0057] 式中：i＝1,2,...,n表示测量次数，(xi,yi,zi)表示AUV导航系统第i次测量点位置。
[0058] 利用泰勒级数展开法和最小二乘法迭代求解公式(1)，将所得结果X(x0,y0,z0)利用直角坐标至球面坐标转换公式，完成水下应答器位置P0(L0,λ0,h0)的精确标定。
[0059] 步骤2、通过距离变换位置算法计算出AUV的绝对位置信息。
[0060] 建立以水下应答器点为原点的东北天直角坐标系，将AUV与水下应答器之间的相对距离信息转换成AUV相对于水下应答器之间的三维几何位置(东向距离xe、北向距离xn和T天向距离xu)。定义AUV导航系统状态变量方程f(XK)为三维状态向量Xk＝[xe xn xu] ，导航系统动态方程f(XK)如下：
[0061]
[0062] 式中：
[0063] xe，xn，xu——AUV相对于水下应答器之间的东向、北向和天向距离；
[0064] vs——AUV合速度；
[0065] φ，β——AUV航向角、俯仰角。
[0066] 在距离组合导航定位系统中，扩展卡尔曼滤波器k时刻观测方程为Zk，由状态变量构成的观测方程h(Xk)＝[d z]T详细表示如下：
[0067]
[0068] 式中：
[0069] di——AUV与水下应答器之间的距离；
[0070] zi——AUV深度；
[0071] ni——距离信息di和深度信息zi的量测噪声为零均值的高斯白噪声。
[0072] 上述两式子构成了纯距离组合导航系统的基本方程，显然这是一个非线性方程，需采用扩展卡尔曼滤波器(EKF)处理，公式(2)状态方程f(Xk)的雅克比矩阵Fk，公式(3)观测方程h(Xk)的雅克比矩阵Hk，扩展卡尔曼滤波器的一步预测方程如下：
[0073]
[0074] 扩展卡尔曼滤波器的一步滤波方程为：
[0075]
[0076] 式中：
[0077] Φk——状态转移矩阵；
[0078] Kk——滤波器增益矩阵；
[0079] Pk,k-1，Pk——一步预测均方误差，估计均方误差。
[0080] 通过扩展卡尔曼滤波器相应运算过程，可将距离信息转换为AUV与水下应答器之间的相对东向距离xe、相对北向距离xn、相对天向距离xu，根据事先标定的水下应答器位置信息P0(L0,λ0,h0)，进而得到AUV的绝对位置信息Pc(Lc,λc,hc)：
[0081]
[0082] 步骤3、通过AUV中的惯性导航系统INS/DVL与计算得到的AUV绝对位置信息进行位置组合导航算法来校正自身航位推算误差。
[0083] 如图2所示，通过距离信息变换位置算法实时更新得到的AUV位置，与AUV中导航设备INS/DVL实时输出的位置信息做差后进入位置组合导航kalman滤波器，对系统姿态、速度和位置输出值进行实时补偿，同时反馈修正导航设备内部状态信息(姿态角、速度、位置及惯性元件漂移等)，从而抑制自身导航设备误差的发散增长，有效提高了导航定位精度。
[0084] 步骤4、当AUV即将超出水下应答器b1作用范围时布放水下应答器b2，利用AUV与水下应答器b1之间的距离组合输出去估计出水下应答器b2位置，当AUV运行区域超出了水下应答器b1的水声作用范围后，AUV再次与水下应答器b2进行距离组合导航，上述过程依次类推。
[0085] 水下AUV继续在水中行进，根据水声导航具有多目标、双向定位的技术特点，利用AUV自身的INS/DVL的导航信息以及AUV与水下应答器之间的声学距离信息既能标定出水下应答器的位置；又能将声学距离信息进行距离组合导航(声学距离信息转换成载体与水下应答器之间的几何位置信息，结合标定后的水下应答器位置实时估算得到载体AUV自身的位置，融合自身的INS/DVL的导航信息进行组合导航)，从而获取AUV自身更加精确的位置估计，同时修正AUV导航设备中的误差。当AUV即将超出当前水下应答器作用范围时，布放并估计新水下应答器的位置，当超出当前水下应答器作用范围时，再利用新水下应答器进行距离组合导航，依此类推，通过在AUV的航行路径上接力式地布放一系列水下应答器，即可实现这种基于多水下应答器的惯性/水声组合导航方法，从而在AUV航行路径中的较大范围内保持一定的定位精度，上述导航方法原理框图如图3所示。
[0086] 需要强调的是，本发明所述的实施例是说明性的，而不是限定性的，因此本发明包括并不限于具体实施方式中所述的实施例，凡是由本领域技术人员根据本发明的技术方案得出的其他实施方式，同样属于本发明保护的范围。
