CN102288177B - Strapdown system speed calculating method based on angular speed output - Google Patents

Abstract

The invention provides a strapdown system speed calculating method based on angular speed output, which comprises the following steps of: according to the principle of sculling compensation algorithm of speed, deducing the sculling compensation algorithm based on angular rate, specific force and measurement bandwidth through fitting a carrier angular speed and a specific force function; under the condition of typical sculling movement, determining the optimal factor of the sculling compensation algorithm through the minimum difference value of direct current quantity in the sculling compensation quantity and a true value; and thus, obtaining the optimal algorithm of the sculling compensation, and realizing high-precision calculating of the speed.

Description

Strapdown system speed resolving method based on angular rate output

Technical Field

The invention relates to a strapdown system speed calculating method, in particular to a strapdown system speed calculating method based on angular rate output.

Background

The strapdown system is an inertial navigation system with a gyroscope and an accelerometer installed on a carrier, and simulates an actual platform in a platform type inertial navigation system by calculating a mathematical platform, so the manufacturing cost is greatly reduced, but the requirement of the strapdown system on navigation calculation is greatly improved. In the past, researchers have proposed a conventional speed calculation method based on an incremental output signal on the premise that a gyroscope output is used as an angle signal. In recent years, with the development of an optical fiber communication technology and an optical fiber sensing technology, an optical fiber gyroscope becomes an important technology of inertial navigation, for a strapdown system formed by an optical fiber gyroscope (such as an interference type), the gyroscope outputs angular rate signals, when the traditional strapdown system speed calculation method based on incremental output signals is applied to the optical fiber gyroscope strapdown system, angular rate is required to be extracted, and then the traditional method is used for realizing speed calculation, so that the design difficulty of system software is increased, and meanwhile, the calculation precision is greatly lost.

Inner appearance of the invention

In order to overcome the defects of the traditional speed calculation method in the application of the fiber optic gyroscope strapdown system, the invention provides the strapdown system speed calculation method based on angular rate output, which directly utilizes the angular rate signal output by a gyroscope to carry out calculation, improves the calculation precision and reduces the calculation complexity.

A strapdown system speed resolving method based on angular rate output specifically comprises the following steps: speed of carrier at time tm $V_m=V_{m-1}+{\∫}_{t_{m-1}}^{t_m}[g\left(t\right)-(2{\mathrm{\ω}}_{\mathrm{ie}}\left(t\right)+{\mathrm{\ω}}_{\mathrm{en}}\left(t\right))\×V_{m-1}]\mathrm{dt}+C_{m-1}\mathrm{\Δ}{V}_{\mathrm{sfm}},$

Wherein,

V_m-1is a vector at t_m-1The speed of the moment in time is,

C_m-1is a vector at t_m-1The matrix of the attitude at the time of day,

g(t)、ω_ie(t)、ω_en(t) the gravity acceleration, the earth rotation angular velocity and the rotation angular velocity of the navigation system relative to the earth system at the time t are respectively;

$\mathrm{\Δ}{V}_{\mathrm{sfm}}=\mathrm{\Δ}{V}_m+\frac{1}{2}\mathrm{\Δ}{\mathrm{\θ}}_m\×\mathrm{\Δ}{V}_m+\mathrm{\Δ}{\hat{V}}_{{\mathrm{scul}}_m};$

the output specific force of the accelerometer at the time t;

the output angular rate of the gyroscope at the time t;

sculling effect compensation term

The solving process of the n subsamples is as follows:

(a1) over n sampling points the speed resolving period T = T_m-t_m-1Are equally divided into intervals of

N sub-periods of time, to $\left\{\begin{array}{c}\mathrm{\ω}\left(t\right)=a_1+{2a}_2(t-t_{m-1})+\…+{\mathrm{na}}_n{(t-t_{m-1})}^{n-1}\\ f\left(t\right)=A_1+{2A}_2(t-t_{m-1})+\…+{\mathrm{nA}}_n{(t-t_{m-1})}^{n-1}\end{array}\right.$ Performing polynomial linear fitting on the angular rate output of the gyroscope and the specific force output of the accelerometer at the n sampling points for an objective function;

(a2) fitting the determined constant vector a using step (a 1)₁…a_n,A₁…A_nComputing

$\left\{\begin{array}{c}\mathrm{\Δ\θ}\left(t\right)={\∫}_{t_{m-1}}^t\mathrm{\ω}\left(\mathrm{\τ}\right)\mathrm{d\τ}=a_1(t-t_{m-1})+a_2{(t-t_{m-1})}^2+\…+a_n{(t-t_{m-1})}^n\\ \mathrm{\ΔV}\left(t\right)={\∫}_{t_{m-1}}^{t}f\left(\mathrm{\τ}\right)\mathrm{d\τ}=A_1(t-t_{m-1})+A_2{(t-t_{m-1})}^2+\…+A_n{(t-t_{m-1})}^n\end{array},,t_{m-1}\≤t\≤t_m;\right.$

(a3) Construction of a calculation formula of a stroke effect compensation term

$\mathrm{\Δ}{{\hat{V}}_{\mathrm{scul}}}_m=\frac{1}{2}{\∫}_{t_{m-1}}^{t_m}[\mathrm{\Δ\θ}\left(t\right)\×f\left(t\right)+\mathrm{\ΔV}\left(t\right)\×\mathrm{\ω}\left(t\right)]\mathrm{dt};$

(a4) Calculating the accurate value of the stroke effect compensation term under the condition of the stroke motion by using the stroke effect compensation term calculation formula of the step (a 3)

(a5) Determining the DC component in the calculation formula of the stroke effect compensation term of step (a 3)

(a6) By difference

And (c) calculating a formula for the stroke effect compensation term in the target optimization step (a 3).

Compared with the traditional algorithm, the method has the following advantages:

when the method is applied to a strapdown system outputting angular rate signals, the output signals of the inertial device can be directly utilized without conversion, and the method is simple and easy to implement.

And secondly, when the method is applied to a strapdown system outputting angular rate signals, the resolving precision is far better than that of the traditional algorithm.

Drawings

FIG. 1 is a schematic diagram comparing error curves of the three-subsample algorithm designed by the invention and the traditional three-subsample algorithm with the variation curves of the paddle motion frequency.

FIG. 2 is a comparison diagram of the error variation curve with the sampling period between the three-subsample algorithm designed by the present invention and the conventional three-subsample algorithm.

Detailed Description

The method is based on the strapdown system speed calculation principle, uses classical rowing motion as an environmental condition, deduces and designs a calculation algorithm under angular rate output, determines an optimization coefficient by minimizing the deviation of the direct current quantity of a rowing compensation item in the algorithm and an actual value, finally provides a residual error expression of the optimization algorithm, and finally proves that the calculation precision of the new method is greatly superior to that of the traditional method through simulation experiments.

The object of the invention is achieved by the following measures:

1 speed resolution of strapdown system

Taking a geographic coordinate system as a navigation system, the velocity basic equation of the strapdown inertial navigation system can be expressed as follows:

${\stackrel{\•}{V}}^n=C_b^n{f}^b-(2{\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)\×V^n+g^n---\left(1\right)$

wherein: n, b denote the navigation system and the carrier system, V, respectivelyⁿRepresenting the projection of the velocity on the navigation system,

in the form of a matrix of poses,

representing the projection of the angular velocity of rotation of the earth on a navigation system,

representing the projection of the angular velocity of rotation of the navigational system relative to the earth system on the navigational system, f^bRepresenting specific force, g, of accelerometer outputⁿRepresenting the projection of the gravitational acceleration on the navigation system.

For the integral of equation (1), a digital recurrence algorithm of velocity resolution can be obtained (for writing convenience, the superscript representing the projection coordinate system is omitted):

$V_m=V_{m-1}+C_{m-1}{\∫}_{t_{m-1}}^{t_m}(f\left(t\right)+\mathrm{\Δ\θ}\left(t\right)\×f\left(t\right))\mathrm{dt}+{\∫}_{t_{m-1}}^{t_m}[g\left(t\right)-(2{\mathrm{\ω}}_{\mathrm{ie}}\left(t\right)+{\mathrm{\ω}}_{\mathrm{en}}\left(t\right))\×V_{m-1}]\mathrm{dt}---\left(2\right)$ in the formula: v_mAnd V_m-1Respectively represent t_mTime t and_m-1velocity vector of time, C_m-1Represents t_m-1Time attitude matrix, speed update period T = T_m-t_m-1，t_m-1Angle increment to t time

To t_mAngle increment in time

For the gyro output angular rate at time t, t_m-1Increment of speed by t timet_m-1To t_mIncrement of speed in time

As shown in the formula (2): the key term of the speed calculation is

Note the book $\mathrm{\Δ}{V_{\mathrm{sf}}}_m={\∫}_{t_{m-1}}^{t_m}(f\left(t\right)+\mathrm{\Δ\θ}\left(t\right)\×f\left(t\right))\mathrm{dt},$ Then ${\mathrm{\Δ}}_{{V_{\mathrm{sf}}}_m}={\mathrm{\ΔV}}_m+{t_{m-1}}_{t_m}^{}\mathrm{\Δ\θ}\left(t\right)\×f\left(t\right)\mathrm{dt},$ The transformation can be further expressed as:

$\mathrm{\Δ}{V_{\mathrm{sf}}}_m=\mathrm{\Δ}{V}_m+\frac{1}{2}\mathrm{\Δ}{\mathrm{\θ}}_m\×\mathrm{\Δ}{V}_m+\frac{1}{2}{\∫}_{t_{m-1}}^{t_m}[\mathrm{\Δ\θ}\left(t\right)\×f\left(t\right)+\mathrm{\ΔV}\left(t\right)\×\mathrm{\ω}\left(t\right)]\mathrm{dt}---\left(3\right)$

second term on right end of formula (3):a rotation effect compensation term called speed;

third item on right:

$\mathrm{\Δ}{V_{\mathrm{scul}}}_m=\frac{1}{2}{\∫}_{t_{m-1}}^{t_m}[\mathrm{\Δ\θ}\left(t\right)\×f\left(t\right)+\mathrm{\ΔV}\left(t\right)\×\mathrm{\ω}\left(t\right)]\mathrm{dt}---\left(4\right)$

a sculling effect compensation term called speed.

When the speed of the strapdown system is updated according to the formula (2), a rotation effect compensation item and a rowing effect compensation item must be considered at the same time due to the speed increment caused by acceleration, otherwise, a rowing error and a rotation error exist in speed calculation.

2 design of rowing compensation algorithm and optimization algorithm thereof

Firstly, general conditions of the method are described, then, the design process of the method is deduced and explained specifically for three-subsample situations, finally, a three-subsample sculling compensation algorithm formula and an optimization coefficient are obtained, a two-subsample sculling compensation algorithm formula and an optimization coefficient are directly given, and simulation examples are compared with the traditional algorithm. The stroke compensation algorithm formula and the optimization coefficient under other sub-sample conditions can be designed according to the general process described in the present invention, which is not described herein again.

The design process of the invention aiming at the n (n > 1) subsampling rowing compensation algorithm is as follows:

in the speed resolving period, the following linear polynomial fit is made to the angular speed and the specific force of the carrier:

$\left\{\begin{array}{c}\mathrm{\ω}\left(t\right)=a_1+{2a}_2(t-t_{m-1})+\…+{\mathrm{na}}_n{(t-t_{m-1})}^{n-1}\\ f\left(t\right)=A_1+{2A}_2(t-t_{m-1})+\…+{\mathrm{nA}}_n{(t-t_{m-1})}^{n-1}\end{array},-,-,-,\left(5\right)\right.$

in the formula: a is₁…a_n,A₁…A_nIs a constant vector.

In the formula (5) lineUnder the condition of sexual fit, t_m-1The angle increment and the speed increment in time to t are respectively:

$\left\{\begin{array}{c}\mathrm{\Δ\θ}\left(t\right)={\∫}_{t_{m-1}}^t\mathrm{\ω}\left(\mathrm{\τ}\right)\mathrm{d\τ}=a_1{(t-t_{m-1})}^2+\…+a_n{(t-t_{m-1})}^n\\ \mathrm{\Δ\θ}\left(t\right)={\∫}_{t_{m-1}}^{t}f\left(\mathrm{\τ}\right)\mathrm{d\τ}=A_1{(t-t_{m-1})}^2+\…+A_n{(t-t_{m-1})}^n\end{array},---\left(6\right)\right.$

speed resolving period T = T by n sampling points_m-t_m-1Are equally divided into intervals ofThe angular rate output of the gyroscope and the specific force output of the accelerometer at the sampling point obtained from equation (5) are respectively:

$\left\{\begin{array}{c}\mathrm{\ω}(\mathrm{\ΔT}+t_{m-1})=a_1+{2a}_2\mathrm{\ΔT}+\…+{\mathrm{na}}_n\mathrm{\Δ}{T}^{n-1}\\ \mathrm{\ω}(2\mathrm{\ΔT}+t_{m-1})=a_1+{4a}_2\mathrm{\ΔT}+\…+2^{n-1}{\mathrm{na}}_n{\mathrm{\ΔT}}^{n-1}\\ \•\\ \•\\ \•\\ \mathrm{\ω}(\mathrm{n\ΔT}+t_{m-1})=a_1+{2\mathrm{na}}_2\mathrm{\ΔT}+\…+n^n{a}_n{\mathrm{\ΔT}}^{n-1}\end{array}---\left(7\right)\right.$

$\left\{\begin{array}{c}f(\mathrm{\&Delta;T}+t_{m-1})=A_1+{2\mathrm{<figure-callout\; id="2"\; label="A"\; filenames="HDA0000075816450000012.png"\; state="\{\{state\}\}">A</figure-callout>}}_2\mathrm{\&Delta;T}+\&hellip;+{\mathrm{nA}}_n{\mathrm{\&Delta;T}}^{n-1}\\ f(2\mathrm{\&Delta;T}+t_{m-1})=A_1+{4A}_2\mathrm{\&Delta;T}+\&hellip;+2^{n-1}n{A}_n\mathrm{\&Delta;}{T}^{n-1}\\ \&bull;\\ \&bull;\\ \&bull;\\ f(\mathrm{n\&Delta;T}+t_{m-1})=A_1+{2\mathrm{<figure-callout\; id="2"\; label="nA"\; filenames="HDA0000075816450000012.png"\; state="\{\{state\}\}">nA</figure-callout>}}_2\mathrm{\&Delta;T}+\&hellip;+n^n{A}_n{\mathrm{\&Delta;T}}^{n-1}\end{array},---\left(8\right)\right.$

the coefficient a can be obtained by respectively solving the equation set (7) and the equation set (8)₁La_nAnd A₁LA_nTwo groups of coefficients are substituted into a formula (6), then combined with a formula (5) to solve a formula (4), and the n-subsample rowing compensation algorithm formula based on angular rate output can be obtained.

The optimization process of the n-subsample rowing compensation algorithm is described as follows:

firstly, calculating an accurate value of a rowing effect compensation term under a rowing motion condition by using a formula (4);

secondly, determining the direct current quantity in the n-subsample rowing compensation algorithm formula;

and thirdly, performing difference on the direct current quantity in the n-sub-sample rowing compensation algorithm formula and the accurate value of the rowing effect compensation item, performing Taylor series expansion on the difference value, and optimizing the n-sub-sample rowing compensation algorithm formula coefficient under the condition of ensuring that the difference value is minimum, wherein the coefficient is the optimization coefficient.

The following gives a specific derivation process of the three-subsample rowing compensation algorithm and the optimization algorithm thereof.

In the speed resolving period, the following linear polynomial fit is made to the angular speed and the specific force of the carrier:

$\left\{\begin{array}{c}\mathrm{\&omega;}\left(t\right)=a_1+{2a}_2(t-t_{m-1})+{3a}_3{(t-t_{m-1})}^2\\ f\left(t\right)=A_1+{2A}_2(t-t_{m-1})+{3A}_3{\left(t{-t}_{m-1}\right)}^2\end{array},---\left(9\right)\right.$

in the formula: a is₁,a₂,a₃,A₁,A₂,A₃Is a constant vector.

Under the condition of linear fitting of formula (9), t_m-1The angle increment and the speed increment in time to t are respectively:

$\left\{\begin{array}{c}\mathrm{\&Delta;\&theta;}\left(t\right)={\&Integral;}_{t_{m-1}}^t\mathrm{\&omega;}\left(\mathrm{\&tau;}\right)\mathrm{d\&tau;}=a_1(t-t_{m-1})+a_2{(t-t_{m-1})}^2+a_3{(t-t_{m-1})}^3\\ \mathrm{\&Delta;\&theta;}\left(t\right)={\&Integral;}_{m-1}^{t}f\left(\mathrm{\&tau;}\right)\mathrm{d\&tau;}=A_1(t-t_{m-1})+A_2{(t-t_{m-1})}^2+A_3{\left(t{-t}_{m-1}\right)}^3\end{array}---\left(10\right)\right.$

velocity resolution period T = T by three sampling points_m-t_m-1Are equally divided into intervals of

The gyroscope angular rate output and the accelerometer specific force output at the sampling point can be obtained from equation (9) as:

$\left\{\begin{array}{c}\mathrm{\&omega;}(\mathrm{\&Delta;T}+t_{m-1})=a_1+{2a}_2\mathrm{\&Delta;T}+{3a}_3\mathrm{\&Delta;}{\mathrm{<figure-callout\; id="2"\; label="T"\; filenames="HDA0000075816450000012.png"\; state="\{\{state\}\}">T</figure-callout>}}^2\\ \mathrm{\&omega;}(2\mathrm{\&Delta;T}+t_{m-1})=a_1+{4a}_2\mathrm{\&Delta;T}+{12a}_3\mathrm{\&Delta;}{\mathrm{<figure-callout\; id="2"\; label="T"\; filenames="HDA0000075816450000012.png"\; state="\{\{state\}\}">T</figure-callout>}}^2\\ \mathrm{\&omega;}(3\mathrm{\&Delta;T}+t_{m-1})=a_1+{6a}_2\mathrm{\&Delta;T}+{27a}_3\mathrm{\&Delta;}{T}^2\end{array}---\left(11\right)\right.$

$\left\{\begin{array}{c}f(\mathrm{\&Delta;T}+t_{m-1})=A_1+{2a}_2\mathrm{\&Delta;T}+{3a}_3\mathrm{\&Delta;}{T}^2\\ f(2\mathrm{\&Delta;T}+t_{m-1})=A_1+{4a}_2\mathrm{\&Delta;T}+{12a}_3\mathrm{\&Delta;}{T}^2\\ f(3\mathrm{\&Delta;T}+t_{m-1})=A_1+{6a}_2\mathrm{\&Delta;T}+{27a}_3\mathrm{\&Delta;}{T}^2\end{array}---\left(12\right)\right.$

and order:

$\left\{\begin{array}{c}{\mathrm{\&omega;}}_{m\left(1\right)}=\mathrm{\&omega;}(\mathrm{\&Delta;T}+t_{m-1})\\ {\mathrm{\&omega;}}_m\left(2\right)=\mathrm{\&omega;}(2\mathrm{\&Delta;T}+t_{m-1})\\ {\mathrm{\&omega;}}_m\left(3\right)=\mathrm{\&omega;}(3\mathrm{\&Delta;T}+t_{m-1})\end{array}\right.,$ $\left\{\begin{array}{c}{f}_m\left(1\right)=f(\mathrm{\&Delta;T}+t_{m-1})\\ f_m\left(2\right)=f(2\mathrm{\&Delta;T}+t_{m-1})\\ f_m\left(3\right)=f(3\mathrm{\&Delta;T}+t_{m-1})\end{array}\right.$

the equations (11) and (12) can be solved separately:

$\left[\begin{array}{c}{a}_1={3\mathrm{\&omega;}}_m\left(1\right)-{3\mathrm{\&omega;}}_m\left(2\right)+{\mathrm{\&omega;}}_m\left(3\right),A_1={3f}_m\left(1\right)-{3f}_m\left(2\right)+f_m\left(3\right)\\ a_2=\frac{1}{\mathrm{\&Delta;T}}[-\frac{5}{4}{\mathrm{\&omega;}}_m\left(1\right)+2{\mathrm{\&omega;}}_m\left(2\right)-\frac{3}{4}{\mathrm{\&omega;}}_m\left(3\right)],A_2=\frac{1}{\mathrm{\&Delta;T}}[-\frac{5}{4}{f}_m\left(1\right)+{2f}_m\left(2\right)-\frac{3}{4}{f}_m\left(3\right)]\\ a_3=\frac{1}{{\mathrm{\&Delta;T}}^2}[\frac{1}{6}{\mathrm{\&omega;}}_m\left(1\right)-\frac{1}{3}{\mathrm{\&omega;}}_m\left(2\right)+\frac{1}{6}{\mathrm{\&omega;}}_m\left(3\right)],A_3=\frac{1}{{\mathrm{\&Delta;T}}^2}[\frac{1}{6}{f}_m\left(1\right)-\frac{1}{3}{f}_m\left(2\right)+\frac{1}{6}{f}_m\left(3\right)]\end{array},---\left(13\right)\right]$

by substituting equation (13) for equation (10) and then combining equation (9), the term shown in equation (4), i.e., the formula of the stroke compensation algorithm based on the angular rate output, is:

$\mathrm{\&Delta;}{{\hat{V}}_{\mathrm{scul}}}_m\left[\begin{array}{c}={\mathrm{<figure-callout\; id="1"\; label="k"\; filenames="HDA0000075816450000011.png"\; state="\{\{state\}\}">k</figure-callout>}}_1{\mathrm{\&Delta;T}}^2[{\mathrm{\&omega;}}_m\left(1\right)\&times;f_m\left(2\right)+f_m\left(1\right)\&times;{\mathrm{\&omega;}}_m\left(2\right)]+\\ {\mathrm{<figure-callout\; id="2"\; label="k"\; filenames="HDA0000075816450000012.png"\; state="\{\{state\}\}">k</figure-callout>}}_2{\mathrm{\&Delta;T}}^2[{\mathrm{\&omega;}}_m\left(1\right)\&times;f_m\left(3\right)-{\mathrm{\&omega;}}_m\left(2\right)\&times;f_m\left(3\right)]+\\ {\mathrm{<figure-callout\; id="2"\; label="k"\; filenames="HDA0000075816450000012.png"\; state="\{\{state\}\}">k</figure-callout>}}_2{\mathrm{\&Delta;T}}^2[f_m\left(1\right)\&times;{\mathrm{\&omega;}}_m\left(3\right)-f_m\left(2\right)\&times;{\mathrm{\&omega;}}_m\left(3\right)]\end{array},---\left(14\right)\right]$

wherein: ${\mathrm{<figure-callout\; id="1"\; label="k"\; filenames="HDA0000075816450000011.png"\; state="\{\{state\}\}">k</figure-callout>}}_1=\frac{81}{40},{\mathrm{<figure-callout\; id="2"\; label="k"\; filenames="HDA0000075816450000012.png"\; state="\{\{state\}\}">k</figure-callout>}}_2=\frac{9}{40}.$

the optimization algorithm design process is as follows:

assuming that the carrier does a classical rowing motion, the model is as follows:

$\left\{\begin{array}{c}\mathrm{\&omega;}\left(t\right)=\mathrm{iB}\cos \mathrm{\&Omega;t}\\ f\left(t\right)=\mathrm{jC}\sin \mathrm{\&Omega;t}\end{array}---\left(15\right)\right.$

wherein: b and C are respectively the angular vibration amplitude and the linear vibration amplitude along two vertical axes of the carrier system, omega is the vibration frequency of the carrier, and i and j are unit vectors.

Let us say λ = Ω Δ T, τ_n=t_m-1+ n Δ T, n =1,2,3, considering the following two formulae (where k is the unit vector):

$\left[\begin{array}{c}{\mathrm{\&omega;}}_m\left(n\right)\&times;f_m\&times;(n+l)=\mathrm{iB\&Omega;}\cos \mathrm{\&Omega;}(t_{m-1}+\mathrm{n\&Delta;T})\&times;\mathrm{jC}\sin \mathrm{\&Omega;}(t_{m-1}+(n+l)\mathrm{\&Delta;T})\\ =\mathrm{<figure-callout\; id="1"\; label="k"\; filenames="HDA0000075816450000011.png"\; state="\{\{state\}\}">k</figure-callout>}\frac{1}{2}\mathrm{BC\&Omega;}(\sin \mathrm{l\&lambda;}+\sin ({2\mathrm{\&Omega;\&tau;}}_n+\mathrm{l\&lambda;}))\end{array},---\left(16\right)\right]$

$\left[\begin{array}{c}{f}_m\left(n\right)\&times;{\mathrm{\&omega;}}_m\&times;(n+l)=\mathrm{jC\&Omega;}\sin \mathrm{\&Omega;}(t_{m-1}+\mathrm{n\&Delta;T})\&times;\mathrm{iB}\cos \mathrm{\&Omega;}(t_{m-1}+(n+l)\mathrm{\&Delta;T})\\ =\mathrm{<figure-callout\; id="1"\; label="k"\; filenames="HDA0000075816450000011.png"\; state="\{\{state\}\}">k</figure-callout>}\frac{1}{2}\mathrm{BC\&Omega;}(\sin \mathrm{l\&lambda;}+\sin ({2\mathrm{\&Omega;\&tau;}}_n+\mathrm{l\&lambda;}))\end{array}---\left(17\right)\right]$

comparing equation (16) with equation (17) shows that: omega_m(n)×f_m(n + l) and f_m(n)×ω_mThe dc-amount in the result of (n + l) is only related to the sampling interval/, independent of the sampling instants and the cross-product order. Therefore, the dc component of equation (14) is:

$\mathrm{\&Delta;}{{\stackrel{\&OverBar;}{\hat{V}}}_{\mathrm{scui}}}_m=k\frac{\mathrm{BC}}{\mathrm{\&Omega;}}{\mathrm{\&lambda;}}^2({\mathrm{<figure-callout\; id="1"\; label="k"\; filenames="HDA0000075816450000011.png"\; state="\{\{state\}\}">k</figure-callout>}}_1\sin \mathrm{\&lambda;}+{\mathrm{<figure-callout\; id="2"\; label="k"\; filenames="HDA0000075816450000012.png"\; state="\{\{state\}\}">k</figure-callout>}}_2\mathrm{<figure-callout\; id="2"\; label="sin"\; filenames="HDA0000075816450000012.png"\; state="\{\{state\}\}">sin</figure-callout>}2\mathrm{\&lambda;}-{\mathrm{<figure-callout\; id="2"\; label="k"\; filenames="HDA0000075816450000012.png"\; state="\{\{state\}\}">k</figure-callout>}}_2\sin \mathrm{\&lambda;})---\left(18\right)$

the calculation shows that the accurate value of the paddling effect compensation term under the paddling motion condition is as follows:

therefore, the algorithm error caused by the three-subsample rowing compensation algorithm is as follows:

$\left[\begin{array}{c}\mathrm{\&Delta;}{{\tilde{V}}_{\mathrm{scui}}}_m=\mathrm{\&Delta;}{{\stackrel{\&OverBar;}{\hat{V}}}_{\mathrm{scul}}}_m-\mathrm{\&Delta;}{{V_{\mathrm{scul}}}_m}^{\&prime;}\\ =k\frac{\mathrm{BC}}{\mathrm{\&Omega;}}({\mathrm{<figure-callout\; id="1"\; label="k"\; filenames="HDA0000075816450000011.png"\; state="\{\{state\}\}">k</figure-callout>}}_1{\mathrm{\&lambda;}}^2\sin \mathrm{\&lambda;}+{\mathrm{<figure-callout\; id="2"\; label="k"\; filenames="HDA0000075816450000012.png"\; state="\{\{state\}\}">k</figure-callout>}}_2{\mathrm{\&lambda;}}^2\mathrm{<figure-callout\; id="2"\; label="sin"\; filenames="HDA0000075816450000012.png"\; state="\{\{state\}\}">sin</figure-callout>}2\mathrm{\&lambda;})+k\frac{\mathrm{BC}}{\mathrm{\&Omega;}}(-{\mathrm{<figure-callout\; id="2"\; label="k"\; filenames="HDA0000075816450000012.png"\; state="\{\{state\}\}">k</figure-callout>}}_2{\mathrm{\&lambda;}}^2\sin \mathrm{\&lambda;}-\frac{3}{2}\mathrm{\&lambda;}+\frac{1}{2}\mathrm{<figure-callout\; id="3"\; label="sin"\; filenames="HDA0000075816450000011.png,HDA0000075816450000012.png"\; state="\{\{state\}\}">sin</figure-callout>}3\mathrm{\&lambda;})\end{array}---\left(19\right)\right]$

the sinusoidal terms in the formula (19) are expanded according to the taylor series to obtain the power terms of lambda as follows:

${\mathrm{\&lambda;}}^3({\mathrm{<figure-callout\; id="1"\; label="k"\; filenames="HDA0000075816450000011.png"\; state="\{\{state\}\}">k</figure-callout>}}_1+k_2-\frac{9}{4}),{\mathrm{\&lambda;}}^5(-\frac{{\mathrm{<figure-callout\; id="1"\; label="k"\; filenames="HDA0000075816450000011.png"\; state="\{\{state\}\}">k</figure-callout>}}_1}{6}-\frac{{7\mathrm{<figure-callout\; id="2"\; label="k"\; filenames="HDA0000075816450000012.png"\; state="\{\{state\}\}">k</figure-callout>}}_2}{6}+\frac{81}{80}),{\mathrm{\&lambda;}}^7(\frac{{\mathrm{<figure-callout\; id="1"\; label="k"\; filenames="HDA0000075816450000011.png"\; state="\{\{state\}\}">k</figure-callout>}}_1}{240}+\frac{{31\mathrm{<figure-callout\; id="2"\; label="k"\; filenames="HDA0000075816450000012.png"\; state="\{\{state\}\}">k</figure-callout>}}_2}{240}-\frac{243}{1120}),\&hellip;\left(20\right)$

since λ = Ω Δ T, the carrier vibration is mechanical vibration, the frequency is usually not high, and Δ T is a sampling period and has an order of millisecond, generally λ <1, and it can be seen that the lower the λ power is, the larger the influence on the error is, so that the coefficient should be selected to have the lowest power coefficient as zero as possible, and for the trigonometric algorithm, it is assumed that the coefficients of the third power and the fifth power are zero, that is, the following steps are provided:

$\left\{\begin{array}{c}{\mathrm{<figure-callout\; id="1"\; label="k"\; filenames="HDA0000075816450000011.png"\; state="\{\{state\}\}">k</figure-callout>}}_1+k_2-\frac{9}{4}=0\\ -\frac{{\mathrm{<figure-callout\; id="1"\; label="k"\; filenames="HDA0000075816450000011.png"\; state="\{\{state\}\}">k</figure-callout>}}_1}{6}-\frac{{7\mathrm{<figure-callout\; id="2"\; label="k"\; filenames="HDA0000075816450000012.png"\; state="\{\{state\}\}">k</figure-callout>}}_2}{6}+\frac{81}{80}=0\end{array},---\left(21\right)\right.$

solving the above equation set, the obtained optimization coefficient is:

the residual error of the optimization algorithm is known from equation (21):

$\mathrm{\&Delta;}{{\tilde{V}}_{\mathrm{scul}}}_m\&ap;{\mathrm{\&lambda;}}^7(\frac{{\mathrm{<figure-callout\; id="1"\; label="k"\; filenames="HDA0000075816450000011.png"\; state="\{\{state\}\}">k</figure-callout>}}_1}{240}+\frac{31{\mathrm{<figure-callout\; id="2"\; label="k"\; filenames="HDA0000075816450000012.png"\; state="\{\{state\}\}">k</figure-callout>}}_2}{240}-\frac{243}{1120})=-\frac{573}{4480}\mathrm{BC}{\mathrm{\&Omega;}}^6\mathrm{\&Delta;}{T}^7---\left(22\right)$

table 1 lists the formulas and optimization coefficients of the two-subsample and three-subsample sculling compensation algorithms designed by the present invention, and other formulas and optimization coefficients of the multi-subsample sculling compensation algorithms can be derived according to the above n-subsample sculling compensation algorithms and the optimization coefficient design process thereof, which are not listed one by one.

3 traditional three-subsample rowing compensation algorithm

In the case of an increment in the output of the inertial devices (gyroscope and accelerometer), the three-subsample rowing compensation algorithm is:

$\left[\begin{array}{c}\mathrm{\&Delta;}{{\hat{V}}_{\mathrm{scul}}}_m=k_1[\mathrm{\&Delta;}{V}_m\left(1\right)\&times;\mathrm{\&Delta;}{\mathrm{\&theta;}}_m\left(3\right)+{\mathrm{\&Delta;V}}_m\left(1\right)\&times;{\mathrm{\&Delta;V}}_m\left(3\right)]+k_2[{\mathrm{\&Delta;V}}_m\left(1\right)\&times;{\mathrm{\&Delta;\&theta;}}_m\left(2\right)+{\mathrm{\&Delta;V}}_m\left(2\right)\&times;{\mathrm{\&Delta;\&theta;}}_m\left(3\right)]+\\ k_2[{\mathrm{\&Delta;\&theta;}}_m\left(1\right)\&times;\mathrm{\&Delta;}{V}_m\left(2\right)+{\mathrm{\&Delta;\&theta;}}_m\left(2\right)\&times;{\mathrm{\&Delta;V}}_m\left(3\right)\end{array},---\left(23\right)\right]$

wherein:

ΔV_m(1),ΔV_m(2),ΔV_m(3) and Δ θ_m(1),Δθ_m(2),Δθ_m(3) Are respectively a sampling time period t_m-1,t_m-1+ΔT],[t_m-1+ΔT,t_m-1+2ΔT],[t_m-1+2ΔT,t_m]Inner velocity increment and angleIncrement, sampling period

And (3) optimizing the coefficient under the condition of the rowing movement:

table 1 lists the formulas and the optimization coefficients of the traditional two-subsample and three-subsample rowing compensation algorithms. Table 2 shows the carrier amplitude along the X-axis

The angular vibration of the carrier is performed along the Y axis, the amplitude C =100 line vibration is performed, the sampling period delta T =0.01s, and when the carrier adopts different motion frequencies, the error value of the two-subsample algorithm designed by the invention is obtained. Table 3 shows the carrier amplitude along the X-axis

The angular vibration of the carrier is performed along the Y axis, the amplitude C =100 line vibration is performed, the sampling period delta T =0.01s, and when the carrier adopts different motion frequencies, the error of the traditional three-subsample algorithm is compared with that of the three-subsample algorithm designed by the invention.

TABLE 1 Algorithm formula and optimization coefficient comparison

TABLE 2 errors of the binary sample algorithm designed by the present invention

TABLE 3 error comparison of conventional three-subsample algorithm and the three-subsample algorithm designed by the present invention

Frequency of motion/Hz	Traditional algorithm (m/s)	The invention (m/s)
			10	-0.0208	-1.3394e-007
20	-0.0822	-8.5721e-006
			50	-0.4665	-0.0021
80	-0.9970	-0.0351

It can be seen from the above table that under the same motion frequency condition, the error of the three-subsample paddling compensation algorithm designed by the invention is far smaller than that of the traditional three-subsample paddling compensation algorithm, and fig. 1 shows the rule that the error of the traditional three-subsample paddling compensation algorithm and the error of the three-subsample paddling compensation algorithm designed by the invention changes along with the motion frequency more intuitively in a curve form. Fig. 2 reflects the rule that the error of the conventional three-subsample sculling compensation algorithm and the three-subsample sculling compensation algorithm designed by the present invention changes with the sampling period, and it can be seen from the figure that the calculation accuracy of the three-subsample sculling compensation algorithm designed by the present invention is far higher than that of the conventional algorithm under the same sampling period condition.

Claims (1)

1. A strapdown system speed resolving method based on angular rate output specifically comprises the following steps: the vector is at t_mVelocity of time of day $V_m=V_{m-1}+{\&Integral;}_{t_{m-1}}^{t_m}[g\left(t\right)-(2{\mathrm{\&omega;}}_{\mathrm{ie}}\left(t\right)+{\mathrm{\&omega;}}_{\mathrm{en}}\left(t\right))\&times;V_{m-1}]\mathrm{dt}+C_{m-1}\mathrm{\&Delta;}{V}_{\mathrm{sfm}},$

Wherein,

V_m-1is a vector at t_m-1The speed of the moment in time is,

C_m-1is a vector at t_m-1The matrix of the attitude at the time of day,

g(t)、ω_ie(t)、ω_en(t) the gravity acceleration, the earth rotation angular velocity and the rotation angular velocity of the navigation system relative to the earth system at the time t are respectively;

$\mathrm{\&Delta;}{V}_{\mathrm{sfm}}=\mathrm{\&Delta;}{V}_m+\frac{1}{2}\mathrm{\&Delta;}{\mathrm{\&theta;}}_m\&times;\mathrm{\&Delta;}{V}_m+\mathrm{\&Delta;}{\hat{V}}_{{\mathrm{scul}}_m};$

the output specific force of the accelerometer at the time t;

the output angular rate of the gyroscope at the time t;

sculling effect compensation term

The solving process of the n subsamples is as follows:

(a1) over n sampling points the speed resolving period T = T_m-t_m-1Are equally divided into intervals of

N sub-periods of time, to $\left\{\begin{array}{c}\mathrm{\&omega;}\left(t\right)=a_1+{2a}_2(t-t_{m-1})+\&hellip;+{\mathrm{na}}_n{(t-t_{m-1})}^{n-1}\\ f\left(t\right)=A_1+{2A}_2(t-t_{m-1})+\&hellip;+{\mathrm{nA}}_n{(t-t_{m-1})}^{n-1}\end{array}\right.$ Performing polynomial linear fitting on the angular rate output of the gyroscope and the specific force output of the accelerometer at the n sampling points for an objective function;

(a2) fitting the determined constant vector a using step (a 1)₁…a_n,A₁…A_nComputing

$\left\{\begin{array}{c}\mathrm{\&Delta;\&theta;}\left(t\right)={\&Integral;}_{t_{m-1}}^t\mathrm{\&omega;}\left(\mathrm{\&tau;}\right)\mathrm{d\&tau;}=a_1(t-t_{m-1})+a_2{(t-t_{m-1})}^2+\&hellip;+a_n{(t-t_{m-1})}^n\\ \mathrm{\&Delta;V}\left(t\right)={\&Integral;}_{t_{m-1}}^{t}f\left(\mathrm{\&tau;}\right)\mathrm{d\&tau;}=A_1(t-t_{m-1})+A_2{(t-t_{m-1})}^2+\&hellip;+A_n{(t-t_{m-1})}^n\end{array},,t_{m-1}\&le;t\&le;t_m;\right.$

(a3) Construction of a calculation formula of a stroke effect compensation term

$\mathrm{\&Delta;}{{\hat{V}}_{\mathrm{scul}}}_m=\frac{1}{2}{\&Integral;}_{t_{m-1}}^{t_m}[\mathrm{\&Delta;\&theta;}\left(t\right)\&times;f\left(t\right)+\mathrm{\&Delta;V}\left(t\right)\&times;\mathrm{\&omega;}\left(t\right)]\mathrm{dt};$

(a4) Calculating the accurate value of the stroke effect compensation term under the condition of the stroke motion by using the stroke effect compensation term calculation formula of the step (a 3)

(a5) Determining the DC component in the calculation formula of the stroke effect compensation term of step (a 3)

(a6) By difference

And (c) calculating a formula for the stroke effect compensation term in the target optimization step (a 3).

Images