CN104374401A

CN104374401A - Compensating method of gravity disturbance in strapdown inertial navigation initial alignment

Info

Publication number: CN104374401A
Application number: CN201410542256.5A
Authority: CN
Inventors: 何昆鹏; 王晓雪; 王刚; 王晨阳; 韩继韬; 邵玉萍; 张兴智; 张琳; 胡璞; 徐旭
Original assignee: Harbin Engineering University
Current assignee: Harbin Engineering University
Priority date: 2014-10-15
Filing date: 2014-10-15
Publication date: 2015-02-25

Abstract

The invention belongs to the field of error compensation in initial alignment, and in particular relates to a compensation method for gravity disturbance in initial alignment of strapdown inertial navigation. The invention includes: collecting the angular velocity data output by the fiber optic gyroscope and the acceleration data output by the quartz accelerometer; calculating the gravity disturbance value of the alignment site through the gravity disturbance position, and compensating the output acceleration data of the quartz accelerometer; using an analytical method to complete For coarse alignment, select the initial value of the filter; estimate the misalignment angle of the platform; and complete the precise initial alignment. The method for compensating the gravitational disturbance during the initial alignment of the strapdown inertial navigation system in the invention is to calculate the gravitational disturbance value of the alignment point through the EGM2008 model based on the known latitude and longitude values, and then remove the calculated gravitational disturbance value from the accelerometer. The output of the accelerometer after gravity disturbance compensation is obtained, and the simulation results show that the initial alignment accuracy can be improved after gravity disturbance compensation.

Description

A Compensation Method for Gravity Disturbance in Strapdown Inertial Navigation Initial Alignment

技术领域 technical field

本发明属于初始对准中误差补偿领域，具体涉及一种捷联惯导初始对准中重力扰动的补偿方法。 The invention belongs to the field of error compensation in initial alignment, and in particular relates to a compensation method for gravity disturbance in initial alignment of strapdown inertial navigation. the

背景技术 Background technique

初始对准是捷联惯性导航系统关键技术之一。初始对准精度直接影响捷联惯性导航系统的工作精度。捷联惯性导航系统初始对准的主要目的是建立姿态矩阵的初值，初始对准中通过初始对准状态空间模型，利用卡尔曼滤波将初始失准角状态估计出来并用以校正姿态矩阵。传统的对准过程包括粗对准和精对准两个阶段，首先用粗对准模型粗略估计出失准角的大小，然后再利用精对准模型估计出失准角的大小而实现精对准。捷联惯性导航系统的严格数学误差模型是一组非线性微分方程，而实际粗对准的失准角在很多情况下为大角度，因此采用非线性模型更能真实的反映误差传播特性。 Initial alignment is one of the key technologies of strapdown inertial navigation system. The initial alignment accuracy directly affects the working accuracy of the SINS. The main purpose of the initial alignment of the strapdown inertial navigation system is to establish the initial value of the attitude matrix. In the initial alignment, the initial alignment state space model is used to estimate the initial misalignment angle state by Kalman filter and used to correct the attitude matrix. The traditional alignment process includes two stages: coarse alignment and fine alignment. First, the rough alignment model is used to roughly estimate the size of the misalignment angle, and then the fine alignment model is used to estimate the size of the misalignment angle to achieve fine alignment. allow. The strict mathematical error model of the strapdown inertial navigation system is a set of nonlinear differential equations, and the misalignment angle of the actual coarse alignment is a large angle in many cases, so the nonlinear model can reflect the error propagation characteristics more truly. the

惯性导航解算时通常用的是正常重力值，正常重力值是将地球近似为一个质量均匀的旋转椭球模型而得到的重力值；而实际由于地球内部质量分布不均匀，重力矢量包括正常重力和重力扰动。由于不同地区地球内部质量分布不同，这些重力扰动在地表或者海平面上是变化的。惯性加速度计不能区分地球重力的切线方向分量和机体的水平加速度，因此这些重力扰动会引起惯导系统解算误差。在惯性导航中，使用加速度计测量载体位置的比力矢量，比力矢量是测量点绝对加速度与重力加速度之差为：从测得的比力中补偿掉引力加速度，就可以得到载体的绝对加速度，再根据绝对加速度和相对加速度的关系，惯导系统可进一步求得载体的相对速度、位置。显然，如果航迹上的重力加速度不能真实的反映实际重力加速度，那么将会导致测量点加速度的误差，进一步导致导航解算误差。随着惯性器件精度不断提高与高精度惯性导航系统需求的发展,重力扰动成为高精度惯导初始对准的一项主要误差源。初始对准的精度的高低直接影响到惯性导航的精度，要实现高精度的惯性导航有必要补偿初始对准中的重力扰动。 The inertial navigation solution usually uses the normal gravity value, which is the gravity value obtained by approximating the earth as a rotating ellipsoid model with uniform mass; in fact, due to the uneven distribution of the internal mass of the earth, the gravity vector includes the normal gravity and gravitational disturbances. These gravitational disturbances vary on the surface or sea level due to the different distribution of mass in the Earth's interior in different regions. Inertial accelerometers cannot distinguish between the tangential component of the earth's gravity and the horizontal acceleration of the body, so these gravitational disturbances will cause error in the inertial navigation system solution. In inertial navigation, an accelerometer is used to measure the specific force vector of the carrier position, which is the absolute acceleration of the measurement point with the acceleration of gravity The difference is: The absolute acceleration of the carrier can be obtained by compensating the gravitational acceleration from the measured specific force, and then according to the relationship between the absolute acceleration and the relative acceleration, the inertial navigation system can further obtain the relative velocity and position of the carrier. Obviously, if the gravitational acceleration on the track If it cannot truly reflect the actual gravitational acceleration, it will lead to an error in the acceleration of the measurement point, which will further lead to an error in the navigation solution. With the continuous improvement of the precision of inertial devices and the development of high-precision inertial navigation systems, gravity disturbance has become a major error source for high-precision inertial navigation initial alignment. The accuracy of initial alignment directly affects the accuracy of inertial navigation. To achieve high-precision inertial navigation, it is necessary to compensate the gravitational disturbance in initial alignment.

发明内容 Contents of the invention

本发明的目的是为了提高初始对准的精度，补偿初始对准时存在的重力扰动，提出用地球重力场模型EGM2008计算重力扰动并加以补偿的捷联惯导初始对准中重力扰动的补偿方法。 The purpose of the present invention is to improve the accuracy of the initial alignment, to compensate for the gravitational disturbance that exists during the initial alignment, and to propose a compensation method for the gravitational disturbance in the initial alignment of the strapdown inertial navigation system that uses the earth's gravity field model EGM2008 to calculate the gravitational disturbance and compensate for it. the

本发明的目的是这样实现的： The purpose of the present invention is achieved like this:

(1)采集光纤陀螺仪输出的角速度数据和石英加速度计输出的加速度数据； (1) Gather the angular velocity data output by the fiber optic gyroscope and the acceleration data output by the quartz accelerometer;

(2)通过重力扰动位计算对准地点的重力扰动值，对石英加速度计的输出加速度数据进行补偿； (2) Compensate the output acceleration data of the quartz accelerometer by calculating the gravitational disturbance value at the alignment site through the gravitational disturbance position;

(3)采用解析法来完成粗对准，通过初始矩阵确定载体的姿态信息即纵摇角、横摇角、航向角，其中b代表载体坐标系，n′代表计算导航坐标系； (3) Analytical method is used to complete the coarse alignment, through the initial matrix Determine the attitude information of the carrier, that is, pitch angle, roll angle, and heading angle, where b represents the carrier coordinate system, and n' represents the calculation navigation coordinate system;

(4)选取滤波初始值 (4) Select the initial value of the filter

${\overset{^^}{x x}}_{00} = = E E. [[{x x}_{00}]],, {P P}_{00} = = E E. [[(({x x}_{00} - - {\overset{^^}{x x}}_{00})) {(({x x}_{00} - - {\overset{^^}{x x}}_{00}))}^{T T}]],,$

其中x₀为状态变量的初值,P₀为状态变量的初始误差协方差矩阵； Where x ₀ is the initial value of the state variable, P ₀ is the initial error covariance matrix of the state variable;

(5)利用无迹卡尔曼滤波方法进行滤波，估计出平台失准角； (5) Use the unscented Kalman filter method to filter and estimate the platform misalignment angle;

(6)利用估计出来的平台失准角修正系统的捷联初始矩阵得到精确的初始捷联矩阵即从而完成精确的初始对准。 (6) Using the estimated platform misalignment angle to correct the strapdown initial matrix of the system get the exact initial strapdown matrix Right now Accurate initial alignment is thus accomplished.

重力扰动位T(ρ,L,λ)， Gravity disturbance position T(ρ,L,λ),

$T T ((ρ ρ,, L L,, λ λ)) = = \frac{GM GM}{ρ ρ} {Σ Σ}_{n no = = 22}^{N N} {((\frac{R R}{ρ ρ}))}^{n no} {Σ Σ}_{m m = = 00}^{n no} [[{C C}_{nm nm} cos cos mλ mλ + + {S S}_{nm nm} sin sin mλ mλ]] {P P}_{nm nm} ((cos cos θ θ)),,$

地球平均半径为R，θ＝90-L是地心余纬度，位系数C_nm,S_nm是已知的重力扰动的系数，可通过EGM2008重力场模型获得，L表示地球纬度，λ为地球经度，ρ为对准地点处的地心向径，GM为地球常系数，P_nm(·)为缔合勒让德多项式： The average radius of the earth is R, θ=90-L is the co-latitude of the earth's center, the potential coefficient C _nm and S _nm are the coefficients of the known gravitational disturbance, which can be obtained through the EGM2008 gravity field model, L represents the latitude of the earth, and λ is the longitude of the earth , ρ is the geocentric radius at the alignment point, GM is the Earth constant coefficient, and P _nm (·) is the associated Legendre polynomial:

${P P}_{nm nm} ((x x)) = = {((11 - - {x x}^{22}))}^{m m / / 22} {Σ Σ}_{k k = = 00}^{N N} \frac{{((- - 11))}^{k k} ((22 n no - - 22 k k))!!}{22^{n no} k k!! ((n no - - k k))!! ((n no - - m m - - 22 k k))!!} {x x}^{n no - - m m - - 22 k k},,$

x为变量，|x|＜1；n称为阶，m称为次，N＝[(n-m)2]； x is a variable, |x|<1; n is called order, m is called order, N=[(n-m)2];

重力扰动位的梯度即为重力扰动矢量δgⁿ： The gradient of the gravitational disturbance potential is the gravitational disturbance vector δg ⁿ :

$δ δ {g g}^{n no} = = grad grad dT dT ((ρ ρ,, L L,, λ λ)) = = {((\frac{&PartialD; &PartialD; T T}{&PartialD; &PartialD; ρ ρ},, \frac{&PartialD; &PartialD; T T}{&PartialD; &PartialD; L L},, \frac{&PartialD; &PartialD; T T}{&PartialD; &PartialD; λ λ}))}^{T T},,$

通过重力扰动矢量得到极坐标系下的重力扰动值： Obtain the gravity disturbance value in the polar coordinate system through the gravity disturbance vector:

${T T}_{ρ ρ} = = \frac{&PartialD; &PartialD; T T}{&PartialD; &PartialD; ρ ρ} = = \frac{GM GM}{ρ ρ} {Σ Σ}_{n no = = 22}^{\infty \infty} ((n no + + 11)) {((\frac{a a}{ρ ρ}))}^{n no} {Σ Σ}_{m m = = 00}^{n no} [[{\overset{&OverBar; &OverBar;}{C C}}_{nm nm} cos cos mλ mλ + + {\overset{&OverBar; &OverBar;}{S S}}_{nm nm} sin sin mλ mλ]] {\overset{&OverBar; &OverBar;}{P P}}_{nm nm} ((sin sin L L))$ ${T T}_{L L} = = \frac{&PartialD; &PartialD; T T}{&PartialD; &PartialD; L L} = = - - \frac{GM GM}{ρ ρ} {Σ Σ}_{n no = = 22}^{\infty \infty} {((\frac{a a}{ρ ρ}))}^{n no} {Σ Σ}_{m m = = 00}^{n no} [[{\overset{&OverBar; &OverBar;}{C C}}_{nm nm} cos cos mλ mλ + + {\overset{&OverBar; &OverBar;}{S S}}_{nm nm} sin sin mλ mλ]] \frac{d d {\overset{&OverBar; &OverBar;}{P P}}_{nm nm} ((sin sin L L))}{dL L}$

${T T}_{λ λ} = = \frac{&PartialD; &PartialD; T T}{&PartialD; &PartialD; λ λ} = = \frac{GM GM}{ρ ρ} {Σ Σ}_{n no = = 22}^{\infty \infty} {((\frac{a a}{ρ ρ}))}^{n no} {Σ Σ}_{m m = = 00}^{n no} [[- - {\overset{&OverBar; &OverBar;}{C C}}_{nm nm} sin sin mλ mλ + + {\overset{&OverBar; &OverBar;}{S S}}_{nm nm} cos cos mλ mλ]] {\overset{&OverBar; &OverBar;}{P P}}_{nm nm} ((sin sin L L))$

在东北天-ENU地理坐标系上，重力扰动矢量3个轴向的分量为转换到地理坐标系为： On the northeast sky-ENU geographic coordinate system, the three axial components of the gravitational disturbance vector are Convert to geographic coordinate system as:

${δg δg}_{E E.}^{n no} = = \frac{{T T}_{λ λ}}{ρ ρ cos cos L L},, {δg δg}_{N N}^{n no} = = \frac{{T T}_{L L}}{ρ ρ},, δ δ {g g}_{U u}^{n no} = = {T T}_{ρ ρ},,$

然后对加速度计的输出进行补偿。 The output of the accelerometer is then compensated. the

初始矩阵： Initial matrix:

θ₀是纵摇角，γ₀是横摇角，是航向角。 θ ₀ is the pitch angle, γ ₀ is the roll angle, is the heading angle.

状态变量是平台失准角、水平速度误差。 The state variables are platform misalignment angle and horizontal velocity error. the

步骤(5)包括： Step (5) includes:

(5.1)构造2n+1维的Sigma点 (5.1) Constructing 2n+1-dimensional Sigma points

$\{\begin{matrix} {ξ ξ}_{00} = = {\overset{^^}{x x}}_{k k - - 11} \\ {ξ ξ}_{i i} = = {\overset{^^}{x x}}_{k k - - 11} + + \sqrt{((m m + + κ κ)) {P P}_{k k - - 11}} \\ {ξ ξ}_{i i + + n no} = = {\overset{^^}{x x}}_{k k - - 11} - - \sqrt{((m m + + κ κ)) {P P}_{k k - - 11}} \end{matrix},, i i = = 1,2 1,2,, . . . . . .,, m m$

其中为k-1时刻的状态估计，P_k-1为k-1时刻的状态误差协方差矩阵，m为系统状态变量的维数，κ为比例系数，可用于调节Sigma点和的距离； in is the state estimation at time k-1, P _k-1 is the state error covariance matrix at time k-1, m is the dimension of system state variables, and κ is a proportional coefficient, which can be used to adjust the Sigma point and distance;

(5.2)确定权值 (5.2) Determining the weight value

${w w}_{i i}^{m m} = = {w w}_{i i}^{c c} = = \{\begin{matrix} κ κ / / ((m m + + κ κ)) & ((i i = = 00)) \\ 11 / / [[22 ((m m + + κ κ))]] & ((i i &NotEqual; &NotEqual; 00)) \end{matrix}$

(5.3)通过滤波方程进行时间更新 (5.3) Time update by filtering equation

γ_k/k-1＝f_k-1(ξ_k-1) γ _k/k-1 = f _k-1 (ξ _k-1 )

${\overset{^^}{x x}}_{k k / / k k - - 11} = = {Σ Σ}_{i i = = 00}^{22 n no} {w w}_{i i} {γ γ}_{i i,, k k / / k k - - 11}$

${P P}_{k k / / k k - - 11} = = {Σ Σ}_{i i = = 00}^{22 n no} {w w}_{i i} (({γ γ}_{i i,, k k / / k k - - 11} - - {\overset{^^}{x x}}_{k k / / k k - - 11})) {(({γ γ}_{i i,, k k / / k k - - 11} - - {\overset{^^}{x x}}_{k k / / k k - - 11}))}^{T T} + + {Q Q}_{k k - - 11}$

由和P_k-1计算Sigma点ξ_k-1，通过非线性状态函数f_k-1(·)传播为γ_k/k-1，由γ_k/k-1可得状态预测和预测误差协方差阵P_k/k-1，Q_k-1为系统噪声； Depend on and P _k-1 to calculate the Sigma point ξ _k-1 , which is propagated as γ k _/k-1 through the nonlinear state function f _k-1 (·), and the state prediction can be obtained from γ _k/k-1 and prediction error covariance matrix P _k/k-1 , Q _k-1 is system noise;

(5.4)通过滤波方程进行量测更新 (5.4) Measurement update through filtering equation

K_k＝P_k/k-1H^T(HP_k/k-1H^T+R_k)^-1 K _k ＝P _k/k-1 H ^T (HP _k/k-1 H ^T +R _k ) ^-1

${\overset{^^}{x x}}_{k k} = = {\overset{^^}{x x}}_{k k / / k k - - 11} + + {K K}_{k k} (({Z Z}_{k k} - - H h {\overset{^^}{x x}}_{k k / / k k - - 11}))$

P_k＝(I-K_kH)P_k/k-1 P _k ＝(IK _k H)P _k/k-1

其中R_k为量测噪声，K_k为滤波增益，P_k为估计误差协方差矩阵，为状态估计值其中包括平台失准角。 where R _k is the measurement noise, K _k is the filter gain, P _k is the estimation error covariance matrix, is the state estimate which includes the platform misalignment angle.

滤波方程包括状态方程 The filter equation includes the state equation

系统噪声向量 $W = {[\begin{matrix} w_{gx}^{b} & w_{gy}^{b} & w_{gz}^{b} & w_{ax}^{b} & w_{ay}^{b} \end{matrix}]}^{T}$ System noise vector $W = {[\begin{matrix} w_{gx}^{b} & w_{gy}^{b} & w_{gz}^{b} & w_{ax}^{b} & w_{ay}^{b} \end{matrix}]}^{T}$

系数阵 $G = [\begin{matrix} G_{1} & 0_{3 \times 2} \\ 0_{2 \times 3} & G_{2} \end{matrix}], G_{1} = [\begin{matrix} G_{11}^{'} & G_{12}^{'} & C_{13}^{'} \\ G_{21}^{'} & C_{22}^{'} & C_{23}^{'} \\ C_{31}^{'} & C_{32}^{'} & C_{33}^{'} \end{matrix}] G_{2} = [\begin{matrix} C_{11}^{'} & C_{12}^{'} \\ C_{21}^{'} & C_{22}^{'} \end{matrix}],$ coefficient array $G = [\begin{matrix} G_{1} & 0_{3 \times 2} \\ 0_{2 \times 3} & G_{2} \end{matrix}], G_{1} = [\begin{matrix} G_{11}^{'} & G_{12}^{'} & C_{13}^{'} \\ G_{twenty one}^{'} & C_{twenty two}^{'} & C_{twenty three}^{'} \\ C_{31}^{'} & C_{32}^{'} & C_{33}^{'} \end{matrix}] G_{2} = [\begin{matrix} C_{11}^{'} & C_{12}^{'} \\ C_{twenty one}^{'} & C_{twenty two}^{'} \end{matrix}],$

f(X)为 f(X) is

${\overset{\cdot \cdot}{φ φ}}_{x x} = = - - ((sin sin {φ φ}_{z z})) {ω ω}_{ie ie} cos cos L L + + {φ φ}_{y the y} {ω ω}_{ie ie} sin sin L L - - \frac{δ δ {v v}_{y the y}}{{R R}_{M m}} + + {C C}_{1111}^{' '} (({ϵ ϵ}_{x x}^{b b} + + {w w}_{gx gx}^{b b})) + + {C C}_{1212}^{' '} (({ϵ ϵ}_{y the y}^{b b} + + {w w}_{gy gy}^{b b})) + + {C C}_{1313}^{' '} (({ϵ ϵ}_{z z}^{b b} + + {w w}_{gz gz}^{b b}))$

${\overset{\cdot \cdot}{φ φ}}_{y the y} = = ((11 - - cos cos {φ φ}_{z z})) {ω ω}_{ie ie} cos cos L L - - {φ φ}_{x x} {ω ω}_{ie ie} sin sin L L + + \frac{δ δ {v v}_{x x}}{{R R}_{N N}} + + {C C}_{21 twenty one}^{' '} (({ϵ ϵ}_{x x}^{b b} + + {w w}_{gx gx}^{b b})) + + {C C}_{22 twenty two}^{' '} (({ϵ ϵ}_{y the y}^{b b} + + {w w}_{gy gy}^{b b})) + + {C C}_{23 twenty three}^{' '} (({ϵ ϵ}_{z z}^{b b} + + {w w}_{gz gz}^{b b}))$

${\overset{\cdot &Center Dot;}{φ φ}}_{z z} = = ((- - {φ φ}_{y the y} sin sin {φ φ}_{z z} + + {φ φ}_{x x} cos cos {φ φ}_{z z})) {ω ω}_{ie ie} cos cos L L + + \frac{δ δ {v v}_{x x} tan the tan L L}{{R R}_{N N}} + + {C C}_{3131}^{' '} (({ϵ ϵ}_{x x}^{b b} + + {w w}_{gx gx}^{b b})) + + {C C}_{3232}^{' '} (({ϵ ϵ}_{y the y}^{b b} + + {w w}_{gy gy}^{b b})) + + {C C}_{3333}^{' '} (({ϵ ϵ}_{z z}^{b b} + + {w w}_{gz gz}^{b b}))$

$\begin{matrix} δ δ {\overset{. .}{v v}}_{x x} = = - - {f f}_{x x} ((cos cos {φ φ}_{z z} - - 11)) + + {f f}_{y the y} sin sin {φ φ}_{z z} - - {f f}_{z z} (({φ φ}_{y the y} cos cos {φ φ}_{z z} + + {φ φ}_{x x} sin sin {φ φ}_{z z})) + + 22 {ω ω}_{ie ie} {δv δ v}_{y the y} sin sin L L + + \\ {C C}_{1111}^{' '} (({&dtri; &dtri;}_{x x}^{b b} + + {w w}_{ax ax}^{b b})) + + {C C}_{1212}^{' '} (({&dtri; &dtri;}_{y the y}^{b b} + + {w w}_{ay ay}^{b b})) \end{matrix}$

$\begin{matrix} δ δ {\overset{\cdot \cdot}{v v}}_{y the y} = = - - {f f}_{x x} sin sin {φ φ}_{z z} - - {f f}_{y the y} ((cos cos {φ φ}_{z z} - - 11)) - - {f f}_{z z} (({φ φ}_{y the y} sin sin {φ φ}_{z z} - - {φ φ}_{x x} cos cos {φ φ}_{z z})) - - 22 {ω ω}_{ie ie} δ δ {v v}_{x x} sin sin L L + + \\ {C C}_{21 twenty one}^{' '} (({&dtri; &dtri;}_{x x}^{b b} + + {w w}_{ax ax}^{b b})) + + {C C}_{22 twenty two}^{' '} (({&dtri; &dtri;}_{y the y}^{b b} + + {w w}_{ay ay}^{b b})) \end{matrix}$

其中，R_M,R_N分别为地球子午、卯酉曲率半径，φ_x,φ_y,φ_z为三个方向的平台失准角；δv_x,δv_y为速度误差；L为当地纬度；w_ie为地球自转角速度；为三个轴向的陀螺漂移；为陀螺零均值高斯白噪声；为三个轴向的加速度零偏；为加速度计零均值高斯白噪声；f_x,f_y,f_z为加速度输出比力在计算地理坐标系上的值；C′_ij是载体系到计算地理系矩阵中的元素； Among them, R _M and R _N are the earth's meridian and UU radius of curvature respectively; φ _x , φ _y , φ _z are the misalignment angles of the platform in three directions; δv _x , δv _y are velocity errors; L is the local latitude; w _ie is the angular velocity of the earth's rotation; is the gyro drift in three axes; is the zero-mean Gaussian white noise of the gyroscope; is the acceleration zero bias of the three axes; is the zero-mean Gaussian white noise of the accelerometer; f _x , f _y , f _z are the values of the acceleration output specific force on the calculation geographic coordinate system; C′ _ij is the element that carries the system into the calculation geographic system matrix;

量测方程为： The measurement equation is:

Z＝HX+V Z＝HX+V

其中，量测量Z为惯导水平速度误差δv_x,δv_y；H＝[I_2×2 0_2×3]，I_2×2为单位二阶矩阵，0_2×3为2行3列零矩阵；V为量测噪声。 Among them, the quantity measurement Z is the inertial navigation horizontal velocity error δv _x , δv _y ; H=[I _2×2 0 _2×3 ], I _2×2 is the unit second-order matrix, and 0 _2×3 is 2 rows and 3 columns zero matrix; V is the measurement noise.

本发明的有益效果在于：本发明建立的系统状态空间模型过程中，失准角和速度误差模型均采用非线性形式，这样可以准确的描述实际的捷联惯导系统误差传播特性；传统的惯性导航解算时通常用的是正常重力值，而实际由于地球内部质量分布不均匀，重力矢量包括正常重力和重力扰动。由于不同地区地球内部质量分布不同，这些重力扰动在地表或者海平面上是变化的。惯性加速度计不能区分重力扰动和机体的加速度，重力扰动的存在会导致一定的初始姿态误差，此误差会通过姿态解算、速度解算及位置解算影响系统各项指标的精度，这是一个不利的因素。随着惯性器件精度不断提高与高精度惯性导航系统需求的发展,重力扰动成为高精度惯导初始对准的一项主要误差源。本发明中捷联惯导初始对准中重力扰动的补偿方法是，根据已知的经纬度值并通过EGM2008模型计算出对准点的重力扰动值，然后算出的重力扰动值从加速度计中剔除掉，就得到重力扰动补偿后加速度计的输出，仿真结果表明重力扰动补偿后可提高初始对准精度。 The beneficial effects of the present invention are: in the process of the system state space model established by the present invention, the misalignment angle and velocity error models all adopt nonlinear forms, which can accurately describe the error propagation characteristics of the actual strapdown inertial navigation system; The normal gravity value is usually used in navigation calculation, but in reality, due to the uneven distribution of mass inside the earth, the gravity vector includes normal gravity and gravity disturbance. These gravitational disturbances vary on the surface or sea level due to the different distribution of mass in the Earth's interior in different regions. The inertial accelerometer cannot distinguish between the gravitational disturbance and the acceleration of the body. The existence of the gravitational disturbance will lead to a certain initial attitude error. This error will affect the accuracy of various indicators of the system through attitude calculation, velocity calculation and position calculation. This is a unfavorable factors. With the continuous improvement of the precision of inertial devices and the development of high-precision inertial navigation systems, gravity disturbance has become a major error source for high-precision inertial navigation initial alignment. The method for compensating the gravitational disturbance in the initial alignment of the strapdown inertial navigation system in the present invention is to calculate the gravitational disturbance value of the alignment point according to the known latitude and longitude values through the EGM2008 model, and then remove the calculated gravitational disturbance value from the accelerometer, The output of the accelerometer after gravity disturbance compensation is obtained, and the simulation results show that the initial alignment accuracy can be improved after gravity disturbance compensation. the

附图说明 Description of drawings

图1表示本发明的流程图。 Figure 1 shows a flow chart of the present invention. the

图2表示存在重力扰动时失准角误差曲线图 Figure 2 shows the misalignment angle error curve when there is gravity disturbance

图3表示补偿重力扰动后失准角误差曲线图。 Fig. 3 shows a curve diagram of misalignment angle error after gravitational disturbance is compensated. the

具体实施方式 Detailed ways

下面结合附图对本发明做进一步描述。 The present invention will be further described below in conjunction with the accompanying drawings. the

1、采集光纤陀螺仪和石英加速度计输出的数据； 1. Collect the data output by fiber optic gyroscope and quartz accelerometer;

2、计算对准地点的重力扰动值，然后对加速度计的输出进行补偿。地球重力场模型就是用一个截断到N阶的引力位球谐函数级数式来表示地球引力场，T(ρ,L,λ)为扰动位，地球平均半径为R，θ＝90-L是地心余纬度。 2. Calculate the gravitational disturbance value of the alignment site, and then compensate the output of the accelerometer. The earth's gravitational field model is to express the earth's gravitational field with a gravitational potential spherical harmonic function series truncated to the Nth order, T(ρ, L, λ) is the disturbance potential, the average radius of the earth is R, and θ=90-L is the earth's gravitational field. Heart latitude. the

$T T ((ρ ρ,, L L,, λ λ)) = = \frac{GM GM}{ρ ρ} {Σ Σ}_{n no = = 22}^{N N} {((\frac{R R}{ρ ρ}))}^{n no} {Σ Σ}_{m m = = 00}^{n no} [[{C C}_{nm nm} cos cos mλ mλ + + {S S}_{nm nm} sin sin mλ mλ]] {P P}_{nm nm} ((cos cos θ θ)) - - - - - - ((11))$

式中：位系数C_nm,S_nm是已知的重力扰动的系数，可通过EGM2008重力场模型获得，L表示地球纬度，λ为地球经度，ρ为计算点处的地心向径，GM为地球常系数，P_nm(·)为缔合勒让德多项式，定义式如下： In the formula: Potential coefficients C _nm and S _nm are the known coefficients of gravitational disturbance, which can be obtained through the EGM2008 gravity field model, L represents the latitude of the earth, λ is the longitude of the earth, ρ is the geocentric radius at the calculation point, and GM is The Earth's constant coefficient, P _nm (·) is the associated Legendre polynomial, and its definition is as follows:

${P P}_{nm nm} ((x x)) = = {((11 - - {x x}^{22}))}^{m m / / 22} {Σ Σ}_{k k = = 00}^{N N} \frac{{((- - 11))}^{k k} ((22 n no - - 22 k k))!!}{22^{n no} k k!! ((n no - - k k))!! ((n no - - m m - - 22 k k))!!} {x x}^{n no - - m m - - 22 k k} - - - - - - ((22))$

式中：式中：x为变量，|x|＜1；n称为阶，m称为次，N＝[(n-m)2]。 In the formula: In the formula: x is a variable, |x|<1; n is called the order, m is called the order, N=[(n-m)2]. the

结合缔合勒让德多项式，根据式(1)可获得地球重力扰动位模型T(ρ,L,λ)，该模型的梯度即为重力扰动矢量δgⁿ： Combined with the associated Legendre polynomials, the Earth’s gravitational disturbance potential model T(ρ,L,λ) can be obtained according to formula (1), and the gradient of this model is the gravitational disturbance vector δg ⁿ :

$δ δ {g g}^{n no} = = grad grad dT dT ((ρ ρ,, L L,, λ λ)) = = {((\frac{&PartialD; &PartialD; T T}{&PartialD; &PartialD; ρ ρ},, \frac{&PartialD; &PartialD; T T}{&PartialD; &PartialD; L L},, \frac{&PartialD; &PartialD; T T}{&PartialD; &PartialD; λ λ}))}^{T T} - - - - - - ((33))$

由(3)式得极坐标系下的重力扰动值： The gravitational disturbance value in the polar coordinate system is obtained from formula (3):

${T T}_{ρ ρ} = = \frac{&PartialD; &PartialD; T T}{&PartialD; &PartialD; ρ ρ} = = \frac{GM GM}{ρ ρ} {Σ Σ}_{n no = = 22}^{\infty \infty} ((n no + + 11)) {((\frac{a a}{ρ ρ}))}^{n no} {Σ Σ}_{m m = = 00}^{n no} [[{\overset{&OverBar; &OverBar;}{C C}}_{nm nm} cos cos mλ mλ + + {\overset{&OverBar; &OverBar;}{S S}}_{nm nm} sin sin mλ mλ]] {\overset{&OverBar; &OverBar;}{P P}}_{nm nm} ((sin sin L L))$

${T T}_{L L} = = \frac{&PartialD; &PartialD; T T}{&PartialD; &PartialD; L L} = = - - \frac{GM GM}{ρ ρ} {Σ Σ}_{n no = = 22}^{\infty \infty} {((\frac{a a}{ρ ρ}))}^{n no} {Σ Σ}_{m m = = 00}^{n no} [[{\overset{&OverBar; &OverBar;}{C C}}_{nm nm} cos cos mλ mλ + + {\overset{&OverBar; &OverBar;}{S S}}_{nm nm} sin sin mλ mλ]] \frac{d d {\overset{&OverBar; &OverBar;}{P P}}_{nm nm} ((sin sin L L))}{dL L} - - - - - - ((44))$

在东、北、天地理坐标系上，重力扰动矢量3个轴向的分量为由于式(4)为极坐标变换表示形式，将其转换到地理坐标系，有如下关系式： On the east, north, and sky geographic coordinate system, the three axial components of the gravitational disturbance vector are Since formula (4) is a representation form of polar coordinate transformation, it can be transformed into a geographic coordinate system, and the following relational formula is obtained:

${δg δg}_{E E.}^{n no} = = \frac{{T T}_{λ λ}}{ρ ρ cos cos L L},, {δg δ g}_{N N}^{n no} = = \frac{{T T}_{L L}}{ρ ρ},, δ δ {g g}_{U u}^{n no} = = {T T}_{ρ ρ} - - - - - - ((55))$

根据已知的经纬度值并利用式(4)、(5)可计算出对准地点的重力扰动值，然后对加速度计的输出进行补偿； According to the known latitude and longitude values and using formulas (4) and (5), the gravitational disturbance value of the alignment site can be calculated, and then the output of the accelerometer is compensated;

3、采用解析法来完成粗对准，初步确定载体的姿态信息 3. Use the analytical method to complete the rough alignment, and initially determine the attitude information of the carrier

式中分别是纵摇角，横摇角，航向角； In the formula They are pitch angle, roll angle and heading angle;

4、粗对准结束后通常水平失准角是小失准角，方位失准角是大失准角，因此建立大方位失准角非线性滤波方程； 4. After the rough alignment, usually the horizontal misalignment angle is a small misalignment angle, and the azimuth misalignment angle is a large misalignment angle, so a nonlinear filter equation for a large azimuth misalignment angle is established;

5、滤波初始化； 5. Filter initialization;

6、利用UKF滤波方法进行滤波估计出失准角； 6. Use the UKF filter method to filter and estimate the misalignment angle;

7、利用估计出来的平台失准角修正系统的捷联初始矩阵得到精确的初始捷联矩阵即从而完成精确的初始对准。 7. Use the estimated platform misalignment angle to correct the strapdown initial matrix of the system get the exact initial strapdown matrix Right now Accurate initial alignment is thus accomplished.

实施例1 Example 1

1、采集光纤陀螺仪和石英加速度计输出的数据； 1. Collect data output by fiber optic gyroscope and quartz accelerometer;

2、计算对准地点的重力扰动值，然后对加速度计的输出进行补偿。地球重力场模型就是用一个截断到N阶的引力位球谐函数级数式来表示地球引力场，T(ρ,L,λ)为扰动位，地球平均半径为R，θ＝90-L是地心余纬度。 2. Calculate the gravitational disturbance value of the alignment site, and then compensate the output of the accelerometer. The earth's gravitational field model is to express the earth's gravitational field with a gravitational potential spherical harmonic function series truncated to Nth order, T(ρ, L, λ) is the disturbance potential, the average radius of the earth is R, and θ=90-L is the earth's gravitational field. Heart latitude. the

式中：位系数C_nm,S_nm是已知的重力扰动的系数，可通过EGM2008重力场模型获得，L表示地球纬度，λ为地球经度，ρ为计算点处的地心向径，GM为地球常系数，P_nm(·)为缔合勒让德多项式，定义式如下： In the formula: Potential coefficients C _nm and S _nm are the known coefficients of gravitational disturbance, which can be obtained through the EGM2008 gravity field model, L represents the latitude of the earth, λ is the longitude of the earth, ρ is the geocentric radius at the calculation point, and GM is The Earth's constant coefficient, P _nm ( ) is the associated Legendre polynomial, and its definition is as follows:

${δg δg}_{E E.}^{n no} = = \frac{{T T}_{λ λ}}{ρ ρ cos cos L L},, {δg δg}_{N N}^{n no} = = \frac{{T T}_{L L}}{ρ ρ},, δ δ {g g}_{U u}^{n no} = = {T T}_{ρ ρ} - - - - - - ((55))$

3、采用解析法来完成系统的粗对准，初步确定载体的姿态信息 3. Use the analytical method to complete the coarse alignment of the system, and initially determine the attitude information of the carrier

4、粗对准结束后通常水平失准角是小失准角，方位失准角是大失准角，因此建立捷联惯性导航系统初始对准非线性状态方程系统噪声向量 $W = {[\begin{matrix} w_{gx}^{b} & w_{gy}^{b} & w_{gz}^{b} & w_{ax}^{b} & w_{ay}^{b} \end{matrix}]}^{T}$ 4. After the rough alignment is completed, the horizontal misalignment angle is usually a small misalignment angle, and the azimuth misalignment angle is a large misalignment angle. Therefore, the initial alignment nonlinear state equation of the strapdown inertial navigation system is established System noise vector $W = {[\begin{matrix} w_{gx}^{b} & w_{gy}^{b} & w_{gz}^{b} & w_{ax}^{b} & w_{ay}^{b} \end{matrix}]}^{T}$

系数阵 $G = [\begin{matrix} G_{1} & 0_{3 \times 2} \\ 0_{2 \times 3} & G_{2} \end{matrix}], G_{1} = [\begin{matrix} G_{11}^{'} & G_{12}^{'} & C_{13}^{'} \\ G_{21}^{'} & C_{22}^{'} & C_{23}^{'} \\ C_{31}^{'} & C_{32}^{'} & C_{33}^{'} \end{matrix}] G_{2} = [\begin{matrix} C_{11}^{'} & C_{12}^{'} \\ C_{21}^{'} & C_{22}^{'} \end{matrix}],$ f(X)为 coefficient array $G = [\begin{matrix} G_{1} & 0_{3 \times 2} \\ 0_{2 \times 3} & G_{2} \end{matrix}], G_{1} = [\begin{matrix} G_{11}^{'} & G_{12}^{'} & C_{13}^{'} \\ G_{twenty one}^{'} & C_{twenty two}^{'} & C_{twenty three}^{'} \\ C_{31}^{'} & C_{32}^{'} & C_{33}^{'} \end{matrix}] G_{2} = [\begin{matrix} C_{11}^{'} & C_{12}^{'} \\ C_{twenty one}^{'} & C_{twenty two}^{'} \end{matrix}],$ f(X) is

${\overset{\cdot &Center Dot;}{φ φ}}_{x x} = = - - ((sin sin {φ φ}_{z z})) {ω ω}_{ie ie} cos cos L L + + {φ φ}_{y the y} {ω ω}_{ie ie} sin sin L L - - \frac{δ δ {v v}_{y the y}}{{R R}_{M m}} + + {C C}_{1111}^{' '} (({ϵ ϵ}_{x x}^{b b} + + {w w}_{gx gx}^{b b})) + + {C C}_{1212}^{' '} (({ϵ ϵ}_{y the y}^{b b} + + {w w}_{gy gy}^{b b})) + + {C C}_{1313}^{' '} (({ϵ ϵ}_{z z}^{b b} + + {w w}_{gz gz}^{b b}))$

其中，R_M,R_N分别为地球子午、卯酉曲率半径，φ_x,φ_y,φ_z为三个方向的失准角(状态量)；δv_x,δv_y为速度误差(状态量)；L为当地纬度；w_ie为地球自转角速度；为三个轴向的陀螺漂移；为陀螺零均值高斯白噪声；为三个轴向的加速度零偏；为加速度计零均值高斯白噪声；f_x,f_y,f_z为加速度输出比力在计算地理坐标系上的值；C′_ij是载体系到计算地理系矩阵中的元素； Among them, R _M and R _N are the meridian of the earth and the radius of curvature of Maoyou respectively; φ _x , φ _y , φ _z are the misalignment angles in three directions (state quantities); δv _x , δv _y are velocity errors (state quantities) ; L is the local latitude; w _ie is the angular velocity of the earth's rotation; is the gyro drift in three axes; is the zero-mean Gaussian white noise of the gyroscope; is the acceleration zero bias of the three axes; is the zero-mean Gaussian white noise of the accelerometer; f _x , f _y , f _z are the values of the acceleration output specific force on the calculation geographic coordinate system; C′ _ij is the element that carries the system into the calculation geographic system matrix;

初始对准量测方程为： The initial alignment measurement equation is:

Z＝HX+V Z＝HX+V

其中，量测量Z为惯导水平速度误差δv_x,δv_y；H＝[I_2×2 0_2×3](I_2×2为单位二位矩阵，0_2×3为2行3列零矩阵)；V为量测噪声； Among them, the quantity measurement Z is the inertial navigation horizontal velocity error δv _x , δv _y ; H=[I _2×2 0 _2×3 ] (I _2×2 is a two-bit matrix of units, 0 _2×3 is 2 rows and 3 columns of zeros matrix); V is the measurement noise;

5、选取滤波初始值 5. Select the initial value of the filter

${\overset{^^}{x x}}_{00} = = E E. [[{x x}_{00}]],, {P P}_{00} = = E E. [[(({x x}_{00} - - {\overset{^^}{x x}}_{00})) {(({x x}_{00} - - {\overset{^^}{x x}}_{00}))}^{T T}]]$

其中x₀为状态变量的初值,P₀为状态变量的初始误差协方差矩阵。 Where x ₀ is the initial value of the state variable, and P ₀ is the initial error covariance matrix of the state variable.

6、利用UKF方法进行滤波估计出失准角，具体如下； 6. Use the UKF method to filter and estimate the misalignment angle, as follows;

(1)构造2n+1维的Sigma点 (1) Construct 2n+1-dimensional Sigma points

$\{\begin{matrix} {ξ ξ}_{00} = = {\overset{^^}{x x}}_{k k - - 11} \\ {ξ ξ}_{i i} = = {\overset{^^}{x x}}_{k k - - 11} + + \sqrt{((m m + + κ κ)) {P P}_{k k - - 11}},, \\ {ξ ξ}_{i i + + n no} = = {\overset{^^}{x x}}_{k k - - 11} - - \sqrt{((m m + + κ κ)) {P P}_{k k - - 11}} \end{matrix},, i i = = 1,2 1,2,, . . . . . .,, n no$

其中为k-1时刻的状态估计，P_k-1为k-1时刻的状态误差协方差矩阵，n为系统状态变量的维数，κ为比例系数，可用于调节Sigma点和的距离。 in is the state estimation at time k-1, P _k-1 is the state error covariance matrix at time k-1, n is the dimension of system state variables, κ is a proportional coefficient, which can be used to adjust the Sigma point and distance.

(2)确定权值 (2) Determine the weight value

(3)时间更新 (3) Time update

γ_k/k-1＝f_k-1(ξ_k-1) γ _k/k-1 = f _k-1 (ξ _k-1 )

按照第(2)步所选择的Sigma采样策略，由和P_k-1计算Sigma点ξ_k-1，通过非线性状态函数f_k-1(·)传播为γ_k/k-1，由γ_k/k-1可得状态预测和预测误差协方差阵P_k/k-1，Q_k-1为系统噪声。 According to the Sigma sampling strategy selected in step (2), by and P _k-1 to calculate the Sigma point ξ _k-1 , which is propagated as γ k _/k-1 through the nonlinear state function f _k-1 (·), and the state prediction can be obtained from γ _k/k-1 And prediction error covariance matrix P _k/k-1 , Q _k-1 is system noise.

(4)量测更新 (4) Measurement update

P_k＝(I-K_kH)P_k/k-1 P _k ＝(IK _k H)P _k/k-1

其中R_k为量测噪声，K_k为滤波增益，P_k为估计误差协方差矩阵 where R _k is the measurement noise, K _k is the filter gain, and P _k is the estimation error covariance matrix

8、对本发明中的方法进行matlab仿真验证： 8, the method among the present invention is carried out matlab simulation verification:

载体初始位置：北纬45.7996°，东经126.6705°，地球半径R＝6378393m，陀螺常值漂移0.001°/h，随机游走系数为0.0002°/h；初始失准φ_x＝1°,φ_y＝1°,φ_z＝10°，加速度计常值偏置10μg，零偏白噪声为5μg，惯性传感器数据产生周期为0.01s，仿真中利用提供的2.5'×2.5'分辨率的重力场模型来模拟对准点的实际重力，并利用分辨率为5'×5'的重力场模型计算来补偿重力扰动。根据所设置的参数，首先存在重力扰动的情况下进行初始对准仿真得到图2失准角误差曲线，然后利用本发明的重力扰动补偿方法可得到图3所示的东向失准角误差δφ_E,北向失准角误差δφ_N,天向失准角误差δφ_U曲线。从图2和图3的对比可以看出重力扰动补偿后可提高初始对准精度。 Initial position of the carrier: 45.7996° north latitude, 126.6705° east longitude, earth radius R=6378393m, gyro constant drift 0.001°/h, random walk coefficient 0.0002°/h; initial misalignment φ _x = 1°, φ _y = 1 °, φ _z = 10°, accelerometer constant value bias is 10μg, zero offset white noise is 5μg, inertial sensor data generation cycle is 0.01s, and the gravity field model with resolution of 2.5'×2.5' is used in the simulation to simulate Actual gravity at the point of alignment and is calculated using a gravity field model with a resolution of 5' x 5' to compensate for gravitational perturbations. According to the set parameters, firstly, the initial alignment simulation is carried out under the condition of gravity disturbance to obtain the misalignment angle error curve in Figure 2, and then the east misalignment angle error δφ shown in Figure 3 can be obtained by using the gravity disturbance compensation method of the present invention _E , North misalignment angle error δφ _N , sky misalignment angle error δφ _U curve. From the comparison of Fig. 2 and Fig. 3, it can be seen that the initial alignment accuracy can be improved after gravity disturbance compensation.

Claims

1. A kind of compensation method of gravitational disturbance in the initial alignment of strapdown inertial navigation, it is characterized in that:

(1) Gather the angular velocity data output by the fiber optic gyroscope and the acceleration data output by the quartz accelerometer;

(2) Compensate the output acceleration data of the quartz accelerometer by calculating the gravitational disturbance value at the alignment site through the gravitational disturbance position;

(3) Analytical method is used to complete the coarse alignment, through the initial matrix Determine the attitude information of the carrier, that is, pitch angle, roll angle, and heading angle, where b represents the carrier coordinate system, and n' represents the calculation navigation coordinate system;

(4) Select the initial value of the filter

{\overset{^^}{x x}}_{00} = = E E. [[{x x}_{00}]],, {P P}_{00} = = E E. [[(({x x}_{00} - - {\overset{^^}{x x}}_{00})) {(({x x}_{00} - - {\overset{^^}{x x}}_{00}))}^{T T}]] . .

Where x ₀ is the initial value of the state variable, P ₀ is the initial error covariance matrix of the state variable;

(5) Use the unscented Kalman filter method to filter and estimate the misalignment angle of the platform;

(6) Using the estimated platform misalignment angle to correct the strapdown initial matrix of the system get the exact initial strapdown matrix Right now Accurate initial alignment is thus accomplished.

2. the compensation method of gravitational disturbance in the initial alignment of a kind of strapdown inertial navigation according to claim 1, is characterized in that: described gravitational disturbance position T (ρ, L, λ),

T T ((ρ ρ,, L L,, λ λ)) = = \frac{GM GM}{ρ ρ} {Σ Σ}_{n no = = 22}^{N N} {((\frac{R R}{ρ ρ}))}^{n no} {Σ Σ}_{m m = = 00}^{n no} [[{C C}_{nm nm} cos cos mλ mλ + + {S S}_{nm nm} sin sin mλ mλ]] {P P}_{nm nm} ((cos cos θ θ)),,

The average radius of the earth is R, θ=90-L is the co-latitude of the earth's center, the potential coefficient C _nm and S _nm are the coefficients of the known gravitational disturbance, which can be obtained through the EGM2008 gravity field model, L represents the latitude of the earth, and λ is the longitude of the earth , ρ is the geocentric radius at the alignment point, GM is the Earth constant coefficient, and P _nm (·) is the associated Legendre polynomial:

{P P}_{nm nm} ((x x)) = = {((11 - - {x x}^{22}))}^{m m / / 22} {Σ Σ}_{k k = = 00}^{N N} \frac{{((- - 11))}^{k k} ((22 n no - - 22 k k))!!}{22^{n no} k k!! ((n no - - k k))!! ((n no - - m m - - 22 k k))!!} {x x}^{n no - - m m - - 22 k k},,

x is a variable, |x|<1; n is called order, m is called order, N=[(n-m)/2];

The gradient of the gravitational disturbance potential is the gravitational disturbance vector δg ⁿ :

δ δ {g g}^{n no} = = grad grad dT dT ((ρ ρ,, L L,, λ λ)) = = {((\frac{&PartialD; &PartialD; T T}{&PartialD; &PartialD; ρ ρ},, \frac{&PartialD; &PartialD; T T}{&PartialD; &PartialD; L L},, \frac{&PartialD; &PartialD; T T}{&PartialD; &PartialD; λ λ}))}^{T T},,

Obtain the gravity disturbance value in the polar coordinate system through the gravity disturbance vector:

{T T}_{ρ ρ} = = \frac{&PartialD; &PartialD; T T}{&PartialD; &PartialD; ρ ρ} = = \frac{GM GM}{ρ ρ} {Σ Σ}_{n no = = 22}^{\infty \infty} ((n no + + 11)) {((\frac{a a}{ρ ρ}))}^{n no} {Σ Σ}_{m m = = 00}^{n no} [[{\overset{&OverBar; &OverBar;}{C C}}_{nm nm} cos cos mλ mλ + + {\overset{&OverBar; &OverBar;}{S S}}_{nm nm} sin sin mλ mλ]] {\overset{&OverBar; &OverBar;}{P P}}_{nm nm} ((sin sin L L))

{T T}_{L L} = = \frac{&PartialD; &PartialD; T T}{&PartialD; &PartialD; L L} = = - - \frac{GM GM}{ρ ρ} {Σ Σ}_{n no = = 22}^{\infty \infty} {((\frac{a a}{ρ ρ}))}^{n no} {Σ Σ}_{m m = = 00}^{n no} [[{\overset{&OverBar; &OverBar;}{C C}}_{nm nm} cos cos mλ mλ + + {\overset{&OverBar; &OverBar;}{S S}}_{nm nm} sin sin mλ mλ]] \frac{d d {\overset{&OverBar; &OverBar;}{P P}}_{nm nm} ((sin sin L L))}{dL L}

{T T}_{λ λ} = = \frac{&PartialD; &PartialD; T T}{&PartialD; &PartialD; λ λ} = = \frac{GM GM}{ρ ρ} {Σ Σ}_{n no = = 22}^{\infty \infty} {((\frac{a a}{ρ ρ}))}^{n no} {Σ Σ}_{m m = = 00}^{n no} [[- - {\overset{&OverBar; &OverBar;}{C C}}_{nm nm} sin sin mλ mλ + + {\overset{&OverBar; &OverBar;}{S S}}_{nm nm} cos cos mλ mλ]] {\overset{&OverBar; &OverBar;}{P P}}_{nm nm} ((sin sin L L))

On the northeast sky-ENU geographic coordinate system, the three axial components of the gravitational disturbance vector are Convert to geographic coordinate system as:

δ δ {g g}_{E E.}^{n no} = = \frac{{T T}_{λ λ}}{ρ ρ cos cos L L},, δ δ {g g}_{N N}^{n no} = = \frac{{T T}_{L L}}{ρ ρ},, δ δ {g g}_{U u}^{n no} = = {T T}_{ρ ρ},,

The output of the accelerometer is then compensated.

3. The compensation method for gravitational disturbance in the initial alignment of a kind of strapdown inertial navigation system according to claim 1, characterized in that: the initial matrix:

θ ₀ is the pitch angle, γ ₀ is the roll angle, is the heading angle.

4. A method for compensating gravity disturbance during initial alignment of strapdown inertial navigation according to claim 1, characterized in that: said state variables are platform misalignment angle and horizontal velocity error.

5. the compensation method of gravitational disturbance in the initial alignment of a kind of strapdown inertial navigation according to claim 1, it is characterized in that: described step (5) comprises:

(5.1) Constructing 2n+1-dimensional Sigma points

\{\begin{matrix} {ξ ξ}_{00} = = {\overset{^^}{x x}}_{k k - - 11} \\ {ξ ξ}_{i i} = = {\overset{^^}{x x}}_{k k - - 11} + + \sqrt{((m m + + κ κ)) {P P}_{k k - - 11}},, i i = = 1,2 1,2,, . . . . . .,, m m \\ {ξ ξ}_{i i + + n no} = = {\overset{^^}{x x}}_{k k - - 11} - - \sqrt{((m m + + κ κ)) {P P}_{k k - - 11}} \end{matrix}

in is the state estimation at time k-1, P _k-1 is the state error covariance matrix at time k-1, m is the dimension of system state variables, and κ is a proportional coefficient, which can be used to adjust the Sigma point and distance;

(5.2) Determine the weight

{w w}_{i i}^{m m} = = {w w}_{i i} = = \{\begin{matrix} κ κ / / ((m m + + κ κ)) & ((i i = = 00)) \\ 11 / / [[22 ((m m + + κ κ))]] & ((i i &NotEqual; &NotEqual; 00)) \end{matrix}

(5.3) Time update by filter equation

γ _k/k-1 = f _k-1 (ξ _k-1 )

{\overset{^^}{x x}}_{k k / / k k - - 11} = = {Σ Σ}_{i i = = 00}^{22 n no} {w w}_{i i} {γ γ}_{i i,, k k / / k k - - 11}

{P P}_{k k / / k k - - 11} = = {Σ Σ}_{i i = = 00}^{22 n no} {w w}_{i i} (({γ γ}_{i i,, k k / / k k - - 11} - - {\overset{^^}{x x}}_{k k / / k k - - 11})) {(({γ γ}_{i i,, k k / / k k - - 11} - - {\overset{^^}{x x}}_{k k / / k k - - 11}))}^{T T} + + {Q Q}_{k k - - 11}

Depend on and P _k-1 to calculate the Sigma point ξ _k-1 , which is propagated as γ k _/k-1 through the nonlinear state function f _k-1 (·), and the state prediction can be obtained from γ _k/k-1 and prediction error covariance matrix P _k/k-1 , Q _k-1 is system noise;

(5.4) Measurement update by filtering equation

K _k ＝P _k/k-1 H ^T (HP _k/k-1 H ^T +R _k ) ^-1

{\overset{^^}{x x}}_{k k} = = {\overset{^^}{x x}}_{k k / / k k - - 11} + + {K K}_{k k} (({Z Z}_{k k} - - H h {\overset{^^}{x x}}_{k k / / k k - - 11}))

P _k ＝(IK _k H)P _k/k-1

where R _k is the measurement noise, K _k is the filter gain, P _k is the estimation error covariance matrix, is the state estimate which includes the platform misalignment angle.

6. The compensation method for gravitational disturbance in the initial alignment of a strapdown inertial navigation system according to claim 5, characterized in that: the filter equation includes a state equation

\dot{x =} f (x) + GW,

System noise vector

W = {[\begin{matrix} w_{gx}^{b} & w_{gy}^{b} & w_{gz}^{b} & w_{ax}^{b} & w_{ay}^{b} \end{matrix}]}^{T}

coefficient array

G = [\begin{matrix} G_{1} & 0_{3 \times 2} \\ 0_{2 \times 3} & G_{2} \end{matrix}], G_{1} = [\begin{matrix} C_{11}^{'} & C_{12}^{'} & C_{13}^{'} \\ C_{twenty one}^{'} & C_{twenty two}^{'} & C_{twenty three}^{'} \\ C_{31}^{'} & C_{32}^{'} & C_{33}^{'} \end{matrix}] G_{2} = [\begin{matrix} G_{11}^{'} & G_{12}^{'} \\ C_{twenty one}^{'} & C_{twenty two}^{'} \end{matrix}],

f(X) is

{\overset{. .}{φ φ}}_{x x} = = - - ((sin sin {φ φ}_{z z})) {ω ω}_{ie ie} cos cos L L + + {φ φ}_{y the y} {ω ω}_{ie ie} sin sin L L - - \frac{δ δ {v v}_{y the y}}{{R R}_{M m}} + + {C C}_{1111}^{' '} (({ϵ ϵ}_{x x}^{b b} + + {w w}_{gx gx}^{b b})) + + {C C}_{1212}^{' '} (({ϵ ϵ}_{y the y}^{b b} + + {w w}_{gy gy}^{b b})) + + {C C}_{1313}^{' '} (({ϵ ϵ}_{z z}^{b b} + + {w w}_{gz gz}^{b b}))

{\overset{. .}{φ φ}}_{y the y} = = ((11 - - cos cos {φ φ}_{z z})) {ω ω}_{ie ie} cos cos L L + + {φ φ}_{x x} {ω ω}_{ie ie} sin sin L L - - \frac{δ δ {v v}_{x x}}{{R R}_{N N}} + + {C C}_{21 twenty one}^{' '} (({ϵ ϵ}_{x x}^{b b} + + {w w}_{gx gx}^{b b})) + + {C C}_{22 twenty two}^{' '} (({ϵ ϵ}_{y the y}^{b b} + + {w w}_{gy gy}^{b b})) + + {C C}_{23 twenty three}^{' '} (({ϵ ϵ}_{z z}^{b b} + + {w w}_{gz gz}^{b b}))

{\overset{. .}{φ φ}}_{z z} = = ((- - {φ φ}_{y the y} sin sin {φ φ}_{z z} + + {φ φ}_{x x} cos cos {φ φ}_{z z})) {ω ω}_{ie ie} cos cos L L + + \frac{δ δ {v v}_{x x} tan the tan L L}{{R R}_{N N}} + + {C C}_{3131}^{' '} (({ϵ ϵ}_{x x}^{b b} + + {w w}_{gx gx}^{b b})) + + {C C}_{3232}^{' '} (({ϵ ϵ}_{y the y}^{b b} + + {w w}_{gy gy}^{b b})) + + {C C}_{3333}^{' '} (({ϵ ϵ}_{z z}^{b b} + + {w w}_{gz gz}^{b b}))

\begin{matrix} δ δ {\overset{. .}{v v}}_{x x} = = - - {f f}_{x x} ((cos cos {φ φ}_{z z} - - 11)) + + {f f}_{y the y} sin sin {φ φ}_{z z} - - {f f}_{z z} (({φ φ}_{y the y} cos cos {φ φ}_{z z} + + {φ φ}_{x x} sin sin {φ φ}_{z z})) + + 22 {ω ω}_{ie ie} δ δ {v v}_{y the y} sin sin L L + + \\ {C C}_{1111}^{' '} (({&dtri; &dtri;}_{x x}^{b b} + + {w w}_{ax ax}^{b b})) + + {C C}_{1212}^{' '} (({&dtri; &dtri;}_{y the y}^{b b} + + {w w}_{ay ay}^{b b})) \end{matrix}

\begin{matrix} δ δ {\overset{. .}{v v}}_{y the y} = = - - {f f}_{x x} sin sin {φ φ}_{z z} - - {f f}_{y the y} ((cos cos {φ φ}_{z z} - - 11)) {f f}_{z z} (({φ φ}_{y the y} sin sin {φ φ}_{z z} - - {φ φ}_{x x} cos cos {φ φ}_{z z})) - - 22 {ω ω}_{ie ie} δ δ {v v}_{x x} sin sin L L + + \\ {C C}_{21 twenty one}^{' '} (({&dtri; &dtri;}_{x x}^{b b} + + {w w}_{ax ax}^{b b})) + + {C C}_{22 twenty two}^{' '} (({&dtri; &dtri;}_{y the y}^{b b} + + {w w}_{ay ay}^{b b})) \end{matrix}

Among them, R _M and R _N are the earth's meridian and UU radius of curvature respectively; φ _x , φ _y , φ _z are the misalignment angles of the platform in three directions; δv _x , δv _y are velocity errors; L is the local latitude; w _ie is the angular velocity of the earth's rotation; is the gyro drift in three axes; is the zero-mean Gaussian white noise of the gyroscope; is the acceleration zero bias of the three axes; is the zero-mean Gaussian white noise of the accelerometer; f _x , f _y , f _z are the values of the acceleration output specific force on the calculation geographic coordinate system; C′ _ij is the element that carries the system into the calculation geographic system matrix;

The measurement equation is:

Z=HX+V

Among them, the quantity measurement Z is the inertial navigation horizontal velocity error δv _x , δv _y ; H=[I _2×2 0 _2×3 ], I _2×2 is the unit second-order matrix, and 0 _2×3 is 2 rows and 3 columns zero matrix; V is the measurement noise.