0 引　言

无人水下航行器（Unmanned Underwater Vehicle, UUV）以其全面的水下作业能力，已然成为海洋探测、科学研究、生态保护以及国防等多个领域中最广泛应用的水下设备之一^[1]。水下导航和定位技术作为UUV能正常运行的基础，重要性日益凸显。然而，由于水下环境中无线电信号传播受限，传统的全球导航卫星系统（Global Navigation Satellite System，GNSS）无法直接应用于水下导航，使得水下导航定位面临着独特的挑战^[2]。

航位推算（Dead Reckoning，DR）方法和长基线（Long Baseline，LBL）水下定位方法被广泛采用。DR因其积分形式的计算方法，长时间运行会导致误差累积^[3]，可用LBL进行校正；传统长基线水下定位方法对测距数据的数量有较高的要求，但实际应用中常因海洋环境和各种噪声影响，无法接收到所需要数量信号，影响解算^[4]。曹方方等^[5]提出基于单信标的组合导航方法，虽然每次解算只需一个测距数据与航速、航向信息组合，但因系统可观性受限，需适当的机动航行路径，且定位结果收敛较慢。另一方面，现有的水声定位算法在应用时假设声速已知，但实际中声速难以实时测量，信号传播路径的声速分布更难以精确测量，这会对距离解算造成误差，最终影响定位精度^{[6 - 10]}。

根据实际实验的情况，本文提出一种自适应多信标DR/LBL紧组合导航方法。基于扩展卡尔曼滤波（Extended Kalman Filter，EKF）算法，在每个测量周期中，使用时延作为观测量，根据接收到的随机数量的时延信息，自适应构建观测方程，计算雅克比矩阵，对预测结果进行修正。此方法适应性强，更符合实际情况，定位效率更高。实验表明，本方法在各个测量周期中信标应答信号接收不稳定时，能够准确、快速的对UUV推算位置进行校正，实现快速收敛，具有较强的实用性。

1 DR/LBL组合导航运动模型

考虑到水声定位作用范围较小（十几公里范围内），可认为其处于水平直角坐标系，其位置用直角坐标$ （x,y） $表示。在较小范围和较短时间内，可认为声速分布固定，因实际应用中难以对声速分布特性进行实时测量，所以将应答器与UUV之间海域中的实际声速，等效为平均声速加入系统状态中进行估计^[11]。

考虑UUV深度已知，建立其二维运动方程，以其位置和声速作为状态，系统状态和外部输入分别为：

$ X = \left[ \begin{gathered} x \\ y \\ C \\ \end{gathered} \right]，$

(1)

$ u = \left[ \begin{gathered} v \\ \varphi \\ \end{gathered} \right]。$

(2)

式中：$(x,y)$为UUV的位置坐标；$\nu $为UUV的速度；$\varphi $为UUV航迹角。

建立运动学方程：

$ \dot X = \left[ \begin{gathered} (v + {\omega _v}) \cdot \cos (\varphi + {\omega _\varphi }) \\ (v + {\omega _v}) \cdot \sin (\varphi + {\omega _\varphi }) \\ {\mu _C} \\ \end{gathered} \right]。$

(3)

系统测量输出为：

$ {{Z}} = \left[ {\begin{array}{*{20}{c}} {\Delta {\tau _1}} \\ \vdots \\ {\Delta {\tau _i}} \end{array}} \right]。$

(4)

式中：$\Delta {\tau _{\text{i}}}$$ (i=1,2,3,4) $为UUV与各信标的应答信号传播时延^[6]：

$ \Delta {\tau _{\text{i}}} = \frac{{2\sqrt {{{(x - {x_{bi}})}^2} + {{(y - {y_{bi}})}^2}{\text{ + }}{{(h - {h_{bi}})}^2}} }}{C}。$

(5)

式中：$ {x_{bi}},{y_{bi}},{h_{bi}} $$ （i=1,2,3,4） $为信标的位置，布标完成后通过绕标获得；$h$为UUV的深度，其数值由自身携带的深度计获得。

将式(3)离散化，得离散系统方程：

$ {\left\{ \begin{array}{c}\\ \begin{array}{l}{X}_{k\text+1}=\left[ \begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array} \right]{X}_{k}+\left[ \begin{array}{c}\Delta T{\text{(v}}_{k}+{\omega }_{{\text{v}}_{k}})\cdot \mathrm{cos}({\phi }_{k}+{\omega }_{{\phi }_{k}})\\ \Delta T{\text{(v}}_{k}+{\omega }_{{\text{v}}_{k}})\cdot \mathrm{sin}({\phi }_{k}+{\omega }_{{\phi }_{k}})\\ 0\end{array} \right]+\left[ \begin{array}{c}0\\ 0\\ {\mu }_{{C}_{k}}\end{array} \right]，\\ {Z}_{k}=h({X}_{k})+{V}_{k}\text=\left[ \begin{array}{c}\displaystyle\frac{2\sqrt{{({x}_{k}-{x}_{b1})}^{2}+{({y}_{k}-{y}_{b1})}^{2}\text+{(h-{h}_{b1})}^{2}}}{{C}_{k}}\\ \vdots \\ \displaystyle\frac{2\sqrt{{({x}_{k}-{x}_{bi})}^{2}+{({y}_{k}-{y}_{bi})}^{2}\text+{(h-{h}_{bi})}^{2}}}{{C}_{k}}\end{array} \right]\text+{V}_{{k}}，\end{array}\end{array}\right.} $

(6)

式中：${X_k}$为状态向量；${Z_k}$为量测向量；$ {\omega _{{{\text{v}}_k}}} $和$ {\omega _{{\varphi _k}}} $分别为速度和航迹角的测量噪声；$ {\mu _{{C_k}}} $为每次测量时的声速随机扰动；${V_k}$为时延量测噪声向量，假设三者为零均值的高斯白噪声序列，且互不相关。

如图1所示，UUV通过航速、航向信息对位置进行推算；当其接近信标时，航行器发送问答信号，并接受信标的回信，通过测量在此期间信号的传播时延$ \Delta {\tau _i} $，结合平均声速C，便可计算UUV与信标${b_i}$之间的距离${R_i}$。

$ {R}_{i}=\frac{\Delta {\tau }_{i}\cdot C}{2}。$

(7)

现有LBL中，需要至少收到3个信标的距离信息，才能进行定位运算，对推算结果进行校正。在实际应用中，受海洋环境、UUV相对位置和航向等影响，在每一次测量过程中，UUV很难接收到全部3个或4个应答信号，此时便无法采用LBL计算位置，无法对推算进行标校。基于单信标的组合导航方法，虽然每次解算只需一个测距数据与航速和航向信息组合，但因环境噪声影响的随机性大，常常会出现在一次测量周期中，收到了测距信号，但不是算法指定的信标的测距信号，此时也无法进行解算，造成了数据的浪费。

2 自适应信标数量DR/LBL紧组合导航滤波方法

本文提出一种自适应多信标DR/LBL紧组合导航方法，其流程如图2所示。

式(6)构成一个非线性的组合导航模型，利用扩展卡尔曼滤波方法进行计算。

状态一步预测：

$ {\widehat{X}}_{k+1,k}=f\text{(}{\widehat{X}}_{k},{u}_{k}\text{,0)}=A{\widehat{X}}_{k}+\left[\begin{array}{cc}\Delta T& 0\\ 0& \Delta T\\ 0& 0\end{array}\right]\left[\begin{array}{c}{\text{v}}_{k}\cdot \mathrm{cos}{\varphi }_{k}\\ {\text{v}}_{k}\cdot \mathrm{sin}{\varphi }_{k}\end{array}\right]。$

(8)

式中：$ A = \left[ {\begin{array}{*{20}{c}} 1&0&0 \\ 0&1&0 \\ 0&0&1 \end{array}} \right] $；$ {u_k} = \left[ \begin{gathered} {v_k} \\ {\varphi _k} \\ \end{gathered} \right] $。

状态方程关于${w_k}$为非线性，其雅克比矩阵：

$ {F}_{\omega k}=\frac{\partial f}{\partial {w}_{k}}=\left[\begin{array}{ccc}\mathrm{cos}{\phi }_{k}\cdot \Delta {T}& -{\nu }_{k}\cdot \mathrm{sin}{\phi }_{k}\cdot \Delta {T}& 0\\ \mathrm{sin}{\phi }_{k}\cdot \Delta {T}& {\nu }_{k}\cdot \mathrm{cos}{\phi }_{k}\cdot \Delta {T}& 0\\ 0& 0& 1\end{array}\right] 。$

(9)

式中：$ {w_k}{\text{ = }}{\left[ {{v_k},{\varphi _k},{\mu _{{C_{k{\text{ - }}1}}}}} \right]^{\mathrm{T}}} $。

状态一步预测均方误差阵：

$ {P_{k + 1,k}} = A{P_k}{A^{\mathrm{T}}} + {F_{wk}}{Q_k}{F_{wk}}^{\mathrm{T}} 。$

(10)

式中：$ {Q_k} = \left[ {\begin{array}{*{20}{c}} {{\sigma _{{v_k}}}^2}&{}&{} \\ {}&{{\sigma _{{\varphi _k}}}^2}&{} \\ {}&{}&{{\sigma _{{C_k}}}^2} \end{array}} \right] $；$ {\sigma _{{v_k}}} $为航速的测量误差的标准差；$ {\sigma _{{\varphi _k}}} $为航向角的测量误差的标准差；$ {\sigma _{{C_k}}} $为声速随机扰动的标准差。

量测矩阵：

$\begin{aligned} {H_{k + 1}} = &{\left. {\frac{{\partial h({{\hat X}_{k + 1}})}}{{\partial {X_{k{\text{ + }}1}}}}} \right|_{{{\hat X}_{k + 1}} = {{\hat X}_{k + 1,k}}}} = \\ & \left[ {\begin{array}{*{20}{c}} \displaystyle{\frac{{2({{\hat x}_{k + 1,k}} - {x_{b1}})}}{{{{\hat C}_{k + 1,k}}{R_1}}}}& \displaystyle{\frac{{2({{\hat y}_{k + 1,k}} - {y_{b1}})}}{{{{\hat C}_{k + 1,k}}{R_1}}}}& \displaystyle{ - \frac{{2{R_1}}}{{{{\hat C}^2}_{k + 1,k}}}} \\ {}& \vdots &{} \\ \displaystyle{\frac{{2({{\hat x}_{k + 1,k}} - {x_{b{\text{i}}}})}}{{{{\hat C}_{k + 1,k}}{R_i}}}}& \displaystyle{\frac{{2({{\hat y}_{k + 1,k}} - {y_{bi}})}}{{{{\hat C}_{k + 1,k}}{R_i}}}}& \displaystyle{ - \frac{{2{R_i}}}{{{{\hat C}^2}_{k + 1,k}}}} \end{array}} \right]。\end{aligned}$

(11)

式中：$ {R_i} = \sqrt {{{({{\hat x}_{k + 1,k}} - {x_{bi}})}^2} + {{({{\hat y}_{k + 1,k}} - {y_{bi}})}^2}{\text{ + }}{{(h - {h_{bi}})}^2}}。$

滤波增益：

$ {K_{k + 1}} = P{ _{k + 1,k}}{H^{\text{T}}}_{k + 1}{({H_{k + 1}}P{ _{k + 1}}{H^{\text{T}}}_{k + 1} + {S_{k + 1}})^{ - 1}}。$

(12)

式中：$ {S_k} = \left[ {\begin{array}{*{20}{c}} {{\sigma _{\Delta {\tau _1}}}^2}&{}&0 \\ {}& \ddots &{} \\ 0&{}&{{\sigma _{\Delta {\tau _{\text{i}}}}}^2} \end{array}} \right] $为系统量测噪声协方差矩阵；$ {\sigma _{\Delta {\tau _{\text{i}}}}} $$ （i=1,2,3,4） $为时延的量测误差的标准差。

系统量测矩阵和系统量测协方差矩阵的维度根据收到的测距信号的数量而相应改变。例如，当仅接收到“信标2”、“信标3”的应答时，量测矩阵为：

$ {H_{k + 1}} = \left[ {\begin{array}{*{20}{c}} \displaystyle{\frac{{2({{\hat x}_{k + 1,k}} - {x_{b2}})}}{{{{\hat C}_{k + 1,k}}{R_2}}}}& \displaystyle{\frac{{2({{\hat y}_{k + 1,k}}- {y_{b2}})}}{{{{\hat C}_{k + 1,k}}{R_2}}}}& \displaystyle{ - \frac{{2{R_2}}}{{{{\hat C}^2}_{k + 1,k}}}} \\ \displaystyle{\frac{{2({{\hat x}_{k + 1,k}} - {x_{b3}})}}{{{{\hat C}_{k + 1,k}}{R_3}}}}& \displaystyle{\frac{{2({{\hat y}_{k + 1,k}} - {y_{b3}})}}{{{{\hat C}_{k + 1,k}}{R_3}}}}& \displaystyle{ - \frac{{2{R_3}}}{{{{\hat C}^2}_{k + 1,k}}}} \end{array}} \right] 。$

(13)

式中：

$ \begin{gathered} {R_2} = \sqrt {{{({{\hat x}_{k + 1,k}} - {x_{b2}})}^2} + {{({{\hat y}_{k + 1,k}} - {y_{b2}})}^2}{\text{ + }}{{(h - {h_{b2}})}^2}}，\\ {R_3} = \sqrt {{{({{\hat x}_{k + 1,k}} - {x_{b3}})}^2} + {{({{\hat y}_{k + 1,k}} - {y_{b3}})}^2}{\text{ + }}{{(h - {h_{b3}})}^2}}。\\ \end{gathered} $

(14)

系统量测噪声协方差矩阵为：

$ {S_k} = \left[ {\begin{array}{*{20}{c}} {{\sigma _{\Delta {\tau _2}}}^2}&0 \\ 0&{{\sigma _{\Delta \tau 3}}^2} \end{array}} \right]。$

(15)

状态估计：

$ {\hat X_{k + 1}} = {\hat X_{k + 1,k}} + {K_{k + 1}}\left[ {{Z_{k + 1}} - h({{\hat X}_{k + 1,k}})} \right]。$

(16)

根据每个信标的位置，提前将其在量测矩阵(11)中所对应的公式提前写好，实际应用中，根据当前测量周期实际数据接收情况，自适应的组成对应的量测矩阵参与滤波。

3 实验验证

为了验证本文提出的自适应多信标DR/LBL紧组合导航方法在实际应用中的效果，在青岛近海进行验证实验。采用实验船搭载卫星接收机和水声问答机模拟航行器的真实行驶过程。

首先，将4个信标投入海中，构成边长为1 km左右的矩形长基线阵，位置标定后，得到长基线阵4个信标的准确位置如表1所示。

实验船携带实验设备在长基线阵中移动，水声问答机持续向各个信标发送问答信号并接收回令，记录信号传播时间；卫星接收机不断解算实验船的位置信息，利用卫星定位信息做位置差分，再根据多普勒计程仪和罗经的数据特性分别加入方差为0.2(m/s)²和0.6°²的高斯噪声，来模拟航向和航速数据用于后续解算。

通过声速剖面仪测得海水平均声速为1470 m/s。实验船出发后，绕行“信标3”一周，然后横穿整个长基线阵。航行轨迹和信标布设位置如图3所示。数据显示，UUV在很多测量周期中都只能接收到1个或2个信标的应答信号，不利于LBL单独使用，如图4所示；通过分析UUV与各个信标单独的测距信号接收情况，发现即便已经行驶在便于对“信标3”信号收发的航线，仍有许多测量周期中，没有接收到“信标3”的测距信号，但接收到了其他信标的测距信号，不利于基于单信标的组合方法的使用，如图5所示。

根据实测数据，人为加入了840 m的初始位置偏差，以模拟水下航行器长时间航行造成的位置累积误差。分别采用DR、基于单信标的DR/LBL紧组合导航方法（固定接收信标3的测距信号）和自适应多信标DR/LBL紧组合导航方法进行解算，解算结果如图6所示，定位误差对比如图7所示，可知，航位推算的解算误差虽在实验期间没有明显增大，但其无法对自身累积误差进行修正，所以只适合在位置校正后的短时间内使用；基于单信标的DR/LBL紧组合导航方法的定位结果，虽然能在初始阶段缓慢收敛，但因系统可观性受限，解算误差没有收敛到可用的精度范围内；而自适应多信标DR/LBL紧组合导航方法，因数据的利用率高，系统可观性好，定位结果能很快的收敛并维持在较高的精度内。

平均声速估计结果如图8所示，经过一段时间后也收敛到了真实值附近，为位置解算提供了良好的支撑。

4 结　语

本文所述的导航算法，能在水下航行器利用长基线进行位置校正时，自适应的处理因受海洋环境等因素影响而造成的在不测量周期中信标测距信号各种接收情况，相较于基于单信标的DR/LBL紧组合导航算法，数据利用率高、解算收敛更快，解算结果也更稳定，并且可以实时估计海水的平均声速，以应对海水声速在一定区域内测量困难的问题。

本文假设通过将测得的时延数据乘以等效平均声速，可以得出信标与水下航行器之间斜距的2倍。然而，这个假设建立在UUV在测距信号发送到接收的整个过程中位置保持不变的前提之上，这与实际情况并不一致。在实际的信号收发过程中，UUV的位置并不始终保持不变，导致测量数据存在一定的误差。因此，我们需要进一步深入研究这一问题，以更准确地理解和解决由于UUV位置变化引起的数据误差。