2. 哈尔滨工业大学 仪器科学与工程学院,黑龙江 哈尔滨 150001;
3. 杭州钱航船舶修造有限公司,浙江 杭州 311256
2. School of Instrument Science and Engineering, Harbin Institute of Technology, Harbin 150001, China;
3. Hangzhou Qianjiang Ship Repair and Construction Limited Company, Hangzhou 311256, China
多水下无人航行器(UUV)协同作业系统能够承担诸多复杂困难任务,具有高效率、高可靠性等优点,应用前景广阔[1-3]。基于水声距离观测的UUV协同定位在UUV协同作业系统中具有重要作用,但因水下情况复杂多变,导致其收到的水声距离观测信息经常被异常噪声干扰。如何有效解决水声距离异常噪声影响条件下高精度水下协同定位,是水下协同定位所面临的一个重要问题,对实现UUV高精度协同定位具有重要意义[4]。
在水下声学定位技术的发展历程中,声学异常噪声处理一直是一项重要研究内容。S. Misra 等[5-6]针对长基线系统中的声学异常量测问题进行了研究,通过设置一固定异常量测判定调节参数,并基于马氏距离判定方法对声学异常量测进行判定。但是该方法需要保证航位推算信息的一致性,且具有相对准确的推算不确定度,否则便会造成对异常噪声的不敏感性。K. H. Kim等[7]采用谱图像分割技术对长基线系统中的水声异常量测问题进行了研究,通过比较多次量测信息间相对一致性的大小来进行异常噪声判定,虽然离线试验数据处理取得比较理想的效果,但是由于该方法是基于长基线系统固定信标条件下的,对于动态条件下的航行器协同定位系统来说则具有局限性。针对新近发展的UUV协同定位问题,C. B. Medeiros等[8]通过在每次数据融合过程中对先前一系列距离观测信息的综合处理,有效实现了对由于异常量测造成的估计误差的及时校正,但还是无法避免异常量测时刻对状态误差的严重影响。
为寻求更为及时、有效的异常噪声处理方法,本文将极大似然估计理论(Maximum Likehood Estimation,M估计)[9]应用于扩展卡尔曼滤波算法(Extended Kalman Filter,EKF),提出一种基于M估计多UUV协同定位算法。该算法可以有效解决UUV协同定位过程中水声距离异常条件下的状态估计问题,进一步改善异常噪声情况下协同定位系统的状态估计性能。
1 UUV协同定位系统运动学模型为简化问题分析,本文中参与协同的低精度UUV以航位推算方式进行自主定位[10]。因UUV定位时在水面上运动,进一步简化为二维空间下的协同定位问题。若已知UUV在起始时刻的东北向位置坐标为
$ \left\{ \begin{aligned} & x_1^k = x_1^{k - 1} + \Delta t \cdot \left( {v_y^k \cdot \cos {\theta ^k} - v_x^k\sin {\theta ^k}} \right)\text{,} \\ & x_2^k = x_2^{k - 1} + \Delta t \cdot \left( {v_y^k \cdot \sin {\theta ^k} + v_x^k\cos {\theta ^k}} \right) \text{。} \end{aligned} \right. $ | (1) |
式中:
根据式(1)可以得到相应离散系统状态方程:
$ {{{x}}^k} = {{{F}}^k}{{{x}}^{k - 1}} + {\bf{\varGamma }}\left( {{u^k} + {w^k}} \right)\text{,} $ | (2) |
式中:
$ {{{Q}}^k} = E\left[ {{w^k}{{\left( {{w^k}} \right)}^{\rm T}}} \right] = \left[ {\begin{array}{*{20}{c}} {\sigma _{{v^k}}^2}&0 \\ 0&{\sigma _{{\theta ^k}}^2} \end{array}} \right]\text{。} $ | (3) |
为抑制航位推算误差的不断积累,ASV间可以通过相对距离观测进行导航误差的协同校正。根据
$ {z^k} = h({x^k}) + {e^k} = \sqrt {{{(x_{1m}^k - {x^k})}^2} + {{(x_{2m}^k - {x^k})}^2}} + {e^k} {\text{。}} $ | (4) |
式中:
$ {{{R}}^k} = E\left[ {{e^k}{{\left( {{e^k}} \right)}^{\rm T}}} \right] = \sigma _{{e^k}}^2\text{。} $ | (5) |
将统计理论中的M估计算法与EKF算法进行融合,提出M估计多UUV协同定位算法。
对式(2)和式(4)进行改写,得到EKF框架下的数学模型:
$ \left\{ \begin{array}{l} {{{x}}_k} = {{{F}}_k}{{{x}}_{k - 1}} + {{{B}}_k}{{{u}}_k} + {{\bf{w}}_k} \text{,} \\ {{{z}}_k} = {{{H}}_k}{{{x}}_k} + {{{e}}_k}\text{。} \\ \end{array} \right. $ | (6) |
其中:
对于M估计多UUV协同定位算法,增加如下状态方程:
$ {{{\hat x}}_{k|k - 1}} = {{{F}}_k}{{\bf{\hat x}}_{k - 1|k - 1}} + {{{B}}_k}{{{u}}_k} + {{{w}}_k} \text{,} $ | (7) |
$ {{{\hat x}}_{k|k - 1}} = {{{x}}_k} + {{\bf{\delta }}_{k|k - 1}}\text{。} $ | (8) |
其中:
根据式(6)和式(8)得到以量测噪声和状态误差最小为评价指标的M估计数学模型:
$ \left[ \begin{array}{l} {{\hat {{x}}}_{k|k - 1}} \\ {{{Z}}_k} \\ \end{array} \right] = \left[ \begin{array}{l} {{I}} \\ {{{H}}_k} \\ \end{array} \right]{{\bf{x}}_k} + \left[ \begin{array}{l} {{\bf{\delta }}_{k|K - 1}} \\ {{{e}}_k} \\ \end{array} \right]\text{,} $ | (9) |
将上式简写为
$ {{\tilde{{Z}}}_k} = {{\tilde{{H}}}_k}{{{x}}_k} + {{\tilde{{e}}}_k} \text{,} $ | (10) |
其中,
对上式改写
$ f\left( {{{\tilde e}_k}} \right) = \tilde { Z}_{_k} - \tilde { H}_{_k}{x_k}{\text{。}} $ | (11) |
对式(11)进行计算,欲令
$ \frac{{\partial J({x_k})}}{{\partial {x_k}}} = \sum\limits_{i = 1}^{m = 2} {\frac{{\partial {\rho _i}(\tilde e_k^i)}}{{\partial (\tilde e_k^i)}}} \cdot \frac{{\partial \tilde e_k^i}}{{\partial {x_k}}} = 0 \text{,} $ | (12) |
$ \frac{{\partial J({x_k})}}{{\partial {x_k}}}= \sum\limits_{i = 1}^{m = 2} {{h_i} \cdot \frac{{\partial {\rho _i}(\tilde e_k^i)}}{{\partial (\tilde e_k^i)}}} = 0 \text{,} $ | (13) |
$ \frac{{\partial J({x_k})}}{{\partial {x_k}}}= \sum\limits_{i = 1}^{m = 2} {{h_i} \cdot \frac{{\tilde e_k^i}}{{\tilde e_k^i}}\frac{{\partial {\rho _i}(\tilde e_k^i)}}{{\partial (\tilde e_k^i)}}} = 0\text{,} $ | (14) |
$ \frac{{\partial J({x_k})}}{{\partial {x_k}}}= \sum\limits_{i = 1}^{m = 2} {{h_i} \cdot (\tilde Z_k^i - {h_i}{x_k})\frac{1}{{\tilde e_k^i}} \cdot \frac{{\partial {\rho _i}(\tilde e_k^i)}}{{\partial (\tilde e_k^i)}}} = 0\text{。} $ | (15) |
其中,
将式(15)改写为
$ \sum\limits_{j = 1}^{m = 2} {{h_i} \cdot {q_i}(e_k^i) \cdot (\tilde Z_k^i - {h_i}{x_k})} = 0\text{,} $ | (16) |
将式(16)写成矩阵形式
$ {{\tilde{{H}}}^{\rm T}}{{Q}}({\tilde{{Z}}} - {\tilde{{Hx}}}) = {{0}}\text{,} $ | (17) |
其中,
利用Newton法解上式方程,得
$ {{\hat x}}_{k|k}^{j + 1} = {({{\tilde{{H}}}^{\rm T}}{{{Q}}^j}{\tilde{{H}}})^{ - 1}}{{\tilde{{H}}}^{\rm T}}{{{Q}}^j}{{\tilde{{Z}}}_k}\text{,} $ | (18) |
其中,j为迭代次数。
利用式(18)进行迭代即可得到
$ {{P}}_{k|k} = {({{\tilde{{H}}}^{\rm T}}{{Q}}{\tilde{{H}}} )^{ - 1}}\text{。} $ | (19) |
对极值函数进行分析。可以看出,当极值函数
对于极值函数
$ \left\{ \begin{aligned} & \rho (t) = \frac{{{t^2}}}{2},\left| t \right| < 1.5\sigma \text{,} \\ & \rho (t) = c\left| t \right|,1.5\sigma \leqslant \left| t \right| < 3\sigma\text{,} \\ & \rho (t) = d,3\sigma \leqslant \left| t \right| \text{。} \end{aligned} \right. $ | (20) |
可以发现,IGG法将权值函数分成了3段,可依次视为可信段、怀疑段和异常噪声段,然后分别采用保留、降权和舍去的方法进行处理,显然这种处理方式很适合处理重尾噪声。
本文所提算法如图1所示。可以看出,相较于经典EKF算法,所提算法的优点是通过函数
采用湖上实验数据进行半实物仿真,验证所提算法有效性。实验具体情况说明如下:
湖上实验采用图2所示水面艇模拟UUV进行,实验方案如图3所示。实验共配置2艘主艇、1艘从艇,实验过程中2条主艇采用高精度的实时GPS位置信息作为基准参考,利用水声通信设备实现主从艇间参考信息的传输及相对距离信息的观测,从而实现对从艇航位推算误差的协同校正。从艇航位推算中的位置信息是采用DVL提供的绝对速度信息以及磁罗经提供的航向信息经推算得到的。为进行比对,从艇上还安装有GPS/PHINS组合导航系统,用来提供从艇相对精确的位置、速度和航向信息。水声通信设备采用图4所示的德国Evologics公司S2CR 7/17型水声通信设备构建水声通信网络,实现主从艇间相对距离的测量以及信息的传递。为便于操作,实验时2艘主艇各自以固定通信周期向从艇发送自身带标识符的GPS位置信息以及二者相对距离量测信息,辅助从艇进行导航误差的协同校正;从艇无论接收到哪个主艇的观测信息,都利用该信息进行协同导航。实验用声学设备采用RSS原理进行测距,当从艇成功接收到主艇参考信息后,向主艇返回一个应答脉冲信号,主艇接收到该应答信号后,利用该信号进行基于RSS原理的信号传播距离的解算,即在协同导航过程中,主艇每次通过水声通信传递给从艇的参考信息为自身GPS位置信息+前一时刻的水声距离量测信息。实验过程中采用的相关传感器指标如表1所示。
主从艇按5 s周期进行通信,实验时间超过2.3 h。主从艇运行轨迹如图5所示。分别采用EKF算法和本文所提算法对从艇位置信息进行解算,实验结果如图6所示。可知,2种算法定位误差均收敛,即都可以用于协同导航;M估计多UUV协同定位算法的定位误差在10 m以下,且在0.5–1.75 h时,误差稳定在5 m左右,相较于传统EKF算法,其定位效果得到提升约30%,说明了所提算法在处理水下异常噪声方面的优越性。
本文针对多UUV协同导航技术中存在的异常量测噪声进行处理,通过实际实验数据分析,对异常量测噪声采用重尾噪声模型进行重新建模,并针对重尾噪声特点,提出M估计多UUV协同定位算法,有效克服了水声距离异常量测噪声对于UUV协同定位系统的影响。实验结果表明,采用本文所提算法,多UUV协同定位精度一般可达5 m左右,相较于传统EKF算法,协同定位精度可提升30%左右,工程应用价值巨大。
[1] |
PAULL L, SAEEDI S, SETO M, LI H. Auv navigation and localization: A review[J]. IEEE Journal of Oceanic Engineering, 2014, 39(1): 131-149. DOI:10.1109/JOE.2013.2278891 |
[2] |
BETHENCOURT A, JAULIN L. Cooperative localization of underwater robots with unsynchronized clocks. paladyn[J]. Journal of Behavioral Robotics, 2013, 4(4): 233-244. |
[3] |
徐博, 白金磊, 郝燕玲, 等. 多AUV协同导航问题的研究现状与进展[J]. 自动化学报, 2015, 41(3): 445. XU Bo, BAI Jinlei, HAO Yanling, et al. The research status and progress of cooperative navigation for multiple AUVs[J]. ACTA Automatica Sinica, 2015, 41(3): 445. |
[4] |
边信黔, 某春晖, 严浙平. 多UUV沿多条给定路径运动的协调编队控制[J]. 哈尔滨工业大学学报, 2013, 45(1): 106-111. BIAN Xinqian, MU Chunhui, YAN Zheping. Coordinated control for multi-UUV formation motion on a set of given paths[J]. Journal of Harbin Institute of Technology, 2013, 45(1): 106-111. DOI:10.11918/hitxb20130120 |
[5] |
MISRA S, DASH S, KHATUA M, et al. Jamming in underwater sensor networks: detection and mitigation[J]. IET communications, 2012, 6(14): 2178-2188. DOI:10.1049/iet-com.2011.0641 |
[6] |
CRASSIDIS J L. Sigma-point kalman filtering for integrated GPS andinertial navigation[J]. IEEE Trans, 2006, 42(2): 750-756. |
[7] |
KIM K H, LEE J G, PARK C G. Adaptive two-stage extended Kalman filter for a fault-tolerant INS-GPS loosely coupled system[J]. IEEE Trans, 2009, 45(1): 125-137. |
[8] |
MEDEIROS C B. Multiple-model linear kalman filter framework for unpredictable signals[J]. IEEE Sensors, 2014, 14(4): 979-991. DOI:10.1109/JSEN.2013.2291683 |
[9] |
ZHOU J, YANG Y, ZHANG J, et al. Tightly coupled INS/GPS using quaternion-based unscented kalman filter[C]// Navigat., Control Conf., Portland, OR, USA, 2011: 1–14.
|
[10] |
HUBER P J, RONCHETTI E M. Robust Statistics (Second Edition)[M]. USA: Wiley, 2009.
|
[11] |
PICHER, SARKKA, HARTIKAINEN J. Recursive outlier-robust filtering and smoothing for nonlinear systems using the multivariate student distribution[C]// 2012 IEEE International Workshop on Machine Learning for Signal Processing. IEEE, 2012: 1–6.
|
[12] |
HOSSEINIAN S, MORGENTHALER S. Robust binary regression[J]. Journal of Statistical Planning and Inference, 2011(141): 1497-1509. |
[13] |
周江文. 经典误差理论与抗差估计[J]. 测绘学报, 1989, 18(2): 115-120. ZHOU Wenjiang. The classical error theory and estimation[J]. Acta Geodaetica et Cartographica Sinica, 1989, 18(2): 115-120. DOI:10.3321/j.issn:1001-1595.1989.02.005 |