CN104634348B - Attitude angle computational methods in integrated navigation - Google Patents
Attitude angle computational methods in integrated navigation Download PDFInfo
- Publication number
- CN104634348B CN104634348B CN201510109100.2A CN201510109100A CN104634348B CN 104634348 B CN104634348 B CN 104634348B CN 201510109100 A CN201510109100 A CN 201510109100A CN 104634348 B CN104634348 B CN 104634348B
- Authority
- CN
- China
- Prior art keywords
- angle
- optimization
- strap
- servo
- actuated
- 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.)
- Active
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
The invention provides the attitude angle computational methods in integrated navigation, specific design navigation field.Attitude angle computational methods in the integrated navigation that the present invention is provided, by the way of being corrected using servo-actuated angle, it is used as input by elder generation using the SINS error angle and angular speed obtained in advance, servo-actuated angle is used as output, according to the gain in servo-actuated angle and SINS loop, the incidence relation of damping ratio and natural angular frequency sets up closed feedback loop, and then the servo-actuated angle with convergence property is obtained as the servo-actuated angle of optimization, and further original strap-down matrix is corrected according to the servo-actuated angle of optimization, to determine that first optimizes strap-down matrix, it is final to optimize strap-down matrix acquisition attitude angle according to first, so as to complete the calculating of more accurate attitude angle, solve deficiency of the prior art.
Description
Technical field
The present invention relates to navigation field, in particular to the attitude angle computational methods in integrated navigation.
Background technology
March operation is carried out by map, or tourism exploration has been conventional mode.Use the purpose of map
It is exactly that target-bound position is advanced, the navigation mode that navigation is also most original is carried out using map.Wherein, the base navigated
Plinth is to obtain two key elements, and first is the position of target, and second is obtained from the position of body.With mechanics of communication and information processing
The progress of technology, occurs in that two kinds of new navigation modes, inertial navigation technology and space star airmanship, and both modes are also all
It is to focus on accurately to obtain the position of itself.
Inertial navigation technology is a sophisticated technology-oriented discipline in modern science and technology, in Aeronautics and Astronautics, the military affairs of navigation
Field and many civil areas are obtained for extensive use.
SINS in inertial navigation is simple in construction, comparatively, and small volume, lightweight, cost are low, with very
High reliability.It is a kind of autonomic navigation system truly, and output information is continuous, independently provide acceleration,
The information such as speed, position and angular speed, attitude angle, fully relies on equipment of itself and autonomously carries out navigation task, the extraneous hair of discord
Raw any photoelectricity contact.Therefore, it possesses good concealment, limited not by external interference, not by the gentle time condition in time region
Plurality of advantages.But the navigation error of SINS increases with time integral.
Specifically, navigated according to inertia, it mainly measures carrier sheet using inertance element (accelerometer)
The acceleration of body, obtains speed and position, so as to reach the purpose to carrier navigator fix by integration and computing.Composition is used
Property navigation system equipment be all arranged in carrier, independent of external information during work, also not outwardly emittance, is difficult
It is interfered, is a kind of autonomic navigation system.Inertial navigation system is generally shown by inertial measuring unit, computer, control
Device etc. is constituted.Inertial measuring unit includes accelerometer and gyroscope, also known as Inertial Measurement Unit.3 single-degree-of-freedom gyroscopies
For measuring 3 rotational motions of carrier；3 accelerometers are used for measuring the acceleration of 3 translational motions of carrier.Meter
Calculation machine calculates the speed and position data of carrier according to the acceleration signal measured.Control display shows various navigation ginsengs
Number.According to the mounting means of gyroscope and accelerometer on carrier, be divided into gimbaled inertial navigation system (gyroscope and plus
Speedometer be arranged on inertial platform stage body on) and strap-down inertial navigation system (gyroscope and accelerometer are directly installed on
On carrier).
Another, the appearance of global position system brings revolutionary development for space navigation location technology.Satellite reception
Machine has higher navigation and positioning accuracy and does not increase with the time and reduce, its be disadvantageous in that Refresh Data rate it is low, easily by
Easily losing star under interference, dynamic environment or when being blocked causes positioning to be interrupted.Space is led by taking the GPS satellite navigation in the U.S. as an example
Positioning in boat is described, and the GPS satellite navigation system in the U.S. has 24 gps satellites, and this 24 satellites are liftoff
On the high-altitude that 20,000 zero 2 hundred kilometers of face, every satellite was orbited round the earth with the cycle of 12 hours so that at any time,
Any point on ground can observe the satellite of more than 4 simultaneously.
Because the position of satellite is accurately understood, in GPS observations, we can obtain satellite to the distance of receiver, utilize
Range formula in three-dimensional coordinate, utilizes 3 satellites, it is possible to constitute 3 equations, solve observation station position (X, Y and
Z).In view of the error between the clock and receiver clock of satellite, actually there are 4 unknown numbers, X, Y, Z and clock correction, thus
Need to introduce the 4th satellite, form 4 equations and solved, so as to obtain the longitude and latitude and elevation of observation station.
Receiver can often pin the satellite of more than 4, at this moment, and receiver can be divided into some by the constellation of satellite
Group, every group 4, then pick out error minimum by algorithm one group is used as positioning, so as to improve precision.
Because there is error in satellite transit track, satellite clock, atmosphere convection layer, influence of the ionosphere to signal, and
Artificial SA protective policies so that civilian GPS positioning precision is relatively low.To improve positioning precision, generally using differential GPS
(DGPS) technology, sets up base station (track station) and carries out GPS observations, using known base station accurate coordinates, enter with observation
Row compares, so as to draw an amendment number, and externally issues.Receiver is received after the amendment number, is compared with the observation of itself
Compared with the most of error of cancellation obtains a more accurately position.Experiment shows, using differential GPS, and positioning precision can be improved
An order of magnitude.
There is certain weak point because first two navigation mode has when self-position is confirmed, therefore generate
New navigation mode, that is, integrated navigation.Integrated Navigation Algorithm is by the signal of SINS and DVB with card
Kalman Filtering mode optimum fusion, learns from other's strong points to offset one's weaknesses, and makes up satellite receiver signal and there is dead band, ins error and increases with the time
Shortcoming so that the system after combination both can guarantee that precision, and reliability and antijamming capability can be improved again.In this fusion method
In, using speed, position as observed quantity, according to systematic error state equation, utilize Kalman filtering recurrence estimation error amount.
In traditional algorithm, when motion carrier (itself) angle of pitch of use Integrated Navigation Algorithm is very big, inertial navigation system
The mathematical platform resolved unite close to one degree of freedom is lost, causes the meter of course angle, the angle of pitch and this navigation angle of roll angle
Calculate error very big, and it is very big to directly result in velocity error.And the Kalman filter demands state equation mould in Integrated Navigation Algorithm
Type is accurate, and the very big caused model error of course angle, roll angle and velocity error causes the recursive state estimation of Kalman filtering
Deviation between time of day is increasing, so as to cause combined navigation system performance drastically to deteriorate.
To sum up, when the motion carrier angle of pitch is very big, posture angular accuracy can be caused to decline, and then cause existing lead
The degree of accuracy of boat mode declines.
The content of the invention
In view of this, the purpose of the embodiment of the present invention is to provide the attitude angle computational methods in integrated navigation, to improve
The degree of accuracy of attitude angle, and then improve the degree of accuracy of integrated navigation.
In a first aspect, the embodiments of the invention provide the attitude angle computational methods in integrated navigation, including：
Using the SINS error angle and angular speed that obtain in advance as input, be servo-actuated angle as output, according to
Dynamic angle and the incidence relation of the gain in SINS loop, damping ratio and natural angular frequency set up closed feedback loop, institute
State the difference that error angle is the angle of pitch and default critical angle；
According to the transmission function formula of the closed feedback loop, the optimization calculated under predetermined state is servo-actuated angle；
The original strap-down matrix obtained in advance is corrected using the servo-actuated angle of the optimization, to determine the first optimization strapdown
Matrix；
Attitude angle is obtained according to the first optimization strap-down matrix.
With reference in a first aspect, the embodiments of the invention provide the possible embodiment of the first of first aspect, wherein, also
Including：
According to the error compensation angle of the course angle obtained in advance and the error compensation angle of roll angle, to current inertial navigation system
The strap-down matrix of system is modified, to determine original strap-down matrix.
With reference in a first aspect, the embodiments of the invention provide the possible embodiment of second of first aspect, wherein, institute
The transmission function formula according to the closed feedback loop is stated, calculating the servo-actuated angle of the optimization under predetermined state includes：
According to the sampling period obtained in advance, the transmission function formula to the closed feedback loop carries out sliding-model control,
To determine the recurrence formula at the servo-actuated angle of optimization；
The servo-actuated angle of the optimization is calculated according to the recurrence formula at the servo-actuated angle of the optimization.
With reference in a first aspect, the embodiments of the invention provide the possible embodiment of the third of first aspect, wherein, it is described
Recurrence formula is Δ θ (k)=Δ θ1(k)+Δθ2(k), Δ θ1(k)=a1Δθ1(k-1)+a2Δθ1(k-2)+b0θ(k)+b1θ(k-1)+
b2θ (k-2), Δ θ2(k)=c1Δθ2(k-1)+c2Δθ2(k-2)+c3Δθ2(k-3)+d0ωx(k)+d1ωx(k-1)+d2ωx(k-2)+
d3ωx(k-3), wherein, K is inertial navigation
The gain of system circuit, ζ is the damping ratio in SINS loop, ωnFor the natural angular frequency in SINS loop.
With reference in a first aspect, the embodiments of the invention provide the possible embodiment of the 4th of first aspect kind, wherein, also
Including：
Calculated according to equation below and refer to the angle of pitch, θ=α θs+(1-α)θa, wherein, θsTo be calculated by Quaternion Method
To first refer to the angle of pitch, θaThe angle of pitch is referred to calculate obtain second according to acceleration decomposition method, α is default power
Value；
If the numerical value of the reference angle of pitch is more than default angle of pitch reference value, perform described in step to obtain in advance
The SINS error angle and angular speed taken is servo-actuated angle as output, according to servo-actuated angle and inertial navigation system as input
The incidence relation of the gain in loop, damping ratio and natural angular frequency of uniting sets up closed feedback loop, and the angle of pitch reference value is
60-70 degree.
With reference in a first aspect, the embodiments of the invention provide the possible embodiment of the 5th of first aspect kind, wherein, also
Including：
According to default course angle initial value and roll angle initial value, scanned for using genetic algorithm；
It is less than default course difference when searching difference of the course angle changing value with course angle actual value, and roll angle becomes
When the difference of change value and roll angle actual value is less than default rolling difference, it is determined that course angle changing value and course angle now is real
The difference of actual value is the error compensation angle of course angle, and the difference of determination roll angle changing value and roll angle actual value is roll angle
Error compensation angle.
With reference in a first aspect, the embodiments of the invention provide the possible embodiment of the 6th of first aspect kind, wherein, institute
State includes according to the first optimization strap-down matrix acquisition attitude angle：
The mathematical platform misalignment of SINS is obtained by the way of Kalman filtering；
The described first optimization strap-down matrix is corrected according to the mathematical platform misalignment and the servo-actuated angle of optimization, with true
Fixed second optimization strap-down matrix；
Attitude angle is exported from the described second optimization strap-down matrix.
With reference in a first aspect, the embodiments of the invention provide the possible embodiment of the 7th of first aspect kind, wherein, institute
State and the mathematical platform misalignment of SINS is obtained by the way of Kalman filtering include：
Sliding-model control is carried out to the integrated navigation system state equation and observational equation obtained in advance, and passes through Kalman
Filtering carries out recurrence estimation, to determine state optimization valuation equation；
The mathematical platform misalignment is determined according to the state optimization valuation equation.
With reference in a first aspect, the embodiments of the invention provide the possible embodiment of the 8th of first aspect kind, wherein, root
The described first optimization strap-down matrix is corrected according to the mathematical platform misalignment and the servo-actuated angle of optimization, to determine the second optimization
Strap-down matrix includes：
The first optimization strap-down matrix is corrected according to the mathematical platform misalignment, to determine incomplete optimization strapdown
Matrix；
The incomplete optimization strap-down matrix is corrected again according to the optimization servo-actuated angle, to determine described second
Optimize strap-down matrix.
With reference in a first aspect, the embodiments of the invention provide the possible embodiment of the 9th of first aspect kind, wherein, also wrap
Measuring and calculating speed and the measuring and calculating position that SINS is calculated according to equation below are included,
WhereinIt is measuring and calculating position vector,It is measuring and calculating velocity, fbBe than force signal,For strap-down matrixInverse matrix, gn
=[0,0, g]TFor terrestrial gravitation vector,For projections of the rotational-angular velocity of the earth Ω in navigational coordinate system,For navigation
Angular speed of the coordinate system with respect to terrestrial coordinate system；
The detection speed obtained according to the measuring and calculating velocity, measuring and calculating position vector and beforehand through DVB is sweared
Amount and test position vector distinguish calculating speed error and site error；
Mathematical platform misalignment, gyro using the velocity error, site error as observational variable and to choose in advance
Instrument zero partially, Gyroscope Random Drift rate and accelerometer bias as combined system error state vector, respectively obtain system shape
State equation and observational equation.
Attitude angle computational methods in integrated navigation provided in an embodiment of the present invention, by the way of being corrected using servo-actuated angle, with
It is of the prior art when the angle of pitch is excessive, cause the attitude angle of integrated navigation system to be difficult to accurate acquisition, and then lead
Cause that navigation error is excessive compares, its by elder generation using the SINS error angle and angular speed that obtain in advance as input, with
Dynamic angle is as output, according to the incidence relation of the gain in servo-actuated angle and SINS loop, damping ratio and natural angular frequency
Closed feedback loop is set up, and then obtains the servo-actuated angle with convergence property as the servo-actuated angle of optimization, and further according to optimization
Servo-actuated angle is corrected to original strap-down matrix, to determine the first optimization strap-down matrix, final to optimize strapdown square according to first
Battle array obtains attitude angle, so as to complete the calculating of more accurate attitude angle, solves deficiency of the prior art.
To enable the above objects, features and advantages of the present invention to become apparent, preferred embodiment cited below particularly, and coordinate
Appended accompanying drawing, is described in detail below.
Brief description of the drawings
In order to illustrate the technical solution of the embodiments of the present invention more clearly, below will be attached to what is used required in embodiment
Figure is briefly described, it will be appreciated that the following drawings illustrate only certain embodiments of the present invention, therefore is not construed as pair
The restriction of scope, for those of ordinary skill in the art, on the premise of not paying creative work, can also be according to this
A little accompanying drawings obtain other related accompanying drawings.
Fig. 1 shows the basic flow sheet of the attitude angle computational methods in the integrated navigation that the embodiment of the present invention is provided；
Fig. 2 shows the attitude angle calculating side in the integrated navigation that the embodiment of the present invention is provided
The servo-actuated backfeed loop schematic diagram of method；
Fig. 3 shows the attitude angle calculating side in the integrated navigation that the embodiment of the present invention is provided
The use genetic algorithm of method determines that the error compensation angle of course angle and the error of roll angle are mended
Repay the schematic flow sheet at angle；
Fig. 4 shows the attitude angle calculating side in the integrated navigation that the embodiment of the present invention is provided
Method by the strap-down matrix after the first suboptimization on the basis of, further obtain more accurate appearance
The refined flow chart at state angle；
Fig. 5 shows the attitude angle calculating side in the integrated navigation that the embodiment of the present invention is provided
The overall logic feedback control schematic diagram of method.
Embodiment
Below in conjunction with accompanying drawing in the embodiment of the present invention, the technical scheme in the embodiment of the present invention is carried out clear, complete
Ground is described, it is clear that described embodiment is only a part of embodiment of the invention, rather than whole embodiments.Generally exist
The component of the embodiment of the present invention described and illustrated in accompanying drawing can be arranged and designed with a variety of configurations herein.Cause
This, the detailed description of the embodiments of the invention to providing in the accompanying drawings is not intended to limit claimed invention below
Scope, but it is merely representative of the selected embodiment of the present invention.Based on embodiments of the invention, those skilled in the art are not doing
The every other embodiment obtained on the premise of going out creative work, belongs to the scope of protection of the invention.
The embodiments of the invention provide the attitude angle computational methods in integrated navigation, as shown in figure 1, comprising the following steps：
S101, the SINS error angle and angular speed to obtain in advance is servo-actuated angle and is used as output, root as input
Closed loop feedback is set up according to the incidence relation of the gain in servo-actuated angle and SINS loop, damping ratio and natural angular frequency to return
Road, error angle is the difference of the angle of pitch and default critical angle；
S102, according to the transmission function formula of closed feedback loop, the optimization calculated under predetermined state is servo-actuated angle；
S103, is corrected using the servo-actuated angle of optimization to the original strap-down matrix obtained in advance, to determine that the first optimization is prompt
Join matrix；
S104, attitude angle is obtained according to the first optimization strap-down matrix.
In step S101, in order that the servo-actuated angle obtained has good convergence property, it is necessary to inside by design system
Determine using servo-actuated angle as the closed feedback loop of output.Specifically, by servo-actuated backfeed loop (closed feedback loop)
Dynamic response characteristic is designed to and gain K, dampingratioζ, natural angular frequency ωnIt is relevant.Input and output meet following linear differential
Equation：
Wherein, θ-θcFor error angle, ωxFor angular speed, Δ θ be with
Dynamic angle, Δ θ1With Δ θ2Respectively it is servo-actuated two components at angle.As shown in Fig. 2 showing the theory structure of the closed feedback loop
Figure, can clearly see with θ-θ very much from figurecAnd ωxAs input, using servo-actuated angle Δ θ as output, and closed by classics
Ring negative-feedback regu- lation mode is come the negative-feedback regu- lation system set up.
Step S102, system model as shown in Figure 2 only has three adjustable parameters K, ζ, ωn, simple in construction, debugging side
Just, it can be easy to make servo-actuated backfeed loop reach desired dynamic response characteristic.The third order transfer function matrix of input and output
Meet following formula：
Wherein, θ-θcFor error angle, ωxFor angular speed, Δ θ is servo-actuated angle.
Wherein according to the transmission function formula of closed feedback loop, calculating the servo-actuated angle of the optimization under predetermined state includes：
According to the sampling period obtained in advance, the transmission function formula to closed feedback loop carries out sliding-model control, with true
The recurrence formula at the fixed servo-actuated angle of optimization；Angle is servo-actuated according to the recurrence formula calculation optimization at the servo-actuated angle of optimization.Namely with combination
When navigational computer carries out servo-actuated backfeed loop calculating, if data sampling period is Ts, use bilinear transformation formulaSliding-model control is carried out, the recursive calculative formula at servo-actuated angle is then obtained：
Δ θ (k)=Δ θ1(k)+Δθ2(k)；
Δθ1(k)=a1Δθ1(k-1)+a2Δθ1(k-2)+b0θ(k)+b1θ(k-1)+b2θ(k-2)；
Δθ2(k)=c1Δθ2(k-1)+c2Δθ2(k-2)+c3Δθ2(k-3)+d0ωx(k)+d1ωx(k-1)+d2ωx(k-
2)+d3ωx(k-3)；
In formula In formula, K is strapdown
The gain in inertial navigation system loop, ζ is the damping ratio in SINS loop, ωnFor the nature angle in SINS loop
Frequency.Thus, it is possible to more quickly obtain the servo-actuated angle under convergence state by the recurrence formula for being servo-actuated angle, that is, optimize servo-actuated
Angle.
Step S103 according to the servo-actuated angle of optimization to the strap-down matrix obtained in advance, it is necessary to be corrected, to determine more to be defined
True strap-down matrix.Specifically, before step S103 is performed, step S105 can also be included, according to the course obtained in advance
The error compensation angle at angle and the error compensation angle of roll angle, are modified to the strap-down matrix of current SINS, with true
Fixed original strap-down matrix.
The strap-down matrix of SINS be on course angle, the angle of pitch, roll angle trigonometric function matrix.If current
Strap-down matrix isMotion carrier course angle ψ, roll angle γ error compensation angle are respectively Δ ψ0、Δγ0(above parameterΔ
ψ0With Δ γ0Can be by what existing means were obtained, can directly use), by according to the coordinate conversion relation under low-angle,
Utilize the error compensation angle Δ ψ of course angle0With the error compensation angle Δ γ of roll angle0To the strap-down matrix of current SINSIt is modified, to obtain original strap-down matrix.Specific correction formula is as follows：
In formula, Δ ψ0For the error compensation angle of course angle, Δ γ0For the error compensation angle of roll angle,For current SINS
Strap-down matrix,For original strap-down matrix.
Further, using the coordinate conversion relation under low-angle, θ pairs of angle Δ is servo-actuated using the step S102 optimizations obtained
Original strap-down matrix is modified again, to determine the first optimization strap-down matrix.The specific formula being modified using servo-actuated angle
It is as follows：In formula, Δ θ is the servo-actuated angle of optimization,For original strap-down matrix,
For the first optimization strap-down matrix.
First optimization strap-down matrixNew strap-down matrix after being updated as SINS, overcomes large pitching angle
Influence, speed, position and the solving of attitude for carrying out SINS with this are accurate, that is, can perform step
Rapid S104, attitude angle is obtained according to the first optimization strap-down matrix, so that the acquisition at exact posture angle is completed, wherein, it is acquired
Attitude angle refer to one or more in course angle, the angle of pitch, roll angle.
Carried it should be noted that the attitude angle computational methods in integrated navigation provided by the present invention are preferably applied in motion
In the case that the angle of pitch of body is very big, be of course also apply to the angle of pitch it is relatively low in the case of, it is less in the motion carrier angle of pitch
In the case of, remain able to play the function of effectively correcting attitude angle.
, can be by prejudging the current pitching of system before step S101 is performed in order to reduce the amount of calculation of system
The mode at angle determines whether to perform step S101- step S103, in this way, when the angle of pitch reaches necessarily required, can be with
Select to perform step S101 and follow-up step as the case may be.
Method namely provided by the present invention also comprises the following steps, is calculated according to equation below and refers to the angle of pitch, θ=
αθs+(1-α)θa, wherein, θsThe angle of pitch, θ are referred to calculate obtain first by Quaternion MethodaFor according to acceleration decomposition side
Method calculates second obtained and refers to the angle of pitch, and α is default weights；
If be more than default angle of pitch reference value with reference to the numerical value of the angle of pitch, strapdown of the step to obtain in advance is performed
Inertial navigation system error angle and angular speed are servo-actuated angle as output, according to servo-actuated angle and SINS loop as input
The incidence relation of gain, damping ratio and natural angular frequency sets up closed feedback loop, and angle of pitch reference value is 60-70 degree.
Wherein, the current acceleration a (after amendment) of motion carrier can pass through accelerometer bias ▽=[▽x,▽y,
▽z]TActual measured value is modified, to obtain more accurate motion carrier acceleration.In three-dimensional system of coordinate, accelerate
Degree, angular speed isovector can be decomposed into three components along three-dimensional system of coordinate reference axis, that is, reality measures what is obtained
am=[amx,amy,amz]T, and then revised accelerationThat is a=[ax,ay,az]T, after amendment
Acceleration carry out vector calculus, be with the numerical value (absolute acceleration) for obtaining revised acceleration
Obtaining revised acceleration a=[ax,ay,az]TAfterwards, the second reference is calculated according to acceleration decomposition method to bow
The elevation angleWherein, g is acceleration of gravity, ayFor the component of acceleration vertically.
It is in the numerical value (absolute acceleration) for obtaining revised accelerationAfterwards, according to inertial navigation
Resolving conventional Quaternion Method in posture angle can calculate first and refer to the angle of pitch, θs=arcsin [2 (q2q3-q0q1)], its
Middle q0、q1、q2And q3Element respectively in quaternary number.
In order to make the obtained reference angle of pitch more accurate, pitching angle theta can be referred to by respectively to firstsWith
Two refer to pitching angle thetaaDifferent weights are assigned to adjust the numerical value of final reference pitching angle theta.It is specific such as to use formula θ=α θs+
(1-α)θaPitching angle theta is referred to calculate, wherein, θsThe angle of pitch, θ are referred to for firstaThe angle of pitch is referred to for second.Work as motion carrier
When current absolute acceleration a is smaller, weighted factor to take it is larger, thus, as | a-g |<α=0.8 is can use when 0.5, otherwise α
=0.2.Certainly, by referring to pitching angle theta to firstsPitching angle theta is referred to secondaThe mode being weighted, bows to determine to refer to
Can first refer to pitching angle theta during elevation angle thetasWeights and second refer to pitching angle thetaaWeights do not produce the side of association
Formula, such as θ=α1θs+α2θa。
When the motion carrier angle of pitch is more than 60 °, integrated navigation penalty is more serious, can set angle of pitch critical value θc
The value in the range of (60 °, 70 °), is used as Rule of judgment.Revised pitching angle theta is more than the critical value θ of settingcThat is θ>θcWhen,
Then start servo-actuated backfeed loop to resolve, that is, can now perform step S101.
Step S105, according to the error compensation angle of the course angle obtained in advance and the error compensation angle of roll angle, to current
The strap-down matrix of SINS is modified, to determine original strap-down matrix.It may include steps of：According to default
Course angle initial value and roll angle initial value, are scanned for using genetic algorithm；
It is less than default course difference when searching difference of the course angle changing value with course angle actual value, and roll angle becomes
When the difference of change value and roll angle actual value is less than default rolling difference, it is determined that course angle changing value and course angle now is real
The difference of actual value is the error compensation angle of course angle, and the difference of determination roll angle changing value and roll angle actual value is roll angle
Error compensation angle.
Namely need to utilize and be servo-actuated during the original strap-down matrix of angle amendment, motion carrier course angle, the mistake of roll angle
Difference compensation angle Δ ψ0、Δγ0Obtained by the Genetic algorithm searching of off-line calculation.The stop condition of Genetic algorithm searching is：Offline meter
Motion carrier course angle ψ, the roll angle γ of calculation and corresponding true value (known roll angle actual value and course angle actual value) difference
In some small angle range, i.e. Δ ψ0=| ψ-ψVery|<β and Δ γ0=| γ-γVery|<β, β value are generally less than 5 degree, then
It is determined that the difference of course angle changing value now and course angle actual value is the error compensation angle Δ ψ of course angle0, and determine to roll
The difference of angle changing value and roll angle actual value is the error compensation angle Δ γ of roll angle0.It should be noted that utilizing heredity calculation
Need first to determine an initial value when method is scanned for, system is not stopped when scanning for according to this initial value
Change numerical value (numerical value for referring to roll angle and course angle is ceaselessly changing, that is, every time change generate one group of roll angle
Changing value and course angle changing value), that is, it is intended to judge that course angle changing value is actual with course angle respectively after each change
Whether the difference of value is less than default course difference, and roll angle changing value and roll angle actual value difference whether be less than it is default
Rolling difference, when the two judgements are set up, it is determined that course angle changing value now and course angle actual value
Difference is the error compensation angle Δ ψ of course angle0, and the difference of determination roll angle changing value and roll angle actual value is roll angle
Error compensation angle Δ γ0。
Specifically, the angular speed gathered using overall process, acceleration information and known motion carrier course angle, roll angle
True value, the Genetic algorithm searching of off-line calculation is carried out to error compensation angle.
Motion carrier course angle, the coding criterion at the error compensation angle of roll angle：Using 8 binary codings, coding
Scope [0x00H, 0xFFH].0x00H correspondences -5 are spent, 0 degree of 0x80H correspondences, 5 degree of 0xFFH correspondences.
Motion carrier course angle, the convergence criterion of the error compensation angle search of roll angle：|ψ-ψVery|<β、γ-γVery|<β。
When error compensation angle carries out Genetic algorithm searching, to binary code using selection, intersection, three kinds of operators of variation.Search
Rope process is shown in Fig. 3.
Search obtains Δ ψ0Encoding value be NΔψ0, thenAlso course has just been calculated
The error compensation angle at angle.
Search obtains Δ γ0Encoding value be NΔγ0, thenAlso rolling has just been calculated
The error compensation angle at angle.
Specifically, as shown in figure 4, step S104, obtains attitude angle according to the first optimization strap-down matrix and comprises the following steps：
S1041, obtains the mathematical platform misalignment of SINS by the way of Kalman filtering；
S1042, is corrected according to mathematical platform misalignment and the servo-actuated angle of optimization to the first optimization strap-down matrix, to determine
Second optimization strap-down matrix；
S1043, attitude angle is exported from the second optimization strap-down matrix.
In step S1041, the mathematical platform misalignment of SINS is obtained by way of Kalman filtering, is had
Body can be divided into two steps, that is, step S1041, and the mathematics of SINS is obtained by the way of Kalman filtering
The misaligned angle of the platform includes：
Sliding-model control is carried out to the integrated navigation system state equation and observational equation obtained in advance, and passes through Kalman
Filtering carries out recurrence estimation, to determine state optimization valuation equation；
Mathematical platform misalignment is determined according to state optimization valuation equation.
Its detailed process learns the misaligned angle of the platform (φ for first accessE、φN、φU), velocity error (δ VE、δVN、δVU), position
Error (δ L, δ λ, δ h), gyroscope zero (ε partiallybx、εby、εbz), Gyroscope Random Drift rate (εrx、εry、εrz), accelerometer bias
(▽x、▽y、▽z) as integrated navigation system error state vector, position, the difference of speed can be as observational variable
System state equation and observational equation： Wherein, F (t) is
The state-transition matrix of SINS, G (t) is the noise inputs matrix of SINS, and w (t) is system noise, δ R
=[δ L, δ λ, δ h]T(site error), δ V=[δ VE,δVNδVU]T(velocity error), VkIt is to measure noise.
Then, it will be obtained respectively after state equation and observational equation discretization：X (k+1)=φ (k+1, k) X (k)+Γ (k+1,
k)W(k)；Z (k)=H (k) X (k)+V (k).So as to obtain state one-step prediction equation
With state optimization valuation equation：
Wherein, filtering gain equation is：
K (k+1)=P (k+1/k) HT(k+1)[H(k+1)P(k+1/k)HT(k+1)+Rk+1]-1；
One-step prediction mean square error equation is：
P (k+1/k)=φ (k+1, k) P (k/k) φT(k+1,k)+Γ(k+1,k)QkΓT(k+1,k)；
Estimate that mean square error equation is：
P (k+1/k+1)=[I-K (k+1) H (k+1)] P (k+1/k).Kalman is carried out using the system model after discretization
Recurrence estimation is filtered, the optimal estimation value of error state vector is obtained, also just having obtained mathematical platform misalignment, (mathematical platform is lost
The optimal estimation value at quasi- angle).Wherein, step S1042, according to the mathematical platform misalignment and the servo-actuated angle of optimization to described first
Optimization strap-down matrix is corrected, and to determine the second optimization strap-down matrix, can be divided into following two steps：
The first optimization strap-down matrix is corrected according to mathematical platform misalignment, to determine incomplete optimization strapdown square
Battle array；
Incomplete optimization strap-down matrix is corrected again according to optimization servo-actuated angle, to determine the second optimization strapdown square
Battle array.
, can be with when being corrected using mathematical platform misalignment and the servo-actuated angle of optimization to the first optimization strap-down matrix
First corrected using servo-actuated angle, it is rear to be corrected using mathematical platform misalignment, first it can also be carried out using mathematical platform misalignment
Correction, it is rear to be corrected using servo-actuated angle；But it is preferred that scheme to be first corrected using mathematical platform misalignment, it is rear using with
Dynamic angle is corrected, and the specific formula being corrected using servo-actuated angle is as follows：Its
InFor tried to achieve before first optimization strap-down matrix,For the second required optimization strap-down matrix, Δ θ is servo-actuated angle,
Try to achieve in step s 102.
It should be noted that (the ε partially of gyroscope zero before Kalman filtering recurrence calculationbx、εby、εbz), Gyroscope Random Drift
Rate (εrx、εry、εrz), accelerometer bias (▽x、▽y、▽z) be obtained ahead of time, in addition it is also necessary to calculating speed error (δ VE、δVN、δ
VU), site error (δ L, δ λ, δ h).Wherein, velocity error is calculated according to fundamental equation by SINS and obtained
The speed that speed and satellite are obtained makes the difference what is obtained, and site error is to be calculated to obtain according to fundamental equation by SINS
Position and the position that obtains of satellite make the difference what is obtained, the algorithm of obtained speed and position is calculated such as by SINS
Under：Wherein,It is position vector,It is velocity, fbBe than force signal,For strap-down matrixInverse matrix, gn=[0,0, g]TFor terrestrial gravitation vector,It is that rotational-angular velocity of the earth Ω is navigating
Projection in coordinate system,Angular speed for navigational coordinate system with respect to terrestrial coordinate system.The observed result for recycling satellite to obtain
(speed and position) can just try to achieve site error and velocity error, and further learn the misaligned angle of the platform (φ to fetchE、φN、
φU), gyroscope zero partially (εbx、εby、εbz), Gyroscope Random Drift rate (εrx、εry、εrz), accelerometer bias (▽x、▽y、
▽z) it is used as integrated navigation system error state vector, velocity error (δ VE、δVN、δVU), site error (δ L, δ λ, δ h), conduct
Observational variable, can obtain system state equation and observational equation.
On the whole attitude angle computational methods in integrated navigation provided by the present invention, as shown in figure 5, inertial navigation is surveyed
Measure unit original angular speed is provided and acceleration analysis information, DVB provide reference speed and positional information (
Used when calculating speed error and site error), Integrated Navigation Algorithm mainly includes SINS resolving, Kalman filtering
Optimal estimation, servo-actuated backfeed loop three parts.These three parts of algorithm interact, influenced each other.So as to by servo-actuated
The servo-actuated angle under convergence state is obtained in backfeed loop, and further strap-down matrix is corrected and used according to servo-actuated angle
Mathematical platform misalignment is corrected to strap-down matrix, it is achieved thereby that extracting accurate posture from the strap-down matrix after optimization
Angle.
The foregoing is only a specific embodiment of the invention, but protection scope of the present invention is not limited thereto, any
Those familiar with the art the invention discloses technical scope in, change or replacement can be readily occurred in, should all be contained
Cover within protection scope of the present invention.Therefore, protection scope of the present invention described should be defined by scope of the claims.
Claims (10)
1. the attitude angle computational methods in integrated navigation, it is characterised in that including：
SINS error angle and angular speed to obtain in advance is servo-actuated angle as output, according to servo-actuated angle as input
Closed feedback loop, the mistake are set up with the incidence relation of the gain in SINS loop, damping ratio and natural angular frequency
Declinate is the difference of the angle of pitch and default critical angle；
According to the transmission function formula of the closed feedback loop, the optimization calculated under predetermined state is servo-actuated angle；
The original strap-down matrix obtained in advance is corrected using the servo-actuated angle of the optimization, to determine the first optimization strapdown square
Battle array；
Attitude angle is obtained according to the first optimization strap-down matrix；
The first optimization strap-down matrix is determined according to equation below,In formula, Δ θ is
The servo-actuated angle of optimization,For original strap-down matrix,For the first optimization strap-down matrix.
2. the attitude angle computational methods in integrated navigation according to claim 1, it is characterised in that also include：
According to the error compensation angle of the course angle obtained in advance and the error compensation angle of roll angle, to current SINS
Strap-down matrix is modified, to determine original strap-down matrix.
3. the attitude angle computational methods in integrated navigation according to claim 1, it is characterised in that closed described in the basis
The transmission function formula of ring backfeed loop, calculating the servo-actuated angle of the optimization under predetermined state includes：
According to the sampling period obtained in advance, the transmission function formula to the closed feedback loop carries out sliding-model control, with true
The recurrence formula at the fixed servo-actuated angle of optimization；
The servo-actuated angle of the optimization is calculated according to the recurrence formula at the servo-actuated angle of the optimization.
4. the attitude angle computational methods in integrated navigation according to claim 3, it is characterised in that the recurrence formula is
Δ θ (k)=Δ θ1(k)+Δθ2(k), Δ θ1(k)=a1Δθ1(k-1)+a2Δθ1(k-2)+b0θ(k)+b1θ(k-1)+b2θ(k-
2), Δ θ2(k)=c1Δθ2(k-1)+c2Δθ2(k-2)+c3Δθ2(k-3)+d0ωx(k)+d1ωx(k-1)+d2ωx(k-2)+d3
ωx(k-3), wherein, K is inertial navigation system
The gain in system loop, ζ is the damping ratio in SINS loop, ωnFor the natural angular frequency in SINS loop.
5. the attitude angle computational methods in integrated navigation according to claim 1, it is characterised in that also include：
Calculated according to equation below and refer to the angle of pitch, θ=α θs+(1-α)θa, wherein, θsCalculated and obtained by Quaternion Method
First refers to the angle of pitch, θaThe angle of pitch is referred to calculate obtain second according to acceleration decomposition method, α is default weights；
If the numerical value of the reference angle of pitch is more than default angle of pitch reference value, perform described in step to obtain in advance
SINS error angle and angular speed are servo-actuated angle as output, returned according to servo-actuated angle and SINS as input
The incidence relation of the gain on road, damping ratio and natural angular frequency sets up closed feedback loop, and the angle of pitch reference value is 60-70
Degree.
6. the attitude angle computational methods in integrated navigation according to claim 2, it is characterised in that also include：
According to default course angle initial value and roll angle initial value, scanned for using genetic algorithm；
It is less than default course difference, and roll angle changing value when searching difference of the course angle changing value with course angle actual value
When being less than default rolling difference with the difference of roll angle actual value, it is determined that course angle changing value now and course angle actual value
Difference be the error compensation angle of course angle, and determine that the difference of roll angle changing value and roll angle actual value is the mistake of roll angle
Difference compensation angle.
7. the attitude angle computational methods in integrated navigation according to claim any one of 1-6, it is characterised in that described
Obtaining attitude angle according to the first optimization strap-down matrix includes：
The mathematical platform misalignment of SINS is obtained by the way of Kalman filtering；
The described first optimization strap-down matrix is corrected according to the mathematical platform misalignment and the servo-actuated angle of optimization, to determine the
Two optimization strap-down matrix；
Attitude angle is exported from the described second optimization strap-down matrix；
The second optimization strap-down matrix is determined according to equation below,
WhereinOptimize strap-down matrix for first,For the second optimization strapdown square
Battle array, Δ θ is the servo-actuated angle of optimization.
8. the attitude angle computational methods in integrated navigation according to claim 7, it is characterised in that the use Kalman
The mathematical platform misalignment that the mode of filtering obtains SINS includes：
Sliding-model control is carried out to the integrated navigation system state equation and observational equation obtained in advance, and passes through Kalman filtering
Recurrence estimation is carried out, to determine state optimization valuation equation；
The mathematical platform misalignment is determined according to the state optimization valuation equation.
9. the attitude angle computational methods in integrated navigation according to claim 8, it is characterised in that flat according to the mathematics
Platform misalignment and the servo-actuated angle of optimization are corrected to the described first optimization strap-down matrix, to determine the second optimization strap-down matrix bag
Include：
The first optimization strap-down matrix is corrected according to the mathematical platform misalignment, to determine incomplete optimization strapdown square
Battle array；
The incomplete optimization strap-down matrix is corrected again according to the optimization servo-actuated angle, to determine second optimization
Strap-down matrix.
10. the attitude angle computational methods in integrated navigation according to claim 9, it is characterised in that also including according to such as
Lower formula calculates measuring and calculating speed and the measuring and calculating position of SINS,WhereinIt is measuring and calculating position vector,It is measuring and calculating velocity, fbBe than force signal,For strap-down matrixInverse matrix, gn=[0,
0,g]TFor terrestrial gravitation vector,For projections of the rotational-angular velocity of the earth Ω in navigational coordinate system,For navigational coordinate system
With respect to the angular speed of terrestrial coordinate system；
The detection speed vector obtained according to the measuring and calculating velocity, measuring and calculating position vector and beforehand through DVB
Test position vector distinguishes calculating speed error and site error；
Mathematical platform misalignment, gyroscope zero using the velocity error, site error as observational variable and to choose in advance
Partially, Gyroscope Random Drift rate and accelerometer bias respectively obtain system mode side as combined system error state vector
Journey and observational equation.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510109100.2A CN104634348B (en) | 2015-03-12 | 2015-03-12 | Attitude angle computational methods in integrated navigation |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510109100.2A CN104634348B (en) | 2015-03-12 | 2015-03-12 | Attitude angle computational methods in integrated navigation |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN104634348A CN104634348A (en) | 2015-05-20 |
| CN104634348B true CN104634348B (en) | 2017-09-15 |
Family
ID=53213359
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201510109100.2A Active CN104634348B (en) | 2015-03-12 | 2015-03-12 | Attitude angle computational methods in integrated navigation |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN104634348B (en) |
Families Citing this family (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109212962A (en) * | 2017-07-03 | 2019-01-15 | 中车株洲电力机车研究所有限公司 | Acceleration calculation method, closed-loop control system and acceleration estimation device |
| CN110873563B (en) * | 2018-08-30 | 2022-03-08 | 杭州海康机器人技术有限公司 | Cloud deck attitude estimation method and device |
| CN113203415B (en) * | 2021-04-12 | 2022-05-17 | 北京航空航天大学 | Atomic gyro navigation system and navigation resolving method thereof |
| CN117516465B (en) * | 2024-01-04 | 2024-03-19 | 北京神导科技股份有限公司 | Inertial navigation system attitude angle extraction method capable of avoiding jump |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101629826A (en) * | 2009-07-01 | 2010-01-20 | 哈尔滨工程大学 | Coarse alignment method for fiber optic gyro strapdown inertial navigation system based on single axis rotation |
| RU2380656C1 (en) * | 2008-12-24 | 2010-01-27 | Олег Степанович Салычев | Integrated strapdown inertial and satellite navigation system on coarse sensors |
| CN101893445A (en) * | 2010-07-09 | 2010-11-24 | 哈尔滨工程大学 | Rapid initial alignment method for low-accuracy strapdown inertial navigation system under swinging condition |
| CN102486377A (en) * | 2009-11-17 | 2012-06-06 | 哈尔滨工程大学 | Method for acquiring initial course attitude of fiber optic gyro strapdown inertial navigation system |
| CN102830414A (en) * | 2012-07-13 | 2012-12-19 | 北京理工大学 | Combined navigation method based on SINS/GPS (strapdown inertial navigation system/global position system) |
| CN103017765A (en) * | 2012-12-06 | 2013-04-03 | 北京遥测技术研究所 | Yaw angle correction method and yaw angle correction device applied to a micro-mechanical integrated navigation system |
-
2015
- 2015-03-12 CN CN201510109100.2A patent/CN104634348B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| RU2380656C1 (en) * | 2008-12-24 | 2010-01-27 | Олег Степанович Салычев | Integrated strapdown inertial and satellite navigation system on coarse sensors |
| CN101629826A (en) * | 2009-07-01 | 2010-01-20 | 哈尔滨工程大学 | Coarse alignment method for fiber optic gyro strapdown inertial navigation system based on single axis rotation |
| CN102486377A (en) * | 2009-11-17 | 2012-06-06 | 哈尔滨工程大学 | Method for acquiring initial course attitude of fiber optic gyro strapdown inertial navigation system |
| CN101893445A (en) * | 2010-07-09 | 2010-11-24 | 哈尔滨工程大学 | Rapid initial alignment method for low-accuracy strapdown inertial navigation system under swinging condition |
| CN102830414A (en) * | 2012-07-13 | 2012-12-19 | 北京理工大学 | Combined navigation method based on SINS/GPS (strapdown inertial navigation system/global position system) |
| CN103017765A (en) * | 2012-12-06 | 2013-04-03 | 北京遥测技术研究所 | Yaw angle correction method and yaw angle correction device applied to a micro-mechanical integrated navigation system |
Also Published As
| Publication number | Publication date |
|---|---|
| CN104634348A (en) | 2015-05-20 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN104655131B (en) | Inertial navigation Initial Alignment Method based on ISTSSRCKF | |
| CN108827310B (en) | Marine star sensor auxiliary gyroscope online calibration method | |
| CN106871928B (en) | Strap-down inertial navigation initial alignment method based on lie group filtering | |
| CN112505737B (en) | GNSS/INS integrated navigation method | |
| CN108731670A (en) | Inertia/visual odometry combined navigation locating method based on measurement model optimization | |
| CN105806363B (en) | The underwater large misalignment angle alignment methods of SINS/DVL based on SRQKF | |
| Stančić et al. | The integration of strap-down INS and GPS based on adaptive error damping | |
| CN105509739A (en) | Tightly coupled INS/UWB integrated navigation system and method adopting fixed-interval CRTS smoothing | |
| CN101246012B (en) | Combinated navigation method based on robust dissipation filtering | |
| CN102937450B (en) | A kind of relative attitude defining method based on gyro to measure information | |
| CN103994763A (en) | SINS (Ship's Inertial Navigation System)/CNS (Celestial Navigation System) deep integrated navigation system of mar rover, and realization method of system | |
| CN112146655B (en) | Elastic model design method for BeiDou/SINS tight integrated navigation system | |
| CN104697520B (en) | Integrated gyro free strap down inertial navigation system and gps system Combinated navigation method | |
| CN112697138B (en) | Bionic polarization synchronous positioning and composition method based on factor graph optimization | |
| CN104698485A (en) | BD, GPS and MEMS based integrated navigation system and method | |
| CN104634348B (en) | Attitude angle computational methods in integrated navigation | |
| Huang et al. | Attitude estimation fusing quasi-Newton and cubature Kalman filtering for inertial navigation system aided with magnetic sensors | |
| CN110058288A (en) | Unmanned plane INS/GNSS integrated navigation system course error modification method and system | |
| CN110954102A (en) | Magnetometer-assisted inertial navigation system and method for robot positioning | |
| Liu et al. | Interacting multiple model UAV navigation algorithm based on a robust cubature Kalman filter | |
| CN104501809B (en) | Attitude coupling-based strapdown inertial navigation/star sensor integrated navigation method | |
| CN107860382A (en) | A kind of method for measuring posture using AHRS in the case of magnetic anomaly | |
| Liu et al. | Improved path following for autonomous marine vehicles with low-cost heading/course sensors: Comparative experiments | |
| CN204255368U (en) | A kind of SINS/CNS deep integrated navigation system being applicable to Marsokhod | |
| CN116839591B (en) | Track tracking and positioning filtering system and fusion navigation method of rescue unmanned aerial vehicle |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |