CN103226022A - Alignment method and system for movable base used for integrated navigation system - Google Patents

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

The moving alignment method and system that are used for integrated navigation system

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,

$\mathrm{\δ}\stackrel{\·}{L}=\frac{{\mathrm{\δV}}_N}{R_M+h}-\frac{V_N}{{(R_M+h)}^2}\mathrm{\δh}$

$\mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{{\mathrm{\δV}}_E}{(R_N+h)\cos L}+\frac{V_E}{(R_N+h)\cos L}\tan \mathrm{L\δL}$

$-\frac{V_E}{{(R_N-h)}^2\cos L}\mathrm{\δh}$

, wherein,

Be attitude differential vector, ε ⁿBe 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 ⁿBe the component of specific force in local navigation coordinate is of accelerometer output, δ V ⁿBe velocity error,

Be rotational-angular velocity of the earth,

For navigation is the angular velocity that the relative earth is, V ⁿBe 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, $\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+{\mathrm{\Γ}}_{k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array},$ $\begin{array}{c}{\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}\\ P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T\\ K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}\\ P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{P}_k{K}_k^T\\ {\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})\end{array},$ Wherein, X _kBe system state vector, Φ _{K, k-1}Be 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,

$\mathrm{\δ}\stackrel{\·}{L}=\frac{{\mathrm{\δV}}_N}{R_M+h}-\frac{V_N}{{(R_M+h)}^2}\mathrm{\δh}$

$\mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{{\mathrm{\δV}}_E}{(R_N+h)\cos L}+\frac{V_E}{(R_N+h)\cos L}\tan \mathrm{L\δL}$

$-\frac{V_E}{{(R_N+h)}^2\cos L}\mathrm{\δh}$

, wherein,

Be attitude differential vector, ε ⁿBe 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 ⁿBe the component of specific force in local navigation coordinate is of accelerometer output, δ V ⁿBe velocity error,

Be rotational-angular velocity of the earth,

For navigation is the angular velocity that the relative earth is, V ⁿBe 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, $\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+{\mathrm{\Γ}}_{k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}$ $\begin{array}{c}{\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}\\ P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T\\ K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}\\ P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{P}_k{K}_k^T\\ {\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})\end{array},$ Wherein, X _kBe system state vector, Φ _{K, k-1}Be 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,

$\mathrm{\δ}\stackrel{\·}{L}=\frac{{\mathrm{\δV}}_N}{R_M+h}-\frac{V_N}{{(R_M+h)}^2}\mathrm{\δh}$

$\mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{{\mathrm{\δV}}_E}{(R_N+h)\cos L}+\frac{V_E}{(R_N+h)\cos L}\tan \mathrm{L\δL}$

$-\frac{V_E}{{(R_N-h)}^2\cos L}\mathrm{\δh}$

, wherein,

Be attitude differential vector, ε ⁿBe 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 ⁿBe the component of specific force in local navigation coordinate is of accelerometer output, δ V ⁿBe velocity error,

Be rotational-angular velocity of the earth,

For navigation is the angular velocity that the relative earth is, V ⁿBe 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, $\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+{\mathrm{\Γ}}_{k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array},$ $\begin{array}{c}{\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}\\ P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T\\ K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}\\ P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{P}_k{K}_k^T\\ {\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})\end{array},$ Wherein, X _kBe system state vector, Φ _{K, k-1}Be 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 ₀, t ₀) 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,

Ρ ₀=diag[5 ²?5 ²?595 ²?1.5 ²?1.5 ²?2.5 ²?10 ²?10 ²?2 ²]

Ρ ₁=diag[4 ²?4 ²?300 ²?1.5 ²?1.5 ²?2.5 ²?10 ²?10 ²?2 ²]

Ρ ₂=diag[2 ²?2 ²?190 ²?1.5 ²?1.5 ²?2.5 ²?10 ²?10 ²?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.

Acquisition module 100 is used to obtain 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.

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

$\mathrm{\δ}\stackrel{\·}{L}=\frac{{\mathrm{\δV}}_N}{R_M+h}-\frac{V_N}{{(R_M+h)}^2}\mathrm{\δh}$

$\mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{{\mathrm{\δV}}_E}{(R_N+h)\cos L}+\frac{V_E}{(R_N+h)\cos L}\tan \mathrm{L\δL}$

$-\frac{V_E}{{(R_N+h)}^2\cos L}\mathrm{\δh}$

In the example, error model is by following formulate, and its formula is,

, wherein,

Be attitude differential vector, ε ⁿBe 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 ⁿBe the component of specific force in local navigation coordinate is of accelerometer output, δ V ⁿBe velocity error,

Be rotational-angular velocity of the earth,

For navigation is the angular velocity that the relative earth is, V ⁿBe 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, $\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+{\mathrm{\Γ}}_{k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array},$ $\begin{array}{c}{\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}\\ P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T\\ K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}\\ P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{P}_k{K}_k^T\\ {\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})\end{array},$ Wherein, X _kBe system state vector, Φ _{K, k-1}Be 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.

Computing module 400 is used for the Kalman and observes controller according to navigation information, GPS observation information and error model, calculates 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.

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 ₀, t ₀) 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,

Ρ ₀=diag[5 ²?5 ²?595 ²?1.5 ²?1.5 ²?2.5 ²?10 ²?10 ²?2 ²]

Ρ ₁=diag[4 ²?4 ²?300 ²?1.5 ²?1.5 ²?2.5 ²?10 ²?10 ²?2 ²]

Ρ ₂=diag[2 ²?2 ²?190 ²?1.5 ²?1.5 ²?2.5 ²?10 ²?10 ²?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,

$\mathrm{\δ}\stackrel{\·}{L}=\frac{{\mathrm{\δV}}_N}{R_M+h}-\frac{V_N}{{(R_M+h)}^2}\mathrm{\δh}$

$\mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{{\mathrm{\δV}}_E}{(R_N+h)\cos L}+\frac{V_E}{(R_N+h)\cos L}\tan \mathrm{L\δL}$

$-\frac{V_E}{{(R_N-h)}^2\cos L}\mathrm{\δh}$

$\mathrm{\δ}\stackrel{\·}{h}=-\mathrm{\δ}{V}_D$

Wherein,

Be attitude differential vector, ε ⁿBe 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 ⁿBe the component of specific force in local navigation coordinate is of accelerometer output, δ V ⁿBe velocity error,

Be rotational-angular velocity of the earth,

For navigation is the angular velocity that the relative earth is, V ⁿBe 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,

$\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+{\mathrm{\Γ}}_{k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array},$ $\begin{array}{c}{\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}\\ P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T\\ K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}\\ P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{P}_k{K}_k^T\\ {\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})\end{array},$

Wherein, X _kBe system state vector, Φ _{K, k-1}Be 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,

$\mathrm{\δ}\stackrel{\·}{L}=\frac{{\mathrm{\δV}}_N}{R_M+h}-\frac{V_N}{{(R_M+h)}^2}\mathrm{\δh}$

$\mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{{\mathrm{\δV}}_E}{(R_N+h)\cos L}+\frac{V_E}{(R_N+h)\cos L}\tan \mathrm{L\δL}$

$-\frac{V_E}{{(R_N+h)}^2\cos L}\mathrm{\δh}$

$\mathrm{\δ}\stackrel{\·}{h}=-\mathrm{\δ}{V}_D$

Wherein,

Be attitude differential vector, ε ⁿBe 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 ⁿBe the component of specific force in local navigation coordinate is of accelerometer output, δ V ⁿBe velocity error,

Be rotational-angular velocity of the earth,

For navigation is the angular velocity that the relative earth is, V ⁿBe 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,

$\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+{\mathrm{\Γ}}_{k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array},$ $\begin{array}{c}{\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}\\ P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T\\ K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}\\ P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{P}_k{K}_k^T\\ {\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})\end{array},$

Wherein, X _kBe system state vector, Φ _{K, k-1}Be 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.

Images