﻿ CNS/SINS陀螺标定系统在UUV中的应用
 舰船科学技术  2020, Vol. 42 Issue (1): 168-171 PDF
CNS/SINS陀螺标定系统在UUV中的应用

Application of CNS/SINS gyro calibration system in UUV
LIU Zhi-hao, YU Xue-yong, XU Zhong-liang
Naval Submarine Academy, Qingdao 266000, China
Abstract: The gyroscope drift would enlarge UUV navigation error after long voyage underwater. GPS/SINS integrated navigation systems were generally used to calibrate IMU errors. However, it would consume much time due to the low-frequency of GPS devices and the bad influence of sea surface. In extreme cases, UUV might be detected or destroyed during floating time in military missions. Aid to this problem, an integrated system was designed by CNS. CNS could supply high accuracy attitude outputs in quaternion format. Random constant drift could be estimated by quaternion difference value between SINS and CNS in Kalman filter. The simulation results proved that this system is feasible and accurate. In future development, UUV could reduce calibration time by the combination of CNS and GPS.
Key words: CNS/SINS     integrated navigation     gyroscope drift     UUV
0 引　言

UUV作为一种新型海上军事装备与海上科研装备，凭借其出色的隐蔽性、环境适应性，能够在水下载人平台难以抵达的区域执行的多种任务，在军民领域均获得了广泛关注。在军事应用中，UUV的突出特点是具有极强的隐蔽性与及安全性，且造价低廉。它的体积远小于潜艇等有人平台，使得敌方的反潜兵力难以发现及破坏，无人驾驶的特征使其可以前往危险海域执行任务，低廉的造价使UUV可以大量部署，在水下战场中以数量上的优势取得主动权，形成非对称优势。

 图 1 CNS/GPS/SINS组合系统框图 Fig. 1 Integratedsystemof CNS/GPS/SINS
1 误差分析及测量原理 1.1 陀螺仪误差模型

 $\left\{ {\begin{array}{*{20}{l}} {\varepsilon = \xi + {n_r}}\text{，} \\ {\dot \xi = 0} \text{。} \end{array}} \right.$ (1)

1.2 CNS姿态测量

 $\psi = {\rm{arc}}\tan \left( {\frac{{2\left( {{q_1}{q_2} - {q_0}{q_3}} \right)}}{{q_0^2 - q_1^2 + q_2^2 - q_3^2}}} \right)\text{，}$ (2)
 $\theta = {\rm{arc}}\sin \left( {2\left( {{q_2}{q_3} + {q_0}{q_1}} \right)} \right)\text{，}$ (3)
 $\gamma = {\rm{arc}}\tan \left( { - \frac{{2\left( {{q_1}{q_3} - {q_0}{q_2}} \right)}}{{q_0^2 - q_1^2 - q_2^2 + q_3^2}}} \right)\text{。}$ (4)

CNS是以星敏感器为核心的导航定位系统。星光信号经光敏元件光电转换后，可结合导航星库确定载体姿态，具有精度高、质量小、功耗低、无漂移和工作方式多样等特点，同样具有无源自主导航能力，是一种性能优良、发展前途广阔的姿态测量部件[2]

 $\tilde {{C}}_b^i = {{{U}}_i}{{U}}_b^{\rm T}{\left( {{{{U}}_b}{{U}}_b^T} \right)^{ - 1}}\text{，}$ (5)

 $\tilde {{C}}_b^n = {{C}}_e^n{{C}}_i^e\tilde {{C}}_b^i\text{。}$ (6)

2 卡尔曼滤波器构建 2.1 状态方程构建

 $\delta Q = \hat Q_b^n \otimes {\left( {Q_b^n} \right)^{ - 1}} = {\left[ {\begin{array}{*{20}{c}} {\delta e}&{\delta q} \end{array}} \right]^{\rm T}}\text{，}$ (7)

 ${\left( {Q_b^n} \right)^{ - 1}} = {\left( {Q_b^n} \right)^*}\text{，}$ (8)

 $\delta \dot Q = \dot {\hat Q}_b^n \otimes {\left( {Q_b^n} \right)^*} + {\hat Q}_b^n \otimes {\left( {{\dot Q}_b^n} \right)^*}\text{，}$ (9)

 $\dot Q_b^n = \frac{1}{2}\omega _{nb}^n \otimes Q_b^n\text{，}$ (10)

 $\begin{split} \delta \dot Q =& \frac{1}{2}\hat \omega _{nb}^n \otimes \hat Q_b^n \otimes {\left( {Q_b^n} \right)^*} + \hat Q_b^n \otimes \frac{1}{2}{\left( {\omega _{nb}^n} \right)^*} \otimes {\left( {Q_b^n} \right)^*}= \\ & \frac{1}{2}\hat \omega _{nb}^n \otimes \delta Q + \hat Q_b^n \otimes \frac{1}{2}{\left( {\omega _{nb}^n} \right)^*} \otimes {\left( {Q_b^n} \right)^*} \text{。} \end{split}$ (11)

 $\hat Q_b^n = {\left[ {\delta \hat e,\delta \hat q} \right]^{\rm T}}\text{，}$

 $\hat Q_b^n \otimes {\left( {\omega _{nb}^n} \right)^*} = \left[ {\begin{array}{*{20}{c}} 0&{\omega {{_{nb}^n}^{\rm T}}} \\ { - \omega _{nb}^n}&{\omega _{nb}^n \times } \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {\delta \hat e} \\ {\delta \hat q} \end{array}} \right]\text{，}$ (12)

 $\begin{split} & \hat Q_b^n \otimes \frac{1}{2}{\left( {\omega _{nb}^n} \right)^*} \otimes {\left( {Q_b^n} \right)^*}= \\ & \qquad \frac{1}{2}\left[ {\begin{array}{*{20}{c}} 0&{\omega {{_{nb}^n}^T}} \\ { - \omega _{nb}^n}&{\omega _{nb}^n \times } \end{array}} \right] \otimes \hat Q_b^n \otimes {\left( {Q_b^n} \right)^*} = \\ & \qquad \frac{1}{2}\left[ {\begin{array}{*{20}{c}} 0&{\omega {{_{nb}^n}^T}} \\ { - \omega _{nb}^n}&{\omega _{nb}^n \times } \end{array}} \right] \otimes \delta Q \text{。} \end{split}$ (13)

 $\delta \dot Q = \frac{1}{2}\left\{ {\left[ {\begin{array}{*{20}{c}} 0&{ - \hat \omega {{_{nb}^n}^{\rm T}}} \\ {\hat \omega _{nb}^n}&{\hat \omega _{nb}^n \times } \end{array}} \right] + \left[ {\begin{array}{*{20}{c}} 0&{\omega {{_{nb}^n}^{\rm T}}} \\ { - \omega _{nb}^n}&{\omega _{nb}^n \times } \end{array}} \right]} \right\} \otimes \delta Q\text{。}$ (14)

$\omega _{nb}^n = \hat \omega _{nb}^n - \varepsilon$ 代入式（14）可得：

 $\delta \dot Q = \frac{1}{2}\left[ {\begin{array}{*{20}{c}} 0&{ - {\varepsilon ^T}} \\ \varepsilon &{2\hat \omega _{nb}^n \times - \varepsilon \times } \end{array}} \right] \otimes \delta Q\text{，}$ (15)

 $\delta \dot q = \hat \omega _{nb}^n \times \delta q + \frac{1}{2}\varepsilon \text{，}$ (16)

 ${{{X}}^{\rm T}} = \left[ {\begin{array}{*{20}{l}} {\delta {q_1}}&{\delta {q_2}}&{\delta {q_3}}&{{\xi _{x1}}}&{{\xi _{x2}}}&{{\xi _{x3}}} \end{array}} \right]\text{，}$

 $\dot {{X}} = {{AX}} + {{W}}\text{。}$

 ${{A}} = \left[ {\begin{array}{*{20}{c}} {\hat \omega _{nb}^n \times }&{\frac{1}{2}{I_{3 \times 3}}} \\ {{0_{3 \times 3}}}&{{0_{3 \times 3}}} \end{array}} \right], {{W}} = {\left[ {\begin{array}{*{20}{c}} {{n_{1 \times 3}}}&{{O_{1 \times 3}}} \end{array}} \right]^{\rm T}}\text{。}$
2.2 量测方程构建

 ${{Z}} = {{HX}} + v = {{{I}}_{3 \times 3}}{{X}} + v\text{，}$

 $\left\{ {\begin{array}{*{20}{l}} {{{{X}}_{k|k - 1}} = {{{\varPhi}} _{k|k - 1}}{{{X}}_{k - 1|k - 1}} + {{{W}}_k}}\text{，} \\ {{{{Z}}_k} = {{{H}}_k}{{{X}}_{k|k}} + {{{V}}_k}} \text{，} \end{array}} \right.$ (17)

 ${{{X}}_{k|k - 1}} = {{{\varPhi}} _{k|k - 1}}{{{X}}_{k - 1|k - 1}}\text{，}$ (18)
 ${{{P}}_{k|k - 1}} = {{{\varPhi}} _{k|k - 1}}{P_{k - 1|k - 1}}{{\varPhi}} _{k|k - 1}^{\rm T} + {{{Q}}_k}\text{，}$ (19)
 ${{{K}}_k} = {{{P}}_{k|k - 1}}{{H}}_k^{\rm T} {{R}}_k^{ - 1}\text{，}$ (20)
 ${{{X}}_{k|k}} = {{{X}}_{k - 1|k - 1}} + {{{K}}_k}\left( {{{{Z}}_k} - {{{H}}_k}{{{X}}_{k|k - 1}}} \right)\text{，}$ (21)
 ${{{P}}_{k|k}} = \left( {{{I}} - {{{K}}_k}{{{H}}_k}} \right){{{P}}_{k|k - 1}} \text{。}$ (22)
3 仿真验证

3.1 仿真参数设置

 $\left\{ {\begin{array}{*{20}{c}} {{{[ - 0.146\;13,0.475\;36,0.443\;98]}^{\rm T}}}\text{，} \\ {{{[ - 0.219\;20,0.713\;04,0.665\;98]}^{\rm T}}}\text{，} \\ {{{[ - 0.423\;45,0.275\;01,0.703\;60]}^{\rm T}}} \text{。} \end{array}} \right.$
3.2 仿真分析

 图 2 X轴方向陀螺常值漂移估计 Fig. 2 Random constant drift in axis X

 图 4 Z轴方向陀螺常值漂移估计 Fig. 4 Random constant drift in axis Z

 图 3 Y轴方向陀螺常值漂移估计 Fig. 3 Random constant drift in axis Y

4 结　语

CNS能够提供不随时间发散的稳定的高精度姿态信息，而GPS能够提供不随时间发散的稳定的位置和速度信息，二者组合在一起时具有极强的互补性。GPS/SINS系统进行失准角和陀螺漂移校准时，需要较大机动以增强可观性，而这与UUV上浮后需要停车，避免螺旋桨击打海面造成损坏这一工程实现的实际情况不相吻合，引入CNS后能够有效解决这种问题。利用Matlab对标定系统进行仿真，验证了其在标定陀螺常值漂移方面的可行性。以这种方法进行标定后，可结合CNS给出的精确姿态，降低GPS/SINS组合导航中状态变量的维数，缩短上浮校准时间，为增强UUV的隐蔽性与安全性提供一种解决问题的思路。

 [1] 王新龙. 捷联式惯导系统动、静基座初始对准[M]. 西安: 西北工业大学出版社, 2013. [2] 刘朝山, 刘光斌, 王新国, 等. 弹载星敏感器原理及系统应用[M]. 北京: 国防工业出版社, 2010. [3] PAUL D. Groves. Principles of GNSS, Inertial, andMultisensor Integrated Navigation Systems[M]. London: Artech HOUSE, 2013. [4] 付梦印, 郑辛, 邓志红, 等.传递对准理论与应用[M]. 北京: 科学出版社,2012. [5] 牟宏伟. 水下航行器组合导航系统与信息融合技术研究[D]. 哈尔滨: 哈尔滨工程大学, 2013. http://www.cnki.com.cn/Article/CJFDTotal-ZGXJ200903020.htm [6] 齐昭. 船用捷联惯导/天文/计程仪组合导航关键技术研究[D]. 哈尔滨: 哈尔滨工程大学. 2015 http://cdmd.cnki.com.cn/Article/CDMD-10217-1017242644.htm [7] 张鹏. 船用星敏感器/惯性导航系统在线标定及组合导航技术研究[D]. 哈尔滨: 哈尔滨工程大学. 2016. [8] 肇慧, 沈继红, 陈涛. SINS/CNS组合导航系统在线标定技术[J]. 计算机工程与应用, 2013, 49(19): 1-5. DOI:10.3778/j.issn.1002-8331.1304-0227 [9] 邓红, 刘光斌, 陈昊明, 等. 发射惯性坐标系下误差角与数学平台失准角的推导与仿真[J]. 宇航学报, 2011, 32(4): 781-786. DOI:10.3873/j.issn.1000-1328.2011.04.012 [10] 王国臣, 齐昭, 张卓. 水下组合导航系统[M]. 北京: 国防工业出版社, 2016.