CN103983277B - A kind of inertial navigation system synthesis correction method being applicable to polar region - Google Patents

Abstract

The invention discloses and be capable of polar navigation vehicle under the conditions of undamped, carry out a kind of inertial navigation system synthesis correction method being applicable to polar region of integrated calibration.This method, by using horizontal geographic coordinate system as navigational coordinate system, is changed by abscissa system in the range of polar region, is transformed under horizontal geographic coordinate system by navigational parameter.Using the inertial navigation output error amount under horizontal geographic coordinate system as quantity of state, speed is as external observation amount, set up Kalman filter model, estimate horizontal error angle and be incorporated in tradition two point correction algorithm, under undamped status condition, completing polar region inertial navigation system integrated calibration process.The present invention realizes polar navigation vehicle integrated calibration under the conditions of undamped, relieves the restriction to vehicle navigation mode of the tradition integrated calibration scheme, improves the motility of polar region inertial navigation system work.

Description

A kind of inertial navigation system synthesis correction method being applicable to polar region

Technical field

The invention belongs to a kind of navigation system integrated calibration technology, in particular to realizing polar navigation vehicle at undamped bar A kind of inertial navigation system synthesis correction method being applicable to polar region of integrated calibration is carried out under part.

Background technology

Along with the development to arctic regions scientific investigation, the importance of polar region is increasingly paid attention to by countries in the world.No matter from ground From the point of view of matter resource, strategic importance or shipping trade, there is important meaning polar region for the development of China.For navigation in pole For the boats and ships in district, the accuracy guarantee of navigation is most important.For ship's inertial navigation system, its difficulty is that cycle of operation Long, required precision is high, it is necessary to periodically system is carried out biharmonic correction.The alignment technique of current platform formula inertial navigation system compares into Ripe, but traditional method requires that boats and ships necessarily be in state of at the uniform velocity sailing through to when carrying out integrated calibration, and otherwise system cannot meet easypro Strangle oscillating condition, produce Spraying machine.This is not-so-practical for navigation boats and ships under polar region complex environment.

Inertial navigation system integrated calibration technology has been done numerous studies work by the most a lot of scholars.In et al. for carrier movement Impact, it is proposed that the integrated calibration algorithm under undamped state, this algorithm needs inertial navigation system to carry out state switching, it is ensured that carrier Time motor-driven, system is in undamped state, weakens the motor-driven impact caused, but its method is to sit for geography based on navigational coordinate system Mark system it is assumed that these are for being operated in polar region SINS and inapplicable；Harbin Engineering University it is proposed that Use position and course information to carry out gyroscopic drift and the method for course error correction under inertial system, but the method is built upon passing In system coordinate frame, boats and ships, during polar navigation, can not carry out navigation calculation because latitude is too high.

Therefore, research can effectively reduce the requirement to carrier maneuver mode and can be applicable to the synthesis correction method of polar region for The navigation marine navigator in polar region is significant and practical value.

Summary of the invention

It is an object of the invention under the conditions of undamped, to realize polar navigation vehicle integrated calibration, therefore devise a kind of suitable Inertial navigation system synthesis correction method for polar region.

The present invention is achieved by the following technical solutions:

A kind of inertial navigation system synthesis correction method being applicable to polar region, including following step:

Step one: the navigational parameter amount that ship's inertial navigation system exports being transformed under horizontal geographic coordinate system, navigational parameter includes: longitude and latitude Value, course, speed and attitude information；

Geographic coordinate system with the transition matrix of horizontal geographic coordinate system is:

$C_n^{\stackrel{\‾}{n}}=C_n^{\stackrel{\‾}{n}}\·C_n^i$

$C_i^{\stackrel{\‾}{n}}=\left[\begin{array}{ccc}\cos \stackrel{\‾}{\mathrm{\λ}}\cos \mathrm{\Ωt}& \cos \stackrel{\‾}{\mathrm{\λ}}\sin \mathrm{\Ωt}& -\sin \stackrel{-}{\mathrm{\λ}}\\ -\sin \stackrel{\‾}{L}\sin \stackrel{\‾}{\mathrm{\λ}}\cos \mathrm{\Ωt}-\cos \stackrel{\‾}{L}\sin \mathrm{\Ωt}& \cos \stackrel{\‾}{L}\cos \mathrm{\Ωt}-\sin \stackrel{\‾}{L}\sin \stackrel{\‾}{\mathrm{\λ}}\sin \mathrm{\Ωt}& -\sin \stackrel{\‾}{L}\cos \stackrel{\‾}{\mathrm{\λ}}\\ \cos \stackrel{\‾}{L}\sin \stackrel{\‾}{\mathrm{\λ}}\cos \mathrm{\Ωt}-\sin \stackrel{\‾}{L}\sin \mathrm{\Ωt}& \cos \stackrel{\‾}{L}\sin \stackrel{\‾}{\mathrm{\λ}}\sin \mathrm{\Ωt}+\sin \stackrel{\‾}{L}\cos \mathrm{\Ωt}& \cos \stackrel{-}{L}\cos \stackrel{\‾}{\mathrm{\λ}}\end{array}\right]$

$C_n^i=\left[\begin{array}{ccc}-\sin (\mathrm{\λ}+\mathrm{\Ωt})& -\sin L\cos (\mathrm{\λ}+\mathrm{\Ωt})& \cos L\cos (\mathrm{\λ}+\mathrm{\Ωt})\\ \cos (\mathrm{\λ}+\mathrm{\Ωt})& -\sin L\sin (\mathrm{\λ}+\mathrm{\Ωt})& \cos L\sin (\mathrm{\λ}+\mathrm{\Ωt})\\ 0& \cos L& \sin L\end{array}\right]$

Represent that inertial coordinate is tied to manage breadthways the transition matrix of coordinate system,Represent that geographical coordinate is tied to the conversion square of inertial coodinate system Battle array, Ω is rotational-angular velocity of the earth, and t is the time, and λ is the longitude under geographic coordinate system,

Latitude and longitude value under horizontal geographic coordinate system is:

$\stackrel{\‾}{L}=\arcsin \left(\cos L\sin \mathrm{\λ}\right)$

$\stackrel{\‾}{\mathrm{\λ}}=\arctan \left(\frac{\cos L\cos \mathrm{\λ}}{\sin L}\right)$

L is the latitude value under geographic coordinate system,For the latitude value under horizontal geographic coordinate system,For the longitude under horizontal geographic coordinate system, Under horizontal geographic coordinate system, course with speed is:

$\stackrel{\‾}{K}=K+\arctan \frac{\sin \mathrm{\λ}}{\sin L\cos \mathrm{\λ}}-\mathrm{\π}$

$\stackrel{\‾}{V}=\left[\begin{array}{c}{\stackrel{\‾}{V}}_x\\ {\stackrel{\‾}{V}}_y\end{array}\right]=\left[\begin{array}{c}V\sin \stackrel{\‾}{K}\\ V\cos \stackrel{\‾}{K}\end{array}\right]$

K represents the course under geographic coordinate system,For the course under horizontal geographic coordinate system, π is pi, and V is under geographic coordinate system Speed,For the speed under horizontal geographic coordinate system,For the east orientation speed under abscissa system,For the north orientation under abscissa system Speed；

Step 2: the navigation mechanization utilizing the navigational parameter under the abscissa system obtained to carry out under abscissa system, determines the shape of system State variable and measurement；

Step 3: state variable and measurement according to system set up Kalman filter, kalman filter method obtain abscissa system Under the x-axis attitude error angle of navigation systemWith y-axis attitude error angle

Step 4: the x-axis attitude error angle that will obtainWith y-axis attitude error angleIt is incorporated in the two point correction of navigation system, complete Become polar region two point correction.

A kind of inertial navigation system synthesis correction method being applicable to polar region of the present invention can also include:

1, in step 2, navigation mechanization is:

$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{\·}{\stackrel{\‾}{L}}=\frac{\mathrm{\δ}{\stackrel{\‾}{V}}_y}{R}\\ \mathrm{\δ}\stackrel{\·}{\stackrel{\‾}{\mathrm{\λ}}}=\frac{{\stackrel{\‾}{V}}_x}{R}\mathrm{\δ}\stackrel{\‾}{L}\tan \stackrel{\‾}{L}\sec \stackrel{\‾}{L}+\frac{\mathrm{\δ}{\stackrel{\‾}{V}}_x}{R}\sec \stackrel{\‾}{L}\\ \mathrm{\δ}{\stackrel{\·}{\stackrel{\‾}{V}}}_x={\stackrel{\‾}{\mathrm{\φ}}}_z{\stackrel{\‾}{f}}_y+\frac{{\stackrel{\‾}{V}}_y}{R}\tan \stackrel{\‾}{L}\·\mathrm{\δ}{\stackrel{\‾}{V}}_x+(2\mathrm{\Ω}\sin \stackrel{\‾}{L}+\frac{{\stackrel{\‾}{V}}_x}{R}\tan \stackrel{\‾}{L})\·\mathrm{\δ}{\stackrel{\‾}{V}}_y+(2\mathrm{\Ω}\cos \stackrel{\‾}{L}{\·\stackrel{\‾}{V}}_y+\frac{{\stackrel{\‾}{V}}_x{\stackrel{\‾}{V}}_y}{R}{\sec }^2\stackrel{\‾}{L})\·\mathrm{\δ}\stackrel{\‾}{L}+\mathrm{\Δ}{\stackrel{\‾}{A}}_x\\ \mathrm{\δ}{\stackrel{\·}{\stackrel{\‾}{V}}}_y=-{\stackrel{\‾}{\mathrm{\φ}}}_z{\stackrel{\‾}{f}}_x-(2\mathrm{\Ω}\sin \stackrel{\‾}{L}+\frac{2{\stackrel{\‾}{V}}_x}{R}\tan \stackrel{\‾}{L})\·\mathrm{\δ}{\stackrel{\‾}{V}}_x-(2\mathrm{\Ω}\cos \stackrel{\‾}{L}{\stackrel{\‾}{V}}_x+\frac{{\left({\stackrel{\‾}{V}}_x\right)}^2}{R})\·\mathrm{\δ}\stackrel{\‾}{L}+\mathrm{\Δ}{\stackrel{\‾}{A}}_y\\ {\stackrel{\·}{\stackrel{\‾}{\mathrm{\φ}}}}_x=-\frac{\mathrm{\δ}{\stackrel{\‾}{V}}_y}{R}+(\mathrm{\Ω}\sin \stackrel{\‾}{L}+\frac{{\stackrel{\‾}{V}}_x\tan \stackrel{\‾}{L}}{R}){\stackrel{\‾}{\mathrm{\φ}}}_y-(\mathrm{\Ω}\cos \stackrel{\‾}{L}+\frac{{\stackrel{\‾}{V}}_x}{R}){\stackrel{\‾}{\mathrm{\φ}}}_z+{\stackrel{\‾}{\mathrm{\ϵ}}}_x\\ {\stackrel{\·}{\stackrel{\‾}{\mathrm{\φ}}}}_y=\frac{\mathrm{\δ}{\stackrel{\‾}{V}}_x}{R}-\mathrm{\Ω}\sin \stackrel{\‾}{L}\mathrm{\δ}\stackrel{\‾}{L}-(\mathrm{\Ω}\sin \stackrel{\‾}{L}+\frac{{\stackrel{\‾}{V}}_x\tan \stackrel{\‾}{L}}{R}){\stackrel{\‾}{\mathrm{\φ}}}_x-\frac{{\stackrel{\‾}{V}}_y}{R}{\stackrel{\‾}{\mathrm{\φ}}}_z+{\stackrel{\‾}{\mathrm{\ϵ}}}_y\\ {\stackrel{\·}{\stackrel{\‾}{\mathrm{\φ}}}}_z=\frac{\mathrm{\δ}{\stackrel{\‾}{V}}_x}{R}\tan \stackrel{\‾}{L}+\frac{{\stackrel{\‾}{V}}_y}{R}{\stackrel{\‾}{\mathrm{\φ}}}_y+(\mathrm{\Ω}\cos \stackrel{\‾}{L}+\frac{{\stackrel{\‾}{V}}_x}{R}{\sec }^2\stackrel{\‾}{L}+\frac{{\stackrel{\‾}{V}}_x}{R}){\stackrel{\‾}{\mathrm{\φ}}}_x+{\stackrel{\‾}{\mathrm{\ϵ}}}_z\end{array}\right.$

It is respectively horizontal latitude error and horizontal longitude error,For local horizontal geographic latitude,It is respectively the horizontal stroke of carrier East orientation velocity error and horizontal north orientation velocity error,It is respectively east orientation and north orientation accelerometer error under abscissa system,It is respectively the ratio force information that under abscissa system, accelerometer x-axis, y-axis and z-axis are measured,It is respectively horizontal stroke Carrier x-axis, y-axis and z-axis attitude error angle under coordinate system,It is respectively x-axis, y-axis and z-axis under abscissa system Gyroscopic drift,

The state variable of system is:

$X={\left[\begin{array}{cccccccccccc}\mathrm{\δ}\stackrel{\‾}{L}& \mathrm{\δ}\stackrel{\‾}{\mathrm{\λ}}& \mathrm{\δ}\stackrel{\‾}{V_x}& \mathrm{\δ}\stackrel{\‾}{V_y}& {\stackrel{\‾}{\mathrm{\φ}}}_x& {\stackrel{\‾}{\mathrm{\φ}}}_y& {\stackrel{\‾}{\mathrm{\φ}}}_z& {\stackrel{\‾}{\mathrm{\ϵ}}}_x& {\stackrel{\‾}{\mathrm{\ϵ}}}_y& {\stackrel{\‾}{\mathrm{\ϵ}}}_z& \mathrm{\Δ}{\stackrel{\‾}{A}}_x& \mathrm{\Δ}{\stackrel{\‾}{A}}_y\end{array}\right]}^T$

The measurement of system is:

$Z=\left[\begin{array}{c}{\stackrel{\‾}{V}}_{\mathrm{sx}}-{\stackrel{\‾}{V}}_{\mathrm{dx}}\\ {\stackrel{\‾}{V}}_{\mathrm{sy}}-{\stackrel{\‾}{V}}_{\mathrm{dy}}\end{array}\right]=\left[\begin{array}{c}\mathrm{\δ}{\stackrel{\‾}{V}}_x\\ \mathrm{\δ}{\stackrel{\‾}{V}}_y\end{array}\right]={\stackrel{\‾}{V}}_s-{\stackrel{\‾}{V}}_d$

For inertial navigation output speed under abscissa system,For electromagnet log output speed under abscissa system,Exist for inertial navigation output speed Horizontal east upward component,For inertial navigation output speed at horizontal north upward component,Divide on horizontal east orientation for electromagnet log output speed Amount,For electromagnet log output speed at horizontal north upward component,For horizontal east orientation velocity error,For horizontal north orientation speed by mistake Difference.

2, in step 3, the Kalman filter of foundation is:

$\left\{\begin{array}{c}\stackrel{\·}{x}=\mathrm{AX}+\mathrm{BW}\\ Z=\mathrm{HX}+v\end{array}\right.$

Wherein, X is the state variable of system, A and B is respectively state matrix and the noise matrix of system, W be system noise to Amount, Z is the measurement vector of system, and H is measurement matrix, and ν is system measurements noise.

3, in step 4, the x-axis attitude error angle that will obtainWith y-axis attitude error angleIt is incorporated in the two point correction of navigation system, Obtain:

$\left[\begin{array}{c}\mathrm{\δ}\stackrel{\‾}{L}+{\widehat{\stackrel{\‾}{\mathrm{\φ}}}}_x\\ \mathrm{\δ}\stackrel{\‾}{\mathrm{\λ}}-\frac{{\widehat{\stackrel{\‾}{\mathrm{\φ}}}}_y}{\cos \stackrel{\‾}{L}}\\ \mathrm{\δ}\stackrel{\‾}{K}-{\widehat{\stackrel{\‾}{\mathrm{\φ}}}}_y\tan \stackrel{\‾}{L}\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& -\sec \stackrel{\‾}{L}& 0\\ 0& -\tan \stackrel{\‾}{L}& 1\end{array}\right]{\left[\begin{array}{c}{\mathrm{\ψ}}_x\\ {\mathrm{\ψ}}_y\\ {\mathrm{\ψ}}_z\end{array}\right]}^{\stackrel{\‾}{n}}$

For the course error under horizontal geographic coordinate system, ψ_x、ψ_y、ψ_zFor inertial navigation system platform drift angle.

Beneficial effects of the present invention:

1, the present invention is by setting up abscissa system, using horizontal geographic coordinate system as polar navigation coordinate system, to the fortune navigated by water in polar region scope When carrier carries out integrated calibration, it is to avoid the problem that singular point occurs during causing because polar region latitude is too high in traditional algorithm calculating, Provide the new approaches of polar region inertial navigation system integrated calibration.

2, the present invention is by setting up the system state equation under abscissa system and measurement equation, utilizes Kalman Filter Technology to estimate inertial navigation System level error angle being incorporated in traditional algorithm improves, and realizes polar region marine integrated correction, solve under the conditions of undamped Except the restriction to carrier navigation mode of the tradition integrated calibration scheme, improve the motility of polar region inertial navigation system work.

Accompanying drawing explanation

Fig. 1 is application Kalman Filter Technology estimating system horizontal error angle principle schematic.

Fig. 2 is polar region inertial navigation system integrated calibration protocol procedures figure.

Detailed description of the invention

Below according to accompanying drawing, the present invention will be further described.

The present invention sets up Kalman filter estimating system horizontal error angle based on external speed observation information under abscissa system, enters One step is incorporated in tradition integrated calibration algorithm, realizes integrated calibration under the conditions of undamped, solves inertial navigation system the most right Carrier navigation mode limits and inapplicable problem in polar region.Can be completed by following steps:

Step one: the navigational parameter that inertial navigation system is exported before starting by corrected time carries out the conversion of abscissa system, sets up under two kinds of coordinate systems Conversion relational expression, it is simple to longitude and latitude positional information, speed, course and attitude information etc. are converted under horizontal geographic coordinate system Parameter amount；

Step 2: the navigational parameter based on horizontal geographic coordinate system utilizing step one to obtain carries out the mechanization under abscissa system；

Step 3: according to the inertial navigation system mechanization under the abscissa system that step 2 obtains, choose the longitude and latitude error under abscissa system, Velocity error, platform error angle, gyroscope constant value drift and acceleration zero deviator, as state variable, choose electromagnet log output The difference of speed and inertial navigation system output speed is as external observation amount；

Step 4: quantity of state and measurement according to step 3 gained set up Kalman filter, and it are carried out discretization, by Kalman Filtering method obtains the horizontal error angle of inertial navigation system under abscissa system

Step 5: step 4 is estimated the horizontal error angle obtainedIt is incorporated in tradition two point correction method, completes polar region pattern Under undamped integrated calibration process.

Below in conjunction with Fig. 1 and Fig. 2, the enforcement step of the present invention is described in detail: this method is by making with horizontal geographic coordinate system For navigational coordinate system, changed by abscissa system in the range of polar region, navigational parameter is transformed under horizontal geographic coordinate system.With horizontal stroke Inertial navigation output error amount under geographic coordinate system as external observation amount, sets up Kalman filter model as quantity of state, speed, Estimate horizontal error angle and be incorporated in tradition two point correction algorithm, under undamped status condition, completing polar region inertial navigation system combine Close trimming process.The method of present invention design is applicable to the integrated calibration under the motor-driven navigation condition in polar region, naval vessel, releases and navigates naval vessel While line mode limits, compensate for the traditional method deficiency at polar navigation integrated calibration,

Step one: before the integrated calibration moment starts, navigational parameter amount (longitude and latitude, speed, the course that ship's inertial navigation system is exported And attitude information) be transformed under horizontal geographic coordinate system.

Longitude and latitude conversion formula is as follows:

$\stackrel{\‾}{L}=\arcsin \left(\cos L\sin \mathrm{\λ}\right)$

$\stackrel{\‾}{\mathrm{\λ}}=\arctan \left(\frac{\cos L\cos \mathrm{\λ}}{\sin L}\right)$

Wherein, L is the latitude value under geographic coordinate system, and λ is the longitude under geographic coordinate system；For the latitude under horizontal geographic coordinate system Angle value,For the longitude under horizontal geographic coordinate system.

Geographic coordinate system with the transition matrix of horizontal geographic coordinate system is:

$C_n^{\stackrel{\‾}{n}}=C_i^{\stackrel{\‾}{n}}\·C_n^i$

Here,

$C_i^{\stackrel{\‾}{n}}=\left[\begin{array}{ccc}\cos \stackrel{\‾}{\mathrm{\λ}}\cos \mathrm{\Ωt}& \cos \stackrel{\‾}{\mathrm{\λ}}\sin \mathrm{\Ωt}& -\sin \stackrel{-}{\mathrm{\λ}}\\ -\sin \stackrel{\‾}{L}\sin \stackrel{\‾}{\mathrm{\λ}}\cos \mathrm{\Ωt}-\cos \stackrel{\‾}{L}\sin \mathrm{\Ωt}& \cos \stackrel{\‾}{L}\cos \mathrm{\Ωt}-\sin \stackrel{\‾}{L}\sin \stackrel{\‾}{\mathrm{\λ}}\sin \mathrm{\Ωt}& -\sin \stackrel{\‾}{L}\cos \stackrel{\‾}{\mathrm{\λ}}\\ \cos \stackrel{\‾}{L}\sin \stackrel{\‾}{\mathrm{\λ}}\cos \mathrm{\Ωt}-\sin \stackrel{\‾}{L}\sin \mathrm{\Ωt}& \cos \stackrel{\‾}{L}\sin \stackrel{\‾}{\mathrm{\λ}}\sin \mathrm{\Ωt}+\sin \stackrel{\‾}{L}\cos \mathrm{\Ωt}& \cos \stackrel{-}{L}\cos \stackrel{\‾}{\mathrm{\λ}}\end{array}\right]$

$C_n^i=\left[\begin{array}{ccc}-\sin (\mathrm{\λ}+\mathrm{\Ωt})& -\sin L\cos (\mathrm{\λ}+\mathrm{\Ωt})& \cos L\cos (\mathrm{\λ}+\mathrm{\Ωt})\\ \cos (\mathrm{\λ}+\mathrm{\Ωt})& -\sin L\sin (\mathrm{\λ}+\mathrm{\Ωt})& \cos L\sin (\mathrm{\λ}+\mathrm{\Ωt})\\ 0& \cos L& \sin L\end{array}\right]$

Wherein,Represent that geographical coordinate is tied to manage breadthways the transition matrix of coordinate system；Represent that inertial coordinate is tied to manage breadthways coordinate system Transition matrix；Represent that geographical coordinate is tied to the transition matrix of inertial coodinate system；Ω is rotational-angular velocity of the earth；T is the time.

Course with the conversion formula of speed is:

$\stackrel{\‾}{K}=K+\arctan \frac{\sin \mathrm{\λ}}{\sin L\cos \mathrm{\λ}}-\mathrm{\π}$

$\stackrel{\‾}{V}=\left[\begin{array}{c}{\stackrel{\‾}{V}}_x\\ {\stackrel{\‾}{V}}_y\end{array}\right]=\left[\begin{array}{c}V\sin \stackrel{\‾}{K}\\ V\cos \stackrel{\‾}{K}\end{array}\right]$

Wherein, the course under K represents geographic coordinate system；For the course under horizontal geographic coordinate system；π is pi；V is geographical seat Speed under mark system；For the speed under horizontal geographic coordinate system；For the east orientation speed under abscissa system；For under abscissa system North orientation speed.

Step 2: carried out the navigation mechanization under abscissa system by the navigational parameter after changing:

$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{\·}{\stackrel{\‾}{L}}=\frac{\mathrm{\δ}{\stackrel{\‾}{V}}_y}{R}\\ \mathrm{\δ}\stackrel{\·}{\stackrel{\‾}{\mathrm{\λ}}}=\frac{{\stackrel{\‾}{V}}_x}{R}\mathrm{\δ}\stackrel{\‾}{L}\tan \stackrel{\‾}{L}\sec \stackrel{\‾}{L}+\frac{\mathrm{\δ}{\stackrel{\‾}{V}}_x}{R}\sec \stackrel{\‾}{L}\\ \mathrm{\δ}{\stackrel{\·}{\stackrel{\‾}{V}}}_x={\stackrel{\‾}{\mathrm{\φ}}}_z{\stackrel{\‾}{f}}_y+\frac{{\stackrel{\‾}{V}}_y}{R}\tan \stackrel{\‾}{L}\·\mathrm{\δ}{\stackrel{\‾}{V}}_x+(2\mathrm{\Ω}\sin \stackrel{\‾}{L}+\frac{{\stackrel{\‾}{V}}_x}{R}\tan \stackrel{\‾}{L})\·\mathrm{\δ}{\stackrel{\‾}{V}}_y+(2\mathrm{\Ω}\cos \stackrel{\‾}{L}{\·\stackrel{\‾}{V}}_y+\frac{{\stackrel{\‾}{V}}_x{\stackrel{\‾}{V}}_y}{R}{\sec }^2\stackrel{\‾}{L})\·\mathrm{\δ}\stackrel{\‾}{L}+\mathrm{\Δ}{\stackrel{\‾}{A}}_x\\ \mathrm{\δ}{\stackrel{\·}{\stackrel{\‾}{V}}}_y=-{\stackrel{\‾}{\mathrm{\φ}}}_z{\stackrel{\‾}{f}}_x-(2\mathrm{\Ω}\sin \stackrel{\‾}{L}+\frac{2{\stackrel{\‾}{V}}_x}{R}\tan \stackrel{\‾}{L})\·\mathrm{\δ}{\stackrel{\‾}{V}}_x-(2\mathrm{\Ω}\cos \stackrel{\‾}{L}{\stackrel{\‾}{V}}_x+\frac{{\left({\stackrel{\‾}{V}}_x\right)}^2}{R})\·\mathrm{\δ}\stackrel{\‾}{L}+\mathrm{\Δ}{\stackrel{\‾}{A}}_y\\ {\stackrel{\·}{\stackrel{\‾}{\mathrm{\φ}}}}_x=-\frac{\mathrm{\δ}{\stackrel{\‾}{V}}_y}{R}+(\mathrm{\Ω}\sin \stackrel{\‾}{L}+\frac{{\stackrel{\‾}{V}}_x\tan \stackrel{\‾}{L}}{R}){\stackrel{\‾}{\mathrm{\φ}}}_y-(\mathrm{\Ω}\cos \stackrel{\‾}{L}+\frac{{\stackrel{\‾}{V}}_x}{R}){\stackrel{\‾}{\mathrm{\φ}}}_z+{\stackrel{\‾}{\mathrm{\ϵ}}}_x\\ {\stackrel{\·}{\stackrel{\‾}{\mathrm{\φ}}}}_y=\frac{\mathrm{\δ}{\stackrel{\‾}{V}}_x}{R}-\mathrm{\Ω}\sin \stackrel{\‾}{L}\mathrm{\δ}\stackrel{\‾}{L}-(\mathrm{\Ω}\sin \stackrel{\‾}{L}+\frac{{\stackrel{\‾}{V}}_x\tan \stackrel{\‾}{L}}{R}){\stackrel{\‾}{\mathrm{\φ}}}_x-\frac{{\stackrel{\‾}{V}}_y}{R}{\stackrel{\‾}{\mathrm{\φ}}}_z+{\stackrel{\‾}{\mathrm{\ϵ}}}_y\\ {\stackrel{\·}{\stackrel{\‾}{\mathrm{\φ}}}}_z=\frac{\mathrm{\δ}{\stackrel{\‾}{V}}_x}{R}\tan \stackrel{\‾}{L}+\frac{{\stackrel{\‾}{V}}_y}{R}{\stackrel{\‾}{\mathrm{\φ}}}_y+(\mathrm{\Ω}\cos \stackrel{\‾}{L}+\frac{{\stackrel{\‾}{V}}_x}{R}{\sec }^2\stackrel{\‾}{L}+\frac{{\stackrel{\‾}{V}}_x}{R}){\stackrel{\‾}{\mathrm{\φ}}}_x+{\stackrel{\‾}{\mathrm{\ϵ}}}_z\end{array}\right.$

In formula,It is respectively horizontal latitude error and horizontal longitude error；For local horizontal geographic latitude；It is respectively and carries The horizontal east orientation velocity error of body and horizontal north orientation velocity error；It is respectively east orientation and north orientation accelerometer under abscissa system to miss Difference；It is respectively the ratio force information that under abscissa system, accelerometer x-axis, y-axis and z-axis are measured；Respectively For carrier x-axis, y-axis and z-axis attitude error angle under abscissa system；It is respectively x-axis, y-axis and z under abscissa system Axle gyroscopic drift.

Step 3: take 12 dimension state variables and set up state equation.Quantity of state is

$X={\left[\begin{array}{cccccccccccc}\mathrm{\δ}\stackrel{\‾}{L}& \mathrm{\δ}\stackrel{\‾}{\mathrm{\λ}}& \mathrm{\δ}\stackrel{\‾}{V_x}& \mathrm{\δ}\stackrel{\‾}{V_y}& {\stackrel{\‾}{\mathrm{\φ}}}_x& {\stackrel{\‾}{\mathrm{\φ}}}_y& {\stackrel{\‾}{\mathrm{\φ}}}_z& {\stackrel{\‾}{\mathrm{\ϵ}}}_x& {\stackrel{\‾}{\mathrm{\ϵ}}}_y& {\stackrel{\‾}{\mathrm{\ϵ}}}_z& \mathrm{\Δ}{\stackrel{\‾}{A}}_x& \mathrm{\Δ}{\stackrel{\‾}{A}}_y\end{array}\right]}^T$

Measurement equation is set up for external observation amount with speed.Measurement is

$Z=\left[\begin{array}{c}{\stackrel{\‾}{V}}_{\mathrm{sx}}-{\stackrel{\‾}{V}}_{\mathrm{dx}}\\ {\stackrel{\‾}{V}}_{\mathrm{sy}}-{\stackrel{\‾}{V}}_{\mathrm{dy}}\end{array}\right]=\left[\begin{array}{c}\mathrm{\δ}{\stackrel{\‾}{V}}_x\\ \mathrm{\δ}{\stackrel{\‾}{V}}_y\end{array}\right]={\stackrel{\‾}{V}}_s-{\stackrel{\‾}{V}}_d$

Wherein,For inertial navigation output speed under abscissa system；For electromagnet log output speed under abscissa system；For inertial navigation Output speed is at horizontal east upward component；For inertial navigation output speed at horizontal north upward component；Exist for electromagnet log output speed Horizontal east upward component；For electromagnet log output speed at horizontal north upward component；For horizontal east orientation velocity error；For horizontal stroke North orientation velocity error.

Step 4: set up Kalman filter model by step 3:

$\left\{\begin{array}{c}\stackrel{\·}{x}=\mathrm{AX}+\mathrm{BW}\\ Z=\mathrm{HX}+v\end{array}\right.$

Wherein, X is the state variable of system；A and B is respectively state matrix and the noise matrix of system；W be system noise to Amount；Z is the measurement vector of system；H is measurement matrix；ν is system measurements noise.

Error model is carried out discretization obtain:

$\left\{\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}\right.$

And then estimated the horizontal error angle under system abscissa system by Kalman filtering programmed algorithm

Step 5: by step 4 horizontal error angle under the abscissa system that Kalman filtering obtainsIt is incorporated into integrated calibration algorithm In, obtain

$\left[\begin{array}{c}\mathrm{\δ}\stackrel{\‾}{L}+{\widehat{\stackrel{\‾}{\mathrm{\φ}}}}_x\\ \mathrm{\δ}\stackrel{\‾}{\mathrm{\λ}}-\frac{{\widehat{\stackrel{\‾}{\mathrm{\φ}}}}_y}{\cos \stackrel{\‾}{L}}\\ \mathrm{\δ}\stackrel{\‾}{K}-{\widehat{\stackrel{\‾}{\mathrm{\φ}}}}_y\tan \stackrel{\‾}{L}\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& -\sec \stackrel{\‾}{L}& 0\\ 0& -\tan \stackrel{\‾}{L}& 1\end{array}\right]{\left[\begin{array}{c}{\mathrm{\ψ}}_x\\ {\mathrm{\ψ}}_y\\ {\mathrm{\ψ}}_z\end{array}\right]}^{\stackrel{\‾}{n}}$

Wherein,For the course error under horizontal geographic coordinate system, ψ_x、ψ_y、ψ_zFor inertial navigation system platform drift angle.

Completed to be applicable to the inertial navigation system integrated calibration of polar region further by above formula.

Claims (2)

1. the inertial navigation system synthesis correction method being applicable to polar region, it is characterised in that include following step:

Step one: the navigational parameter amount that ship's inertial navigation system exports being transformed under horizontal geographic coordinate system, navigational parameter includes: longitude and latitude Value, course, speed and attitude information；

Geographic coordinate system with the transition matrix of horizontal geographic coordinate system is:

$C_n^{\stackrel{\‾}{n}}=C_i^{\stackrel{\‾}{n}}\·C_n^i$

$C_i^{\stackrel{\‾}{n}}=\left[\begin{array}{ccc}\cos \stackrel{\‾}{\mathrm{\λ}}\cos \mathrm{\Ω}t& \cos \stackrel{\‾}{\mathrm{\λ}}\sin \mathrm{\Ω}t& -\sin \stackrel{\‾}{\mathrm{\λ}}\\ -\sin \stackrel{\‾}{L}\sin \stackrel{\‾}{\mathrm{\λ}}\cos \mathrm{\Ω}t-\cos \stackrel{\‾}{L}\sin \mathrm{\Ω}t& \cos \stackrel{\‾}{L}\cos \mathrm{\Ω}t-\sin \stackrel{\‾}{L}\sin \stackrel{\‾}{\mathrm{\λ}}\sin \mathrm{\Ω}t& -\sin \stackrel{\‾}{L}\cos \stackrel{\‾}{\mathrm{\λ}}\\ \cos \stackrel{\‾}{L}\sin \stackrel{\‾}{\mathrm{\λ}}\cos \mathrm{\Ω}t-\sin \stackrel{\‾}{L}\sin \mathrm{\Ω}t& \cos \stackrel{\‾}{L}\sin \stackrel{\‾}{\mathrm{\λ}}\sin \mathrm{\Ω}t+\sin \stackrel{\‾}{L}\cos \mathrm{\Ω}t& \cos \stackrel{\‾}{L}\cos \stackrel{\‾}{\mathrm{\λ}}\end{array}\right]$

$C_n^i=\left[\begin{array}{ccc}-\sin (\mathrm{\λ}+\mathrm{\Ω}t)& -\sin L\cos (\mathrm{\λ}+\mathrm{\Ω}t)& \cos L\cos (\mathrm{\λ}+\mathrm{\Ω}t)\\ \cos (\mathrm{\λ}+\mathrm{\Ω}t)& -\sin L\sin (\mathrm{\λ}+\mathrm{\Ω}t)& \cos L\sin (\mathrm{\λ}+\mathrm{\Ω}t)\\ 0& \cos L& \sin L\end{array}\right]$

Represent that inertial coordinate is tied to manage breadthways the transition matrix of coordinate system,Represent that geographical coordinate is tied to the conversion square of inertial coodinate system Battle array, Ω is rotational-angular velocity of the earth, and t is the time, and λ is the longitude under geographic coordinate system,

Latitude and longitude value under horizontal geographic coordinate system is:

$\stackrel{\‾}{L}=arcsin\left(\cos Lsin\mathrm{\λ}\right)$

$\stackrel{\‾}{\mathrm{\λ}}=arctan\left(\frac{\cos Lcos\mathrm{\λ}}{\sin L}\right)$

L is the latitude value under geographic coordinate system,For the latitude value under horizontal geographic coordinate system,For the longitude under horizontal geographic coordinate system, Under horizontal geographic coordinate system, course with speed is:

$\stackrel{\‾}{K}=K+arctan\frac{sin\mathrm{\λ}}{\sin L\cos \mathrm{\λ}}-\mathrm{\π}$

$\stackrel{\‾}{V}=\left[\begin{array}{c}{\stackrel{\‾}{V}}_x\\ {\stackrel{\‾}{V}}_y\end{array}\right]=\left[\begin{array}{c}V\sin \stackrel{\‾}{K}\\ V\cos \stackrel{\‾}{K}\end{array}\right]$

K represents the course under geographic coordinate system,For the course under horizontal geographic coordinate system, π is pi, and V is under geographic coordinate system Speed,For the speed under horizontal geographic coordinate system,For the east orientation speed under horizontal geographic coordinate system,For horizontal geographic coordinate system Under north orientation speed；

Step 2: the navigation mechanization utilizing the navigational parameter under the horizontal geographic coordinate system obtained to carry out under horizontal geographic coordinate system, determines The state variable of system and measurement；

Step 3: state variable and measurement according to system set up Kalman filter, is managed seat breadthways by kalman filter method The x-axis attitude error angle of the navigation system under mark systemWith y-axis attitude error angle

Step 4: the x-axis attitude error angle that will obtainWith y-axis attitude error angleIt is incorporated in the two point correction of navigation system, complete Become polar region two point correction；

In described step 2, navigation mechanization is:

$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{\·}{\stackrel{\‾}{L}}=\frac{\mathrm{\δ}{\stackrel{\‾}{V}}_y}{R}\\ \mathrm{\δ}\stackrel{\·}{\stackrel{\‾}{\mathrm{\λ}}}=\frac{{\stackrel{\‾}{V}}_x}{R}\mathrm{\δ}\stackrel{\‾}{L}\tan \stackrel{\‾}{L}\sec \stackrel{\‾}{L}+\frac{\mathrm{\δ}{\stackrel{\‾}{V}}_x}{R}\sec \stackrel{\‾}{L}\\ \mathrm{\δ}{\stackrel{\·}{\stackrel{\‾}{V}}}_x={\stackrel{\‾}{\mathrm{\φ}}}_z{\stackrel{\‾}{f}}_y+\frac{{\stackrel{\‾}{V}}_y}{R}\tan \stackrel{\‾}{L}\·\mathrm{\δ}{\stackrel{\‾}{V}}_x+(2\mathrm{\Ω}\sin \stackrel{\‾}{L}+\frac{{\stackrel{\‾}{V}}_x}{R}\tan \stackrel{\‾}{L})\·\mathrm{\δ}{\stackrel{\‾}{V}}_y+(2\mathrm{\Ω}\cos \stackrel{\‾}{L}\·{\stackrel{\‾}{V}}_y+\frac{{\stackrel{\‾}{V}}_x{\stackrel{\‾}{V}}_y}{R}{\sec }^2\stackrel{\‾}{L})\·\mathrm{\δ}\stackrel{\‾}{L}+\mathrm{\Δ}{\stackrel{\‾}{A}}_x\\ \mathrm{\δ}{\stackrel{\·}{\stackrel{\‾}{V}}}_y=-{\stackrel{\‾}{\mathrm{\φ}}}_z{\stackrel{\‾}{f}}_x-(2\mathrm{\Ω}\sin \stackrel{\‾}{L}+\frac{2{\stackrel{\‾}{V}}_x}{R}\tan \stackrel{\‾}{L})\·\mathrm{\δ}{\stackrel{\‾}{V}}_x-(2\mathrm{\Ω}\cos \stackrel{\‾}{L}{\stackrel{\‾}{V}}_x+\frac{{\left({\stackrel{\‾}{V}}_x\right)}^2}{R}{\sec }^2\stackrel{\‾}{L})\·\mathrm{\δ}\stackrel{\‾}{L}+\mathrm{\Δ}{\stackrel{\‾}{A}}_y\\ {\stackrel{\·}{\stackrel{\‾}{\mathrm{\φ}}}}_x=-\frac{\mathrm{\δ}{\stackrel{\‾}{V}}_y}{R}+(\mathrm{\Ω}\sin \stackrel{\‾}{L}+\frac{{\stackrel{\‾}{V}}_x\tan \stackrel{\‾}{L}}{R}){\stackrel{\‾}{\mathrm{\φ}}}_y-(\mathrm{\Ω}\cos \stackrel{\‾}{L}+\frac{{\stackrel{\‾}{V}}_x}{R}){\stackrel{\‾}{\mathrm{\φ}}}_z+{\stackrel{\‾}{\mathrm{\ϵ}}}_x\\ {\stackrel{\·}{\stackrel{\‾}{\mathrm{\φ}}}}_y=-\frac{\mathrm{\δ}{\stackrel{\‾}{V}}_x}{R}-\mathrm{\Ω}\sin \stackrel{\‾}{L}\mathrm{\δ}\stackrel{\‾}{L}-(\mathrm{\Ω}\sin \stackrel{\‾}{L}+\frac{{\stackrel{\‾}{V}}_x\tan \stackrel{\‾}{L}}{R}){\stackrel{\‾}{\mathrm{\φ}}}_x-\frac{{\stackrel{\‾}{V}}_y}{R}{\stackrel{\‾}{\mathrm{\φ}}}_z+{\stackrel{\‾}{\mathrm{\ϵ}}}_y\\ {\stackrel{\·}{\stackrel{\‾}{\mathrm{\φ}}}}_z=\frac{\mathrm{\δ}{\stackrel{\‾}{V}}_x}{R}\tan \stackrel{\‾}{L}+\frac{{\stackrel{\‾}{V}}_y}{R}{\stackrel{\‾}{\mathrm{\φ}}}_y+(\mathrm{\Ω}\cos \stackrel{\‾}{L}+\frac{{\stackrel{\‾}{V}}_x}{R}{\sec }^2\stackrel{\‾}{L})\mathrm{\δ}\stackrel{\‾}{L}+(\mathrm{\Ω}\cos \stackrel{\‾}{L}+\frac{{\stackrel{\‾}{V}}_x}{R}){\stackrel{\‾}{\mathrm{\φ}}}_x+{\stackrel{\‾}{\mathrm{\ϵ}}}_z\end{array}\right.$

It is respectively horizontal latitude error and horizontal longitude error,For local horizontal geographic latitude,It is respectively the horizontal stroke of carrier East orientation velocity error and horizontal north orientation velocity error,It is respectively east orientation and north orientation accelerometer under horizontal geographic coordinate system to miss Difference,It is respectively the ratio force information that under horizontal geographic coordinate system, accelerometer x-axis, y-axis and z-axis are measured, It is respectively carrier x-axis, y-axis and z-axis attitude error angle under horizontal geographic coordinate system,It is respectively under horizontal geographic coordinate system X-axis, y-axis and z-axis gyroscopic drift,

The state variable of system is:

$X={\left[\begin{array}{cccccccccccc}\mathrm{\δ}\stackrel{\‾}{L}& \mathrm{\δ}\stackrel{\‾}{\mathrm{\λ}}& \mathrm{\δ}{\stackrel{\‾}{V}}_x& \mathrm{\δ}{\stackrel{\‾}{V}}_y& {\stackrel{\‾}{\mathrm{\φ}}}_x& {\stackrel{\‾}{\mathrm{\φ}}}_y& {\stackrel{\‾}{\mathrm{\φ}}}_z& {\stackrel{\‾}{\mathrm{\ϵ}}}_x& {\stackrel{\‾}{\mathrm{\ϵ}}}_y& {\stackrel{\‾}{\mathrm{\ϵ}}}_z& \mathrm{\Δ}{\stackrel{\‾}{A}}_x& \mathrm{\Δ}{\stackrel{\‾}{A}}_y\end{array}\right]}^T$

The measurement of system is:

$Z=\left[\begin{array}{c}{\stackrel{\‾}{V}}_{sx}-{\stackrel{\‾}{V}}_{dx}\\ {\stackrel{\‾}{V}}_{sy}-{\stackrel{\‾}{V}}_{dy}\end{array}\right]=\left[\begin{array}{c}\mathrm{\δ}{\stackrel{\‾}{V}}_x\\ \mathrm{\δ}{\stackrel{\‾}{V}}_y\end{array}\right]={\stackrel{\‾}{V}}_s-{\stackrel{\‾}{V}}_d$

For inertial navigation output speed under horizontal geographic coordinate system,For electromagnet log output speed under horizontal geographic coordinate system,Defeated for inertial navigation Go out speed at horizontal east upward component,For inertial navigation output speed at horizontal north upward component,For electromagnet log output speed at horizontal stroke East upward component,For electromagnet log output speed at horizontal north upward component,For horizontal east orientation velocity error,For horizontal north To velocity error；

In described step 4, the x-axis attitude error angle that will obtainWith y-axis attitude error angleIt is incorporated into 2 schools of navigation system Center, obtains:

$\left[\begin{array}{c}\mathrm{\δ}\stackrel{\‾}{L}+{\widehat{\stackrel{\‾}{\mathrm{\φ}}}}_x\\ \mathrm{\δ}\stackrel{\‾}{\mathrm{\λ}}-\frac{{\widehat{\stackrel{\‾}{\mathrm{\φ}}}}_y}{\cos \stackrel{\‾}{L}}\\ \mathrm{\δ}\stackrel{\‾}{K}-{\widehat{\stackrel{\‾}{\mathrm{\φ}}}}_y\tan \stackrel{\‾}{L}\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& -\sec \stackrel{\‾}{L}& 0\\ 0& -\tan \stackrel{\‾}{L}& 1\end{array}\right]{\left[\begin{array}{c}{\mathrm{\ψ}}_x\\ {\mathrm{\ψ}}_y\\ {\mathrm{\ψ}}_z\end{array}\right]}^{\stackrel{\‾}{n}}$

For the course error under horizontal geographic coordinate system, ψ_x、ψ_y、ψ_zFor inertial navigation system platform drift angle.

A kind of inertial navigation system synthesis correction method being applicable to polar region the most according to claim 1, it is characterised in that: described step In rapid three, the Kalman filter of foundation is:

$\left\{\begin{array}{c}\stackrel{\·}{X}=AX+BW\\ Z=HX+v\end{array}\right.$

Wherein, X is the state variable of system, A and B is respectively state matrix and the noise matrix of system, W be system noise to Amount, Z is the measurement of system, and H is measurement matrix, and ν is system measurements noise.