CN103047999A - Quick estimation method for gyro errors in ship-borne master/sub inertial navigation transfer alignment process - Google Patents

Abstract

The invention discloses a quick estimation method for gyro errors in a ship-borne master/sub inertial navigation transfer alignment process, and catering to transfer alignment of ship master/sub combinations, designs a quick estimation method for the gyro errors, which is suitable for mooring/constant-speed direct flight conditions and is used for quickly estimating constant errors of sub inertial navigation gyros. Under the support of a real-time multiple task operating system, a sub inertial navigation system implements the circular navigation resolving and the information fusing for one group of data; and the high-speed performance of a computer is fully used to quickly finish the estimation of the gyro constant errors in the transfer alignment process. The method dose not change the existing algorithm of navigation resolving, dose not change the filter structure and the information matching mode in the transfer alignment information fusing algorithm, fully uses the redundant resources of the existing navigation computer, and quickens the gyro error estimation speed in the transfer alignment process.

Description

Gyro error method for quick estimating in a kind of carrier-borne master/sub-inertial navigation Transfer Alignment process

Technical field

The present invention relates to carrier-borne master/sub-inertial navigation fast transfer alignment method, be specially adapted under naval vessel mooring or the at the uniform velocity direct route condition, the quick estimation of gyro error is the gyro error method for quick estimating in a kind of carrier-borne master/sub-inertial navigation Transfer Alignment process.

Background technology

Under the carrier-borne environment, the precision of main inertial navigation (hip-based platform formula inertial navigation system) generally is higher than several magnitudes of precision of sub-inertial navigation (such as the ship-based missile strap-down inertial navigation system).The process that sub-inertial navigation utilizes the navigation information of main inertial navigation to finish initialization, misalignment estimation, device estimation of error is called Transfer Alignment.

Transfer Alignment is widely used in the initial alignment of the sub-inertial navigation system that carries on the weapon platforms such as battlebus, opportunity of combat, naval vessel, spacecraft as the technology a kind of general, general character in inertial technology field.Based on the requirement of armament systems rapid-action, the rapidity of Transfer Alignment is the target of initial alignment always.1989, Kain and Cloutier proposed " speed+attitude " matching way first, and cooperated the wing that shakes with carrier aircraft to move, and can reach the alignment precision of 1mrad in 10s.In addition, the people such as Spalding, Shortelle and Graham have also verified the validity of said method.

When said method misalignment in having emphasized alignment procedures is estimated rapidity, weaken or ignored estimation to the device error.But there is following problem in inertia device: 1) after long storage time, can have the drift of certain magnitude, the device precision is lower, and it is more serious to drift about; 2) existence starts error one by one, and the device precision is lower, and error is larger.For in for the sub-inertial navigation system of low precision, when its initial alignment, if the device error is not estimated, will cause the device error in long-time navigation calculation process, being integrated amplification, thereby reduce system's navigation accuracy.

In fact, the observability of each quantity of state depends on information matches mode and carrier maneuver mode in the Transfer Alignment Kalman wave filter.When oscillating motion, adopt " speed+attitude " matching way, the estimation of site error needed several minutes, was unable to estimate in 10s.

Summary of the invention

The problem to be solved in the present invention is: under the carrier-borne environment, existing Transfer Alignment has been ignored the estimation of error to inertia device, has reduced navigation accuracy, and prior art is quick not to the estimation of error, the real-time of impact navigation.

Technical scheme of the present invention is: the gyro error method for quick estimating in a kind of carrier-borne master/sub-inertial navigation Transfer Alignment process, under the support of real-time multi-task operating system, the gyroscope constant value error is estimated fast, in the master/sub-inertial navigation Transfer Alignment process, navigation calculation is carried out in sub-inertial navigation, sub-inertial navigation utilizes the navigation data of main inertial navigation to carry out information fusion, establishes t ₀Being the initial time of gyroscope constant value estimation of error, also is the initial time of initial alignment, t ₀~ t ₁For data acquisition, data storage, real-time navigation resolves and information fusion and attitude matrix

Updating period, t ₁~ t ₂For based on t ₀~ t ₁Between the storage information circular navigation resolve and information fusion and attitude matrix

Updating period, above-mentioned each time definite as follows:

T1) for concrete sub-inertial navigation system, leading/sub-inertial navigation Transfer Alignment in, determine to finish the needed time of whole aligning by sub-inertial navigation gyro error convergence curve, again according to sub-inertial navigation measurement and navigation calculation update cycle Δ t, determine to finish the total update times of the needed navigation calculation of whole aligning, be set as k _SumAccording to main inertial navigation navigation information updating cycle Δ T, determine to finish the needed information fusion number of times of whole aligning, Δ T is the information fusion cycle; Under the working environment of determining, navigation calculation and information fusion sequential relationship determine that after the navigation calculation number of times was determined, the information fusion number of times can be determined;

T2) according to sub-inertial navigation instrumented data quality, main inertial navigation measurement information quality is set constantly t ₁And time period t ₀~ t ₁, and calculate navigation calculation update times in this time period according to Δ t, be set as k _Sum1

T3) from the total update times k of the needed navigation calculation of whole aligning _SumIn, deduction step T2) middle t ₀~ t ₁Navigation calculation update times k in time period _Sum1, obtaining finishing whole aligning also needs the navigation calculation update times finished, is set as k _Sum2, k _Sum2=k _Sum-k _Sum1Thereby, obtain t ₁~ t ₂The circular navigation of time period is resolved cycle index, is set as m, m=k _Sum2/ k _Sum1, when m is not integer, with m value rounding upwards, and according to the adjustment of the m value behind rounding k _Sum2Moment t ₂Resolved the performance of cycle index m and navigational computer by circular navigation and determine that jointly when m determined, the navigational computer dominant frequency was higher, t ₂With t ₁Between difference less;

According to described definite time, in Transfer Alignment, finish sub-inertial navigation gyroscope error estimation, comprise the steps:

1) at t ₀Constantly, utilize the corresponding navigation information of position, speed and the inertial navigation of attitude information antithetical phrase of main inertial navigation to carry out initialization, obtain position, speed and the attitude matrix of sub-inertial navigation

T is time variable; Flash setting t ₀Carrier coordinate system constantly is b ₀, obtain

I is (3 * 3) unit matrix;

2) in time period t ₀~ t ₁In, sub-inertial navigation is carried out inertia type instrument data acquisition, storage, navigation calculation by cycle Δ t, and utilizes formula (6) to upgrade sub-inertial navigation carrier coordinate system b with respect to b ₀The attitude matrix of system

$C_{{b,k}_1}^{{\mathrm{<figure-callout\; id="0"\; label="b"\; filenames="HDA00002609635900031.png"\; state="\{\{state\}\}">b</figure-callout>}}_0}=C_{{b,k}_1-1}^{b_0}(I+\mathrm{\&Delta;t}{\mathrm{\&omega;}}_{{\mathrm{ib},k}_1}^b\&times;)---\left(6\right)$

In the formula, subscript k ₁=1 ~ k _Sum1The number of times label that the expression navigation calculation upgrades,

Be the data of sub-inertial navigation gyro Real-time Collection, $C_{b,0}^{b_0}=C_b^{b_0}\left(t_0\right)=I;$

Simultaneously sub-inertial navigation receives the navigation information of autonomous inertial navigation according to cycle Δ T and stores, and described navigation information comprises speed and course, and records the sequential relationship between main inertial navigation navigation information and sub-inertial navigation instrumented data; By information fusion lead, sub-inertial navigation Transfer Alignment;

3) in step 2) end t ₁Constantly, obtain the real-time carrier matrix in this moment

This carrier coordinate system constantly of flash setting is b ₁, have

4) in time period t ₁~ t ₂In, sub-inertial navigation system carries out following work:

41) to step 2) in the sub-inertial navigation inertia type instrument data that gather carry out m circular navigation and resolve and information fusion t ₁~ t ₂In carry out m circular navigation and resolve, each subcycle cycle is: t ₁₀~ t ₁₁, t ₂₀~ t ₂₁..., t _J0~ t _J1... t _M0~ t _M1, j=1 ~ m represents the label in subcycle cycle, in the navigation calculation and information fusion process of each subcycle, and used sub-inertial navigation instrumented data and the corresponding t of main inertial navigation information difference ₀~ t ₁The instrumented data that interior sub-inertial navigation system gathers and the navigation information of main inertial navigation, within each subcycle cycle, navigation calculation update times and information fusion number of times and t ₀~ t ₁Interior number of times equates respectively; Circulation is resolved in the process, the naval vessel is in mooring or at the uniform velocity sails through to state, each subcycle is resolved speed, the position of the zero hour and is directly got subcycle end cycle speed and position constantly, but because there is oscillating motion in boats and ships, the attitude that the zero hour is resolved in each subcycle needs following processing, at each subcycle t _J0~ t _J1The zero hour t _J0Initial attitude matrix:

$C_b^n\left(t_{j0}\right)=C_b^n\left(t_{(j-1)1}\right){\left(C_b^{b_0}\left(t_1\right)\right)}^T---\left(7\right)$

In the formula,

For at t _{(j-1) 1}The attitude matrix of the carrier coordinate system b Relative Navigation coordinate system n that constantly calculates, the attitude matrix when namely end is resolved in j-1 son circulation, $C_b^{b_0}\left(t_1\right)=C_{{b,k}_{\mathrm{sum}1}}^{{\mathrm{<figure-callout\; id="0"\; label="b"\; filenames="HDA00002609635900031.png"\; state="\{\{state\}\}">b</figure-callout>}}_0},$ $C_b^n\left(t_{10}\right)=C_b^n\left(t_1\right){\left(C_b^{b_0}\left(t_1\right)\right)}^T;$

In each subcycle process, by step 2) in the record sequential relationship carry out navigation calculation and information fusion;

42) carry out above-mentioned circular navigation resolve with information fusion in, sub-inertial navigation is carried out Real-time Collection to the inertia type instrument data, upgrade to calculate current carrier coordinate system b system with respect to b ₁The attitude matrix of system

Its discrete form is:

$C_{{b,k}_2}^{{\mathrm{<figure-callout\; id="1"\; label="b"\; filenames="HDA00002609635900031.png,HDA00002609635900032.png"\; state="\{\{state\}\}">b</figure-callout>}}_1}=C_{{b,k}_2-1}^{b_1}(I+\mathrm{\&Delta;t}{\mathrm{\&omega;}}_{\mathrm{ib}{k}_2}^b\&times;)---\left(9\right)$

Wherein $C_{b,0}^{b_1}=C_b^{b_1}\left(t_1\right)=I,$ k ₂For

Update times, step 42) finally obtain

5) the circular navigation of finishing m cycle resolve and corresponding information fusion after, sub-inertial navigation system is according to t _M1Carrier coordinate system b is the attitude estimated matrix of n with respect to navigation coordinate constantly

Be t ₁The constantly afterwards estimated value of attitude matrix, and t ₂Carrier coordinate system b is with respect to b constantly ₁Real-time attitude matrix

Obtain t ₂Real-time attitude matrix constantly Be used for the navigation of sub-inertial navigation; Simultaneously sub-inertial navigation obtains t _M1Gyroscope error estimation value constantly, i.e. t ₁The afterwards estimated value of moment gyro error is with t _M1Gyroscope error estimation value constantly is as final gyroscope error estimation value;

T4) repeatedly carry out above-mentioned steps T2) and step T3), by the form of off-line simulation, select different time point t ₁, obtain different circular navigation and resolve period m, according to step 1)-5) gyro error is estimated fast selecting can be near T1) in the t of estimation effect ₁With m as optimum time point t ₁Count m with the circular navigation computation cycles, formally be used for the Transfer Alignment of carrier-borne master/sub-inertial navigation, described estimation effect refers to T1) the convergence effect of neutron inertial navigation gyro error convergence curve; In the carrier-borne master of reality/sub-inertial navigation Transfer Alignment process, carry out step 1)-5), the realization gyro error is estimated fast.

Described real-time multi-task operating system is the multiple task real-time operation system VxWorks, finishes following four tasks, and the scheduling of task is automatically performed by the priority level of multiple task real-time operation system VxWorks according to task, following four tasks of operation in sub-inertial navigation:

1) task one: the collection of sub-inertial navigation instrumented data, storage and navigation calculation and

New Algorithm more, priority level is the highest;

2) task two: sub-inertial navigation instrumented data collection,

New Algorithm more, priority level second;

3) task three: sub-inertial navigation is carried out data acquisition, storage and information fusion to main inertial navigation, priority level the 3rd;

4) task four: sub-inertial navigation is resolved and information fusion based on the circular navigation of storage data, and priority level is minimum.In the Transfer Alignment process, the navigation calculation algorithm of sub-inertial navigation is:

$C_{b,k}^n=C_{b,k-1}^n(I+\mathrm{\&Delta;t}{\mathrm{\&omega;}}_{\mathrm{nb},k}^b\&times;)---\left(1\right)$

$V_k^n=V_{k-1}^n+\mathrm{\&Delta;t}[C_{b,k-1}^n{f}_k^b-(2{\mathrm{\&omega;}}_{\mathrm{ie},k-1}^n+{\mathrm{\&omega;}}_{\mathrm{en},k-1}^n)\&times;V_{k-1}^n+g^n]---\left(2\right)$

$L_k=L_{k-1}+\frac{\mathrm{\&Delta;t}{V}_{N,k-1}^n}{R_M+h_{k-1}},{\mathrm{\&lambda;}}_k={\mathrm{\&lambda;}}_{k-1}+\frac{\mathrm{\&Delta;t}{V}_{E,k-1}^n\sec L_{k-1}}{R_N+h_{k-1}},h_k=h_{k-1}+\mathrm{\&Delta;t}{V}_{U,k-1}^n---\left(3\right)$

In the formula,

Be the attitude matrix of sub-inertial navigation carrier coordinate system b Relative Navigation coordinate system n, I is (3 * 3) unit matrix, subscript k=1 ~+∞ represents the update times label of navigation calculation, and n represents navigation coordinate system, and b represents carrier coordinate system, and i represents inertial coordinates system, Expression terrestrial coordinate system e is projected angle speed among the n with respect to inertial coordinates system i at navigation coordinate,

The expression navigation coordinate is n with respect to terrestrial coordinate system e is projected angle speed among the n at navigation coordinate; In the formula (1) ${\mathrm{\&omega;}}_{\mathrm{nb}}^b={\mathrm{\&omega;}}_{\mathrm{ib}}^b-{\left(C_b^n\right)}^T({\mathrm{\&omega;}}_{\mathrm{ie}}^n+{\mathrm{\&omega;}}_{\mathrm{en}}^n),$ Be sub-inertial navigation gyro image data, ${\mathrm{\&omega;}}_{\mathrm{en}}^n={\left[\begin{array}{ccc}-V_N^n/(R_M+h)& V_E^n/(R_N+h)& \left(V_E^n\tan L\right)/(R_N+h)\end{array}\right]}^T,$ $V^n=\left[\begin{array}{ccc}{V}_E^n& V_N^n& V_U^n\end{array}\right]$ Be respectively the east, north of sub-inertial navigation, day to speed, L, λ and h represent respectively the lower speed of navigation coordinate system, latitude, longitude and highly, f ^bBe sub-inertial navigation accelerometer image data, g ⁿBe the projection of terrestrial gravitation acceleration in navigation coordinate system, R _MAnd R _NBe respectively meridian circle and the prime vertical radius of the navigation location earth; The vectorial backslash computing of operational symbol " * " expression, under the carrier-borne environment, directly get sub-inertial navigation sky to speed with highly be zero.

In the Transfer Alignment process, main, sub-inertial navigation information fusion is:

Adopt the Kalman wave filter as the information fusion filtering device, get sub-inertial navigation east orientation/north orientation velocity error, misalignment and gyro error as the system state vector, namely

X=[δV _E δV _N φ _E φ _N φ _U ε _x ε _y ε _z] ^T

Wherein, δ V _E/ δ V _NBe east orientation/north orientation velocity error; φ _E, φ _N, φ _UBe respectively pitching, rolling, course misalignment; ε _x, ε _y, ε _zBe respectively three axle gyro errors;

Get system state equation,

$\stackrel{\&CenterDot;}{X}\left(t\right)=A\left(t\right)X\left(t\right)+W\left(t\right)---\left(4\right)$

In the formula, A (t) is state matrix, and W (t) is system noise, and according to system state variables medium velocity, misalignment error equation, and gyro is from the projection relation of carrier coordinate system b navigation coordinate system n, and state matrix A (t) is expressed as:

$A\left(t\right)=\left[\begin{array}{cccccccc}\frac{V_N}{R}\tan \mathrm{<figure-callout\; id="2"\; label="L"\; filenames="HDA00002609635900011.png,HDA00002609635900031.png"\; state="\{\{state\}\}">L</figure-callout>}& {2\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_E}{R}\tan L& 0& {-f}_{\mathrm{<figure-callout\; id="0"\; label="U\; f\; N"\; filenames="HDA00002609635900031.png"\; state="\{\{state\}\}">U</figure-callout>}}& f_N{}_{}& 0& 0& 0\\ -({2\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_E}{R}\tan L)& 0& f_U& 0& {-\mathrm{<figure-callout\; id="0"\; label="f\; E"\; filenames="HDA00002609635900031.png"\; state="\{\{state\}\}">f</figure-callout>}}_E& 0& 0& 0\\ 0& -\frac{1}{R}& 0& {\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_E}{R}\tan L& -({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_E}{R})& -T_{11}& {-T}_{12}& {-T}_{13}\\ \frac{1}{R}& 0& -({\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_E}{R}\tan L)& 0& -\frac{V_N}{R}& {-T}_{21}& -T_{22}& {-T}_{23}\\ \frac{1}{R}\tan \mathrm{<figure-callout\; id="0"\; label="L"\; filenames="HDA00002609635900031.png"\; state="\{\{state\}\}">L</figure-callout>}& 0& {\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_E}{R}& \frac{V_N}{R}& 0& -T_{31}& {-T}_{32}& {-T}_{33}\\ 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

Wherein, V _E, V _NBe sub-inertial navigation east orientation and north orientation speed, ω _IeBe rotational-angular velocity of the earth, R is earth radius, and L is local geographic latitude, f _E, f _N, f _UBe respectively sub-inertial navigation acceleration measurement f ^bBe projection among the n at navigation coordinate, T _Pq, p, q=1,2,3 is the attitude matrix of sub-inertial navigation carrier coordinate system b Relative Navigation coordinate system n

Each element;

" speed+course " the information matches mode of employing is got speed, the course difference of son, main inertial navigation and is directly constructed measurement information, and namely son/main inertial navigation navigational system measurement vector is

Z=[V _E-V _ME V _N-V _MN H-H _M] ^T

Wherein, V _EWith V _MEBe respectively the east orientation speed of son, main inertial navigation; V _NWith V _MNBe respectively the north orientation speed of son, main inertial navigation; H and H _MBe respectively the course information of son, main inertial navigation, H is from sub-inertial navigation attitude matrix

In directly extract;

The system measurements equation is

Z(t)=H(t)X(t)+V(t) （5）

In the formula, H (t) is measurement matrix, and V (t) is measurement noise, according to measuring the relation of vector with state vector, has measurement matrix to be

$H=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& \\ 0& 1& 0& 0& 0& 0_{3\&times;3}\\ 0& 0& -\frac{T_{12}{T}_{32}}{T_{12}^2+T_{22}^2}& -\frac{T_{22}{T}_{32}}{T_{12}^2+T_{22}^2}& -1& \end{array}\right]$

In the information fusion process, adopt the Closed-cycle correction mode, information fusion is estimated that the velocity error, misalignment and the gyroscope error estimation value that obtain feed back to sub-inertial navigation system and participate in navigation calculation.

For the Transfer Alignment in naval vessel master/sub-portfolio, the present invention has designed a kind of gyro error method for quick estimating that is applicable under mooring/at the uniform velocity direct route condition, antithetical phrase inertial navigation gyroscope constant value error is estimated fast, under the support of real-time multi-task operating system, sub-inertial navigation system carries out to same group of data that circular navigation is resolved and information fusion, take full advantage of the high speed performance of computing machine, in the Transfer Alignment process, finish fast the estimation to the gyroscope constant value error.Method of the present invention has following advantage: (1) does not change the information matches pattern of existing Transfer Alignment, does not require that carrier does specific motor-driven; (2) directly utilize the Kalman filtering technique of present comparative maturity to estimate; (3) computer resource more than needed in the cycle carries out work in posture renewal cycle and information fusion to utilize navigational computer, requirements at the higher level are not proposed computing power, although alignment speed still depends on computing power to a great extent, but with the situation of the same computer resource of existing navigational system configuration under, can obtain faster Transfer Alignment speed.

Description of drawings

Fig. 1 is that the present invention is along time shaft alignment procedures step exploded view;

Fig. 2 is the information fusion algorithm synoptic diagram that the present invention uses;

Fig. 3 is loop calculation figure of the present invention;

Fig. 4 is misalignment evaluated error figure under the mooring condition of the present invention;

Fig. 5 is to the estimation curve figure of site error under the mooring condition of the present invention;

Fig. 6 is misalignment evaluated error figure under the at the uniform velocity direct route condition of the present invention;

Fig. 7 is to the estimation curve figure of gyro error under the at the uniform velocity direct route condition of the present invention.

Embodiment

The present invention thinks the analysis strap-down inertial navigation system replaces the relevant feature of " physical platform " in the gimbaled inertial navigation system with " mathematical platform " after, in strap-down inertial navigation system, can utilize same data to carry out navigation calculation, and constantly adjust mathematical platform, take full advantage of the high speed performance of computing machine, in the Transfer Alignment process, finish fast the estimation to site error.

Below in conjunction with accompanying drawing the invention process method is described in more detail:

Among Fig. 1, t ₀Be the initial time of gyroscope constant value estimation of error, t ₀~ t ₁For data storage, navigation calculation and information fusion and

Updating period, t ₁~ t ₂For resolving based on the circular navigation of storage information with information fusion and based on gyro to measure information Updating period.Δ t is inertia type instrument data and the navigation calculation update cycle of sub-inertial navigation; Δ T is main inertial navigation information updating cycle and boss's inertial navigation information fusion cycle, and Δ T is the integral multiple of Δ t, and in the present invention, Δ t is the navigation calculation cycle, and Δ T is the information fusion cycle.For the master who determines/sub-inertial navigation combined system, Δ t and Δ T are determined value, and sub-inertial navigation instrumented data and main inertial navigation are determined with reference to the navigation information sequential relationship, also be that navigation calculation and information fusion sequential relationship are determined, after the navigation calculation number of times was determined, the information fusion number of times can be determined.Above-mentioned t ₁Selection and definite need of cycle index m determine by Multi simulation running according to concrete system.Specific as follows:

T1) for concrete sub-inertial navigation system, leading/sub-inertial navigation Transfer Alignment in, by observing the convergence situation of sub-inertial navigation gyroscope error estimation curve, determine to finish the needed time of whole aligning, measure and the navigation update cycle according to sub-inertial navigation, determine to finish the total update times k of the needed navigation calculation of whole aligning _SumAccording to the main inertial navigation navigation information updating cycle, determine to finish the needed information fusion number of times of whole aligning;

T2) according to sub-inertial navigation instrumented data quality, main inertial navigation measurement information quality, select time t ₁, and calculate navigation calculation update times in this time period according to Δ t, be set as k _Sum1, t ₀~ t ₁The time period of obtaining basic data among the present invention, the t of back ₁~ t ₂Time period is according to t ₀~ t ₁The data that time period obtains are carried out further estimation of error, t ₀~ t ₁Length determine according to the quality of data of sub-inertial navigation, main inertial navigation as required;

T3) from the total update times k of the needed navigation calculation of whole aligning _SumIn, deduction step T2) middle t ₀~ t ₁Navigation calculation update times k in time period _Sum1, obtaining finishing whole aligning also needs the navigation calculation update times finished, is set as k _Sum2=k _Sum-k _Sum1Thereby, obtain t ₁~ t ₂The circular navigation of time period is resolved cycle index, is set as m=k _Sum2/ k _Sum1, when m is not positive number, with m value rounding upwards, and according to the adjustment of the m value behind rounding k _Sum2Time t ₂Resolved the performance of cycle index and navigational computer by circular navigation and determine that jointly when m determined, computer main frequency was higher, t ₂With t ₁Between difference less;

T4) according to above-mentioned steps T2) and step T3), by the form of off-line simulation, alternative optimum time point t ₁Count m with the circular navigation computation cycles, be actually used in the Transfer Alignment of master/sub-inertial navigation, described the best refers to according to selected time point t ₁The convergence situation of the sub-inertial navigation gyroscope error estimation of the closing to reality curve of gyroscope error estimation that obtains with period m.

The present invention carries out gyroscope error estimation under the support of real-time multi-task operating system:

Among Fig. 1, (a) at t ₀~ t ₁Time period, because the information fusion calculation amount is relatively large, generally need some Δ t to finish, if adopt the single task program schema, just can carry out instrumented data collection and navigation calculation after namely finishing information fusion, then program design is complicated, is difficult to satisfy the navigation calculation requirement of real-time; (b) at t ₁~ t ₂Time period is based on the t of storage ₀~ t ₁The data of time period are carried out navigation calculation and information fusion, need to take to remove

Upgrade all outer computer resources, if adopt the single task pattern, then program design is complicated, is difficult to satisfy the navigation calculation requirement of real-time.

Can not satisfy above-mentioned requirements based on the single task pattern, introduce the real-time multi-task operating system VxWorks, under the VxWorks environment, be defined as follows task:

1) task one: instrumented data collection, storage and navigation calculation and

New Algorithm more, priority level is the highest;

2) task two: the instrumented data collection,

New Algorithm more, priority level second;

3) task three: main inertial navigation data acquisition, storage and information fusion, priority level the 3rd;

4) task four: the circular navigation based on the storage data is resolved and information fusion, and priority level is minimum.

Above-mentioned task one, three is initiated after initialization, takies the navigational computer resource according to the priority level of task, and when not carrying out information fusion, task one takies separately the navigational computer resource; When carrying out information fusion, if need to carry out instrumented data collection, storage and navigation calculation, then task three is abandoned the taking of navigational computer resource, and task one takies, and after task one was finished, task three took again.

At t ₁Constantly, after task one was finished, task one was deleted, and task two is initiated simultaneously; After task three was finished, task three was also deleted, and task four is initiated simultaneously; At t ₁~ t ₂Time period, task two takies the navigational computer resource with task four according to priority level, and when task two did not need to carry out, task four took the navigational computer resource and carries out circular navigation and resolve.

Navigation calculation of the present invention is specially:

$C_{b,k}^n=C_{b,k-1}^n(I+\mathrm{\&Delta;t}{\mathrm{\&omega;}}_{\mathrm{nb},k}^b\&times;)---\left(1\right)$

$V_k^n=V_{k-1}^n+\mathrm{\&Delta;t}[C_{b,k-1}^n{f}_k^b-({2\mathrm{\&omega;}}_{\mathrm{ie},k-1}^n+{\mathrm{\&omega;}}_{\mathrm{en},k-1}^n)\&times;V_{k-1}^n+g^n]---\left(2\right)$

$L_k=L_{k-1}+\frac{\mathrm{\&Delta;t}{V}_{N,k-1}^n}{R_M+h_{k-1}},$ ${\mathrm{\&lambda;}}_k={\mathrm{\&lambda;}}_{k-1}+\frac{\mathrm{\&Delta;t}{V}_{E,k-1}^n\sec L_{k-1}}{R_N+h_{k-1}},$ $h_k=h_{k-1}+\mathrm{\&Delta;t}{V}_{U,k-1}^n---\left(3\right)$

In the formula,

Be the attitude matrix of sub-inertial navigation carrier coordinate system b Relative Navigation coordinate system n, I is (3 * 3) unit matrix, subscript k=1 ~+∞ represents the update times label of navigation calculation, and n represents navigation coordinate system, and b represents carrier coordinate system, and i represents inertial coordinates system, Expression terrestrial coordinate system e is projected angle speed among the n with respect to inertial coordinates system i at navigation coordinate,

The expression navigation coordinate is n with respect to terrestrial coordinate system e is projected angle speed among the n at navigation coordinate.In the formula (4) ${\mathrm{\&omega;}}_{\mathrm{nb}}^b={\mathrm{\&omega;}}_{\mathrm{ib}}^b-{\left(C_b^n\right)}^T({\mathrm{\&omega;}}_{\mathrm{ie}}^n+{\mathrm{\&omega;}}_{\mathrm{en}}^n),$ Be sub-inertial navigation gyro image data, ${\mathrm{\&omega;}}_{\mathrm{en}}^n={\left[\begin{array}{ccc}-V_N^n/(R_M+h)& V_E^n/(R_N+h)& \left(V_E^n\tan L\right)/(R_N+h)\end{array}\right]}^T,$ $V^n=\left[\begin{array}{ccc}{V}_E^n& V_N^n& V_U^n\end{array}\right]$ Be respectively the east, north of sub-inertial navigation, day to speed, L, λ and h represent respectively the lower speed of navigation coordinate system, latitude, longitude and highly, f ^bBe sub-inertial navigation accelerometer image data, g ⁿBe the projection of terrestrial gravitation acceleration in navigation coordinate system, R _MAnd R _NBe respectively meridian circle and the prime vertical radius of the navigation location earth; The vectorial backslash computing of operational symbol " * " expression, under the carrier-borne environment, directly get sub-inertial navigation sky to speed with highly be zero.Information fusion of the present invention specifically adopts the matching way in Kalman wave filter and " speed+course ", as shown in Figure 2.

Get east orientation/north orientation velocity error, misalignment and gyro error as the system state vector, namely

X=[δV _E δV _N φ _E φ _N φ _U ε _xε _yε _z] ^T

Wherein, δ V _E/ δ V _NBe east orientation/north orientation velocity error; φ _{E ~ U}Be pitching, rolling, course misalignment; ε _{X ~ z}Be three axle gyro errors.

Get system state equation,

$\stackrel{\&CenterDot;}{X}\left(t\right)=A\left(t\right)X\left(t\right)+W\left(t\right)---\left(4\right)$

In the formula, A (t) is state matrix, and W (t) is system noise, and according to system state variables medium velocity, misalignment error equation, and gyro is from carrier coordinate system b navigation coordinate system n projection relation, and state matrix A (t) is expressed as:

$A\left(t\right)=\left[\begin{array}{cccccccc}\frac{V_N}{R}\mathrm{<figure-callout\; id="2"\; label="tan\; L"\; filenames="HDA00002609635900011.png,HDA00002609635900031.png"\; state="\{\{state\}\}">tan</figure-callout>}L& {2\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_E}{R}\tan L& 0& {-f}_{\mathrm{<figure-callout\; id="0"\; label="U\; f\; N"\; filenames="HDA00002609635900031.png"\; state="\{\{state\}\}">U</figure-callout>}}& f_N{}_{}& 0& 0& 0\\ -({2\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_E}{R}\tan L)& 0& f_U& 0& {-\mathrm{<figure-callout\; id="0"\; label="f\; E"\; filenames="HDA00002609635900031.png"\; state="\{\{state\}\}">f</figure-callout>}}_E& 0& 0& 0\\ 0& -\frac{1}{\mathrm{<figure-callout\; id="0"\; label="R"\; filenames="HDA00002609635900031.png"\; state="\{\{state\}\}">R</figure-callout>}}& 0& {\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_E}{R}\tan L& -({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_E}{R})& -T_{11}& {-T}_{12}& {-T}_{13}\\ \frac{1}{R}& 0& -({\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_E}{R}\tan L)& 0& -\frac{V_N}{R}& {-T}_{21}& -T_{22}& {-T}_{23}\\ \frac{1}{\mathrm{<figure-callout\; id="0"\; label="R\; tan\; L"\; filenames="HDA00002609635900031.png"\; state="\{\{state\}\}">R</figure-callout>}}\tan L& 0& {\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_E}{R}& \frac{V_N}{R}& 0& -T_{31}& {-T}_{32}& {-T}_{33}\\ 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

Wherein, V _E, V _NBe east orientation and north orientation speed, ω _IeBe rotational-angular velocity of the earth, R is earth radius, and L is local geographic latitude, f _E, f _N, f _UBe respectively acceleration measurement f ^bBe projection among the n at navigation coordinate, T _Ij(i, j=1,2,3) are the attitude matrix of sub-inertial navigation carrier coordinate system b Relative Navigation coordinate system n Each element;

Employing speed+course information matching way.

Get son, the speed of main inertial navigation, course difference and directly construct measurement information, namely the system measurements vector is

Z=[V _E-V _ME V _N-V _MN H-H _M] ^T

Wherein, V _EWith V _MEBe respectively the east orientation speed of son, main inertial navigation; V _NWith V _MNBe respectively the north orientation speed of son, main inertial navigation; H and H _MBe respectively the course information of son, main inertial navigation, H can be from sub-inertial navigation attitude matrix

In directly extract.

The system measurements equation is

Z(t)=H(t)X(t)+V(t) （5）

In the formula, H (t) is measurement matrix, and V (t) is measurement noise, according to measuring the relation of vector with state vector, has measurement matrix to be

$H=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& \\ 0& 1& 0& 0& 0& 0_{3\&times;3}\\ 0& 0& -\frac{T_{12}{T}_{32}}{T_{12}^2+T_{22}^2}& -\frac{T_{22}{T}_{32}}{T_{12}^2+T_{22}^2}& -1& \end{array}\right]$

System adopts the Closed-cycle correction mode, and as shown in Figure 2, in the information fusion process, what obtain is fed back to sub-inertial navigation system participation computing about velocity error, misalignment and gyro error.Finish circular navigation resolve with information fusion after, the t that obtains ₁Gyroscope error estimation value constantly is namely as the estimated value of sub-inertial navigation system gyro error.

The below specifies gyroscope error estimation process of the present invention:

1) at t ₀Constantly, utilize the corresponding navigation information of position, speed and the inertial navigation of attitude information antithetical phrase of main inertial navigation to carry out initialization, obtain position, speed and the attitude matrix of sub-inertial navigation

T is time variable; Flash setting t ₀Carrier coordinate system constantly is b ₀, obtain

I is (3 * 3) unit matrix;

2) in time period t ₀~ t ₁In, sub-inertial navigation is carried out inertia type instrument data acquisition, storage, navigation calculation by cycle Δ t, and utilizes formula (6) to upgrade sub-inertial navigation carrier coordinate system b with respect to b ₀The attitude matrix of system

$C_{{b,k}_1}^{b_0}=c_{{b,k}_1-1}^{b_0}(I+\mathrm{\&Delta;t}{\mathrm{\&omega;}}_{{\mathrm{ib},k}_1}^b\&times;)---\left(6\right)$

In the formula, subscript k ₁=1 ~ k _Sum1The number of times label that the expression navigation calculation upgrades, update times k ₁With attitude matrix

Variable time t be corresponding,

Be the data of sub-inertial navigation gyro Real-time Collection,

Simultaneously sub-inertial navigation is by navigation information and the storage of the next autonomous inertial navigation of cycle Δ T reception, and described navigation information comprises speed and course, and records the sequential relationship between main inertial navigation navigation information and sub-inertial navigation instrumented data; By information fusion lead, sub-inertial navigation Transfer Alignment;

3) in step 2) finish _t1 constantly, obtains the real-time carrier matrix in this moment according to navigation calculation

This carrier coordinate system constantly of flash setting is b ₁, have

4) in time period t ₁~ t ₂In, sub-inertial navigation system carries out following work:

41) to step 2) in the sub-inertial navigation inertia type instrument data that gather carry out m circular navigation and resolve and information fusion t ₁~ t ₂In carry out m circular navigation and resolve, as shown in Figure 3, each subcycle cycle is: t ₁₀~ t ₁₁, t ₂₀~ t ₂₁..., t _J0～t _J1... t _M0~ t _M1, j=1 ~ m represents the label in subcycle cycle, in the navigation calculation and information fusion process of each subcycle, and used sub-inertial navigation instrumented data and the corresponding t of main inertial navigation information difference ₀~ t ₁The instrumented data that interior sub-inertial navigation system gathers and the navigation information of main inertial navigation, within each subcycle cycle, navigation calculation update times and information fusion number of times and t ₀~ t ₁Interior number of times equates respectively, at each subcycle process t _J0～t _J1In, used instrumented data and the corresponding t of main inertial navigation information difference ₀~ t ₁In each collection point corresponding, its physical significance embodies t ₀~ t ₁The real-time of time period, but the process of resolving takies t ₁~ t ₂The computer resource of time, namely each subcycle all is to t ₀~ t ₁The data of time period are carried out navigation calculation, at t ₁~ t ₂In carry out m time, circulation is resolved in the process, the naval vessel is in mooring or at the uniform velocity sails through to state, each subcycle is resolved speed, the position of the zero hour and is directly got subcycle end cycle speed and position constantly, but because there is oscillating motion in boats and ships, the attitude that the zero hour is resolved in each subcycle needs following processing, at each subcycle t _J0~ t _J1The zero hour t _J0Initial attitude matrix:

$C_b^n\left(t_{j0}\right)=C_b^n\left(t_{(j-1)1}\right){\left(C_b^{b_0}\left(t_1\right)\right)}^T---\left(7\right)$

In the formula,

For at t _{(j-1) 1}The attitude matrix of the carrier coordinate system b Relative Navigation coordinate system n that constantly calculates, the attitude matrix when namely end is resolved in j-1 son circulation,

For example at t ₁₀~ t ₁₁In the cycle period, t ₁₀Be initial time, its initial attitude matrix

Can be calculated as follows:

$C_b^n\left(t_{10}\right)=C_{b_1}^n{C}_{b_0}^{b_1}=C_b^n\left(t_1\right)C_{b_0}^b\left(t_1\right)---\left(8\right)$

In the formula,

For at t ₁Constantly resolve the attitude matrix of the carrier coordinate system b Relative Navigation coordinate system n that obtains.According to

Obtain behind the navigation calculation Carry out again the next round circulation and resolve, until obtain

In each subcycle process, by step 2) in the record sequential relationship carry out navigation calculation and information fusion; Circulation is resolved when finishing, namely at t ₂Constantly, because be to t ₀~ t ₁The data of time period are carried out navigation calculation, and above-mentioned circular navigation is resolved and obtained attitude matrix

The real-time meaning corresponding to t ₁Constantly, by

Obtain according to aforementioned navigation calculation;

42) carry out above-mentioned circular navigation resolve with information fusion in, sub-inertial navigation is carried out Real-time Collection to the inertia type instrument data, upgrade to calculate current carrier coordinate system b system with respect to b ₁The attitude matrix of system

Its discrete form is:

$C_{{b,k}_2}^{b_1}=c_{{b,k}_2-1}^{b_1}(I+\mathrm{\&Delta;t}{\mathrm{\&omega;}}_{{\mathrm{ib},k}_2}^b\&times;)---\left(9\right)$

Wherein $C_{b,0}^{b_1}=C_b^{b_1}\left(t_1\right)=I,$ k ₂For Number of times.Step 42) finally obtains

k ₂The value upper limit jointly determined by cycle index m and computer main frequency.Time period t ₁~ t ₂Interior step 41) with step 42) carry out simultaneously step 41 on the time) understand prior to step 42) finish.

5) the circular navigation of finishing m cycle resolve and corresponding information fusion after, sub-inertial navigation system obtains t _M1Carrier coordinate system b is the attitude estimated matrix of n with respect to navigation coordinate constantly

Be t ₁The constantly afterwards estimated value of attitude matrix, and t ₂Carrier coordinate system b is with respect to b constantly ₁Real-time attitude matrix Obtain t ₂Real-time attitude matrix constantly $C_b^n\left(t_2\right)=C_{b_1}^n{C}_b^{b_1}\left(t_2\right)=C_b^n\left(t_{m1}\right)C_b^{b_1}\left(t_2\right),$ Be used for the navigation of sub-inertial navigation; Simultaneously sub-inertial navigation obtains t by the information fusion of the m time subcycle _M1Gyroscope error estimation value constantly, i.e. t ₁The afterwards estimated value of moment gyro error is with t _M1Gyroscope error estimation value constantly is as final gyroscope error estimation value;

T4) repeatedly carry out above-mentioned steps T2) and step T3), by the form of off-line simulation, select different time point t ₁, obtain different circular navigation and resolve period m, according to step 1)-5) gyro error is estimated fast selecting can be near T1) in the t of estimation effect ₁With m as optimum time point t ₁Count m with the circular navigation computation cycles, estimation effect refers to T1 herein) the convergence effect of neutron inertial navigation gyro error convergence curve, the formal Transfer Alignment that is used for carrier-borne master/sub-inertial navigation, in the carrier-borne master of reality/sub-inertial navigation Transfer Alignment process, carry out step 1)-5), the realization gyro error is estimated fast.

Beneficial effect of the present invention is verified by following emulation:

1) Matlab simulation inertia type instrument data and main inertial navigation data

The generation of (1) mooring condition Imitating data

The naval vessel is done oscillating motion with sinusoidal rule around three axles under the mooring condition, its pitching, laterally and the course wave mathematical model and be:

In the formula, θ, γ and ψ are respectively the angle variables in pitching, rolling and course; A _P, A _R, A _YBe respectively the amplitude of waving in pitching, rolling and course; ω _P, ω _PWith ω _YThe angle of oscillation frequency that represents respectively pitching, rolling and course;

With

Difference pitching, rolling and course initial phase; ψ ₀The expression initial heading; ω _i=2 π/T _i, i=P, R, Y, T _iRepresent corresponding rolling period.

As main inertial navigation information, main inertial navigation is 1s to the sampling period of described data with above-mentioned emulated data and Additive White Noise.

Obtain sub-inertial navigation instrument gross data by the simulation of above-mentioned emulated data, and the corresponding site error that superposes is as the instrument actual acquired data thereon, sub-inertial navigation is sampled to described instrument actual acquired data, is used for navigation calculation, and the sampling period is 10ms.

The correlation parameter of emulation:

Naval vessel initial position: 118 ° of east longitudes, 32 ° of north latitude;

Ship speed: 0m/s;

The ship sway amplitude: 9 ° of pitchings, 14 ° of rolling, boat are shaken 12 °;

The ship sway cycle: pitching 8s, rolling 10s, boat are shaken 6s;

Ship sway initial phase: be 0;

Initial heading, naval vessel: 0 °;

Equatorial radius: 6378165m;

Earth ellipsoid degree: 1/298.3;

Earth surface acceleration of gravity: 9.8m/s ²

Rotational-angular velocity of the earth: 15.04088 °/h;

Gyroscope constant value error: 0.5 °/h;

Gyro white noise error: 0.5 °/h;

Accelerometer bias: 500ug;

Accelerometer white noise error: 500ug;

Main inertial navigation system measures white noise parameter: east/north orientation velocity error variance 0.2m/s, 0.15 °/h of course error variance.

(2) at the uniform velocity sail through to the generation of condition Imitating data

Correlation parameter is identical with the mooring condition under the at the uniform velocity direct route condition, and difference is: ship speed is 10m/s.

2) mathematics checking on the navigational computer

On the navigational computer of x-86 structure, adopt respectively two schemes to verify scheme one: classical Transfer Alignment, namely carry out instrumented data collection, navigation calculation take the time as order, and when main inertial navigation measurement information is arranged, carry out information fusion; Scheme two: adopt the rapid alignment algorithm of the present invention's design, wherein data storage 120s, i.e. t ₁=120s.The alignment precision and the time of aiming at that compare respectively two schemes.

The curve of Fig. 4 and Fig. 5 shows that under the mooring condition, the alignment precision of two schemes is suitable, and is also suitable to the estimation of gyro error.Fig. 6 and Fig. 7 show that under at the uniform velocity direct route condition, the alignment precision of two schemes is suitable, and be also suitable to the estimation of gyro error.

But the aligning asynchronism(-nization) that two schemes is used.

For scheme one, horizontal ordinate represents the time in Fig. 4～7, and under mooring and at the uniform velocity direct route condition, the estimation that scheme one is finished misalignment needs about 200s, and the estimation of gyro error is approximately needed 600s.

For scheme two, horizontal ordinate represents iterations in Fig. 4～7, and the estimation of finishing misalignment needs iterations 20000 times, and the estimation of finishing gyro error approximately needs 60000 times.Consuming time to finish No. 60000 anacoms of iteration, wherein front 12000 times, need storage inertia type instrument data and main inertial navigation metric data, 120s consuming time, this is consuming time identical with scheme one; But amount to 480000 at 12001～60000() inferior, the used time is determined by computer speed.

In this emulation, used computer main frequency 333MHz.This type computing machine finish navigation calculation and

Or

Need time 1ms; Finish once separately and finish

Or

Need about 0.2ms; Finish the primary information fusion and need 25ms.In 48000 navigation are upgraded, need to carry out information fusion 480 times, 38.4s consuming time is upgraded in navigation, and information fusion 12s consuming time amounts to 50.4s consuming time.In loop calculation, computing machine needs

Upgrade and the system task scheduling overhead computing machine actual measurement 59.2s consuming time.Whole alignment procedures 120+59.2=179.2s consuming time, i.e. t ₂=179.2s is to the estimated time of gyro error of the 600s much smaller than scheme one.

Therefore the method for the present invention's design is suitable with classical alignment scheme on alignment precision, but the estimating speed of gyro error is significantly improved.

Claims (4)

1. the gyro error method for quick estimating in carrier-borne master/sub-inertial navigation Transfer Alignment process, it is characterized in that under the support of real-time multi-task operating system, the gyroscope constant value error being estimated fast, in the master/sub-inertial navigation Transfer Alignment process, navigation calculation is carried out in sub-inertial navigation, sub-inertial navigation utilizes the navigation data of main inertial navigation to carry out information fusion, establishes t ₀Being the initial time of gyroscope constant value estimation of error, also is the initial time of initial alignment, t ₀~ t ₁For data acquisition, data storage, real-time navigation resolves and information fusion and attitude matrix

Updating period, t ₁~ t ₂For based on t ₀~ t ₁Between the storage information circular navigation resolve and information fusion and attitude matrix

Updating period, above-mentioned each time definite as follows:

T1) for concrete sub-inertial navigation system, leading/sub-inertial navigation Transfer Alignment in, determine to finish the needed time of whole aligning by sub-inertial navigation gyro error convergence curve, again according to sub-inertial navigation measurement and navigation calculation update cycle Δ t, determine to finish the total update times of the needed navigation calculation of whole aligning, be set as k _SumAccording to main inertial navigation navigation information updating cycle Δ T, determine to finish the needed information fusion number of times of whole aligning, Δ T is the information fusion cycle; Under the working environment of determining, navigation calculation and information fusion sequential relationship determine that after the navigation calculation number of times was determined, the information fusion number of times can be determined;

T2) according to sub-inertial navigation instrumented data quality, main inertial navigation measurement information quality is set constantly t ₁And time period t ₀~ t ₁, and calculate navigation calculation update times in this time period according to Δ t, be set as k _Sum1

T3) from the total update times k of the needed navigation calculation of whole aligning _SumIn, deduction step T2) middle t ₀~ t ₁Navigation calculation update times k in time period _Sum1, obtaining finishing whole aligning also needs the navigation calculation update times finished, is set as k _Sum2, k _Sum2=k _Sum-k _Sum1Thereby, obtain t ₁~ t ₂The circular navigation of time period is resolved cycle index, is set as m, m=k _Sum2/ k _Sum1, when m is not integer, with m value rounding upwards, and according to the adjustment of the m value behind rounding k _Sum2Moment t ₂Resolved the performance of cycle index m and navigational computer by circular navigation and determine that jointly when m determined, the navigational computer dominant frequency was higher, t ₂With t ₁Between difference less;

According to described definite time, in Transfer Alignment, finish sub-inertial navigation gyroscope error estimation, comprise the steps:

1) at t ₀Constantly, utilize the corresponding navigation information of position, speed and the inertial navigation of attitude information antithetical phrase of main inertial navigation to carry out initialization, obtain position, speed and the attitude matrix of sub-inertial navigation

T is time variable; Flash setting t ₀Carrier coordinate system constantly is b ₀, obtain

I is (3 * 3) unit matrix;

2) in time period t ₀~ t ₁In, sub-inertial navigation is carried out inertia type instrument data acquisition, storage, navigation calculation by cycle Δ t, and utilizes formula (6) to upgrade sub-inertial navigation carrier coordinate system b with respect to b ₀The attitude matrix of system

$C_{{b,k}_1}^{b_0}=c_{{b,k}_1-1}^{b_0}(I+\mathrm{\&Delta;t}{\mathrm{\&omega;}}_{{\mathrm{ib},k}_1}^b\&times;)---\left(6\right)$

In the formula, subscript k ₁=1 ~ k _Sum1The number of times label that the expression navigation calculation upgrades,

Be the data of sub-inertial navigation gyro Real-time Collection, $C_{b,0}^{b_0}=C_b^{b_0}\left(t_0\right)=I;$

Simultaneously sub-inertial navigation receives the navigation information of autonomous inertial navigation according to cycle Δ T and stores, and described navigation information comprises speed and course, and records the sequential relationship between main inertial navigation navigation information and sub-inertial navigation instrumented data; By information fusion lead, sub-inertial navigation Transfer Alignment;

3) in step 2) end t ₁Constantly, obtain the real-time carrier matrix in this moment This carrier coordinate system constantly of flash setting is b ₁, have

4) in time period t ₁~ t ₂In, sub-inertial navigation system carries out following work:

41) to step 2) in the sub-inertial navigation inertia type instrument data that gather carry out m circular navigation and resolve and information fusion t ₁~ t ₂In carry out m circular navigation and resolve, each subcycle cycle is: t ₁₀~ t ₁₁, t ₂₀~ t ₂₁..., t _J0~ t _J1... t _M0~ t _M1, j=1 ~ m represents the label in subcycle cycle, in the navigation calculation and information fusion process of each subcycle, and used sub-inertial navigation instrumented data and the corresponding t of main inertial navigation information difference ₀~ t ₁The instrumented data that interior sub-inertial navigation system gathers and the navigation information of main inertial navigation, within each subcycle cycle, navigation calculation update times and information fusion number of times and t ₀~ t ₁Interior number of times equates respectively; Circulation is resolved in the process, the naval vessel is in mooring or at the uniform velocity sails through to state, each subcycle is resolved speed, the position of the zero hour and is directly got subcycle end cycle speed and position constantly, but because there is oscillating motion in boats and ships, the attitude that the zero hour is resolved in each subcycle needs following processing, at each subcycle t _J0~ t _J1The zero hour t _J0Initial attitude matrix:

$C_b^n\left(t_{j0}\right)=C_b^n\left(t_{(j-1)1}\right){\left(C_b^{b_0}\left(t_1\right)\right)}^T---\left(7\right)$

In the formula,

For at t _{(j-1) 1}The attitude matrix of the carrier coordinate system b Relative Navigation coordinate system n that constantly calculates, the attitude matrix when namely end is resolved in j-1 son circulation, $C_b^{b_0}\left(t_1\right)=C_{{b,k}_{\mathrm{sum}1}}^{b_0},$ $C_b^n\left(t_{10}\right)=C_b^n\left(t_1\right){\left(C_b^{b_0}\left(t_1\right)\right)}^T;$

In each subcycle process, by step 2) in the record sequential relationship carry out navigation calculation and information fusion;

42) carry out above-mentioned circular navigation resolve with information fusion in, sub-inertial navigation is carried out Real-time Collection to the inertia type instrument data, upgrade to calculate current carrier coordinate system b system with respect to b ₁The attitude matrix of system Its discrete form is:

$C_{{b,k}_2}^{b_1}=C_{{b,k}_2-1}^{b_1}(I+\mathrm{\&Delta;t}{\mathrm{\&omega;}}_{\mathrm{ib}{k}_2}^b\&times;)---\left(9\right)$

Wherein $C_{b,0}^{b_1}=C_b^{b_1}\left(t_1\right)=I,$ k ₂For

Update times, step 42) finally obtain

5) the circular navigation of finishing m cycle resolve and corresponding information fusion after, sub-inertial navigation system is according to t _M1Carrier coordinate system b is the attitude estimated matrix of n with respect to navigation coordinate constantly

Be t ₁The constantly afterwards estimated value of attitude matrix, and t ₂Carrier coordinate system b is with respect to b constantly ₁Real-time attitude matrix

Obtain t ₂Real-time attitude matrix constantly

Be used for the navigation of sub-inertial navigation; Simultaneously sub-inertial navigation obtains t _M1Gyroscope error estimation value constantly, i.e. t ₁The afterwards estimated value of moment gyro error is with t _M1Gyroscope error estimation value constantly is as final gyroscope error estimation value;

T4) repeatedly carry out above-mentioned steps T2) and step T3), by the form of off-line simulation, select different time point t ₁, obtain different circular navigation and resolve period m, according to step 1)-5) gyro error is estimated fast selecting can be near T1) in the t of estimation effect ₁With m as optimum time point t ₁Count m with the circular navigation computation cycles, formally be used for the Transfer Alignment of carrier-borne master/sub-inertial navigation, described estimation effect refers to T1) the convergence effect of neutron inertial navigation gyro error convergence curve; In the carrier-borne master of reality/sub-inertial navigation Transfer Alignment process, carry out step 1)-5), the realization gyro error is estimated fast.

2. the gyro error method for quick estimating in a kind of carrier-borne master according to claim 1/sub-inertial navigation Transfer Alignment process, it is characterized in that described real-time multi-task operating system is the multiple task real-time operation system VxWorks, finish following four tasks, the scheduling of task is automatically performed by the priority level of multiple task real-time operation system VxWorks according to task, following four tasks of operation in sub-inertial navigation:

1) task one: the collection of sub-inertial navigation instrumented data, storage and navigation calculation and

New Algorithm more, priority level is the highest;

2) task two: sub-inertial navigation instrumented data collection, New Algorithm more, priority level second;

3) task three: sub-inertial navigation is carried out data acquisition, storage and information fusion to main inertial navigation, priority level the 3rd;

Task four: sub-inertial navigation is resolved and information fusion based on the circular navigation of storage data, and priority level is minimum.

3. the gyro error method for quick estimating in a kind of carrier-borne master according to claim 1 and 2/sub-inertial navigation Transfer Alignment process is characterized in that in the Transfer Alignment process that the navigation calculation algorithm of sub-inertial navigation is:

$C_{b,k}^n=C_{b,k-1}^n(I+\mathrm{\&Delta;t}{\mathrm{\&omega;}}_{\mathrm{nb},k}^b\&times;)---\left(1\right)$

$V_k^n=V_{k-1}^n+\mathrm{\&Delta;t}[C_{b,k-1}^n{f}_k^b-(2{\mathrm{\&omega;}}_{\mathrm{ie},k-1}^n+{\mathrm{\&omega;}}_{\mathrm{en},k-1}^n)\&times;V_{k-1}^n+g^n]---\left(2\right)$

$L_k=L_{k-1}+\frac{\mathrm{\&Delta;t}{V}_{N,k-1}^n}{R_M+h_{k-1}},$ ${\mathrm{\&lambda;}}_k={\mathrm{\&lambda;}}_{k-1}+\frac{\mathrm{\&Delta;t}{V}_{E,k-1}^n\sec L_{k-1}}{R_N+h_{k-1}},$ $h_k=h_{k-1}+\mathrm{\&Delta;t}{V}_{U,k-1}^n---\left(3\right)$

In the formula,

Be the attitude matrix of sub-inertial navigation carrier coordinate system b Relative Navigation coordinate system n, I is (3 * 3) unit matrix, subscript k=1 ~+∞ represents the update times label of navigation calculation, and n represents navigation coordinate system, and b represents carrier coordinate system, and i represents inertial coordinates system,

Expression terrestrial coordinate system e is projected angle speed among the n with respect to inertial coordinates system i at navigation coordinate,

The expression navigation coordinate is n with respect to terrestrial coordinate system e is projected angle speed among the n at navigation coordinate; In the formula (1) ${\mathrm{\&omega;}}_{\mathrm{nb}}^b={\mathrm{\&omega;}}_{\mathrm{ib}}^b-{\left(C_b^n\right)}^T({\mathrm{\&omega;}}_{\mathrm{ie}}^n+{\mathrm{\&omega;}}_{\mathrm{en}}^n),$

Be sub-inertial navigation gyro image data, ${\mathrm{\&omega;}}_{\mathrm{en}}^n={\left[\begin{array}{ccc}-V_N^n/(R_M+h)& V_E^n/(R_N+h)& \left(V_E^n\tan L\right)/(R_N+h)\end{array}\right]}^T,$ $V^n=\left[\begin{array}{ccc}{V}_E^n& V_N^n& V_U^n\end{array}\right]$ Be respectively the east, north of sub-inertial navigation, day to speed, L, λ and h represent respectively the lower speed of navigation coordinate system, latitude, longitude and highly, f ^bBe sub-inertial navigation accelerometer image data, g ⁿBe the projection of terrestrial gravitation acceleration in navigation coordinate system, R _MAnd R _NBe respectively meridian circle and the prime vertical radius of the navigation location earth; The vectorial backslash computing of operational symbol " * " expression, under the carrier-borne environment, directly get sub-inertial navigation sky to speed with highly be zero.

4. the gyro error method for quick estimating in a kind of carrier-borne master according to claim 1 and 2/sub-inertial navigation Transfer Alignment process is characterized in that in the Transfer Alignment process that main, sub-inertial navigation information fusion is:

Adopt the Kalman wave filter as the information fusion filtering device, get sub-inertial navigation east orientation/north orientation velocity error, misalignment and gyro error as the system state vector, namely

X=[δV _E δV _N φ _E φ _N φ _U ε _x ε _y ε _z] ^T

Wherein, δ V _E/ δ V _NBe east orientation/north orientation velocity error; φ _E, φ _N, φ _UBe respectively pitching, rolling, course misalignment; ε _x, ε _y, ε _zBe respectively three axle gyro errors;

Get system state equation,

$\stackrel{\&CenterDot;}{X}\left(t\right)=A\left(t\right)X\left(t\right)+W\left(t\right)---\left(4\right)$

In the formula, A (t) is state matrix, and W (t) is system noise, and according to system state variables medium velocity, misalignment error equation, and gyro is from the projection relation of carrier coordinate system b navigation coordinate system n, and state matrix A (t) is expressed as:

$A\left(t\right)=\left[\begin{array}{cccccccc}\frac{V_N}{R}\tan L& {2\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_E}{R}\tan L& 0& {-f}_U& f_N& 0& 0& 0\\ -({2\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_E}{R}\tan L)& 0& f_U& 0& {-f}_E& 0& 0& 0\\ 0& -\frac{1}{R}& 0& {\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_E}{R}\tan L& -({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_E}{R})& -T_{11}& {-T}_{12}& {-T}_{13}\\ \frac{1}{R}& 0& -({\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_E}{R}\tan L)& 0& -\frac{V_N}{R}& {-T}_{21}& -T_{22}& {-T}_{23}\\ \frac{1}{R}\tan L& 0& {\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_E}{R}& \frac{V_N}{R}& 0& -T_{31}& {-T}_{32}& {-T}_{33}\\ 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

Wherein, V _E, V _NBe sub-inertial navigation east orientation and north orientation speed, ω _IeBe rotational-angular velocity of the earth, R is earth radius, and L is local geographic latitude, f _E, f _N, f _UBe respectively sub-inertial navigation acceleration measurement f ^bBe projection among the n at navigation coordinate, T _Pq, p, q=1,2,3 is the attitude matrix of sub-inertial navigation carrier coordinate system b Relative Navigation coordinate system n

Each element;

" speed+course " the information matches mode of employing is got speed, the course difference of son, main inertial navigation and is directly constructed measurement information, and namely son/main inertial navigation navigational system measurement vector is

Z=[V _E-V _MEV _N-V _MNH-H _M] ^TWherein, V _EWith V _MEBe respectively the east orientation speed of son, main inertial navigation; V _NWith V _MNBe respectively the north orientation speed of son, main inertial navigation; H and H _MBe respectively the course information of son, main inertial navigation, H is from sub-inertial navigation attitude matrix

In directly extract;

The system measurements equation is

Z(t)=H(t)X(t)+V(t) （5）

In the formula, H (t) is measurement matrix, and V (t) is measurement noise, according to measuring the relation of vector with state vector, has measurement matrix to be

$H=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& \\ 0& 1& 0& 0& 0& 0_{3\&times;3}\\ 0& 0& -\frac{T_{12}{T}_{32}}{T_{12}^2+T_{22}^2}& -\frac{T_{22}{T}_{32}}{T_{12}^2+T_{22}^2}& -1& \end{array}\right]$

In the information fusion process, adopt the Closed-cycle correction mode, information fusion is estimated that the velocity error, misalignment and the gyroscope error estimation value that obtain feed back to sub-inertial navigation system and participate in navigation calculation.

Images