﻿ 基于MHE的多UUV协同定位方法
 舰船科学技术  2017, Vol. 39 Issue (12): 81-85 PDF

The cooperation localization method of MUUVs based on MHE
YANG Jian, LUO Tao, WEI Shi-le, WANG Ya-bo, WANG Hong-hua
Wuhan Second Ship Design and Research Institute, Wuhan 430064, China
Abstract: Aiming at the non-liner problem of the cooperation localization model of MUUVs, the paper proposed a Moving Horizon Estimation method to get the optimization eatate estimation of cooperation localization, the theoretical analysis and experimental results proved the high accuracy and availability of the proposed method.
Key words: MUUVs     cooperation localization     MHE
0 引　言

1 多UUV协同定位状态空间模型 1.1 状态方程

 $\left\{ \begin{array}{l}{x_{ik + 1}} = {x_{ik}} + \delta t \cdot {V_{ik}}\sin {\phi _{ik}}\text{，}\\{y_{ik + 1}} = {y_{ik}} + \delta t \cdot {V_{ik}}\cos {\phi _{ik}}\text{，}\\{\phi _{ik + 1}} = {\phi _{ik}} + \delta t \cdot {\omega _{ik}}\text{。}\end{array} \right.$ (1)

 $\left\{ \begin{array}{l}{V_{ik}} = {{\bar V}_{ik}} + {w_{ivk}}\text{，}\\{\omega _{ik}} = {{\bar \omega }_{ik}} + {w_{i\omega k}}\text{。}\end{array} \right.$ (2)

 ${\mathit{\boldsymbol{Q}}_{ik}} = E\left[ {{\mathit{\boldsymbol{w}}_{ik}}\mathit{\boldsymbol{w}}_{ik}^{\rm T}} \right] = \left[ {\begin{array}{*{20}{c}}{q_{i{v_k}}^2} & 0\\0 & {q_{i{\omega _k}}^2}\end{array}} \right]\text{。}$ (3)

 ${\mathit{\boldsymbol{X}}_{k + 1}} = f({\mathit{\boldsymbol{X}}_k},{\mathit{\boldsymbol{u}}_k},{\mathit{\boldsymbol{w}}_k})\text{。}$ (4)

 ${\mathit{\boldsymbol{X}}_{k + 1}} = \left[ {\begin{array}{*{20}{c}}{{\mathit{\boldsymbol{X}}_{1k + 1}}}\\{{\mathit{\boldsymbol{X}}_{2k + 1}}}\\{{\mathit{\boldsymbol{X}}_{3k + 1}}}\end{array}} \right] = \left[ {\begin{array}{*{20}{c}}{{f_1}({\mathit{\boldsymbol{X}}_{1k}},{\mathit{\boldsymbol{u}}_{1k}},{\mathit{\boldsymbol{w}}_{1k}})}\\{{f_2}({\mathit{\boldsymbol{X}}_{2k}},{\mathit{\boldsymbol{u}}_{2k}},{\mathit{\boldsymbol{w}}_{2k}})}\\{{f_3}({\mathit{\boldsymbol{X}}_{3k}},{\mathit{\boldsymbol{u}}_{3k}},{\mathit{\boldsymbol{w}}_{3k}})}\end{array}} \right]\text{。}$ (5)

 ${\mathit{\boldsymbol{X}}_{ik + 1}} = {\mathit{\boldsymbol{F}}_{ik}}{\mathit{\boldsymbol{X}}_{ik}} + {\mathit{\boldsymbol{G}}_{ik}}{\mathit{\boldsymbol{w}}_{ik}}\text{，}$ (6)

 $\left\{ \begin{array}{l}{F_{ik}} = I + \delta t \cdot \frac{{\partial {f_1}}}{{\partial X{{_{ik}^{}}^T}}} = \\\;\;\;\;\;\;\;\;\;\;\;I + \delta t \cdot \left[ {\begin{array}{*{20}{c}}1&0&{\delta t \cdot {V_{ik}}\cos {\phi _{ik}}}\\0&1&{ - \delta t \cdot {V_{ik}}\sin {\phi _{ik}}}\\0&0&1\end{array}} \right]\text{，}\\{G_{ik}} = \frac{{\partial f}}{{\partial {w_{ik}}^T}} = \left[ {\begin{array}{*{20}{c}}{\delta t \cdot \sin {\phi _{ik}}}&0\\{\delta t \cdot \cos {\phi _{ik}}}&0\\0&{\delta t}\end{array}} \right]\text{。}\end{array} \right.$ (7)

i=1，2，3时，3个UUV的状态方程联合起来，则两主一从协同定位系统的线性状态方程定义为：

 $\begin{split}&{\mathit{\boldsymbol{X}}_{k + 1}} = {\mathit{\boldsymbol{F}}_k}{\mathit{\boldsymbol{X}}_k} + {\mathit{\boldsymbol{G}}_k}{\mathit{\boldsymbol{w}}_k}=\\& \left[ {\begin{array}{*{20}{c}}{{\mathit{\boldsymbol{F}}_{1k}}} & {\bf{0}} & {\bf{0}}\\{\bf{0}} & {{\mathit{\boldsymbol{F}}_{2k}}} & {\bf{0}}\\{\bf{0}} & {\bf{0}} & {{\mathit{\boldsymbol{F}}_{3k}}}\end{array}} \right]\left[ {\begin{array}{*{20}{c}}{{\mathit{\boldsymbol{X}}_{1k}}}\\{{\mathit{\boldsymbol{X}}_{2k}}}\\{{\mathit{\boldsymbol{X}}_{3k}}}\end{array}} \right]+\\& \left[ {\begin{array}{*{20}{c}}{{\mathit{\boldsymbol{G}}_{1k}}} & {\bf{0}} & {\bf{0}}\\{\bf{0}} & {{\mathit{\boldsymbol{G}}_{2k}}} & {\bf{0}}\\{\bf{0}} & {\bf{0}} & {{\mathit{\boldsymbol{G}}_{3k}}}\end{array}} \right]\left[ {\begin{array}{*{20}{c}}{{\mathit{\boldsymbol{w}}_{1k}}}\\{{\mathit{\boldsymbol{w}}_{2k}}}\\{{\mathit{\boldsymbol{w}}_{3k}}}\end{array}} \right]\text{。}\end{split}$ (8)
1.2 量测方程

 ${r_{13k}} = \sqrt {{{({x_{1k}} - {x_{3k}})}^2} + {{({y_{1k}} - {y_{3k}})}^2}} + {v_r}\text{。}$ (9)

 $\begin{split}&{\mathit{\boldsymbol{Z}}_{13k}} = h({\mathit{\boldsymbol{X}}_k}) + {v_{13k}} = \left[ {\begin{array}{*{20}{c}}{{x_{1k}}}\\{{y_{1k}}}\\{{r_{1k}}}\end{array}} \right] + {v_{13k}}=\\& \left[ {\begin{array}{*{20}{c}}{{x_{1k}}}\\{{y_{1k}}}\\{\sqrt {{{({x_{1k}} - {x_{3k}})}^2} + {{({y_{1k}} - {y_{3k}})}^2}} }\end{array}} \right] + {\mathit{\boldsymbol{v}}_{13k}}\text{。}\end{split}$ (10)

 $\begin{split}&{\mathit{\boldsymbol{Z}}_{13k}} = \left[ {\begin{array}{*{20}{c}}{{x_{1k}}}\\{{y_{1k}}}\\{\sqrt {{{({x_{1k}} - {x_{3k}})}^2} + {{({y_{1k}} - {y_{3k}})}^2}} }\end{array}} \right] + {\mathit{\boldsymbol{v}}_{13k}}=\\& {\mathit{\boldsymbol{H}}_{13k}}{\mathit{\boldsymbol{X}}_k} + {\mathit{\boldsymbol{v}}_{13k}}=\\& \left[ {\begin{array}{*{20}{c}}{{\mathit{\boldsymbol{H}}_{1k}}} & {\bf{0}} & {{\mathit{\boldsymbol{H}}_{3k}}}\end{array}} \right]\left[ {\begin{array}{*{20}{c}}{{\mathit{\boldsymbol{X}}_{1k}}}\\{{\mathit{\boldsymbol{X}}_{2k}}}\\{{\mathit{\boldsymbol{X}}_{3k}}}\end{array}} \right] + {\mathit{\boldsymbol{v}}_{13k}}\text{。}\end{split}$ (11)

 ${\mathit{\boldsymbol{R}}_{{\rm{13k}}}} = E\left[ {{\mathit{\boldsymbol{v}}_{{\rm{13k}}}}\mathit{\boldsymbol{v}}_{13k}^{\rm T}} \right]\text{。}$ (12)

 $\left\{ {\begin{array}{*{20}{c}}\begin{array}{l}\!\!\!\!\!\!\!\!\!\!{X_{k + 1}} = \left[ {\begin{array}{*{20}{c}}{{X_{1k + 1}}}\\{{X_{2k + 1}}}\\{{X_{3k + 1}}}\end{array}} \right] = \left[ {\begin{array}{*{20}{c}}{{F_{1k}}}&{\bf{0}}&{\bf{0}}\\{\bf{0}}&{{F_{2k}}}&{\bf{0}}\\{\bf{0}}&{\bf{0}}&{{F_{3k}}}\end{array}} \right]\left[ {\begin{array}{*{20}{c}}{{X_{1k}}}\\{{X_{2k}}}\\{{X_{3k}}}\end{array}} \right] + \\\;\;\;\;\;\;\left[ {\begin{array}{*{20}{c}}{{G_{1k}}}&{\bf{0}}&{\bf{0}}\\{\bf{0}}&{{G_{2k}}}&{\bf{0}}\\{\bf{0}}&{\bf{0}}&{{G_{3k}}}\end{array}} \right]\left[ {\begin{array}{*{20}{c}}{{w_{1k}}}\\{{w_{2k}}}\\{{w_{3k}}}\end{array}} \right]\text{，}\end{array}\\\!\!\!\!\!{{Z_k} \!\!=\!\! {{\bf{H}}_{13k}}{X_k} \!+\! {{\bf{v}}_{13k}} \!=\! \left[ {\begin{array}{*{20}{c}}{{{\bf{H}}_{1k}}}&\!\!\!\!\!\!{\bf{0}}&\!\!\!\!\!{{{\bf{H}}_{3k}}}\end{array}} \right]\left[ {\begin{array}{*{20}{c}}{{X_{1k}}}\\{{X_{2k}}}\\{{X_{3k}}}\end{array}} \right] \!\!+\!\! {{\bf{v}}_{13k}}}\text{。}\!\!\!\!\end{array}} \right.$ (13)
 图 1 协同定位示意图 Fig. 1 Schematic diagram of cooperation localization
2 基于MHE的协同定位滤波方法

2.1 无量测更新阶段CKF滤波

 ${{\mathit{\boldsymbol{P}}}_{k - 1|k - 1}} = {{\mathit{\boldsymbol{S}}}_{k - 1|k - 1}}{\mathit{\boldsymbol{S}}}_{k - 1|k - 1}^{\rm T}\text{，}$ (14)

 ${{\mathit{\boldsymbol{X}}}_{i,k - 1|k - 1}} = {{\mathit{\boldsymbol{S}}}_{k - 1|k - 1}}{{\mathit{\boldsymbol{\xi }}}_i} + {{\hat{x}}_{k - 1|k - 1}}\text{，}$ (15)

 $\left\{ \begin{array}{l}{{\mathit{\boldsymbol{\xi }}}_i} = \sqrt {\displaystyle\frac{m}{2}} {[1]_i}\text{，}\\{{\mathit{\boldsymbol{\omega }}}_i} = \frac{1}{m},i = 1,...m,m = 2n\text{。}\end{array} \right.$ (16)

 ${\mathit{\boldsymbol{X}}}_{i,k|k - 1}^ * = f({{\mathit{\boldsymbol{X}}}_{i,k - 1|k - 1}}){\rm{ }}\text{，}$ (17)

 ${{\hat{x}}_{k|k - 1}} = \frac{1}{m}\sum\limits_{i = 1}^m {{\mathit{\boldsymbol{X}}}_{i,k|k - 1}^ * } \text{，}$ (18)

 ${{\mathit{\boldsymbol{P}}}_{k|k - 1}} = \frac{1}{m}\sum\limits_{i = 1}^m {{\mathit{\boldsymbol{X}}}_{i,k|k - 1}^ * } {\mathit{\boldsymbol{X}}}_{i,k|k - 1}^{ * {\rm T}} - {{\hat{x}}_{k|k - 1}}{\hat{x}}_{k|k - 1}^{\rm T} + {{\mathit{\boldsymbol{Q}}}_{k - 1}}{\rm{ }}\text{，}$ (19)

 ${{\mathit{\boldsymbol{P}}}_{k|k - 1}} = {{\mathit{\boldsymbol{S}}}_{k|k - 1}}{\mathit{\boldsymbol{S}}}_{k|k - 1}^{\rm T}\text{，}$ (20)

 ${{\mathit{\boldsymbol{X}}}_{i,k|k - 1}} = {{\mathit{\boldsymbol{S}}}_{k|k - 1}}{{\mathit{\boldsymbol{\xi }}}_i} + {{\hat{x}}_{k|k - 1}}\text{，}$ (21)

 ${{\hat{z}}_{k|k - 1}} = \frac{1}{m}\sum\limits_{i = 1}^m {{{\mathit{\boldsymbol{Z}}}_{i,k|k - 1}}} \text{，}$ (22)

 ${{\mathit{\boldsymbol{P}}}_{zz,k|k - 1}} = \frac{1}{m}\sum\limits_{i = 1}^m {{{\mathit{\boldsymbol{Z}}}_{i,k|k - 1}}} {\mathit{\boldsymbol{Z}}}_{i,k|k - 1}^{\rm T} - {{\hat{z}}_{k|k - 1}}{\hat{z}}_{k|k - 1}^{\rm T} + {{\mathit{\boldsymbol{R}}}_k}\text{，}$ (23)

 ${{\mathit{\boldsymbol{W}}}_k} = {{\mathit{\boldsymbol{P}}}_{xz,k|k - 1}}{\mathit{\boldsymbol{P}}}_{zz,k|k - 1}^{ - 1}\text{，}$ (24)

k时刻状态估计值：

 ${{\hat{x}}_{k|k}} = {{\hat{x}}_{k|k - 1}} + {{\mathit{\boldsymbol{W}}}_k}({{\mathit{\boldsymbol{z}}}_k} - {{\hat{z}}_{k|k - 1}})\text{，}$ (25)

k时刻状态误差协方差估计值：

 ${{\mathit{\boldsymbol{P}}}_{k|k}} = {{\mathit{\boldsymbol{P}}}_{k|k - 1}} - {{\mathit{\boldsymbol{W}}}_k}{{\mathit{\boldsymbol{P}}}_{zz,k|k - 1}}{\mathit{\boldsymbol{W}}}_k^{\rm T}\text{。}$ (26)
2.2 有量测更新阶段MHE状态估计

 $\mathop {\min }\limits_{\left\{ {{{\mathit{\boldsymbol{x}}}_i}} \right\}_{i = k}^s、\left\{ {{{\mathit{\boldsymbol{w}}}_j}} \right\}_{j = k}^{s - 1}} {\rm{ }}{{\mathit{\boldsymbol{J}}}_s} = \mathop {\min }\limits_{\left\{ {{{\mathit{\boldsymbol{x}}}_i}} \right\}_{i = k}^s、\left\{ {{{\mathit{\boldsymbol{w}}}_j}} \right\}_{j = k}^{s - 1}} {\rm{ }}\left( {\begin{array}{*{20}{c}}{\displaystyle\sum\limits_{i = k}^{s - 1} {\left\| {{{\mathit{\boldsymbol{w}}}_i}} \right\|_{{\mathit{\boldsymbol{Q}}}_i^{ - 1}}^2} }\\{ + \displaystyle\sum\limits_{i = k}^s {\left\| {{{\mathit{\boldsymbol{v}}}_i}} \right\|_{{\mathit{\boldsymbol{R}}}_i^{ - 1}}^2} }\\{ + {{\mathit{\boldsymbol{\Phi }}}_k}\left( {{{\mathit{\boldsymbol{x}}}_0}、\left\{ {{{\mathit{\boldsymbol{w}}}_i}} \right\}_{i = 0}^{k - 1}} \right)}\end{array}} \right)\text{。}$ (27)

 ${{\mathit{\boldsymbol{\varPhi }}}_k} = {\left( {{{\mathit{\boldsymbol{x}}}_k} - {{{\hat{x}}}_k}} \right)^{\rm T}}{\mathit{\boldsymbol{P}}}_k^{ - 1}\left( {{{\mathit{\boldsymbol{x}}}_k} - {{{\hat{x}}}_k}} \right) + {{\mathit{\boldsymbol{J}}}_k}*\text{。}$ (28)

3 仿真试验分析

 图 2 UUV运动轨迹 Fig. 2 The trajectories of UUVs

 图 3 从UUV导航轨迹 Fig. 3 The navigation trajectories of slaver UUVs

 图 4 从UUV定位误差比较图 Fig. 4 Comparison chart of slaver UUVs’ localization errors
4 结　语

 [1] 许真珍, 封锡盛. 多UUV协作系统的研究现状与发展[J]. 机器人, 2007, 29 (2): 186–192. [2] BACCOU P, JOUVENCEL B, CREUZE V. Single beacon acoustic for AUV navigation[C]// International Coference on Advanced Robotics, Budapest ICAR, 2001:1253–1260. [3] 李闻白, 刘明雍, 张立川, 等. 单领航者相对位移测量的多自主水下航行器协同导航[J]. 兵工学报, 2011, 32 (8): 1002–1007. [4] BAHR A, WALTER M R, LEONARD J J. Consistent cooperative localization. in robotics and automation[C]// IEEE International Conference on Robotics and Automation (ICRA'09), 2009: 3415–3422. [5] BAHR A. Cooperative Localization for Autonomous Underwater Vehicles [D]. MIT, 2009. [6] AGUIARY A, ALMEIDAY J, BAYATY M, et al. Cooperative autonomous marine vehicle motion control in the scope of the EU GREX project: theory and practice[C]// Oceans 2009-Europe. IEEE, 2009: 1–10. [7] DIBA M. Strategies in tracking and localization of distributed underwater systems [D]. San Diego, University of California. 2010. [8] 张立川, 刘明雍, 徐德民, 等. 基于水声传播延迟的主从式多无人水下航行器协同导航定位研究[J]. 兵工学报, 2009, 30 (12): 1674–1678. DOI: 10.3321/j.issn:1000-1093.2009.12.020 [9] 穆华. 多运动平台协同导航的分散式算法研究[D]. 长沙: 国防科学技术大学, 2010 [10] C. Constraint state estimation for nonlinear discrete-time systems: Stability and moving horizon approximations[J]. IEEE Transactions on Automatic Control, 2003, 48 (2): 246–258. DOI: 10.1109/TAC.2002.808470