CN102175095A - Strap-down inertial navigation transfer alignment algorithm parallel implementation method - Google Patents

Abstract

The invention discloses a strap-down inertial navigation transfer alignment algorithm parallel implementation method. A primary binding module, a globe related parameter calculating module, a sub inertial navigation system navigation calculating module, a filter parameter calculating module, a Kalman filter module and an alignment output module are adopted in the method; the primary binding module receives data information of a first frame main inertial navigation system, and the data information is subjected to compensating calculation and then used as an initial value of initial alignment navigation calculation of a sub inertial navigation system; the globe related parameter calculating module, the sub inertial navigation system navigation calculating module, the filter parameter calculating module and the Kalman filter module form a fine alignment process of transfer alignment, and the fine alignment process is circularly executed till reaching the set cycle times; and the alignment output module performs one-time correction on the attitude information of the sub inertial navigation system after the fine alignment process is finished, and outputs attitude, speed and position initial values required by navigation calculation of the sub inertial navigation system. By the method, the calculation velocity of the strap-down inertial navigation transfer alignment algorithm is quickened, and the alignment precision of transfer alignment is improved.

Description

A kind of strap-down inertial Transfer Alignment algorithm Parallel Implementation method

Technical field

The invention belongs to the strap-down inertial field, particularly relate to a kind of Parallel Implementation method of strapdown inertial navigation system Transfer Alignment algorithm.

Background technology

For adapting to the needs of modern war, tactical missile has developed into the medium-scale strike weapon that becomes more and more important.Simultaneously, along with the development of war is also more and more higher to the requirement of the reaction speed of tactical missile and accuracy at target.Tactical missile is generally launched by carrier, generally adopts inertia midcourse guidance and light, electric terminal guidance, and before MISSILE LAUNCHING, the initialization of missile-borne SINS adopts Transfer Alignment to finish usually.The deflection deformation of wing and aircaft configuration and the alignment error of sub-inertial navigation make the true attitude battle array of bookbinding value and sub-inertial navigation inconsistent, about the misalignment of caused sub-inertial navigation once can reach.Therefore, on carrier, the tactical missile inertial navigation system is carried out the key technology that initial alignment just becomes tactical missile quickly and accurately.

Improve in the Transfer Alignment process computing frequency of navigation calculation and digital filtering in the alignment algorithm, can in setting-up time, improve the precision of Transfer Alignment, and then the strike accuracy of lifting weapon.The method of traditional raising computing frequency is to adopt the more computing chip of high frequency.At present, adopt DSP as main process chip, all operational orders all are that serial is carried out in dsp chip more, and such characteristics make Transfer Alignment algorithm calculated rate be difficult to be greatly enhanced.

The PLD technology that with FPGA is representative has in recent years obtained fast development, and abundant configurable logic block resource that high-end FPGA device is not only integrated also comprises a large amount of DSP48 (E) unit towards the computation-intensive application.With regard to hardware, FPGA has incomparable advantage in the parallel computation field.

Traditional serial inertial navigation Transfer Alignment algorithm being carried out parallelization handle, and realized by the FPGA device, is a kind of feasible program that improves inertial navigation Transfer Alignment algorithm computing frequency.Use the parallel computation characteristic of FPGA, Transfer Alignment algorithm implementation is carried out parallelization to be handled, and each module of pass-algorithm is carried out simultaneously by a plurality of flow processs, and can accelerate the computation rate of Transfer Alignment algorithm greatly, the raising of inertial navigation Transfer Alignment precision is had great value.

Summary of the invention

Be difficult to the effectively problem of raising for solving conventional serial inertial navigation Transfer Alignment algorithm arithmetic speed, the invention provides a kind of inertial navigation Transfer Alignment Parallel Implementation method based on FPGA, this method is with the Transfer Alignment algoritic moduleization, to the design that walks abreast of each module, and design realizes on single FPGA, accelerate inertial navigation Transfer Alignment algorithm arithmetic speed greatly, improved the precision of Transfer Alignment.

The technical solution adopted for the present invention to solve the technical problems is: a kind of strap-down inertial Transfer Alignment algorithm Parallel Implementation method, employing speed adds the attitude matching algorithm, resolves module, sub-inertial navigation system navigation calculation module, filtering parameter computing module, Kalman filtering module and aims at output module and form by once binding module, earth relevant parameter.

Described compensation hypercomplex number, the main inertial navigation data of armed lever vector of once binding module according to this locality storage to receiving, carrier attitude quaternion and carrier ground speed compensated calculating after, as the initial value of Transfer Alignment algorithm navigation calculation.Once bind module and comprised the submodule of attitude bookbinding module and two parallel runnings of speed bookbinding module.

Described earth relevant parameter resolves that carrier position, the velocity information that module transmits according to main inertial navigation calculates rotational-angular velocity of the earth, main inertial navigation place navigation coordinate is the angular velocity of rotation of the relative earth and the information such as acceleration of gravity of main inertial navigation position.Earth relevant parameter resolves module and has comprised that it is that angular speed resolves module and local gravitational acceleration resolves the submodule of three parallel runnings of module over the ground that rotational-angular velocity of the earth resolves module, navigation coordinate.

The angular speed that described sub-inertial navigation system navigation calculation module provides according to sub-inertial navigation and compare force signal, carry out navigation calculation to be output as initial value after once binding, wherein the attitude algorithm algorithm adopts the hypercomplex number algorithm, and the velocity calculated algorithm adopts list sample rate algorithm.Sub-inertial navigation system navigation calculation module has comprised that attitude quaternion resolves the submodule of module and two parallel runnings of velocity calculated module.

After the required noise allocation matrix of described filtering parameter computing module computer card Kalman Filtering, state-transition matrix and the coupling amount, result of calculation is passed to the Kalman filtering module, carry out a Kalman filtering and calculate.The filtering parameter computing module has comprised the speed difference calculating module, has calculated the submodule of attitude error angle computing module, noise allocation matrix computations module and four parallel runnings of state-transition matrix computing module.

Described Kalman filtering module has comprised status predication module, estimate covariance prediction module, kalman gain computing module, state estimation module and five modules of covariance estimation module, wherein status predication module, estimate covariance prediction module are according to the filtering parameter parallel running, the estimate covariance prediction module is delivered to the kalman gain computing module with the estimate covariance predicted value and is calculated kalman gain and measurement mean square deviation behind the end of run, start parallel module status estimation module and covariance estimation module at last, draw state estimation value and covariance estimated value.

The error angle antithetical phrase inertial navigation attitude quaternion that described aligning output module obtains according to Kalman Filter Estimation is done once and is revised, and exports as the initial value of sub-inertial navigation navigation calculation in conjunction with sub-inertial navigation velocity amplitude after the compensation and main inertial navigation positional information.

Compared with prior art, advantage of the present invention is: each Module Division of Transfer Alignment serial algorithm has been become the submodule of some executed in parallel, and adopted FPGA to realize further improving the algorithm degree of parallelism, improved the arithmetic speed of inertial navigation Transfer Alignment greatly.As shown in table 1, with the example that is estimated as at north orientation misalignment angle, the fast more then alignment precision of arithmetic speed is high more, therefore, improves the precision that arithmetic speed can improve Transfer Alignment, has great importance.

Table 1 calculated rate and evaluated error relation table (north orientation quickens 15s)

Calculated rate (Hz)	50	100	200	500	1000
						Evaluated error (mrad)	-2.08	-1.20	-0.68	-0.33	-0.19

Description of drawings

Fig. 1 is an algorithm flow chart of the present invention.

Fig. 2 is the parallel design principle figure of module that once binds of the present invention.

Fig. 3 is that earth relevant parameter of the present invention resolves the parallel design principle figure of module.

Fig. 4 is the parallel design principle figure of sub-inertial navigation system navigation calculation module of the present invention.

Fig. 5 is the parallel design principle figure of filtering parameter computing module of the present invention.

Fig. 6 is the parallel design principle figure of Kalman filtering module of the present invention.

Fig. 7 is the parallel design principle figure of aligning output module of the present invention.

The specific embodiment

The formula symbol description is as follows:

The height above sea level at place, h carrier place

The latitude at place, l carrier place

g ₀The acceleration of gravity size on sea level, equator

The T computing cycle

Q _kThe system noise matrix

R _kThe measurement noise matrix

f _A/q() changes the attitude angle dress into the function of corresponding attitude quaternion

Below to add the attitude matching algorithm with speed commonly used in the current moving pedestal Transfer Alignment be example, specify Parallel Implementation method of the present invention.

Transfer Alignment algorithm flow chart of the present invention comprises as shown in Figure 1: once bind module, earth relevant parameter and resolve module, sub-inertial navigation system navigation calculation module, filtering parameter computing module, Kalman filtering module and aim at output module.The main-process stream of this algorithm is: 1) carry out and once bind module (M1), main inertial navigation attitude quaternion, main inertial navigation speed in the input data are carried out single compensation after, as the initial value of sub-inertial navigation attitude algorithm and velocity calculated; 2) carry out earth related data and resolve module (M2),, calculate current rotational-angular velocity of the earth, navigation coordinate and be angular speed and local gravitational acceleration over the ground according to main inertial navigation position and the main inertial navigation speed in the input data; 3) call sub-inertial navigation navigation calculation module (M3), separate operator inertial navigation attitude quaternion and sub-inertial navigation ground speed; 4) call filtering parameter computing module (M4), the time-varying parameter of using in the computer card Kalman Filtering; 5) call Kalman filtering module (M5), carry out a Kalman filtering according to the filtering parameter that transmits and calculate; 6) judge whether filter times reaches setting value N, if do not reach then continue the process of execution in step 2 to 5; 7) call aligning output module (M6), antithetical phrase inertial navigation attitude quaternion is once revised, and exports the required attitude of sub-inertial navigation navigation calculation, speed and position initial value.

The module of once binding of the present invention walks abreast design principle figure as shown in Figure 2, once binds attitude bookbinding module (M1_1) and speed bookbinding module (M1_2) that module (M1) comprises concurrent operation.Attitude bookbinding module (M1_1) is according to the main inertial navigation attitude quaternion q in the input data _NbWith the given compensation hypercomplex number q of system _Comp, antithetical phrase inertial navigation attitude is bound, formula specific as follows:

${\mathrm{<figure-callout\; id="0"\; label="q\; ns"\; filenames="HDA0000048560790000031.png"\; state="\{\{state\}\}">q</figure-callout>}}_{\mathrm{ns}}=q_{\mathrm{nb}}\&CircleTimes;q_{\mathrm{comp}}---\left(1\right)$

Speed bookbinding module (M1_2) is according to main inertial navigation speed

Main inertial navigation angular speed

And armed lever vector r ^b, antithetical phrase inertial navigation speed is bound, formula specific as follows:

$V_{s0}^n=V_m^n+C_b^n({\mathrm{\&omega;}}_{\mathrm{ib}}^b\&times;r^b)---\left(2\right)$

After once binding module (M1) computing end, output q _Ns0,

As sub-inertial navigation navigation calculation push up attitude and speed initial value.

Earth relevant parameter of the present invention resolves the parallel design principle figure of module as shown in Figure 3, and earth relevant parameter resolves module (M2) and comprises that the rotational-angular velocity of the earth of concurrent operation resolves module (M2_1), navigation coordinate is that rotational angular velocity resolves module (M2_2) and local gravitational acceleration resolves module (M2_3).Rotational-angular velocity of the earth resolves module (M2_1) according to the main inertial navigation position P in the input data _m, resolve current time navigation coordinate system rotational-angular velocity of the earth down

Navigation coordinate is that rotational angular velocity resolves module (M2_2) according to the main inertial navigation position P in the input data _mWith main inertial navigation speed

Resolve navigation coordinate system down navigation coordinate be the rotational angular velocity of spherical coordinate system relatively

Local gravitational acceleration resolves module (M2_3) according to the main inertial navigation position P in the input data _m, calculate g ⁿ, formula specific as follows:

$g^n=[g_0(1+5.27094*{10}^{-3}{\sin }^{2}l+2.32718*{10}^{-5}{\sin }^{4}l)-3.086*{10}^{-6}{h}^{}]\left[\begin{array}{c}0\\ 0\\ -1\end{array}\right]---\left(3\right)$

Sub-inertial navigation system navigation calculation module of the present invention walks abreast design principle figure as shown in Figure 4, and sub-inertial navigation system navigation calculation module (M3) comprises that the attitude quaternion of concurrent operation resolves module (M3_1) and velocity calculated module (M3_2).Attitude quaternion resolves module (M3_1) according to rotational-angular velocity of the earth Navigation coordinate is the rotational angular velocity of spherical coordinate system relatively

With sub-inertial navigation angular speed

The attitude quaternion q of antithetical phrase inertial navigation _NsUpgrade formula specific as follows:

$q_{\mathrm{ns}}\left(t_{k+1}\right)=q_{\mathrm{ns}}\left(t_k\right)\&CircleTimes;q\left(h\right)---\left(4a\right)$

$q\left(h\right)=\cos \frac{\mathrm{\&Phi;}}{2}+\frac{\mathrm{\&Phi;}}{\mathrm{\&Phi;}}\sin \frac{\mathrm{\&Phi;}}{2}---\left(4b\right)$

$\mathrm{\&Phi;}=[{\mathrm{\&omega;}}_{\mathrm{ib}}^b-C_n^b({\mathrm{\&omega;}}_{\mathrm{ie}}^n+{\mathrm{\&omega;}}_{\mathrm{en}}^n)]*T---\left(4c\right)$

Velocity calculated module (M3_2) is according to rotational-angular velocity of the earth Navigation coordinate is the rotational angular velocity of spherical coordinate system relatively

Sub-inertial navigation angular speed Sub-inertial navigation specific force acceleration

The attitude quaternion q of sub-inertial navigation _NsWith local gravitational acceleration g ⁿThe speed of antithetical phrase inertial navigation

Upgrade formula specific as follows:

$V_s^n\left(t_{k+1}\right)=V_s^n\left(t_k\right)+C_b^n\mathrm{\&Delta;}{V}_{\mathrm{sfm}}+\mathrm{\&Delta;}{V}_{g/\mathrm{corm}}---\left(5a\right)$

$\mathrm{\&Delta;}{V}_{g/\mathrm{corm}}=[g^n-(2{\mathrm{\&omega;}}_{\mathrm{ie}}^n+{\mathrm{\&omega;}}_{\mathrm{en}}^n)\&times;V_s^n\left(t_k\right)]*T---\left(5b\right)$

ΔV _sfm＝ΔV _m+Δθ _m×ΔV _m (5c)

$\mathrm{\&Delta;}{\mathrm{\&theta;}}_m={\mathrm{\&omega;}}_{\mathrm{is}}^{s}T---\left(5d\right)$

$\mathrm{\&Delta;}{V}_m=f_{\mathrm{sf}}^{s}T---\left(5e\right)$

Filtering parameter computing module of the present invention walks abreast design principle figure as shown in Figure 5, and filtering parameter computing module (M4) comprises speed difference calculating module (M4_1), calculating attitude error angle computing module (M4_2), noise allocation matrix computations module (M4_3) and the state-transition matrix computing module (M4_4) of concurrent operation.Speed difference calculating module (M4_1) is at first calculated the main inertial navigation speed after the compensation

Use sub-inertial navigation speed again Deduct

Obtain velocity error Δ V _cCalculate attitude error angle computing module (M4_2), according to main inertial navigation hypercomplex number q _NbWith sub-inertial reference calculation attitude q _NsCalculate sub-inertial navigation and calculate the Eulerian angles of carrier coordinate system, calculate the attitude error angle to main inertial navigation carrier coordinate system

Noise allocation matrix computations module (M4_3) is according to sub-inertial reference calculation attitude q _NsCalculate noise allocation matrix Γ _K/k-1State-transition matrix computing module (M4_4) is according to sub-inertial reference calculation attitude q _Ns, sub-inertial navigation angular speed Rotational-angular velocity of the earth

Navigation coordinate is an angular speed over the ground

And sub-inertial navigation specific force acceleration

Calculate state-transition matrix Φ _K/k-1

Kalman filtering module of the present invention walks abreast design principle figure as shown in Figure 6, and Kalman filtering module (M5) comprises status predication module (M5_1), estimate covariance prediction module (M5_2), kalman gain computing module (M5_3), state estimation module (M5_4) and covariance estimation module (M5_5).Status predication module (M5_1) is according to state-transition matrix Φ _K/k-1System mode X with a last moment _kObtain system mode predicted value X _K/k-1, formula specific as follows:

X _K/k-1=Φ _K/k-1X _k(6) estimate covariance prediction module (M5_2) is according to state-transition matrix Φ _K/k-1, noise allocation matrix Γ _K/k-1Estimate covariance P with a last moment _kObtain estimate covariance prediction P _K/k-1, formula specific as follows:

$P_{k/k-1}={\mathrm{\&Phi;}}_{k/k-1}{P}_k{\mathrm{\&Phi;}}_{k/k-1}^T+{\mathrm{\&Gamma;}}_{k/k-1}{Q}_k{\mathrm{\&Gamma;}}_{k/k-1}^T---\left(7\right)$

Status predication module (M5_1) and estimate covariance prediction module (M5_2) executed in parallel are called kalman gain computing module (M5_3) after finishing.Kalman gain computing module (M5_3) is according to estimate covariance prediction P _K/k-1Calculate Kalman filtering gain K _k, formula specific as follows:

$P_{\mathrm{zz}}=H_k{P}_{k/k-1}{H}_k^T+R_k---\left(8a\right)$

$K_k=P_{k/k-1}{H}_k^T{P_{\mathrm{zz}}}^{-1}---\left(8b\right)$

State estimation module (M5_4) is according to calculating the attitude error angle

Velocity error Δ V _c, system mode predicted value X _K/k-1With Kalman filtering gain K _kCalculate the system mode X of estimation _k, formula specific as follows:

X _k=X _K/k-1+ K _k(z _k-H _kX _K/k-1) (9b) covariance estimation module (M5_5) is according to measuring prediction mean square deviation P _Zz, Kalman filtering gain K _kWith estimate covariance prediction P _K/k-1Calculate the covariance P of system of estimation _k, formula specific as follows:

P _k=(I-K _kH _k) P _K/k-1(10) state estimation module (M5_4) and covariance estimation module (M5_5) executed in parallel, complete back judge whether filter times has reached setting value N.

The parallel design principle figure of aligning output module of the present invention aims at output module (M6) and comprises the attitude quaternion correcting module (M6_1) of executed in parallel and speed, position assignment module (M6_2) as shown in Figure 7.Attitude quaternion correcting module (M6_1) is according to estimating system state X _kIn the evaluated error angle Hypercomplex number q is calculated in the antithetical phrase inertial navigation _NsOnce revise the attitude initial value q that obtains sub-inertial navigation navigation calculation through row _Ns/c0, formula specific as follows:

Main inertial navigation speed after speed, position assignment module (M6_2) will compensate

Speed initial value as sub-inertial navigation navigation calculation

With main inertial navigation position Pos _mPosition initial value Pos as sub-inertial navigation navigation calculation _S/c0

Claims (5)

1. strap-down inertial Transfer Alignment algorithm Parallel Implementation method, resolve module, sub-inertial navigation system navigation calculation module, filtering parameter computing module, Kalman filtering module and aim at output module and form by once binding module, earth relevant parameter, it is characterized in that:

The described module of once binding is made up of the attitude bookbinding module and the speed bookbinding module of concurrent operation;

Described earth relevant parameter resolves module, and to resolve module, navigation coordinate by the rotational-angular velocity of the earth of concurrent operation be that angular speed resolves module and local gravitational acceleration and resolves module and form over the ground;

Described sub-inertial navigation system navigation calculation module resolves module by the attitude quaternion of concurrent operation and the velocity calculated module is formed;

Described filtering parameter computing module is made up of speed difference calculating module, calculating attitude error angle computing module, noise allocation matrix computations module and the state-transition matrix computing module of concurrent operation;

Described Kalman filtering module is made up of status predication module, estimate covariance prediction module, kalman gain computing module, state estimation module and covariance estimation module;

Described aligning output module is made up of attitude quaternion correcting module and speed, the position assignment module of concurrent operation;

The described module of once binding receives the navigation information of first frame master inertial navigation system and gyroscope signal, accelerometer signal, after compensation is calculated as the initial value of sub-initial Alignment of Inertial Navigation System navigation calculation; Described earth relevant parameter resolves the fine alignment process that module, sub-inertial navigation system navigation calculation module, filtering parameter computing module and Kalman filtering module are formed Transfer Alignment, and the circulation of fine alignment process is carried out till reaching setting cycle-index N; Described aligning output module is after the fine alignment process finishes, and the attitude information of antithetical phrase inertial navigation system carries out disposable correction, and exports the required attitude of sub-inertial navigation system navigation calculation, speed and position initial value.

2. strap-down inertial Transfer Alignment algorithm Parallel Implementation method according to claim 1 is characterized in that: described earth relevant parameter resolves that position, the velocity information that module transmits according to main inertial navigation calculates rotational-angular velocity of the earth, main inertial navigation place navigation coordinate is the angular velocity of rotation of the relative earth and the acceleration of gravity of main inertial navigation position.

3. strap-down inertial Transfer Alignment algorithm Parallel Implementation method according to claim 1, it is characterized in that: the angular speed that described sub-inertial navigation system navigation calculation module provides according to sub-inertial navigation and compare force signal, carry out navigation calculation to be output as initial value after once binding, comprise that attitude quaternion resolves and velocity calculated.

4. strap-down inertial Transfer Alignment algorithm Parallel Implementation method according to claim 1, it is characterized in that: after required noise allocation matrix, state-transition matrix and the match parameter of described filtering parameter computing module computer card Kalman Filtering, result of calculation is passed to the Kalman filtering module, carry out a Kalman filtering and calculate.

5. strap-down inertial Transfer Alignment algorithm Parallel Implementation method according to claim 1, it is characterized in that: the navigation information of described main inertial navigation system comprises carrier attitude quaternion, carrier ground speed, carrier position.

Images