﻿ 基于SRUKF的模型辅助AUV惯性导航系统
 舰船科学技术  2021, Vol. 43 Issue (7): 153-157    DOI: 10.3404/j.issn.1672-7649.2021.07.031 PDF

A model-aided inertial navigation system for miniature AUV exploiting SRUKF
WANG Ke-yu, WANG Jun-xiong
State Key Laboratory of Ocean Engineering, Shanghai Jiaotong University, Shanghai 200240, China
Abstract: The equipment of sensors for miniature Autonomous Underwater Vehicles (AUVs) is restricted because of limited space and low power. And Extend Kalman Filter (EKF) suffers from the potential divergence due to the high nonlinear of AUV dynamic equation. Consequently, traditional navigation system which relies on sensor fusion is difficult to be deployed in AUVs navigation. In this paper, an inertial navigation system (INS) based on Square-Root Unscented Kalman Filter (SRUKF) is proposed, using AUVs model as aiding source. Navigation errors are compensated via fusing six-axis acceleration sensor data and AUV motion data calculated by dynamic equation. The feasibility of this INS is validated by half-real test and Matlab simulation test, in which it shows higher accuracy and better robust than EKF, and navigation demands of miniature AUVs can be satisfied.
Key words: inertial navigation     SRUKF     model-aided     miniature AUV
0 引　言

1 AUV建模 1.1 坐标系定义

 图 1 坐标系定义 Fig. 1 The definition of coordinate
1.2 运动学与动力学建模

 $\left\{ {\begin{array}{*{20}{c}} {{{{{\dot \eta }}}_1} = {{{J}}_1}\left( {{{{\eta }}_2}} \right){{{\nu }}_1}}\text{，} \\ {{{{{\dot \eta }}}_2} = {{{J}}_2}\left( {{{{\eta }}_2}} \right){{{\nu }}_2}} \text{。} \end{array}} \right.$ (1)

 ${{{J}}_1} \!\!=\!\! \left[\!\!\!\! {\begin{array}{*{20}{c}} {c\theta c\psi }&{ - s\psi c\phi + c\psi s\theta s\phi }&{s\psi s\phi + c\psi s\theta c\phi } \\ {s\psi c\theta }&{c\psi c\phi + s\psi s\theta s\phi }&{ - c\psi s\phi + s\psi s\theta c\phi } \\ { - s\theta }&{c\theta s\phi }&{c\theta c\phi } \end{array}} \!\!\!\!\right]\text{，}$ (2)
 ${{{J}}_2} = \left[ {\begin{array}{*{20}{c}} 1&{s\phi t\theta }&{c\phi t\theta } \\ 0&{c\phi }&{ - s\phi } \\ 0&{s\phi /c\theta }&{c\phi /c\theta } \end{array}} \right]\text{。}$ (3)

 ${{M\dot \nu }} + {{C}}\left( \nu \right){{\nu }} + {{D}}\left( \nu \right){{\nu }} + {{g}}\left( \eta \right) = {{\tau }}\text{。}$ (4)

 ${{{M}}_A} = \left[ {\begin{array}{*{20}{c}} {{{{A}}_{11}}}&{{{{A}}_{12}}} \\ {{{{A}}_{21}}}&{{{{A}}_{22}}} \end{array}} \right]\text{，}$ (5)
 ${{{M}}_{RB}} = \left[ {\begin{array}{*{20}{c}} {m{{{I}}_{3 \times 3}}}&{{{{0}}_{3 \times 3}}} \\ {{{{0}}_{3 \times 3}}}&{{{{I}}_0}} \end{array}} \right]\text{，}$ (6)
 ${{{C}}_A}\left( {{\nu }} \right)\!\! = \!\!\left[ \!\!\!\!{\begin{array}{*{20}{c}} {{{{0}}_{3 \times 3}}}&\!\!{ - {{S}}\left( {{{{A}}_{11}}{{{\nu }}_1} + {{{A}}_{12}}{{{\nu }}_2}} \right)} \\ { - {{S}}\left( {{{{A}}_{11}}{{{\nu }}_1} + {{{A}}_{12}}{{{\nu }}_2}} \right)}&\!\!{ - {{S}}\left( {{{{A}}_{21}}{{{\nu }}_1} + {{{A}}_{22}}{{{\nu }}_2}} \right)} \end{array}} \!\!\!\!\right]\text{，}$ (7)
 $\begin{split} &{C}_{RB}(\nu )=\\ &\left[\begin{array}{cc}{0}_{3\times 3}& -mS({\nu }_{1})-mS(S({\nu }_{2})){r}_{G})\\ -mS({\nu }_{1})-mS(S({\nu }_{2})){r}_{G})& mS(S({\nu }_{1})){r}_{G})-S({I}_{0}{\nu }_{2})\end{array}\right]\end{split}\text{。}$ (8)

 ${{g}}\left( {{\eta }} \right)\!\! =\!\! \left[ \!\!\!\!{\begin{array}{*{20}{c}} {\left( {W - B} \right)s\theta } \\ { - \left( {W - B} \right)s\phi c\theta } \\ { - \left( {W - B} \right)c\phi c\theta } \\ { - \left( {{y_G}W - {y_B}B} \right)c\phi c\theta + \left( {{z_G}W - {z_B}B} \right)s\phi c\theta } \\ {\left( {{z_G}W - {z_B}B} \right)s\theta + \left( {{x_G}W - {x_B}B} \right)c\phi c\theta } \\ {\left( {{x_G}W - {x_B}B} \right)s\phi c\theta + \left( {{y_G}W - {y_B}B} \right)s\theta } \end{array}}\!\! \right]\text{。}$ (9)
2 惯性导航算法设计

 图 2 惯性导航系统结构框图 Fig. 2 The diagram of inertial navigation system
2.1 位姿解算算法设计

k时刻AUV角度变化量为 $\Delta {{\theta }} = {[{\theta _x},{\theta _y},{\theta _z}]^{\rm T}}$ ，加速度值为 ${{{a}}_k} = {[{a_x},{a_y},{a_z}]^{\rm T}}$ 。采用角度变化量形式的毕卡算法求解四元数[5]。本文采用4阶近似形式以保证精度，则

 $\Delta {{\theta }} = {{{\nu }}_2}\left( {{t_k} - {t_{k - 1}}} \right)\text{，}$ (10)
 $\begin{split}{{{q}}_{new}} =& {{{I}}_{4 \times 4}}\left( 1 - 0.125\Delta {{{\theta }}^{\rm T}}\Delta {{\theta }} + 0.0026{{\left( {\Delta {{{\theta }}^{\rm T}}\Delta {{\theta }}} \right)}^2} +\right.\\ &\left.\left( {0.5 - 0.0208\Delta {{{\theta }}^{\rm T}}\Delta {{\theta }}} \right)\Delta {{\Theta }} \right){{q}}\text{，}\end{split}$ (11)

 $\left\{ {\begin{array}{*{20}{l}} {\phi {{ }} = arctan\left( {{{C}}_b^n\left( {3,2} \right),{{C}}_b^n\left( {3,3} \right)} \right)}\text{，} \\ {\theta {{ }} = - arcsin\left( {{{C}}_b^n\left( {3,1} \right)} \right)}\text{，} \\ {\psi {{ }} = arctan\left( {{{C}}_b^n\left( {2,1} \right),{{C}}_b^n\left( {1,1} \right)} \right)} \text{。} \end{array}} \right.$ (12)

 $\begin{split} C_{{b}}^n =& \left[ {\begin{array}{*{20}{c}} {1 - 2(q{{(2)}^2} + q{{(3)}^2})}&{2(q(1)q(2) - q(0)q(3))} \\ {2(q(1)q(2) - q(0)q(3))}&{1 - 2(q{{(1)}^2} + q{{(3)}^2})}\\ {2(q(1)q(3) - q(0)q(2))}&{2(q(2)q(3) - q(0)q(1))} \end{array}} \right.\\ &\left.\begin{array}{*{20}{c}} {2(q(1)q(3) - q(0)q(2))}\\ {2(q(2)q(3) - q(0)q(1))} \\ {1 - 2(q{{(1)}^2} + q{{(2)}^2})} \end{array}\right]\text{。}\\[-30pt]\end{split}$ (13)

 ${{{\nu }}_{{1_k}}} = \int_{{t_{k - 1}}}^{{t_k}} {{a}} {\rm d}t \doteq \frac{1}{2}\left( {{{{a}}_{k - 1}} + {{{a}}_k}} \right)\left( {{t_k} - {t_{k - 1}}} \right) + {{{\nu }}_{{1_{k - 1}}}}\text{，}$ (14)
 ${{{\dot \eta }}_{{1_k}}} = {{{J}}_1}\left( {{{{\eta }}_{{2_k}}}} \right)\text{，}$ (15)
 ${{{\eta }}_{{1_k}}} = \int_{{t_{k - 1}}}^{{t_k}} {{{{{\dot \eta }}}_1}} {\rm d}t \doteq \frac{1}{2}\left( {{{{{\dot \eta }}}_{{1_{k - 1}}}} + {{{{\dot \eta }}}_{{1_k}}}} \right)\left( {{t_k} - {t_{k - 1}}} \right) + {{{\eta }}_{{1_{k - 1}}}}\text{。}$ (16)
2.2 平方根无迹卡尔曼滤波算法设计

1）参数设置及初始化

 ${{{\hat X}}_0} = E\left[ {{{{X}}_0}} \right]\text{，}$ (17)
 ${{{S}}_{0|0}} = chol\left( {E\left[ {\left( {{{{X}}_0} - {{{{\hat X}}}_0}} \right){{\left( {{{{X}}_0} - {{{{\hat X}}}_0}} \right)}^{\rm T}}} \right]} \right)\text{。}$ (18)

2）计算sigma点

 ${{\bar X}} = {{{\hat X}}_{k - 1}}\text{，}$ (19)
 ${{S}} = {{{S}}_{k - 1|k - 1}}\text{，}$ (20)
 ${{\xi }} = \left[ {{{\bar X}},{{\bar X}} + \sqrt {n + \lambda } {{S}},{{\bar X}} - \sqrt {n + \lambda } {{S}}} \right]\text{。}$ (21)

3）计算时间更新方程

 ${{X}}_{k|k - 1}^{\left( i \right)} = f\left( {{{{\xi }}^{\left( i \right)}},{{u}}} \right)\text{，}$ (22)

 ${{{\hat X}}_{k|k - 1}} = {\omega _{m0}}{{X}}_{k|k - 1}^{\left( 1 \right)} + \sum\limits_{i = 2}^{2n + 1} {{\omega _m}} {{X}}_{k|k - 1}^{\left( i \right)}\text{，}$ (23)
 $\begin{split} {{S}}_{k|k - 1}^ - =& qr\left( \left[ \sqrt {{\omega _c}} \left( {{{X}}_{k|k - 1}^{\left( 2 \right)} - {{{{\hat X}}}_{k|k - 1}}} \right)\quad \sqrt {{\omega _c}} \left( {{{X}}_{k|k - 1}^{\left( 3 \right)} - {{{{\hat X}}}_{k|k - 1}}} \right)\right.\right.\\ & \left.\left. \cdots \quad \sqrt {{\omega _c}} \left( {{{X}}_{k|k - 1}^{\left( {2n + 1} \right)} - {{{{\hat X}}}_{k|k - 1}}} \right),\sqrt {{Q}} \right] \right)\text{，}\\[-10pt]\end{split}$ (24)
 ${{{S}}_{k|k - 1}} = {\rm{cholupdate}}\left[ {{{S}}_{k|k - 1}^ - ,{{X}}_{k|k - 1}^{\left( 1 \right)} - {{{{\hat X}}}_{k|k - 1}},{\omega _{c0}}} \right]\text{。}$ (25)

 $\left\{ {\begin{array}{*{20}{l}} {{\omega _{m0}} = \dfrac{\lambda }{{n + \lambda }}}\text{，} \\ {{\omega _{c0}} = {\omega _{m0}} + \left( {1 - {\alpha ^2} + \beta } \right)}\text{，} \\ {{\omega _m} = {\omega _c} = \dfrac{1}{{2\left( {n + \lambda } \right)}}} \text{。} \end{array}} \right.$ (26)

4）计算观测更新方程

 ${{Z}}_{k|k - 1}^{\left( i \right)} = h\left( {{{X}}_{k|k - 1}^{\left( i \right)}} \right)\text{，}$ (27)

 ${{{\hat Z}}_{k|k - 1}} = {\omega _{c0}}{{Z}}_{k|k - 1}^{\left( 1 \right)} + \sum\limits_{i = 2}^{2n + 1} {{\omega _c}} {{Z}}_{k|k - 1}^{\left( i \right)}\text{，}$ (28)
 $\begin{split}{{S}}_Z^ - =& qr\left( \left[ \sqrt {{\omega _c}} \left( {{{Z}}_{k|k - 1}^{\left( 2 \right)} - {{{{\hat Z}}}_{k|k - 1}}} \right)\quad \sqrt {{\omega _c}} \left( {{{Z}}_{k|k - 1}^{\left( 3 \right)} - {{{{\hat Z}}}_{k|k - 1}}} \right)\right.\right.\\ &\left.\left.\cdots \quad \sqrt {{\omega _c}} \left( {{{Z}}_{k|k - 1}^{\left( {2n + 1} \right)} - {{{{\hat Z}}}_{k|k - 1}}} \right),\sqrt {{R}} \right] \right)\text{，}\\[-10pt]\end{split}$ (29)
 ${{{S}}_Z} = {\rm{cholupdate}}\left[ {{{S}}_Z^ - ,{{Z}}_{k|k - 1}^{\left( 1 \right)} - {{{{\hat Z}}}_{k|k - 1}},{\omega _{c0}}} \right]$ (30)
 $\begin{split}{{{P}}_{XZ}} =& {\omega _{c0}}\left( {{{X}}_{k|k - 1}^{\left( 1 \right)} - {{{{\hat X}}}_{k|k - 1}}} \right){\left( {{{Z}}_{k|k - 1}^{\left( 1 \right)} - {{{{\hat Z}}}_{k|k - 1}}} \right)^T} +\\ &\sum\limits_{i = 2}^{2n + 1} {{\omega _c}} \left( {{{X}}_{k|k - 1}^{\left( i \right)} - {{{{\hat X}}}_{k|k - 1}}} \right){\left( {{{Z}}_{k|k - 1}^{\left( i \right)} - {{{{\hat Z}}}_{k|k - 1}}} \right)^T}\text{。}\end{split}$ (31)

5）结果更新

 ${{{K}}_k} = {{{P}}_{XZ}}{\left( {{{{S}}_Z}{{S}}_Z^T} \right)^{ - 1}}\text{，}$ (32)
 ${{{\hat X}}_{k|k}} = {{{\hat X}}_{k|k - 1}} + {{{K}}_k}\left( {{{{Z}}_k} - {{{{\hat Z}}}_{k|k - 1}}} \right)\text{，}$ (33)
 ${{U}} = {{{K}}_k}{{{S}}_Z}\text{，}$ (34)
 ${{{S}}_{k|k}} = {\rm{cholupdate}}\left[ {{{{S}}_{k|k - 1}},{{U}}, - 1} \right]\text{。}$ (35)
3 仿真、实验与结果分析

 图 3 静态姿态解算结果 Fig. 3 The angle estimation with still state

 图 4 仿真流程图 Fig. 4 The diagram of simulation flowchart

 $\begin{split}\left[ {\begin{array}{*{20}{c}} {{{\bar a}_x}} \\ {{{\bar a}_y}} \\ {{{\bar a}_z}} \end{array}} \right] =& \left[ {\begin{array}{*{20}{c}} {1.0021}&{ - 0.0063}&{ - 0.0041} \\ {0.0003}&{1.0027}&{0.0020} \\ { - 0.0128}&{0.0011}&{1.0003} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{a_x}} \\ {{a_y}} \\ {{a_z}} \end{array}} \right] +\\ &\left[ {\begin{array}{*{20}{c}} { - 0.0055} \\ {0.0790} \\ {0.0105} \end{array}} \right] + {{\tilde a}}\text{。}\end{split}$ (36)

 ${{{\tilde a}}_k} = - 0.3387{{{\tilde a}}_{k - 1}} + {{{\alpha }}_t}\text{，}$ (37)

 $\bar \Delta {{\theta }} = \Delta {{\theta }} - 0.2373\tilde \Delta {{\theta }} + {{{\beta }}_t}\text{。}$ (38)

 图 5 姿态解算结果 Fig. 5 Angle estimation

 图 10 位移解算误差 Fig. 10 The error of position estimation

 图 6 速度解算结果 Fig. 6 Velocity estimation

 图 7 位置解算结果 Fig. 7 Position estimation

 图 8 姿态解算误差 Fig. 8 The error of angle estimation

 图 9 速度解算误差 Fig. 9 The error of velocity estimation

4 结　语

 [1] BLIDBERG D R. The development of autonomous underwater vehicles (AUV); a brief summary[C]//IEEE Icra. 2001(4): 1. [2] FOSSEN T I, FJELLSTAD O-E. Nonlinear modelling of marine vehicles in 6 degrees of freedom[J]. Mathematical Modelling of Systems, 1995, 1(1): 17-27. DOI:10.1080/13873959508837004 [3] HEGRENAES Ø, HALLINGSTAD O. Model-aided INS with sea current estimation for robust underwater navigation[J]. IEEE Journal of Oceanic Engineering, 2011, 36(2): 316-337. DOI:10.1109/JOE.2010.2100470 [4] Bryson, Mitchell, Sukkarieh, et al. Vehicle model aided inertial navigation for a UAV using low-cost sensors[C]// Intelligent Transportation Systems, IEEE. IEEE, 2006. [5] 邓正隆. 惯性技术[M]. 哈尔滨: 哈尔滨工业大学出版社, 2006. [6] ALLOTTA B, CAITI A, COSTANZI R, et al. A new AUV navigation system exploiting unscented Kalman filter[J]. Ocean Engineering, 2016, 113: 121-132. DOI:10.1016/j.oceaneng.2015.12.058 [7] JULIER S J, UHLMANN J K. Unscented filtering and nonlinear estimation[J]. Proceedings of the IEEE, 2004, 92(3): 401-422. DOI:10.1109/JPROC.2003.823141 [8] 赵明亮, 汪立新, 秦伟伟. 基于状态扩增的MEMS陀螺随机误差实时滤波研究[J]. 电光与控制, 2019, 26(5): 72-76.