CN1908584A

CN1908584A - Method for determining initial status of strapdown inertial navigation system

Info

Publication number: CN1908584A
Application number: CN 200610112526
Authority: CN
Inventors: 房建成; 宫晓琳; 盛蔚; 杨胜; 徐帆; 刘百奇
Original assignee: Beihang University
Current assignee: Beihang University
Priority date: 2006-08-23
Filing date: 2006-08-23
Publication date: 2007-02-07
Anticipated expiration: 2026-08-23
Also published as: CN100516775C

Abstract

A method for determining the initial attitude of a strapdown inertial navigation system, by rotating the SINS from the initial position around any three-dimensional axis to any position, and according to the relationship between the output of the SINS at position 1 and the angular velocity of the earth's rotation and the acceleration of gravity, the SINS is initially determined. initial posture. Due to the change of the position of the SINS, the observability of the system is improved, and the Kalman filter technology is further used to accurately estimate the attitude misalignment angle, the constant drift of the gyroscope, and the constant value of the accelerometer to obtain a more accurate initial attitude of the SINS. The invention has the characteristics of independence, high precision, flexibility and convenience, and is suitable for various strapdown inertial navigation systems of medium and high precision.

Description

A Method for Determining Initial Attitude of Strapdown Inertial Navigation System

技术领域technical field

本发明涉及一种捷联惯性导航系统(SINS)初始姿态的确定方法，可用于各种中高精度的捷联惯性导航系统的初始姿态确定，特别适合于机载SINS安装时无转位机构，或转位机构精度不高、转角有限的情况。The invention relates to a method for determining the initial attitude of a strapdown inertial navigation system (SINS), which can be used for determining the initial attitude of various medium and high-precision strapdown inertial navigation systems, and is particularly suitable for the installation of an airborne SINS without an indexing mechanism, or The case where the precision of the indexing mechanism is not high and the rotation angle is limited.

背景技术Background technique

捷联惯性导航系统(SINS)的基本原理是根据牛顿提出的相对惯性空间的力学定律，利用陀螺仪、加速度计测量载体相对惯性空间的线运动和角运动参数，在给定的运动初始条件下，由计算机进行积分运算，连续、实时地提供位置、速度和姿态信息。SINS完全依靠自身的惯性敏感元件，不依赖任何外界信息测量导航参数，因此，它具有隐蔽性好，不受气候条件限制，无信号丢失，不受干扰等优点，是一种完全自主式、全天候的导航系统，已广泛应用于航空、航天、航海等领域。根据SINS的基本原理，SINS在导航定位解算之前必须获得初始信息，包括初始位置、速度和姿态。SINS的初始位置和速度信息较初始姿态容易获得，初始姿态确定后的精度就是SINS进入导航工作状态时的初始精度。因此，SINS开始工作时进行快速精确的初始姿态确定是十分重要的一步。The basic principle of the strapdown inertial navigation system (SINS) is based on Newton’s law of mechanics relative to the inertial space, using gyroscopes and accelerometers to measure the linear and angular motion parameters of the carrier relative to the inertial space. , the computer performs integral calculations to provide position, speed and attitude information continuously and in real time. SINS completely relies on its own inertial sensitive components and does not rely on any external information to measure navigation parameters. Therefore, it has the advantages of good concealment, no limitation of weather conditions, no signal loss, and no interference. It is a completely autonomous, all-weather The navigation system has been widely used in aviation, aerospace, navigation and other fields. According to the basic principle of SINS, SINS must obtain initial information, including initial position, velocity and attitude, before calculating navigation and positioning. The initial position and velocity information of SINS is easier to obtain than the initial attitude, and the accuracy after the initial attitude is determined is the initial accuracy when SINS enters the navigation working state. Therefore, fast and accurate initial attitude determination is a very important step when SINS starts to work.

现有的捷联惯导系统初始姿态确定可分为粗对准和精对准两个阶段。粗对准阶段就是在静基座条件下，将单个位置或两个位置上(固定两位置或任意两位置)的陀螺仪和加速度计输出直接引入计算机，计算出载体的初始姿态。用此方法时，常常忽略陀螺仪与加速度计的误差及外部干扰因素，然而这些因素会导致误差，因此初始姿态计算精度不高。特别地，当采用固定两位置上的陀螺仪和加速度计的输出进行计算时，要求SINS绕Z轴旋转180度或90度，这就需要将SINS安装在一个转位机构上，利用转位机构实现180度或者90度的转动，工程使用时极不方便，而且转位机构的精度不高，转动角度也无法精确测量，降低了初始姿态确定的精度。由于具体工程中SINS安装位置的限制，还存在转动角度无法满足180度或90度要求的情况，此时，现有的固定两位置初始姿态确定方法将无法应用。此外，当利用任意两位置上的陀螺仪和加速度计输出时，需计算反正弦函数，陀螺仪和加速度计本身的误差以及测量误差等，容易造成计算结果不稳定，出现虚数现象，因此常需要进行多次任意两位置上陀螺仪和加速度计输出的采集和计算，对计算结果进行挑选，取平均值后，作为载体的初始姿态。这样一来，初始姿态确定的时间将成倍增加，而且初始姿态计算的精度也无法得到保证。The initial attitude determination of the existing strapdown inertial navigation system can be divided into two stages: coarse alignment and fine alignment. In the coarse alignment stage, under the condition of a static base, the output of the gyroscope and accelerometer at a single position or two positions (fixed two positions or any two positions) is directly introduced into the computer to calculate the initial attitude of the carrier. When using this method, the errors of gyroscopes and accelerometers and external interference factors are often ignored, but these factors will cause errors, so the initial attitude calculation accuracy is not high. In particular, when the output of the gyroscope and accelerometer at two fixed positions is used for calculation, the SINS is required to rotate 180 degrees or 90 degrees around the Z axis, which requires the SINS to be installed on an indexing mechanism. It is extremely inconvenient to realize the rotation of 180 degrees or 90 degrees, and the precision of the indexing mechanism is not high, and the rotation angle cannot be accurately measured, which reduces the accuracy of determining the initial attitude. Due to the limitation of the installation position of SINS in specific projects, there are still cases where the rotation angle cannot meet the requirements of 180 degrees or 90 degrees. At this time, the existing method for determining the initial attitude of fixed two positions will not be applicable. In addition, when using the output of the gyroscope and accelerometer at any two positions, it is necessary to calculate the arcsine function, the error of the gyroscope and accelerometer itself and the measurement error, etc., which will easily cause the calculation result to be unstable and the phenomenon of imaginary numbers to appear, so it is often necessary to Collect and calculate the output of the gyroscope and accelerometer at any two positions multiple times, select the calculation results, and take the average value as the initial attitude of the carrier. In this way, the time to determine the initial attitude will be multiplied, and the accuracy of the initial attitude calculation cannot be guaranteed.

精对准阶段是在粗对准的基础上进行，利用现代控制理论的状态空间法，对陀螺仪和加速度计输出的数据进行处理。当对单个位置的数据进行处理时，方位失准角的可观度差，收敛速度较慢，所需时间较长，而且陀螺仪和加速度计误差不可观测，因此，无法进行较好的估计，达不到提高初始姿态精度的目的，也不能在初始姿态计算的同时实现对陀螺仪和加速度计的标定。当对固定两个位置的数据进行处理时，如上已叙述，现有的固定两位置初始姿态确定方法存在旋转角度的限制，且对转位机构精度要求较高，具体工程中往往不能应用。The fine alignment stage is carried out on the basis of coarse alignment, using the state space method of modern control theory to process the data output by the gyroscope and accelerometer. When processing the data of a single location, the observability of the azimuth misalignment angle is poor, the convergence speed is slow, and the time required is long, and the errors of the gyroscope and accelerometer are not observable, so it is impossible to make a good estimate. The purpose of improving the accuracy of the initial attitude is not achieved, and the calibration of the gyroscope and the accelerometer cannot be realized at the same time as the initial attitude calculation. When processing the data of fixed two positions, as described above, the existing method for determining the initial attitude of fixed two positions has the limitation of rotation angle, and requires high precision of the indexing mechanism, so it is often not applicable in specific projects.

发明内容Contents of the invention

本发明的技术解决问题是：克服现有技术的不足，提供一种精确、方便的捷联惯性导航系统初始姿态确定方法。The technical problem of the present invention is to overcome the deficiencies of the prior art and provide an accurate and convenient method for determining the initial attitude of the strapdown inertial navigation system.

本发明的技术解决方案为：一种捷联惯性导航系统初始姿态确定方法，The technical solution of the present invention is: a method for determining the initial attitude of a strapdown inertial navigation system,

具体步骤如下：Specific steps are as follows:

(1)SINS预热准备，具体准备时间根据不同的系统而不同；(1) SINS warm-up preparation, the specific preparation time varies according to different systems;

(2)SINS准备完毕后，保持SINS在初始位置(称为第一位置)静止不动，采集陀螺仪输出和加速度计输出；(2) After the SINS is ready, keep the SINS stationary at the initial position (referred to as the first position), and collect the output of the gyroscope and the output of the accelerometer;

(3)利用加速度计输出与重力加速度的关系以及陀螺仪输出与地球自转角速率的关系，计算出第一位置的航向角₁、俯仰角θ₁和横滚角γ₁；(3) Utilize the relationship between the accelerometer output and the acceleration of gravity and the relationship between the gyroscope output and the earth's rotation angular rate to calculate the heading angle  ₁ , pitch angle θ ₁ and roll angle γ ₁ of the first position;

(4)无需转位机构，通过任意方法将SINS从第一位置绕空间三维任意轴旋转到任意一个位置(称为第二位置)，保持SINS在第二位置静止不动，采集陀螺仪输出和加速度计输出；(4) Without an indexing mechanism, rotate the SINS from the first position to any position (called the second position) around any three-dimensional axis in space by any method, keep the SINS stationary at the second position, and collect the gyroscope output and accelerometer output;

(5)采用卡尔曼滤波技术，将₁、θ₁和γ₁作为初始参数，对两个位置上陀螺仪和加速度计输出的数据进行处理。由于SINS位置的改变，变化了SINS误差模型中的系统矩阵，从而SINS系统的可观测性得到改善，滤波效果得到提高，更好的估计出第二位置时计算地理坐标系n′与真实地理坐标系n之间的误差角(简称为失准角φ_x、φ_y和φ_z)及陀螺仪常值漂移、加速度计常值偏置；(5) Using Kalman filter technology, with  ₁ , θ ₁ and γ ₁ as initial parameters, the data output by the gyroscope and accelerometer at two positions are processed. Due to the change of the SINS position, the system matrix in the SINS error model is changed, so that the observability of the SINS system is improved, the filtering effect is improved, and the geographic coordinate system n' and the real geographic coordinates are better estimated when the second position is calculated. The error angle between the system n (abbreviated as the misalignment angle φ _x , φ _y and φ _z ) and the constant value drift of the gyroscope and the constant value bias of the accelerometer;

(6)利用估计出的φ_x、φ_y和φ_z计算真实地理坐标系n与计算地理坐标系n′之间的转换矩阵C_n′ ⁿ。根据陀螺仪输出的角增量或角速度信息，采用四元数方法，计算载体坐标系b与计算地理坐标系n′之间的转换矩阵C_b ^n′。由C_n′ ⁿ和C_b ^n′，计算出载体坐标系b与真实地理坐标系n之间的转换矩阵C_b ⁿ，再由C_b ⁿ计算第二位置的航向角₂、俯仰角θ₂和横滚角γ₂，将其作为SINS的初始姿态。(6) Use the estimated φ _x , φ _y and φ _z to calculate the conversion matrix C _n′ ⁿ between the real geographic coordinate system n and the calculated geographic coordinate system n′. According to the angular increment or angular velocity information output by the gyroscope, the conversion matrix C _b ^n′ between the carrier coordinate system b and the calculation geographic coordinate system n′ is calculated by using the quaternion method. From C _n′ ⁿ and C _b ^n′ , calculate the conversion ^matrix C b n between the carrier coordinate _{system b and the real geographic coordinate system n, and then use C b} _n ^to calculate the heading angle  ₂ and pitch angle θ of the second position ₂ and the roll angle γ ₂ , which are used as the initial attitude of SINS.

本发明的原理是：SINS在某一个位置保持静止不动时，加速度计输出与重力加速度以及陀螺仪输出与地球自转角速率有如下关系：Principle of the present invention is: when SINS remains stationary at a certain position, the accelerometer output and the acceleration of gravity and the gyroscope output have the following relationship with the earth's rotation angular rate:

$[\begin{matrix} {f f}_{x x 11} \\ {f f}_{y the y 11} \\ {f f}_{z z 11} \end{matrix}] = = {C C}_{n no}^{b b} [\begin{matrix} 00 \\ 00 \\ g g \end{matrix}] . . . . . . ((11))$

$[\begin{matrix} {ω ω}_{x x 11} \\ {ω ω}_{y the y 11} \\ {ω ω}_{z z 11} \end{matrix}] = = {C C}_{n no}^{b b} [\begin{matrix} 00 \\ {ω ω}_{ie ie} cos cos L L \\ {ω ω}_{ie ie} sin sin L L \end{matrix}] . . . . . . ((22))$

其中，f_x1、f_y1和f_z1以及ω_x1、ω_y1和ω_z1分别为此位置上SINS的X轴、Y轴和Z轴输出的比力和角速率；g为重力加速度；ω_ie为地球自转角速率，其在东、北、天方向上的投影为Ω＝[0 ω_iecosL ω_iesinL]；L为当地纬度；C_n ^b为导航坐标系到载体坐标系的转换矩阵， $C_{n}^{b} = {(C_{b}^{n})}^{T} = [\begin{matrix} C_{11} & C_{13} & C_{13} \\ C_{21} & C_{12} & C_{23} \\ C_{31} & C_{32} & C_{33} \end{matrix}] .$ Among them, f _x1 , f _y1 and f _z1 and ω _x1 , ω _y1 and ω _z1 are respectively the specific force and angular rate output by the X-axis, Y-axis and Z-axis of SINS at this position; g is the gravitational acceleration; ω _ie is The angular rate of the earth's rotation, its projection in the east, north, and sky directions is Ω=[0 ω _ie cosL ω _ie sinL]; L is the local latitude; C _n ^b is the transformation matrix from the navigation coordinate system to the carrier coordinate system, $C_{no}^{b} = {(C_{b}^{no})}^{T} = [\begin{matrix} C_{11} & C_{13} & C_{13} \\ C_{twenty one} & C_{12} & C_{twenty three} \\ C_{31} & C_{32} & C_{33} \end{matrix}] .$

C_n ^b可以表达成此位置处航向角₁、俯仰角θ₁和横滚角γ₁的表达式，利用(1)式和(2)式可以计算出此位置处的₁、β₁和γ₁。计算公式如下：C _n ^b can be expressed as the expressions of heading angle  ₁ , pitch angle θ ₁ and roll angle γ ₁ at this position, and  ₁ and β ₁ at this position can be calculated using formula (1) and formula (2) and γ ₁ . Calculated as follows:

由(1)、(2)式可得：From (1), (2) formula can get:

${C C}_{1313} = = \frac{{f f}_{x x 11}}{g g}$

${C C}_{23 twenty three} = = \frac{{f f}_{y the y 11}}{g g}$

${C C}_{3333} = = \frac{{f f}_{z z 11}}{g g}$

${C C}_{1212} = = \frac{{ω ω}_{x x 11}}{{ω ω}_{ie ie} cos cos L L} - - \frac{{f f}_{x x 11} tan the tan L L}{g g}$

${C C}_{22 twenty two} = = \frac{{ω ω}_{y the y 11}}{{ω ω}_{ie ie} cos cos L L} - - \frac{{f f}_{y the y 11} tan the tan L L}{g g} . . . . . . ((33))$

${C C}_{3232} = = \frac{{ω ω}_{z z 11}}{{ω ω}_{ie ie} cos cos L L} - - \frac{{f f}_{z z 11} tan the tan L L}{g g}$

C₁₁＝C₂₂C₃₃-C₂₃C₃₂ C ₁₁ =C ₂₂ C ₃₃ -C ₂₃ C ₃₂

C₂₁＝-C₁₂C₃₃+C₁₃C₃₂ C ₂₁ ＝-C ₁₂ C ₃₃ +C ₁₃ C ₃₂

C₃₁＝C₁₂C₂₃-C₁₃C₂₂ C ₃₁ ＝C ₁₂ C ₂₃ -C ₁₃ C ₂₂

航向角₁、俯仰角θ₁和横滚角γ₁的主值计算公式为：The main value calculation formulas of heading angle  ₁ , pitch angle θ ₁ and roll angle γ ₁ are:

若航向角₁、俯仰角θ₁和横滚角γ₁的取值范围定义为[0，2π]、

[-π，+π]，那么₁、θ₁和γ₁的真值可按如下方法确定。If the range of heading angle  ₁ , pitch angle θ ₁ and roll angle γ ₁ is defined as [0, 2π],

[-π, +π], then the true values of  ₁ , θ ₁ and γ ₁ can be determined as follows.

θ₁＝θ_1主 (5)θ ₁ = θ _{1 main} (5)

由(5)式确定的₁、θ₁和γ₁即为SINS在此位置上的航向角、俯仰角和横滚角。 ₁ , θ ₁ and γ ₁ determined by formula (5) are the heading angle, pitch angle and roll angle of SINS at this position.

由于(1)式和(2)式中未考虑加速度计偏置、陀螺仪漂移以及加速度计和陀螺仪输出信息的测量误差，求得的航向角₁、俯仰角θ₁和横滚角γ₁不能准确描述载体坐标系与当地地理坐标系之间的真实角度关系。因此，应当在此基础上，进一步利用现代估计理论将初始姿态失准角从随机误差和随机干扰中估计出来。Since the accelerometer bias, gyroscope drift and the measurement error of the output information of the accelerometer and gyroscope are not considered in the formulas (1) and (2), the obtained heading angle  ₁ , pitch angle θ ₁ and roll angle γ ₁ cannot accurately describe the true angular relationship between the carrier coordinate system and the local geographic coordinate system. Therefore, on this basis, we should further use modern estimation theory to estimate the initial attitude misalignment angle from random errors and random disturbances.

利用卡尔曼滤波技术，将₁、θ₁和γ₁作为初始参数，可以估计出计算地理坐标系n′与真实地理坐标系n之间的误差角，校正C_b ^n′后，可获得更为准确的初始姿态。但是，当卡尔曼滤波对单个位置的加速度计和陀螺仪输出进行处理时，系统不完全可观测，其中，两个加速度计和一个陀螺仪的误差不可观测，因此估计效果差，达不到提高初始姿态精度的目的。本发明通过将SINS从第一位置绕空间三维任意轴旋转到任意一个位置，即改变SINS的位置，来变化SINS误差模型中的系统矩阵，从而改善SINS系统的可观测性，提高卡尔曼滤波效果，更好地估计出失准角及陀螺仪、加速度计的误差。利用估计出的失准角对转换矩阵C_b ^n′进行校正，得到更为准确的航向角、俯仰角和横滚角。Using the Kalman filter technology, using  ₁ , θ ₁ and γ ₁ as initial parameters, the error angle between the calculated geographic coordinate system n′ and the real geographic coordinate system n can be estimated. After correcting C _b ^n′ , a more accurate for the correct initial pose. However, when Kalman filtering is performed on the accelerometer and gyroscope output at a single location, the system is not fully observable, where the errors of two accelerometers and one gyroscope are not observable, so the estimation effect is poor and the improvement cannot be achieved. The purpose of the initial pose accuracy. The present invention changes the system matrix in the SINS error model by rotating the SINS from the first position around any three-dimensional axis in space to any position, that is, changing the position of the SINS, thereby improving the observability of the SINS system and improving the effect of Kalman filtering , to better estimate the misalignment angle and the errors of the gyroscope and accelerometer. Using the estimated misalignment angle to correct the conversion matrix C _b ^n' , more accurate heading angle, pitch angle and roll angle can be obtained.

本发明与现有技术相比的优点在于：The advantage of the present invention compared with prior art is:

(1)本发明打破了传统固定两位置初始姿态计算需要通过转位机构将SINS绕Z轴旋转180度或90度的约束，而是无需通过转位机构，将SINS绕任意轴旋转到任意位置，是一种空间三维的任意双位置初始姿态计算方法。避免了因转位机构精度不高，造成的固定两位置初始姿态计算精度的降低，即提高了初始姿态计算的精度。此外，将SINS绕任意轴旋转到任意位置也极大的方便了在实际工程中的应用。(1) The present invention breaks the constraint that the traditional fixed two-position initial attitude calculation needs to rotate the SINS by 180 degrees or 90 degrees around the Z axis through the indexing mechanism, but does not need to use the indexing mechanism to rotate the SINS to any position around any axis , is a three-dimensional space three-dimensional arbitrary two-position initial attitude calculation method. It avoids the reduction of the initial attitude calculation accuracy of the fixed two positions caused by the low precision of the indexing mechanism, that is, the accuracy of the initial attitude calculation is improved. In addition, rotating SINS around any axis to any position also greatly facilitates the application in practical engineering.

(2)本发明利用两个位置上陀螺仪和加速度计输出的数据，改善SINS系统的可观测性，提高卡尔曼滤波器的滤波效果，更好地估计出失准角及陀螺仪常值漂移、加速度计常值偏置，利用估计出的失准角对姿态矩阵进行校正后，得到更为准确的航向角、俯仰角和横滚角。(2) The present invention utilizes the data output by the gyroscope and the accelerometer on two positions to improve the observability of the SINS system, improve the filtering effect of the Kalman filter, and better estimate the misalignment angle and the constant value drift of the gyroscope , Accelerometer constant value bias, after using the estimated misalignment angle to correct the attitude matrix, more accurate heading angle, pitch angle and roll angle are obtained.

(3)本发明能够在初始姿态计算的同时完成测漂和定标任务，不但提高了系统初始姿态计算的精度，而且定标精度也得到了提高，在SINS导航状态给予补偿，可有效地提高导航定位精度。(3) The present invention can complete the tasks of drift measurement and calibration while calculating the initial attitude, which not only improves the accuracy of the initial attitude calculation of the system, but also improves the calibration accuracy. Compensation is given in the SINS navigation state, which can effectively improve Navigation positioning accuracy.

附图说明Description of drawings

图1为本发明的初始姿态确定方法的流程图；Fig. 1 is the flow chart of the initial posture determination method of the present invention;

图2为航向角、俯仰角θ和横滚角γ的示意图，图中Ox_ny_nz_n为导航坐标系，即东北天地理坐标系，Ox_by_bz_b为载体坐标系。其中，图2a表示从导航坐标系Ox_ny_nz_n绕zn轴逆时针旋转与载体坐标系Ox_by_bz_b重合，即为航向角；图2b表示从导航坐标系Ox_ny_nz_n绕x_n轴逆时针旋转θ与载体坐标系Ox_by_bz_b重合，θ即为俯仰角；图2c表示从导航坐标系Ox_ny_nz_n绕y_n轴逆时针旋转γ与载体坐标系Ox_by_bz_b重合，γ即为横滚角。Figure 2 is a schematic diagram of heading angle , pitch angle θ, and roll angle γ. In the figure, Ox _n y _n z _n is the navigation coordinate system, that is, the northeast sky geographic coordinate system, and Ox _by _b z _b is the carrier coordinate system. Among them, Figure 2a shows that the counterclockwise rotation  from the navigation coordinate system Ox _n y _n z _n around the zn axis coincides with the carrier coordinate system Ox _b y _b z _b , and  is the heading angle; Figure 2b shows that from the navigation coordinate system Ox _n y The counterclockwise rotation of _n z _n around the x _n axis θ coincides with the carrier coordinate system Ox _by y _b z _b , and θ is the pitch angle; Figure 2c shows the counterclockwise rotation of the navigation coordinate system Ox _n y _n z _n around the y _n axis γ It coincides with the carrier coordinate system Ox _by _b z _b , and γ is the roll angle.

具体实施方式Detailed ways

如图1所示，本发明的具体实施方法如下：As shown in Figure 1, the specific implementation method of the present invention is as follows:

1、捷联惯性导航系统的准备1. Preparation of strapdown inertial navigation system

SINS开机后，进入准备状态。After SINS is turned on, it enters the ready state.

2、第一位置数据采集2. First position data collection

SINS准备完毕，保持SINS在初始位置(称为第一位置)静止不动，采集5分钟陀螺仪输出和加速度计输出，如果SINS的精度较低可适当延长采数时间。After the SINS is ready, keep the SINS still at the initial position (called the first position), and collect the output of the gyroscope and the output of the accelerometer for 5 minutes. If the accuracy of the SINS is low, the data collection time can be extended appropriately.

3、第一位置姿态计算3. Calculation of the first position and attitude

导航坐标系取为东北天地理坐标系，第一位置处航向角₁、俯仰角θ₁和横滚角γ₁的定义如图2a、图2b和图2c所示。The navigation coordinate system is taken as the northeast sky geographic coordinate system. The definitions of heading angle  ₁ , pitch angle θ ₁ and roll angle γ ₁ at the first position are shown in Fig. 2a, Fig. 2b and Fig. 2c.

加速度计输出与重力加速度以及陀螺仪输出与地球自转角速率有如下关系：The accelerometer output is related to the acceleration of gravity and the gyroscope output is related to the angular rate of the Earth's rotation as follows:

$[\begin{matrix} {f f}_{x x 11} \\ {f f}_{y the y 11} \\ {f f}_{z z 11} \end{matrix}] = = {C C}_{n no}^{b b} [\begin{matrix} 00 \\ 00 \\ g g \end{matrix}] . . . . . . ((66))$

$[\begin{matrix} {ω ω}_{x x 11} \\ {ω ω}_{y the y 11} \\ {ω ω}_{z z 11} \end{matrix}] = = {C C}_{n no}^{b b} [\begin{matrix} 00 \\ {ω ω}_{ie ie} cos cos L L \\ {ω ω}_{ie ie} sin sin L L \end{matrix}] . . . . . . ((77))$

式中，f_x1、f_y1和f_z1以及ω_x1、ω_y1和ω_z1分别为第一个位置上SINS的X轴、Y轴和Z轴输出的比力和角速率；g为重力加速度；ω_ie为地球自转角速率，其在东、北、天方向上的投影为Ω＝[0 ω_iecosL ω_iesinL]，L表示当地纬度；C_n ^b为导航系到载体系的转换矩阵，可写为：In the formula, f _x1 , f _y1 and f _z1 and ω _x1 , ω _y1 and ω _z1 are the specific force and angular rate output by the X-axis, Y-axis and Z-axis of SINS at the first position, respectively; g is the gravitational acceleration; ω _ie is the earth's rotation angular rate, and its projection in the east, north, and sky directions is Ω=[0 ω _ie cosL ω _ie sinL], L represents the local latitude; C _n ^b is the transformation matrix from the navigation system to the carrier system, can be written as:

${C C}_{n no}^{b b} = = {(({C C}_{b b}^{n no}))}^{T T} = = [\begin{matrix} {C C}_{1111} & {C C}_{1313} & {C C}_{1313} \\ {C C}_{21 twenty one} & {C C}_{1212} & {C C}_{23 twenty three} \\ {C C}_{3131} & {C C}_{3232} & {C C}_{3333} \end{matrix}] . .$

由(6)、(7)式可得：From formulas (6) and (7), we can get:

${C C}_{1313} = = \frac{{f f}_{x x 11}}{g g}$

${C C}_{23 twenty three} = = \frac{{f f}_{y the y 11}}{g g}$

${C C}_{3333} = = \frac{{f f}_{z z 11}}{g g}$

${C C}_{22 twenty two} = = \frac{{ω ω}_{y the y 11}}{{ω ω}_{ie ie} cos cos L L} - - \frac{{f f}_{y the y 11} tan the tan L L}{g g} . . . . . . ((88))$

θ_1主＝arcsin(C₂₃) (9)θ _{1 main} = arcsin(C ₂₃ ) (9)

θ₁＝θ_1主 (10)θ ₁ = θ _{1 main} (10)

由(10)式确定的₁、θ₁和γ₁即为SINS在第一位置上的航向角、俯仰角和横滚角。 ₁ , θ ₁ and γ ₁ determined by formula (10) are the heading angle, pitch angle and roll angle of the SINS at the first position.

4、第二位置数据采集4. Second location data collection

通过任意方法将SINS从初始位置绕空间三维任意轴旋转到任意一个位置(称为第二位置)，保持SINS在第二位置静止不动，采集5分钟陀螺仪输出和加速度计输出。Rotate the SINS from the initial position around any three-dimensional axis to any position (called the second position) by any method, keep the SINS stationary at the second position, and collect the output of the gyroscope and the output of the accelerometer for 5 minutes.

5、对两个位置的数据进行滤波处理5. Filter the data at two locations

将₁、θ₁和γ₁作为初始参数，利用卡尔曼滤波技术，对两个位置上陀螺仪和加速度计输出的数据进行处理，精确估计出失准角φ_x、φ_y和φ_z(东北天地理坐标系下是φ_E、φ_N和φ_U)及陀螺仪常值漂移、加速度计常值偏置。Taking  ₁ , θ ₁ and γ ₁ as initial parameters, using Kalman filter technology, the data output by the gyroscope and accelerometer at two positions are processed, and the misalignment angles φ _x , φ _y and φ _z ( In the northeast sky geographic coordinate system, there are φ _E , φ _N and φ _U ), the constant value drift of the gyroscope, and the constant value bias of the accelerometer.

(1)SINS初始姿态确定误差模型的建立(1) Establishment of SINS initial attitude determination error model

导航坐标系取为东北天地理坐标系，位置和垂直速度误差省略，加速度计和陀螺仪的误差视为随机偏置加白噪声过程，此时SINS的误差模型为：The navigation coordinate system is taken as the northeast sky geographic coordinate system, the position and vertical velocity errors are omitted, and the errors of the accelerometer and gyroscope are regarded as a process of random bias plus white noise. At this time, the error model of SINS is:

$[\begin{matrix} δ δ {\overset{\cdot \cdot}{V V}}_{E E.} \\ δ δ {\overset{\cdot \cdot}{V V}}_{N N} \\ {\overset{\cdot \cdot}{φ φ}}_{E E.} \\ {\overset{\cdot &Center Dot;}{φ φ}}_{N N} \\ {\overset{\cdot \cdot}{φ φ}}_{U u} \\ {\overset{\cdot \cdot}{&dtri; &dtri;}}_{x x} \\ {\overset{\cdot \cdot}{&dtri; &dtri;}}_{y the y} \\ {\overset{\cdot &Center Dot;}{ϵ ϵ}}_{x x} \\ {\overset{\cdot &Center Dot;}{ϵ ϵ}}_{y the y} \\ {\overset{\cdot &Center Dot;}{ϵ ϵ}}_{z z} \end{matrix}] = = [\begin{matrix} 00 & 22 {Ω Ω}_{U u} & 00 & - - g g & 00 & {T T}_{1111} & {T T}_{1212} & 00 & 00 & 00 \\ - - 22 {Ω Ω}_{U u} & 00 & g g & 00 & 00 & {T T}_{21 twenty one} & {T T}_{22 twenty two} & 00 & 00 & 00 \\ 00 & 00 & 00 & {Ω Ω}_{U u} & - - {Ω Ω}_{N N} & 00 & 00 & {T T}_{1111} & {T T}_{1212} & {T T}_{1313} \\ 00 & 00 & - - {Ω Ω}_{U u} & 00 & 00 & 00 & 00 & {T T}_{21 twenty one} & {T T}_{22 twenty two} & {T T}_{23 twenty three} \\ 00 & 00 & - - {Ω Ω}_{N N} & 00 & 00 & 00 & 00 & {T T}_{3131} & {T T}_{3232} & {T T}_{3333} \\ 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 \\ 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 \\ 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 \\ 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 \\ 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 \end{matrix}] [\begin{matrix} δ δ {V V}_{E E.} \\ δ δ {V V}_{N N} \\ {φ φ}_{E E.} \\ {φ φ}_{N N} \\ {φ φ}_{U u} \\ {&dtri; &dtri;}_{x x} \\ {&dtri; &dtri;}_{y the y} \\ {ϵ ϵ}_{x x} \\ {ϵ ϵ}_{y the y} \\ {ϵ ϵ}_{z z} \end{matrix}] . . . . . . ((1111))$

式中，下标E、N、U分别表示东、北、天。地球自转角速率ω_ie在东、北、天方向上的投影为Ω＝[0 ω_iecosL ω_iesinL]＝[0 Ω_N Ω_U]，L表示当地纬度；下标x、y、z为载体坐标系；T_ij(i＝1，2，3；j＝1，2，3)为姿态矩阵C_b ⁿ中的元素，In the formula, the subscripts E, N, and U represent east, north, and sky, respectively. The projection of the earth's rotation angular rate ω _ie in the east, north, and sky directions is Ω=[0 ω _ie cosL ω _ie sinL]=[0 Ω _N Ω _U ], L represents the local latitude; the subscripts x, y, and z are Carrier coordinate system; T _ij (i=1,2,3; j=1,2,3) is an element in the posture matrix C _b ⁿ ,

${C C}_{b b}^{n no} {= = {{{T T}_{ij ij}}}}_{i i = = 1,2,3 1,2,3;; j j = = 1,2,3 1,2,3} . .$

(2)SINS初始姿态确定卡尔曼滤波模型的建立(2) Establishment of Kalman filter model for SINS initial attitude determination

对于捷联惯导系统，考虑到陀螺仪随机漂移误差和加速度计的随机偏差，将方程(11)修正为如下形式：For the strapdown inertial navigation system, considering the random drift error of the gyroscope and the random deviation of the accelerometer, the equation (11) is modified into the following form:

$[[\frac{{\overset{\cdot \cdot}{X x}}_{a a} ((t t))}{{\overset{\cdot &Center Dot;}{X x}}_{b b} ((t t))}]] = = [\begin{matrix} F f & {T T}_{} & i i \\ 00_{55 \times \times 55} & 00_{55 \times \times 55} \end{matrix}] [[\frac{{X x}_{a a} ((t t))}{{X x}_{b b} ((t t))}]] + + [[\frac{{W W}^{' '} ((t t))}{00_{55 \times \times 11}}]] = = {A A}_{i i} X x ((t t)) + + W W ((t t)) . . . . . . ((1212))$

式中，W(t)为N(O，Q)的高斯白噪声；状态变量X_a＝[δV_E δV_N φ_E φ_N φ_U]^T，X_b＝[_x_yε_xε_yε_z]^T；随机噪声状态矢量 $W^{'} (t) = {[\begin{matrix} w_{δ V_{E}} & w_{δ V_{N}} & w_{φE} & w_{φN} & w_{φU} \end{matrix}]}^{T},$ 其中，δV_E、δV_N分别为东向和北向速度误差，φ_E、φ_N为水平失准角；φ_U为方位失准角；_x、_y为加速度计随机常值偏置，ε_x、ε_y、ε_z是陀螺仪随机常值漂移；0_5×5和0_5×1均为指定维数的零矩阵；F和T_i代表的内容如下：In the formula, W(t) is Gaussian white noise of N(O, Q); state variable X _a ＝[δV _E δV _N φ _E φ _N φ _U ] ^T , X _b ＝[ _x  _y ε _x ε _y ε _z ] ^T ; random noise state vector $W^{'} (t) = {[\begin{matrix} w_{δ V_{E.}} & w_{δ V_{N}} & w_{φE} & w_{φN} & w_{φU} \end{matrix}]}^{T},$ Among them, δV _E , δV _N are eastward and northward velocity errors respectively, φ _E , φ _N are horizontal misalignment angles; φ _U is azimuth misalignment angle;  _x ,  _y are random constant biases of accelerometers, ε _x , ε _y , ε _z are the random constant value drift of the gyroscope; 0 _5×5 and 0 _5×1 are zero matrices with specified dimensions; the contents represented by F and T _i are as follows:

${T T}_{i i} = = [\begin{matrix} {T T}_{1111} & {T T}_{1212} & 00 & 00 & 00 \\ {T T}_{21 twenty one} & {T T}_{22 twenty two} & 00 & 00 & 00 \\ 00 & 00 & {T T}_{1111} & {T T}_{1212} & {T T}_{1313} \\ 00 & 00 & {T T}_{21 twenty one} & {T T}_{22 twenty two} & {T T}_{23 twenty three} \\ 00 & 00 & {T T}_{3131} & {T T}_{3232} & {T T}_{3333} \end{matrix}],,$ $F f = = [\begin{matrix} 00 & 22 {Ω Ω}_{U u} & 00 & - - g g & 00 \\ - - 22 {Ω Ω}_{U u} & 00 & g g & 00 & 00 \\ 00 & 00 & 00 & {Ω Ω}_{U u} & - - {Ω Ω}_{N N} \\ 00 & 00 & - - {Ω Ω}_{U u} & 00 & 00 \\ 00 & 00 & {Ω Ω}_{N N} & 00 & 00 \end{matrix}]$

(12)式为捷联惯导系统初始姿态确定卡尔曼滤波模型的系统方程。为了应用卡尔曼滤波器进行状态向量的最优估计，还需建立系统观测方程。选取两个水平速度误差为观测量，建立的系统观测方程为Equation (12) is the system equation of the Kalman filter model for determining the initial attitude of the strapdown inertial navigation system. In order to apply the Kalman filter to the optimal estimation of the state vector, it is necessary to establish the system observation equation. Two horizontal velocity errors are selected as observation quantities, and the established system observation equation is

$Z Z ((t t)) = = [\begin{matrix} 11 & 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 \\ 00 & 11 & 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 \end{matrix}] [\begin{matrix} {X x}_{a a} \\ {X x}_{b b} \end{matrix}] + + [\begin{matrix} {η η}_{E E.} \\ {η η}_{N N} \end{matrix}] = = HX HX ((t t)) + + η η ((t t)) . . . . . . ((1313))$

其中，η(t)是系统观测噪声向量，为N(O，R)的高斯白噪声过程。Among them, η(t) is the system observation noise vector, which is a Gaussian white noise process of N(O, R).

(3)离散卡尔曼滤波模型的建立(3) Establishment of discrete Kalman filter model

根据上述系统方程和观测方程，可建立离散卡尔曼滤波方程如下。According to the above system equation and observation equation, the discrete Kalman filter equation can be established as follows.

滤波方程：Filtering equation:

增益方程：Gain equation:

${K K}_{k k} = = {P P}_{k k,, k k - - 11} {H h}_{k k}^{T T} {[[{H h}_{k k} {P P}_{k k,, k k - - 11} {H h}_{k k}^{T T} + + {R R}_{k k}]]}^{- - 11} . . . . . . ((1515))$

预报误差方差方程：Forecast error variance equation:

${P P}_{k k,, k k - - 11} = = {Φ Φ}_{k k,, k k - - 11} {P P}_{k k - - 11} {Φ Φ}_{k k,, k k - - 11}^{T T} + + {Q Q}_{k k - - 11} . . . . . . ((1616))$

滤波误差方差方程：Filtering error variance equation:

P_k＝[I-K_kH_k]P_k，k-1 (17)P _k = [IK _k H _k ] P _{k, k-1} (17)

式中，Φ_k，k-1为离散化的状态转移矩阵(系统矩阵)，Q、R分别为系统噪声和观测噪声的协方差矩阵。In the formula, Φ _{k, k-1} is the discretized state transition matrix (system matrix), Q, R are the covariance matrix of system noise and observation noise respectively.

(4)滤波初始条件(4) Filtering initial conditions

X(0)＝0_10×1；X(0)=0 _10×1 ;

P(0)、Q和R对应中等精度SINS的取值如下：The values of P(0), Q and R corresponding to medium-precision SINS are as follows:

P(0)＝diag{(0.3m/s)²，(0.3m/s)²，(30′)²，(10″)²，(10″)²，P(0)=diag{(0.3m/s) ² , (0.3m/s) ² , (30′) ² , (10″) ² , (10″) ² ,

(100μg)²，(100μg)²，(0.1°)²，(0.1°)²，(0.1°)²}；(100μg) ² , (100μg) ² , (0.1°) ² , (0.1°) ² , (0.1°) ² };

Q＝diag{(100μg)²，(100μg)²，(0.1°)²，(0.1°)²，(0.1°)²，0，0，0，0，0}；Q=diag{(100 μg) ² , (100 μg) ² , (0.1°) ² , (0.1°) ² , (0.1°) ² , 0, 0, 0, 0, 0};

R＝diag{(0.1m/s)²，(0.1m/s)²}。R=diag{(0.1m/s) ² , (0.1m/s) ² }.

6、计算陀螺仪随机常值漂移和加速度计随机常值偏置6. Calculate the random constant value drift of the gyroscope and the random constant value bias of the accelerometer

滤波器估计出的_x、_y即为加速度计随机常值偏置，ε_x、ε_y、ε_z为陀螺仪随机常值漂移。The  _x ,  _y estimated by the filter are the random constant bias of the accelerometer, and ε _x , ε _y , ε _z are the random constant drift of the gyroscope.

7、计算载体坐标系b与计算地理坐标系n′之间的转换矩阵C_b ^n′ 7. Calculate the transformation matrix C _b ^n' between the carrier coordinate system b and the geographic coordinate system n'

可利用陀螺仪输出的角增量或角速度信息，采用四元数方法计算C_b ^n′，计算步骤如下：The angular increment or angular velocity information output by the gyroscope can be used to calculate C _b ^n′ using the quaternion method, and the calculation steps are as follows:

(1)利用航向角₁、俯仰角θ₁和横滚角γ₁初始化第一个位置时的姿态，计算初始转换矩阵C_b ^n′和四元数q，计算公式如下：(1) Use the heading angle  ₁ , pitch angle θ ₁ and roll angle γ ₁ to initialize the attitude of the first position, and calculate the initial transformation matrix C _b ^n′ and quaternion q, the calculation formula is as follows:

令 $C_{b}^{n^{'}} = [\begin{matrix} T_{11} & T_{12} & T_{13} \\ T_{21} & T_{22} & T_{23} \\ T_{31} & T_{32} & T_{33} \end{matrix}]$ make $C_{b}^{{no}^{'}} = [\begin{matrix} T_{11} & T_{12} & T_{13} \\ T_{twenty one} & T_{twenty two} & T_{twenty three} \\ T_{31} & T_{32} & T_{33} \end{matrix}]$

则有：Then there are:

${q q}_{00} = = &PlusMinus; &PlusMinus; \frac{11}{22} \sqrt{11 + + {T T}_{1111} + + {T T}_{22 twenty two} - - {T T}_{3333}}$

${q q}_{11} = = &PlusMinus; &PlusMinus; \frac{11}{22} \sqrt{11 + + {T T}_{1111} + + {T T}_{22 twenty two} - - {T T}_{3333}}$

${q q}_{22} = = &PlusMinus; &PlusMinus; \frac{11}{22} \sqrt{11 + + {T T}_{1111} + + {T T}_{22 twenty two} - - {T T}_{3333}}$

${q q}_{33} = = &PlusMinus; &PlusMinus; \frac{11}{22} \sqrt{11 + + {T T}_{1111} + + {T T}_{22 twenty two} - - {T T}_{3333}}$

(2)更新四元数q₀、q₁、q₂、q₃和转换矩阵C_b ^n′ (2) Update quaternions q ₀ , q ₁ , q ₂ , q ₃ and transformation matrix C _b ^n′

$q q ((n no + + 11)) = = {{((11 - - \frac{{((Δ Δ {θ θ}_{00}))}^{22}}{88} + + \frac{{((Δ Δ {θ θ}_{00}))}^{44}}{384384})) I I + + ((\frac{11}{22} - - \frac{{((Δ Δ {θ θ}_{00}))}^{22}}{4848})) ((Δθ Δθ))}} q q ((n no)) . . . . . . ((1919))$

其中，in,

$Δθ Δθ = = [\begin{matrix} 00 & - - Δ Δ {θ θ}_{x x} & - - Δ Δ {θ θ}_{y the y} & - - {Δθ Δθ}_{z z} \\ {Δθ Δθ}_{x x} & 00 & {Δθ Δθ}_{z z} & - - {Δθ Δθ}_{y the y} \\ {Δθ Δθ}_{y the y} & {- - Δθ Δθ}_{z z} & 00 & {Δθ Δθ}_{x x} \\ {Δθ Δθ}_{z z} & {Δθ Δθ}_{y the y} & {- - Δθ Δθ}_{x x} & 00 \end{matrix}]$

$Δ Δ {θ θ}_{00} = = \sqrt{{Δθ Δθ}_{x x}^{22} + + {Δθ Δθ}_{y the y}^{22} + + {Δθ Δθ}_{z z}^{22}}$

转换矩阵C_b ^n′的更新公式如下：The update formula of the conversion matrix C _b ^n′ is as follows:

${C C}_{b b}^{{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}] . . . . . . ((2020))$

8、计算航向角₂、俯仰角θ₂和横滚角γ₂ 8. Calculate heading angle  ₂ , pitch angle θ ₂ and roll angle γ ₂

利用卡尔曼滤波估计出的φ_E、φ_N、φ_U计算真实地理坐标系n与计算地理坐标系n′之间的转换矩阵C_n′ ⁿ。根据C_n′ ⁿ和(20)式计算出的C_b ^n′，计算载体坐标系b与真实地理坐标系n之间的转换矩阵C_b ⁿ，再由C_b ⁿ计算第二位置的航向角₂、俯仰角θ₂和横滚角γ₂，将其作为SINS的初始姿态。具体计算步骤如下：Use φ _E , φ _N , φ _U estimated by Kalman filter to calculate the conversion matrix C _n′ ⁿ between the real geographic coordinate system n and the calculated geographic coordinate system n′. According ^to C _n′ ⁿ and C _b ^n′ calculated by formula (20), calculate the conversion matrix C b ⁿ between the carrier coordinate system b and the real geographic coordinate system n, and then use C _b _n to calculate the heading angle of the second position  ₂ , pitch angle θ ₂ and roll angle γ ₂ , which are used as the initial attitude of SINS. The specific calculation steps are as follows:

(1)计算真实地理坐标系n与计算地理坐标系n′之间的转换矩阵C_n ^n′ (1) Calculate the conversion matrix C _{n n} ' between the real geographic coordinate system n and the calculated geographic coordinate system n ^'

${C C}_{{n no}^{' '}}^{n no} = = [\begin{matrix} 11 & - - {φ φ}_{U u} & {φ φ}_{N N} \\ {φ φ}_{U u} & 11 & - - {φ φ}_{E E.} \\ - - {φ φ}_{N N} & {φ φ}_{E E.} & 11 \end{matrix}] . . . . . . ((21 twenty one))$

(2)计算载体坐标系b与真实地理坐标系n之间的转换矩阵C_b ⁿ (2) Calculate the transformation matrix C _b ⁿ between the carrier coordinate system b and the real geographic coordinate system n

${C C}_{b b}^{n no} = = {C C}_{{n no}^{' '}}^{n no} {C C}_{b b}^{{n no}^{' '}} . . . . . . ((22 twenty two))$

(3)计算航向角₂、俯仰角θ₂和横滚角γ₂ (3) Calculate heading angle  ₂ , pitch angle θ ₂ and roll angle γ ₂

航向角₂、俯仰角θ₂和横滚角γ₂的定义如图2a、图2b和图2c所示。The definitions of heading angle  ₂ , pitch angle θ ₂ and roll angle γ ₂ are shown in Fig. 2a, Fig. 2b and Fig. 2c.

将(22)式计算出的C_b ⁿ记为The C _b ⁿ calculated by formula (22) is recorded as

${C C}_{b b}^{n no} = = [\begin{matrix} {T T}_{1111} & {T T}_{1212} & {T T}_{1313} \\ {T T}_{21 twenty one} & {T T}_{22 twenty two} & {T T}_{23 twenty three} \\ {T T}_{3131} & {T T}_{3232} & {T T}_{3333} \end{matrix}] . . . . . . ((23 twenty three))$

又因为also because

因此，由(23)式和(24)式，可以确定航向角₂、俯仰角θ₂和横滚角γ₂的主值，即Therefore, from equations (23) and (24), the main values of heading angle  ₂ , pitch angle θ ₂ and roll angle γ ₂ can be determined, namely

若航向角₂、俯仰角θ₂和横滚角γ₂的取值范围分别定义为[0，2π]、

[-π，+π]。那么，₂、θ₂和γ₂的真值可由下式确定：If the value ranges of heading angle  ₂ , pitch angle θ ₂ and roll angle γ ₂ are respectively defined as [0, 2π],

[-π, +π]. Then, the true values of  ₂ , θ ₂ and γ ₂ can be determined by the following formula:

θ₂＝θ_2主 (26)θ ₂ = θ _{2 main} (26)

由(26)式确定的₂、θ₂和γ₂即为SINS在第二位置上的航向角、俯仰角和横滚角，将其作为SINS进入导航工作状态的初始姿态。 ₂ , θ ₂ and γ ₂ determined by formula (26) are the heading angle, pitch angle and roll angle of the SINS at the second position, which are taken as the initial attitude of the SINS to enter the navigation working state.

Claims

1. A strap-down inertial navigation system initial attitude determination method is characterized by comprising the following steps:

(1) after the SINS preheating preparation is finished, keeping the SINS at an initial position, namely a first position, standing still, and collecting gyroscope output and accelerometer output;

(2) calculating a heading angle * for the first location based on the relationship of the accelerometer output to gravitational acceleration and the relationship of the gyroscope output to the angular rate of rotation of the earth₁Angle of pitch theta₁And roll angle γ₁；

(3) Rotating the SINS from a first position to any position around a spatial three-dimensional arbitrary axis, wherein the position is called a second position, keeping the SINS still at the second position, and collecting gyroscope output and accelerometer output;

(4) using kalman filtering technique, *₁、θ₁And gamma₁Processing data output by the gyroscope and the accelerometer on two positions as initial parameters, and calculating an error angle between a geographic coordinate system n' and a real geographic coordinate system n when estimating a second position, which is referred to as a misalignment angle phi for short_x、φ_yAnd phi_zGyroscope constant drift and accelerometer constant bias;

(5) phi estimated using Kalman filtering_x、φ_yAnd phi z calculating a transformation matrix C between the real geographic coordinate system n and the calculated geographic coordinate system n_n＇ ⁿAccording to the angular increment or angular speed information output by the gyroscope, a quaternion method is adopted to calculate a conversion matrix C between a carrier coordinate system b and a geographical coordinate system n_b ^n′Then from C_b ⁿCalculating a heading angle * for the second location₂Angle of pitch theta₂And roll angle γ₂This is taken as the initial attitude of the SINS.