CN103090866B

CN103090866B - Method for restraining speed errors of single-shaft rotation optical fiber gyro strapdown inertial navigation system

Info

Publication number: CN103090866B
Application number: CN201310005528.3A
Authority: CN
Inventors: 王秋滢; 齐昭; 高峰; 高伟; 孙枫
Original assignee: Harbin Engineering University
Current assignee: Harbin Engineering University
Priority date: 2012-11-02
Filing date: 2013-01-08
Publication date: 2015-05-27
Anticipated expiration: 2033-01-08
Also published as: CN103090866A

Abstract

The invention discloses a method for restraining speed errors of a single-shaft rotation optical fiber gyro strapdown inertial navigation system. The method comprises the following steps of: 1. acquiring carrier position information through a global positioning system (GPS); 2. acquiring data output by an optical fiber gyro and a quartz accelerometer; 3. driving an inertial component to perform single-shaft forward and backward rotation and stop motion by a rotary mechanism, and adopting a rotation scheme that eight rotation and stop sequences are used as a rotation period; 4. acquiring information of linear velocity and angular velocity of the motion of measurement carriers of the optical fiber gyro and the quartz accelerometer, and performing navigation solution to obtain navigation information; and 5. constructing a Butterworth band elimination filter, and performing Butterworth filter processing on the carrier velocity obtained under a navigation system. According to the method, the Butterworth band elimination filter is designed according to the rotation angular velocity, oscillation error items related to the rotation angular velocity under the navigation system are filtered and removed, and the velocity information accuracy is improved.

Description

A speed error suppression method for single-axis rotating fiber optic gyro strapdown inertial navigation system

技术领域technical field

本发明涉及一种基于Butterworth（巴特沃兹）数字滤波的单轴旋转光纤陀螺捷联惯导系统速度误差抑制方法，属于惯性技术领域中减小导航信息误差的抑制方法。The invention relates to a speed error suppression method for a single-axis rotating optical fiber gyro strapdown inertial navigation system based on Butterworth (Butterworth) digital filtering, and belongs to the suppression method for reducing navigation information errors in the inertial technical field.

背景技术Background technique

捷联惯导系统（SINS）是将惯性组件（陀螺仪和加速度计）直接安装在载体上的一种全自主导航系统。它不需要任何外界信息，利用采集惯性组件测量载体运动的线运动和角运动信息，经导航解算连续输出载体的速度、位置和姿态信息。由于SINS具有体积小、重量轻、易于维护、可靠性高等优点被广泛用于航空、航天、航海等领域。但是，由于惯性组件常值偏差的存在，导致系统定位误差随时间发散而不断增大是制约SINS长时间导航的重要因素之一。A strapdown inertial navigation system (SINS) is a fully autonomous navigation system with inertial components (gyroscopes and accelerometers) mounted directly on the carrier. It does not require any external information, uses the collection of inertial components to measure the linear motion and angular motion information of the carrier movement, and continuously outputs the speed, position and attitude information of the carrier through navigation calculations. Due to the advantages of small size, light weight, easy maintenance and high reliability, SINS is widely used in aviation, aerospace, navigation and other fields. However, due to the existence of the constant value deviation of the inertial components, the system positioning error diverges and increases with time, which is one of the important factors restricting the long-term navigation of SINS.

为了提高系统定位精度，一方面可以提高惯性元件精度，但是由于受加工技术水平的限制，无限制的提高元件精度是很难实现的；另一方面就是采取捷联惯性导航系统的误差抑制技术，自动抵消惯性器件的误差对系统精度的影响。这样就可以应用现有精度的惯性元件构成较高精度的捷联惯性导航系统。In order to improve the positioning accuracy of the system, on the one hand, the accuracy of the inertial components can be improved, but due to the limitation of the processing technology level, it is difficult to achieve unlimited improvement of the component accuracy; on the other hand, the error suppression technology of the strapdown inertial navigation system is adopted. Automatically offset the influence of the error of the inertial device on the system accuracy. In this way, the existing high-precision inertial components can be used to form a higher-precision strapdown inertial navigation system.

旋转调制技术是一种惯性器件偏差自补偿方法，该方法通过旋转机构带动惯性组件有规律的转动，对惯性器件常值偏差的调制来抵消该误差项对系统的影响，进而提高系统定位精度。虽然旋转调制能够有效地抑制定位误差发散，但同时又给速度等导航信息带来了新误差，制约了速度信息的可用性。对于工作在单轴正反转停旋转方案的捷联惯导系统：转动过程中，系统解算速度误差中，出现了与旋转转速有关的新振荡误差；停位过程中，一次旋转运动变换IMU停位位置后，器件常值偏差在导航系投影有所变化，从而使得系统中引入了新的扰动，导致速度误差再一次激励舒勒、地球周期振荡。也就是说，每变换一个停位位置，在停位时间段内，就会重新激励振荡误差。Rotation modulation technology is a self-compensation method for inertial device deviation. This method drives the inertial component to rotate regularly through the rotating mechanism, and modulates the constant value deviation of the inertial device to offset the influence of the error item on the system, thereby improving the positioning accuracy of the system. Although rotation modulation can effectively suppress the divergence of positioning errors, it also brings new errors to navigation information such as speed, which restricts the availability of speed information. For the strapdown inertial navigation system working in the single-axis forward and reverse stop rotation scheme: during the rotation process, the system solves the speed error, and a new oscillation error related to the rotation speed appears; during the stop process, a rotation motion transforms the IMU After the parking position, the constant value deviation of the device changes in the projection of the navigation system, which introduces a new disturbance into the system, and causes the speed error to excite Schuler and the Earth's periodic oscillation again. That is to say, every time a stop position is changed, the oscillation error will be re-energized during the stop time period.

在CNKI库中公开报道有：1.《系统级双轴旋转调制捷联惯导误差分析及标校》，该文章主要提出了一种系统级双轴旋转调制式捷联惯导工程实现方案，找出了影响系统长航时导航精度的误差源。2.《旋转激光陀螺惯导系统误差传播特性分析》，该文章主要对单轴旋转式激光陀螺惯导系统的误差传播特性进行了深入研究，针对单轴正反转停旋转方案，对各误差项调制效果进行了分析。3.《旋转惯导系统中转轴方向对系统调制精度的影响》，本文主要分析了单轴旋转惯导系统中转台转轴方向对系统精度的影响。以上文献都在于抑制发散式定位误差，并没有提及旋转调制对系统解算速度信息精度及适用性的影响。The public reports in the CNKI library include: 1. "System-level dual-axis rotation modulation strapdown inertial navigation error analysis and calibration", this article mainly proposes a system-level dual-axis rotation modulation strapdown inertial navigation project implementation plan, The error sources that affect the system's long-endurance navigation accuracy are found out. 2. "Analysis of Error Propagation Characteristics of Rotary Laser Gyro Inertial Navigation System", this article mainly conducts in-depth research on the error propagation characteristics of single-axis rotary laser gyro inertial navigation system. Modulation effects of the term were analyzed. 3. "Influence of Rotary Axis Direction in Rotary Inertial Navigation System on System Modulation Accuracy", this paper mainly analyzes the influence of the rotation axis direction of turntable in single-axis rotary inertial navigation system on system accuracy. The above literatures all focus on suppressing divergent positioning errors, and do not mention the influence of rotation modulation on the accuracy and applicability of the system to solve the speed information.

发明内容Contents of the invention

本发明的目的是为了解决上述问题，提出一种单轴旋转光纤陀螺捷联惯导系统速度误差抑制方法，基于Butterworth数字滤波器，将旋转调制惯导系统解算速度信息作为输入，通过以旋转角速度为依据而设计的Butterworth数字滤波器滤除速度误差中导航系下与旋转角速度有关的振荡误差项，提高速度精度，增强系统解算速度信息的适用性。The purpose of the present invention is to solve the above problems, and propose a speed error suppression method for a single-axis rotating fiber optic gyro strapdown inertial navigation system. The Butterworth digital filter designed based on the angular velocity filters out the oscillation error item related to the rotational angular velocity in the navigation system in the velocity error, improves the velocity accuracy, and enhances the applicability of the system to solve the velocity information.

一种单轴旋转光纤陀螺捷联惯导系统速度误差抑制方法，包括以下步骤：A speed error suppression method for a single-axis rotating fiber optic gyro strapdown inertial navigation system, comprising the following steps:

步骤一：通过全球定位GPS系统采集载体位置信息，并装订至导航计算机中；Step 1: collect carrier position information through the global positioning GPS system, and bind it to the navigation computer;

步骤二：将旋转机构转动至IMU系与载体系重合的位置，有其中b表示载体坐标系，s表示IMU坐标系，表示s系到b系转换矩阵，I表示单位阵；将光纤陀螺捷联惯导系统进行充分预热后，采集光纤陀螺仪和石英加速度计输出的数据；得到载体的角运动信息和线运动信息；角运动信息包括角速度值，线运动信息包括比力值；Step 2: Turn the rotating mechanism to the position where the IMU system and the carrier system coincide, and there is Where b represents the carrier coordinate system, s represents the IMU coordinate system, Indicates the conversion matrix from the s system to the b system, and I represents the unit matrix; after the fiber optic gyro strapdown inertial navigation system is fully preheated, the data output by the fiber optic gyroscope and the quartz accelerometer are collected; the angular motion information and linear motion information of the carrier are obtained ; Angular motion information includes angular velocity values, and linear motion information includes specific force values;

步骤三：旋转机构带动惯性组件以ω进行单轴正反转停运动；采用八个转停次序为一个旋转周期的旋转方案；Step 3: The rotation mechanism drives the inertial component to perform single-axis forward and reverse stop motion with ω; adopt a rotation scheme in which eight rotation and stop sequences are one rotation cycle;

步骤四：实时采集光纤陀螺仪和石英加速度计测量载体运动的线速度和角速度信息，导航解算得到导航信息；Step 4: Collect the linear velocity and angular velocity information of the carrier motion measured by the fiber optic gyroscope and quartz accelerometer in real time, and obtain the navigation information through navigation calculation;

步骤五：构造Butterworth带阻滤波器，将导航系下得到的载体速度进行Butterworth滤波器处理，滤除导航系下与旋转角速度有关的振荡误差项，滤波后的速度作为最终导航解算输出信息。Step 5: Construct a Butterworth band-stop filter, process the carrier velocity obtained in the navigation system with a Butterworth filter, filter out the oscillation error term related to the rotational angular velocity in the navigation system, and use the filtered velocity as the final navigation solution output information.

本发明的优点在于：The advantages of the present invention are:

本发明针对调制型捷联惯导系统解算导航信息中速度误差形式，在得出调制过程中速度误差的具体形式后，通过旋转角速度设计了Butterworth带阻滤波器，将导航系下解算出的载体速度进行带阻滤波器处理，滤除导航系下与旋转角速度有关的振荡误差项，滤波后的速度作为最终导航解算输出信息，提高了速度信息精度，增强了该信息的适用性。The present invention aims at the modulation type strapdown inertial navigation system to solve the speed error form in the navigation information. After obtaining the specific form of the speed error in the modulation process, a Butterworth band-stop filter is designed through the rotation angular velocity, and the navigation system is solved to calculate the speed error. The velocity of the carrier is processed by a band-stop filter to filter out the oscillation error term related to the rotational angular velocity in the navigation system. The filtered velocity is used as the output information of the final navigation solution, which improves the accuracy of the velocity information and enhances the applicability of the information.

附图说明Description of drawings

图1是本发明的方法流程图；Fig. 1 is method flowchart of the present invention;

图2为本发明的步骤三中IMU四位置转停示意图；Fig. 2 is a schematic diagram of IMU four-position rotation and stop in step 3 of the present invention;

图2a为①～④旋转过程；图2b为⑤～⑧旋转过程；Figure 2a is the rotation process of ①～④; Figure 2b is the rotation process of ⑤～⑧;

图3为本发明步骤三构造的Butterworth滤波器幅频响应曲线；Fig. 3 is the Butterworth filter amplitude-frequency response curve that step 3 of the present invention constructs;

图4为本发明实施例1中利用Visual C++仿真得到滤波前后速度误差比较曲线；Fig. 4 utilizes Visual C++ simulation to obtain the speed error comparison curve before and after filtering in the embodiment of the present invention 1;

图5为本发明实施例2中利用单轴转台试验得到滤波前后速度误差比较曲线。Fig. 5 is a comparison curve of speed error before and after filtering obtained by using a single-axis turntable test in Example 2 of the present invention.

具体实施方式Detailed ways

下面将结合附图和实施例对本发明作进一步的详细说明。The present invention will be further described in detail with reference to the accompanying drawings and embodiments.

本发明是一种单轴旋转光纤陀螺捷联惯导系统速度误差抑制方法，方法流程如图1所示，包括以下步骤：The present invention is a speed error suppression method of a single-axis rotating fiber optic gyro strapdown inertial navigation system. The method flow is shown in Figure 1, including the following steps:

导航初始时刻，通过全球定位GPS系统采集初始时刻载体位置信息、速度信息，并装订至导航计算机中。载体位置信息包括载体所在位置的经度、纬度信息。At the initial moment of navigation, the carrier position information and speed information at the initial moment are collected through the global positioning GPS system, and bound into the navigation computer. The carrier location information includes longitude and latitude information of the location of the carrier.

导航过程中，利用该初始信息进行更新，得到任意时刻载体的速度、位置。During the navigation process, the initial information is used for updating to obtain the speed and position of the carrier at any time.

步骤二：将旋转机构转动至IMU系与载体系重合的位置，有其中b表示载体坐标系，s表示IMU坐标系，表示s系到b系转换矩阵，I表示单位阵。将光纤陀螺捷联惯导系统进行充分预热后，采集光纤陀螺仪和石英加速度计输出的数据，Step 2: Turn the rotating mechanism to the position where the IMU system and the carrier system coincide, and there is Where b represents the carrier coordinate system, s represents the IMU coordinate system, Indicates the s-system to b-system transformation matrix, and I represents the identity matrix. After fully warming up the fiber optic gyroscope strapdown inertial navigation system, collect the data output by the fiber optic gyroscope and quartz accelerometer,

得到载体的角运动信息和线运动信息。角运动信息包括角速度值，线运动信息包括比力值。Obtain the angular motion information and linear motion information of the carrier. Angular motion information includes angular velocity values, and linear motion information includes specific force values.

根据加速度计输出的比力值与重力加速度关系、以及陀螺仪输出的角速度值与地球自转角速度关系，确定载体姿态角，完成系统初始对准，建立惯导系统初始捷联矩阵 According to the relationship between the specific force value output by the accelerometer and the acceleration of gravity, and the relationship between the angular velocity value output by the gyroscope and the angular velocity of the earth, determine the attitude angle of the carrier, complete the initial alignment of the system, and establish the initial strapdown matrix of the inertial navigation system

${C C}_{s the s}^{n no} = = {C C}_{b b}^{n no} {C C}_{s the s}^{b b} = = [\begin{matrix} cos cos {φ φ}_{y the y 00} cos cos {φ φ}_{z z 00} - - sin sin {φ φ}_{x x 00} sin sin {φ φ}_{y the y 00} sin sin {φ φ}_{z z 00} & - - cos cos {φ φ}_{x x 00} sin sin {φ φ}_{z z 00} & sin sin {φ φ}_{y the y 00} cos cos {φ φ}_{z z 00} + + sin sin {φ φ}_{z z 00} sin sin {φ φ}_{x x 00} cos cos {φ φ}_{y the y 00} \\ sin sin {φ φ}_{z z 00} cos cos {φ φ}_{y the y 00} + + sin sin {φ φ}_{x x 00} sin sin {φ φ}_{y the y 00} cos cos {φ φ}_{z z 00} & cos cos {φ φ}_{x x 00} cos cos {φ φ}_{z z 00} & sin sin {φ φ}_{z z 00} sin sin {φ φ}_{y the y 00} + + cos cos {φ φ}_{z z 00} sin sin {φ φ}_{x x 00} cos cos {φ φ}_{y the y 00} \\ - - cos cos {φ φ}_{x x 00} sin sin {φ φ}_{y the y 00} & sin sin {φ φ}_{x x 00} & cos cos {φ φ}_{x x 00} cos cos {φ φ}_{y the y 00} \end{matrix}] - - - - - - ((11))$

其中，φ_x0、φ_y0、φ_z0分别表示初始时刻载体俯仰角、横滚角、航向角。Among them, φ _x0 , φ _y0 , and φ _z0 represent the pitch angle, roll angle, and heading angle of the carrier at the initial moment, respectively.

步骤三：旋转机构带动惯性组件以ω进行单轴正反转停运动。其中，可以取ω＝6°/s。采用八个转停次序为一个旋转周期的旋转方案；Step 3: The rotating mechanism drives the inertial component to perform single-axis forward and reverse stop motion with ω. Among them, ω=6°/s can be taken. A rotation scheme with eight rotation-stop sequences as one rotation cycle;

所述惯性组件（Inertial Measurement Unit，简称IMU）转动过程采用八个转停次序为一个旋转周期的转位方案，具体为：The rotation process of the inertial component (Inertial Measurement Unit, referred to as IMU) adopts an indexing scheme in which eight rotation and stop sequences are one rotation cycle, specifically:

如图2所示，其中（a）为次序1至次序4的转位示意图，图中，①～④表示前4个旋转过程，A、B、C、D表示四个停留位置，x_b、y_b表示载体坐标系的水平轴，并要求旋转初始时刻IMU与载体坐标系完全重合。次序1，IMU从A点出发顺时针转动180°到达位置C，停止时间T_r；次序2，IMU从C点出发逆时针转动90°到达位置B，停止时间T_r；次序3，IMU从B点出发顺时针转动180°到达位置D，停止时间T_r；次序4，IMU从D点出发逆时针转动270°，到达位置A，停止时间T_r；如图2所示，其中（b）为次序5至次序8的转位示意图，图中，⑤～⑧表示后4个旋转过程，A、B、C、D表示四个停留位置，x_b、y_b表示载体坐标系的水平轴。次序5，IMU从A点出发逆时针转动180°到达位置C，停止时间T_r；次序6，IMU从C点出发顺时针转动90°到达位置D，停止时间T_r；次序7，IMU从D点出发逆时针转动180°到达位置B，停止时间T_r；次序8，IMU从B点出发顺时针转动270°，到达位置A，停止时间T_r；IMU按照此转动顺序循环进行。As shown in Fig. 2, (a) is the transposition diagram of sequence 1 to sequence 4. In the figure, ①～④ represent the first 4 rotation processes, A, B, C, D represent the four stop positions, x _b , y _b represents the horizontal axis of the carrier coordinate system, and requires the IMU to completely coincide with the carrier coordinate system at the initial moment of rotation. Sequence 1, IMU starts from point A and rotates 180° clockwise to position C, stop time T _r ; Sequence 2, IMU starts from point C to rotate 90° counterclockwise to position B, stop time T _r ; Sequence 3, IMU starts from B Starting from point D, turn clockwise 180° to reach position D, stop time T _r ; sequence 4, IMU rotates 270° counterclockwise from point D, arrive at position A, stop time T _r ; as shown in Figure 2, where (b) is Schematic diagram of transposition from sequence 5 to sequence 8. In the figure, ⑤～⑧ represent the last 4 rotation processes, A, B, C, D represent the four stop positions, x _b , y _b represent the horizontal axis of the carrier coordinate system. Sequence 5, IMU starts from point A and rotates 180° counterclockwise to position C, stop time T _r ; Sequence 6, IMU starts from point C to rotate 90° clockwise to position D, stop time T _r ; Start from point B and turn 180° counterclockwise to reach position B, stop time T _r ; Sequence 8, IMU rotate clockwise 270° from point B to reach position A, stop time T _r ; IMU follows this rotation sequence cycle.

其中，可以取时间T_r＝300s。Wherein, the time T _r =300s can be taken.

通过旋转调制状态下采集的陀螺仪数据，更新捷联矩阵具体为：Update the strapdown matrix with the gyroscope data collected in the rotational modulation state Specifically:

角速度更新：Angular velocity update:

${ω ω}_{ns ns}^{s the s} = = {ω ω}_{is is}^{s the s} - - {(({C C}_{s the s}^{n no}))}^{T T} (({ω ω}_{ie ie}^{s the s} + + {ω ω}_{en en}^{s the s})) - - - - - - ((22))$

其中，i表示地心惯性系，e表示地球坐标系，s表示惯性组件坐标系，n表示导航坐标系，这里采用当地地理坐标系；表示s系到n系转换矩阵；表示p系相对m系旋转角速度在q系投影。Among them, i represents the geocentric inertial system, e represents the earth coordinate system, s represents the inertial component coordinate system, n represents the navigation coordinate system, and the local geographic coordinate system is used here; Indicates the s-system to n-system conversion matrix; Indicates that the rotation angular velocity of the p system relative to the m system is projected on the q system.

四元数姿态矩阵更新：Quaternion pose matrix update:

设任意时刻载体坐标系相对平台坐标系的转动四元数为：Let the rotation quaternion of the carrier coordinate system relative to the platform coordinate system at any time be:

Q＝q₀+q₁i_b+q₂j_b+q₃k_b （3）Q＝q ₀ +q ₁ i _b +q ₂ j _b +q ₃ k _b (3)

其中，Q为四元数；q₀、q₁、q₂、q₃为四元数的四个实数；i_b、j_b、k_b分别表示IMU坐标系ox_s轴、oy_s轴、oz_s轴上的单位方向向量。Among them, Q is a quaternion; q ₀ , q ₁ , q ₂ , and q ₃ are four real numbers of a quaternion; ib , j _b , and _{k b} _respectively represent the ox _s axis, oy _s axis, and oz of the IMU coordinate system Unit direction vector on the _s -axis.

四元数Q的及时修正：Timely correction of quaternion Q:

$[\begin{matrix} {\overset{\cdot \cdot}{q q}}_{11} \\ {\overset{\cdot &Center Dot;}{q q}}_{11} \\ {\overset{\cdot \cdot}{q q}}_{22} \\ {\overset{\cdot \cdot}{q q}}_{33} \end{matrix}] = = \frac{11}{22} [\begin{matrix} 00 & - - {ω ω}_{nsx nsx}^{s the s} & - - {ω ω}_{nsy nsy}^{s the s} & - - {ω ω}_{nsz nsz}^{s the s} \\ {ω ω}_{nsx nsx}^{s the s} & 00 & {ω ω}_{nsz nsz}^{s the s} & - - {ω ω}_{nsx nsx}^{s the s} \\ {ω ω}_{nsy nsy}^{s the s} & - - {ω ω}_{nsz nsz}^{s the s} & 00 & {ω ω}_{nsx nsx}^{s the s} \\ {ω ω}_{nsz nsz}^{s the s} & {ω ω}_{nsy nsy}^{s the s} & - - {ω ω}_{nsx nsx}^{s the s} & 00 \end{matrix}] [\begin{matrix} {q q}_{00} \\ {q q}_{11} \\ {q q}_{22} \\ {q q}_{33} \end{matrix}] - - - - - - ((44))$

其中，分别表示旋转机构相对导航系的运动角速度在IMU坐标系ox_s轴、oy_s轴、oz_s轴上的分量。分别表示q₀、q₁、q₂、q₃的变化率；in, Respectively represent the components of the angular velocity of the rotating mechanism relative to the navigation system on the ox _s axis, oy _s axis, and oz _s axis of the IMU coordinate system. respectively represent the rate of change of q ₀ , q ₁ , q ₂ , and q ₃ ;

根据k时刻载体坐标系相对平台坐标系的转动四元数q₀(k)、q₁(k)、q₂(k)、q₃(k)，求取k时刻转动四元数的变化率为：According to the rotation quaternion q ₀ (k), q ₁ (k), q ₂ (k), q ₃ (k) of the carrier coordinate system relative to the platform coordinate system at k time, calculate the change rate of the rotation quaternion at k time for:

$[\begin{matrix} {\overset{\cdot &Center Dot;}{q q}}_{11} ((k k)) \\ {\overset{\cdot \cdot}{q q}}_{11} ((k k)) \\ {\overset{\cdot &Center Dot;}{q q}}_{22} ((k k)) \\ {\overset{\cdot \cdot}{q q}}_{33} ((k k)) \end{matrix}] = = \frac{11}{22} [\begin{matrix} 00 & - - {ω ω}_{nsx nsx}^{s the s} & - - {ω ω}_{nsy nsy}^{s the s} & - - {ω ω}_{nsz nsz}^{s the s} \\ {ω ω}_{nsx nsx}^{s the s} & 00 & {ω ω}_{nsz nsz}^{s the s} & - - {ω ω}_{nsx nsx}^{s the s} \\ {ω ω}_{nsy nsy}^{s the s} & - - {ω ω}_{nsz nsz}^{s the s} & 00 & {ω ω}_{nsx nsx}^{s the s} \\ {ω ω}_{nsz nsz}^{s the s} & {ω ω}_{nsy nsy}^{s the s} & - - {ω ω}_{nsx nsx}^{s the s} & 00 \end{matrix}] [\begin{matrix} {q q}_{00} ((k k)) \\ {q q}_{11} ((k k)) \\ {q q}_{22} ((k k)) \\ {q q}_{33} ((k k)) \end{matrix}] - - - - - - ((55))$

在k+1时刻载体的转动四元数具体为：The rotation quaternion of the carrier at time k+1 is specifically:

$\{\begin{matrix} {q q}_{00} ((k k + + 11)) = = {q q}_{00} ((k k)) + + {\overset{\cdot &Center Dot;}{q q}}_{00} ((k k)) \\ {q q}_{11} ((k k + + 11)) = = {q q}_{11} ((k k)) + + {\overset{\cdot \cdot}{q q}}_{11} ((k k)) \\ {q q}_{22} ((k k + + 11)) = = {q q}_{22} ((k k)) + + {\overset{\cdot \cdot}{q q}}_{22} ((k k)) \\ {q q}_{33} ((k k + + 11)) = = {q q}_{33} ((k k)) + + {\overset{\cdot \cdot}{q q}}_{33} ((k k)) \end{matrix} - - - - - - ((66))$

利用得到的q₀(k+1)、q₁(k+1)、q₂(k+1)、q₃(k+1)，更新捷联矩阵 Using the obtained q ₀ (k+1), q ₁ (k+1), q ₂ (k+1), q ₃ (k+1), update the strapdown matrix

${C C}_{s the s}^{n no} = = [\begin{matrix} {q q}_{00}^{22} + + {q q}_{11}^{22} - - {q q}_{22}^{22} - - {q q}_{33}^{22} & 22 (({q q}_{11} {q q}_{22} - - {q q}_{00} {q q}_{33})) & 22 (({q q}_{11} {q q}_{33} + + {q q}_{00} {q q}_{22})) \\ 22 (({q q}_{11} {q q}_{22} + + {q q}_{00} {q q}_{33})) & {q q}_{00}^{22} - - {q q}_{11}^{22} + + {q q}_{22}^{22} - - {q q}_{33}^{22} & 22 (({q q}_{22} {q q}_{33} - - {q q}_{00} {q q}_{11})) \\ 22 (({q q}_{11} {q q}_{33} - - {q q}_{00} {q q}_{22})) & 22 (({q q}_{22} {q q}_{33} + + {q q}_{00} {q q}_{11})) & {q q}_{00}^{22} - - {q q}_{11}^{22} - - {q q}_{22}^{22} + + {q q}_{33}^{22} \end{matrix}] - - - - - - ((77))$

其中，（7）式中的q_i(i＝1,2,3,4)为（6）式中q_i(k+1)(i＝1,2,3,4)，（7）式中省略了(k+1)。Among them, q _i (i=1,2,3,4) in (7) formula is q _i (k+1) (i=1,2,3,4) in (6) formula, and (7) formula (k+1) is omitted in .

更新载体姿态信息，具体为：Update the carrier attitude information, specifically:

$\{\begin{matrix} {φ φ}_{x x} = = arcsin arcsin (({c c}_{3333})) \\ {φ φ}_{y the y} = = arctan arctan (({c c}_{3232} / / {c c}_{3131})) \\ {φ φ}_{z z} = = arctan arctan (({c c}_{1313} / / {c c}_{23 twenty three})) \end{matrix} - - - - - - ((88))$

其中，c_ij（i＝1,2,3，j＝1,2,3）表示中第i行第j列矩阵元素；φ_x、φ_y、φ_z表示载体纵摇角、横摇角、航向角。Among them, c _ij (i=1,2,3, j=1,2,3) means In row i and column j of matrix elements; φ _x , φ _y , φ _z represent carrier pitch angle, roll angle, and heading angle.

将加速度计沿IMU坐标系测量的比力信息，通过捷联矩阵进行投影转换：The specific force information measured by the accelerometer along the IMU coordinate system is passed through the strapdown matrix Do a projective transformation:

${f f}^{n no} = = {C C}_{s the s}^{n no} {f f}^{s the s} - - - - - - ((99))$

其中，fⁿ、f^s分别表示加速度计测量比力在n系和s系投影；Among them, f ⁿ and f ^s represent the projection of the specific force measured by the accelerometer on the n system and the s system respectively;

利用下列微分方程求解载体运动速度：The velocity of the carrier motion is solved using the following differential equation:

$[\begin{matrix} {\overset{\cdot &Center Dot;}{v v}}_{x x} \\ {\overset{\cdot &Center Dot;}{v v}}_{y the y} \\ {\overset{\cdot &Center Dot;}{v v}}_{z z} \end{matrix}] = = [\begin{matrix} {f f}_{x x}^{n no} \\ {f f}_{y the y}^{n no} \\ {f f}_{z z}^{n no} \end{matrix}] - - [\begin{matrix} 00 \\ 00 \\ g g \end{matrix}] + + [\begin{matrix} 00 & {22 ω ω}_{iez iez}^{n no} & - - (({22 ω ω}_{iey iey}^{n no} + + {ω ω}_{eny enny}^{n no})) \\ - - {ω ω}_{iez iez}^{n no} & 00 & {22 ω ω}_{iex iex}^{n no} + + {ω ω}_{enx enx}^{n no} \\ {22 ω ω}_{iey iey}^{n no} + + {ω ω}_{eny enny}^{n no} & - - (({22 ω ω}_{iex iex}^{n no} + + {ω ω}_{enx enx}^{n no})) & 00 \end{matrix}] [\begin{matrix} {v v}_{x x} \\ {v v}_{y the y} \\ {v v}_{z z} \end{matrix}] - - - - - - ((1010))$

其中，v_x、v_y、v_z分别表示解算载体速度在导航系ox_n轴、oy_n轴、oz_n轴上的分量；表示v_x、v_y、v_z的变化率，即载体沿导航系ox_n轴、oy_n轴、oz_n轴的运动加速度；分别表示加速度计测量比力在导航系ox_n轴、oy_n轴、oz_n轴上的分量；g为重力加速度。分别表示地球自转角速度在导航系ox_n轴、oy_n轴、oz_n轴上的分量；分别表示由于载体运动而导致导航系相对地球系变化的旋转角速度在导航系ox_n轴、oy_n轴、oz_n轴上的分量。Among them, v _x , v _y , and v _z represent the components of the calculated carrier velocity on the ox _n- axis, oy _n- axis, and oz _n- axis of the navigation system, respectively; Indicates the rate of change of v _x , v _y , and v _{z ,} that is, the motion acceleration of the carrier along the ox _n axis, oy _n axis, and oz _n axis of the navigation system; respectively represent the components of the specific force measured by the accelerometer on the ox _n axis, oy _n axis, and oz _n axis of the navigation system; g is the gravitational acceleration. represent the components of the earth's rotation angular velocity on the ox _n- axis, oy _n- axis, and oz _n- axis of the navigation system, respectively; Respectively represent the components of the rotation angular velocity of the navigation system relative to the earth system due to the movement of the carrier on the ox _n axis, oy _n axis, and oz _n axis of the navigation system.

根据k时刻的载体东向水平速度v_x(k)、北向水平速度v_y(k)和天向速度v_z(k)，求取k时刻载体速度变化率为：According to the carrier's eastward horizontal velocity v _x (k), northward horizontal velocity v _y (k) and skyward velocity v _z (k) at time k, the carrier velocity change rate at time k is calculated as:

$[\begin{matrix} {\overset{\cdot &Center Dot;}{v v}}_{x x} ((k k)) \\ {\overset{\cdot &Center Dot;}{v v}}_{y the y} ((k k)) \\ {\overset{\cdot \cdot}{v v}}_{z z} ((k k)) \end{matrix}] = = [\begin{matrix} {f f}_{x x}^{n no} \\ {f f}_{y the y}^{n no} \\ {f f}_{z z}^{n no} \end{matrix}] - - [\begin{matrix} 00 \\ 00 \\ g g \end{matrix}] + + [\begin{matrix} 00 & {22 ω ω}_{iez iez}^{n no} & - - (({22 ω ω}_{iey iey}^{n no} + + {ω ω}_{eny enny}^{n no})) \\ - - {ω ω}_{iez iez}^{n no} & 00 & {22 ω ω}_{iex iex}^{n no} + + {ω ω}_{enx enx}^{n no} \\ {22 ω ω}_{iey iey}^{n no} + + {ω ω}_{eny enny}^{n no} & - - (({22 ω ω}_{iex iex}^{n no} + + {ω ω}_{enx enx}^{n no})) & 00 \end{matrix}] [\begin{matrix} {v v}_{x x} ((k k)) \\ {v v}_{y the y} ((k k)) \\ {v v}_{z z} ((k k)) \end{matrix}] - - - - - - ((1111))$

在k+1时刻载体速度和位置分别为：The speed and position of the carrier at time k+1 are:

$\{\begin{matrix} {v v}_{x x} ((k k + + 11)) = = {v v}_{x x} ((k k)) + + {\overset{\cdot &Center Dot;}{v v}}_{x x} ((k k)) \\ {v v}_{y the y} ((k k + + 11)) = = {v v}_{y the y} ((k k)) + + {\overset{\cdot \cdot}{v v}}_{y the y} ((k k)) \\ {v v}_{z z} ((k k + + 11)) = = {v v}_{z z} ((k k)) + + {\overset{\cdot \cdot}{v v}}_{z z} ((k k)) \end{matrix} - - - - - - ((1212))$

其中，R表示地球半径；λ分别表示计算载体所在地理位置的纬度和经度信息，当k＝1时，v_x(1)、v_y(1)、v_z(1)为步骤一种利用GPS获得的载体初始速度，λ(1)为步骤一种利用GPS获得的载体初始位置。Among them, R represents the radius of the earth; λ respectively represents the latitude and longitude information of the geographical location of the calculation carrier, when k=1, v _x (1), v _y (1), v _z (1) is the initial velocity of the carrier obtained by using GPS in the first step, λ(1) is the initial position of the carrier obtained by using GPS in step one.

至此，根据（8）、（12）、（13）式得到载体的速度、位置、姿态。然后将姿态、位置直接输出至导航计算机显示器，提供载体各项导航信息；速度待进行步骤五中的进一步滤波处理优化，以减小其误差。So far, according to (8), (12), (13) the speed, position and attitude of the carrier are obtained. Then the attitude and position are directly output to the display of the navigation computer to provide various navigation information of the carrier; the speed is to be further optimized by filtering in step 5 to reduce its error.

（1）构造Butterworth带阻滤波器，具体为：(1) Construct a Butterworth band-stop filter, specifically:

步骤三中旋转机构采用旋转角速度为ω，此角速度引起导航信息的振荡误差频率约为f＝1/ω。设定Butterworth带阻滤波器的通带下限截止频率为(0.59～0.61)f，通带上限截止频率为(1.67～1.69)f，阻带下限截止频率为(0.83～0.85)f，阻带上限截止频率为(1.13～1.15)f，则有：In Step 3, the rotational angular velocity of the rotary mechanism is ω, and this angular velocity causes the oscillation error frequency of the navigation information to be about f=1/ω. Set the lower cutoff frequency of the passband of the Butterworth bandstop filter to (0.59～0.61)f, the upper limit cutoff frequency of the passband to (1.67～1.69)f, the lower limit cutoff frequency of the stopband to (0.83～0.85)f, the upper limit of the stopband The cutoff frequency is (1.13～1.15)f, then:

$\{\begin{matrix} {Ω Ω}_{11} = = ((0.59 0.59 ~ ~ 0.61 0.61)) f f \\ {Ω Ω}_{33} = = ((1.67 1.67 ~ ~ 1.69 1.69)) f f \\ {Ω Ω}_{sl sl} = = ((0.83 0.83 ~ ~ 0.85 0.85)) f f \\ {Ω Ω}_{sh sh} = = ((1.13 1.13 ~ ~ 1.15 1.15)) f f \end{matrix} - - - - - - ((1414))$

其中，Ω₁表示通带下限截止频率；Ω₃表示通带上限截止频率；Ω_sl表示阻带下限截止频率；Ω_sh表示阻带上限截止频率。Among them, Ω ₁ represents the cut-off frequency of the lower limit of the pass band; Ω ₃ represents the cut-off frequency of the upper limit of the pass band; Ω _sl represents the cut-off frequency of the lower limit of the stop band; Ω _sh represents the cut-off frequency of the upper limit of the stop band.

$\{\begin{matrix} {Ω Ω}_{BW BW} = = {Ω Ω}_{33} - - {Ω Ω}_{11} \\ {Ω Ω}_{22}^{22} = = {Ω Ω}_{11} {Ω Ω}_{33} \end{matrix} - - - - - - ((1515))$

其中，Ω_BW表示通带带宽，Ω₂表示阻带中心频率。Among them, Ω _BW represents the bandwidth of the passband, and Ω ₂ represents the center frequency of the stop band.

频率归一化，为：Frequency normalization is:

$\{\begin{matrix} {η η}_{11} = = {Ω Ω}_{11} / / {Ω Ω}_{BW BW} \\ {η η}_{33} = = {Ω Ω}_{33} / / {Ω Ω}_{BW BW} \\ {η η}_{sl sl} = = {Ω Ω}_{sl sl} / / {Ω Ω}_{BW BW} \\ {η η}_{sh sh} = = {Ω Ω}_{sh sh} / / {Ω Ω}_{BW BW} \\ {η η}_{22}^{22} = = {η η}_{11} {η η}_{33} = = {Ω Ω}_{11} {Ω Ω}_{33} / / {Ω Ω}_{BW BW}^{22} \end{matrix} - - - - - - ((1616))$

其中，η_i(i＝1,2,3,sl,sh)分别表示相应频率Ω_i(i＝1,2,3,sl,sh)对应的归一化频率。Wherein, η _i (i=1, 2, 3, sl, sh) respectively represent normalized frequencies corresponding to corresponding frequencies Ω _i (i=1, 2, 3, sl, sh).

带阻滤波器对应低通滤波器的频率转换，具体过程为：The band-stop filter corresponds to the frequency conversion of the low-pass filter, and the specific process is:

$\{\begin{matrix} {λ λ}_{p p} = = {η η}_{33} - - {η η}_{11} = = 11 \\ - - {λ λ}_{s the s}^{' '} = = {η η}_{sl sl} / / (({η η}_{sl sl}^{22} - - {η η}_{22}^{22})) \\ {λ λ}_{s the s}^{' '} = = {η η}_{sh sh} / / (({η η}_{sh sh}^{22} - - {η η}_{22}^{22})) \end{matrix} - - - - - - ((1717))$

其中，λ_p表示低通滤波器的通带截止频率；λ_s′表示低通滤波器的阻带下限截止频率；-λ_s′表示低通滤波器的阻带下限截止频率的对称频率。Among them, λ _p represents the cut-off frequency of the passband of the low-pass filter; λ _s ′ represents the lower cut-off frequency of the stop-band of the low-pass filter; -λ _s ′ represents the symmetrical frequency of the lower cut-off frequency of the stop-band of the low-pass filter.

依据（17）式，取λ_s与-λ_s绝对值较小的一个为最终阻带下限截止频率，即λ_s＝min{λ_s′,-λ_s′}。According to formula (17), the smaller absolute value of λ _s and -λ _s is taken as the lower limit cut-off frequency of the final stop band, that is, λ _s =min{λ _s ′,-λ _s ′}.

设计低通滤波器G(p)。求取滤波器阶次为：Design a low-pass filter G(p). Find the filter order as:

$N N = = lg lg \sqrt{\frac{1010^{{α α}_{s the s} / / 1010} - - 11}{1010^{{α α}_{p p} / / 1010} - - 11}} / / lg lg {λ λ}_{s the s} - - - - - - ((1818))$

其中，N表示滤波器的阶次，α_s表示阻带应达到的最小衰减，α_p表示通带允许最大衰减通常采用α_s＝14dB、α_p＝3dB。Among them, N represents the order of the filter, α _s represents the minimum attenuation that should be achieved in the stop band, and α _p represents the allowable maximum attenuation in the pass band. Usually, α _s =14dB and α _p =3dB are used.

因此，therefore,

$G G ((p p)) = = \frac{11}{{p p}^{N N} + + 11} - - - - - - ((1919))$

其中，p表示滤波器参量。Among them, p represents the filter parameter.

低通滤波器与带阻滤波器之间的参量固定关系为，The parameter fixed relationship between the low-pass filter and the band-stop filter is,

$p p = = {\frac{s the s (({Ω Ω}_{33} - - {Ω Ω}_{11}))}{{s the s}^{22} + + {Ω Ω}_{11} {Ω Ω}_{33}} | |}_{s the s = = \frac{z z - - 11}{z z + + 11}} = = \frac{(({z z}^{22 - -} 11)) (({Ω Ω}_{33} - - {Ω Ω}_{11}))}{{((z z - - 11))}^{22} + + {Ω Ω}_{11} {Ω Ω}_{33} {((z z + + 11))}^{22}} - - - - - - ((2020))$

（20）式代入（19）式，构造出离散型二阶带阻Butterworth滤波器的转移函数，Equation (20) is substituted into Equation (19) to construct the transfer function of the discrete second-order bandstop Butterworth filter,

$H h ((z z)) = = {G G ((p p)) | |}_{p p = = \frac{(({z z}^{22} - - 11)) (({Ω Ω}_{33} - - {Ω Ω}_{11}))}{{((z z - - 11))}^{22} + + {Ω Ω}_{11} {Ω Ω}_{33} {((z z + + 11))}^{22}}} = = \frac{11}{{[[\frac{(({z z}^{22} - - 11)) (({Ω Ω}_{33} - - {Ω Ω}_{11}))}{{((z z - - 11))}^{22} + + {Ω Ω}_{11} {Ω Ω}_{33} {((z z + + 11))}^{22}}]]}^{N N} + + 11} - - - - - - ((21 twenty one))$

根据步骤3中，若ω＝6°/s，则f＝0.1667Hz，设定Butterworth带阻滤波器的通带下限截止频率为0.01Hz，通带上限截止频率为0.028Hz，阻带下限截止频率为0.014Hz，阻带上限截止频率为0.019Hz，求得λ_s＝2.9993。采用α_s＝14dB、α_p＝3dB时，求取滤波器阶次N＝1，最终构造离散型二阶带阻Butterworth滤波器的转移函数为According to step 3, if ω=6°/s, then f=0.1667Hz, the passband lower limit cutoff frequency of setting Butterworth band rejection filter is 0.01Hz, the passband upper limit cutoff frequency is 0.028Hz, the stopband lower limit cutoff frequency is 0.014Hz, the upper limit cut-off frequency of the stop band is 0.019Hz, and λ _s =2.9993 is obtained. When α _s = 14dB, α _p = 3dB, the filter order N = 1 is obtained, and the transfer function of the final discrete second-order band-stop Butterworth filter is constructed as

$H h ((z z)) = = \frac{0.98232 0.98232 - - 1.96355 1.96355 {z z}^{- - 11} + + {0.98177 0.98177 z z}^{- - 22}}{1.00000 1.00000 - - {1.96355 1.96355 z z}^{- - 11} + + {0.96465 0.96465 z z}^{- - 22}} - - - - - - ((22 twenty two))$

图3为（22）式构造的Butterworth滤波器幅频响应曲线。从图中可以看出该滤波器能够满足要求，将输入信息中频率约为0.0167Hz的信息剔除。Figure 3 shows the magnitude-frequency response curve of the Butterworth filter constructed by formula (22). It can be seen from the figure that the filter can meet the requirements and eliminate information with a frequency of about 0.0167 Hz in the input information.

（2）将步骤四中解算得到的导航系下载体速度进行带阻滤波器处理，滤除该速度信息中与旋转角速度有关的振荡误差项，滤波后的速度作为最终导航解算输出信息，具体为：(2) Perform band-rejection filter processing on the body velocity of the navigation system obtained in step 4, filter out the oscillation error item related to the rotational angular velocity in the velocity information, and use the filtered velocity as the final navigation solution output information, Specifically:

对单轴旋转捷联惯导系统解算速度建模为：The velocity model for the single-axis rotating strapdown inertial navigation system is:

V_INS＝V+δV_INS(r)+δV_INS(s) （23）V _INS ＝V+δV _INS(r) +δV _INS(s) (23)

其中，V_INS表示步骤四中经导航解算得到的速度信息；V表示载体运动速度；δV_INS(r)表示由旋转调制引起的振荡误差；δV_INS(s)表示惯导解算舒勒、地球振荡误差和常值误差。Among them, V _INS represents the speed information obtained by navigation _solution in step 4; V represents the velocity of the carrier; δV _INS (r) represents the oscillation error caused by rotation modulation; Earth Oscillation Error and Constant Error.

由于所在频段与其它几项误差所在频段相差悬殊，对使用步骤（1）构建的滤波器，得到：because The frequency band where it is located is quite different from the frequency band where several other errors are located. Using the filter built in step (1), we get:

V′_INS＝H(z)·V_INS （24）V′ _INS ＝H(z) V _INS (24)

式中，H(z)表示根据旋转角速度构建的带阻滤波器；V′_INS为滤波后的速度，其中只包含V、δV_INS(s)。In the formula, H(z) represents the band-stop filter constructed according to the rotational angular velocity; V' _INS is the filtered velocity, which only includes V and δV _INS(s) .

这样，就剔除了由于旋转调制引起的速度振荡误差。以滤波后的速度V′_INS作为系统最终解算速度信息，并输出至导航计算机的显示装置，提供载体航行中的运动速度信息。In this way, velocity oscillation errors due to rotational modulation are eliminated. The filtered speed _V'INS is used as the final speed information of the system, and is output to the display device of the navigation computer to provide the speed information of the carrier during navigation.

实施例：Example:

对本发明的有益效果如下方式得以验证：The beneficial effects of the present invention are verified in the following manner:

（1）在Visual C++仿真条件下，对该方法进行仿真实验：(1) Under the condition of Visual C++ simulation, the simulation experiment of this method is carried out:

载体做三轴摇摆运动。载体以正弦规律绕航向角、纵摇角和横摇角摇摆，其数学模型为：The carrier makes a three-axis rocking motion. The carrier swings around the heading angle, pitch angle and roll angle in a sinusoidal law, and its mathematical model is:

$\{\begin{matrix} ψ ψ = = {ψ ψ}_{m m} sin sin ((22 π π / / {T T}_{ψ ψ} + + {φ φ}_{ψ ψ})) + + k k \\ θ θ = = {θ θ}_{m m} sin sin ((22 π π / / {T T}_{θ θ} + + {φ φ}_{θ θ})) \\ γ γ = = {γ γ}_{m m} sin sin ((22 π π / / {T T}_{γ γ} + + {φ φ}_{γ γ})) \end{matrix} - - - - - - ((2525))$

其中，ψ、θ、γ分别表示绕航向角、纵摇角和横摇角的摇摆角度变量；ψ_m、θ_m、γ_m分别表示相应的摇摆角度幅值，ψ_m＝θ_m＝γ_m＝5°φ_ψ、φ_θ、φ_γ分别表示相应的初相位，T_ψ、T_θ、T_γ分别表示相应摇摆轴的摇摆周期，T_ψ＝T_θ＝T_γ＝4s；k为真航迹，k＝30°；oAmong them, ψ _, θ _, and _γ represent the swing angle variables around the heading _angle , pitch angle and roll _angle respectively _; ＝5°φ _ψ , φ _θ , φ _γ represent the corresponding initial phases respectively, T _ψ , T _θ , T _γ respectively represent the swing period of the corresponding swing axis, T _ψ = T _θ = T _γ = 4s; k is the true track, k = 30°; o

载体初始位置：北纬45.7796°，东经126.6705°；The initial position of the carrier: 45.7796° north latitude, 126.6705° east longitude;

载体匀速直航运动，运动速度为v＝15m/s；The carrier moves in a straight flight at a uniform speed, and the moving speed is v=15m/s;

赤道半径：R＝6378393.0m；Equatorial radius: R=6378393.0m;

由万有引力可得的地球表面重力加速度：g＝9.78049m/s²；The gravitational acceleration on the earth's surface obtained from the universal gravitation: g＝9.78049m/s ² ;

地球自转角速度：Ω＝7.2921158×10^-5rad/s；Earth rotation angular velocity: Ω＝7.2921158×10 ^-5 rad/s;

常数：π＝3.1415926535；Constant: π=3.1415926535;

光纤陀螺常值漂移：0.01°/h；Fiber optic gyroscope constant drift: 0.01°/h;

光纤陀螺白噪声误差：0.005°/h；Optical fiber gyroscope white noise error: 0.005°/h;

光纤陀螺刻度因数误差：10ppm；Optical fiber gyroscope scale factor error: 10ppm;

光纤陀螺安装误差：1×10^-3rad；Fiber optic gyroscope installation error: 1×10 ^-3 rad;

加速度计零偏：10^-4g₀；Accelerometer zero bias: 10 ^-4 g ₀ ;

加速度计白噪声误差：5×10^-5g₀；Accelerometer white noise error: 5×10 ^-5 g ₀ ;

加速度计刻度因数误差：10ppm；Accelerometer scale factor error: 10ppm;

加速度计安装误差：1×10^-3rad；Accelerometer installation error: 1×10 ^-3 rad;

仿真时间：t＝48h；Simulation time: t=48h;

采样频率：Hn＝0.01s；Sampling frequency: Hn=0.01s;

IMU四位置转停方案的参数：Parameters of IMU four-position turn-stop scheme:

四个位置的停顿时间：T_r＝300s；Dwell time at four positions: T _r =300s;

转动180°和90°的转动角速度：ω＝6°/s；Angular velocity of rotation of 180° and 90°: ω=6°/s;

转动180°和90°的过程中，每一个转位中的角加（减）速度：α＝3°/s²；In the process of turning 180° and 90°, the angular acceleration (decrease) speed in each index: α=3°/s ² ;

利用发明所述方法，得到滤波前后速度误差比较曲线如图4所示，其中（a）、（b）图分别表示滤波前的东向速度误差和北向速度误差，（c）、（d）图分别表示滤波后的东向速度误差和北向速度误差。结果表明本发明能够较好的抑制调制状态下系统解算导航误差，提高导航精度，增强速度信息可用性。Using the method described in the invention, the speed error comparison curve before and after filtering is obtained as shown in Figure 4, where (a) and (b) graphs represent the eastward speed error and northward speed error before filtering respectively, and (c) and (d) graphs Denote the filtered eastward velocity error and northward velocity error, respectively. The results show that the present invention can better suppress the navigation error of the system in the modulation state, improve the navigation accuracy, and enhance the availability of speed information.

（2）光纤陀螺惯导系统的单轴转台试验(2) Single-axis turntable test of fiber optic gyro inertial navigation system

采用920E型单轴测试转台和自行研制的光纤陀螺惯导系统构建试验系统。The test system is constructed by using the 920E single-axis test turntable and the self-developed fiber optic gyro inertial navigation system.

所用光纤陀螺惯导系统主要技术指标如下：The main technical indicators of the fiber optic gyro inertial navigation system are as follows:

动态范围：±100°/s；Dynamic range: ±100°/s;

零偏稳定性：≤0.005°/h；Zero bias stability: ≤0.005°/h;

随机游走： Random walk:

标度因数非线性度：≤5ppm。Scale factor nonlinearity: ≤5ppm.

920E型单轴转台台主要技术指标如下：The main technical indicators of the 920E single-axis turntable are as follows:

面直径：450mm；Surface diameter: 450mm;

负载要求：重量50kg；Load requirements: weight 50kg;

台体回转精度：±2′′；Table rotation accuracy: ±2'';

台体转角范围：连续无限；Table body angle range: continuous and infinite;

位置精度：±3′′；Position accuracy: ±3'';

位置分辨力：0.0001°；Position resolution: 0.0001°;

速率范围：0.005-200°/s；Speed range: 0.005-200°/s;

速率精度：5×10^-5（360°平均）、5×10^-4（10°平均）、1×10^-2（1°平均）。Rate accuracy: 5×10 ^-5 (360° average), 5×10 ^-4 (10° average), 1×10 ^-2 (1° average).

利用发明所述方法，得到滤波前后速度误差比较曲线如图5所示，其中（a）、（b）图分别表示滤波前的东向速度误差和北向速度误差，（c）、（d）图分别表示滤波后的东向速度误差和北向速度误差。结果表明本发明抑制速度误差能力较好，可以满足实际需求。Using the method described in the invention, the speed error comparison curve before and after filtering is obtained as shown in Figure 5, where (a) and (b) respectively represent the eastward speed error and northward speed error before filtering, and (c) and (d) Denote the filtered eastward velocity error and northward velocity error, respectively. The results show that the present invention has a better capability of suppressing speed errors and can meet actual needs.

Claims

1. A method for suppressing speed error of a single-axis rotating fiber-optic gyroscope strapdown inertial navigation system is characterized by comprising the following steps:

the method comprises the following steps: collecting carrier position information through a Global Positioning System (GPS), and binding the carrier position information into a navigation computer;

the navigation initial time, carrier position information and speed information at the initial time are collected through a Global Positioning System (GPS), and are bound into a navigation computer; the carrier position information comprises longitude and latitude information of the position of the carrier;

in the navigation process, updating by using the initial information to obtain the speed and the position of the carrier at any moment;

step two: rotating the rotating mechanism to a position where the IMU system and the carrier system coincide with each otherWhere b denotes the carrier coordinate system, s denotes the IMU coordinate system,expressing s is a transformation matrix to b, and I is a unit matrix; fully preheating a fiber optic gyroscope strapdown inertial navigation system, and acquiring data output by a fiber optic gyroscope and a quartz accelerometer; obtaining angular motion information and line motion information of the carrier; the angular motion information comprises an angular velocity value, and the line motion information comprises a specific force value;

determining a carrier attitude angle according to the relation between a specific force value output by an accelerometer and the gravity acceleration and the relation between an angular velocity value output by a gyroscope and the earth rotation angular velocity, finishing the initial alignment of the system, and establishing an initial strapdown matrix of the inertial navigation system

Wherein phi is_x0、φ_y0、φ_z0Respectively representing a pitch angle, a roll angle and a course angle of the carrier at an initial moment;

step three: the rotating mechanism drives the inertia assembly to perform single-shaft positive and negative rotation stop motion by omega; adopting a rotation scheme with eight rotation-stop sequences as one rotation period;

the inertial component is called an IMU for short, and an indexing scheme with eight rotation and stop sequences as one rotation period is adopted in the rotation process of the IMU, and the method specifically comprises the following steps:

sequence 1, IMU makes a clockwise rotation of 180 ° from point A, reaches position C, and stops for time T_r(ii) a Sequence 2, IMU makes a counter-clockwise rotation of 90 ° from point C, reaches position B, and stops for time T_r(ii) a Sequence 3, IMU makes a clockwise rotation of 180 ° from point B, reaches position D, and stops for time T_r(ii) a Sequence 4, IMU makes a counterclockwise rotation 270 from point D, reaches position A, and stops for time T_r(ii) a Sequence 5, IMU makes a counter-clockwise rotation of 180 ° from point A, reaches position C, and stops for time T_r(ii) a Sequence 6, IMU makes a clockwise rotation of 90 ° from point C, reaches position D, and stops for time T_r(ii) a Sequence 7, IMU makes a counter-clockwise rotation of 180 ° from point D, reaches position B, and stops for time T_r(ii) a Sequence 8, IMU makes a clockwise 270 from point B, reaches position A, and stops for time T_r(ii) a The IMU is circularly carried out according to the rotation sequence;

step four: acquiring linear velocity and angular velocity information of carrier motion measured by an optical fiber gyroscope and a quartz accelerometer in real time, and performing navigation calculation to obtain navigation information;

updating a strapdown matrix by rotating gyroscope data acquired under a modulation stateThe method specifically comprises the following steps:

updating the angular speed:

wherein i represents a geocentric inertial system, e represents a terrestrial coordinate system, s represents an inertial component coordinate system, n represents a navigation coordinate system, and a local geographic coordinate system is adopted;representing a transformation matrix from s to n;the projection of p series relative m series rotation angular velocity on q series is shown, m is equal to n, i, e, p is equal to s, e, n, q is equal to s;

updating a quaternion attitude matrix:

and setting the rotation quaternion of the carrier coordinate system relative to the platform coordinate system at any moment as follows:

Q＝q₀+q₁i_b+q₂j_b+q₃k_b (3)

wherein Q is a quaternion; q. q.s₀、q₁、q₂、q₃Four real numbers that are quaternions; i.e. i_b、j_b、k_bRespectively represent the IMU coordinate system ox_sAxle, oy_sAxis, oz_sUnit direction vector on axis;

and (3) timely correcting the quaternion Q:

wherein,respectively representing the motion angular speed of the rotating mechanism relative to the navigation system in an IMU coordinate system ox_sAxle, oy_sAxis, oz_sAn on-axis component;respectively represent q₀、q₁、q₂、q₃The rate of change of (c);

rotating quaternion q relative to the platform coordinate system according to the carrier coordinate system at the moment k₀(k)、q₁(k)、q₂(k)、q₃(k) And solving the change rate of the rotation quaternion at the moment k as follows:

the rotation quaternion of the carrier at the time k +1 is specifically as follows:

using the resulting q₀(k+1)、q₁(k+1)、q₂(k+1)、q₃(k +1), updating the strapdown matrix

C_{s}^{n} = [\begin{matrix} q_{0}^{2} + q_{1}^{2} - q_{2}^{2} - q_{3}^{2} & 2 (q_{1} q_{2} - q_{0} q_{3}) & 2 (q_{1} q_{3} + q_{0} q_{2}) \\ 2 (q_{1} q_{2} + q_{0} q_{3}) & q_{0}^{2} - q_{1}^{2} + q_{2}^{2} - q_{3}^{2} & 2 (q_{2} q_{3} - q_{0} q_{1}) \\ 2 (q_{1} q_{3} - q_{0} q_{2}) & 2 (q_{2} q_{3} + q_{0} q_{1}) & q_{0}^{2} - q_{1}^{2} - q_{2}^{2} + q_{3}^{2} \end{matrix}] - - - (7)

Wherein q in the formula (7)_iIs represented by the formula (6) wherein q is_i(k+1)，i＝1,2,3,4；

Updating carrier attitude information, specifically:

<math> <mrow> <mfenced open='{' close=''> <mtable> <mtr> <mtd> <msub> <mi>φ</mi> <mi>x</mi> </msub> <mo>=</mo> <mi>arcsin</mi> <mrow> <mo>(</mo> <msub> <mi>c</mi> <mn>33</mn> </msub> <mo>)</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <msub> <mi>φ</mi> <mi>y</mi> </msub> <mo>=</mo> <mi>arctan</mi> <mrow> <mo>(</mo> <msub> <mi>c</mi> <mn>32</mn> </msub> <mo>/</mo> <msub> <mi>c</mi> <mn>31</mn> </msub> <mo>)</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <msub> <mi>φ</mi> <mi>z</mi> </msub> <mo>=</mo> <mi>arctan</mi> <mrow> <mo>(</mo> <msub> <mi>c</mi> <mn>13</mn> </msub> <mo>/</mo> <msub> <mi>c</mi> <mn>23</mn> </msub> <mo>)</mo> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>8</mn> <mo>)</mo> </mrow> </mrow> </math>

wherein, c_ijTo representThe matrix elements in the ith row and the jth column, i is 1,2,3, and j is 1,2, 3; phi is a_x、φ_y、φ_zRepresenting the longitudinal rocking angle, the transverse rocking angle and the course angle of the carrier;

the specific force information measured by the accelerometer along the IMU coordinate system is processed by a strapdown matrixAnd (3) projection conversion is carried out:

f^{n} = C_{s}^{n} f^{s} - - - (9)

wherein f isⁿ、f^sRespectively representing the projections of the specific force measured by the accelerometer on an n system and an s system;

solving the speed of motion of the carrier by using the following differential equation:

wherein v is_x、v_y、v_zRespectively representing the speed of the calculation carrier in the navigation system ox_nAxle, oy_nAxis, oz_nAn on-axis component; denotes v_x、v_y、v_zRate of change of, i.e. carrier along, the navigational system ox_nAxle, oy_nAxis, oz_nAcceleration of motion of the shaft;respectively representing the specific force measured by the accelerometer in the navigation system ox_nAxle, oy_nAxis, oz_nAn on-axis component; g is the acceleration of gravity; respectively representing the rotational angular velocity of the earth in the navigation system ox_nAxle, oy_nAxis, oz_nAn on-axis component;respectively representing the variation of the rotation angular velocity of the navigation system relative to the earth system caused by the carrier motion in the navigation system ox_nAxle, oy_nAxis, oz_nAn on-axis component;

vector east horizontal velocity v according to time k_x(k) North direction horizontal velocity v_y(k) And velocity v in the direction of the sky_z(k) And solving the carrier speed change rate at the moment k as follows:

at time k +1 the carrier velocity and position are:

wherein R represents the radius of the earth;λ represents latitude and longitude information of the geographical position of the calculation carrier, respectively, and v is the same as v when k is 1_x(1)、v_y(1)、v_z(1) For the initial velocity of the carrier obtained by using GPS in step one,lambda (1) is the initial position of the carrier obtained by using the GPS in the step one;

obtaining the speed, position and posture of the carrier according to the formulas (8), (12) and (13); then directly outputting the posture and the position to a navigation computer display to provide various navigation information of the carrier; the speed is to be subjected to further filtering processing optimization in the fifth step;

step five: constructing a Butterworth band elimination filter, carrying out Butterworth filter processing on the carrier speed obtained under the navigation system, filtering an oscillation error item related to a rotation angular speed under the navigation system, and taking the filtered speed as final navigation resolving output information;

(1) constructing a Butterworth band elimination filter, which specifically comprises the following steps:

in the third step, the rotating mechanism adopts a rotation angular velocity of omega, and the angular velocity causes the oscillation error frequency of the navigation information to be about f ═ 1/omega; setting the lower limit cut-off frequency of a passband of the Butterworth band elimination filter to be (0.59-0.61) f, the upper limit cut-off frequency of the passband to be (1.67-1.69) f, the lower limit cut-off frequency of a stopband to be (0.83-0.85) f, and the upper limit cut-off frequency of the stopband to be (1.13-1.15) f, then:

wherein omega₁Represents the passband lower cutoff frequency; omega₃Represents the passband upper cutoff frequency; omega_slRepresents the stopband lower limit cut-off frequency; omega_shRepresenting stop bandAn upper cut-off frequency;

wherein omega_BWRepresents the passband bandwidth, Ω₂Represents the stopband center frequency;

frequency normalization, which is:

wherein eta is_iRespectively representing the corresponding frequencies omega_iCorresponding normalized frequency, i ═ 1,2,3, sl, sh;

the frequency conversion of the band elimination filter corresponding to the low-pass filter comprises the following specific processes:

wherein λ is_pRepresents the passband cutoff frequency of the low pass filter; lambda'_sRepresents the stopband lower limit cut-off frequency of the low-pass filter; - λ'_sA symmetric frequency representing a stopband lower limit cutoff frequency of the low-pass filter;

according to formula (17), take λ_s' and-lambda_sThe smaller of the absolute values is the final stopband lower cut-off frequency, i.e., λ_s＝min{|λ′_s|,|-λ′_s|}；

Designing a low-pass filter G (p); the filter order is found as:

wherein N denotes the order of the filter, α_sIndicating the minimum attenuation, a, that should be achieved for the stop band_pThe maximum attenuation allowed by the passband is usually represented by alpha_s＝14dB、α_p＝3dB；

Therefore, the temperature of the molten metal is controlled,

G (p) = \frac{1}{p^{N} + 1} - - - (19)

wherein p represents a filter parameter;

the parametric fixed relationship between the low-pass filter and the band-stop filter is,

(20) substituting the formula into the formula (19) to construct a transfer function of the discrete second-order band-stop Butterworth filter,

(2) and (3) performing band elimination filter processing on the carrier speed of the navigation system obtained by resolving in the fourth step, filtering an oscillation error item related to the rotation angular velocity in the speed information, and taking the filtered speed as final navigation resolving output information, wherein the band elimination filter processing method specifically comprises the following steps of:

the calculation speed of the single-axis rotation strapdown inertial navigation system is modeled as follows:

V_INS＝V+V_INS(r)+V_INS(s) (22)

wherein, V_INSRepresenting the speed information obtained by navigation resolving in the step four; v represents the carrier moving speed; v_INS(r)Representing oscillation errors caused by rotational modulation; v_INS(s)Expressing inertial navigation resolving Schuler, earth oscillation error and constant error;

due to the fact thatThe frequency band is very different from the frequency bands of other errors, and the pairUsing the filter constructed in step (1), obtaining:

V′_INS＝H(z)·V_INS (23)

wherein h (z) represents a band elimination filter constructed according to a rotation angular velocity; v'_INSIs the filtered velocity, which only contains V, V_INS(s)；

At filtered speed V'_INSAnd finally, the speed information is calculated by the system and is output to a display device of the navigation computer, and the movement speed information of the carrier in the navigation process is provided.