0 引　言

海洋在资源、经济、安全等方面具有战略意义，自治式潜水器（Autonomous Underwater Vehicle，AUV）作为一种新型水下无人运载平台，因其机动性、隐蔽性强，具备大范围搜索与探测能力，被广泛应用于海洋科考、海洋探测、海洋安全等领域。为保障AUV在水下具备长航时作业能力，高精度、高可靠性的导航定位系统至关重要^[1-3]。捷联惯性导航系统（Strapdown Inertial Navigation System,SINS）不受外界干扰，可在任何介质和任何环境条件下实现导航^[4]，一般作为AUV导航定位系统的关键子系统。由于SINS导航定位误差随时间累积，长期稳定性差，需采用外部辅助导航手段对其进行补偿修正，通过组合导航技术提高导航定位系统的整体性能。

水下主要采用声波进行导航定位，其中，长基线（Long Baseline, LBL）定位系统能够在较大的范围和较深的海水中得到较高的导航定位精度，利用LBL辅助SINS的组合导航技术是抑制误差发散、提高导航定位精度的有效手段^[5-6]。LBL定位通过测量布放在海底的应答器阵与AUV上换能器之间的距离，求解AUV的位置坐标，再通过Kalman滤波实现对惯导的校正。但由于水下环境多变，干扰因素复杂，组合导航系统精度难以保证。文献[7]采用异步量测序贯滤波方法提高了在应答信号缺失情况下组合导航系统的精度；文献[8]研究了SINS/LBL紧组合系统量测方程非线性对定位精度的影响，选用UKF滤波算法进行信息融合；文献[9]提出一种迭代声速修正算法，提高了LBL在同步工作方式下目标定位精度。

这些算法主要集中在提高LBL自身的定位精度及改进组合导航滤波算法，针对这两点问题，本文提出一种基于自适应滤波的水下长基线导航定位技术。一方面，研究基于伪距辅助的SINS/LBL组合算法，解决水声通信时延导致LBL定位系统位置坐标计算不准确的问题；另一方面，针对系统噪声统计特性未知问题，引入自适应滤波算法，通过改进组合导航系统信息融合方式，提高AUV导航定位精度及系统可靠性。

1 伪距辅助的SINS/LBL组合导航系统 1.1 系统原理及构成

LBL定位系统是通过测量AUV与海底应答器基阵之间的距离解算出目标AUV位置坐标，其定位原理如图1所示。

LBL海底应答器之间距离约100～6000 m，其绝对位置坐标已知，主要用于接收声信号并发射不同频率的应答信号。AUV上安装收发器，用于发射询问信号。当海底应答器基阵收到询问信号后，以不同频率应答。AUV接收应答信号后，通过信号收发时间差即可计算出与相应应答器的距离，并通过球面交汇法或双曲面交汇法解算出AUV位置坐标。

为实现水下高精度导航定位，AUV组合导航系统以SINS为关键子系统，LBL定位系统为辅助系统。传统组合导航方式一般以LBL解算出的位置信息作为量测量，通过Kalman滤波校正SINS系统。但在实际应用中，由于声信号在水中传播速度慢，随着AUV运动将会产生时延误差，导致通过几何交汇进行位置解算的LBL定位系统失效，且水下复杂的环境使得系统噪声无法精确建模，使用常规kalman滤波无法得到最优估计，甚至会使组合导航系统误差发散。针对以上问题，本文设计基于自适应滤波的SINS/LBL组合导航系统，以SINS系统推算的伪距 $\;{\rho _{SINS}^{\;}}$ 与LBL系统量测的伪距 ${\rho _{LBL}}$ 之差作为组合导航系统量测量，并通过自适应滤波实现信息融合。系统结构如图2所示。

本节通过构建伪距辅助的SINS/LBL组合导航系统解决水下时延误差对导航定位精度的影响，建立组合导航系统状态方程及量测方程。

1.2 状态方程

设AUV真实位置为 $(X,Y,Z)$ ，海底应答器坐标为 $({x^i},{y^i},{z^i}),(i = 1,2,3,4)$ ， $t_{SL}^i$ 为AUV到第 $i$ 个海底应答器水声信号的传播时间， $\rho _{LBL}^i$ 为AUV到第 $i$ 个海底应答器的量测水声伪距，则有：

$ t_{SL}^i = {t^i} + \delta t + \varepsilon _t^i {\text{，}} $

(1)

$ \rho _{LBL}^i = c \cdot t_{SL}^i {\text{。}} $

(2)

其中： ${t^i}$ 为AUV到第 $i$ 个海底应答器传播时间真值； $\delta t$ 为AUV到海底应答器钟差； $\varepsilon _t^i$ 为测量误差； $c$ 为声波在海水中传播速度。

$\delta t$ 的等效距离误差 $\delta {t_u}$ 可用一阶马尔科夫过程表示为：

$\delta {\dot t_u} = - \frac{1}{{{\tau _{tu}}}}\delta {t_u} + {w_{tu}}{\text{。}}$

(3)

其中： ${\tau _{tu}}$ 为相关时间； ${w_{tu}}$ 为白噪声。

对于SINS系统，选取15维状态量，分别是三维位置误差 $\delta L,\delta \lambda ,\delta h$ ，三维速度误差 $\delta {v_E},\delta {v_N},\delta {v_U}$ ，三维姿态角误差 ${\varphi _E},{\varphi _N},{\varphi _U}$ ，陀螺漂移误差 ${\varepsilon _x},{\varepsilon _y},{\varepsilon _z}$ ，加速计偏置误差 ${\nabla _x},{\nabla _y},{\nabla _z}$ ，表示为：

${X_{SINS}} = [ {{\varphi _E}}\quad{{\varphi _N}}\quad{{\varphi _U}}\quad{\delta {v_E}}\quad{\delta {v_N}}\quad{\delta {v_U}}\quad{\delta L}\quad {\delta \lambda } \quad{\delta h}$ ${{\varepsilon _x}}\quad{{\varepsilon _y}}\quad{{\varepsilon _z}}\quad{{\nabla _x}}\quad{{\nabla _y}}\quad{{\nabla _z}} ]^{\rm{T}}$ 其状态方程为：

${\dot X_{SINS}} = {F_{SINS}}{X_{SINS}} + {w_{SINS}}{\text{，}}$

(4)

对于LBL定位系统，选取等效距离误差 $\delta {t_u}$ 为状态量，其状态方程可表示为：

${\dot X_{LBL}} = {F_{LBL}}{X_{LBL}} + {w_{LBL}}{\text{，}}$

(5)

其中， ${X_{LBL}} = \delta {t_u},{F_{LBL}} = - \dfrac{1}{{{\tau _{tu}}}},{w_{LBL}} = {w_{tu}}$ 。

由式（4）和式（5）可得，伪距辅助的SINS/LBL组合导航系统16维状态量表示为：

$ X = {\left[ {\begin{array}{*{20}{c}} {{X_{SINS}}}&{{X_{LBL}}} \end{array}} \right]^{\rm{T}}}{\text{，}} $

(6)

其状态方程表示为：

$\dot X = FX + w = \left[\!\!\! {\begin{array}{*{20}{l}} {{F_{SINS}}}&0 \\ 0&{{F_{LBL}}} \end{array}} \!\!\!\right]\left[\!\!\! {\begin{array}{*{20}{c}} {{X_{SINS}}} \\ {{X_{LBL}}} \end{array}} \!\!\!\right] + \left[\!\!\! {\begin{array}{*{20}{c}} {{w_{SINS}}} \\ {{w_{LBL}}} \end{array}}\!\!\! \right]{\text{。}}$

(7)

1.3 量测方程

由于水声信号时延，AUV接收到海底应答器发出信号所在的位置并不一致，即采用几何交汇法无法解算出位置坐标。AUV由惯导解算出的位置坐标表示为 $({x_{SINS}},{y_{SINS}},{z_{SINS}})$ ，设 ${t_i}$ 时刻AUV接收到第 $i$ 个应答器的水声信号，以 $\rho _{SINS}^i$ 表示AUV到第 $i$ 个应答器的伪距，则 $\rho _{SINS}^i$ 表示为：

$\begin{split}&\rho _{SINS}^i = \\ &\sqrt {{{({x_{SINS}} \!-\! \delta {x_{ti}} \!-\! {x^i})}^2} \!+\! {{({y_{SINS}} \!-\! \delta {y_{ti}} \!-\! {y^i})}^2} \!+\! {{({z_{SINS}} \!-\! \delta {z_{ti}} \!-\! {z^i})}^2}}{\text{，}} \end{split}$

(8)

其中， $(\delta {x_{ti}},\delta {y_{ti}},\delta {z_{ti}})$ 表示为第 ${t_i}$ 时刻起AUV移动的位置增量，即需先计算得出AUV在 ${t_i}$ 时刻位置，然后进行伪距计算。

对式（8）在AUV真实位置 $(X,Y,Z)$ 处进行泰勒展开，伪距方程可简化为：

$\rho _{SINS}^i = {R^i} + e_X^i\partial X + e_Y^i\partial Y + e_Z^i\partial Z{\text{，}}$

(9)

其中， ${R^i}$ 为AUV到第 $i$ 个应答器的真实距离。

$ \begin{split} \left\{ \begin{aligned} & {R^i} = \sqrt {{{(X - \delta {x_{ti}} - {x^i})}^2} + {{(Y - \delta {y_{ti}} - {y^i})}^2} + {{(Z - \delta {z_{ti}} - {z^i})}^2}} {\text{，}} \\ & e_X^i = \frac{{X - \delta {x_{ti}} - {x^i}}}{{{R^i}}}{\text{，}} \\ & e_Y^i = \frac{{Y - \delta {y_{ti}} - {y^i}}}{{{R^i}}} {\text{，}} \\ & e_Z^i = \frac{{Z - \delta {z_{ti}} - {z^i}}}{{{R^i}}} {\text{。}} \end{aligned} \right.\\[-50pt] \end{split} $

(10)

AUV到第 $i$ 个应答器的伪距量测值可表示为：

$\rho _{LBL}^i = {R^i} - \delta {t_u} - v_\rho ^i{\text{，}}$

(11)

其中，量测误差 $v_\rho ^i$ 为白噪声。

伪距辅助的SINS/LBL组合导航系统量测量表示为：

$Z = {\rho _{SINS}^{\;}} - {\rho _{LBL}^{\;}} = \left[ {\begin{array}{*{20}{c}} {\rho _{SINS}^1 - \rho _{LBL}^1} \\ {\rho _{SINS}^2 - \rho _{LBL}^2} \\ {\rho _{SINS}^3 - \rho _{LBL}^3} \\ {\rho _{SINS}^4 - \rho _{LBL}^4} \end{array}} \right]{\text{，}}$

(12)

伪距辅助的SINS/LBL组合导航系统量测方程表示为：

$\begin{split}Z =& HX + v = [\begin{array}{*{20}{c}} {{{\bf{0}}_{4 \times 6}}}&h&{{{\bf{0}}_{4 \times 6}}}&{{{\bf{1}}_{4 \times 1}}} \end{array}]\left[ {\begin{array}{*{20}{c}} {{X_{SINS}}} \\ {{X_{LBL}}} \end{array}} \right] +\\ &{\left[ {\begin{array}{*{20}{c}} {v_\rho ^1}&{v_\rho ^2}&{v_\rho ^3}&{v_\rho ^4} \end{array}} \right]^{\rm{T}}}{\text{。}}\end{split}$

(13)

其中：

$\begin{split}&h = \left[ {\begin{array}{*{20}{c}} {e_X^1}&{e_Y^1}&{e_Z^1} \\ {e_X^2}&{e_Y^2}&{e_Z^2} \\ {e_X^3}&{e_Y^3}&{e_Z^3} \\ {e_X^4}&{e_Y^4}&{e_Z^4} \end{array}} \right]\times\\ &\left[\!\! {\begin{array}{*{20}{c}} { - ({R_N} \!+\! h)\sin L\cos \lambda }\!\!&\!\!{ - ({R_N} \!+\! h)\cos L\sin \lambda }\!\!&\!\!{\cos L\cos \lambda } \\ { - ({R_N} \!+\! h)sinL\sin \lambda }\!\!&\!\!{({R_N} \!+\! h)\cos L\cos \lambda }\!\!&\!\!{\cos L\sin \lambda } \\ {[{R_N}(1 \!-\! {e^2}) \!+\! h]cosL}\!\!&\!\!0\!\!&\!\!{\sin L} \end{array}}\!\! \right]{\text{。}}\end{split}$

(14)

其中， ${R_N}$ 为卯酉圈曲率半径， $e$ 为旋转椭球扁率。

2 自适应滤波算法

组合导航系统通过滤波算法实现最优估计，传统的组合导航系统一般采用Kalman滤波作为信息融合手段，但是Kalman滤波需要构建噪声模型，由于水下环境产生的干扰复杂，往往无法对其进行精确建模，一般采用固定值直接带入滤波方程，这会极大影响Kalman滤波的稳定性及组合导航精度。为此，本文采用自适应滤波算法估计组合导航系统误差，对系统量测噪声的协方差阵 $R$ 进行在线辨识，实现对AUV姿态、速度、位置信息的最优估计。

式（7）、式（13）离散化后系统状态方程及量测方程如下：

$\left\{ \begin{array}{l} {{\dot X}_k} = F{}_{k,k - 1}{X_{k - 1}} + {w_{k - 1}} {\text{，}} \\ {Z_k} = {H_k}{X_k} + {v_k} {\text{，}} \end{array} \right.$

(15)

自适应滤波算法主要通过引入遗忘因子 $b$ ，不断强化量测量 ${Z_k}$ 的作用，并削弱 ${Z_1} \cdots {Z_{k - 1}}$ 的影响，通过新息 ${\varepsilon _k}$ 在线估计协方差阵 ${R_k}$ ，并进一步优化滤波增益阵 ${K_k}$ ，从而影响最终滤波结果。自适应滤波算法流程如下：

$\left\{ \begin{aligned} & {d_k} = {{(1 - b)} / {(1 - {b^k})}}{\text{，}} \\ & {X_{k/k - 1}} = {F_{k,k - 1}}{X_{k - 1}} {\text{，}} \\ & {P_{k/k - 1}} = {F_{k,k - 1}}{P_{k - 1}}F_{_{k,k - 1}}^{\rm{T}} + {Q_{k - 1}}{\text{，}} \\ & {\varepsilon _k} = {Z_k} - {H_k}{X_{k/k - 1}} {\text{，}} \\ &{R_k} = (1 - {d_k}){R_{k - 1}} + {d_k}[{\varepsilon _k}\varepsilon _k^{\rm{T}} - {H_k}{P_{k/k - 1}}H_k^{\rm{T}}]{\text{，}} \\ & {K_k} = {P_{k/k - 1}}H_k^{\rm{T}}{({H_k}{P_{k/k - 1}}H_k^{\rm{T}} + {R_k})^{ - 1}}{\text{，}} \\ &{X_k} = {X_{k/k - 1}} + {K_k}{\varepsilon _k} {\text{，}} \\ & {P_k} = (I - {K_k}{H_k}){P_{k/k - 1}} {\text{，}} \end{aligned} \right.$

(16)

3 仿真结果分析 3.1 仿真环境及运动轨迹

设4个海底应答器之间间隔1000 m，AUV初始姿态为 $(\psi ,\theta ,\gamma ) = (0,0,0)$ ，初始速度为 $({v_E},{v_N},{v_U}) = (0,0,0)$ ，初始位置为 $(\lambda ,L,h) = (0,0,0)$ 。采样频率100 Hz，总航时3260 s。采用Matlab对AUV运动轨迹进行仿真，其三维运动轨迹如图3所示。

3.2 仿真结果对比

在建立状态与量测方程的基础上，用Matlab对本文提出组合算法进行仿真试验。考虑到惯导的高度通道发散，AUV可直接通过深度计测量深度信息，故重点针对二维平面导航定位精度进行对比分析。AUV依照图3所示轨迹运动，在纯惯导算法及在伪距辅助的长基线组合导航算法下的二维平面仿真轨迹对比如图4所示。

由图4可知，在3260 s的航时中，随着时间累积，惯性导航定位误差出现增长发散情况，导致纯惯导算法轨迹与真实轨迹出现千米级偏移量。这说明在无外界信息参考的情况下，仅依靠纯惯导无法满足AUV水下导航定位精度要求。而在图4（b）中，伪距辅助的长基线组合导航算法由于提供了位置参考，抑制了惯导误差累积，其轨迹基本能够跟随真实轨迹，没有出现误差发散情况，导航定位精度较高。

为进一步模拟水下复杂噪声干扰情况，仿真时增强量测噪声协方差阵 ${{R}}$ ，用以验证自适应滤波算法针对噪声统计模型未知情况下的修正能力及算法有效性。AUV依照图3所示轨迹运动，采用本文提出的基于自适应滤波的长基线导航定位算法与基于传统Kalman滤波的长基线组合导航算法的速度、位置误差进行对比，仿真与分析结果如图5和图6所示。

可知，在噪声出现大幅变化的情况下，组合导航系统的信息融合算法将影响最终的导航定位精度。如图5（b）和图6（b）所示，由于量测噪声协方差阵 ${{R}}$ 未知，仿真初始阶段误差震荡幅度较大。在航行时间2000 s后，由于自适应滤波算法对 ${{R}}$ 进行在线估计，采用自适应滤波算法的长基线组合导航系统误差基本收敛，其东向、北向速度误差在 $ \pm 0.05\;{\rm{m/s}}$ 范围内波动，东向、北向位置误差在 $ \pm 10\;{\rm{m}}$ 范围内波动。对比图5（a）和图6（a），在航行时间2 000 s后，采用传统Kalman滤波算法的长基线组合导航系统的东向、北向速度误差达到 $ - 0.1\;{\rm{m/s}}$ ，且有发散趋势，其东向位置误差达到 $ - 15\;{\rm{m}}$ ，北向位置误差达到 $ - 27\;{\rm{m}}$ 。这说明在环境噪声干扰较强时，即使采用长基线通过位置校正惯导，也无法达到高精度导航定位要求，甚至无法确保系统的导航定位误差收敛。

4 结　语

本文提出一种基于自适应滤波的水下长基线导航定位技术，通过伪距辅助的长基线组合导航系统对惯导进行校正，防止导航误差随时间累积；通过自适应滤波算法对噪声进行在线辨识，实现对系统导航定位参数的最优估计，提高系统的导航定位精度及可靠性。仿真结果表明，面对水下复杂的噪声干扰，本文设计的水下组合导航算法具有较强的抗干扰性能，并能够满足AUV水下长航时、高精度的导航定位要求。