文章信息
- 孟庆波, 王振杰, 王柏杨, 刘杨范. 2019.
- MENG Qing-bo, WANG Zhen-jie, WANG Bo-yang, LIU Yang-fan. 2019.
- 一种改进的水下声学定位算法
- An improved underwater acoustic localization algorithm
- 海洋科学, 43(11): 84-90
- Marine Sciences, 43(11): 84-90.
- http://dx.doi.org/10.11759/hykx20190109004
-
文章历史
- 收稿日期:2019-01-09
- 修回日期:2019-04-28
2. 青岛海洋科学与技术试点国家实验室海洋矿产资源评价与探测技术功能实验室, 山东 青岛 266071
2. Laboratory for Marine Mineral Resource, Pilot National Laboratory for Marine Science and Technology(Qingdao), Qingdao 266071, China
随着国家海洋战略的推进, 高精度的水下定位技术在获取大范围海洋空间及环境数据, 进行海洋勘探与开发的应用越来越广泛[1]。当前的水下声学定位模型常采用“圆曲线”或“双曲线”模型, 解算时将非线性观测方程转换成为近似线性的方程, 然后采用最小二乘方法进行水下目标的坐标求解[2]。由于泰勒级数展开法在处理非线性方程有着解算精度高、收敛速度快、适用性强和稳健性强等特点, 当前水下声学定位的目标解算多是采用该方法。但它对初始参考值的依赖性强, 如果提供的初始值不正确, 会导致迭代次数过多或者结果不收敛, 而实际中我们很难获取水下目标的先验位置信息, 无法提供可靠的参考初始值[3-4]。相比于泰勒级数展开法, Chan算法则是一种求解曲线方程组的非递归算法, 该算法特点是不需设定初始值, 计算量小, 无需作迭代计算, 只需两次最小二乘即可解算出结果[5], 但对测量误差有一定要求。
本文提出了一种结合Chan算法与泰勒级数展开法的协同水下声学定位算法, 先用Chan算法求取出目标的初始位置, 以此作为参考值再通过泰勒级数展开法求取出目标的精确位置。实验结果表明该方法可行, 且能满足一定的定位需求。
1 算法原理 1.1 水下定位原理进行水下声学定位时, 安置在船底的换能器发射声波信号, 水下应答器接收信号并给出反馈信号, 换能器接收到反馈信号计算出传播时间, 通过声速剖面仪等获取的声速乘以单向传播时间得到距离观测值。理论上获取3个以上的距离观测可交汇出应答器的三维坐标。
假设应答器的位置为(X, Y, Z), 换能器在第i个观测历元的位置为(xi, yi, zi)声波单向传播时间为ti, 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) |
(X0, Y0, Z0)为应答器近似坐标, 令
$ \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) |
其中:
依据最小二乘原理可得:
$ {\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) |
进行下一次迭代时, 令X1 = X0 + ΔX, Y1 = Y0 + ΔY, Z1 = Z0 + ΔZ, 重复以上步骤, 直到dx0足够小则迭代停止, 在定位中一般设定迭代终止阀值为1 mm。
泰勒级数展开法可以得到精确的定位结果, 但是它的缺点是需要一个与实际位置偏差较小的初始值来保证算法的收敛, 这在实际情况中并不容易实现, 且不能提前判断何时出现不收敛情况。
1.3 Chan算法在一次观测中, 获取到了n组观测数据, 待定位应答器的坐标为(X, Y, Z), 第i个观测历元的换能器位置为(xi, yi, zi), 根据测量得到的时间信息及声速可得换能器与应答器之间的观测方程为[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) |
其中:
令
$ 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{R1, R2, …, Rn}。依据加权最小二乘原理得:
${\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) |
此时的Za为第一次估计值, 设定Za, i为Za的第i个分量, 利用Za重新构造B1矩阵进而得到新的协方差阵φ1:
$ {\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) |
式中:
最终应答器的定位计算结果为:
$ {\left( {X, Y, Z} \right)^{\rm{T}}} = \pm \sqrt {{{\mathit{\boldsymbol{Z'}}}_{\rm{a}}}} {({x_1}, {y_1}, {z_1})^{\rm{T}}}, $ | (14) |
正负号的选取, 以X坐标分量取值为例, 若
由于泰勒级数展开法初始值的选取对定位结果影响很大, 初始值的选取不当会导致算法的发散, 而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]:
算法 | σx/m | σy/m | σz/m | σxyz/m | 耗时/s |
泰勒法(无初值) | 无 | 无 | 无 | 无 | 无 |
泰勒级数(有初值) | 0.136 | 0.129 | 0.142 | 0.235 | 0.225 |
Chan算法 | 0.442 | 0.336 | 0.465 | 0.725 | 0.115 |
本文方法 | 0.135 | 0.130 | 0.142 | 0.235 | 0.121 |
$ \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) |
式中:
从表 3中可以看出, 本文方法的定位精度与泰勒级数展开法的定位精度相当, 但运算时间耗时上少于泰勒级数展开法。两种算法的收敛值接近, 但本文算法减少了迭代运算次数, 节约了一定的时间。实际测量中, 受测量船姿态及周边环境影响, 测量值存在一定的粗差影响, 而Chan算法采用二重最小二乘法对定位方程组进行求解, 当在测量误差比较小时, 具有最优的估计性能, 但随着观测距离和测量误差增大, 算法的性能下降, 故在本次实验中, Chan算法定位精度最差。
4 结论本文在介绍传统水下定位原理上详细介绍了泰勒级数展开法和Chan算法解算定位非线性方程的原理, 并在此基础上提出了基于Chan算法与泰勒级数展开法协同的水下定位算法, 通过仿真和实测计算结果分析, 对比3种算法的定位精度及定位时间, 得到以下结论:
1) 基于Chan算法和泰勒级数展开法协同的水下定位结果优于传统的单泰勒级数展开法和Chan算法, 并且在提高精度的同时保持定位时间的效率;
2) Chan算法会随测量误差的增大算法性能迅速下降, 若测量的数据含有较大的误差, 解算的坐标并不可靠, 对于观测条件等有一定要求, 需注意观测数据质量;
3) 本文忽略声学定位系统的其它因素影响, 仅从定位算法通过仿真计算和海上实验进行了研究, 结果表明该方法在精度和计算时间上比泰勒级数展开有一定的提高。
[1] |
杨元喜, 徐天河, 薛树强. 我国海洋大地测量基准与海洋导航技术研究进展与展望[J]. 测绘学报, 2017, 46(1): 1-8. Yang Yuanxi, Xu Tianhe, Xue Shuqiang. Progresses and prospects in developing marine geodetic datum and marine navigation of China[J]. Acta Geodaetica et Cartographica Sinica, 2017, 46(1): 1-8. |
[2] |
张红梅. 水下导航定位技术[M]. 武汉: 武汉大学出版社, 2010. Zhang Hongmei. Underwater Navigation and Positioning Technolog[M]. Wuhan: Wuhan University Press, 2010. |
[3] |
Fabrizio C, Jonathan G. On power series expansions of the S-resolvent operator and the Taylor formula[J]. Journal of Geometry and Physics, 2016, 110(8): 154-157. |
[4] |
Foy W H. Position-location solutions by Taylor-series estimation[J]. IEEE Transactions on Aerospace & Electronic Systems, 2007, 12(2): 187-194. |
[5] |
Chan Y T, Ho K C. A simple and efficient estimator for hyperbolic location[J]. IEEE Transactions on Signal Processing, 2002, 42(8): 1905-1915. |
[6] |
易昌华, 任文静, 王钗. 二次水声定位系统误差分析[J]. 石油地球物理勘探, 2009, 44(2): 136-139. Yi Changhua, Ren Wenjing, Wang Chai. Analysis on error of secondary acoustic positioning system[J]. Oil Geophysical Prospecting, 2009, 44(2): 136-139. |
[7] |
赵爽, 王振杰, 吴绍玉, 等. 基于选权迭代的走航式水声差分定位方法[J]. 石油地球物理勘探, 2017, 52(6): 1137-1145. Zhao Shuang, Wang Zhenjie, Wu Shaoyu, et al. A ship-board acoustic difference positioning method based on selection weight iteration[J]. Oil Geophysical Prospecting, 2017, 52(6): 1137-1145. |
[8] |
Yang Fanlin, Lu Xiushan, Li Jiabiao, et al. Precise positioning of underwater static objects without sound speed profile[J]. Marine Geodesy, 2011, 34(2): 138-151. DOI:10.1080/01490419.2010.518501 |
[9] |
李招华, 汪毓铎, 邵青. 基于Chan的TDOA三维定位算法[J]. 现代电信科技, 2014(11): 36-40. Li Zhaohua, Wang Yuduo, Shao Qing. Chan-based TDOA three-dimensional positioning algorithm[J]. Modern Science & Technology of Telecommunications, 2014(11): 36-40. |
[10] |
张保峰, 刘同佩, 韩燕, 等. 基于TOA的三维空间定位算法研究[J]. 计算机工程与设计, 2007, 28(14): 3364-3366. Zhang Baofeng, Liu Tongpei, Han Yan, et al. 3-Dimension location algorithm research based on TOA[J]. Computer Engineering and Design, 2007, 28(14): 3364-3366. |
[11] |
徐振鲁.时差法多站无源定位及其信号处理技术研究[D].南京: 南京航空航天大学, 2011. Xu Zhenlu. Research on multi-station passive location and signal processing technology based on time difference method[D]. Nanjing: Nanjing University of Aeronautics and Astronautics, 2011. http://www.wanfangdata.com.cn/details/detail.do?_type=perio&id=zdyyx200102008 |
[12] |
Zhao Jianhu, Zou Yajing, Zhang Hongmei, et al. A new method for absolute datum transfer in seafloor control network measurement[J]. Journal of Marine Science & Technology, 2016, 21(2): 216-226. |
[13] |
刘林, 邓平, 范平志. 基于Chan氏算法和Taylor级数展开法的协同定位方法[J]. 电子与信息学报, 2004, 26(1): 41-46. Liu Lin, Deng Ping, Fan Pingzhi. A cooperative location method based on Chan and Taylor algorithms[J]. Journal of Electronics & Information Technology, 2004, 26(1): 41-46. |
[14] |
张居成.深水长基线定位导航技术研究[D].哈尔滨: 哈尔滨工程大学, 2014. Zhang Jucheng. Research of deepwater LBL positoning and navigation technology[D]. Harbin: Harbin Engineering University, 2014 http://cdmd.cnki.com.cn/Article/CDMD-10217-1017245695.htm |
[15] |
Xu Peiliang, Ando M, Tadokoro K. Precise, three-dimensional seafloor geodetic deformation measurements using difference techniques[J]. Earth Planets & Space, 2005, 57(9): 795-808. |
[16] |
崔希璋, 於宗俦, 陶本藻, 等. 广义测量平差(新版)[M]. 武汉: 武汉测绘科技大学出版社, 2001. Cui Xizhang, Yu Zongchou, Tao Benzao, et al. Generalized Surveying Adjustment[M]. Wuhan: Wuhan University Press, 2001. |