0 引　言

当前，应用于AUV上常见的导航定位装备主要包括惯性导航系统（INS）、多普勒计程仪（DVL）以及水声定位系统^[1]。其中INS基于惯性原理工作，不受外界环境影响，能够自主解算出载体的姿态、速度、位置参数，但在水下长时间导航过程中，误差不断累积，单独使用效果不佳，一般用组合导航方法对其进行校正^[2]。DVL能够测量AUV对底、对流速度，可修正INS速度信息，但其对位置精度提升具有局限性，无法实现精准定位^[3]。水声定位系统一般分为超短基线（USBL）、短基线（SBL）、长基线（LBL），通过测量应答器与声基阵之间的水声信号传播时间或相位差来求解载体位置信息^[4-5]，其中LBL作用范围广、定位精度高，通过LBL对INS进行校正，可以获得高精度水下导航定位信息。

在AUV采用INS/LBL组合导航系统进行水下导航定位的过程中，受到水下复杂环境影响，仍难以保证定位精度。一方面是由于水声信号传播过程收到干扰而导致系统组合导航模型不够精准；另一方面是传统组合导航采用卡尔曼滤波方法进行数据融合，而常规卡尔曼滤波需要建立精确的系统先验模型，且无法抑制非高斯噪声，这在实际工程应用中难以实现^[6]。在水下复杂背景下的组合导航方法上，高胜峰^[7]采用序贯滤波方法引入组合导航模型，在长基线信号部分缺失的情况下保证定位精度；董萍等^[8]提出一种基于相对信息观测量的INS/USBL非线性组合导航方法，能够解决非线性系统中噪声统计特性未知的问题；杨建^[9]提出用滚动时域估计算法解决水声通信延时、丢包问题。

针对水下实际应用中，水声信号传播误差及复杂噪声干扰导致的定位精度受限、定位鲁棒性差的问题，本文设计了一种基于最大熵卡尔曼滤波的水下INS-LBL组合导航方法，通过引入时延误差建立系统状态方程及量测方程，同时采用最大熵卡尔曼滤波进行数据融合，最后通过仿真验证算法有效性。

1 INS/LBL组合导航系统模型 1.1 系统原理

LBL定位系统一般由固定在海底的声基阵与安装在AUV上的换能器组成，海底声基阵由数量不小于3个的应答器组成，其距离可达100~6000 m。工作时，海底应答器位置已知，每个应答器以预定周期发送频率不同的水声信号, 当AUV接收到水声信号时，可通过同步时钟测得传播时间，并计算得出与不同应答器之间的相对距离，建立定位方程从而求解自身位置。

本文设计的水下导航定位系统，利用INS与LBL对同一状态的输出作为量测值，通过最大熵卡尔曼滤波对2个系统的导航信息进行数据融合。传统INS-LBL组合导航系统一般直接用LBL水声定位获得的位置坐标对INS导航信息进行修正。但在实际任务中，由于水声信号在传输过程中产生时延，导致AUV在运动过程中接收声学信号时出现位置偏移，即实际量测的应答器与AUV的距离并非真实距离，因此引入伪距来建立组合导航模型。

1.2 组合导航模型

组合导航系统状态方程由分系统INS、LBL两部分组成。对于INS系统，选取姿态角误差$ {\varphi _E}、{\varphi _N}、{\varphi _U} $，速度误差$ \delta {v_E}、\delta {v_N}、\delta {v_U} $，位置误差$ \delta L、\delta \lambda 、\delta h $，加速计偏差$ {\nabla _x}、{\nabla _y}、{\nabla _z} $，陀螺仪漂移$ {\varepsilon _{bx}}、{\varepsilon _{by}}、{\varepsilon _{bz}} $，作为INS系统状态量$ {X_{SINS}} $，则$ {X_{SINS}} $可表示：为${X_{SINS}} = $[$ {{\varphi _E}}\;\;{{\varphi _N}}\;\;{{\varphi _U}} \;\;{\delta {v_e}}\;\; {\delta {v_n}}\;\;{\delta {v_u}}\;\;{\delta L} \;\;{\delta \lambda }\;\;{\delta h}\;\;{{\varepsilon _x}} \;\;{{\varepsilon _y}}\;\;{{\varepsilon _z}} \;\;{{\nabla _x}}\;\;{{\nabla _y}}\;\; {{\nabla _z}} $]^T建立INS的状态方程：

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

(1)

对于LBL系统，已知海底应答器F₁、F₂、F₃、F₄位置坐标为$ ({x_{Fi}},{y_{Fi}},{z_{Fi}}),i = 1,2,3,4 $，AUV到F_i应答器的水声信号传播时间为$ {t_{Fi}} $，声速为$ c $，则AUV到F_i应答器的伪距$ \rho_{_{Fi}} $可表示为：

$ {\rho_{_{Fi}}} = c \cdot {t_{Fi}} 。$

(2)

由于海底应答器与AUV存在系统时间的钟差，而钟差是影响伪距的重要因素，因此选取钟差等效距离$ \delta {t_u} $作为LBL系统状态量，其误差表示为$ \delta {\dot t_u} = \delta {t_u} + {w_{tu}} $，其中$ {w_{tu}} $为等效距离误差噪声。

建立LBL的状态方程：

$ {\dot X_{LBL}} = {F_{LBL}}{X_{LBL}} + {w_{LBL}} = \delta {t_u} + {w_{tu}}。$

(3)

INS-LBL系统的状态方程为：

$ \left[ {\begin{array}{*{20}{c}} {{{\dot X}_{SINS}}} \\ {{{\dot X}_{LBL}}} \end{array}} \right] = \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]。$

(4)

在此基础上，建立系统量测方程。AUV在水下由INS实时提供位置坐标$ (x,y,z) $，通过模型计算的AUV相对于海底应答器的伪距$ \rho _{SINS}^{Fi} $为：

$ \begin{gathered} \rho _{SINS}^{Fi} = \sqrt {{{(x - {x_{Fi}})}^2} + {{(y - {y_{Fi}})}^2} + {{(z - {z_{Fi}})}^2}} \\ (i = 1,2,3,4)。\\ \end{gathered} $

(5)

但由于水声时延存在，AUV接收到应答器信号的位置并不一致，因此式(5)需将AUV的位移$ (\Delta x,\Delta y,\Delta z) $代入，表示为：

$ \rho _{SINS}^{Fi} = \sqrt {{{(x - \vartriangle x - {x_{Fi}})}^2} + {{(y - \vartriangle y - {y_{Fi}})}^2} + {{(z - \vartriangle z - {z_{Fi}})}^2}}。$

(6)

其在AUV坐标真值$ ({x_t},{y_t},{z_t}) $处展开得：

$ \begin{aligned}[b] \rho _{SINS}^{Fi} = &{r_i} + \frac{{{x_t} - \vartriangle x - {x_{Fi}}}}{{{r_i}}}\delta {x_t} + \\ &\frac{{{y_t} - \vartriangle y - {y_{Fi}}}}{{{r_i}}}\delta {y_t} + \frac{{{z_t} - \vartriangle z - {z_{Fi}}}}{{{r_i}}}\delta {z_t} \end{aligned}。$

(7)

式中：$ {r_i} $为距离真值，通过实际量测的AUV相对于海底应答器的伪距$ {\rho '_{Fi}} $为：

$ {\rho '_{Fi}} = {r_i} - \delta {t_u} - {v_i}。$

(8)

式中：$ {v_i} $为测量噪声。

INS-LBL系统的量测量构建为：$ Z = \rho _{SINS}^{Fi} - {\rho '_{Fi}}= $

$ \begin{gathered} \left[ {\begin{array}{*{20}{c}} \displaystyle {\frac{{{x_t} - \vartriangle x - {x_{F1}}}}{{{r_1}}}}&\displaystyle{\frac{{{y_t} - \vartriangle y - {y_{F1}}}}{{{r_1}}}}&\displaystyle{\frac{{{z_t} - \vartriangle z - {z_{F1}}}}{{{r_1}}}}&1 \\ \displaystyle{\frac{{{x_t} - \vartriangle x - {x_{F2}}}}{{{r_2}}}}&\displaystyle{\frac{{{y_t} - \vartriangle y - {y_{F2}}}}{{{r_2}}}}&\displaystyle{\frac{{{z_t} - \vartriangle z - {z_{F2}}}}{{{r_2}}}}&1 \\ \displaystyle{\frac{{{x_t} - \vartriangle x - {x_{F3}}}}{{{r_3}}}}&\displaystyle{\frac{{{y_t} - \vartriangle y - {y_{F3}}}}{{{r_3}}}}&\displaystyle{\frac{{{z_t} - \vartriangle z - {z_{F3}}}}{{{r_3}}}}&1 \\ \displaystyle{\frac{{{x_t} - \vartriangle x - {x_{F4}}}}{{{r_4}}}}&\displaystyle{\frac{{{y_t} - \vartriangle y - {y_{F4}}}}{{{r_4}}}}&\displaystyle{\frac{{{z_t} - \vartriangle z - {z_{F4}}}}{{{r_4}}}}&1 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {\delta x} \\ {\delta y} \\ {\delta z} \\ {\delta {t_u}} \end{array}} \right] \\ \end{gathered}。$

(9)

式中，$ (x,y,z) $与$ (L,\lambda ,h) $坐标系之间可通过转移矩阵进行坐标变换，构建系统量测方程为：

$ Z = H\left[ {\begin{array}{*{20}{c}} {{X_{SINS}}} \\ {{X_{LBL}}} \end{array}} \right] + v 。$

(10)

2 最大熵卡尔曼滤波算法

本文利用最大熵卡尔曼滤波算法，针对非高斯噪声背景下的组合导航系统建立滤波模型。

其算法流程如下：

步骤1　设置滤波器状态量初始估计$ {\hat X_{0/0}} $，初始系统噪声协方差$ {\hat P_{0/0}} $，构建核函数$ {G_\sigma }({e_i}) = \exp \left( - \displaystyle\frac{{||{e_i}|{|^2}}}{{2{\sigma ^2}}}\right) $。

步骤2　对状态量及噪声协方差阵进行更新：

$ \begin{aligned}[b] &{{\hat X}_{k/k - 1}} = {F_{k,k - 1}}{{\hat X}_{k - 1}} ，\\ &{\boldsymbol{P}_{k/k - 1}} = {F_{k,k - 1}}{P_{k - 1}}F_{_{k,k - 1}}^{\text{T}} + {Q_{k - 1}}。\end{aligned} $

(11)

同时，对系统噪声协方差阵$ {\boldsymbol{P}_{k/k - 1}} $与量测噪声协方差阵$ {\boldsymbol{R}_k} $进行三角分解：

$ \left[ {\begin{array}{*{20}{c}} {{\boldsymbol{P}_{k/k - 1}}}{} \\ {}& {{\boldsymbol{R}_k}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {B_{k/k - 1}^p{{(B_{k/k - 1}^p)}^{{\mathrm{T}}} }}{} \\ {}& {B_k^r{{(B_k^r)}^{{\mathrm{T}}} }} \end{array}} \right] = {B_k}{B_k}^{\text{T}} 。$

(12)

步骤3　根据时间更新结果，计算

$ \begin{split} & {D_k} = {B_k}^{ - 1}\left[ {\begin{array}{*{20}{c}} {{{\hat X}_{k/k - 1}}} \\ {{Z_k}} \end{array}} \right],{W_k} = {B_k}^{ - 1}\left[ {\begin{array}{*{20}{c}} I \\ {{H_k}} \end{array}} \right]，\\ & {e_k} = {D_k} - {W_k}{X_k} ，\\ & {P_{k/k - 1}} = B_{k/k - 1}^p{[{\rm diag}({G_\sigma }({e_{1,k}}), \cdots ,{G_\sigma }({e_{n,k}}))]^{ - 1}}{(B_{k/k - 1}^p)^{\text{T}}}，\\ & {R_k} = B_k^r{[{\rm diag}({G_\sigma }({e_{n + 1,k}}), \cdots ,{G_\sigma }({e_{n + m,k}}))]^{ - 1}}{(B_k^r)^{\text{T}}}，\\ & {K_k} = {P_{k/k - 1}}H_k^{\text{T}}{({H_k}{P_{k/k - 1}}H_k^{\text{T}} + {R_k})^{ - 1}}，\\ & {{\hat X}_k} = {{\hat X}_{k/k - 1}} + {K_k}({Z_k} - {H_k}{{\hat X}_{k/k - 1}})。\\[-1pt] \end{split} $

(13)

式中：n=16为16维状态量；m=4为4维量测量。

步骤4　若当前步骤估计$ {\hat X_k} $与步骤3估计的变化小于误差阈值$ \varepsilon $，则更新系统噪声协方差阵：

$ {P_k} = (I - {K_k}{H_k}){P_{k/k - 1}}。$

(14)

否则返回步骤3重复迭代。

可以看出，当系统受到噪声干扰时，通过引入残差$ {e_k} $，对系统噪声$ {P_{k/k - 1}} $与量测噪声$ {R_k} $进行最优估计，从而不断改善滤波增益$ {K_k} $，削弱对因噪声产生的异常量测值所占权重，有效分辨并利用量测信息，进而提高系统滤波精度，增强系统鲁棒性。

3 仿真分析

LBL系统布设在海底的应答器坐标为$ ( - 500, - 500), (1500,500),(1500,1500),( - 500,1500) $，AUV初始姿态为$ (\psi ,\theta ,\gamma ) = (0,0,0) $，初始速度为$ ({v_E},{v_N},{v_U}) = (0,0,0) $，初始位置为$ (\lambda ,L,h) = (0,0,0) $。采样频率100 Hz，总航时3880 s。采用Matlab对AUV运动轨迹进行仿真，其运动轨迹如图1和图2所示，由于INS高度通道发散，因此将三维运动简化成平面运动。

为验证算法有效性，模拟水下噪声干扰情况，在LBL系统给出的三维位置量测量中加入混合高斯噪声，表示为：

$ v{\text ～}90\text% N(0,R) + 10\text% N(0,4R)。$

(15)

图3～图6分别为同一仿真条件下使用常规卡尔曼与使用最大熵卡尔曼滤波的北向、东向速度误差对比与北向、东向位置误差对比，可以看出，基于最大熵卡尔曼滤波的速度、位置误差曲线波动幅度较小，使用卡尔曼滤波的北向位置最大误差达到21.54 m，东向位置最大误差达到25.27 m，而使用最大熵卡尔曼滤波的北向位置最大误差达到7.19 m，东向位置最大误差达到6.67 m。从表1可更直观看出，基于最大熵卡尔曼滤波的INS-LBL组合导航系统输出的速度、位置误差更低，在非高斯噪声干扰下，其定位精度明显提升，鲁棒性更强。

4 结　语

本文提出一种非高斯噪声背景的水下INS-LBL组合导航方法，考虑到水声通信时延较大，引入伪距建立组合导航系统的状态方程及量测方程，在此基础上，通过以最大熵为准则的卡尔曼滤波对系统进行数据融合，解决了水下非高斯噪声干扰下，传统方法定位精度下降的问题。仿真结果表明，在受到水下复杂噪声干扰的情况下，本文设计的非高斯噪声背景的水下INS-LBL组合导航方法具有更高的定位精度，能够抑制噪声干扰，满足AUV水下高精度自主定位需求。