随着国家海洋战略的推进, 高精度的水下定位技术在获取大范围海洋空间及环境数据, 进行海洋勘探与开发的应用越来越广泛^[1]。当前的水下声学定位模型常采用“圆曲线”或“双曲线”模型, 解算时将非线性观测方程转换成为近似线性的方程, 然后采用最小二乘方法进行水下目标的坐标求解^[2]。由于泰勒级数展开法在处理非线性方程有着解算精度高、收敛速度快、适用性强和稳健性强等特点, 当前水下声学定位的目标解算多是采用该方法。但它对初始参考值的依赖性强, 如果提供的初始值不正确, 会导致迭代次数过多或者结果不收敛, 而实际中我们很难获取水下目标的先验位置信息, 无法提供可靠的参考初始值^[3-4]。相比于泰勒级数展开法, Chan算法则是一种求解曲线方程组的非递归算法, 该算法特点是不需设定初始值, 计算量小, 无需作迭代计算, 只需两次最小二乘即可解算出结果^[5], 但对测量误差有一定要求。

本文提出了一种结合Chan算法与泰勒级数展开法的协同水下声学定位算法, 先用Chan算法求取出目标的初始位置, 以此作为参考值再通过泰勒级数展开法求取出目标的精确位置。实验结果表明该方法可行, 且能满足一定的定位需求。

1 算法原理 1.1 水下定位原理

进行水下声学定位时, 安置在船底的换能器发射声波信号, 水下应答器接收信号并给出反馈信号, 换能器接收到反馈信号计算出传播时间, 通过声速剖面仪等获取的声速乘以单向传播时间得到距离观测值。理论上获取3个以上的距离观测可交汇出应答器的三维坐标。

假设应答器的位置为(X, Y, Z), 换能器在第i个观测历元的位置为(x_i, y_i, z_i)声波单向传播时间为t_i, c为改正后的声速^[6], 则

$ \sqrt{\left(x_{i}-X\right)^{2}+\left(y_{i}-Y\right)^{2}+\left(z_{i}-Z\right)^{2}}=c t_{i}, $

(1)

获取多组观测值后使用最小二乘方法进行解算。由于观测方程为非线性方程, 不能直接进行运算, 为了方便计算, 通常使用泰勒级数展开法将上式转换为近似的线性方程然后进行求解^[7]。

1.2 泰勒级数展开法

泰勒级数展开法(又称迭代法)是一种递归法^[4], 假定应答器的初始估计位置已知或由经验给出, 将方程组在初始位置处作泰勒级数展开, 忽略二阶以上分量, 得改正方程^[8]:

$ l = \mathit{\boldsymbol{A}}{\rm{d}}{x_0} + \varepsilon , $

(2)

式中: l为常数项, A为观测方程系数矩阵, ε为观测中的偶然误差。其具体表达式为:

$ l = \left[ {\begin{array}{*{20}{c}} {c{t_1} - \sqrt {{{\left( {{x_1} - {X^0}} \right)}^2} + {{\left( {{y_1} - {Y^0}} \right)}^2} + {{\left( {{z_1} - {Z^0}} \right)}^2}} }\\ {c{t_2} - \sqrt {{{\left( {{x_2} - {X^0}} \right)}^2} + {{\left( {{y_2} - {Y^0}} \right)}^2} + {{\left( {{z_2} - {Z^0}} \right)}^2}} }\\ {\begin{array}{*{20}{c}} \vdots \\ {c{t_n} - \sqrt {{{\left( {{x_n} - {X^0}} \right)}^2} + {{\left( {{y_n} - {Y^0}} \right)}^2} + {{\left( {{z_n} - {Z^0}} \right)}^2}} } \end{array}} \end{array}} \right], $

(3)

(X⁰, Y⁰, Z⁰)为应答器近似坐标, 令$\sqrt {{{\left( {{x_i} - {X^0}} \right)}^2} + {{\left( {{y_i} - {Y^0}} \right)}^2} + {{\left( {{z_i} - {Z^0}} \right)}^2}} = {f_i}$, 那么

$ \mathit{\boldsymbol{A}} = \left[ {\begin{array}{*{20}{c}} {\begin{array}{*{20}{c}} {\frac{{\partial {f_1}}}{{\partial x}}}&{\frac{{\partial {f_1}}}{{\partial y}}}&{\frac{{\partial {f_1}}}{{\partial z}}} \end{array}}\\ {\begin{array}{*{20}{c}} {\frac{{\partial {f_2}}}{{\partial x}}}&{\frac{{\partial {f_2}}}{{\partial y}}}&{\frac{{\partial {f_2}}}{{\partial z}}}\\ \vdots & \vdots & \vdots \\ {\frac{{\partial {f_n}}}{{\partial x}}}&{\frac{{\partial {f_n}}}{{\partial y}}}&{\frac{{\partial {f_n}}}{{\partial z}}} \end{array}} \end{array}} \right], $

(4)

其中: $\frac{{\partial {f_i}}}{{\partial x}} = \frac{{{X^0} - {x_i}}}{{f_i^0}}$, $\frac{{\partial {f_i}}}{{\partial y}} = \frac{{{Y^0} - {y_i}}}{{f_i^0}} $, $\frac{{\partial {f_i}}}{{\partial z}} = \frac{{{Z^0} - {z_i}}}{{f_i^0}}$; ${\rm{d}}{x_0} = \left[ {\begin{array}{*{20}{c}} {\Delta X}\\ {\Delta Y}\\ {\Delta Z} \end{array}} \right]$表示坐标的改正值, $\varepsilon = \left[ {\begin{array}{*{20}{c}} {{\varepsilon _1}}\\ {{\varepsilon _2}}\\ \vdots \\ {{\varepsilon _n}} \end{array}} \right]$表示测量的误差。

依据最小二乘原理可得:

$ {\rm{d}}{x_0} = \left[ {\begin{array}{*{20}{c}} {\Delta X}\\ {\Delta Y}\\ {\Delta Z} \end{array}} \right] = {\left( {{\mathit{\boldsymbol{A}}^{\rm{T}}}\mathit{\boldsymbol{A}}} \right)^{ - 1}}{\mathit{\boldsymbol{A}}^{\rm{T}}}l, $

(5)

进行下一次迭代时, 令X¹ = X⁰ + ΔX, Y¹ = Y⁰ + ΔY, Z¹ = Z⁰ + ΔZ, 重复以上步骤, 直到dx₀足够小则迭代停止, 在定位中一般设定迭代终止阀值为1 mm。

泰勒级数展开法可以得到精确的定位结果, 但是它的缺点是需要一个与实际位置偏差较小的初始值来保证算法的收敛, 这在实际情况中并不容易实现, 且不能提前判断何时出现不收敛情况。

1.3 Chan算法

在一次观测中, 获取到了n组观测数据, 待定位应答器的坐标为(X, Y, Z), 第i个观测历元的换能器位置为(x_i, y_i, z_i), 根据测量得到的时间信息及声速可得换能器与应答器之间的观测方程为^[9-12]:

$\begin{array}{l} R_i^2 = {\left( {{x_i} - X} \right)^2} + {\left( {{y_i} - Y} \right)^2} + {\left( {{z_i} - Z} \right)^2}\\ \;\;\;\;\; = {K_i} - 2{x_i}X - 2{y_i}Y - 2{z_i}Z + R = {\left( {c{t_i}} \right)^2}, \end{array}$

(6)

其中: ${K_i} = x_i^2 + y_i^2 + z_i^2$, i=1, 2, 3, …n, R = X² + Y² + Z², c为声速。

令${\mathit{\boldsymbol{Z}}_{\rm{a}}} = \left[ {\mathit{\boldsymbol{Z}}_{\rm{p}}^{\rm{T}}, \mathit{\boldsymbol{R}}} \right]$为未知向量, 其中Z_p=[X, Y, Z]^T, 从式(6)建立以Z_a为变量的线性方程为:

$ h=\boldsymbol{G}_{\mathrm{a}} \boldsymbol{Z}_{\mathrm{a}}, $

(7)

式中:

$ \mathit{\boldsymbol{h}} = \frac{1}{2}\left[ {\begin{array}{*{20}{c}} {R_1^2 - {K_1}}\\ {R_2^2 - {K_2}}\\ \vdots \\ {R_n^2 - {K_n}} \end{array}} \right], \;{\mathit{\boldsymbol{G}}_{\rm{a}}} = - \left[ {\begin{array}{*{20}{c}} {{x_1}}&{{y_1}}&{{z_1}}&1\\ {{x_2}}&{{y_2}}&{{z_2}}&1\\ \vdots & \vdots & \vdots & \vdots \\ {{x_n}}&{{y_n}}&{{z_n}}&1 \end{array}} \right], $

(8)

在实际测量中, 每个观测历元的换能器与应答器之间的距离可以由公式计算所得, 当测量误差较小时, 可构造如下协方差阵:

$\mathit{\boldsymbol{\varphi }} = {\mathit{\boldsymbol{B}}^{\rm{T}}}\mathit{\boldsymbol{B, }}$

(9)

式中B=diag{R₁, R₂, …, R_n}。依据加权最小二乘原理得:

${\mathit{\boldsymbol{Z}}_{\rm{a}}} = {\left( {{\mathit{\boldsymbol{G}}_{\rm{a}}}^{\rm{T}}{\mathit{\boldsymbol{\varphi }}^{ - 1}}{\mathit{\boldsymbol{G}}_{\rm{a}}}} \right)^{ - 1}}{\mathit{\boldsymbol{G}}_{\rm{a}}}^{\rm{T}}{\mathit{\boldsymbol{\varphi }}^{ - 1}}\mathit{\boldsymbol{h}}, $

(10)

此时的Z_a为第一次估计值, 设定Z_{a, i}为Z_a的第i个分量, 利用Z_a重新构造B₁矩阵进而得到新的协方差阵φ₁:

$ {\mathit{\boldsymbol{B}}_1} = {\rm{diag\{ }}{\mathit{\boldsymbol{Z}}_{{\rm{a, }}1}} - {x_1}, {\mathit{\boldsymbol{Z}}_{{\rm{a, 2}}}} - {y_1}, {\mathit{\boldsymbol{Z}}_{{\rm{a}}, 3}} - {z_1}, {\mathit{\boldsymbol{Z}}_{{\rm{a}}, 4}}\} , $

(11)

${\mathit{\boldsymbol{\varphi }}_1} = 4{\mathit{\boldsymbol{B}}_1}^{\rm{T}}{\mathit{\boldsymbol{B}}_1}, $

(12)

然后依据加权最小二乘原理得到第二次的估计值:

$ {\mathit{\boldsymbol{Z'}}_{\rm{a}}} = {\left( {{{\mathit{\boldsymbol{G'}}}_{\rm{a}}}^{\rm{T}}\mathit{\boldsymbol{\varphi }}_1^{ - 1}{{\mathit{\boldsymbol{G'}}}_{\rm{a}}}} \right)^{ - 1}}{\mathit{\boldsymbol{G}}_{\rm{a}}}^{\rm{T}}\mathit{\boldsymbol{\varphi }}_1^{ - 1}{\mathit{\boldsymbol{h}}_1}, $

(13)

式中: ${\mathit{\boldsymbol{Z'}}_{\rm{a}}} = \left[ {\begin{array}{*{20}{c}} {{{(X - {x_1})}^2}}\\ {{{(Y - {y_1})}^2}}\\ {{{(Z - {z_1})}^2}} \end{array}} \right], \;{\mathit{\boldsymbol{G'}}_{\rm{a}}} = \left[ {\begin{array}{*{20}{c}} 1&0&0\\ 0&1&0\\ 0&0&1\\ 1&1&1 \end{array}} \right], \;{\mathit{\boldsymbol{h}}_1} = \left[ {\begin{array}{*{20}{c}} {{{({Z_{{\rm{a}}, 1}} - {x_1})}^2}}\\ {{{({Z_{{\rm{a}}, 2}} - {y_1})}^2}}\\ {{{({Z_{{\rm{a}}, 3}} - {z_1})}^2}}\\ {Z_{{\rm{a}}, 4}^2} \end{array}} \right]$

最终应答器的定位计算结果为:

$ {\left( {X, Y, Z} \right)^{\rm{T}}} = \pm \sqrt {{{\mathit{\boldsymbol{Z'}}}_{\rm{a}}}} {({x_1}, {y_1}, {z_1})^{\rm{T}}}, $

(14)

正负号的选取, 以X坐标分量取值为例, 若$\left| {\left( {\sqrt {{{\mathit{\boldsymbol{Z'}}}_{\rm{a}}}} + {x_1}} \right) - {\mathit{\boldsymbol{Z}}_{{\rm{a}}, 1}}} \right| < \left| {\left( { - \sqrt {{{\mathit{\boldsymbol{Z'}}}_{\rm{a}}}} + {x_1}} \right) - {\mathit{\boldsymbol{Z}}_{{\rm{a}}, 1}}} \right|$则取正号, 若$ \left| {\left( {\sqrt {{{\mathit{\boldsymbol{Z'}}}_{\rm{a}}}} + {x_1}} \right) - {\mathit{\boldsymbol{Z}}_{{\rm{a}}, 1}}} \right| \ge \left| {\left( { - \sqrt {{{\mathit{\boldsymbol{Z'}}}_{\rm{a}}}} + {x_1}} \right) - {\mathit{\boldsymbol{Z}}_{{\rm{a}}, 1}}} \right|$则取负号, Y与Z坐标分量的选取同X。Chan算法特点是计算量小, 仅需两次迭代, 但随着定位距离和观测误差的增加, 定位精度下降^[13]。

1.4 协同算法

由于泰勒级数展开法初始值的选取对定位结果影响很大, 初始值的选取不当会导致算法的发散, 而Chan算法可以大致地估算出目标的位置, 因此可通过Chan算法进行初次定位, 将定位的结果作为泰勒级数展开的初始值, 再利用泰勒级数展开法进行迭代求解, 提高定位精度。若定位要求不高, Chan算法的定位结果亦可满足需求。协同算法流程如图 1所示。

2 仿真计算及结果分析

仿真计算背景是模拟海上石油勘探中对海底节点的声学二次定位, 以“走航式”方法进行测量。在100 m水深的海底布设间距为40 m的应答器阵列, 测量船以4 kn速度按照预规划的航线持续走航, 采样间隔为2 s, 模拟海浪振幅为1 m的余弦波动。测量船航迹与应答器相对位置示意图如图 2所示, 模拟的声速剖面如图 3所示。实验设置误差:换能器定位中误差各方向为10 cm, 应答器时延造成的测距误差为5 cm^{[7, 14]}, 系统误差模拟参考Xu文章中的方法^[15](软件: Matlab2017b;电脑: 4G RAM, CPU i5-6500)。

按照设计方案, 分别采用常规的泰勒级数展开法, Chan算法及本文方法进行解算, 共进行500次模拟计算并统计3种方法定位结果的均方根值(RMS)。仿真计算分为两种情况:一是不提供应答器坐标初始值; 另外一种是提供初始值。

(1) 不提供坐标初始值各方法解算结果

为了比较3种方法的定位效果, 以RMS作为结果统计对比, 计算如下:

$ \mathrm{RMS}=\sqrt{\Delta X^{2}+\Delta Y^{2}+\Delta Z^{2}}, $

(15)

式中, ΔX、ΔY、ΔZ分别为X、Y、Z这3个方向上解得坐标与真实坐标的平均偏差值。得到的解算结果和运行时间分别为图 4和图 5, 统计平均定位精度为表 1所示。

从图 4、表 1结果可以得出, 由于没有提供接近真实坐标的初始值, 泰勒级数展开法定位失败, 未能统计出RMS。通过表 1的对比, Chan算法可以粗略的对目标位置进行定位, 定位结果的RMS为0.669 m, 但每次的定位结果波动范围较大, 达1 m左右, 这是由于Chan算法易受观测误差的影响。而本文算法则明显提高了Chan算法的定位精度, 定位结果的RMS为0.021 m, 且两种算法所耗时间相差无异, 从计算结果可以看出, 两种算法每次的定位耗时相差仅0.003 s。

(2) 提供坐标初始值各方法解算结果

对比各方法的解算精度以及耗时, 得到的解算结果为图 6、图 7, 3种定位方法运行时间对比见图 8。其中图 6为3种方法的定位精度对比, 图 7为本文提出的方法和泰勒级数展开法对比。统计平均定位精度见表 2。

当提供了可靠的初始坐标值后, 泰勒级数展开法可以得到很好的定位结果, 相比于Chan算法的定位精度, 泰勒级数展开法的定位精度与本文方法定位精度更加接近, 仅相差0.002 m, 但泰勒级数展开法在每次模拟中的RMS波动范围明显高于本文方法, 稳定性较差。从图 7、图 8可以看出, 提供初始值后, 虽然泰勒级数展开法与本文方法的收敛结果接近, 由于需要较多的迭代次数, 耗时相对较长。

3 实测验证与结果分析

实测数据采用青岛附近某海域实测数据。数据采集记录时间为2017年12月1日8点至10点。船上搭载有电罗经、GPS、测深系统、声速剖面仪及声学定位系统, 测区水深约24 m, 水下布置单信标。采用“走航式”观测手段, 以信标为中心进行“画圆”与“画十字”观测, 采样间隔为2 s。声速剖面采样间隔为0.3 m。图 9和图 10分别表示测量船航行轨迹, 应答器相对位置及测区的声速剖面。

对原始数据进行数据处理后, 分别采用泰勒级数法, Chan算法和本文算法进行解算。由于测试区内无已知坐标的点信息, 无法获得定位解算绝对坐标差, 故通过内符合精度来比较3种定位方法效果, 得到统计结果列于表 3。内符合精度通过平差参数中误差进行衡量, 计算公式如下^[16]:

$ \left\{\begin{array}{l}{\sigma_{x_{i}}=\sigma_{0} \sqrt{Q_{x_{i} x_{i}}}} \\ {Q_{x_{i}}=\left(A^{\mathrm{T}} P A\right)^{-1}}\end{array}\right. $

(16)

式中: $ {Q_{x_{i}}}$为平差参数中误差, 对角元素$ Q_{x_{i} x_{i}}$是参数x_i的协因数。

从表 3中可以看出, 本文方法的定位精度与泰勒级数展开法的定位精度相当, 但运算时间耗时上少于泰勒级数展开法。两种算法的收敛值接近, 但本文算法减少了迭代运算次数, 节约了一定的时间。实际测量中, 受测量船姿态及周边环境影响, 测量值存在一定的粗差影响, 而Chan算法采用二重最小二乘法对定位方程组进行求解, 当在测量误差比较小时, 具有最优的估计性能, 但随着观测距离和测量误差增大, 算法的性能下降, 故在本次实验中, Chan算法定位精度最差。

4 结论

本文在介绍传统水下定位原理上详细介绍了泰勒级数展开法和Chan算法解算定位非线性方程的原理, 并在此基础上提出了基于Chan算法与泰勒级数展开法协同的水下定位算法, 通过仿真和实测计算结果分析, 对比3种算法的定位精度及定位时间, 得到以下结论:

1) 基于Chan算法和泰勒级数展开法协同的水下定位结果优于传统的单泰勒级数展开法和Chan算法, 并且在提高精度的同时保持定位时间的效率;

2) Chan算法会随测量误差的增大算法性能迅速下降, 若测量的数据含有较大的误差, 解算的坐标并不可靠, 对于观测条件等有一定要求, 需注意观测数据质量;

3) 本文忽略声学定位系统的其它因素影响, 仅从定位算法通过仿真计算和海上实验进行了研究, 结果表明该方法在精度和计算时间上比泰勒级数展开有一定的提高。