CN112577706A - Method for acquiring pose of embedded wind tunnel free flight test model - Google Patents

Abstract

The invention discloses an embedded wind tunnel free flight test model pose acquisition method, which relates to the technical field of flight tests, and is characterized in that test effective data are obtained by intercepting data of an inertial sensor according to a preset rule, wherein the effective data comprise triaxial accelerometer data and triaxial gyroscope output data; carrying out low-pass filtering on the triaxial accelerometer data to obtain first data and removing high-frequency vibration interference; performing band elimination filtering on the first data and the output data of the three-axis gyroscope to obtain second data, and removing the pulsation interference of the pneumatic airflow; and carrying out recursive pose calculation based on the second data to obtain pose information of six degrees of freedom of the aircraft model in effective time in the test process, and aiming at an application scene of a wind tunnel free flight test to improve the test precision.

Description

A method for obtaining the pose of an embedded wind tunnel free-flying test model

technical field

The technical field of wind tunnel flight test, in particular to a method for acquiring the pose of an embedded wind tunnel free flight test model.

Background technique

The wind tunnel free flight test is a specific test method in the field of wind tunnel testing. Any support constraints, other than gravity, are only subject to aerodynamic forces. By capturing the position and attitude information of the aircraft model in real time, the static and dynamic stability derivative coefficients and other related aerodynamic coefficients of the aircraft can be obtained. Compared with the traditional wind tunnel test, its biggest advantage is that there is no support interference, and it can more realistically simulate the actual flight state of the aircraft.

The pose capture of the aircraft model is the core part of the wind tunnel free flight test. At present, the pose capture of wind tunnel free-flight test models is based on high-speed photography methods. However, it has the problems of less degrees of freedom in obtaining poses and less effective data, which restricts the further development of wind tunnel free flight tests. The advantages of inertial sensors are that they are completely independent and have high output frequency, but there are accumulated errors, and long-term use errors are large. Therefore, in most cases, inertial sensors need to be combined with other sensors (GPS, magnetometer, odometer, etc.) to achieve pose calculation and complement each other's advantages. The patent "A method for calculating wellbore trajectory parameters of radial horizontal wells: CN 106703787 B" uses geomagnetic information to correct inertial calculation information to obtain real-time wellbore trajectory. For the pure inertial pose calculation method without the assistance of other types of sensors, the patent "Adaptive Zero-speed Correction Pedestrian Navigation Method Based on MEMS Sensor: CN201710943499.3" uses MEMS inertial sensors to achieve zero-speed correction, and at the same time obtains through strapdown solution Pedestrian position and speed.

In wind tunnel tests, inertial devices are currently mostly used for static angle measurement rather than pose calculation, and due to the low accuracy of MEMS inertial sensors.

SUMMARY OF THE INVENTION

The purpose of the present invention is to provide an embedded wind tunnel free flight test model pose acquisition method, which can improve the test accuracy for the application scenario of the wind tunnel free flight test.

In a first aspect, an embodiment of the present invention provides a method for obtaining a pose of an embedded wind tunnel free flight test model, wherein the method for obtaining a pose includes the following steps:

Intercepting the inertial sensor data according to preset rules to obtain valid test data, wherein the valid data includes three-axis accelerometer data and three-axis gyro output data;

Perform low-pass filtering on the three-axis accelerometer data to obtain the first data to remove high-frequency vibration interference;

Band-stop filtering is performed on the first data and the output data of the three-axis gyro to obtain the second data, so as to remove the pulsating interference of the wind-driven airflow;

Based on the second data, recursive pose calculation is performed to obtain the six-degree-of-freedom pose information of the aircraft model within the valid time during the test.

Optionally, the pose calculation method is applied to the pose measurement of the wind tunnel free flight test model, and the measurement system is based on an inertial sensor and is placed inside the model.

Optionally, the preset rule is: the data before the moment when the acceleration change amount |a(i)-a(i-1)| is greater than the threshold a _th for the first time is the valid data of the experiment, wherein,

a(i-1) is the value of a(i) at the last moment. a _x , a _y and a _z are the three-axis acceleration output by the accelerometer, respectively, and a _th is obtained from multiple tests before the test.

Optionally, the low pass filter cutoff frequency is half the frequency of inertial sensor data acquisition.

Optionally, the center frequency and bandwidth of the stop band of the band-stop filter are determined by the pulsation level of the air flow in the wind tunnel. Under different wind tunnels and different Mach numbers, the center frequency and bandwidth of the stop band of the band-stop filter are different, and are calibrated by the pre-order flow field. get.

Optionally, performing the recursive pose calculation based on the second data to obtain the six-degree-of-freedom pose information of the aircraft model within the valid time during the test process includes:

a. Define that the x-axis of the inertial device points to the front of the model in the vertical plane of the model, the z-axis points to the bottom of the model in the vertical plane of the model, and the y-axis and the x and z-axes form a right-handed system and point to the right side of the model; the definition of the inertial sensor axis is the same as The model is consistent; initialize the solution parameters, including: proportional constant K _p , integral constant K _i , time constant T, quaternion initial value q=[q ₀ q ₁ q ₂ q ₃ ] ^T , superscript T represents the vector Or matrix transposition; initial value of error integral term e _i =[e _ix e _iy e _iz ] ^T ; initial value of velocity vel=[vel _x vel _y vel _z ] ^T , initial value of position pos=[pos _x pos _y pos _z ] ^T ;

b. Normalize the three-axis gyro data and accelerometer data at the current moment:

a _n =a/|a|, gn=g/|g|

where, an =[ _anx a _ny a _nz ] ^T is the normalized accelerometer data, g _n = _{[g nx g ny g nz ] T is} ^the normalized gyro data, a=[a _x a _y a _z ] ^T is the original three-axis accelerometer data, g=[g _x g _y g _z ] ^T is the original three-axis gyro data, and

c. Calculate the unit gravity vector v=[v _x v _y v _z ] ^T :

为姿态矩阵，根据欧拉角旋转序列的不同，

一共有12种表示，可选择任意一种；Wherein, g _n =[0 0 1] ^T ;

is the attitude matrix, according to the different Euler angle rotation sequences,

There are a total of 12 representations, you can choose any one;

d. Calculate the attitude error e=[e _x e _y e _z ] ^T at the current moment, and integrate the error:

e=a _n ×v

The integral term e _i =[e _ix e _iy e _iz ] ^T is updated as follows:

e _i (i)=e _i (i-1)+K _i T·e

Among them, e _i (i-1) represents the error integral term at the previous moment.

e. Correct the zero offset of the gyro:

g _nr =g _n +K _p ·e+e _i (i)

Wherein, g _nr =[g _nrx g _nry g _nrz ] ^T is the corrected gyro data.

f. Update the quaternion and normalize the quaternion:

q(i)=q(i-1)+TΩq(i-1)

in,

Normalize the quaternion to get q _n (i)=[q _1n (i) q _2n (i) q _3n (i) q _4n (i)] ^T

q _n (i)=q(i)/|q(i)|

in

g. Use the updated quaternion to solve the attitude of the aircraft model, including the roll angle, pitch angle and sideslip angle. The update formula is determined by the rotation sequence determined in step c. The rotation sequence is different, and the calculation method of the attitude angle is different.

h. Calculate the three-axis acceleration of the aircraft model in the horizon coordinate system:

g_e为当地重力加速度。where a _ne = [a _nex a _ney a _nez ] ^T ,

g _e is the local gravitational acceleration.

i. Calculate and filter the three-axis velocity of the aircraft model in the horizon coordinate system:

vel(i)=vel(i-1)+a _ne ·T

The velocity is high-pass filtered to remove bias errors:

vel _f (i) = Fil ₁ (vel (i))

Among them, Fil ₁ (*) is a designed high-pass filter whose pass-band cutoff frequency is less than 0.2Hz.

j. Calculate the three-axis position of the aircraft model in the horizon coordinate system and filter:

pos(i)=pos(i-1)+vel _f ·T

The position is high-pass filtered to remove bias errors:

pos _f (i) = Fil ₂ (pos (i))

Among them, Fil ₂ (*) is a designed high-pass filter whose pass-band cutoff frequency is less than 0.2Hz.

k. Return to step b, and iteratively calculate until the position and attitude information of the aircraft model within the valid data time is obtained.

beneficial effect

The invention discloses a method for obtaining the pose and attitude of an embedded wind tunnel free flight test model. The valid data of the test is obtained by intercepting the inertial sensor data according to preset rules, wherein the valid data includes a triaxial accelerometer. data and three-axis gyro output data; perform low-pass filtering on the three-axis accelerometer data to obtain the first data to remove high-frequency vibration interference; perform band-stop filtering on the first data and the three-axis gyro output data to obtain the second data. data to remove the pulsating interference of wind and airflow; based on the second data, recursive pose calculation is performed to obtain the 6-DOF pose information of the aircraft model during the valid time during the test, which can be used for the application scenario of the wind tunnel free flight test , in order to improve the test accuracy.

Description of drawings

1 is a flowchart of an embodiment of a method for obtaining a pose of an embedded wind tunnel free flight test model of the present invention;

Fig. 2 is a flow chart of an embodiment of performing a recursive pose calculation based on the second data to obtain the six-degree-of-freedom pose information of the aircraft model within the valid time during the test;

Detailed ways

The technical solutions of the present invention will be clearly and completely described below with reference to the embodiments. Obviously, the described embodiments are part of the embodiments of the present invention, but not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

In the description of the present invention, it should be understood that the terms "thickness", "upper", "lower", "front", "rear", "left", "right", "inner", "outer", etc. The indicated orientation or positional relationship is based on the orientation or positional relationship shown in the accompanying drawings, which is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the indicated device or system must have a specific orientation or a specific orientation. construction and operation, and therefore should not be construed as limiting the invention.

In addition, the terms "first" and "second" are only used for descriptive purposes, and should not be understood as indicating or implying relative importance or implying the number of indicated technical features. Thus, features defined as "first", "second" may expressly or implicitly include one or more of said features. For those of ordinary skill in the art, the specific meanings of the above terms in the present invention can be understood in specific situations.

In the wind tunnel test, inertial devices are currently mostly used for static angle measurement rather than position and attitude calculation, and due to the low accuracy of MEMS inertial sensors, it is necessary to design a position and attitude capture algorithm for the application scenario of wind tunnel free flight test. to meet the test accuracy. The following issues must be considered when applying MEMS inertial devices to wind tunnel tests: 1. Limited by the volume of the aircraft model and the metal shielding environment, it is difficult to combine with other sensors, and the pose solution can only be improved by improving the inertial solution accuracy. 2. In the wind tunnel test, the aircraft model vibration and airflow pulsation interfere greatly, and the MEMS sensor is more sensitive to vibration, so the original data must be effectively filtered.

An embodiment of the present invention discloses a method for obtaining a pose of an embedded wind tunnel free flight test model, which specifically includes the following steps:

S20, intercepting the inertial sensor data according to a preset rule to obtain valid test data, wherein the valid data includes three-axis accelerometer data and three-axis gyro output data;

S40, performing low-pass filtering on the three-axis accelerometer data to obtain first data to remove high-frequency vibration interference;

S60, band-stop filtering is performed on the first data and the output data of the three-axis gyro to obtain the second data, and the pulsating interference of the wind-driven airflow is removed;

S80. Perform recursive pose calculation based on the second data to obtain the six-degree-of-freedom pose information of the aircraft model within the valid time during the test.

Further, in step S20, the data interception rule is: the data before the time when the acceleration change amount |a(i)-a(i-1)| is greater than the threshold value a _th for the first time is valid data for the experiment. in,

a(i-1) is the value of a(i) at the last moment. a _x , a _y and _az are the three-axis acceleration output by the accelerometer, respectively. a _th is obtained from multiple tests before the test.

Further, in step S40, the filter cut-off frequency _fc is half of the inertial sensor data acquisition frequency.

Further, in step S60, the center frequency f ₀ and the bandwidth BW of the band rejection filter are determined by the pulsation level of the wind tunnel airflow. Under different wind tunnels and different Mach numbers, the center frequency f ₀ and bandwidth BW of the stop band of the band-stop filter are different, which are obtained from the pre-order flow field calibration.

The beneficial effects of the present invention are further described below by introducing specific parameters with a preferred embodiment:

Step 1: Intercept the inertial sensor data collected by the embedded wind tunnel free flight test model pose measurement system to obtain test valid data, which is described as data 1. Among them, test valid data means that the model does not touch the wind tunnel test Before the previous paragraph, only inertial sensor data of the wind tunnel free-flight test model under aerodynamic action. Because the model encounters the wind tunnel test section, there will be a sudden acceleration of the acceleration. Therefore, the data before the sudden change of acceleration is valid data.

Specifically, the data interception rule is: the data before the time when the acceleration change amount |a(i)-a(i-1)| is greater than the threshold a _th for the first time is the valid data of the experiment. in,

a(i-1) is the value of a(i) at the last moment. a _x , a _y and _az are the three-axis acceleration output by the accelerometer, respectively. a _th is obtained from multiple tests before the test.

Step 2: Perform low-pass filtering on the triaxial accelerometer output data in the data 1 obtained in step 1, and form data 2 together with the unfiltered triaxial gyro output data. Among them, the filter cutoff frequency _fc is half of the inertial sensor data acquisition frequency. The steps are described below by taking the low-pass filtering of a _x as an example, where the acquisition frequency is 100 Hz, and f _c =50 Hz. The a _y and a _z low-pass filtering steps are exactly the same. The filtering formula is as follows:

x(n)=0.858·a _x (n)

y(n)=x(n)+x(n-1)-0.716 y(n-1)

Among them, a _x is the N-dimensional original array, N is the data length; y is the low-pass filtered array of a _x , and y(n) is the nth element of y.

Step 3: Band-stop filtering is performed on the data 2 obtained in step 2 to obtain data 3. Among them, the center frequency f ₀ of the band-stop filter and the bandwidth BW are determined by the pulsation level of the wind tunnel airflow. Under different wind tunnels and different Mach numbers, the center frequency f ₀ and bandwidth BW of the stop band of the band-stop filter are different, which are obtained from the pre-order flow field calibration. The following also takes the band-stop filtering of a _x as an example to describe the steps, wherein the acquisition frequency is 100 Hz, f ₀ =25 Hz, and BW = 10 Hz. The bandstop filtering steps for a _y and _az are exactly the same. The filtering formula is as follows:

x(n)=0.6486·a _x (n)

y ₁ (n)=x(n)-0.4478·x(n-1)+x(n-2)-0.7503·y ₁ (n-1)-0.4701·y ₁ (n-2)

x ₁ (n)=0.6486 y ₁ (n)

y ₂ (n)=x ₁ (n)+0.4478·x ₁ (n-1)+x ₁ (n-2)+0.7503·y ₂ (n-1)-0.4701·y _i (n-2)

Where a _x is the N-dimensional original array, N is the data length; y ₂ is the band-stop filtered array of a _x , and y ₂ (n) is the nth element of y ₂ .

Step 4: Use the data 3 obtained in step 3 to perform recursive pose calculation, and obtain the six-degree-of-freedom pose information of the aircraft model within the valid time during the test process.

Specifically, as shown in Figure 2:

Use the data 3 obtained in step 3 to perform recursive pose calculation, and obtain the six-degree-of-freedom pose information of the aircraft model within the valid time during the test process, including the following steps:

a. Define that the x-axis of the inertial device points to the front of the model in the vertical plane of the model, the z-axis points to the bottom of the model in the vertical plane of the model, and the y-axis and the x- and z-axes form a right-handed system and point to the right side of the model. The inertial sensor shaft system definition is consistent with the model. Initialize the calculation parameters. The parameters in this embodiment are set as follows: proportional constant K _p =2.5, integral constant K _i =0.001, time constant T = 0.005, quaternion initial value q = [q ₀ q ₁ q ₂ q ₃ ] ^T =[1 0 0 0] ^T , superscript T represents vector or matrix transposition; initial value of error integral term e _i =[e _ix e _iy e _iz ] ^T =[0 0 0] ^T ; initial value of speed vel =[vel _x vel _y vel _z ] ^T =[0 0 0] ^T , the position initial value pos=[pos _x pos _y pos _z ] ^T =[0 0 0] ^T .

b. Normalize the three-axis gyro and accelerometer data at the current moment:

a _n =a/|a|, g _n =g/|g|

where, an =[ _anx a _ny a _nz ] ^T is the normalized accelerometer data, g _n = _{[g nx g ny g nz ] T is} ^the normalized gyro data, a=[a _x a _y a _z ] ^T is the original three-axis accelerometer data, g=[g _x g _y g _z ] ^T is the original three-axis gyro data, and

c. Calculate the unit gravity vector v=[v _x v _y v _z ] ^T :

为姿态矩阵，根据欧拉角旋转序列的不同，

一共有12种表示，可选择任意一种，之后的解算均需以此旋转序列为准。本实施例中，采用“ZYX”旋转序列，得到

如下：Wherein, g _n =[0 0 1] ^T ;

is the attitude matrix, according to the different Euler angle rotation sequences,

There are a total of 12 representations, any one of which can be selected, and this rotation sequence shall prevail in subsequent solutions. In this embodiment, the "ZYX" rotation sequence is used to obtain

as follows:

Therefore, the component expression of v=[v _x v _y v _z ] ^T can be obtained:

v _x =2(q ₁ q ₃ -q ₀ q ₂ )

v _y =2(q ₀ q ₁ +q ₂ q ₃ )

d. Calculate the attitude error e=[e _x e _y e _z ] ^T at the current moment, and integrate the error:

e=a _n ×v

The integral term e _i =[e _ix e _iy e _iz ] ^T is updated as follows:

e _i (i)=e _i (i-1)+K _i T·e

Among them, e _i (i-1) represents the error integral term at the previous moment, and the specific calculation results are as follows:

e _x = a _ny *v _z -a _nz *v _y

e _y =a _nz *v _x -a _nx *v _z

e _z =a _nx *v _y -a _ny *v _x

e _ix (i)=e _ix (i-1)+K _i *e _x *T

e _iy (i)=e _iy (i-1)+K _i *e _y *T

e _iz (i)=e _iz (i-1)+K _i *e _z *T

e. Correct the zero offset of the gyro:

g _nr =g _n +K _p ·e+e _i (i)

Wherein, g _nr =[g _nrx g _nry g _nrz ] ^T is the corrected gyro data. The specific calculation results are as follows:

g _nrx =g _nx +K _p *e _x +e _ix (i)

g _nry =g _ny +K _p *e _y +e _iy (i)

g _nrz =g _nz +K _p *e _z +e _iz (i)

f. Update the quaternion and normalize the quaternion

q(i)=q(i-1)+TΩq(i-1)

in,

Expand as follows:

q ₀ (i)=q ₀ (i-1)+(-q ₁ (i-1)g _nrx -q ₂ (i-1)g _nry -q ₃ (i-1)g _nrz )T/2

q ₁ (i)=q ₁ (i-1)+(q ₀ (i-1)g _nrx +q ₂ (i-1)g _nrz -q ₃ (i-1)g _nry )T/2

q ₂ (i)=q ₂ (i-1)+(q ₀ (i-1)g _nry -q ₁ (i-1)g _nrz +q ₃ (i-1)g _nrx )T/2

q ₃ (i)=q ₃ (i-1)+(q ₀ (i-1)g _nrz +q ₁ (i-1)g _nry -q ₂ (i-1)g _nrx )T/2

Normalize the quaternion to get q _n (i)=[q _1n (i) q _2n (i) q _3n (i) q _4n (i)] ^T

q _n (i)=q(i)/|q(i)|

in

俯仰角θ以及侧偏角ψ如下：g. Use the updated quaternion to solve the attitude of the aircraft model. The update formula is determined by the rotation sequence determined in step c. The rotation sequence is different, and the calculation method of the attitude angle is different. According to the "ZYX" rotation sequence, the roll angle of the aircraft model is calculated respectively

The pitch angle θ and the sideslip angle ψ are as follows:

θ=-arcsin(2q _1n (i)q _3n (i)+2q _0n (i)q _2n (i))

h. Calculate the three-axis acceleration of the aircraft model in the horizon coordinate system:

g_e为当地重力加速度，本发明取g_e＝9.81，具体结果计算如下：where a _ne = [a _nex a _ney a _nez ] ^T ,

g _e is the local gravitational acceleration, and the present invention takes g _e =9.81, and the specific results are calculated as follows:

i. Calculate the three-axis velocity vel=[vel _x vel _y vel _z ] ^T of the aircraft model under the horizon coordinate system and filter:

vel _x (i)=vel _x (i-1)+a _nex ·T

vel _y (i)=vel _y (i-1)+a _ney ·T

vel _z (i)=vel _z (i-1)+a _nez ·T

The velocity is high-pass filtered to remove bias errors:

vel _f (i) = Fil ₁ (vel (i))

Wherein, Fil ₁ (*) is a designed high-pass filter, and the cut-off frequency of the pass-band is selected as 0.1 Hz in this embodiment.

j. Calculate the three-axis position of the aircraft model in the horizon coordinate system pos=[pos _x pos _y pos _z ] ^T and filter:

pos _x (i)=pos _x (i-1)+vel _fx (i)

pos _y (i)=pos _y (i-1)+vel _fy (i)

pos _z (i)=pos _z (i-1)+vel _fz (i)

The position is high-pass filtered to remove bias errors:

pos _f (i) = Fil ₂ (pos (i))

Wherein, Fil ₂ (*) is a designed high-pass filter, and the cut-off frequency of the pass band is selected as 0.1 Hz in this embodiment.

k. Return to step b, and iteratively calculate until the position and attitude information of the aircraft model within the valid data time is obtained, and the calculation ends.

The invention discloses a method for acquiring the pose of an embedded wind tunnel free flight test model, which is applied to an inertial sensor-based embedded wind tunnel free flight test model pose measurement system. Low-pass filtering is used to remove the vibration interference of the accelerometer, and band-stop filtering is used to specifically remove the pulsating interference of the wind tunnel airflow. Using the filtered inertial sensor data for inertial calculation, the position and attitude six degrees of freedom information of the aircraft model are obtained. The accuracy of the pose calculation of this method meets the test requirements and has strong universality.

Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, but not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: The technical solutions described in the foregoing embodiments can still be modified, or some or all of the technical features thereof can be equivalently replaced; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the technical solutions of the embodiments of the present invention. scope.

Claims (8)

1. The method for acquiring the pose of the embedded wind tunnel free flight test model is characterized by comprising the following steps of:

intercepting the inertial sensor data according to a preset rule to obtain effective test data, wherein the effective data comprises three-axis accelerometer data and three-axis gyroscope output data;

carrying out low-pass filtering on the triaxial accelerometer data to obtain first data and removing high-frequency vibration interference;

performing band elimination filtering on the first data and the output data of the three-axis gyroscope to obtain second data, and removing the pulsation interference of the pneumatic airflow;

and carrying out recursive pose calculation based on the second data to obtain pose information of six degrees of freedom of the aircraft model in the effective time in the test process.

2. The pose acquisition method according to claim 1, wherein the pose acquisition method is applied to pose measurement of a wind tunnel free flight test model, and a measurement system is based on an inertial sensor and is arranged inside the model.

3. The pose acquisition method according to claim 2, characterized in that the effective test data is inertial sensor data of the wind tunnel free flight test model only under the aerodynamic action before the model does not touch the wind tunnel test segment.

4. The method for acquiring the pose of the embedded wind tunnel free flight test model according to claim 2, wherein the preset rule is that: the acceleration variation | a (i) | (i) -a (i-1) | is greater than the threshold value a for the first time_thThe data before the moment is experimental valid data, wherein,

a (i-1) is the value of a (i) at the previous time. a is_x、a_yAnd a_zRespectively, the three-axis acceleration of the accelerometer output, a_thObtained from multiple tests prior to the test.

5. The pose acquisition method according to claim 2, characterized in that: the low pass filter cutoff frequency is half the inertial sensor data acquisition frequency.

6. The pose acquisition method according to claim 2, characterized in that: the center frequency and the bandwidth of the band elimination filter rejection band are determined by the pulsation level of the air flow of the wind tunnel, and the center frequency and the bandwidth of the band elimination filter rejection band are different under different wind tunnels and different Mach numbers and are obtained by sequencing flow field calibration.

7. The pose acquisition method according to claim 2, characterized in that: the calculation of the recursive pose based on the second data to obtain the pose information of the six degrees of freedom of the aircraft model in the effective time in the test process comprises the following steps:

a. defining that an x axis of an inertial device points to the front of the model in a vertical plane of the model, a z axis points to the lower part of the model in the vertical plane of the model, and a right-handed system consisting of a y axis, the x axis and the z axis points to the right side of the model; the definition of the inertial sensor shafting is consistent with that of the model; initializing resolving parameters, including: constant of proportionality K_pIntegral constant K_iTime constant T, initial quaternion value q ═ q₀ q₁ q₂ q₃]^TThe superscript T represents the vector or matrix transposition; initial value e of error integral term_i＝[e_ix e_iy e_iz]^T(ii) a Initial value of speed vel [ ]_xvel_y vel_z]^TInitial position value pos ═ pos_x pos_y pos_z]^T；

b. Normalizing the triaxial gyro data and the accelerometer data at the current moment:

a_n＝a/|a|，g_n＝g/|g|

wherein, a_n＝[a_nx a_ny a_nz]^TFor normalized accelerometer data, g_n＝[g_nx g_ny g_nz]^TFor normalized gyro data, a ═ a_x a_y a_z]^TFor raw triaxial accelerometer data, g ═ g_x g_y g_z]^TIs raw three-axis gyro data, an

c. Calculating unit gravity vector v ═ v_x v_y v_z]^T：

Wherein, g_n＝[0 0 1]^T；

Is an attitude matrix, according to the difference of Euler angle rotation sequences,

there are 12 representations in total, and any one of them can be selected;

d. calculating the attitude error e ═ e at the current moment_x e_y e_z]^TAnd integrating the error:

e＝a_n×v

integral term e_i＝[e_ix e_iy e_iz]^TThe update is as follows:

e_i(i)＝e_i(i-1)+K_iT·e

wherein e is_i(i-1) representsAn error integral term of the last moment;

e. correcting zero offset of the gyro:

g_nr＝g_n+K_p·e+e_i(i)

wherein, g_nr＝[g_nrx g_nry g_nrz]^TIs the corrected gyro data;

f. updating and normalizing quaternion:

q(i)＝q(i-1)+TΩq(i-1)

wherein,

normalizing the quaternion to obtain q_n(i)＝[q_1n(i) q_2n(i) q_3n(i) q_4n(i)]^T

q_n(i)＝q(i)/|q(i)|

Wherein,

g. solving the aircraft model attitude by using the updated quaternion;

h. calculating the triaxial acceleration of the aircraft model under the horizon coordinate system:

wherein a is_ne＝[a_nex a_ney a_nez]^T，

g_eIs the local gravitational acceleration;

i. calculating and filtering the three-axis speed of the aircraft model in the horizon coordinate system:

vel(i)＝vel(i-1)+a_ne·T

high-pass filtering is performed on the three-axis velocity to remove offset errors:

vel_f(i)＝Fil₁(vel(i))

wherein, Fil₁(. X) is designed high-pass filter with wave-pass band cut-off frequency less than 0.2 Hz.

j. Calculating and filtering three-axis positions of the aircraft model in the horizon coordinate system:

pos(i)＝pos(i-1)+vel_f·T

the position is high-pass filtered with the aim of removing the offset error:

pos_f(i)＝Fil₂(pos(i))

wherein, Fil₂(. X) is designed high-pass filter with wave-pass band cut-off frequency less than 0.2 Hz.

k. And (c) returning to the step (b), and performing iterative calculation until the position and attitude information of the aircraft model within the effective data time is obtained.

8. The pose acquisition method according to claim 7, wherein the step g solves the aircraft model attitude by using the updated quaternion, including a roll angle, a pitch angle and a yaw angle; the update formula is determined by the rotation sequence determined in step c.

Images