﻿ 一种基于移动矢径的单信标测距定位算法
 舰船科学技术  2019, Vol. 41 Issue (2): 119-123 PDF

A single beacon distance measurement location algorithm based on motion radius vector
ZHU Zi-yao, HAN Shu-ping, HUANG Rui, MA Xin
Navy Submarine Academy, Qingdao 266044, China
Abstract: Aiming at the underwater vehicle using single channel standard synchronous ranging positioning is influenced by the initial position error and velocity error, given a moving radius of synchronous location algorithm based on Dan Xinbiao. The concept of reference radius of a single mobile beacon location model, using the extended Calman filter algorithm to achieve the design positioning algorithm. The simulation and experimental results show that the algorithm can effectively reduce the location error caused by the initial position error, and estimate the unknown sound speed error in real time, which significantly improves the positioning accuracy of the underwater vehicle.
Key words: single beacon distance measurement location algorithm     motion radius vector     extended Kalman filter     sound velocity error
0 引　言

1 基于移动矢径的单信标测距定位模型

 图 1 单信标同步式测距定位示意图 Fig. 1 Map of single beacon synchronous range measurement location

 ${{({{x}_{k}}-{{X}_{k}})}^{2}}+{{({{y}_{k}}-{{Y}_{k}})}^{2}}+{{({{z}_{k}}-{{Z}_{k}})}^{2}}={{(c{{\tau }_{k}})}^{2}} {\text{。}}$ (1)

 \left\{ \begin{align} & {{({{x}_{k}}-{{X}_{k}})}^{2}}+{{({{y}_{k}}-{{Y}_{k}})}^{2}}={{r}_{k}}^{2}{\text{，}} \\ & {{r}_{k}}^{2}={{(c{{\tau }_{k}})}^{2}}-{{({{z}_{k}}-{{Z}_{k}})}^{2}}{\text{。}} \\ \end{align} \right. (2)

 图 2 基于移动矢径的单信标测距定位模型 Fig. 2 Single beacon synchronous range measurement location model based on MRV

 \left\{ \begin{align} & {{x}_{k+1}}{\rm{=}}{{x}_{k}}+d{{x}_{k,k+1}} {\text{，}}\\ & {{y}_{k+1}}{\rm{=}}{{y}_{k}}+d{{y}_{k,k+1}}_{k}{\text{。}} \\ \end{align} \right. (3)

 \left\{ \begin{aligned} & {{({{x}_{k+1}} -d{{x}_{k,k+1}}-{{X}_{k}})}^{2}}+{{({{y}_{k+1}}-d{{y}_{k,k+1}}_{k}-{{Y}_{k}})}^{2}}\\ & \quad\quad ={{(c{{\tau }_{k}})}^{2}}-{{({{z}_{k}}-{{Z}_{k}})}^{2}} {\text{，}}\\ & {{({{x}_{k+1}}-{{X}_{k+1}})}^{2}}+{{({{y}_{k+1}}-{{Y}_{k+1}})}^{2}}\\ &\quad\quad ={{(c{{\tau }_{k+1}})}^{2}}-{{({{z}_{k+1}}-{{Z}_{k+1}})}^{2}} {\text{。}}\\ \end{aligned} \right. (4)

2 基于EKF的定位滤波算法

 ${{{X}}_{k+1}}={{[{{x}_{k+1}},{{y}_{k+1}},\Delta c]}^{\rm{T}}}{\text{，}}$

 \left\{ \begin{align} & {{x}_{k+1}}{\rm{=}}{{x}_{k}}+d{{x}_{k,k+1}} {\text{，}}\\ & {{y}_{k+1}}{\rm{=}}{{y}_{k}}+d{{y}_{k,k+1}}_{k}{\text{，}} \\ & \Delta c=\Delta c {\text{。}}\\ \end{align} \right. (5)

 ${{{\omega }}_{{k}}}={{[{{x}_{k}},{{y}_{k}},d{{x}_{k,k+1}},d{{y}_{k,k+1}},\Delta c]}^{\rm{T}}}{\text{，}}$

 ${{{Q}}_{{k}}}=E[{{{\omega }}_{{k}}}-{{{\hat{\omega }}}_{{k}}}]{{[{{{\omega }}_{{k}}}-{{{\hat{\omega }}}_{{k}}}]}^{\rm{T}}}{\text{。}}$ (6)

 \left\{ \begin{aligned} & \tau _k^2{\rm{ = [}}{({x_k} - {X_k})^2} + {({y_k} - {Y_k})^2} + {({z_k} - {Z_k})^2}]/{(c + \Delta c)^2}{\text{，}}\\ &\tau _{k + 1}^2{\rm{ = [}}{({x_k} + d{x_{k,k + 1}} - {X_{k + 1}})^2} + {({y_k} + d{y_{k,k + 1}} - {Y_{k + 1}})^2} \\ & \quad\;\;\; + {({z_{k + 1}} - {Z_{k + 1}})^2}]/{(c + \Delta c)^2}{\text{。}} \end{aligned} \right. (7)

 ${{{v}}_{{k}}} = {[{x_k},{y_k},d{x_{k,k + 1}},d{y_{k,k + 1}},{X_k},{Y_k},{X_{k + 1}},{Y_{k + 1}},\Delta c]^{\rm{T}}}{\text{，}}$

 ${{{R}}_{{k}}} = E[{{{v}}_{{k}}} - {{{\hat v}}_{{k}}}]{[{{{v}}_{{k}}} - {{{\hat v}}_{{k}}}]^{\rm{T}}}{\text{。}}$ (8)

 $\left\{ \begin{array}{l} {{{X}}_{k + 1}} = f({{{X}}_k},{{{\omega }}_{{k}}}){\text{，}}\\ {{{Z}}_{k + 1}}{\rm{ = }}h({{{X}}_k},{{{v}}_{{k}}}){\text{，}} \end{array} \right.$ (9)

 ${{{F}}_{k,k + 1}} = {\left. {\frac{{\partial f({{{X}}_k},{{{\omega }}_{{k}}})}}{{\partial {{{X}}_k}}}} \right|_{{{{X}}_k}{\rm{ = }}{{{{\hat X}}}_k},{{{\omega }}_{{k}}} = {{{{\hat \omega }}}_{{k}}}}}{\text{，}}$ (10)

 ${{{W}}_{k,k + 1}} = {\left. {\frac{{\partial f({{{X}}_k},{{{\omega }}_{{k}}})}}{{\partial {{{\omega }}_{{k}}}}}} \right|_{{{{X}}_k}{\rm{ = }}{{{{\hat X}}}_k},{{{\omega }}_{{k}}} = {{{{\hat \omega }}}_{{k}}}}}{\text{，}}$ (11)

 ${{{H}}_{k,k + 1}} = {\left. {\frac{{\partial h({{{X}}_k},{{{v}}_{{k}}})}}{{\partial {{{X}}_k}}}} \right|_{{{{X}}_k}{\rm{ = }}{{{X}}_{k + 1|k}},{{{v}}_{{k}}}={{{{\hat v}}}_{{k}}}}}{\text{，}}$ (12)

 ${{{V}}_{k,k + 1}} = {\left. {\frac{{\partial h({{{X}}_k},{{{v}}_{{k}}})}}{{\partial {{{v}}_{{k}}}}}} \right|_{{{{X}}_k}{\rm{ = }}{{{X}}_{k + 1|k}},{{{v}}_{{k}}}={{{{\hat v}}}_{{k}}}}}{\text{。}}$ (13)

 \begin{array}{l} {{{F}}_{k,k + 1}} = \left[ {\begin{array}{*{20}{c}} 1\!\!&\!\!0\!\!&\!\!0\\ 0\!\!&\!\!1\!\!&\!\!0\\ 0\!\!&\!\!0\!\!&\!\!1 \end{array}} \right]{\text{，}}\\ {\rm{}}\\ {{{W}}_{k,k + 1}} = \left[ {\begin{array}{*{20}{c}} 1\!\!&\!\!0\!\!&\!\!1\!\!&\!\!0\!\!&\!\!0\\ 0\!\!&\!\!1\!\!&\!\!0\!\!&\!\!1\!\!&\!\!0\\ 0\!\!&\!\!0\!\!&\!\!0\!\!&\!\!0\!\!&\!\!1 \end{array}} \right]{\text{，}}\\ {\rm{}}\\ {{{H}}_{k,k + 1}} = \left[ {\begin{array}{*{20}{c}} {H(1,1)}\!\!&\!\!{H(1,2)}\!\!&\!\!{H(1,3)}\\ {H(2,1)}\!\!&\!\!{H(2,2)}\!\!&\!\!{H(2,3)} \end{array}} \right]{\text{，}}\\ {\rm{}}\\ {\begin{aligned} {{{V}}_{k,k + 1}} =& \left[\!\!{\begin{array}{*{20}{c}} {V(1,1)}\!\!&\!\!{V(1,2)}\!\!&\!\!0\!\!&\!\!0\!\!&\!\!{V(1,5)}\!\!&\!\! \\ {V(2,1)}\!\! &\!\!{V(2,2)}\!\!&\!\!{V(2,3)}\!\!&\!\!{V(2,4)}\!\!&\!\!0\!\!\end{array}}\right.\\ & \left.{\begin{array}{*{20}{c}}{V(1,6)}\!\!&\!\!0\!\!&\!\!0\!\!&\!\!{V(1,9)}\\ 0\!\!&\!\!{V(2,7)}\!\!&\!\!{V(2,8)}\!\!&\!\!{V(2,9)} \end{array}}\!\right]{\text{。}} \end{aligned}} \end{array}

 $\begin{array}{l} H(1,1){\rm{ = }}2({x_k} - {X_k})/{(c + \Delta c)^2}{\text{，}}\\ H(1,2){\rm{ = }}2({y_k} - {Y_k})/{(c + \Delta c)^2}{\text{，}}\\ H(1,3){\rm{ = - }}2{\rm{[}}{({x_k} - {X_k})^2} + {({y_k} - {Y_k})^2} + {({z_k} - {Z_k})^2}]/{(c + \Delta c)^3}{\text{，}}\\ H(2,1){\rm{ = }}2({x_k} + d{x_{k,k + 1}} - {X_{k + 1}})/{(c + \Delta c)^2}{\text{，}}\\ H(2,2){\rm{ = }}2({y_k} + d{y_{k,k + 1}} - {Y_{k + 1}})/{(c + \Delta c)^2}{\text{，}}\\ H(2,3){\rm{ = - }}2{\rm{[}}{({x_k} + d{x_{k,k + 1}} - {X_{k + 1}})^2} + \\ \quad\quad \cdots {({y_k} + d{y_{k,k + 1}} - {Y_{k + 1}})^2} + {({z_{k + 1}} - {Z_{k + 1}})^2}]/{(c + \Delta c)^3}{\text{，}} \end{array}$
 $\begin{array}{l} V(1,1){\rm{ = }}2({x_k} - {X_k})/{(c + \Delta c)^2}{\text{，}}\\ V(1,2){\rm{ = }}2({y_k} - {Y_k})/{(c + \Delta c)^2}{\text{，}}\\ V(1,5){\rm{ = }}2({X_k} - {x_k})/{(c + \Delta c)^2}{\text{，}}\\ V(1,6){\rm{ = }}2({Y_k} - {y_k})/{(c + \Delta c)^2}{\text{，}}\\ V(1,9){\rm{ = - }}2{\rm{[}}{({x_k} - {X_k})^2} + {({y_k} - {Y_k})^2} + {({z_k} - {Z_k})^2}]/{(c + \Delta c)^3}{\text{，}}\\ V(2,1){\rm{ = }}2({x_k} + d{x_{k,k + 1}} - {X_{k + 1}})/{(c + \Delta c)^2}{\text{，}}\\ V(2,2){\rm{ = }}2({y_k} + d{y_{k,k + 1}} - {Y_{k + 1}})/{(c + \Delta c)^2}{\text{，}}\\ V(2,3){\rm{ = }}2({x_k} + d{x_{k,k + 1}} - {X_{k + 1}})/{(c + \Delta c)^2}{\text{，}}\\ V(2,4){\rm{ = }}2({y_k} + d{y_{k,k + 1}} - {Y_{k + 1}})/{(c + \Delta c)^2}{\text{，}}\\ V(2,7){\rm{ = - }}2({x_k} + d{x_{k,k + 1}} - {X_{k + 1}})/{(c + \Delta c)^2}{\text{，}}\\ V(2,8){\rm{ = - }}2({y_k} + d{y_{k,k + 1}} - {Y_{k + 1}})/{(c + \Delta c)^2}{\text{，}}\\ V(2,9){\rm{ = - }}2{\rm{[}}{({x_k} + d{x_{k,k + 1}} - {X_{k + 1}})^2} + \\ \begin{array}{*{20}{c}} {}&{}& \cdots \end{array}{({y_k} + d{y_{k,k + 1}} - {Y_{k + 1}})^2} + {({z_{k + 1}} - {Z_{k + 1}})^2}]/{(c + \Delta c)^3}{\text{。}} \end{array}$

 ${{{X}}_{k + 1|k}} = f({{{\hat X}}_k},{{{\hat \omega }}_{{k}}}){\text{，}}$ (14)

 ${{{P}}_{k + 1|k}} = {{{F}}_{k,k + 1}}{{{P}}_k}{{F}}_{k,k + 1}^{\rm{T}} + {{{W}}_{k,k + 1}}{{{Q}}_{{k}}}{{W}}_{k,k + 1}^{\rm{T}}{\text{，}}$ (15)

 ${{{Z}}_{k + 1|k}}{\rm{ = }}h({{{\hat X}}_k},{{{\hat v}}_{{k}}}){\text{，}}$ (16)

 $\begin{split} {{{K}}_{k + 1}} = {{{P}}_{k + 1|k}}{{H}}_{k,k + 1}^{\rm{T}}({{{V}}_{k,k + 1}}{{{R}}_{{k}}}{{V}}_{k,k + 1}^{\rm{T}} + \\ {{{H}}_{k,k + 1}}{{{P}}_{k + 1|k}}{{H}}_{k,k + 1}^{\rm{T}}{)^{ - 1}}{\text{，}} \end{split}$ (17)

 ${{{P}}_{k + 1}} = ({{I}} - {{{K}}_{k + 1}}{{{H}}_{k,k + 1}}){{{P}}_{k + 1|k}}{\text{，}}$ (18)

 ${{{\hat X}}_{k + 1}}{\rm{ = }}{{{X}}_{k + 1|k}} + {{{K}}_{k + 1}}({{{Z}}_{k + 1}} - {{{Z}}_{k + 1|k}}){\text{。}}$ (19)
3 仿真分析

 图 3 仿真航迹 Fig. 3 Simulation track
 $\left\{\begin{array}{l} {{{X}}_1} = {[{x_1} + 500,{y_1} + 500,0]^{\rm{T}}}{\text{，}}\\ {Q_0} = {\rm diag}(1,1,1,1,16){\text{，}}\\ {R_0} = {\rm diag}(\!1,\!1,\!1,\!1,\!0.001,\!0.001,\!0.001,\!0.001,\!16\!)\!{\text{，}}\\ {P_0} = {\rm diag}({10^6},{10^6},{10^2}){\text{。}} \end{array}\right.$ (20)

 图 4 声速误差估计 Fig. 4 Error estimation of sound velocity

 图 5 定位误差 Fig. 5 Positioning error
4 试验验证

 图 6 试验航迹 Fig. 6 Experiment track

 图 7 定位误差 Fig. 7 Positioning error

 图 8 声速误差估计 Fig. 8 Error estimation of sound velocity
5 结　语

 [1] 张光普, 梁国龙, 王燕, 等. 分布式水下导航、定位、通信一体化系统设计[J]. 兵工学报, 2007, 28(12): 1455-1462. DOI:10.3321/j.issn:1000-1093.2007.12.011 [2] 孙大军, 郑翠娥. 水声导航、定位技术发展趋势探讨[J]. 海洋技术学报, 2015, 34(3): 64-68. [3] 张延顺, 郭雅静, 黄小娟, 等. 水下运载体航位推算系统初始位置确定方法[J]. 北京航空航天大学学报, 2006, 32(8): 946-949. DOI:10.3969/j.issn.1001-5965.2006.08.017 [4] ALEXANDER P S.The AUV positioning using ranges from one transponder LBL[C]//Proceedings of the 1995 MTS/IEEE Oceans Conference, San Die-go, CA, USA, IEEE, 1995: 1620–1623. [5] FERREIRA B, MATOS A, CRUZ N. Single beacon navigation: Localization and control of the MARES AUV[J]. Oceans, 2010, 58(1): 1-9. [6] SARAH E. WEBSTER C J C. Decentralized single beacon acoustic navigation : communication and navi-gation for underwater vehicles[D]. Washington D.C.: The Johns Hopkins University , 2010, 6–22. [7] LI Wen-bai, LIU Ming-yong, LIU Fu-qiang, et al. Coop-erative navigation for autonomous underwater vehicles based on estimation of motion radius vectors[J]. Defence Technology, 2011, 07(1): 9-14. [8] 刘明雍, 张加全, 张立川. 洋流影响下基于运动矢径的AUV协同定位方法[J]. 控制与决策, 2011, 26(11): 1632-1636. [9] 张加全, 刘明雍, 胡俊伟. 运动矢径对AUV协同定位精度影响的仿真[J]. 火力与指挥控制, 2012, 37(1): 43-46. DOI:10.3969/j.issn.1002-0640.2012.01.011 [10] 王小旭, 潘泉, 黄鹤, 等. 非线性系统确定采样型滤波算法综述[J]. 控制与决策, 2012, 27(6): 801-812.