CN103226022B - For the moving alignment method and system of integrated navigation system - Google Patents

Abstract

The present invention proposes a kind of moving alignment method and system for integrated navigation system.Wherein, method comprises: S1: obtain navigation information and GPS observation information; S2: the error model setting up moving alignment; S3: set up Kalman and observe controller, wherein, Kalman observes the work of controller be divided into multiple stage, and the corresponding state covariance matrix of each stage; S4: Kalman observes controller according to navigation information, GPS observation information and error model, calculates the ride gain in each stage; S5: judge whether ride gain meets Leah Pu Nuofu stability condition, if do not meet, returns step S3, if meet, according to ride gain calculation compensation vector, and feeds back to Inertial Measurement Unit by compensation vector, and is adjusted by Inertial Measurement Unit.According to the method for the embodiment of the present invention, by adopting Kalman to observe controller, rapidly course angle error can be forced down very little scope, thus greatly reduce the aligning time, improve alignment precision.

Description

For the moving alignment method and system of integrated navigation system

Technical field

The present invention relates to field of navigation systems, particularly a kind of moving alignment method and system for integrated navigation system.

Background technology

MEMS/GPS(Micro-Electro-Mechanical Systems MEMS (micro electro mechanical system)/Global PositioningSystem GPS) integrated navigation system refers to using the Inertial Measurement Unit (IMU) based on MEMS technology as sensor, carried out the navigational system that the reference informations such as position and speed are auxiliary by gps satellite navigational system.MEMS/GPS integrated navigation system because its volume is little, lightweight, power consumption is little, cost is low in vehicle-mounted, the aerospace applications in future, have good prospect.Moving base autoregistration refers in the motor-driven situation of carrier, and not relying on the method for external information (navigation information as High Accuracy Inertial) completion system initial alignment, is the significant process obtaining system initial heading.

Scherzinger and Rogers proposes to modify to original linearized stability model, utilize Two Variables to represent that namely azimuth angle error can solve nonlinear problem with linear model, the speed+positional information utilizing only GPS to provide and Kalman filtering realize aiming at.This method can complete the aligning of 180 ° of full angle scopes, but amended error model is more complicated, and calculated amount increases.Hong, based on revised course angle error model, utilize Baseline Methods to solve large misalignment angle problem in initial alignment, but baseline selection can affect the practicality of aligning, and still have requirement to initial error.Crassidis proposes based on the Nonlinear Error Models in large misalignment angle situation, utilizes unscented Kalman filter to complete aligning, but is difficult to prove its stability theoretically without track filtering.

Summary of the invention

Object of the present invention is intended at least solve 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 for integrated navigation system, comprise the following steps: S1: obtain navigation information and GPS observation information, wherein, described navigation information is obtained by Inertial Measurement Unit, and carries out inertial navigation to described navigation information and resolve; S2: the error model setting up moving alignment; S3: set up Kalman and observe controller, wherein, described Kalman observes the work of controller be divided into multiple stage, and the corresponding state covariance matrix of each stage; S4: Kalman observes controller according to described navigation information, GPS observation information and error model, calculates the ride gain in each stage in described multiple stage; S5: judge whether described ride gain meets Leah Pu Nuofu stability condition, if do not meet, then returns step S3, if meet, then enters step S6; S6: according to described ride gain calculation compensation vector, and described compensation vector is fed back to described Inertial Measurement Unit, and adjusted by described Inertial Measurement Unit.

According to the method for the embodiment of the present invention, by adopting Kalman to observe controller, rapidly course angle error can be forced down very little scope, thus greatly reduce 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, N, E, D be respectively the north orientation of local navigational coordinate system, east orientation and ground to, φ is attitude angle, and δ is that error represents, be respectively north orientation, east orientation and ground to velocity error, L, λ and h are respectively latitude, longitude and are highly, δ L, δ λ, and δ h is respectively latitude error, longitude error and height error, and T is that matrix transpose represents.

In one embodiment of the invention, described error model is represented by following formula, and its formula is,

, wherein, for attitude differential vector, ε ⁿfor gyro zero vector partially, for local navigational coordinate system relative geocentric inertial coordinate system angular velocity of rotation error, for attitude angle, for the relative geocentric inertial coordinate system angular velocity of rotation of local navigational coordinate system, for velocity error differential vector, for attitude error, f ^bfor the specific force that accelerometer exports, for attitude matrix, f ⁿfor the component of specific force in local navigational coordinate system that accelerometer exports, δ V ⁿfor velocity error, for rotational-angular velocity of the earth, for the angular velocity that navigation is relative earth system, V ⁿfor the speed of the relative earth of carrier, for accelerometer bias vector, for latitude error differential, δ V _nfor north orientation velocity error, R _mand R _nbe respectively the radius-of-curvature of local earth meridian circle and prime vertical, h is height, V _nfor north orientation speed, δ h is height error, for longitude error differential, δ V _efor east orientation velocity error, V _efor east orientation speed, δ L is latitude error, for height error differential, δ V _dfor ground is to velocity error, superscript n represents the projection of this physical quantity in navigational coordinate system, and superscript b represents the projection of this physical quantity in carrier coordinate system.

In one embodiment of the invention, described Kalman observes the governing equation of controller be represented by following formula, and its formula is, $\begin{array}{cc}& {\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}\\ X_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+{\mathrm{\Γ}}_{k-1}{W}_{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\\ Z_k=H_k{X}_k+V_k,& 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 _kfor system state vector, Φ _{k, k-1}for the system state matrix in kth-1 stage, Γ _k-1for the system noise input matrix in kth-1 stage, W _k-1for system noise, Z _kfor systematic observation vector, H _kfor the systematic observation matrix in kth stage, V _kfor kth stage observation noise, for quantity of state estimating system upgrades, for kth-1 stage condition amount is estimated, P _k/k-1for state covariance matrix system update, P _k-1for kth-1 stage condition covariance matrix, for the system state matrix transpose in kth-1 stage, Q _k-1for the system noise in kth-1 stage, for the system noise input matrix transposition in kth-1 stage, K _kfor the ride gain in kth stage, for kth stage system observing matrix transposition, R _kfor kth stage observation covariance matrix, P _kfor kth stage condition covariance matrix, for kth stage condition amount is estimated, Q _kfor kth stage system noise, Z _kfor the observed quantity of kth stage.

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, for obtaining navigation information and GPS observation information, wherein, described navigation information is obtained by Inertial Measurement Unit, and carries out inertial navigation to described navigation information and resolve;

First sets up module, for setting up the error model of moving alignment; Second sets up module, observes controller for setting up Kalman, and wherein, described Kalman observes the work of controller be divided into multiple stage, and the corresponding state covariance matrix of each stage; Computing module, observes controller according to described navigation information, GPS observation information and error model for Kalman, calculates the ride gain in each stage in described multiple stage; Judge module, for judging whether described ride gain meets Leah Pu Nuofu stability condition, if do not meet, then sets up module by second and computing module re-starts process; Feedback adjusting module, for vectorial according to described ride gain calculation compensation, and feeds back to described Inertial Measurement Unit by described compensation vector, and is adjusted by described Inertial Measurement Unit.

According to the system of the embodiment of the present invention, by adopting Kalman to observe controller, rapidly course angle error can be forced down very little scope, thus greatly reduce the aligning time, improve alignment precision.

In one embodiment of the invention, the quantity of state of described error model is expressed as, wherein, N, E, D be respectively the north orientation of local navigational coordinate system, east orientation and ground to, φ is attitude angle, and δ is that error represents, be respectively north orientation, east orientation and ground to velocity error, L, λ and h are respectively latitude, longitude and are highly, δ L, δ λ, and δ h is respectively latitude error, longitude error and height error, and T is that matrix transpose represents.

In one embodiment of the invention, described error model is represented by following formula, and its formula is,

, wherein, for attitude differential vector, ε ⁿfor gyro zero vector partially, for local navigational coordinate system relative geocentric inertial coordinate system angular velocity of rotation error, for attitude angle, for the relative geocentric inertial coordinate system angular velocity of rotation of local navigational coordinate system, for velocity error differential vector, for attitude error, f ^bfor the specific force that accelerometer exports, for attitude matrix, f ⁿfor the component of specific force in local navigational coordinate system that accelerometer exports, δ V ⁿfor velocity error, for rotational-angular velocity of the earth, for the angular velocity that navigation is relative earth system, V ⁿfor the speed of the relative earth of carrier, for accelerometer bias vector, for latitude error differential, δ V _nfor north orientation velocity error, R _mand R _nbe respectively the radius-of-curvature of local earth meridian circle and prime vertical, h is height, V _nfor north orientation speed, δ h is height error, for longitude error differential, δ V _efor east orientation velocity error, V _efor east orientation speed, δ L is latitude error, for height error differential, δ V _dfor ground is to velocity error, superscript n represents the projection of this physical quantity in navigational coordinate system, and superscript b represents the projection of this physical quantity in carrier coordinate system.

In one embodiment of the invention, described Kalman observes the governing equation of controller be represented by following formula, and its formula is, $\begin{array}{cc}& {\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}\\ X_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+{\mathrm{\Γ}}_{k-1}{W}_{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\\ Z_k=H_k{X}_k+V_k,& 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 _kfor system state vector, Φ _{k, k-1}for the system state matrix in kth-1 stage, Γ _k-1for the system noise input matrix in kth-1 stage, W _k-1for system noise, Z _kfor systematic observation vector, H _kfor the systematic observation matrix in kth stage, V _kfor kth stage observation noise, for quantity of state estimating system upgrades, for kth-1 stage condition amount is estimated, P _k/k-1for state covariance matrix system update, P _k-1for kth-1 stage condition covariance matrix, for the system state matrix transpose in kth-1 stage, Q _k-1for the system noise in kth-1 stage, for the system noise input matrix transposition in kth-1 stage, K _kfor the ride gain in kth stage, for kth stage system observing matrix transposition, R _kfor kth stage observation covariance matrix, P _kfor kth stage condition covariance matrix, for kth stage condition amount is estimated, Q _kfor kth stage system noise, Z _kfor the observed quantity of kth stage.

The aspect that the present invention adds and advantage will part provide in the following description, and part will become obvious from the following description, or be recognized by practice of the present invention.

Accompanying drawing explanation

The present invention above-mentioned and/or additional aspect and advantage will become obvious and easy understand from the following description of the accompanying drawings of embodiments, wherein:

Fig. 1 is according to an embodiment of the invention for the process flow diagram of the moving alignment method of integrated navigation system;

Fig. 2-Fig. 7 is the data statistics figure of simulation result according to an embodiment of the invention; And

Fig. 8 is according to an embodiment of the invention for the frame diagram of the moving alignment system of integrated navigation system.

Embodiment

Be described below in detail embodiments of the invention, the example of embodiment is shown in the drawings, and wherein same or similar label represents same or similar element or has element that is identical or similar functions from start to finish.Being exemplary below by the embodiment be described with reference to the drawings, only for explaining the present invention, and can not limitation of the present invention being interpreted as.

In describing the invention, it is to be appreciated that term " first ", " second " only for describing object, and can not be interpreted as instruction or hint relative importance or the implicit quantity indicating indicated technical characteristic.Thus, be limited with " first ", the feature of " second " can express or impliedly comprise one or more these features.In describing the invention, the implication of " multiple " is two or more, unless otherwise expressly limited specifically.

Fig. 3 is the process flow diagram of the self-organizing network method based on moonlet of the embodiment of the present invention.As shown in Figure 3, according to the self-organizing network method based on moonlet of the embodiment of the present invention, comprise the following steps:

Step S101, obtain navigation information and GPS observation information, wherein, navigation information is obtained by Inertial Measurement Unit, and carries out inertial navigation to navigation information and resolve.

Particularly, only need adopt linearized stability model when designing the observation controller of moving alignment algorithm, namely the linearization of former nonlinear system near initial point launches.Because the initial alignment time is shorter, zero inclined change of gyro and accelerometer is little.Error model and Kalman observe controller adopt at least 9 dimension quantity of states, reduce dimension to simplify alignment algorithm, can only adopt 9 dimension error state amounts to carry out observing 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, N, E, D be respectively the north orientation of local navigational coordinate system, east orientation and ground to, φ is attitude angle, and δ is that error represents, be respectively north orientation, east orientation and ground to velocity error, L, λ and h are respectively latitude, longitude and are highly, δ L, δ λ, and δ h is respectively latitude error, longitude error and height error, and T is that matrix transpose represents.

In one embodiment of the invention, error model is represented by following formula, and its formula is,

, wherein, for attitude differential vector, ε ⁿfor gyro zero vector partially, for local navigational coordinate system relative geocentric inertial coordinate system angular velocity of rotation error, for attitude angle, for the relative geocentric inertial coordinate system angular velocity of rotation of local navigational coordinate system, for velocity error differential vector, for attitude error, f ^bfor the specific force that accelerometer exports, for attitude matrix, f ⁿfor the component of specific force in local navigational coordinate system that accelerometer exports, δ V ⁿfor velocity error, for rotational-angular velocity of the earth, for the angular velocity that navigation is relative earth system, V ⁿfor the speed of the relative earth of carrier, for accelerometer bias vector, for latitude error differential, δ V _nfor north orientation velocity error, R _mand R _nbe respectively the radius-of-curvature of local earth meridian circle and prime vertical, h is height, V _nfor north orientation speed, δ h is height error, for longitude error differential, δ V _efor east orientation velocity error, V _efor east orientation speed, δ L is latitude error, for height error differential, δ V _dfor ground is to velocity error, superscript n represents the projection of this physical quantity in navigational coordinate system, and superscript b represents the projection of this physical quantity in carrier coordinate system.

Step S103, sets up Kalman and observes controller, and wherein, Kalman observes the work of controller be divided into multiple stage, and the corresponding state covariance matrix of each stage.

Fig. 1 and Fig. 2 is respectively filter construction and Kalman according to an embodiment of the invention and observes the structural representation of controller.As depicted in figs. 1 and 2, Kalman observes controller be carry out improvement by wave filter to form.

Particularly, Kalman observes the governing equation of controller be represented by following formula, and its formula is, $\begin{array}{cc}& {\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}\\ X_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+{\mathrm{\Γ}}_{k-1}{W}_{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\\ Z_k=H_k{X}_k+V_k,& 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 _kfor system state vector, Φ _{k, k-1}for the system state matrix in kth-1 stage, Γ _k-1for the system noise input matrix in kth-1 stage, W _k-1for system noise, Z _kfor systematic observation vector, H _kfor the systematic observation matrix in kth stage, V _kfor kth stage observation noise, for quantity of state estimating system upgrades, for kth-1 stage condition amount is estimated, P _k/k-1for state covariance matrix system update, P _k-1for kth-1 stage condition covariance matrix, for the system state matrix transpose in kth-1 stage, Q _k-1for the system noise in kth-1 stage, for the system noise input matrix transposition in kth-1 stage, K _kfor the ride gain in kth stage, for kth stage system observing matrix transposition, R _kfor kth stage observation covariance matrix, P _kfor kth stage condition covariance matrix, for kth stage condition amount is estimated, Q _kfor kth stage system noise, Z _kfor the observed quantity of kth stage.

Step S104, Kalman observes controller according to navigation information, GPS observation information and error model, calculates the ride gain in each stage in multiple stage.

Particularly, first, by the system state amount in kth-1 stage do system update, be namely multiplied by the intermediate quantity that system state matrix obtains kth-1 stage and kth stage then to system covariance matrix P _k-1carry out the intermediate quantity P that system update obtains kth-1 stage and kth stage _k/k-1, utilize this intermediate quantity and systematic observation matrix and observation noise to estimate the ride gain in kth stage according to above formula.

Step S105, judges whether ride gain meets Leah Pu Nuofu stability condition, if do not meet, then returns step S3, if meet, then enters step S6.Wherein, Leah Pu Nuofu stability condition is if initial point equilibrium state is stable under Lyapunov Meaning, and when time t is tending towards infinity disturbed motion Φ (t; x ₀, t ₀) converge to equilibrium state x _ε=0, and in this process, do not depart from S (ε), then claim system balancing state to be asymptotically stable.A ball territory of wherein S (ε) represents in state space to take initial point as the centre of sphere with ε be radius, Φ represents system equation, and x represents system state amount.

Step S106, according to ride gain calculation compensation vector, and feeds back to Inertial Measurement Unit by compensation vector, and is adjusted by Inertial Measurement Unit.

Particularly, the kth stage system quantity of state will solved be brought in inertial navigation algorithm, by system in inertial navigation algorithm attitude angle, deduct to numerical value such as speed, Latitude-Longitude height north orientation east orientation then complete the correction to system.

According to the method for the embodiment of the present invention, by adopting Kalman to observe controller, rapidly course angle error can be forced down very little scope, thus greatly reduce the aligning time, improve alignment precision.

In order to verify the validity of the present embodiment, by utilizing Desired Track, the motor-driven lower moving alignment of difference is emulated.Its simulated conditions is, gyro offset 50deg/h, gyro noise 200deg/h, accelerometer skew 1mg, and accelerometer noise 10mg, GPS location noise criteria difference is 2m, GPS velocity survey noise criteria difference is 0.2m/s.Angle, initial heading is set as that 45 degree, 135 degree ,-135 are spent and-45 degree respectively, and initial attitude angle is 0 degree.Coordinate turn process is: aircraft flies at a constant speed in direction northeastward along 45 degree of initial angles, and initial velocity is 200m/s, first 1 minute roll angle 30 degree, keeps the turning process of 6 minutes afterwards, within last 1 minute, recovers level.Become accelerate linear motion into: aircraft is along 45 degree of initial angles direction rectilinear motion northeastward, and initial line speed is 20m/s, and within first 4 minutes, acceleration is, within latter four minutes, acceleration is.Utilize the emulation that above-mentioned flight path carries out 8 minutes respectively, employing to be aligned on basis that Kalman observes controller points three sections, every section of 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.As shown in Figures 4 to 7, wherein Fig. 4 gives the image of aircraft longitude and latitude and north orientation speed thereof under coordinate turn flight path.Fig. 5 gives under coordinate turn flight path, adopts general filtering mode respectively and based on observer controller two kinds of modes, under providing angle, the initial heading situation of 45 ° ,-45 °, 135 ° ,-135 ° respectively, and the convergence situation of course angle.Fig. 6 gives the image of aircraft longitude and latitude and north orientation speed thereof under variable accelerated motion flight path, Fig. 7 gives under variable accelerated motion flight path, adopt general filtering mode respectively and based on observer controller two kinds of modes, under providing angle, the initial heading situation of 45 ° ,-45 °, 135 ° ,-135 ° respectively, the convergence situation of course angle.Under two kinds of conditions, the angle, initial heading of Desired Track is 45 °.Can find out according to above-mentioned simulation result, in large misalignment angle situation, the Kalman adopted observes the performance of controller alignment algorithm greatly will be better than general filtering algorithm for estimating, especially in the starting stage, when the angle, initial heading provided is-135 °, with when Desired Track differs 180 °, general filtering algorithm for estimating slowly can only be restrained from large misalignment angle state, and course angle error can be forced down rapidly very little scope by the present embodiment algorithm, greatly reduce the aligning time, improve alignment precision.

Fig. 8 is according to an embodiment of the invention for the frame diagram of the moving alignment system of integrated navigation system.As shown in Figure 8, comprise acquisition module 100, first according to the moving alignment system for integrated navigation system of the embodiment of the present invention to set up module 200, second and set up module 300, computing module 400, judge module 500 and feedback adjusting module 600.

Acquisition module 100 is for obtaining navigation information and GPS observation information, and wherein, navigation information is obtained by Inertial Measurement Unit, and carries out inertial navigation to navigation information and resolve.

Particularly, only need adopt linearized stability model when designing the observation controller of moving alignment algorithm, namely the linearization of former nonlinear system near initial point launches.Because the initial alignment time is shorter, zero inclined change of gyro and accelerometer is little.Error model and Kalman observe controller adopt at least 9 dimension quantity of states, reduce dimension to simplify alignment algorithm, can only adopt 9 dimension error state amounts to carry out observing the foundation of controller.

First sets up module 200 for setting up the error model of moving alignment.

Particularly, the quantity of state of error model is expressed as, wherein, N, E, D be respectively the north orientation of local navigational coordinate system, east orientation and ground to, φ is attitude angle, and δ is that error represents, be respectively north orientation, east orientation and ground to velocity error, L, λ and h are respectively latitude, longitude and are highly, δ L, δ λ, and δ h is respectively latitude error, longitude error and height error, and T is that matrix transpose represents.In one embodiment of the invention, error model is represented by following formula, and its formula is, wherein, for attitude differential vector, ε ⁿfor gyro zero vector partially, for local navigational coordinate system relative geocentric inertial coordinate system angular velocity of rotation error, for attitude angle, for the relative geocentric inertial coordinate system angular velocity of rotation of local navigational coordinate system, for velocity error differential vector, for attitude error, f ^bfor the specific force that accelerometer exports, for attitude matrix, f ⁿfor the component of specific force in local navigational coordinate system that accelerometer exports, δ V ⁿfor velocity error, for rotational-angular velocity of the earth, for the angular velocity that navigation is relative earth system, V ⁿfor the speed of the relative earth of carrier, for accelerometer bias vector, for latitude error differential, δ V _nfor north orientation velocity error, R _mand R _nbe respectively the radius-of-curvature of local earth meridian circle and prime vertical, h is height, V _nfor north orientation speed, δ h is height error, for longitude error differential, δ V _efor east orientation velocity error, V _efor east orientation speed, δ L is latitude error, for height error differential, δ V _dfor ground is to velocity error, superscript n represents the projection of this physical quantity in navigational coordinate system, and superscript b represents the projection of this physical quantity in carrier coordinate system.

Second sets up module 300 observes controller for setting up Kalman, and wherein, Kalman observes the work of controller be divided into multiple stage, and the corresponding state covariance matrix of each stage.

Fig. 1 and Fig. 2 is respectively filter construction and Kalman according to an embodiment of the invention and observes the structural representation of controller.As depicted in figs. 1 and 2, Kalman observes controller be carry out improvement by wave filter to form.

Particularly, Kalman observes the governing equation of controller be represented by following formula, and its formula is, $\begin{array}{cc}& {\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}\\ X_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+{\mathrm{\Γ}}_{k-1}{W}_{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\\ Z_k=H_k{X}_k+V_k,& 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 _kfor system state vector, Φ _{k, k-1}for the system state matrix in kth-1 stage, Γ _k-1for the system noise input matrix in kth-1 stage, W _k-1for system noise, Z _kfor systematic observation vector, H _kfor the systematic observation matrix in kth stage, V _kfor kth stage observation noise, for quantity of state estimating system upgrades, for kth-1 stage condition amount is estimated, P _k/k-1for state covariance matrix system update, P _k-1for kth-1 stage condition covariance matrix, for the system state matrix transpose in kth-1 stage, Q _k-1for the system noise in kth-1 stage, for the system noise input matrix transposition in kth-1 stage, K _kfor the ride gain in kth stage, for kth stage system observing matrix transposition, R _kfor kth stage observation covariance matrix, P _kfor kth stage condition covariance matrix, for kth stage condition amount is estimated, Q _kfor kth stage system noise, Z _kfor the observed quantity of kth stage.

Computing module 400 observes controller according to navigation information, GPS observation information and error model for Kalman, calculates the ride gain in each stage in multiple stage.

Particularly, first, by the system state amount in kth-1 stage do system update, be namely multiplied by the intermediate quantity that system state matrix obtains kth-1 stage and kth stage then to system covariance matrix P _k-1carry out the intermediate quantity P that system update obtains kth-1 stage and kth stage _k/k-1, utilize this intermediate quantity and systematic observation matrix and observation noise to estimate the ride gain in kth stage according to above formula.

Judge module 500, for judging whether ride gain meets Leah Pu Nuofu stability condition, if do not meet, then sets up module by second and computing module re-starts process.Wherein, Leah Pu Nuofu stability condition is if initial point equilibrium state is stable under Lyapunov Meaning, and when time t is tending towards infinity disturbed motion Φ (t; x ₀, t ₀) converge to equilibrium state x _ε=0, and in this process, do not depart from S (ε), then claim system balancing state to be asymptotically stable.A ball territory of wherein S (ε) represents in state space to take initial point as the centre of sphere with ε be radius, Φ represents system equation, and x represents system state amount.

Compensation vector for vectorial according to ride gain calculation compensation, and is fed back to Inertial Measurement Unit by feedback adjusting module 600, and is adjusted by Inertial Measurement Unit.

Particularly, the kth stage system quantity of state will solved be brought in inertial navigation algorithm, by system in inertial navigation algorithm attitude angle, deduct to numerical value such as speed, Latitude-Longitude height north orientation east orientation then complete the correction to system.

According to the system of the embodiment of the present invention, by adopting Kalman to observe controller, rapidly course angle error can be forced down very little scope, thus greatly reduce the aligning time, improve alignment precision.

In order to verify the validity of the present embodiment, by utilizing Desired Track, the motor-driven lower moving alignment of difference is emulated.Its simulated conditions is, gyro offset 50deg/h, gyro noise 200deg/h, accelerometer skew 1mg, and accelerometer noise 10mg, GPS location noise criteria difference is 2m, GPS velocity survey noise criteria difference is 0.2m/s.Angle, initial heading is set as that 45 degree, 135 degree ,-135 are spent and-45 degree respectively, and initial attitude angle is 0 degree.Coordinate turn process is: aircraft flies at a constant speed in direction northeastward along 45 degree of initial angles, and initial velocity is 200m/s, first 1 minute roll angle 30 degree, keeps the turning process of 6 minutes afterwards, within last 1 minute, recovers level.Become accelerate linear motion into: aircraft is along 45 degree of initial angles direction rectilinear motion northeastward, and initial line speed is 20m/s, and within first 4 minutes, acceleration is, within latter four minutes, acceleration is.Utilize the emulation that above-mentioned flight path carries out 8 minutes respectively, employing to be aligned on basis that Kalman observes controller points three sections, every section of 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.As shown in Figures 4 to 7, wherein Fig. 4 gives the image of aircraft longitude and latitude and north orientation speed thereof under coordinate turn flight path.Fig. 5 gives under coordinate turn flight path, adopts general filtering mode respectively and based on observer controller two kinds of modes, under providing angle, the initial heading situation of 45 ° ,-45 °, 135 ° ,-135 ° respectively, and the convergence situation of course angle.Fig. 6 gives the image of aircraft longitude and latitude and north orientation speed thereof under variable accelerated motion flight path, Fig. 7 gives under variable accelerated motion flight path, adopt general filtering mode respectively and based on observer controller two kinds of modes, under providing angle, the initial heading situation of 45 ° ,-45 °, 135 ° ,-135 ° respectively, the convergence situation of course angle.Under two kinds of conditions, the angle, initial heading of Desired Track is 45 °.Can find out according to above-mentioned simulation result, in large misalignment angle situation, the Kalman adopted observes the performance of controller alignment algorithm greatly will be better than general filtering algorithm for estimating, especially in the starting stage, when the angle, initial heading provided is-135 °, with when Desired Track differs 180 °, general filtering algorithm for estimating slowly can only be restrained from large misalignment angle state, and course angle error can be forced down rapidly very little scope by the present embodiment algorithm, greatly reduce the aligning time, improve alignment precision.

Should be appreciated that the modules in system embodiment of the present invention can be identical with the description in embodiment of the method with the specific operation process of unit, be not described in detail herein.

Although illustrate and describe embodiments of the invention above, be understandable that, above-described embodiment is exemplary, can not be interpreted as limitation of the present invention, those of ordinary skill in the art can change above-described embodiment within the scope of the invention when not departing from principle of the present invention and aim, revising, replacing and modification.

Claims (10)

1., for a moving alignment method for integrated navigation system, it is characterized in that, comprise the following steps:

S1: obtain navigation information and GPS observation information, wherein, described navigation information is obtained by Inertial Measurement Unit, and inertial navigation is carried out to described navigation information resolve;

S2: the error model setting up moving alignment;

S3: set up Kalman and observe controller, wherein, described Kalman observes the work of controller be divided into multiple stage, and the corresponding state covariance matrix of each stage;

S4: Kalman observes controller according to described navigation information, GPS observation information and error model, calculates the ride gain in each stage in described multiple stage;

S5: judge whether described ride gain meets Leah Pu Nuofu stability condition, if do not meet, then returns step S3, if meet, then enters step S6;

S6: according to described ride gain calculation compensation vector, and described compensation vector is fed back to described Inertial Measurement Unit, and adjusted by described Inertial Measurement Unit.

2. as claimed in claim 1 for the moving alignment method of integrated navigation system, it is characterized in that, described error model and Kalman observe controller adopt at least 9 dimension quantity of states.

3., as claimed in claim 1 for the moving alignment method of integrated navigation system, it is characterized in that, the quantity of state of described error model is expressed as,

Wherein, N, E, D be respectively the north orientation of local navigational coordinate system, east orientation and ground to, for attitude angle, δ is that error represents, be respectively north orientation, east orientation and ground to velocity error, L, λ and h be respectively latitude, longitude and height, δ L, δ λ, δ h is respectively latitude error, longitude error and height error, and T is that matrix transpose represents.

4., as claimed in claim 1 for the moving alignment method of integrated navigation system, it is characterized in that, described error model is represented by following formula, and its formula is,

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

$\begin{array}{c}\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}\end{array}$

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

Wherein, for attitude differential vector, ε ⁿfor gyro zero vector partially, for local navigational coordinate system relative geocentric inertial coordinate system angular velocity of rotation error, for attitude angle, for the relative geocentric inertial coordinate system angular velocity of rotation of local navigational coordinate system, for velocity error differential vector, for attitude error, f ^bfor the specific force that accelerometer exports, for attitude matrix, f ⁿfor the component of specific force in local navigational coordinate system that accelerometer exports, δ V ⁿfor velocity error, for rotational-angular velocity of the earth, for the angular velocity that navigation is relative earth system, V ⁿfor the speed of the relative earth of carrier, for accelerometer bias vector, for latitude error differential, δ V _nfor north orientation velocity error, R _mand R _nbe respectively the radius-of-curvature of local earth meridian circle and prime vertical, h is height, V _nfor north orientation speed, δ h is height error, for longitude error differential, δ V _efor east orientation velocity error, V _efor east orientation speed, δ L is latitude error, for height error differential, δ V _dfor ground is to velocity error, superscript n represents the projection of this physical quantity in navigational coordinate system, and superscript b represents the projection of this physical quantity in carrier coordinate system.

5., as claimed in claim 1 for the moving alignment method of integrated navigation system, it is characterized in that, described Kalman observes the governing equation of controller be represented by following formula, and its formula is,

${\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}$

$X_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+{\mathrm{\Γ}}_{k-1}{W}_{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$

$Z_k=H_k{X}_k+V_k,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})$

Wherein, X _kfor system state vector, Φ _{k, k-1}for the system state matrix in kth-1 stage, Γ _k-1for the system noise input matrix in kth-1 stage, W _k-1for system noise, Z _kfor systematic observation vector, H _kfor the systematic observation matrix in kth stage, V _kfor kth stage observation noise, for quantity of state estimating system upgrades, for kth-1 stage condition amount is estimated, P _k/k-1for state covariance matrix system update, P _k-1for kth-1 stage condition covariance matrix, for the system state matrix transpose in kth-1 stage, Q _k-1for the system noise in kth-1 stage, for the system noise input matrix transposition in kth-1 stage, K _kfor the ride gain in kth stage, for kth stage system observing matrix transposition, R _kfor kth stage observation covariance matrix, P _kfor kth stage condition covariance matrix, for kth stage condition amount is estimated, Q _kfor kth stage system noise, Z _kfor the observed quantity of kth stage.

6., for a moving alignment system for integrated navigation system, it is characterized in that, comprising:

Acquisition module, for obtaining navigation information and GPS observation information, wherein, described navigation information is obtained by Inertial Measurement Unit, and carries out inertial navigation to described navigation information and resolve;

First sets up module, for setting up the error model of moving alignment;

Second sets up module, observes controller for setting up Kalman, and wherein, described Kalman observes the work of controller be divided into multiple stage, and the corresponding state covariance matrix of each stage;

Computing module, observes controller according to described navigation information, GPS observation information and error model for Kalman, calculates the ride gain in each stage in described multiple stage;

Judge module, for judging whether described ride gain meets Leah Pu Nuofu stability condition, if do not meet, then sets up module by second and computing module re-starts process;

Feedback adjusting module, for vectorial according to described ride gain calculation compensation, and feeds back to described Inertial Measurement Unit by described compensation vector, and is adjusted by described Inertial Measurement Unit.

7. as claimed in claim 6 for the moving alignment system of integrated navigation system, it is characterized in that, described error model and Kalman observe controller adopt at least 9 dimension quantity of states.

8., as claimed in claim 6 for the moving alignment system of integrated navigation system, it is characterized in that, the quantity of state of described error model is expressed as,

Wherein, N, E, D be respectively the north orientation of local navigational coordinate system, east orientation and ground to, for attitude angle, δ is that error represents, be respectively north orientation, east orientation and ground to velocity error, L, λ and h be respectively latitude, longitude and height, δ L, δ λ, δ h is respectively latitude error, longitude error and height error, and T is that matrix transpose represents.

9., as claimed in claim 6 for the moving alignment system of integrated navigation system, it is characterized in that, described error model is represented by following formula, and its formula is,

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

$\begin{array}{c}\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}\end{array}$

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

Wherein, for attitude differential vector, ε ⁿfor gyro zero vector partially, for local navigational coordinate system relative geocentric inertial coordinate system angular velocity of rotation error, for attitude angle, for the relative geocentric inertial coordinate system angular velocity of rotation of local navigational coordinate system, for velocity error differential vector, for attitude error, f ^bfor the specific force that accelerometer exports, for attitude matrix, f ⁿfor the component of specific force in local navigational coordinate system that accelerometer exports, δ V ⁿfor velocity error, for rotational-angular velocity of the earth, for the angular velocity that navigation is relative earth system, V ⁿfor the speed of the relative earth of carrier, for accelerometer bias vector, for latitude error differential, δ V _nfor north orientation velocity error, R _mand R _nbe respectively the radius-of-curvature of local earth meridian circle and prime vertical, h is height, V _nfor north orientation speed, δ h is height error, for longitude error differential, δ V _efor east orientation velocity error, V _efor east orientation speed, δ L is latitude error, for height error differential, δ V _dfor ground is to velocity error, superscript n represents the projection of this physical quantity in navigational coordinate system, and superscript b represents the projection of this physical quantity in carrier coordinate system.

10., as claimed in claim 6 for the moving alignment system of integrated navigation system, it is characterized in that, described Kalman observes the governing equation of controller be represented by following formula, and its formula is,

${\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}$

$X_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+{\mathrm{\Γ}}_{k-1}{W}_{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$

$Z_k=H_k{X}_k+V_k,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})$

Wherein, X _kfor system state vector, Φ _{k, k-1}for the system state matrix in kth-1 stage, Γ _k-1for the system noise input matrix in kth-1 stage, W _k-1for system noise, Z _kfor systematic observation vector, H _kfor the systematic observation matrix in kth stage, V _kfor kth stage observation noise, for quantity of state estimating system upgrades, for kth-1 stage condition amount is estimated, P _k/k-1for state covariance matrix system update, P _k-1for kth-1 stage condition covariance matrix, for the system state matrix transpose in kth-1 stage, Q _k-1for the system noise in kth-1 stage, for the system noise input matrix transposition in kth-1 stage, K _kfor the ride gain in kth stage, for kth stage system observing matrix transposition, R _kfor kth stage observation covariance matrix, P _kfor kth stage condition covariance matrix, for kth stage condition amount is estimated, Q _kfor kth stage system noise, Z _kfor the observed quantity of kth stage.