CN101644579B - Method and system for measuring kinematical parameters of micro projected object in air - Google Patents

Abstract

The invention relates to a method and a system for measuring kinematical parameters of a micro projected object in air. The method comprises the following steps: (1) installing a micro-measuring unit consisting of a triaxial micro gyroscope, a triaxial micro accelerometer and a triaxial micro gaussmeter in a micro projected object, and measuring and recording sensor data of the projected object from the projected time to a period of time after landing; (2) calculating the sensor data of the projected object landing for a period of time by using the landing position of the projected object as the initial position so as to obtain the attitude of the projected object after landing and the zero deviation of the triaxial micro gyroscope; and (3) obtaining real-time kinematical parameters of the projected object in the whole flying process through a reverse-time inertial navigation method by using the data of the triaxial micro gyroscope and the triaxial micro accelerometer, which are recorded in the flying process of the projected object. The measuring system comprises a micro measuring unit, a nonvolatile memory and a flush bonding processor. The invention has the advantages of simple and compact structure, simple principle, high measuring precision and reliability, strong disturbance resisting capability and the like.

Description

A kind of aerial small projected object movement parameter measurement method and system

Technical field

The present invention is mainly concerned with the fields of measurement of small projected object movement locus, refers in particular to a kind of measuring method and system that is used for aerial small projected object motion.

Background technology

From a lot of small projected object of shedding in the air, its real time kinematics parameter is for analyzing its offline mode, and it is significant to optimize the projected object design, and the real time kinematics parameter of therefore obtaining small projected object becomes the basic problem that a lot of applications need solve.

Adopt traditional inertia device, constitute Inertial Measurement Unit as quartz flexible accelerometer, mechanical gyro, optical gyroscope etc., though precision can satisfy measurement requirement, but the measuring equipment volume is big, can't satisfy the requirement of projected object measuring system small size, HI high impact, and known position, speed and attitude (initial parameter) the ability operate as normal that shed constantly of inertia system needs, this is difficult to obtain for projected object.

Because the projected object flight time is very short, independent satellite positioning receiver probably before acquisition and tracking is finished projected object finished flight, the projected object volume is little also to make many antennas satellite decide the appearance technology to be difficult to dispose and use, so satellite positioning tech also is difficult to be applied to the measurement of projected object.

As adopt exterior trajectory measurement methods such as traditional thunder survey, flash ranging when being applied to the flight parameter measurement of projected object, the implementation cost height, difficulty is big, precision is low, and can only obtain projected object line kinematic parameter usually, be difficult to obtain comprising the complete flight parameter of attitude motion.

By testing the distribution situation of back each projected object drop point of statistics, only can estimate probably also whether each projected object flight is normal, but can't do quantitative test.

Up to the present, also do not have disclosedly can carry out complete high-acruracy survey to the projected object flight parameter that comprises angular motion information about measuring equipment and method.

Summary of the invention

The technical problem to be solved in the present invention just is: at the technical matters that prior art exists, the invention provides a kind of simple and compact for structure, principle is simple, measuring accuracy is high, good reliability, the measuring method and the system that are used for aerial small projected object motion that antijamming capability is strong.

For solving the problems of the technologies described above, the solution that the present invention proposes is:

A kind of measuring method that is used for aerial small projected object motion is characterized in that step is:

(1), will be installed in the projected object by the micrometering amount unit that three little gyros, three-axis micro accelerometer and three little magnetometers constitute, measure and the record projected object is carved into the sensing data that lands in a period of time of static back when shedding;

(2) land position after static as relative initial position with projected object, adopt three little magnetometers and three-axis micro accelerometer to constitute the static number compass, projected object is landed the sensing data of static back a period of time to be calculated, obtain projected object and land attitude after static, and calculate three little gyros zero partially;

(3), according to three little gyros that write down in the projected object flight course and three-axis micro accelerometer data, adopt that the inertial navigation method obtains the various real time kinematics parameters of projected object in whole flight course between the inverse time.

As a further improvement on the present invention.

Inertial navigation method between the described inverse time, be obtain static back kinematic parameter the basis on, according to the data of the three little gyros and the three-axis micro accelerometer of mission phase, carry out that inertial navigation calculates between the inverse time, obtain the kinematic parameter of projected object; Its concrete steps are:

A. determine real-time attitude: the hypercomplex number of employing formula (12) is upgraded and is calculated

Wherein, q _kExpression T _kAttitude quaternion constantly, r _K+1 ^*For calculate by three little gyro to measure values from T _kTo T _K+1Rotation hypercomplex number constantly, ^*The conjugation of expression hypercomplex number, о represents that hypercomplex number multiplies each other;

B. determine real-time speed: employing formula (18) is calculated

$V_k=V_{k+1}-(C_b^n(k+1)A_f\left(T_{k+1}\right)+g^n(k+1))\mathrm{\Δt}---\left(18\right)$

V wherein _kExpression T _kSpeed constantly, C _b ⁿ(k+1) be T _K+1Direction cosine matrix constantly, A _f(T _K+1) be T _K+1The measured value of three-axis micro accelerometer constantly, g ⁿ(k+1) be T _K+1Gravitational vector constantly, Δ t is a sampling step length;

C. determine real time position: employing formula (21) is calculated

P _k＝P _k+1-V _k+1*Δt (21)

Wherein, P (T _k) expression T _kPosition vector constantly.

The present invention further proposes a kind of measuring system that is used for aerial small projected object motion, it is characterized in that it comprises:

Micrometering amount unit is made up of three little gyros, three-axis micro accelerometer, three little magnetometers, and described three little gyros, three-axis micro accelerometer, three little magnetometers are used for measuring angular velocity of rotation, magnetic-field vector of the earth and the specific force of projected object respectively;

Nonvolatile memory is used for storing projected object and is carved into when shedding and lands the sensing data that is obtained by each sensor measurement of micrometering amount unit in a period of time of static back;

Flush bonding processor comprises data processing unit, is used for the sensing data that reads is write nonvolatile memory, and data are calculated; Described data processing unit by data read module, land time processing module, data pick-up and segmentation module, gyro zero partially computing module, gyro error correcting module, land static kinematic parameter computing module, the inertial navigation computing module is formed between the inverse time, be used for calculating the kinematic parameter of projected object according to the sensing data of storing in the nonvolatile memory; Described data read module is used for reading the sensing data in the nonvolatile memory, the described time processing module of landing obtains the static moment of landing of projected object according to the sensing data that reads, described data pick-up and segmentation module are divided into sensing data inflight phase and land static segment according to landing the static moment, described gyro zero computing module and gyro error correcting module partially is used for deal with data and obtains the gyro zero-deviation and gyro data is revised, describedly land static kinematic parameter computing module according to sensing data with land the static moment and obtain projected object and land position after static, speed and attitude, inertial navigation computing module all data computation after according to above-mentioned resume module obtain the various real time kinematics parameters of projected object in whole flight course between the described inverse time.

Compared with prior art, advantage of the present invention just is:

1) micrometering amount unit is made up of three little magnetometers, three little gyros, three-axis micro accelerometers among the present invention, and above-mentioned each sensor has advantages such as volume is little, low in energy consumption, shock resistance, and therefore the micrometering amount unit volume that constitutes is little, the projected object of can packing into;

2) the present invention adopts inertial navigation Calculation Method between the inverse time, does not need known projected object to shed constantly kinematic parameter, just can calculate the complete kinematic parameters such as position, speed, acceleration, attitude, angular velocity of projected object accurately;

3) contained each component costs in micrometering amount unit among the present invention is cheap, by pack one in each projected object, can measure a large amount of projected object in single test at low cost;

4) the present invention is used for the measuring method and the system of aerial small projected object motion, has little, the low cost and other advantages of volume, not only can measure complete flight parameter (can obtain position, speed, attitude, acceleration, the angular velocity information of projected object simultaneously), and do not need known initial position, speed and the attitude information of shedding constantly, be not subjected to the projected object flight time, shed and the influence of flying height.

Description of drawings

Fig. 1 is the schematic flow sheet of measuring method of the present invention;

Fig. 2 is a micrometering amount of the present invention unit theory structure synoptic diagram;

Fig. 3 is the detailed process synoptic diagram of measuring method in the specific embodiment of the invention.

Embodiment

Below with reference to the drawings and specific embodiments the present invention is described in further details.

As shown in Figure 1, the present invention is used for the measuring method of aerial small projected object motion, the steps include:

(1), will be installed in the projected object by the micrometering amount unit that three little gyros, three-axis micro accelerometer and three little magnetometers constitute, measure and the record projected object is carved into the sensing data that lands in a period of time of static back when shedding;

(2), land position after static as relative initial position with projected object, adopt three little magnetometers and three-axis micro accelerometer to constitute the static number compass, projected object is landed the sensing data of static back a period of time to be calculated, obtain projected object and land attitude after static, and calculate three little gyros zero partially;

(3), according to three little gyros that write down in the projected object flight course and three-axis micro accelerometer data, adopt that the inertial navigation method obtains the various real time kinematics parameters of projected object in whole flight course between the inverse time.

As shown in Figure 2, the present invention's measuring system of being used for aerial small projected object motion comprises micrometering amount unit, nonvolatile memory and flush bonding processor.Micrometering amount unit is made up of three little gyros, three-axis micro accelerometer, three little magnetometers, and described three little gyros, three-axis micro accelerometer, three little magnetometers are used for measuring angular velocity of rotation, magnetic-field vector of the earth and the specific force of projected object respectively; The sensor information that three little gyros, three-axis micro accelerometer and three little magnetometer surveys obtain is connected with flush bonding processor by digital communication bus (as SPI or RS232 etc.), sends to flush bonding processor.The measurement data that the flush bonding processor receiving sensor sends records it on nonvolatile memory.Nonvolatile memory (for example CF card) is used for storing projected object and is carved into when shedding and lands the sensing data that is obtained by each sensor measurement of micrometering amount unit in a period of time of static back; Flush bonding processor comprises data processing unit, is used for the sensing data that reads is write nonvolatile memory, and data are calculated; Described data processing unit by data read module, land time processing module, data pick-up and segmentation module, gyro zero partially computing module, gyro error correcting module, land static kinematic parameter computing module, the inertial navigation computing module is formed between the inverse time, be used for calculating the kinematic parameter of projected object according to the sensing data of storing in the nonvolatile memory; Described data read module is used for reading the sensing data in the nonvolatile memory, the described time processing module of landing obtains the static moment of landing of projected object according to the sensing data that reads, described data pick-up and segmentation module are divided into sensing data inflight phase and land static segment according to landing the static moment, described gyro zero computing module and gyro error correcting module partially is used for deal with data and obtains the gyro zero-deviation and gyro data is revised, describedly land static kinematic parameter computing module according to sensing data with land the static moment and obtain projected object and land position after static, speed and attitude, inertial navigation computing module all data computation after according to above-mentioned resume module obtain the various real time kinematics parameters of projected object in whole flight course between the described inverse time.

Wherein three little gyros are existing known products, in order to measure the angular velocity of rotation of carrier with respect to inertial coordinates system.Three-axis micro accelerometer is existing known products, in order to measure the specific force of carrier.Three little magnetometers are existing known products, and the Magnetic Sensor that it adopts three quadratures to install is measured the magnetic field of the earth component.

Data processing unit is by data read module; Land the time processing module; Data pick-up and segmentation module; Gyro zero is computing module partially; The gyro error correcting module; Land static kinematic parameter computing module; The inertial navigation computing module is formed between the inverse time.As shown in Figure 3, in specific embodiment, its flow process is:

1. data read module reads the sensing data that writes down in the nonvolatile memory.Sensing data comprises:

Three little gyro datas: G (T _k) k=0,1 ₁..., N;

Three-axis micro accelerometer: A (T _k) k=0,1 ₁..., N;

Three little magnetometer datas: M (T _k) k=0,1 ₁..., N;

Here G (T _k) expression T _kThe angular velocity of rotation vector of the projected object relative inertness system that the moment three little gyro to measure obtain, A (T _k) expression T _kThe ratio force vector that moment three-axis micro accelerometer measures, M (T _k) expression T _kThe moment three ground magnetic vectors that little magnetometer survey obtains.T wherein _kRepresent k sampling instant, and T is arranged _i=T ₀+ Δ ti, Δ t are sampling step length.Subscript T represents transposition.

2. the time processing module of landing is calculated and to be landed the static moment (T _J).Projected object land static after, accelerometer is output as the local gravity value.According to the error characteristics of accelerometer, calculate and land the static moment, specifically be calculated as follows:

$\mathrm{\δA}\left(T_k\right)=\sqrt{A^T\left(T_k\right)A\left(T_k\right)}-g_0---\left(1\right)$

Here, g ₀Be the local gravity value, δ A (T _k) be T _kConstantly the acceleration of gravity that obtains of accelerometer measures and local gravity is poor.If T is arranged _J, make for all T _k〉=T _J, satisfy: δ A (T _k)＜δ ₀T then _JFor landing the static moment.δ ₀Be the threshold value of selecting according to the accelerometer error characteristic.

3. data pick-up and segmentation module.According to landing the static moment (T _J), three little gyros, three-axis micro accelerometer, three little magnetometer datas are divided into independent flight, land static 2 parts, the data after the segmentation are expressed as:

Gyro zero partially computing module according to landing the gyro data of static segment, calculate gyro zero partially.Because the MEMS Gyro Precision is low, zero is remote greater than rotational-angular velocity of the earth, and on the other hand, the projected object independence inflight phase time is short, ignores the influence of earth rotation to attitude, and the theory of static back gyro of then landing is output as zero, can calculate gyro zero E partially by following formula _G:

$E_G=\frac{1}{T_N-T_J}\underset{k=J}{\overset{k=N}{\mathrm{\Σ}}}G\left(T_k\right)---\left(5\right)$

5. the gyro error correcting module is according to the gyro that calculates zero inclined to one side E _G, press the gyro data in the following formula Caliberation Flight section, further improve the precision of gyro:

${\stackrel{\‾}{G}}_f\left(T_k\right)=G_f\left(T_k\right)-E_G,k=0,\·\·\·,J---\left(6\right)$

6. the static kinematic parameter computing module of landing calculates projected object and lands position, speed and attitude after static.Getting reference frame (n-system) is true origin for the position of landing after static with projected object, and coordinate direction is got the terrestrial coordinate system on east northeast ground respectively, and then projected object is landed the position P (T in the static moment _J) be:

P(T _J)＝[0?0?0] ^T (7)

Projected object is landed the speed V (T after static _J) be:

V(T _J)＝[0?0?0] ^T (8)

Calculate the attitude of landing after static according to the little magnetometer data of static three-axis micro accelerometer and three.Its calculation process is:

1) calculates and to land the average of static post-acceleration meter

That is:

$\stackrel{\‾}{A}=\frac{1}{T_N-T_J}\underset{k=J}{\overset{k=N}{\mathrm{\Σ}}}A\left(T_k\right)---\left(9\right)$

2) calculate and to land the average of static back magnetometer

That is:

$\underset{\·}{\overset{\‾}{M}}=\frac{1}{T_N-T_J}\underset{k=J}{\overset{k=N}{\mathrm{\Σ}}}M\left(T_k\right)---\left(10\right)$

3) according to inertial navigation system static-base alignment principle, the static moment is shed the direction cosine matrix C between object coordinate system and the reference frame _b ⁿThrough type (11) calculates:

$C_b^n\left(T_J\right)=\left[\begin{array}{ccc}{g}^n& m^n& g^n\×m^n\end{array}\right]{\left[\begin{array}{ccc}{g}^b& m^b& g^b\×m^b\end{array}\right]}^{-1}---\left(11\right)$

$=\left[\begin{array}{ccc}{g}^n& m^n& g^n\×m^n\end{array}\right]{\left[\begin{array}{ccc}-\stackrel{\‾}{A}& \stackrel{\‾}{M}& -\stackrel{\‾}{A}\×\stackrel{\‾}{M}\end{array}\right]}^{-1}$

Here, g ⁿBe the projection of local gravity vector in reference frame; m ⁿBe the projection of local magnetic-field vector of the earth in reference frame.m ⁿAnd g ⁿBe known quantity.

Between the inverse time inertial navigation computing module on known above-mentioned basis of landing static moment kinematic parameter, use the three little gyros and the three-axis micro accelerometer data of inflight phase, calculate the flight parameter of projected object independence inflight phase, comprise position, speed and attitude information.The flow process of inertial navigation module is between the inverse time:

1) attitude of calculating projected object relative reference coordinate system

The discrete form that hypercomplex number between reference frame and the body coordinate system is described has:

q(T _k+1)＝q(T _k)оr(T _k) (12)

Wherein, q _kExpression T _kAttitude quaternion constantly, r _K+1 ^*For calculate by three little gyro to measure values from T _kTo T _K+1Rotation hypercomplex number constantly, ^*The conjugation of expression hypercomplex number, о represents the hypercomplex number multiplying.

Calculate for the projected object attitude quaternion,, therefore can ignore earth rotation and, be write as the form of calculating between the inverse time, then have with respect to the motion of the earth influence to attitude because the projected object flight time is short:

q(T _k)＝q(T _k+1)оr ^*(T _k+1) (13)

$r^{*}\left(T_{k+1}\right)=(\cos \left(\frac{\mathrm{\σ}\left(T_{k+1}\right)}{2}\right)-\sin \left(\frac{\mathrm{\σ}\left(T_{k+1}\right)}{2}\right)\frac{s\left(T_{k+1}\right)}{\mathrm{\σ}\left(T_{k+1}\right)})---\left(14\right)$

σ(T _k+1)＝|s(T _k+1)| (15)

$s\left(T_{k+1}\right)={\stackrel{\‾}{G}}_f\left(T_{k+1}\right)\·\mathrm{\Δt}---\left(16\right)$

Here, s (T _K+1) be the rotating vector that calculates according to zero three little gyro to measure values of revised partially inflight phase, σ (T _K+1) be the size of the rotating vector that calculates.

2) calculate projected object speed.

Because the projected object flight time is short, ignores the influence of Corioli's acceleration, thereby the speed of projected object is write as discrete form (employing trigonometric integral) is arranged:

$V\left(T_{k+1}\right)=V\left(T_k\right)+(C_b^n\left(T_k\right)f^b\left(T_k\right)+g^n\left(T_k\right))\mathrm{\Δt}---\left(17\right)$

Here V (T _k) expression T _kSpeed constantly, C _b ⁿ(T _k) expression T _kConstantly shed the direction cosine matrix between object coordinate system and the reference frame, g ⁿ(T _k) expression T _kThe projection of local gravity vector in reference frame constantly, f ^b(T _k) be T _kSpecific force measured value constantly.

For projected object, the specific force measured value of the three-axis micro accelerometer of inflight phase output is A _f(T _k), therefore being write as the form of calculating between the inverse time has:

V(T _k)＝V(T _k+1)-(C _b ⁿ(T _k+1)A _f(T _k+1)+g ⁿ(T _k+1))Δt (18)

Here, the pass of hypercomplex number and direction cosine matrix is:

$C_b^n=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]---\left(19\right)$

Here q ₀, q ₁, q ₂, q ₃Be respectively four components of hypercomplex number.

3) position of calculating projected object.The projected object position calculation adopts numerical integration to have:

P(T _k+1)＝P(T _k)+V(T _k)*Δt (20)

Write as between the inverse time there be form:

P(T _k)＝P(T _k+1)-V(T _k+1)*Δt (21)

Here, P (T _k) expression T _kPosition vector constantly.

The above only is a preferred implementation of the present invention, and protection scope of the present invention also not only is confined to the foregoing description, and all technical schemes that belongs under the thinking of the present invention all belong to protection scope of the present invention.Should be pointed out that for those skilled in the art in the some improvements and modifications that do not break away under the principle of the invention prerequisite, these improvements and modifications also should be considered as protection scope of the present invention.

Claims (2)

1. one kind is used for the measuring method that aerial small projected object is moved, and it is characterized in that step is:

(1), will be installed in the projected object by the micrometering amount unit that three little gyros, three-axis micro accelerometer and three little magnetometers constitute, measure and the record projected object is carved into the sensing data that lands in a period of time of static back when shedding; The read sensor data write in the nonvolatile memory;

(2), land position after static as relative initial position with projected object, adopt three little magnetometers and three-axis micro accelerometer to constitute the static number compass, projected object is landed the sensing data of static back a period of time to be calculated, obtain projected object and land attitude after static, and calculate three little gyros zero partially; That is, read the sensing data in the nonvolatile memory, obtain the static moment of landing of projected object according to the sensing data in the nonvolatile memory that reads; According to landing the static moment sensing data is divided into inflight phase and lands static segment; Calculate zero of gyro and revise partially and to gyro data according to landing the gyro data of static segment; Calculate projected object and land position after static, speed and calculate attitude after static according to static three-axis micro accelerometer and three little magnetic strength;

(3), on known basis of landing static back kinematic parameter, according to the three little gyros and the three-axis micro accelerometer data of projected object inflight phase, adopt that the inertial navigation method obtains the various real time kinematics parameters of projected object in whole flight course between the inverse time;

Inertial navigation method between the described inverse time, be obtain static back kinematic parameter the basis on, according to the data of the three little gyros and the three-axis micro accelerometer of mission phase, carry out that inertial navigation calculates between the inverse time, obtain the kinematic parameter of projected object; Its concrete steps are:

A. determine real-time attitude: the hypercomplex number of employing formula (12) is upgraded and is calculated

Wherein, q _kExpression T _kAttitude quaternion constantly,

For calculate by three little gyro to measure values from T _kTo T _K+1Rotation hypercomplex number constantly, ^*The conjugation of expression hypercomplex number, o represents that hypercomplex number multiplies each other;

B. determine real-time speed: employing formula (18) is calculated

$V_k=V_{k+1}-(C_b^n(k+1)A_f\left(T_{k+1}\right)+g^n(k+1))\mathrm{\Δt}---\left(18\right)$

V wherein _kExpression T _kSpeed constantly,

Be T _K+1Direction cosine matrix constantly, A _f(T _K+1) be T _K+1The measured value of three-axis micro accelerometer constantly, g ⁿ(k+1) be T _K+1Gravitational vector constantly, Δ t is a sampling step length;

C. determine real time position: employing formula (21) is calculated

P _k＝P _k+1-V _k+1*Δt (21)

Wherein, P (T _k) expression T _kPosition vector constantly.

2. one kind is used for the measuring system that aerial small projected object is moved, and it is characterized in that it comprises:

Micrometering amount unit is made up of three little gyros, three-axis micro accelerometer, three little magnetometers, and described three little gyros, three-axis micro accelerometer, three little magnetometers are used for measuring angular velocity of rotation, magnetic-field vector of the earth and the specific force of projected object respectively;

Nonvolatile memory is used for storing projected object and is carved into when shedding and lands the sensing data that is obtained by each sensor measurement of micrometering amount unit in a period of time of static back;

Flush bonding processor comprises data processing unit, is used for the sensing data that reads is write nonvolatile memory, and data are calculated; Described data processing unit by data read module, land time processing module, data pick-up and segmentation module, gyro zero partially computing module, gyro error correcting module, land static kinematic parameter computing module, the inertial navigation computing module is formed between the inverse time, be used for calculating the kinematic parameter of projected object according to the sensing data of storing in the nonvolatile memory; Described data read module is used for reading the sensing data in the nonvolatile memory, the described time processing module of landing obtains the static moment of landing of projected object according to the sensing data that reads, described data pick-up and segmentation module are divided into the independent inflight phase and the static segment of landing according to landing the static moment with three little gyros, three-axis micro accelerometer, three little magnetometer datas, described gyro zero partially computing module according to landing the gyro data of static segment, calculate gyro zero partially; Described gyro error correcting module is revised gyro data in the independent inflight phase partially according to zero of gyro, describedly land static kinematic parameter computing module and obtain projected object and land position after static according to landing the static moment, speed, and calculate the attitude of landing after static according to the little magnetometer data of three-axis micro accelerometer and three, the inertial navigation computing module is on known above-mentioned basis of landing static moment kinematic parameter between the described inverse time, use the three little gyros and the three-axis micro accelerometer data of independent inflight phase, calculate the flight parameter of projected object at independent inflight phase;

The inertial navigation computing module is carried out inertial navigation method between the following inverse time between the described inverse time, promptly be obtain static back kinematic parameter the basis on, data according to the three little gyros and the three-axis micro accelerometer of mission phase, carry out inertial navigation calculating between the inverse time, obtain the kinematic parameter of projected object; Its concrete steps are:

A. determine real-time attitude: the hypercomplex number of employing formula (12) is upgraded and is calculated

Wherein, q _kExpression T _kAttitude quaternion constantly,

For calculate by three little gyro to measure values from T _kTo T _K+1Rotation hypercomplex number constantly, ^*The conjugation of expression hypercomplex number, o represents that hypercomplex number multiplies each other;

B. determine real-time speed: employing formula (18) is calculated

$V_k=V_{k+1}-(C_b^n(k+1)A_f\left(T_{k+1}\right)+g^n(k+1))\mathrm{\Δt}---\left(18\right)$

V wherein _kExpression T _kSpeed constantly,

Be T _K+1Direction cosine matrix constantly, A _f(T _K+1) be T _K+1The measured value of three-axis micro accelerometer constantly, g ⁿ(k+1) be T _K+1Gravitational vector constantly, Δ t is a sampling step length;

C. determine real time position: employing formula (21) is calculated

P _k＝P _k+1-V _k+1*Δt (21)

Wherein, P (T _k) expression T _kPosition vector constantly.

Images