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

Abstract

The invention discloses a parallel implementation method of a strapdown inertial navigation transfer alignment algorithm. It consists of a primary binding module, an earth-related parameter calculation module, a sub-inertial navigation system navigation calculation module, a filter parameter calculation module, a Kalman filter module, and an alignment output module. The primary binding module receives the first frame of data information from the main inertial navigation system , which is used as the initial value of the sub-inertial navigation system's initial alignment navigation calculation after compensation calculation; the earth-related parameter calculation module, sub-inertial navigation system navigation calculation module, filter parameter calculation module and Kalman filter module constitute the transfer alignment The fine alignment process is executed cyclically until the set number of cycles is reached; after the fine alignment process is completed, the alignment output module performs a one-time correction on the attitude information of the sub-inertial navigation system, and outputs the navigation solution result of the sub-inertial navigation system. Initial values of attitude, velocity and position required. The method of the invention accelerates the calculation rate of the transfer alignment algorithm of the strapdown inertial navigation and improves the alignment accuracy of the transfer alignment.

Description

A Parallel Implementation Method of Strapdown Inertial Navigation Transfer Alignment Algorithm

technical field

The invention belongs to the field of strapdown inertial navigation, and in particular relates to a parallel implementation method of a transfer alignment algorithm of a strapdown inertial navigation system.

Background technique

In order to meet the needs of modern warfare, tactical missiles have developed into increasingly important medium-scale strike weapons. At the same time, with the development of war, the requirements for the reaction speed and hit accuracy of tactical missiles are getting higher and higher. Tactical missiles are generally launched by carriers, and generally use inertial guidance and optical and electronic terminal guidance. Before the missile is launched, the initialization of the missile-borne strapdown inertial navigation system is usually completed by transfer alignment. The deflection deformation of the wing and aircraft structure and the installation error of the sub-inertial navigation make the binding value inconsistent with the real attitude array of the sub-inertial navigation, and the misalignment angle of the sub-inertial navigation caused by it can reach about one degree. Therefore, the initial alignment of the tactical missile inertial navigation system on the carrier quickly and accurately becomes a key technology of the tactical missile.

Increasing the operation frequency of navigation calculation and digital filtering in the alignment algorithm in the process of transfer alignment can improve the accuracy of transfer alignment within the set time, thereby improving the strike accuracy of weapons. The traditional way to increase the operating frequency is to use higher frequency computing chips. At present, DSP is mostly used as the main processing chip, and all operation instructions in the DSP chip are executed serially, which makes it difficult to greatly increase the calculation frequency of the transfer alignment algorithm.

In recent years, programmable logic device technology represented by FPGA has achieved rapid development. High-end FPGA devices not only integrate rich configurable logic block resources, but also include a large number of DSP48(E) units for computing-intensive applications. As far as hardware is concerned, FPGA has incomparable advantages in the field of parallel computing.

Parallelizing the traditional serial SINS transfer alignment algorithm and implementing it with FPGA devices is a feasible solution to increase the operation frequency of the SINS transfer alignment algorithm. Applying the parallel computing characteristics of FPGA, parallelize the execution process of the transfer alignment algorithm, and make each module of the transfer algorithm run simultaneously according to multiple processes, which can greatly speed up the calculation rate of the transfer alignment algorithm, and the strapdown inertial navigation transfer pair The improvement in quasi-accuracy is of great value.

Contents of the invention

In order to solve the problem that the operation rate of the traditional serial strapdown inertial navigation transfer alignment algorithm is difficult to effectively improve, the present invention provides an FPGA-based strapdown inertial navigation transfer alignment parallel implementation method, which modularizes the transfer alignment algorithm , each module is designed in parallel, and designed and implemented on a single FPGA, which greatly speeds up the calculation speed of the strapdown inertial navigation transfer alignment algorithm and improves the transfer alignment accuracy.

The technical solution adopted by the present invention to solve its technical problems is: a parallel implementation method of strapdown inertial navigation transfer alignment algorithm, which adopts a speed plus attitude matching algorithm, and consists of a primary binding module, an earth-related parameter calculation module, and a sub-inertial navigation system It consists of a navigation calculation module, a filter parameter calculation module, a Kalman filter module and an alignment output module.

According to the main inertial navigation data received by the locally stored compensation quaternion and the arm lever vector, the one-time binding module compensates and calculates the attitude quaternion of the carrier and the speed of the carrier to the ground, and uses it as a transfer alignment algorithm navigation The initial value for the solution. The primary binding module includes two sub-modules running in parallel, the posture binding module and the speed binding module.

The earth-related parameter calculation module calculates and obtains information such as the angular velocity of the earth's rotation, the rotational angular velocity of the navigation coordinate system where the main inertial navigation is located relative to the earth, and the gravitational acceleration at the location of the main inertial navigation according to the carrier position and speed information transmitted by the main inertial navigation . The earth-related parameter calculation module includes three parallel-running sub-modules: the earth rotation angular velocity calculation module, the navigation coordinate system ground angular velocity calculation module and the local gravity acceleration calculation module.

The sub-inertial navigation system navigation calculation module is based on the angular velocity and the specific force signal provided by the sub-inertial navigation system, and uses the output after one binding as the initial value to perform navigation calculation, wherein the attitude calculation algorithm adopts the quaternion algorithm, and the velocity solution The calculation algorithm adopts the single-sample velocity algorithm. The sub-inertial navigation system navigation calculation module includes two sub-modules running in parallel, the attitude quaternion calculation module and the speed calculation module.

After the filter parameter calculation module calculates the noise allocation matrix, state transition matrix and matching quantity required by Kalman filtering, the calculation result is passed to the Kalman filtering module to perform a Kalman filtering calculation. The filter parameter calculation module includes four parallel sub-modules: the speed difference calculation module, the attitude error angle calculation module, the noise distribution matrix calculation module and the state transition matrix calculation module.

Described Kalman filtering module has included five modules of state prediction module, estimation covariance prediction module, Kalman gain calculation module, state estimation module and covariance estimation module, wherein state prediction module, estimation covariance prediction module according to filter parameter Run in parallel, after the running, the estimated covariance prediction module sends the estimated covariance prediction value to the Kalman gain calculation module to calculate the Kalman gain and measurement mean square error, and finally start the parallel module state estimation module and covariance estimation module, and get State estimates and covariance estimates.

The alignment output module performs a correction on the sub-inertial navigation attitude quaternion according to the error angle estimated by the Kalman filter, and combines the compensated sub-inertial navigation speed value and the main inertial position information as the sub-inertial navigation solution Calculated initial value output.

Compared with the prior art, the present invention has the advantages of: dividing each module of transfer alignment serial algorithm into several sub-modules executed in parallel, and adopting FPGA to further improve the parallelism of the algorithm, greatly improving the efficiency of SINS The operation rate of the transfer alignment. As shown in Table 1, taking the estimation of the north misalignment angle as an example, the faster the operation rate, the higher the alignment accuracy. Therefore, increasing the operation rate can improve the accuracy of transfer alignment, which is of great significance.

Table 1. Relationship between calculation frequency and estimation error (acceleration in the north direction for 15s)

Calculation frequency (Hz) 50 100 200 500 1000 Estimate 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 a schematic diagram of the parallel design of the primary binding module of the present invention.

Fig. 3 is a schematic diagram of the parallel design of the earth-related parameter calculation module of the present invention.

Fig. 4 is a schematic diagram of parallel design of the sub-inertial navigation system navigation calculation module of the present invention.

Fig. 5 is a schematic diagram of parallel design of the filter parameter calculation module of the present invention.

Fig. 6 is a schematic diagram of the parallel design of the Kalman filter module of the present invention.

FIG. 7 is a schematic diagram of the parallel design of the alignment output module of the present invention.

Detailed ways

The formula symbols are explained as follows:

h Altitude where the carrier is located

l The latitude of the carrier

g ₀ is the gravitational acceleration at equatorial sea level

T calculation cycle

Q _k system noise matrix

R _k measurement noise matrix

f _a/q () A function that converts the attitude angle into the corresponding attitude quaternion

In the following, the parallel implementation method of the present invention will be described in detail by taking the commonly used speed plus attitude matching algorithm in the transfer alignment of the moving base as an example.

The transfer alignment algorithm flow chart of the present invention is shown in Figure 1, including: a binding module, an earth-related parameter calculation module, a sub-inertial navigation system navigation calculation module, a filter parameter calculation module, a Kalman filter module and an alignment output module. The overall process of the algorithm is: 1) Execute the binding module (M1) once, and after compensating the main inertial navigation attitude quaternion and main inertial navigation speed in the input data, it is used as the sub-inertial navigation attitude calculation and speed calculation 2) Execute the earth-related data calculation module (M2), and calculate the current angular velocity of the earth, the angular velocity of the navigation coordinate system and the local acceleration of gravity according to the main inertial navigation position and main inertial navigation velocity in the input data ; 3) Call the sub-inertial navigation solution module (M3) to solve the sub-inertial navigation attitude quaternion and sub-inertial speed to the ground; 4) Call the filter parameter calculation module (M4) to calculate the Kalman filter used Time-varying parameters; 5) call the Kalman filter module (M5), and perform a Kalman filter calculation according to the passed filter parameters; 6) judge whether the number of filters reaches the set value N, if not, continue to execute steps 2 to 5 Process; 7) calling the alignment output module (M6), performing a correction to the attitude quaternion of the sub-inertial navigation, and outputting the initial values of attitude, velocity and position required for the sub-inertial navigation solution.

The schematic diagram of parallel design of one-time binding module of the present invention is shown in Fig. 2, one-time binding module (M1) includes posture binding module (M1_1) and speed binding module (M1_2) of parallel operation. The attitude binding module (M1_1) binds the sub-inertial navigation attitude according to the main inertial navigation attitude quaternion q _nb in the input data and the compensation quaternion q _comp given by the system, as follows:

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

主惯导角速度

以及臂杆矢量r^b，对子惯导速度进行装订，具体如下式：The speed binding module (M1_2) according to the main inertial navigation speed

Primary inertial angular velocity

and the arm lever vector r ^b , to bind the sub-inertial navigation speed, the specific formula is as follows:

${\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; the\; <figure-callout\; id="0"\; label="s"\; filenames="HDA0000048560790000031.png"\; state="\{\{state\}\}">s</figure-callout></span>}\mathrm{<span\; class="notranslate">\; 0</span>}}^{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}}^{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; b</span>}}^{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; ib</span>}}^{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; \&times;</span>{\mathrm{<span\; class="notranslate">\; r</span>}}^{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>$

作为子惯导导航解算顶得姿态和速度初值。After the operation of the binding module (M1) finishes, output q _ns0 ,

As a sub-inertial navigation solution, the attitude and velocity initial values are obtained.

导航坐标系转动角速度解算模块(M2_2)根据输入数据中的主惯导位置P_m和主惯导速度

解算导航坐标系下导航坐标系相对地球坐标系的转动角速度

当地重力加速度解算模块(M2_3)根据输入数据中的主惯导位置P_m，解算出gⁿ，具体如下式：As shown in Figure 3, the parallel design principle diagram of the earth-related parameter calculation module of the present invention, the earth-related parameter calculation module (M2) includes a parallel computing earth rotation angular velocity calculation module (M2_1), a navigation coordinate system rotation angular velocity calculation module (M2_2) and the local gravity acceleration calculation module (M2_3). The earth rotation angular velocity calculation module (M2_1) calculates the earth rotation angular velocity in the navigation coordinate system at the current moment according to the main inertial navigation position P _m in the input data

The navigation coordinate system rotation angular velocity calculation module (M2_2) is based on the main inertial navigation position P _m and the main inertial navigation velocity in the input data

Calculate the rotational angular velocity of the navigation coordinate system relative to the earth coordinate system in the navigation coordinate system

The local gravitational acceleration calculation module (M2_3) calculates g ⁿ according to the main inertial navigation position P _m in the input data, the specific formula is as follows:

${\mathrm{<span\; class="notranslate">\; g</span>}}^{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; =</span><span\; class="notranslate">\; [</span>{\mathrm{<span\; class="notranslate">\; g</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 5.27094</span>}<span\; class="notranslate">\; *</span>{\mathrm{<span\; class="notranslate">\; 10</span>}}^{<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 3</span>}}{\mathrm{<span\; class="notranslate">\; sin</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}\mathrm{<span\; class="notranslate">\; l</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 2.32718</span>}<span\; class="notranslate">\; *</span>{\mathrm{<span\; class="notranslate">\; 10</span>}}^{<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 5</span>}}{\mathrm{<span\; class="notranslate">\; sin</span>}}^{\mathrm{<span\; class="notranslate">\; 4</span>}}\mathrm{<span\; class="notranslate">\; l</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 3.086</span>}<span\; class="notranslate">\; *</span>{\mathrm{<span\; class="notranslate">\; 10</span>}}^{<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 6</span>}}{\mathrm{<span\; class="notranslate">\; h</span>}}^{}<span\; class="notranslate">\; ]</span>\left[\begin{array}{c}\mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}\\ <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; )</span>$

和子惯导角速度

对子惯导的姿态四元数q_ns进行更新，具体如下式：Sub-inertial navigation system navigation solution module parallel design schematic diagram as shown in Figure 4 of the present invention, sub-inertial navigation system navigation solution module (M3) comprises the attitude quaternion solution module (M3_1) and speed solution module of parallel operation module (M3_2). Attitude quaternion calculation module (M3_1) according to the earth's rotation angular velocity The rotational angular velocity of the navigation coordinate system relative to the earth coordinate system

and sub inertial angular velocity

Update the attitude quaternion q _ns of the sub-inertial navigation, as follows:

${\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; ns</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; ns</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&CircleTimes;</span>\mathrm{<span\; class="notranslate">\; q</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 4</span>}\mathrm{<span\; class="notranslate">\; a</span>}<span\; class="notranslate">\; )</span>$

$\mathrm{<span\; class="notranslate">\; q</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; cos</span>}\frac{\mathrm{<span\; class="notranslate">\; \&Phi;</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>\frac{\mathrm{<span\; class="notranslate">\; \&Phi;</span>}}{\mathrm{<span\; class="notranslate">\; \&Phi;</span>}}\mathrm{<span\; class="notranslate">\; sin</span>}\frac{\mathrm{<span\; class="notranslate">\; \&Phi;</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 4</span>}\mathrm{<span\; class="notranslate">\; b</span>}<span\; class="notranslate">\; )</span>$

$\mathrm{<span\; class="notranslate">\; \&Phi;</span>}<span\; class="notranslate">\; =</span><span\; class="notranslate">\; [</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; ib</span>}}^{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}^{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; ie</span>}}^{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; en</span>}}^{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; ]</span><span\; class="notranslate">\; *</span>\mathrm{<span\; class="notranslate">\; T</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 4</span>}\mathrm{<span\; class="notranslate">\; c</span>}<span\; class="notranslate">\; )</span>$

子惯导角速度子惯导比力加速度

子惯导的姿态四元数q_ns和当地重力加速度gⁿ对子惯导的速度

进行更新，具体如下式：The velocity calculation module (M3_2) is based on the angular velocity of the earth's rotation The rotational angular velocity of the navigation coordinate system relative to the earth coordinate system

sub inertial angular velocity Sub-inertial navigation specific force acceleration

The sub-inertial navigation attitude quaternion q _ns and the local gravitational acceleration g ⁿ pair the speed of the sub-inertial navigation

Update as follows:

${\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}}^{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}}^{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; b</span>}}^{\mathrm{<span\; class="notranslate">\; no</span>}}\mathrm{<span\; class="notranslate">\; \&Delta;</span>}{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; sfm</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&Delta;</span>}{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; g</span>}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; corm</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 5</span>}\mathrm{<span\; class="notranslate">\; a</span>}<span\; class="notranslate">\; )</span>$

$\mathrm{<span\; class="notranslate">\; \&Delta;</span>}{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; g</span>}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; corm</span>}}<span\; class="notranslate">\; =</span><span\; class="notranslate">\; [</span>{\mathrm{<span\; class="notranslate">\; g</span>}}^{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; ie</span>}}^{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; en</span>}}^{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&times;</span>{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}}^{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; ]</span><span\; class="notranslate">\; *</span>\mathrm{<span\; class="notranslate">\; T</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 5</span>}\mathrm{<span\; class="notranslate">\; b</span>}<span\; class="notranslate">\; )</span>$

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

$\mathrm{<span\; class="notranslate">\; \&Delta;</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; is</span>}}^{\mathrm{<span\; class="notranslate">\; the\; s</span>}}\mathrm{<span\; class="notranslate">\; T</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 5</span>}\mathrm{<span\; class="notranslate">\; d</span>}<span\; class="notranslate">\; )</span>$

$\mathrm{<span\; class="notranslate">\; \&Delta;</span>}{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; sf</span>}}^{\mathrm{<span\; class="notranslate">\; the\; s</span>}}\mathrm{<span\; class="notranslate">\; T</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 5</span>}\mathrm{<span\; class="notranslate">\; e</span>}<span\; class="notranslate">\; )</span>$

再用子惯导速度减去

得到速度误差ΔV_c；计算姿态误差角计算模块(M4_2)，根据主惯导四元数q_nb和子惯导解算姿态q_ns计算得到子惯导计算载体坐标系到主惯导载体坐标系的欧拉角，计算姿态误差角

噪声分配矩阵计算模块(M4_3)根据子惯导解算姿态q_ns计算得到噪声分配矩阵Γ_k/k-1；状态转移矩阵计算模块(M4_4)根据子惯导解算姿态q_ns、子惯导角速度地球自转角速度

导航坐标系对地角速度

以及子惯导比力加速度

解算出状态转移矩阵Φ_k/k-1。The filter parameter calculation module parallel design schematic diagram of the present invention is as shown in Figure 5, and the filter parameter calculation module (M4) includes the speed difference calculation module (M4_1) of parallel operation, the calculation attitude error angle calculation module (M4_2), and the noise distribution matrix Calculation module (M4_3) and state transition matrix calculation module (M4_4). The speed difference calculation module (M4_1) first calculates the main inertial navigation speed after compensation

Reuse sub-inertial speed minus

Obtain the speed error ΔV _c ; calculate the attitude error angle calculation module (M4_2), calculate and obtain the sub-inertial navigation calculation carrier coordinate system to the main inertial navigation carrier coordinate system according to the main inertial navigation quaternion q _nb and the sub-inertial navigation solution attitude q _ns Euler angle, calculate attitude error angle

The noise allocation matrix calculation module (M4_3) calculates the noise allocation matrix Γ _k/k-1 according to the attitude q _ns calculated by the sub-inertial navigation; the state transition matrix calculation module (M4_4) calculates the attitude q _ns and sub-inertial navigation angular velocity Earth's rotational angular velocity

Angular velocity of the navigation coordinate system to the ground

and sub inertial navigation specific force acceleration

Solve and calculate the state transition matrix Φ _k/k-1 .

As shown in Figure 6, the Kalman filter module parallel design schematic diagram of the present invention, the Kalman filter module (M5) includes a state prediction module (M5_1), an estimated covariance prediction module (M5_2), a Kalman gain calculation module (M5_3), State estimation module (M5_4) and covariance estimation module (M5_5). The state prediction module (M5_1) obtains the system state prediction value X _{k/k-1 according to the state transition matrix Φ k/k-1 and} the system state X _k at the last moment, specifically as follows:

X _k/k-1 ＝Φ _k/k-1 X _k (6) Estimated covariance prediction module (M5_2) according to the state transition matrix Φ _k/k-1 , the noise distribution matrix Γ _k/k-1 and the previous moment The estimated covariance P _k to get the estimated covariance prediction P _k/k-1 , the specific formula is as follows:

${\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&Phi;</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}{\mathrm{<span\; class="notranslate">\; \&Phi;</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&Gamma;</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; Q</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}{\mathrm{<span\; class="notranslate">\; \&Gamma;</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 7</span>}<span\; class="notranslate">\; )</span>$

The state prediction module (M5_1) and the estimated covariance prediction module (M5_2) are executed in parallel, and the Kalman gain calculation module (M5_3) is called after completion. The Kalman gain calculation module (M5_3) calculates the Kalman filter gain K _k according to the estimated covariance prediction P _k/k-1 , specifically as follows:

${\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; zz</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}{\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 8</span>}\mathrm{<span\; class="notranslate">\; a</span>}<span\; class="notranslate">\; )</span>$

${\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}{{\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; zz</span>}}}^{<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 8</span>}\mathrm{<span\; class="notranslate">\; b</span>}<span\; class="notranslate">\; )</span>$

速度误差ΔV_c、系统状态预测值X_k/k-1和卡尔曼滤波增益K_k计算得到估计的系统状态X_k，具体如下式：The state estimation module (M5_4) calculates the attitude error angle according to

The estimated system state X _k is obtained by calculating the speed error ΔV _c , the system state prediction value X _k/k-1 and the Kalman filter gain K _k , as follows:

X _k ＝X _k/k-1 +K _k (z _k -H _k X _k/k-1 ) (9b) The covariance estimation module (M5_5) predicts the mean square error P _zz and the Kalman filter gain K _k according to the measurement and the estimated covariance prediction P _k/k-1 to calculate the estimated system covariance P _k , specifically as follows:

P _k =(IK _k H _k )P _k/k-1 (10) The state estimation module (M5_4) and the covariance estimation module (M5_5) are executed in parallel, and after the execution is completed, it is judged whether the number of filtering times has reached the set value N.

The principle diagram of the parallel design of the alignment output module of the present invention is shown in FIG. 7 . The alignment output module (M6) includes a parallel execution of the attitude quaternion correction module (M6_1) and the speed and position assignment module (M6_2). The attitude quaternion correction module (M6_1) is based on the estimated error angle in the estimated system state X _k The quaternion q _ns calculated by the sub-inertial navigation is corrected once to obtain the initial attitude value q _ns/c0 of the sub-inertial navigation solution, specifically as follows:

作为子惯导导航解算的速度初值

将主惯导位置Pos_m作为子惯导导航解算的位置初值Pos_s/c0。The speed and position assignment module (M6_2) will compensate the main inertial navigation speed

As the initial value of the velocity for the sub-inertial navigation solution

The main inertial navigation position Pos _m is used as the initial position value Pos _s/c0 of the sub-inertial navigation solution.

Claims (5)

1. A parallel implementation method of strapdown inertial navigation transfer alignment algorithm, consisting of a binding module, an earth-related parameter calculation module, a sub-inertial navigation system navigation calculation module, a filter parameter calculation module, a Kalman filter module and an alignment output Module composition, characterized in that: The one-time binding module is composed of a posture binding module and a speed binding module of parallel computing; The earth-related parameter calculation module is composed of a parallel computing earth rotation angular velocity calculation module, a navigation coordinate system earth angular velocity calculation module and a local gravitational acceleration calculation module; The sub-inertial navigation system navigation solution module is composed of an attitude quaternion solution module and a speed solution module of parallel operation; The filter parameter calculation module is composed of a speed difference calculation module of parallel operation, a calculation module of attitude error angle calculation, a noise distribution matrix calculation module and a state transition matrix calculation module; Described Kalman filtering module is made up of state prediction module, estimated covariance prediction module, Kalman gain calculation module, state estimation module and covariance estimation module; The alignment output module is composed of an attitude quaternion correction module and a speed and position assignment module of parallel operation; The primary binding module receives the navigation information, gyroscope signal and accelerometer signal of the main inertial navigation system in the first frame, and is used as the initial value of the sub-inertial navigation system's initial alignment navigation solution after compensation calculation; the earth-related parameters The calculation module, the sub-inertial navigation system navigation calculation module, the filter parameter calculation module and the Kalman filter module form a fine alignment process of transfer alignment, and the fine alignment process is executed cyclically until the set number of cycles N is reached; the described After the fine alignment process, the alignment output module performs a one-time correction to the attitude information of the sub-inertial navigation system, and outputs the initial values of attitude, velocity and position required for the navigation solution of the sub-inertial navigation system. 2. The strapdown inertial navigation transfer alignment algorithm parallel implementation method according to claim 1, is characterized in that: the described earth-related parameter calculation module calculates the earth's rotation angular velocity, The rotation angular velocity of the navigation coordinate system where the main inertial navigation is located relative to the earth and the gravitational acceleration at the location of the main inertial navigation. 3. The strapdown inertial navigation transfer alignment algorithm parallel implementation method according to claim 1, characterized in that: the sub-inertial navigation system navigation calculation module provides an angular velocity and a specific force signal according to the sub-inertial navigation system once The bound output is the initial value for navigation calculation, including attitude quaternion calculation and velocity calculation. 4. The strapdown inertial navigation transfer alignment algorithm parallel implementation method according to claim 1, characterized in that: after the noise distribution matrix, state transition matrix and matching parameters required for Kalman filtering are calculated by the filter parameter calculation module, , and pass the calculation result to the Kalman filter module to perform a Kalman filter calculation. 5. The strapdown inertial navigation transfer alignment algorithm parallel implementation method according to claim 1, characterized in that: the navigation information of the main inertial navigation system includes the attitude quaternion of the carrier, the speed of the carrier over the ground, the carrier position.

Images