CN102288177A - Strapdown system speed calculating method based on angular speed output - Google Patents
Strapdown system speed calculating method based on angular speed output Download PDFInfo
- Publication number
- CN102288177A CN102288177A CN201110199065XA CN201110199065A CN102288177A CN 102288177 A CN102288177 A CN 102288177A CN 201110199065X A CN201110199065X A CN 201110199065XA CN 201110199065 A CN201110199065 A CN 201110199065A CN 102288177 A CN102288177 A CN 102288177A
- Authority
- CN
- China
- Prior art keywords
- delta
- centerdot
- omega
- paddle
- output
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Landscapes
- Gyroscopes (AREA)
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
Technical field
The present invention relates to a kind of strapdown system velocity calculated method, especially at strapdown system velocity calculated method based on angular speed output.
Background technology
Strapdown system is a kind of inertial navigation system that gyroscope and accelerometer is installed on carrier, it comes actual platform in the analog platform formula inertial navigation system by calculating " mathematical platform ", thereby cost reduces greatly, but strapdown system but improves greatly to the requirement of navigation calculation.In the past, researchers are output as angle signal as prerequisite with gyroscope, have proposed traditional velocity calculated method based on the increment output signal.In recent years, development along with optical fiber communication technology and optical fiber sensing technology, fibre optic gyroscope becomes the important technology of inertial navigation, for the strapdown system that constitutes by optical fibre gyro (as interfere type), gyroscope is output as angle rate signal, when traditional strapdown system velocity calculated method based on the increment output signal is applied to optical fiber gyroscope strapping system, need extract angle increment by angular speed, and then utilize classic method to realize velocity calculated, so both increased the Design of System Software difficulty, greater loss had also been arranged on calculation accuracy simultaneously.
Invention Inner holds
In order to overcome the deficiency of conventional speed calculation method in optical fiber gyroscope strapping system is used, the present invention proposes a kind of strapdown system velocity calculated method based on angular speed output, directly utilize the angle rate signal of gyroscope output to calculate, improve calculation accuracy, reduce computation complexity.
A kind of strapdown system velocity calculated method based on angular speed output, be specially: carrier is at t
mSpeed constantly
Wherein,
V
M-1For carrier at t
M-1Speed constantly,
C
M-1For carrier at t
M-1Attitude matrix constantly,
G (t), ω
Ie(t), ω
En(t) being respectively t acceleration of gravity, rotational-angular velocity of the earth, navigation constantly is the rotational angular velocity that the relative earth is;
ω (t) is that gyro is in t output angle speed constantly;
(a1) cross n sampled point with velocity calculated period T=t
m-t
M-1Be divided into and be spaced apart
N sub-period, with
For objective function is made the polynomial expression linear fit to the specific force output of gyrostatic angular speed output of this n sample point and accelerometer;
(a2) utilize step (a1) to fit definite normal vector a
1A
n, A
1A
nCalculate
(a3) make up paddle effect compensating item computing formula
(a4) utilize the paddle effect compensating item computing formula of step (a3) to calculate the exact value of paddle effect compensating item under the paddle moving condition
(a6) with difference
Minimum is the paddle effect compensating item computing formula in the objective optimization step (a3).
The present invention compares traditional algorithm and has following advantage:
1. when being applied to be output as the strapdown system of angle rate signal, can directly utilize the output signal of inertia device, need not conversion, simple.
2. when being applied to be output as the strapdown system of angle rate signal, calculation accuracy is far superior to traditional algorithm.
Description of drawings
Fig. 1 is that three increment algorithms and the traditional three increment Algorithm Error that the present invention designs contrast synoptic diagram with paddle motion frequency change curve.
Fig. 2 is that three increment algorithms and the traditional three increment Algorithm Error that the present invention designs contrast synoptic diagram with the sampling period change curve.
Embodiment
The present invention is from strapdown system velocity calculated principle, paddle campaign with classics is an environmental baseline, derive and designed the algorithm that resolves under the angular speed output, and by making paddle compensation term DC quantity and the minimum next coefficient of determining to optimize of actual value deviation in the algorithm, and finally provide the remainder error expression formula of optimized Algorithm, proved that by emulation experiment the calculation accuracy of new method will be better than classic method greatly at last.
Purpose of the present invention can reach by following measure:
The velocity calculated of 1 strapdown system
Getting geographic coordinate is navigation system, and strapdown inertial navitation system (SINS) speed fundamental equation can be expressed as:
Wherein: n, b represent navigation system and carrier system, V respectively
nThe projection that expression speed is fastened in navigation,
Be attitude matrix,
The projection that the expression rotational-angular velocity of the earth is fastened in navigation,
Expression navigation is the projection that the rotational angular velocity of relative earth system is fastened in navigation, f
bThe specific force of expression accelerometer output, g
nThe projection that expression acceleration of gravity is fastened in navigation.
To (1) formula integration, can get the digital recursive algorithm (, omitting the subscript of expression projected coordinate system) of velocity calculated for writing conveniently:
In the formula: V
mAnd V
M-1Represent t respectively
mThe moment and t
M-1Velocity constantly, C
M-1Expression t
M-1Moment attitude matrix, Velocity Updating period T=t
m-t
M-1, t
M-1To interior angle step of t time
To t
mAngle step in time
ω (t) is a t gyro output angle speed constantly, t
M-1To interior speed increment of t time
t
M-1To t
mSpeed increment in time
By formula (2) as can be known: the critical item of velocity calculated is
Note
Then
Can further be expressed as by conversion:
Second of formula (3) right-hand member:
The rotation effect compensation term that is called speed;
The 3rd of right-hand member:
The paddle effect compensating item that is called speed.
When strapdown system is done the speed renewal by formula (2), because the speed increment that acceleration causes must be considered rotation effect compensation term and paddle effect compensating item simultaneously, otherwise will there be paddle sum of errors rotation error in speed calculation, the present invention is directed to wherein main error component paddle error design paddle backoff algorithm and optimized Algorithm thereof.
The design of 2 paddle backoff algorithms and optimized Algorithm thereof
At first be described at generalized case of the present invention, specifically derive then design process of the present invention is described at three increment situations, obtained three increment paddle backoff algorithm formula at last and optimized coefficient also directly having provided two increment situation paddle backoff algorithm formula and having optimized coefficient, and carried out the simulation example contrast with traditional algorithm.Paddle backoff algorithm formula under other increment situation and optimization coefficient can design according to the general process that the present invention describes, and do not give unnecessary details one by one at this.
The design process that the present invention is directed to n (n>1) increment paddle backoff algorithm is as follows:
At velocity calculated in the cycle, to the angular velocity of carrier with than the following linear polynomial match of masterpiece:
In the formula: a
1A
n, A
1A
nBe normal vector.
Under formula (5) linear fit condition, t
M-1Be respectively to interior angle step of t time and speed increment:
By n sampled point with velocity calculated period T=t
m-t
M-1Be divided into and be spaced apart
N sub-period, then can get sample point gyroscope angular speed output and accelerometer specific force and export and be respectively by formula (5):
Solving equation group (7) and system of equations (8) can get coefficient a respectively
1A
nAnd A
1A
n, with two groups of coefficient substitution formulas (6), association type (5) is found the solution formula (4) again, gets final product based on the n increment paddle backoff algorithm formula of angular speed output.
N increment paddle backoff algorithm optimizing process is described below:
1. at first utilize formula (4) to calculate the exact value of paddle effect compensating item under the paddle moving condition;
2. determine the DC quantity in the n increment paddle backoff algorithm formula then;
3. it is poor at last the exact value of DC quantity in the n increment paddle backoff algorithm formula and paddle effect compensating item to be done, difference is made Taylor series expansion, guaranteeing that difference is to optimize n increment paddle backoff algorithm formula coefficient under the minimum situation, this coefficient is the optimization coefficient.
Provide the concrete derivation of three increment paddle backoff algorithms and optimized Algorithm thereof below.
At velocity calculated in the cycle, to the angular velocity of carrier with than the following linear polynomial match of masterpiece:
In the formula: a
1, a
2, a
3, A
1, A
2, A
3Be normal vector.
Under formula (9) linear fit condition, t
M-1Be respectively to interior angle step of t time and speed increment:
By three sampled points with velocity calculated period T=t
m-t
M-1Be divided into and be spaced apart
Three sub-periods, then can get sample point gyroscope angular speed output and the accelerometer specific force is output as by formula (9):
And order:
Find the solution formula (11) respectively and formula (12) can get:
a
1=3ω
m(1)-3ω
m(2)+ω
m(3),A
1=3f
m(1)-3f
m(2)+f
m(3)
With formula (13) substitution formula (10), association type (9) again, formula (4) institute aspect then, that is based on the paddle backoff algorithm formula of angular speed output be:
Wherein:
The Design of Optimal Algorithm process is as follows:
Suppose the classical paddle campaign of carrier do, its model is as follows:
Wherein: B, C are respectively to be the angular oscillation amplitude and the line vibration amplitude of two Z-axises along carrier, and Ω is the carrier vibration frequency, and i, j are unit vector.
If note λ=Ω Δ T, τ
n=t
M-1+ n Δ T, n=1,2,3, consider following two formulas (wherein k is a unit vector):
Comparison expression (16) and formula (17) be as can be known: ω
m(n) * f
m(n+l) and f
m(n) * ω
m(n+l) DC quantity among the result is only relevant with sampling interval l, with sampling instant and multiplication cross sequence independence.Therefore formula (14) DC quantity is:
Calculate as can be known, the exact value of paddle effect compensating item is under the paddle moving condition:
It is as follows that sine term in the formula (19) is obtained each power of λ time after by Taylor series expansion:
Because λ=Ω Δ T, the carrier vibration is mechanical vibration, usually frequency is not high, and Δ T is the sampling period, and magnitude is a Millisecond, so general λ<1, as seen time low more influence to error of λ power is big more, and should make low order power coefficient when therefore selecting coefficient is zero as far as possible, for three increment algorithms, suppose that third power and five power coefficients are zero, promptly have:
The remainder error of being known optimized Algorithm by formula (21) is:
Table 1 has been listed two increments and the three increment paddle backoff algorithm formula of the present invention's design and has been optimized coefficient, other many increments paddle backoff algorithm formula and optimization coefficient can and be optimized the derivation of coefficient design process according to said n increment paddle backoff algorithm and draw, and have not enumerated one by one here.
3 three traditional increment paddle backoff algorithms
Be output as under the increment situation at inertia device (gyroscope and accelerometer), three increment paddle backoff algorithms are:
Wherein:
Δ V
m(1), Δ V
m(2), Δ V
m(3) and Δ θ
m(1), Δ θ
m(2), Δ θ
m(3) be respectively sampling time section [t
M-1, t
M-1+ Δ T], [t
M-1+ Δ T, t
M-1+ 2 Δ T], [t
M-1+ 2 Δ T, t
m] interior speed increment and angle increment, the sampling period
Under the paddle motion conditions, optimize coefficient:
Table 1 has been listed traditional two increments and three increment paddle backoff algorithm formula and has been optimized coefficient.What table 2 was listed is that carrier is done amplitude along X-axis
Angular oscillation, do the line vibration of amplitude C=100 along Y-axis, and sampling period Δ T=0.01s, when carrier is taked the different motion frequency, two increment Algorithm Error values of the present invention's design.What table 3 was listed is that carrier is done amplitude along X-axis
Angular oscillation, do the line vibration of amplitude C=100 along Y-axis, and sampling period Δ T=0.01s, when carrier is taked the different motion frequency, the contrast of the three increment Algorithm Error that traditional three increment algorithms and the present invention design.
Table 1 algorithmic formula and optimization index contrast
The error of two increment algorithms of table 2 the present invention design
The traditional three increment algorithms of table 3 and the present invention design the contrast of three increment Algorithm Error
Motion frequency/Hz | Traditional algorithm (m/s) | The present 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 |
From above-mentioned table as can be seen, under the same movement frequency condition, three increment paddle backoff algorithm errors of the present invention's design are far smaller than traditional three increment Algorithm Error, and Fig. 1 has reflected more intuitively that with curve form traditional three increment paddle backoff algorithms and the present invention design the rule of the error of three increment paddle backoff algorithms with the motion frequency variation.What Fig. 2 reflected is that traditional three increment paddle backoff algorithms and the present invention design the rule of the error of three increment paddle backoff algorithms with the sampling period variation, as can be seen from the figure, under the identical sampling period condition, the three increment paddles compensation of the present invention's design is calculated precision and is higher than traditional algorithm far away.
Claims (2)
1. strapdown system velocity calculated method based on angular speed output, be specially: carrier is at t
mSpeed constantly
Wherein,
V
M-1For carrier at t
M-1Speed constantly,
C
M-1For carrier at t
M-1Attitude matrix constantly,
G (t), ω
Ie(t), ω
En(t) being respectively t acceleration of gravity, rotational-angular velocity of the earth, navigation constantly is the rotational angular velocity that the relative earth is;
(a1) cross n sampled point with velocity calculated period T=t
m-t
M-1Be divided into and be spaced apart
N sub-period, with
For objective function is made the polynomial expression linear fit to the specific force output of gyrostatic angular speed output of this n sample point and accelerometer;
(a2) utilize step (a1) to fit definite normal vector a
1A
n, A
1A
nCalculate
(a3) make up paddle effect compensating item computing formula
2. the strapdown system velocity calculated method based on angular speed output according to claim 1 is characterized in that,
N predictor process also comprise step:
(a4) utilize the paddle effect compensating item computing formula of step (a3) to calculate the exact value of paddle effect compensating item under the paddle moving condition
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN 201110199065 CN102288177B (en) | 2011-07-14 | 2011-07-14 | Strapdown system speed calculating method based on angular speed output |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN 201110199065 CN102288177B (en) | 2011-07-14 | 2011-07-14 | Strapdown system speed calculating method based on angular speed output |
Publications (2)
Publication Number | Publication Date |
---|---|
CN102288177A true CN102288177A (en) | 2011-12-21 |
CN102288177B CN102288177B (en) | 2013-10-30 |
Family
ID=45334801
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN 201110199065 Expired - Fee Related CN102288177B (en) | 2011-07-14 | 2011-07-14 | Strapdown system speed calculating method based on angular speed output |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN102288177B (en) |
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102519458A (en) * | 2011-12-16 | 2012-06-27 | 浙江大学 | Rowing motion compensation method for fiber optic gyroscope trapdown inertial navigation |
CN103175528A (en) * | 2013-02-19 | 2013-06-26 | 哈尔滨工程大学 | Strap-down compass gesture measurement method based on strap-down inertial navigation system |
CN104677356A (en) * | 2015-03-16 | 2015-06-03 | 黄磊 | Paddling speed calculation method based on angle increment output and specific force output |
CN105771219A (en) * | 2014-12-24 | 2016-07-20 | 北京中体动力数字技术有限公司 | Stroke frequency acquisition device and acquisition method thereof |
CN110345942A (en) * | 2019-07-17 | 2019-10-18 | 哈尔滨工程大学 | A kind of three increment Compensation for Coning Error algorithm of interpolation based on angular speed input |
CN110487268A (en) * | 2019-07-17 | 2019-11-22 | 哈尔滨工程大学 | A kind of three increment sculling Error Compensation Algorithm of interpolation inputted based on angular speed and specific force |
CN111351483A (en) * | 2020-03-31 | 2020-06-30 | 北京控制工程研究所 | Recursive multi-subsample large dynamic inertial navigation method |
-
2011
- 2011-07-14 CN CN 201110199065 patent/CN102288177B/en not_active Expired - Fee Related
Non-Patent Citations (3)
Title |
---|
张泽等: "新的捷联惯性导航划桨误差补偿算法", 《吉林大学学报(工学版)》 * |
秦永元: "《惯性导航》", 31 May 2006, 北京:科学出版社 * |
肖虎等: "光纤陀螺捷联惯导系统速度算法的改进研究", 《弹箭与制导学报》 * |
Cited By (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102519458A (en) * | 2011-12-16 | 2012-06-27 | 浙江大学 | Rowing motion compensation method for fiber optic gyroscope trapdown inertial navigation |
CN103175528A (en) * | 2013-02-19 | 2013-06-26 | 哈尔滨工程大学 | Strap-down compass gesture measurement method based on strap-down inertial navigation system |
CN103175528B (en) * | 2013-02-19 | 2015-04-08 | 哈尔滨工程大学 | Strap-down compass gesture measurement method based on strap-down inertial navigation system |
CN105771219A (en) * | 2014-12-24 | 2016-07-20 | 北京中体动力数字技术有限公司 | Stroke frequency acquisition device and acquisition method thereof |
CN104677356A (en) * | 2015-03-16 | 2015-06-03 | 黄磊 | Paddling speed calculation method based on angle increment output and specific force output |
CN104677356B (en) * | 2015-03-16 | 2017-06-16 | 黄磊 | A kind of paddle speed calculation method exported based on angle increment and specific force |
CN110345942A (en) * | 2019-07-17 | 2019-10-18 | 哈尔滨工程大学 | A kind of three increment Compensation for Coning Error algorithm of interpolation based on angular speed input |
CN110487268A (en) * | 2019-07-17 | 2019-11-22 | 哈尔滨工程大学 | A kind of three increment sculling Error Compensation Algorithm of interpolation inputted based on angular speed and specific force |
CN110487268B (en) * | 2019-07-17 | 2023-01-03 | 哈尔滨工程大学 | Interpolation three-subsample sculling effect error compensation method based on angular rate and specific force input |
CN111351483A (en) * | 2020-03-31 | 2020-06-30 | 北京控制工程研究所 | Recursive multi-subsample large dynamic inertial navigation method |
Also Published As
Publication number | Publication date |
---|---|
CN102288177B (en) | 2013-10-30 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN102288177A (en) | Strapdown system speed calculating method based on angular speed output | |
CN100547352C (en) | The ground speed testing methods that is suitable for fiber optic gyro strapdown inertial navigation system | |
CN103776446B (en) | A kind of pedestrian's independent navigation computation based on double MEMS-IMU | |
CN102486377B (en) | Method for acquiring initial course attitude of fiber optic gyro strapdown inertial navigation system | |
CN103900574B (en) | Attitude estimation method based on iteration volume Kalman filter | |
CN101706287B (en) | Rotating strapdown system on-site proving method based on digital high-passing filtering | |
CN103344260B (en) | Based on the strapdown inertial navitation system (SINS) Initial Alignment of Large Azimuth Misalignment On method of RBCKF | |
CN101915588A (en) | Method for compensating temperature error of inertial device | |
CN103278163A (en) | Nonlinear-model-based SINS/DVL (strapdown inertial navigation system/doppler velocity log) integrated navigation method | |
CN101187561A (en) | Carrier posture measuring method suitable for optical fiber gyroscope | |
CN103090866A (en) | Method for restraining speed errors of single-shaft rotation optical fiber gyro strapdown inertial navigation system | |
CN100559190C (en) | A kind of method of demarcating that the accelerometer zero drift is carried out at rail | |
CN103076026A (en) | Method for determining speed measurement error of Doppler velocity log (DVL) in strapdown inertial navigation system | |
CN107741240A (en) | A kind of combined inertial nevigation system self-adaption Initial Alignment Method suitable for communication in moving | |
CN101294811A (en) | Strapdown inertial navigation system adopting strange perturbation method for taper cone error and rowing error compensation | |
CN103364842B (en) | A kind of error separation method of strapdown airborne gravitometer | |
CN109443378B (en) | Speed-assisted backtracking initial alignment method between marchs | |
CN102506862B (en) | Cone algorithm based on second-order non-communicative error compensation model | |
CN102997932B (en) | A kind of method eliminating high accuracy inertial navigation system demarcation intermediate station effect of jitter | |
CN103591960B (en) | A kind of quiet base inertial navigation system coarse alignment method based on rotation modulation | |
CN103292810B (en) | A kind of rotary inertia guiding systems signal synchronous compensation method | |
CN105136150A (en) | Attitude determination method based on multiple star-sensor measure information fusion | |
CN104677356A (en) | Paddling speed calculation method based on angle increment output and specific force output | |
CN110455288A (en) | A kind of posture renewal method based on angular speed high-order moment | |
CN103411614A (en) | Iteration SKF (Schmidt Kalman Filter) method of multi-source information integrated navigation of Mars power descent stage |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C14 | Grant of patent or utility model | ||
GR01 | Patent grant | ||
CF01 | Termination of patent right due to non-payment of annual fee | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20131030 Termination date: 20180714 |