舰船科学技术  2020, Vol. 42 Issue (8): 157-161    DOI: 10.3404/j.issn.1672-7649.2020.08.029 PDF

1. 海军装备部，北京 100071;
2. 中国船舶集团有限公司，北京 100097;
3. 中国舰船研究院，北京 100192

Research on high precision and fast self-alignment of strapdown inertial navigation system with sloshing base
MI Xiao-long1, GUI Shi-hong2, YIN Hong-liang3
1. Naval Equipment Department, Beijing 100071, China;
2. China State Shipbuilding Corporation Limited, Beijing 100097, China;
3. China Ship Research and Development Academy, Beijing 100192, China
Abstract: Traditional autonomous alignment requires the SINS to accurately sense the angular velocity of the earth rotation, so the SINS must be in a static or slightly wobble state during alignment, which limits the applicable scope of autonomous alignment. Moreover, it is difficult for general shipborne weapon systems to be in an absolute static state.In order to realize the self-alignment of ship-borne weapons under the shaking condition, a self-alignment scheme under the shaking base is proposed in this paper.Firstly, in the coarse alignment stage, based on the projection of gravity acceleration in the inertial space, the attitude array is divided into four matrices and calculated respectively to reduce the influence of sloshing on the coarse alignment. Secondly, the correlation filter is designed with the improved observability of the system under the condition of sloshing.The feasibility of this alignment scheme is verified by experiments.
Key words: strapdown inertial navigation system     autonomous alignment     sloshing base
0 引　言

1 粗对准算法设计 1.1 坐标系定义

1）导航坐标系（ $o{x_n}{y_n}{z_n}$ 系）

2）载体坐标系（ $o{x_b}{y_b}{z_b}$ 系）

3）经线地球坐标系（ $o{x_{{e_0}}}{y_{{e_0}}}{z_{{e_0}}}$ 系）

4）经线地心惯性坐标系（ $o{x_{{i_0}}}{y_{{i_0}}}{z_{{i_0}}}$ 系）

5）载体惯性坐标系（ $o{x_{{i_{{b_0}}}}}{y_{{i_{{b_0}}}}}{z_{{i_{{b_0}}}}}$ 系）

1.2 算法设计

 ${{C}}_b^n\left( t \right) = {{C}}_{{e_0}}^nC_{{i_0}}^{{e_0}}\left( t \right){{C}}_{{i_{{b_0}}}}^{{i_0}}{{C}}_b^{{i_{{b_0}}}}\left( t \right)\text{,}$ (1)

 ${{C}}_{{e_0}}^n = \left[ {\begin{array}{*{20}{c}} 0&{\cos L}&{ - \sin L} \\ 0&{\sin L}&{\cos L} \\ 1&0&0 \end{array}} \right]\text{,}$ (2)
 ${{C}}_{{i_0}}^{{e_0}}\left( t \right) = \left[ {\begin{array}{*{20}{c}} {\cos {\omega _{ie}}\left( {t - {t_0}} \right)}&0&{ - \sin {\omega _{ie}}\left( {t - {t_0}} \right)} \\ 0&1&0 \\ {\sin {\omega _{ie}}\left( {t - {t_0}} \right)}&0&{\cos {\omega _{ie}}\left( {t - {t_0}} \right)} \end{array}} \right]\text{。}$ (3)

${{C}}_{{e_0}}^n$ ${{C}}_{{i_0}}^{{e_0}}\left( t \right)$ ${{C}}_b^{{i_{{b_0}}}}\left( t \right)$ 可利用已知信息计算确定，因此只要能得到 ${{C}}_{{i_{{b_0}}}}^{{i_0}}$ 的估值就能完成 ${{C}}_b^n$ 的粗略估算，即粗对准的任务由对 ${{C}}_b^n$ 的估算问题转化为对 ${{C}}_{{i_{{b_0}}}}^{{i_0}}$ 的估算问题。而 ${{C}}_{{i_{{b_0}}}}^{{i_0}}$ 是载体惯性坐标系到经线地心惯性坐标系的姿态转换矩阵，因此可利用在这2个惯性空间内2个不共线矢量的测量来确定。受地球自转影响，重力在惯性坐标系会发生转动，所以重力在不同时间段内在这两个坐标系的积分所得矢量是不共线的，故可以利用重力信息进行 ${{C}}_{{i_{{b_0}}}}^{{i_0}}$ 的求取。

 ${\tilde f^{{i_{{b_0}}}}} = \hat {{C}}_b^{{i_{{b_0}}}}{\tilde f^b}\text{。}$ (4)

 $\begin{split} {{\tilde f}^b} = & \left( {I + \delta {K_A}} \right)\left( {I + \delta A} \right) \times\\ & \left( { - {g^b} + a_{LA}^b + a_D^b + a_{gor}^b} \right) + {\nabla ^b} = \\ {} & - {g^b} + a_{LA}^b + a_D^b + a_{gor}^b + \left( {\delta A + \delta {K_A}} \right)\times \\ & \left( { - {g^b} + a_{LA}^b + a_D^b + a_{gor}^b} \right) + {\nabla ^b} \text{。}\\ \\[-8pt]\end {split}$ (5)

 $\begin{split} \hat C_b^{{i_{{b_0}}}}{{\tilde f}^b} = & - \hat C_b^{{i_{{b_0}}}}{g^b} + \hat C_b^{{i_{{b_0}}}}a_{LA}^b + \hat C_b^{{i_{{b_0}}}}a_D^b + \hat C_b^{{i_{{b_0}}}}{\nabla ^b} + \\ & \hat C_b^{{i_{{b_0}}}}\left( {\delta A + \delta {K_A}} \right)\left( { - {g^b} + a_{LA}^b + a_D^b + a_{gor}^b} \right) + \\ & \hat C_b^{{i_{{b_0}}}}{g^b} + \hat C_b^{{i_{{b_0}}}}a_{LA}^b + \delta {a^{{i_{{b_0}}}}} \hfill\text{。} \\ \\[-8pt]\end{split}$ (6)

 \begin{aligned} \delta {a^{{i_{{b_0}}}}} = & \hat C_b^{{i_{{b_0}}}}{\nabla ^b} + \hat C_b^{{i_{{b_0}}}}\left( {\delta A + \delta {K_A}} \right)\times \\ & \left( { - {g^b} + a_{LA}^b + a_D^b + a_{gor}^b} \right)\text{。} \\ \end{aligned} (7)

 $\begin{split} {{\hat V}^{{i_{{b_0}}}}} = &\int_{{t_0}}^{{t_k}} {\hat C_b^{{i_{{b_0}}}}{{\tilde f}^b}} {\rm{d}}t =\\ & - \int_{{t_0}}^{{t_k}} {\hat C_b^{{i_{{b_0}}}}{g^b}} {\rm{d}}t + \int_{{t_0}}^{{t_k}} {\hat C_b^{{i_{{b_0}}}}a_{LA}^b} {\rm{d}}t + \int_{{t_0}}^{{t_k}} {\delta {a^{{i_{{b_0}}}}}} {\rm{d}}t= \\ & - \hat C_{{i_0}}^{{i_{{b_0}}}}\int_{{t_0}}^{{t_k}} {{g^{{i_0}}}} {\rm{d}}t + V_{LA}^{{i_{{b_0}}}} + \delta {V^{{i_{{b_0}}}}} \\ \\[-8pt]\end{split}$ (8)

 $\hat V_{LA}^{{i_{{b_0}}}} = V_{LA}^{{i_{{b_0}}}} + \delta V_{LA}^{{i_{{b_0}}}}\text{，}$ (9)

 $\begin{split} \tilde V_{}^{{i_{{b_0}}}} = & \hat V_{}^{{i_{{b_0}}}} - \hat V_{LA}^{{i_{{b_0}}}} = \\ & - \hat C_{{i_0}}^{{i_{{b_0}}}}\int_{{t_0}}^{{t_k}} {{g^{{i_0}}}{\rm{d}}t - \delta V_{LA}^{{i_{{b_0}}}} + \delta {V^{{i_{{b_0}}}}}} \approx\\ & C_{{i_0}}^{{i_{{b_0}}}}{V^{{i_0}}} {\text{。}} \end{split}$ (10)

 $\begin{split} {g^{{i_0}}} = & C_{{e_0}}^{{i_0}}C_n^{{e_0}}{g^n} \hfill =\\ &\left[ {\begin{array}{*{20}{l}} { - g\cos L\sin {\omega _{ie}}\left( {t - {t_0}} \right)} \\ { - g\sin L} \\ { - g\cos L\cos {\omega _{ie}}\left( {t - {t_0}} \right)} \end{array}} \right]{\text{，}} \end{split}$ (11)

 {V^{{i_0}}}\left( {{t_k}} \right) = \left[ \begin{aligned} & \frac{{ - g\cos L\left( {\cos {\omega _{ie}}\Delta {t_k} - 1} \right)}}{{{\omega _{ie}}}} \\ & g\sin L\Delta {t_k} \\ & \frac{{g\cos L\sin {\omega _{ie}}\Delta {t_k}}}{{{\omega _{ie}}}} \\ \end{aligned} \right]{\text{，}} (12)

 $\left\{ {\begin{array}{*{20}{c}} {{{\tilde V}^{{i_{{b_0}}}}}\left( {{t_{k2}}} \right) = C_{{i_0}}^{{i_{{b_0}}}}{V^{{i_0}}}\left( {{t_{k2}}} \right)} \text{,}\\ {{{\tilde V}^{{i_{{b_0}}}}}\left( {{t_{k2}}} \right) = C_{{i_0}}^{{i_{{b_0}}}}{V^{{i_0}}}\left( {{t_{k2}}} \right)} \text{,} \end{array}} \right.$ (13)

 $\hat{\rm{ C}}_{{i_{{b_0}}}}^{{i_0}} = {\left[ {\begin{array}{*{20}{c}} {{V^{{i_0}{\rm{T}}}}\left( {{t_{k1}}} \right)} \\ {{V^{{i_0}{\rm{T}}}}\left( {{t_{k2}}} \right)} \\ {{{\left[ {{V^{{i_0}}}\left( {{t_{k1}}} \right) \!\!\!\times {V^{{i_0}}}\left( {{t_{k2}}} \right)} \right]}^{\rm{T}}}} \end{array}} \!\!\right]^{ - 1}}\!\!\left[ {\begin{array}{*{20}{c}} {{{\tilde V}^{{i_{{b_0}}}{\rm{T}}}}\left( {{t_{k1}}} \right)} \\ {{{\tilde V}^{{i_{{b_0}}}{\rm{T}}}}\left( {{t_{k2}}} \right)} \\ \!\!\!{{{\left[ {{{\tilde V}^{{i_{{b_0}}}}}\left( {{t_{k1}}} \right) \times {{\tilde V}^{{i_{{b_0}}}}}\left( {{t_{k2}}} \right)} \right]}^{\rm{T}}}} \end{array}} \right]\text{。}$ (14)

2 精对准方案 2.1 精对准方案的选择

2.2 捷联惯导系统误差模型的建立

1）捷联惯导系统误差方程

 ${\dot \varphi ^n} = - \omega _{in}^n \times {\varphi ^n} + \delta \omega _{in}^n - C_b^n{\varepsilon ^b}\text{，}$ (15)

$\omega _{in}^n = \omega _{ie}^n + \omega _{en}^n$ $\delta \omega _{in}^n\delta \omega _{ie}^n + \delta \omega _{en}^n$ $\omega _{ie}^n = {\left[ {\begin{array}{*{20}{c}} {{\omega _{ie}}\cos L}& {{\omega _{ie}}\sin L}&0 \end{array}} \right]^{\rm{T}}}$ $\omega _{en}^n = {\left[ {\begin{array}{*{20}{c}} {\displaystyle\frac{{{V_E}}}{{{R_N} + h}}}&{\displaystyle\frac{{{V_E}}}{{{R_N} + h}}\tan L}&{ -\displaystyle \frac{{{V_N}}}{{{R_M} + h}}} \end{array}} \right]^{\rm{T}}}$ ${R_N}$ ${R_M}$ 分别为子午面内曲率半径、卯酉圈内曲率半径、 ${\varepsilon ^b}$ 为载体系陀螺漂移。

 $\delta \dot V_e^n = {f^n} \times {\varphi ^n} - \left( {2\omega _{ie}^n + \omega _{en}^n} \right) \times \delta V_e^n + C_b^n{\nabla ^b}\text{.}$ (16)

2）捷联惯导系统状态空间模型

 $\dot X = Fx + Gwt\text{。}$ (17)

 $\begin{split} { X} = [{\varphi _N},{\varphi _U},{\varphi _E},\delta {V_N},\delta {V_U},\delta {V_E}, \\ {\varepsilon _x},{\varepsilon _y},{\varepsilon _z},{\nabla _x},{\nabla _y},{\nabla _z}{]^{\rm{T}}}{\text{，}} \end{split}$ (18)

 ${{w}} = {\left[ {{w_{{V_n}}},{w_{{V_u}}},{w_{{V_e}}},{w_{{\varphi _n}}},{w_{{\varphi _u}}},{w_{{\varphi _e}}},{0_{1 \times 6}}} \right]^{\rm{T}}}{\text{，}}$ (19)
 ${{F}} = \left[ {\begin{array}{*{20}{c}} { - \omega _{in}^n \times }&A&{ - C_b^n}&{{0_{3 \times 3}}}\\ {{f^n} \times }&B&{{0_{3 \times 3}}}&{C_b^n}\\ {{0_{3 \times 3}}}&{{0_{3 \times 3}}}&{{0_{3 \times 3}}}&{{0_{3 \times 3}}}\\ {{0_{3 \times 3}}}&{{0_{3 \times 3}}}&{{0_{3 \times 3}}}&{{0_{3 \times 3}}} \end{array}} \right]{\text{，}}$ (20)
 $A = \left[ {\begin{array}{*{20}{c}} 0&0&{\displaystyle\frac{1}{{{R_e}}}} \\ 0&0&{\displaystyle\frac{{\tan L}}{{{R_e}}}} \\ { - \displaystyle\frac{1}{{{R_n}}}}&0&0 \end{array}} \right]{\text{，}}$ (21)
 $B = \left[ {\begin{array}{*{20}{c}} { - \displaystyle\frac{{{V_u}}}{{{R_n}}}}&{ - \displaystyle\frac{{{V_n}}}{{{R_n}}}} \\ { - 2{\omega _{ie}}\sin L - \displaystyle\frac{{{V_e}}}{{{R_e}}}\tan L}&{} \\ {\displaystyle\frac{{2{V_n}}}{{{R_n}}}}&0 \\ {2{\omega _{ie}}\cos L + \displaystyle\frac{{{V_e}}}{{{R_e}}}}&{} \\ {2{\omega _{ie}}\sin L + \displaystyle\frac{{{V_e}}}{{{R_e}}}\tan L}&{ - 2{\omega _{ie}}\cos L - \displaystyle\frac{{{V_e}}}{{{R_e}}}} \\ {\displaystyle\frac{{{V_n}}}{{{R_e}}}\tan L - \displaystyle\frac{{{V_u}}}{{{R_e}}}}&{} \end{array}} \right]{\text{，}}$ (22)

 $Z = HX + V\text{。}$ (23)

3 试验验证与结果分析 3.1 试验条件

3.2 试验过程

 图 1 惯导系统晃动规律 Fig. 1 Inertial navigation system sloshing law
3.3 试验结果及分析

 图 2 静基座对准及纯惯性导航结果 Fig. 2 Static base alignment and pure inertial navigation results

 图 3 静基座对准及纯惯性导航结果 Fig. 3 Wobble base alignment and pure inertial navigation results

4 结　语

 [1] 付强文, 秦永元, 李四海, 等. 车辆运动学约束辅助的惯性导航算法[J]. 中国惯性技术学报, 2012, 20(6): 640-643. DOI:10.3969/j.issn.1005-6734.2012.06.005 [2] ALAM, KEALY, DEMPSTER. An INS-aided tight integration approach for relative positioning enhancement in VANETs[J]. IEEE Transactions on Intelligent Transportation Systems, 2013, 14(4): 1992-1996. DOI:10.1109/TITS.2013.2265235 [3] DISSANAYAKE G, SUKKARIEH S, HUGH D W. The aiding of a low-cost strapdown inertial measurement unit using vehicle model constraints for land vehicle application[J]. IEEE Transactions on Robotics and Automation, 2011, 17(5): 731-747. [4] PETER J Hr, ELVEZIO M. Ronchetti. Robust Statistics (Second Edition)[M]. USA: Wiley, 2009.