CN103226022A - Alignment method and system for movable base used for integrated navigation system - Google Patents
Alignment method and system for movable base used for integrated navigation system Download PDFInfo
- Publication number
- CN103226022A CN103226022A CN2013101028527A CN201310102852A CN103226022A CN 103226022 A CN103226022 A CN 103226022A CN 2013101028527 A CN2013101028527 A CN 2013101028527A CN 201310102852 A CN201310102852 A CN 201310102852A CN 103226022 A CN103226022 A CN 103226022A
- Authority
- CN
- China
- Prior art keywords
- stage
- error
- navigation
- delta
- matrix
- 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
- Navigation (AREA)
Abstract
The invention provides an alignment method and system for a movable base for an integrated navigation system. The method comprises the steps that firstly, navigation information and GPS observation information are obtained; secondly, an error model for alignment of the movable base is built; thirdly, a Kalman observation controller is built, wherein work of the Kalman observation controller is divided into multiple stages, and each stage corresponds to a state covariance matrix; fourthly, control gains of each stage are calculated through the Kalman observation controller based on the navigation information, the GPS observation information and the error model; fifthly, whether the control gains meet the Lyapunov stability condition or not is judged, if not, the third step is returned, and if yes, a compensation vector is calculated based on the control gains, the compensation vector is fed back to an inertial measurement unit, and the inertial measurement unit performs adjustment. According to the method, by the adoption of the Kalman observation controller, a navigation angle error can be lowered within a quite small range, and therefore alignment time is greatly reduced and alignment precision is improved.
Description
Technical field
The present invention relates to field of navigation systems, particularly a kind of moving alignment method and system that are used for integrated navigation system.
Background technology
MEMS/GPS(Micro-Electro-Mechanical Systems MEMS (micro electro mechanical system)/Global Positioning System GPS) integrated navigation system be meant with based on the Inertial Measurement Unit (IMU) of MEMS technology as sensor, carry out the auxiliary navigational system of reference information such as position and speed by the gps satellite navigational system.The MEMS/GPS integrated navigation system has good prospect because of its volume is little, in light weight, power consumption is little, cost hangs down in vehicle-mounted, the aerospace applications in future.Moving pedestal autoregistration is meant under the motor-driven situation of carrier, disobeys the method that outer information (as the navigation information of High Accuracy Inertial) is finished system's initial alignment, is the significant process that obtains the system initial heading.
Scherzinger and Rogers propose original linearization error model is made amendment, utilize two variablees to represent that azimuth angle error promptly can solve nonlinear problem with linear model, speed+positional information and the Kalman filtering of utilizing only GPS to provide realize aiming at.This method can be finished the aligning of 180 ° of full angle scopes, but amended error model is complicated more, and calculated amount increases.Hong utilizes the baseline method to solve big misalignment problem in the initial alignment based on revised course angle error model, but the practicality that the baseline meeting of selection influence is aimed at, and still initial error is had requirement.Crassidis proposes to utilize no track Kalman filtering to finish aligning based on the nonlinearity erron model under the big misalignment situation, is not difficult to prove theoretically its stability but there is track filtering.
Summary of the invention
Purpose of the present invention is intended to solve at least one of above-mentioned technological deficiency.
For achieving the above object, the embodiment of one aspect of the present invention proposes a kind of moving alignment method that is used for integrated navigation system, may further comprise the steps: S1: obtain navigation information and GPS observation information, wherein, described navigation information obtains by Inertial Measurement Unit, and described navigation information is carried out inertial navigation resolve; S2: the error model of setting up moving alignment; S3: set up the Kalman and observe controller, wherein, described Kalman observes the work of controller be divided into a plurality of stages, and corresponding state covariance matrix of each stage; S4: the Kalman observes controller according to described navigation information, GPS observation information and error model, calculates the ride gain in each stage in described a plurality of stage; S5: judge whether described ride gain satisfies Leah Pu Nuofu stability condition,, then return step S3,, then enter step S6 if satisfy if do not satisfy; S6: calculate compensation vector according to described ride gain, and described compensation vector is fed back to described Inertial Measurement Unit, and adjust by described Inertial Measurement Unit.
According to the method for the embodiment of the invention, observe controller by adopting the Kalman, can rapidly the course angle error be forced down very little scope, thereby reduce greatly the aligning time, improve alignment precision.
In one embodiment of the invention, described error model and Kalman observe controller adopt at least 9 dimension quantity of states.
In one embodiment of the invention, the quantity of state of described error model is expressed as,
Wherein, north orientation, east orientation and the ground that N, E, D are respectively local navigation coordinate system to, φ is an attitude angle, δ is that error is represented,
Be respectively north orientation, east orientation and ground to velocity error, L, λ and h are respectively latitude, longitude and highly are, δ L, δ λ, δ h is respectively latitude error, longitude error and height error, T is that matrix transpose is represented.
In one embodiment of the invention, described error model is by following formulate, and its formula is,
, wherein,
Be attitude differential vector, ε
nBe gyro zero inclined to one side vector,
For local navigation coordinate is relative geocentric inertial coordinate system angular velocity of rotation error,
Be attitude angle,
For local navigation coordinate is relative geocentric inertial coordinate system angular velocity of rotation,
Be the velocity error differential vector,
Be attitude error, f
bBe the specific force of accelerometer output,
Be attitude matrix, f
nBe the component of specific force in local navigation coordinate is of accelerometer output, δ V
nBe velocity error,
Be rotational-angular velocity of the earth,
For navigation is the angular velocity that the relative earth is, V
nBe the speed of the relative earth of carrier,
Be the accelerometer bias vector,
Be the latitude error differential, δ V
NBe north orientation velocity error, R
MAnd R
NBe respectively the radius-of-curvature of local earth meridian circle and prime vertical, h is a height, V
NBe north orientation speed, δ h is a height error,
Be the longitude error differential, δ V
EBe east orientation velocity error, V
EBe east orientation speed, δ L is a latitude error,
Be the height error differential, δ V
DFor ground to velocity error, superscript n represents the projection of this physical quantity in navigation coordinate system, superscript b represents the projection of this physical quantity in carrier coordinate system.
In one embodiment of the invention, the governing equation that described Kalman observes controller is by following formulate, and its formula is,
Wherein, X
kBe system state vector, Φ
K, k-1Be the system state matrix in k-1 stage, Γ
K-1Be the system noise input matrix in k-1 stage, W
K-1Be system noise, Z
kBe systematic observation vector, H
kBe the systematic observation matrix in k stage, V
kBe k stage observation noise,
For the quantity of state estimating system upgrades,
Be that k-1 stage condition amount is estimated P
K/k-1Be state covariance matrix system update, P
K-1Be k-1 stage condition covariance matrix,
Be the system state matrix transpose in k-1 stage, Q
K-1Be the system noise in k-1 stage,
Be the system noise input matrix transposition in k-1 stage, K
kBe the ride gain in k stage,
Be k stage system observing matrix transposition, R
kBe the k stage to observe covariance matrix, P
kBe k stage condition covariance matrix,
Be that k stage condition amount is estimated Q
kBe k stage system noise, Z
kIt is k stage observed quantity.
For achieving the above object, embodiments of the invention propose a kind of ad hoc network system based on moonlet on the other hand, comprise: acquisition module, be used to obtain navigation information and GPS observation information, wherein, described navigation information obtains by Inertial Measurement Unit, and described navigation information is carried out inertial navigation resolve;
First sets up module, is used to set up the error model of moving alignment; Second sets up module, is used to set up the Kalman and observes controller, and wherein, described Kalman observes the work of controller be divided into a plurality of stages, and corresponding state covariance matrix of each stage; Computing module is used for the Kalman and observes controller according to described navigation information, GPS observation information and error model, calculates the ride gain in each stage in described a plurality of stage; Judge module is used to judge whether described ride gain satisfies Leah Pu Nuofu stability condition, if do not satisfy, then sets up module by second and computing module is handled again; The feedback adjusting module is used for calculating compensation vector according to described ride gain, and described compensation vector is fed back to described Inertial Measurement Unit, and adjusted by described Inertial Measurement Unit.
According to the system of the embodiment of the invention, observe controller by adopting the Kalman, can rapidly the course angle error be forced down very little scope, thereby reduce greatly the aligning time, improve alignment precision.
In one embodiment of the invention, the quantity of state of described error model is expressed as,
Wherein, north orientation, east orientation and the ground that N, E, D are respectively local navigation coordinate system to, φ is an attitude angle, δ is that error is represented,
Be respectively north orientation, east orientation and ground to velocity error, L, λ and h are respectively latitude, longitude and highly are, δ L, δ λ, δ h is respectively latitude error, longitude error and height error, T is that matrix transpose is represented.
In one embodiment of the invention, described error model is by following formulate, and its formula is,
, wherein,
Be attitude differential vector, ε
nBe gyro zero inclined to one side vector,
For local navigation coordinate is relative geocentric inertial coordinate system angular velocity of rotation error,
Be attitude angle,
For local navigation coordinate is relative geocentric inertial coordinate system angular velocity of rotation,
Be the velocity error differential vector,
Be attitude error, f
bBe the specific force of accelerometer output,
Be attitude matrix, f
nBe the component of specific force in local navigation coordinate is of accelerometer output, δ V
nBe velocity error,
Be rotational-angular velocity of the earth,
For navigation is the angular velocity that the relative earth is, V
nBe the speed of the relative earth of carrier,
Be the accelerometer bias vector,
Be the latitude error differential, δ V
NBe north orientation velocity error, R
MAnd R
NBe respectively the radius-of-curvature of local earth meridian circle and prime vertical, h is a height, V
NBe north orientation speed, δ h is a height error,
Be the longitude error differential, δ V
EBe east orientation velocity error, V
EBe east orientation speed, δ L is a latitude error,
Be the height error differential, δ V
DFor ground to velocity error, superscript n represents the projection of this physical quantity in navigation coordinate system, superscript b represents the projection of this physical quantity in carrier coordinate system.
In one embodiment of the invention, the governing equation that described Kalman observes controller is by following formulate, and its formula is,
Wherein, X
kBe system state vector, Φ
K, k-1Be the system state matrix in k-1 stage, Γ
K-1Be the system noise input matrix in k-1 stage, W
K-1Be system noise, Z
kBe systematic observation vector, H
kBe the systematic observation matrix in k stage, V
kBe k stage observation noise,
For the quantity of state estimating system upgrades,
Be that k-1 stage condition amount is estimated P
K/k-1Be state covariance matrix system update, P
K-1Be k-1 stage condition covariance matrix,
Be the system state matrix transpose in k-1 stage, Q
K-1Be the system noise in k-1 stage,
Be the system noise input matrix transposition in k-1 stage, K
kBe the ride gain in k stage,
Be k stage system observing matrix transposition, R
kBe the k stage to observe covariance matrix, P
kBe k stage condition covariance matrix,
Be that k stage condition amount is estimated Q
kBe k stage system noise, Z
kIt is k stage observed quantity.
Aspect that the present invention adds and advantage part in the following description provide, and part will become obviously from the following description, or recognize by practice of the present invention.
Description of drawings
Above-mentioned and/or additional aspect of the present invention and advantage are from obviously and easily understanding becoming the description of embodiment below in conjunction with accompanying drawing, wherein:
Fig. 1 is for being used for the process flow diagram of the moving alignment method of integrated navigation system according to an embodiment of the invention;
Fig. 2-Fig. 7 is the data statistics figure of simulation result according to an embodiment of the invention; And
Fig. 8 is for being used for the frame diagram of the moving alignment system of integrated navigation system according to an embodiment of the invention.
Embodiment
Describe embodiments of the invention below in detail, the example of embodiment is shown in the drawings, and wherein identical from start to finish or similar label is represented identical or similar elements or the element with identical or similar functions.Below by the embodiment that is described with reference to the drawings is exemplary, only is used to explain the present invention, and can not be interpreted as limitation of the present invention.
In description of the invention, it will be appreciated that term " first ", " second " only are used to describe purpose, and can not be interpreted as indication or hint relative importance or the implicit quantity that indicates indicated technical characterictic.Thus, one or more a plurality of this feature can be expressed or impliedly be comprised to the feature that is limited with " first ", " second ".In description of the invention, the implication of " a plurality of " is two or more, unless clear and definite concrete qualification is arranged in addition.
Fig. 3 is the process flow diagram based on the self-organizing network method of moonlet of the embodiment of the invention.As shown in Figure 3, the self-organizing network method based on moonlet according to the embodiment of the invention may further comprise the steps:
Step S101 obtains navigation information and GPS observation information, and wherein, navigation information obtains by Inertial Measurement Unit, and navigation information is carried out inertial navigation resolve.
Particularly, only need adopt the linearization error model when the observation controller of design moving alignment algorithm, near the linearization of promptly former nonlinear system initial point launches.Because the initial alignment time is shorter, zero of gyro and accelerometer changes little partially.Error model and Kalman observe controller adopt at least 9 dimension quantity of states, reduce dimension in order to simplify alignment algorithm, can only adopt 9 dimension error state amounts to observe the foundation of controller.
Step S102 sets up the error model of moving alignment.
Particularly, the quantity of state of error model is expressed as,
Wherein, north orientation, east orientation and the ground that N, E, D are respectively local navigation coordinate system to, φ is an attitude angle, δ is that error is represented,
Be respectively north orientation, east orientation and ground to velocity error, L, λ and h are respectively latitude, longitude and highly are, δ L, δ λ, δ h is respectively latitude error, longitude error and height error, T is that matrix transpose is represented.
In one embodiment of the invention, error model is by following formulate, and its formula is,
, wherein,
Be attitude differential vector, ε
nBe gyro zero inclined to one side vector,
For local navigation coordinate is relative geocentric inertial coordinate system angular velocity of rotation error,
Be attitude angle,
For local navigation coordinate is relative geocentric inertial coordinate system angular velocity of rotation,
Be the velocity error differential vector,
Be attitude error, f
bBe the specific force of accelerometer output,
Be attitude matrix, f
nBe the component of specific force in local navigation coordinate is of accelerometer output, δ V
nBe velocity error,
Be rotational-angular velocity of the earth,
For navigation is the angular velocity that the relative earth is, V
nBe the speed of the relative earth of carrier,
Be the accelerometer bias vector,
Be the latitude error differential, δ V
NBe north orientation velocity error, R
MAnd R
NBe respectively the radius-of-curvature of local earth meridian circle and prime vertical, h is a height, V
NBe north orientation speed, δ h is a height error,
Be the longitude error differential, δ V
EBe east orientation velocity error, V
EBe east orientation speed, δ L is a latitude error,
Be the height error differential, δ V
DFor ground to velocity error, superscript n represents the projection of this physical quantity in navigation coordinate system, superscript b represents the projection of this physical quantity in carrier coordinate system.
Step S103 sets up the Kalman and observes controller, and wherein, the Kalman observes the work of controller be divided into a plurality of stages, and corresponding state covariance matrix of each stage.
Fig. 1 and Fig. 2 are respectively the structural representation that filter construction and Kalman are according to an embodiment of the invention observed controller.As depicted in figs. 1 and 2, the Kalman to observe controller be to be improved by wave filter to form.
Particularly, the governing equation that the Kalman observes controller is by following formulate, and its formula is,
Wherein, X
kBe system state vector, Φ
K, k-1Be the system state matrix in k-1 stage, Γ
K-1Be the system noise input matrix in k-1 stage, W
K-1Be system noise, Z
kBe systematic observation vector, H
kBe the systematic observation matrix in k stage, V
kBe k stage observation noise,
For the quantity of state estimating system upgrades,
Be that k-1 stage condition amount is estimated P
K/k-1Be state covariance matrix system update, P
K-1Be k-1 stage condition covariance matrix,
Be the system state matrix transpose in k-1 stage, Q
K-1Be the system noise in k-1 stage,
Be the system noise input matrix transposition in k-1 stage, K
kBe the ride gain in k stage,
Be k stage system observing matrix transposition, R
kBe the k stage to observe covariance matrix, P
kBe k stage condition covariance matrix,
Be that k stage condition amount is estimated Q
kBe k stage system noise, Z
kIt is k stage observed quantity.
Step S104, Kalman observe controller according to navigation information, GPS observation information and error model, calculate the ride gain in each stage in a plurality of stages.
Particularly, at first, with the system state amount in k-1 stage
Do system update, promptly multiply by the intermediate quantity that the system state matrix obtains k-1 stage and k stage
Then to the covariance matrix P of system
K-1Carry out system update and obtain the intermediate quantity P in k-1 stage and k stage
K/k-1, utilize this intermediate quantity and systematic observation matrix and observation noise to estimate the ride gain in k stage according to following formula.
Step S105 judges whether ride gain satisfies Leah Pu Nuofu stability condition, if do not satisfy, then returns step S3, if satisfy, then enters step S6.Wherein, if Leah Pu Nuofu stability condition for the initial point equilibrium state be stable under the Liapunov meaning, and when the time, t was tending towards infinity disturbed motion Φ (t; x
0, t
0) converge to equilibrium state x
ε=0, and in this process, do not break away from S (ε), claim that then the system balancing state is asymptotically stable.Be that the centre of sphere is a ball territory of radius with ε with the initial point in S (ε) the expression state space wherein, Φ represents system equation, and x represents system state amount.
Step S106 calculates compensation vector according to ride gain, and compensation vector is fed back to Inertial Measurement Unit, and is adjusted by Inertial Measurement Unit.
Particularly, with the k stage system quantity of state that solves
Be brought in the inertial navigation algorithm, the attitude angle, the north orientation east orientation ground that are about to system in the inertial navigation algorithm deduct to numerical value such as speed, latitude longitude height
Then finished correction to system.
According to the method for the embodiment of the invention, observe controller by adopting the Kalman, can rapidly the course angle error be forced down very little scope, thereby reduce greatly the aligning time, improve alignment precision.
In order to verify the validity of present embodiment, the motor-driven moving alignment down of difference has been carried out emulation by utilizing Desired Track.Its simulated conditions is, gyro skew 50deg/h, and gyro noise 200deg/h, accelerometer skew 1mg, accelerometer noise 10mg, GPS location noise criteria difference is 2m, GPS velocity survey noise criteria difference is 0.2m/s.The angle, initial heading is set at 45 degree, 135 degree ,-135 degree and-45 degree respectively, and the initial attitude angle is 0 degree.The coordinate turn process is: aircraft along 45 degree initial angles northeastward direction fly at a constant speed, initial velocity is 200m/s, preceding 1 minute roll angle 30 degree keep 6 minutes turning process, last 1 minute recovery level afterwards.Become to quicken the rectilinear motion process into: aircraft along 45 degree initial angles direction rectilinear motion northeastward, initial line speed is 20m/s, acceleration was in preceding 4 minutes, acceleration was in back four minutes.The emulation that utilizes above-mentioned flight path to carry out respectively 8 minutes, being aligned on the basis that the Kalman observes controller of employing divides three sections, and every section initialized state covariance matrix is,
Ρ
0=diag[5
2?5
2?595
2?1.5
2?1.5
2?2.5
2?10
2?10
2?2
2]
Ρ
1=diag[4
2?4
2?300
2?1.5
2?1.5
2?2.5
2?10
2?10
2?2
2]
Ρ
2=diag[2
2?2
2?190
2?1.5
2?1.5
2?2.5
2?10
2?10
2?2
2]
。
Fig. 4-Fig. 7 is the data statistics figure of simulation result according to an embodiment of the invention.To shown in Figure 7, wherein Fig. 4 has provided the image of aircraft longitude and latitude and north orientation speed thereof under the coordinate turn flight path as Fig. 4.Fig. 5 has provided under the coordinate turn flight path, adopts general filtering mode respectively and based on observer controller dual mode, provides under 45 ° ,-45 °, 135 ° ,-135 ° angle, the initial heading situation convergence situation of course angle respectively.Fig. 6 has provided the image of aircraft longitude and latitude and north orientation speed thereof under the variable accelerated motion flight path, Fig. 7 has provided under the variable accelerated motion flight path, adopt general filtering mode respectively and based on observer controller dual mode, provide under 45 ° ,-45 °, 135 ° ,-135 ° angle, the initial heading situation convergence situation of course angle respectively.The angle, initial heading of Desired Track is 45 ° under two kinds of conditions.According to above-mentioned simulation result as can be seen, under big misalignment situation, the Kalman who adopts observes the performance of controller alignment algorithm will be better than general Filtering Estimation algorithm greatly, especially in the starting stage, when the angle, initial heading that provides differs 180 ° for-135 ° and Desired Track, general Filtering Estimation algorithm can only slowly be restrained from big misalignment state, and the present embodiment algorithm can force down the course angle error very little scope rapidly, reduces the aligning time greatly, improves alignment precision.
Fig. 8 is for being used for the frame diagram of the moving alignment system of integrated navigation system according to an embodiment of the invention.As shown in Figure 8, comprise that according to the moving alignment system that is used for integrated navigation system of the embodiment of the invention acquisition module 100, first sets up module 200, second and set up module 300, computing module 400, judge module 500 and feedback adjusting module 600.
Particularly, only need adopt the linearization error model when the observation controller of design moving alignment algorithm, near the linearization of promptly former nonlinear system initial point launches.Because the initial alignment time is shorter, zero of gyro and accelerometer changes little partially.Error model and Kalman observe controller adopt at least 9 dimension quantity of states, reduce dimension in order to simplify alignment algorithm, can only adopt 9 dimension error state amounts to observe the foundation of controller.
First sets up the error model that module 200 is used to set up moving alignment.
Particularly, the quantity of state of error model is expressed as,
Wherein, north orientation, east orientation and the ground that N, E, D are respectively local navigation coordinate system to, φ is an attitude angle, δ is that error is represented,
Be respectively north orientation, east orientation and ground to velocity error, L, λ and h are respectively latitude, longitude and highly are, δ L, δ λ, δ h is respectively latitude error, longitude error and height error, T is that matrix transpose is represented.In an enforcement of the present invention
In the example, error model is by following formulate, and its formula is,
, wherein,
Be attitude differential vector, ε
nBe gyro zero inclined to one side vector,
For local navigation coordinate is relative geocentric inertial coordinate system angular velocity of rotation error,
Be attitude angle,
For local navigation coordinate is relative geocentric inertial coordinate system angular velocity of rotation,
Be the velocity error differential vector,
Be attitude error, f
bBe the specific force of accelerometer output,
Be attitude matrix, f
nBe the component of specific force in local navigation coordinate is of accelerometer output, δ V
nBe velocity error,
Be rotational-angular velocity of the earth,
For navigation is the angular velocity that the relative earth is, V
nBe the speed of the relative earth of carrier,
Be the accelerometer bias vector,
Be the latitude error differential, δ V
NBe north orientation velocity error, R
MAnd R
NBe respectively the radius-of-curvature of local earth meridian circle and prime vertical, h is a height, V
NBe north orientation speed, δ h is a height error,
Be the longitude error differential, δ V
EBe east orientation velocity error, V
EBe east orientation speed, δ L is a latitude error,
Be the height error differential, δ V
DFor ground to velocity error, superscript n represents the projection of this physical quantity in navigation coordinate system, superscript b represents the projection of this physical quantity in carrier coordinate system.
Second sets up module 300 is used to set up the Kalman and observes controller, and wherein, the Kalman observes the work of controller be divided into a plurality of stages, and corresponding state covariance matrix of each stage.
Fig. 1 and Fig. 2 are respectively the structural representation that filter construction and Kalman are according to an embodiment of the invention observed controller.As depicted in figs. 1 and 2, the Kalman to observe controller be to be improved by wave filter to form.
Particularly, the governing equation that the Kalman observes controller is by following formulate, and its formula is,
Wherein, X
kBe system state vector, Φ
K, k-1Be the system state matrix in k-1 stage, Γ
K-1Be the system noise input matrix in k-1 stage, W
K-1Be system noise, Z
kBe systematic observation vector, H
kBe the systematic observation matrix in k stage, V
kBe k stage observation noise,
For the quantity of state estimating system upgrades,
Be that k-1 stage condition amount is estimated P
K/k-1Be state covariance matrix system update, P
K-1Be k-1 stage condition covariance matrix,
Be the system state matrix transpose in k-1 stage, Q
K-1Be the system noise in k-1 stage,
Be the system noise input matrix transposition in k-1 stage, K
kBe the ride gain in k stage,
Be k stage system observing matrix transposition, R
kBe the k stage to observe covariance matrix, P
kBe k stage condition covariance matrix,
Be that k stage condition amount is estimated Q
kBe k stage system noise, Z
kIt is k stage observed quantity.
Particularly, at first, with the system state amount in k-1 stage
Do system update, promptly multiply by the intermediate quantity that the system state matrix obtains k-1 stage and k stage
Then to the covariance matrix P of system
K-1Carry out system update and obtain the intermediate quantity P in k-1 stage and k stage
K/k-1, utilize this intermediate quantity and systematic observation matrix and observation noise to estimate the ride gain in k stage according to following formula.
Whether the ride gain that is used to judge module 500 judge satisfies Leah Pu Nuofu stability condition, if do not satisfy, then sets up module by second and computing module is handled again.Wherein, if Leah Pu Nuofu stability condition for the initial point equilibrium state be stable under the Liapunov meaning, and when the time, t was tending towards infinity disturbed motion Φ (t; x
0, t
0) converge to equilibrium state x
ε=0, and in this process, do not break away from S (ε), claim that then the system balancing state is asymptotically stable.Be that the centre of sphere is a ball territory of radius with ε with the initial point in S (ε) the expression state space wherein, Φ represents system equation, and x represents system state amount.
Feedback adjusting module 600 is used for calculating compensation vector according to ride gain, and compensation vector is fed back to Inertial Measurement Unit, and is adjusted by Inertial Measurement Unit.
Particularly, with the k stage system quantity of state that solves
Be brought in the inertial navigation algorithm, the attitude angle, the north orientation east orientation ground that are about to system in the inertial navigation algorithm deduct to numerical value such as speed, latitude longitude height
Then finished correction to system.
According to the system of the embodiment of the invention, observe controller by adopting the Kalman, can rapidly the course angle error be forced down very little scope, thereby reduce greatly the aligning time, improve alignment precision.
In order to verify the validity of present embodiment, the motor-driven moving alignment down of difference has been carried out emulation by utilizing Desired Track.Its simulated conditions is, gyro skew 50deg/h, and gyro noise 200deg/h, accelerometer skew 1mg, accelerometer noise 10mg, GPS location noise criteria difference is 2m, GPS velocity survey noise criteria difference is 0.2m/s.The angle, initial heading is set at 45 degree, 135 degree ,-135 degree and-45 degree respectively, and the initial attitude angle is 0 degree.The coordinate turn process is: aircraft along 45 degree initial angles northeastward direction fly at a constant speed, initial velocity is 200m/s, preceding 1 minute roll angle 30 degree keep 6 minutes turning process, last 1 minute recovery level afterwards.Become to quicken the rectilinear motion process into: aircraft along 45 degree initial angles direction rectilinear motion northeastward, initial line speed is 20m/s, acceleration was in preceding 4 minutes, acceleration was in back four minutes.The emulation that utilizes above-mentioned flight path to carry out respectively 8 minutes, being aligned on the basis that the Kalman observes controller of employing divides three sections, and every section initialized state covariance matrix is,
Ρ
0=diag[5
2?5
2?595
2?1.5
2?1.5
2?2.5
2?10
2?10
2?2
2]
Ρ
1=diag[4
2?4
2?300
2?1.5
2?1.5
2?2.5
2?10
2?10
2?2
2]
Ρ
2=diag[2
2?2
2?190
2?1.5
2?1.5
2?2.5
2?10
2?10
2?2
2]
。
Fig. 4-Fig. 7 is the data statistics figure of simulation result according to an embodiment of the invention.To shown in Figure 7, wherein Fig. 4 has provided the image of aircraft longitude and latitude and north orientation speed thereof under the coordinate turn flight path as Fig. 4.Fig. 5 has provided under the coordinate turn flight path, adopts general filtering mode respectively and based on observer controller dual mode, provides under 45 ° ,-45 °, 135 ° ,-135 ° angle, the initial heading situation convergence situation of course angle respectively.Fig. 6 has provided the image of aircraft longitude and latitude and north orientation speed thereof under the variable accelerated motion flight path, Fig. 7 has provided under the variable accelerated motion flight path, adopt general filtering mode respectively and based on observer controller dual mode, provide under 45 ° ,-45 °, 135 ° ,-135 ° angle, the initial heading situation convergence situation of course angle respectively.The angle, initial heading of Desired Track is 45 ° under two kinds of conditions.According to above-mentioned simulation result as can be seen, under big misalignment situation, the Kalman who adopts observes the performance of controller alignment algorithm will be better than general Filtering Estimation algorithm greatly, especially in the starting stage, when the angle, initial heading that provides differs 180 ° for-135 ° and Desired Track, general Filtering Estimation algorithm can only slowly be restrained from big misalignment state, and the present embodiment algorithm can force down the course angle error very little scope rapidly, reduces the aligning time greatly, improves alignment precision.
The specific operation process that should be appreciated that each module in the system embodiment of the present invention and unit can be identical with the description among the method embodiment, is not described in detail herein.
Although illustrated and described embodiments of the invention above, be understandable that, the foregoing description is exemplary, can not be interpreted as limitation of the present invention, those of ordinary skill in the art can change the foregoing description under the situation that does not break away from principle of the present invention and aim within the scope of the invention, modification, replacement and modification.
Claims (10)
1. a moving alignment method that is used for integrated navigation system is characterized in that, may further comprise the steps:
S1: obtain navigation information and GPS observation information, wherein, described navigation information obtains by Inertial Measurement Unit, and described navigation information is carried out inertial navigation resolve;
S2: the error model of setting up moving alignment;
S3: set up the Kalman and observe controller, wherein, described Kalman observes the work of controller be divided into a plurality of stages, and corresponding state covariance matrix of each stage;
S4: the Kalman observes controller according to described navigation information, GPS observation information and error model, calculates the ride gain in each stage in described a plurality of stage;
S5: judge whether described ride gain satisfies Leah Pu Nuofu stability condition,, then return step S3,, then enter step S6 if satisfy if do not satisfy;
S6: calculate compensation vector according to described ride gain, and described compensation vector is fed back to described Inertial Measurement Unit, and adjust by described Inertial Measurement Unit.
2. the moving alignment method that is used for integrated navigation system as claimed in claim 1 is characterized in that, described error model and Kalman observe controller adopt at least 9 dimension quantity of states.
3. the moving alignment method that is used for integrated navigation system as claimed in claim 1 is characterized in that the quantity of state of described error model is expressed as,
Wherein, north orientation, east orientation and the ground that N, E, D are respectively local navigation coordinate system to, φ is an attitude angle, δ is that error is represented,
Be respectively north orientation, east orientation and ground to velocity error, L, λ and h are respectively latitude, longitude and highly are, δ L, δ λ, δ h is respectively latitude error, longitude error and height error, T is that matrix transpose is represented.
4. the moving alignment method that is used for integrated navigation system as claimed in claim 1 is characterized in that, described error model is by following formulate, and its formula is,
Wherein,
Be attitude differential vector, ε
nBe gyro zero inclined to one side vector,
For local navigation coordinate is relative geocentric inertial coordinate system angular velocity of rotation error,
Be attitude angle,
For local navigation coordinate is relative geocentric inertial coordinate system angular velocity of rotation,
Be the velocity error differential vector,
Be attitude error, f
bBe the specific force of accelerometer output,
Be attitude matrix, f
nBe the component of specific force in local navigation coordinate is of accelerometer output, δ V
nBe velocity error,
Be rotational-angular velocity of the earth,
For navigation is the angular velocity that the relative earth is, V
nBe the speed of the relative earth of carrier,
Be the accelerometer bias vector,
Be the latitude error differential, δ V
NBe north orientation velocity error, R
MAnd R
NBe respectively the radius-of-curvature of local earth meridian circle and prime vertical, h is a height, V
NBe north orientation speed, δ h is a height error,
Be the longitude error differential, δ V
EBe east orientation velocity error, V
EBe east orientation speed, δ L is a latitude error,
Be the height error differential, δ V
DFor ground to velocity error, superscript n represents the projection of this physical quantity in navigation coordinate system, superscript b represents the projection of this physical quantity in carrier coordinate system.
5. the moving alignment method that is used for integrated navigation system as claimed in claim 1 is characterized in that, the governing equation that described Kalman observes controller is by following formulate, and its formula is,
Wherein, X
kBe system state vector, Φ
K, k-1Be the system state matrix in k-1 stage, Γ
K-1Be the system noise input matrix in k-1 stage, W
K-1Be system noise, Z
kBe systematic observation vector, H
kBe the systematic observation matrix in k stage, V
kBe k stage observation noise,
For the quantity of state estimating system upgrades,
Be that k-1 stage condition amount is estimated P
K/k-1Be state covariance matrix system update, P
K-1Be k-1 stage condition covariance matrix,
Be the system state matrix transpose in k-1 stage, Q
K-1Be the system noise in k-1 stage,
Be the system noise input matrix transposition in k-1 stage, K
kBe the ride gain in k stage,
Be k stage system observing matrix transposition, R
kBe the k stage to observe covariance matrix, P
kBe k stage condition covariance matrix,
Be that k stage condition amount is estimated Q
kBe k stage system noise, Z
kIt is k stage observed quantity.
6. a moving alignment system that is used for integrated navigation system is characterized in that, comprising:
Acquisition module is used to obtain navigation information and GPS observation information, and wherein, described navigation information obtains by Inertial Measurement Unit, and described navigation information is carried out inertial navigation resolve;
First sets up module, is used to set up the error model of moving alignment;
Second sets up module, is used to set up the Kalman and observes controller, and wherein, described Kalman observes the work of controller be divided into a plurality of stages, and corresponding state covariance matrix of each stage;
Computing module is used for the Kalman and observes controller according to described navigation information, GPS observation information and error model, calculates the ride gain in each stage in described a plurality of stage;
Judge module is used to judge whether described ride gain satisfies Leah Pu Nuofu stability condition, if do not satisfy, then sets up module by second and computing module is handled again;
The feedback adjusting module is used for calculating compensation vector according to described ride gain, and described compensation vector is fed back to described Inertial Measurement Unit, and adjusted by described Inertial Measurement Unit.
7. the moving alignment system that is used for integrated navigation system as claimed in claim 6 is characterized in that, described error model and Kalman observe controller adopt at least 9 dimension quantity of states.
8. the moving alignment system that is used for integrated navigation system as claimed in claim 6 is characterized in that the quantity of state of described error model is expressed as,
Wherein, north orientation, east orientation and the ground that N, E, D are respectively local navigation coordinate system to, φ is an attitude angle, δ is that error is represented,
Be respectively north orientation, east orientation and ground to velocity error, L, λ and h are respectively latitude, longitude and highly are, δ L, δ λ, δ h is respectively latitude error, longitude error and height error, T is that matrix transpose is represented.
9. the moving alignment system that is used for integrated navigation system as claimed in claim 6 is characterized in that, described error model is by following formulate, and its formula is,
Wherein,
Be attitude differential vector, ε
nBe gyro zero inclined to one side vector,
For local navigation coordinate is relative geocentric inertial coordinate system angular velocity of rotation error,
Be attitude angle,
For local navigation coordinate is relative geocentric inertial coordinate system angular velocity of rotation,
Be the velocity error differential vector,
Be attitude error, f
bBe the specific force of accelerometer output,
Be attitude matrix, f
nBe the component of specific force in local navigation coordinate is of accelerometer output, δ V
nBe velocity error,
Be rotational-angular velocity of the earth,
For navigation is the angular velocity that the relative earth is, V
nBe the speed of the relative earth of carrier,
Be the accelerometer bias vector,
Be the latitude error differential, δ V
NBe north orientation velocity error, R
MAnd R
NBe respectively the radius-of-curvature of local earth meridian circle and prime vertical, h is a height, V
NBe north orientation speed, δ h is a height error,
Be the longitude error differential, δ V
EBe east orientation velocity error, V
EBe east orientation speed, δ L is a latitude error,
Be the height error differential, δ V
DFor ground to velocity error, superscript n represents the projection of this physical quantity in navigation coordinate system, superscript b represents the projection of this physical quantity in carrier coordinate system.
10. the moving alignment system that is used for integrated navigation system as claimed in claim 6 is characterized in that, the governing equation that described Kalman observes controller is by following formulate, and its formula is,
Wherein, X
kBe system state vector, Φ
K, k-1Be the system state matrix in k-1 stage, Γ
K-1Be the system noise input matrix in k-1 stage, W
K-1Be system noise, Z
kBe systematic observation vector, H
kBe the systematic observation matrix in k stage, V
kBe k stage observation noise,
For the quantity of state estimating system upgrades,
Be that k-1 stage condition amount is estimated P
K/k-1Be state covariance matrix system update, P
K-1Be k-1 stage condition covariance matrix,
Be the system state matrix transpose in k-1 stage, Q
K-1Be the system noise in k-1 stage,
Be the system noise input matrix transposition in k-1 stage, K
kBe the ride gain in k stage,
Be k stage system observing matrix transposition, R
kBe the k stage to observe covariance matrix, P
kBe k stage condition covariance matrix,
Be that k stage condition amount is estimated Q
kBe k stage system noise, Z
kIt is k stage observed quantity.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201310102852.7A CN103226022B (en) | 2013-03-27 | 2013-03-27 | For the moving alignment method and system of integrated navigation system |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201310102852.7A CN103226022B (en) | 2013-03-27 | 2013-03-27 | For the moving alignment method and system of integrated navigation system |
Publications (2)
Publication Number | Publication Date |
---|---|
CN103226022A true CN103226022A (en) | 2013-07-31 |
CN103226022B CN103226022B (en) | 2015-08-12 |
Family
ID=48836556
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201310102852.7A Active CN103226022B (en) | 2013-03-27 | 2013-03-27 | For the moving alignment method and system of integrated navigation system |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN103226022B (en) |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2015149203A1 (en) * | 2014-03-31 | 2015-10-08 | Intel Corporation | Inertial measurement unit for electronic devices |
CN105737823A (en) * | 2016-02-01 | 2016-07-06 | 东南大学 | GPS/SINS/CNS integrated navigation method based on five-order CKF |
CN107703774A (en) * | 2017-09-12 | 2018-02-16 | 江西洪都航空工业集团有限责任公司 | A kind of air weapon moving base Transfer Alignment emulation mode |
CN108195400A (en) * | 2017-12-22 | 2018-06-22 | 清华大学 | The moving alignment method of strapdown micro electro mechanical inertia navigation system |
CN114136340A (en) * | 2021-11-29 | 2022-03-04 | 重庆华渝电气集团有限公司 | Method for eliminating influence of misalignment angle error on initial alignment |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6691033B1 (en) * | 2000-07-26 | 2004-02-10 | Hughes Electronics Corporation | System and method for calibrating inter-star-tracker misalignments in a stellar inertial attitude determination system |
CN101131311A (en) * | 2007-10-15 | 2008-02-27 | 北京航空航天大学 | Alignment and calibration method for intelligentized aircraft missile movable base |
US7626534B1 (en) * | 2007-06-12 | 2009-12-01 | Lockheed Martin Corporation | Unified navigation and inertial target tracking estimation system |
CN101750066A (en) * | 2009-12-31 | 2010-06-23 | 中国人民解放军国防科学技术大学 | SINS dynamic base transfer alignment method based on satellite positioning |
-
2013
- 2013-03-27 CN CN201310102852.7A patent/CN103226022B/en active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6691033B1 (en) * | 2000-07-26 | 2004-02-10 | Hughes Electronics Corporation | System and method for calibrating inter-star-tracker misalignments in a stellar inertial attitude determination system |
US7626534B1 (en) * | 2007-06-12 | 2009-12-01 | Lockheed Martin Corporation | Unified navigation and inertial target tracking estimation system |
CN101131311A (en) * | 2007-10-15 | 2008-02-27 | 北京航空航天大学 | Alignment and calibration method for intelligentized aircraft missile movable base |
CN101750066A (en) * | 2009-12-31 | 2010-06-23 | 中国人民解放军国防科学技术大学 | SINS dynamic base transfer alignment method based on satellite positioning |
Non-Patent Citations (3)
Title |
---|
季超等: "INS/GPS组合导航系统动基座初始对准研究", 《舰船电子工程》 * |
朱让剑等: "动基座条件下组合导航初始对准研究", 《测控技术》 * |
雷堰龙等: "多辅助信息下动基座初始对准方法研究", 《惯性技术发展动态发展方向研讨会文集》 * |
Cited By (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2015149203A1 (en) * | 2014-03-31 | 2015-10-08 | Intel Corporation | Inertial measurement unit for electronic devices |
KR20160117526A (en) * | 2014-03-31 | 2016-10-10 | 인텔 코포레이션 | Inertial measurement unit for electronic devices |
CN106662448A (en) * | 2014-03-31 | 2017-05-10 | 英特尔公司 | Inertial measurement unit for electronic devices |
KR101948242B1 (en) * | 2014-03-31 | 2019-05-10 | 인텔 코포레이션 | Inertial measurement unit for electronic devices |
CN106662448B (en) * | 2014-03-31 | 2021-01-01 | 英特尔公司 | Inertial measurement unit for electronic devices |
CN105737823A (en) * | 2016-02-01 | 2016-07-06 | 东南大学 | GPS/SINS/CNS integrated navigation method based on five-order CKF |
CN105737823B (en) * | 2016-02-01 | 2018-09-21 | 东南大学 | A kind of GPS/SINS/CNS Combinated navigation methods based on five rank CKF |
CN107703774A (en) * | 2017-09-12 | 2018-02-16 | 江西洪都航空工业集团有限责任公司 | A kind of air weapon moving base Transfer Alignment emulation mode |
CN108195400A (en) * | 2017-12-22 | 2018-06-22 | 清华大学 | The moving alignment method of strapdown micro electro mechanical inertia navigation system |
CN114136340A (en) * | 2021-11-29 | 2022-03-04 | 重庆华渝电气集团有限公司 | Method for eliminating influence of misalignment angle error on initial alignment |
Also Published As
Publication number | Publication date |
---|---|
CN103226022B (en) | 2015-08-12 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106443746B (en) | A kind of low cost double antenna GNSS/AHRS combination survey attitude positioning method | |
CN101793523B (en) | Combined navigation and photoelectric detection integrative system | |
CN106871928B (en) | Strap-down inertial navigation initial alignment method based on lie group filtering | |
CN106767787A (en) | A kind of close coupling GNSS/INS combined navigation devices | |
CN102901514B (en) | Collaborative initial alignment method based on multiple-inertia-unit informational constraint | |
CN101825467B (en) | Method for realizing integrated navigation through ship's inertial navigation system (SINS) and celestial navigation system (SNS) | |
CN101246012B (en) | Combinated navigation method based on robust dissipation filtering | |
CN104501838B (en) | SINS Initial Alignment Method | |
CN103226022A (en) | Alignment method and system for movable base used for integrated navigation system | |
CN108151737A (en) | A kind of unmanned plane bee colony collaborative navigation method under the conditions of the mutual observed relationships of dynamic | |
CN103196448A (en) | Airborne distributed inertial attitude measurement system and transfer alignment method of airborne distributed inertial attitude measurement system | |
CN101788296A (en) | SINS/CNS deep integrated navigation system and realization method thereof | |
CN102519485B (en) | Gyro information-introduced double-position strapdown inertial navigation system initial alignment method | |
CN104181574A (en) | Strapdown inertial navigation system/global navigation satellite system combined based navigation filter system and method | |
CN102508275A (en) | Multiple-antenna GPS(Global Positioning System)/GF-INS (Gyroscope-Free-Inertial Navigation System) depth combination attitude determining method | |
CN106840194B (en) | A kind of Large azimuth angle linear alignment method | |
CN111189442B (en) | CEPF-based unmanned aerial vehicle multi-source navigation information state prediction method | |
CN111399023B (en) | Inertial basis combined navigation filtering method based on lie group nonlinear state error | |
CN107389099A (en) | The aerial fast alignment device of SINS and method | |
CN102538821A (en) | Fast and parameter sectional type self-alignment method for strapdown inertial navigation system | |
CN102645223A (en) | Serial inertial navigation vacuum filtering correction method based on specific force observation | |
CN107907898A (en) | Polar region SINS/GPS Integrated Navigation Algorithms based on grid frame | |
CN101900573A (en) | Method for realizing landtype inertial navigation system movement aiming | |
ITTO20060107A1 (en) | AUTOMATIC PILOT EQUIPMENT FOR A ROTARY SAILING AIRCRAFT | |
CN107270905B (en) | Inertial platform continuously rolls self-calibration Alignment Method under a kind of quiet pedestal |
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 |