CN106646569A - Navigation and positioning method and device - Google Patents

Abstract

The invention discloses a navigation and positioning method and device. The method includes the following steps that: when satellite signal failure is detected, with the magnetic heading angle of a k-th calculation period adopted as an observation quantity, a first state quantity combination is obtained through a DR (Dead-reckoning)/magnetic heading combined navigation algorithm model, and with the lateral velocity of the k-th calculation period adopted as an observation quantity, and a second state quantity combination is obtained through a DR/vehicle motion combined navigation algorithm model; according to the first state quantity combination and the second state quantity combination, closed-loop Kalman combined filtering is performed based on the system state equation of the k-th calculation period, so that a first error correction value is obtained; and the initial heading angle of the k-th calculation period is corrected according to the first error correction value, so that a target heading angle can be obtained, and a target speed and a target location are calculated according to the target heading angle and the DR model of the k-th calculation period. With the navigation and positioning method and device provided by the embodiments of the invention adopted, vehicle positioning and navigation during the failure of satellite signals can be realized, and the accuracy of vehicle-mounted positioning can be improved.

Description

A kind of navigation locating method and equipment

Technical field

The present invention relates to vehicle mounted guidance technical field, and in particular to a kind of navigation locating method and equipment.

Background technology

Automotive positioning technology is an important component part of intelligent transportation system, is also one of key technology.Car Carry navigation request can continuously, position that is real-time, accurately determining vehicle, be presently available for the navigation mode of vehicle mounted guidance technology Have：Satellite navigation (such as global positioning system, Beidou satellite navigation system), odometer, MEMS (Microelectro Mechanical Systems, MEMS) sensor, Magnetic Sensor, map match etc..Satellite navigation positioning precision is higher, skill Art is ripe, easy to use, but it is easily failed by the masking of high building, overhead, tunnel, underground garage etc.；And with mileage It is a kind of inexpensive, autonomous that meter, MEMS sensor carry out the dead reckoning algorithm (Dead-reckoning, DR) of navigator fix Automobile navigation positioning mode, its advantage is strong antijamming capability, can at short notice be provided according to the sensing data for providing The more accurate navigational parameter of vehicle, but the error of DR algorithms is with time integral, is not suitable for prolonged individually navigation；Map With algorithm mainly as a kind of assisting navigation means, auxiliary is carried out to navigation results with reference to road route situation in numerical map and is repaiied Just, to improve positioning precision, but have some limitations.

At present, onboard navigation system both domestic and external, mostly all by the way of satellite navigation and other localization method combinations To realize the navigation to vehicle and positioning, mainly there are following two combinations：Map match/satellite combined guidance system and DR/ satellite combined guidance systems, but in the case where satellite-signal is interfered even failure, both integrated navigation systems Navigation error will significantly increase or even can not use.

The content of the invention

The embodiment of the invention discloses a kind of navigation locating method and equipment, to realizing vehicle when navigation signal is invalid Location navigation, improves the accuracy of automotive positioning.

In a first aspect, the embodiment of the present invention discloses a kind of navigation locating method.Method includes：

When detect satellite-signal fail when, the magnetic heading angle with k-th calculating cycle as observed quantity, by the kth The dead reckoning algorithm (Dead-reckoning, DR) of individual calculating cycle/magnetic heading Integrated Navigation Algorithm model obtains the kth Individual calculating cycle first state amount combination, the lateral velocity with k-th calculating cycle as observed quantity, by the kth The DR/ vehicle movement Integrated Navigation Algorithm models of individual calculating cycle obtain the second quantity of state combination of k-th calculating cycle, The k is the integer more than 2；

According to the system state equation of k-th calculating cycle, for first shape of k-th calculating cycle State amount is combined and second quantity of state combination of k-th calculating cycle carries out closed loop Kalman's combined filter, obtains institute State the first error correction values of k-th calculating cycle；

K-th calculating cycle is initial according to the first error correction values amendment of k-th calculating cycle Course angle, obtains the target course of k-th calculating cycle, according to the bogey heading of k-th calculating cycle Angle and the DR models of k-th calculating cycle, are calculated target velocity and the target location of k-th calculating cycle.

Therefore, navigation locating method disclosed in the embodiment of the present invention, when equipment detects satellite-signal to fail, with The magnetic heading angle of k-th calculating cycle is observed quantity, and by DR/ magnetic heading Integrated Navigation Algorithm models first state amount group is obtained Close, the lateral velocity with k-th calculating cycle obtains second as observed quantity by DR/ vehicle movement Integrated Navigation Algorithm models Quantity of state is combined；According to the system state equation of k-th calculating cycle, for the combination of first state amount and the combination of the second quantity of state Carry out closed loop Kalman's combined filter and obtain the first error correction values；According to first k-th calculating cycle of error correction values amendment Initial heading angle obtain target course, according to target course and the DR models of k-th calculating cycle, be calculated target Speed and target location.It can be seen that, by DR/ magnetic heading Integrated Navigation Algorithm models and DR/ vehicle movement Integrated Navigation Algorithm moulds Type is modified when satellite-signal fails to course angle, and then extrapolates target velocity and target location letter according to DR models Breath, realizes the vehicle positioning and navigation when satellite-signal is invalid, improves the accuracy of vehicle positioning.

In a possible design, the DR/ magnetic heading Integrated Navigation Algorithm models of k-th calculating cycle are bases The magnetic heading angle of the initial heading angle of k-th calculating cycle and k-th calculating cycle and set up, the kth The magnetic heading angle of individual calculating cycle be it is calculated according to Magnetic Sensor output data, k-th calculating cycle it is initial Course angle is obtained according to the target course of -1 calculating cycle of kth and the angular speed calculation of k-th calculating cycle acquisition, The DR/ vehicle movement Integrated Navigation Algorithm models of k-th calculating cycle are the horizontal speed according to k-th calculating cycle Degree and set up, the lateral velocity of k-th calculating cycle is according to the initial velocity of k-th calculating cycle and described The initial heading angle of k-th calculating cycle is calculated, and the initial velocity of k-th calculating cycle is according to the kth The MEMS at the initial heading angle of individual calculating cycle, the target velocity of -1 calculating cycle of kth and k-th calculating cycle is used to Property sensor accelerometer output valve is calculated；

The system state equation of k-th calculating cycle is built according to the quantity of state of -1 calculating cycle of the kth Vertical；

The DR models of k-th calculating cycle are the initial velocities and initial position according to k-th calculating cycle And set up, the initial position of k-th calculating cycle is initial velocity and kth -1 according to k-th calculating cycle The target location of individual calculating cycle is calculated.

In a possible design, methods described also includes：

When detecting satellite-signal and being effective, the initial velocity and initial position with+1 calculating cycle of kth as observed quantity, The 3rd of+1 calculating cycle of the kth is obtained by the DR/ combinations of satellites navigation algorithm models of+1 calculating cycle of the kth Quantity of state is combined, and the DR/ combinations of satellites navigation algorithm models of+1 calculating cycle of the kth are according to+1 calculating of the kth The initial velocity and the initial position in cycle and set up；

According to the system state equation of+1 calculating cycle of the kth, for described the of+1 calculating cycle of the kth One quantity of state is combined and the third state amount combination of+1 calculating cycle of the kth carries out the closed loop Kalman combination filter Ripple, obtains second error correction values of+1 calculating cycle of the kth；

+ 1 calculating cycle of kth according to the second error correction values amendment of+1 calculating cycle of the kth Initial heading angle, obtains the target course of+1 calculating cycle of the kth, according to+1 calculating cycle of the kth The DR models of+1 calculating cycle of target course and the kth, are calculated the target velocity of+1 calculating cycle of the kth And target location.

Therefore, the navigation locating method disclosed in this possible design, except realizing when satellite-signal is invalid Vehicle positioning and navigation, improve outside the accuracy of vehicle positioning, equipment is received in urban canyons, the overhead satellite-signal such as block To interference but under non-failed environment, course angle is modified using the second error correction values, improves satellite-signal and be subject to dry The accuracy of the navigator fix in the case of disturbing.

In a possible design, the first state amount combination of k-th calculating cycle includes:The kth The magnetic heading angle combination state quantity measurement Z of individual calculating cycle_{Mag, k}, the first measurement matrix H_Mag, and k-th calculating cycle First measurement noise V_{Mag, k}；

The Z is calculated based on equation below_{Mag, k}

Z_Mag,k=[Φ_DR,k-Φ_Mag,k]

The H is calculated based on equation below_Mag

H_Mag=[1 000000 0]

The V is calculated based on equation below_{Mag, k}

Wherein, Φ_DR,kFor k-th calculating cycle initial heading angle, Φ_{Mag, k}For the magnetic of k-th calculating cycle Course angle,For the course angle error that k-th calculating cycle is obtained.

In a possible design, second quantity of state combination of k-th calculating cycle includes：The kth The vehicle lateral speed state quantity measurement Z of individual calculating cycle_{Virt, k}, the second measurement matrix H of k-th calculating cycle_{Virt, k}, with And the second measurement noise V of k-th calculating cycle_{Virt, k}；

The Z is calculated based on equation below_{Virt, k}

The H is calculated based on equation below_{Virt, k}

H_Virt,k=[- ν_eDR,ksinΦ_DR,k-ν_nDR,kcosΦ_DR,k cosΦ_DR,k-sinΦ_DR,k 0 0 0 0 0 0]

The V is calculated based on equation below_{Virt, k}

V_Virt,k=[δ v_x,k]

Wherein,East orientation and north orientation speed error that respectively described k-th calculating cycle is obtained, v_eDR,kFor the initial east orientation speed of k-th calculating cycle；v_nDR,kFor the initial north orientation speed of k-th calculating cycle； δv_x,kFor during k-th calculating cycle vehicle movement due to caused additional lateral velocity variations of breakking away.

In a possible design, the third state amount combination of+1 calculating cycle of the kth includes：The kth+1 The combination state quantity measurement Z of the longitude of individual calculating cycle, latitude, east orientation speed and north orientation speed_Sat,k+1,+1 calculating of the kth The 3rd measurement matrix H in cycle_Sat,k+1, and the 3rd measurement noise V of+1 calculating cycle of the kth_Sat,k+1；

The Z is calculated based on equation below_Sat,k+1

The H is calculated based on equation below_Sat,k+1

The V is calculated based on equation below_Sat,k+1

Wherein, λ_DR,k+1And L_DR,k+1The longitude and latitude of respectively described+1 calculating cycle of kth；λ_Sat,k+1、L_Sat,k+1、 v_eSat,k+1And v_nSat,k+1Longitude, latitude, the east orientation speed of the DVB positioning of respectively described+1 calculating cycle of kth And north orientation speed；R_mAnd R_nRespectively meridian circle radius and prime vertical radius；N_eSat,k+1、N_nSat,k+1、M_eSat,k+1And M_nSat,k+1 Longitude, latitude, east orientation that the location information that respectively described+1 calculating cycle DVB of kth is calculated contains in itself Speed and north orientation speed error.

Therefore, the navigation locating method disclosed in this possible design, except realizing when satellite-signal is invalid Vehicle positioning and navigation, improve outside the accuracy of vehicle positioning, the initial velocity that equipment is updated by each calculating cycle The first state amount combination is continuously updated with initial position, the second quantity of state combination, the third state amount is combined, First correction value is improve, the output accuracy of second correction value maintains holding for each calculating cycle navigation information Continuous reliable output.

In a possible design, the system state equation of k-th calculating cycle is：

X_k=A_k,k-1X_k-1+G_k-1W_k-1

Wherein, A_k,k-1The state-transition matrix of k calculating cycle, X described in_kIt is for k-th calculating cycle System quantity of state,G_k-1For -1 calculating cycle of the kth System noise matrix, W_k-1For the white noise random error vector of -1 calculating cycle of the kth,WithRespectively institute State precision and latitude error that k-th calculating cycle is obtained, ε_rFor the single order Markov error of gyro, ε_bArbitrary constant error,WithThe accelerometer dextrad and forward direction single order Markov error of respectively described k-th calculating cycle.

In a possible design, the system state equation according to k-th calculating cycle, for described The first state amount combination of k calculating cycle and second quantity of state combination of k-th calculating cycle are closed Ring Kalman's combined filter obtains the first error correction of k-th calculating cycle, including：

First error correction values of k-th calculating cycle are calculated by equation below

Wherein,For the first error correction values of k-th calculating cycle,

In a possible design, according to the system state equation of+1 calculating cycle of the kth, for the kth+ The first state amount combination of 1 calculating cycle and the third state amount combination of+1 calculating cycle of the kth are carried out The closed loop Kalman combined filter obtains second error correction values of+1 calculating cycle of the kth, including：

Second error correction values of+1 calculating cycle of the kth are calculated by equation below

Wherein,For second error correction values of+1 calculating cycle of the kth,

In a possible design, the first error correction values amendment according to k-th calculating cycle The initial heading angle of k-th calculating cycle to obtain the target course of k-th calculating cycle, including：

The target course Φ ' of k-th calculating cycle is calculated by equation below_k,

Wherein, the Φ '_kFor the target course of k-th calculating cycle.

Therefore, the navigation locating method disclosed in this possible design, except realizing when satellite-signal is invalid Vehicle positioning and navigation, improve outside the accuracy of vehicle positioning, equipment by closed-series combination Kalman filtering algorithm, to boat It is modified to angle, further increases the output accuracy of navigator fix information.

In a possible design, the target course and the kth according to k-th calculating cycle The DR models of individual calculating cycle, are calculated target velocity and the target location of k-th calculating cycle, including：

Target east orientation speed v of k-th calculating cycle is calculated by equation below_{E, k}, k-th calculating Target cycle north orientation speed v_{N, k}, the target longitude λ of k-th calculating cycle_kWith the target latitude of k-th calculating cycle Degree L_k,

f_e,k=f_x′_,kcosΦ′_k+f′_y,ksinΦ′_k

f_n,k=-f '_x,ksinΦ′_k+f′_y,kcosΦ′_k

v_e,k=v_e,k-1+Tf_e,k

v_n,k=v_n,k-1+Tf_n,k

L_k=L_k-1+Tv_n,k/R_m

λ_k=λ_k-1+Tv_e,k/(R_ncosL_k)

Wherein, f '_{X, k}With f '_{Y, k}Accelerometer output in respectively described k-th calculating cycle MEMS inertial sensor The correction value of dextrad and forward acceleration value, v_{E, k}For the target east orientation speed of k-th calculating cycle, v_{N, k}For the kth The target north orientation speed of individual calculating cycle；f_e,kAnd f_n,kThe east orientation of respectively described k-th calculating cycle and the acceleration of north orientation Projection；T is sensor sample time interval；L_kFor the target latitude of k-th calculating cycle, λ_kWeek is calculated for described k-th The target longitude of phase.

In this possible design, the right side of accelerometer output in k-th calculating cycle MEMS inertial sensor To the correction value f ' with forward acceleration value_{X, k}With f '_{Y, k}Respectively by bowing present in the MEMS inertial sensor installation process In k-th calculating cycle MEMS inertial sensor described in elevation deflection and roll angle drift correction accelerometer output dextrad and Forward acceleration value gained.

Therefore, the navigation locating method disclosed in this possible design, except realizing when satellite-signal is invalid Vehicle positioning and navigation, improve outside the accuracy of vehicle positioning, equipment is pacified by MEMS inertial sensor to low precision Pitch angle deviation present in dress process and roll angular displacement are modified, and improve MEMS inertial sensor horizontal accelerometer Output accuracy, and then the target velocity and the output accuracy of target location of height navigator fix.

Second aspect, the embodiment of the present invention discloses a kind of equipment, and the equipment has equipment in said method design of realizing The function of behavior.The function can be realized by hardware, it is also possible to performed corresponding software by hardware and realized.The hardware Or software includes one or more modules corresponding with above-mentioned functions.

In a possible design, equipment includes processor, and it is above-mentioned that the processor is configured to holding equipment execution Corresponding function in method.Further, equipment can also include receiver and transmitter, and the receiver and transmitter are used for Communication between equipment and other equipment such as aeronautical satellite.Further, equipment can also include memory, and the memory is used In coupling with processor, it preserves the necessary programmed instruction of equipment and data.

The third aspect, the embodiment of the present invention discloses a kind of computer-readable recording medium, wherein, the computer-readable is deposited Storage media is stored with for the program code of computer equipment execution, and the program code specifically includes execute instruction, the execution Instruct the part or all of step for performing described in embodiment of the present invention first aspect either method.

Therefore, navigation locating method disclosed in the embodiment of the present invention, when equipment detects satellite-signal to fail, with The magnetic heading angle of k-th calculating cycle is observed quantity, and by DR/ magnetic heading Integrated Navigation Algorithm models first state amount group is obtained Close, the lateral velocity with k-th calculating cycle obtains second as observed quantity by DR/ vehicle movement Integrated Navigation Algorithm models Quantity of state is combined；According to the system state equation of k-th calculating cycle, for the combination of first state amount and the combination of the second quantity of state Carry out closed loop Kalman's combined filter and obtain the first error correction values；According to first k-th calculating cycle of error correction values amendment Initial heading angle obtain target course, according to target course and the DR models of k-th calculating cycle, be calculated target Speed and target location.It can be seen that, by DR/ magnetic heading Integrated Navigation Algorithm models and DR/ vehicle movement Integrated Navigation Algorithm moulds Type is modified when satellite-signal fails to course angle, and then extrapolates target velocity and target location letter according to DR models Breath, realizes the vehicle positioning and navigation when satellite-signal is invalid, improves the accuracy of vehicle positioning.

Description of the drawings

In order to be illustrated more clearly that the embodiment of the present invention or technical scheme of the prior art, below will be to embodiment or existing The accompanying drawing to be used needed for having technology description is briefly described, it should be apparent that, drawings in the following description are only this Some embodiments of invention, for those of ordinary skill in the art, on the premise of not paying creative work, can be with Other accompanying drawings are obtained according to these accompanying drawings.

Fig. 1 is a kind of schematic flow sheet of navigation locating method disclosed in the embodiment of the present invention；

Fig. 2A is a kind of functional unit block diagram of navigation control device disclosed in the embodiment of the present invention；

Fig. 2 B are a kind of structural representations of navigation control device disclosed in the embodiment of the present invention.

Specific embodiment

With reference to the accompanying drawing in the embodiment of the present invention, the technical scheme in the embodiment of the present invention is described.

Fig. 1 is referred to, Fig. 1 is the embodiment of the invention discloses a kind of schematic flow sheet of navigation locating method, the method Navigation positioning system is applied to, the navigation positioning system can be set by the navigator display, Navigation Control that are installed in vehicle Standby, DVB, sensor (such as Magnetic Sensor, inertial sensor (accelerometer and gyroscope)) composition, as illustrated, This navigation locating method includes：

S101, when navigation control device detects satellite-signal to fail, with the magnetic heading angle of k-th calculating cycle to see Measurement, is calculated by the dead reckoning algorithm (Dead-reckoning, DR) of k-th calculating cycle/magnetic heading integrated navigation Method model obtains the first state amount combination of k-th calculating cycle, with the lateral velocity of k-th calculating cycle to see Measurement, by the DR/ vehicle movement Integrated Navigation Algorithm models of k-th calculating cycle k-th calculating cycle is obtained The second quantity of state combination；

Wherein, the k is the integer more than 2, the DR/ magnetic heading Integrated Navigation Algorithm models of k-th calculating cycle It is to be set up, institute according to the initial heading angle of k-th calculating cycle and the magnetic heading angle of k-th calculating cycle The magnetic heading angle for stating k-th calculating cycle be it is calculated according to Magnetic Sensor output data, k-th calculating cycle Initial heading angle is obtained according to the target course of -1 calculating cycle of kth and the angular speed calculation of k-th calculating cycle acquisition Arrive, the DR/ vehicle movement Integrated Navigation Algorithm models of k-th calculating cycle are according to k-th calculating cycle Lateral velocity and set up, the lateral velocity of k-th calculating cycle is the initial velocity according to k-th calculating cycle Calculated with the initial heading angle of k-th calculating cycle, the initial velocity of k-th calculating cycle is according to institute State initial heading angle, the target velocity of -1 calculating cycle of kth and k-th calculating cycle of k-th calculating cycle MEMS inertial sensor accelerometer output valve is calculated.

Specifically, the DR/ magnetic heading Integrated Navigation Algorithm models of k-th calculating cycle are according to described k-th meter The magnetic heading angle of the initial heading angle in calculation cycle and k-th calculating cycle and set up, including：

First, the navigation control device is according to k-th calculating cycle horizontal direction Magnetic Sensor output data, as the following formula Calculate magnetic heading angle Φ of k-th calculating cycle_{Mag, k}：

In formula, mag_{X, k}And mag_{Y, k}The output data of respectively k-th calculating cycle Magnetic Sensor X-axis and Y-axis.

Meanwhile, the course angle of the Magnetic Sensor output of the initial heading angle of k-th calculating cycle and k-th calculating cycle Information can be expressed as：

Φ_Mag,k=Φ_k-E_Mag,k

In formula, Φ_DR,kFor the initial heading angle of k-th calculating cycle,For the course angle that k-th calculating cycle is obtained Error, E_{Mag, k}For the magnetic heading angle error that k-th calculating cycle is obtained, Φ_kRepresent k-th calculating cycle course angle it is true Value.

Wherein, the recurrence formula at the initial heading angle of k-th calculating cycle is：

Φ_DR,k=Φ '_k-1-Tω_z,k

In formula, the initial value at the initial heading angle of first calculating cycle is approximate with the magnetic heading angle of first calculating cycle Equal, the magnetic heading angle of first calculating cycle is obtained by the Magnetic Sensor of the navigation positioning system, Φ '_k-1 For the target course of -1 calculating cycle of kth, ω_z,kFor the MEMS inertia sensings of the vertical direction of k-th calculating cycle The angular speed of the gyroscope output in device.

Secondly, the navigation control device sets up k-th calculating cycle magnetic heading angle combination and measures state equation, including the One quantity of state combines Z_{Mag, k}、H_Mag、V_{Mag, k}；

Wherein, k-th calculating cycle magnetic heading angle combination measurement state equation is：

Wherein, the first measurement matrix H_MagFor：

H_Mag=[1 000000 0]

First measurement noise V of k-th calculating cycle_{Mag, k}For：

Specifically, the DR/ vehicle movement Integrated Navigation Algorithm models of k-th calculating cycle are according to described k-th The lateral velocity of calculating cycle and set up, including：

First, the navigation control device sets up the state quantity measurement equation mould of the Velicle motion velocity of k-th calculating cycle Type：

According to initial east orientation speed v of k-th calculating cycle_{EDR, k}With initial north orientation speed v_{NDR, k}, calculate k-th meter The vehicle lateral speed parameter in calculation cycle is,

Wherein, v_{EDR, k}, v_{NDR, k}East orientation and north orientation speed error that respectively described k-th calculating cycle is obtained.

Secondly, the lateral velocity is carried out minimum quantization expansion by the navigation control device, obtains linearizing measurement State equation：

Again, under actual vehicle motion conditions, it is contemplated that the factor such as pavement roughness and turning slip, then k-th The lateral velocity constraint equation of calculating cycle is：

v_x,CAR,k=v_x,k-δv_x,k=(v_eDR,kcosΦ_DR,k-v_nDR,ksinΦ_DR,k)-δv_x,k

In formula, v_x,kFor the horizontal lateral movement speed actual value of k-th calculating cycle, δ v_x,kRepresent k-th calculating cycle car Due to caused additional lateral velocity variations of breakking away during motion, be modeled as white noise, its value size and vehicle it is concrete Kinetic characteristic is relevant, and when running velocity is big, slipspeed is big.

Finally, the navigation control device is according to the linearisation lateral velocity state quantity measurement equation and the lateral velocity Constraint equation, the lateral velocity virtual measurement equation that can construct k-th calculating cycle is：

So as to obtain the vehicle lateral speed state quantity measurement equation of k-th calculating cycle, including the combination of the second quantity of state Z_Virt, k, H_{Virt, k}、V_{Virt, k}。

The vehicle lateral speed state quantity measurement equation of k-th calculating cycle is：

Z_Virt,k=H_Virt,kX_k+V_Virt,k

Wherein, the second measurement matrix H of k-th calculating cycle_Virt,kFor：

H_Virt,k=[- ν_eDR,ksinΦ_DR,k-ν_nDR,kcosΦ_DR,k cosΦ_DR,k-sinΦ_DR,k 0 0 0 0 0 0]

Second measurement noise V of k-th calculating cycle_Virt,kFor：

V_Virt,k=[δ v_x,k]

S102, the navigation control device according to the system state equation of k-th calculating cycle, for the kth The first state amount combination of individual calculating cycle and second quantity of state combination of k-th calculating cycle carry out closed loop Kalman's combined filter, obtains the first error correction values of k-th calculating cycle；

Wherein, the system state equation is：

In formula, A is state-transition matrix, and G is system noise matrix, W white noise random error vectors, wherein：

W=[ω_gz ω_rz ω_ax ω_ay]

In formula, f_xAnd f_yThe accelerometer output of the MEMS inertial sensor obtained in DR models respectively under static position Dextrad and forward acceleration value；R_mAnd R_nRespectively meridian circle radius and prime vertical radius；L_DRFor initial latitude；v_eDRFor first Beginning east orientation speed；ω_gzFor gyro white noise, ω_rzWhite noise, ω are driven for gyro single order Markov_axAnd ω_ayFor acceleration Meter single order Markov drives white noise；T_εAnd T_aThe single order horse of gyroscope and accelerometer respectively in MEMS inertial sensor Er Kefu correlation times.

Wherein, the system state equation is with course angle errorHorizontal velocity errorWithSite error WithThe single order Markov error ε of gyro_rWith arbitrary constant error ε_bAnd accelerometer single order Markov errorWithAs system state amount, system state equation is set up, the system state amount is：

The system state equation sliding-model control is obtained into the system state equation of k-th calculating cycle：

X_k=A_k,k-1X_k-1+G_k-1W_k-1

Wherein,

Specifically, when satellite-signal fails, vehicle system of the navigation control device according to k-th calculating cycle System state equation, for k-th calculating cycle the first state amount combination and k-th calculating cycle described in The combination of second quantity of state carries out closed loop Kalman's combined filter, obtains the first error correction values of k-th calculating cycleIncluding：

In formula,

Wherein：

The initial value of P, Q is relevant with the error parameter of MEMS sensor.

S103, the first error correction values amendment institute of the navigation control device according to k-th calculating cycle The initial heading angle of k-th calculating cycle is stated, the target course of k-th calculating cycle is obtained, according to described k-th meter The DR models of the target course in calculation cycle and k-th calculating cycle, are calculated k-th calculating cycle Target velocity and target location.

Wherein, the DR models of k-th calculating cycle are according to the initial velocity of k-th calculating cycle and initial Position and set up, the initial position of k-th calculating cycle is the initial velocity and according to k-th calculating cycle The target location of k-1 calculating cycle is calculated.

First, the first error correction values amendment institute of the navigation control device according to k-th calculating cycle The initial heading angle of k-th calculating cycle is stated, the target course of k-th calculating cycle is obtained, including：

By in first error correction values of k-th calculating cycleFormula as described below is calculated To the target course Φ ' of k-th calculating cycle_k,

Wherein, the Φ '_kFor the target course of k-th calculating cycle.

Secondly, the navigation control device is according to the target course of k-th calculating cycle and described k-th The DR models of calculating cycle, are calculated target velocity and the target location of k-th calculating cycle, including：

The DR moulds of k-th calculating cycle are set up according to the initial velocity and initial position of k-th calculating cycle Type；

Specifically, the navigational coordinate system of the DR models of k-th calculating cycle selects northeast day coordinate system, carrier coordinate System elects upper coordinate system before the right side as；

The target east orientation of k-th calculating cycle is calculated by the DR model formations of following k-th calculating cycle Speed v_{E, k}, k-th calculating cycle target north orientation speed v_n,k, the target longitude λ of k-th calculating cycle_kWith it is described The target latitude L of k-th calculating cycle_k,

f_e,k=f '_x,kcosΦ′_k+f′_y,ksinΦ′_k

f_n,k=-f '_x,ksinΦ′_k+f′_y,kcosΦ′_k

v_e,k=v_e,k-1+Tf_e,k

v_n,k=v_n,k-1+Tf_n,k

L_k=L_k-1+Tv_n,k/R_m

λ_k=λ_k-1+Tv_e,k/(R_ncosL_k)

Wherein, f '_x,kWith f '_y,kAccelerometer output in respectively described k-th calculating cycle MEMS inertial sensor The correction value of dextrad and forward acceleration value, v_e,kFor the target east orientation speed of k-th calculating cycle, v_n,kFor the kth The target north orientation speed of individual calculating cycle；f_e,kAnd f_n,kThe east orientation of respectively described k-th calculating cycle and the acceleration of north orientation Projection；T is sensor sample time interval；L_kFor the target latitude of k-th calculating cycle, λ_kWeek is calculated for described k-th The target longitude of phase.

Wherein, k-th calculating cycle MEMS (Microelectro Mechanical Systems, MEMS dextrad and the correction value f ' of forward acceleration value that) accelerometer is exported in inertial sensor_x,kWith f '_y,kRespectively by described K-th calculating cycle MEMS described in pitch angle deviation present in MEMS inertial sensor installation process and roll angle drift correction The dextrad of accelerometer output and forward acceleration value gained in inertial sensor, including：

The accelerometer output f ' of k-th calculating cycle after formula as described below is corrected_x,kWith f '_y,kFor：

F'=(C₁C₂)^-1f

Wherein：

F '=[f '_x,k f′_y,k f′_z,k]^T

F=[f_x,k f_y,k f_z,k]^T

In formula, Δ θ is pitch angle deviation；Δ γ is roll angular displacement；f_x,k, f_y,kAnd f_z,kDextrad before respectively correcting, Forward direction and on to accelerometer output valve.

Wherein, the f_x,k, f_y,kAnd f_z,kFor the accelerometer output valve of k-th calculating cycle acquisition under static position, root According to acceleration projection relation, the Δ θ and Δ γ may be calculated：

Sin Δs θ=f_y,k/g

Tan Δ γ=- f_x,k/f_z

In formula, g is acceleration of gravity.

Further, the first error correction values of k-th calculating cycleIt is additionally operable to correct following message：

ω_z,k+1=ω_z,k-ε_b-ε_r

Specifically, by the first error correction values of k-th calculating cycleIn ε_b,ε_r,Φ of the data to k-th calculating cycle_DR,k, v_eDR,k, v_nDR,k, λ_DR,k, L_DR,k, ω_z,k, f_x,k, f_y,kIt is modified, obtains the Φ of+1 calculating cycle of kth_DR,k+1, v_eDR,k+1, v_nDR,k+1, λ_DR,k+1, L_DR,k+1, ω_z,k+1, f_x,k+1, f_y,k+1Value, in the navigation locating method for+1 calculating cycle of kth.

Navigation locating method disclosed in the embodiment of the present invention, when navigation control device detects satellite-signal to fail, with The magnetic heading angle of k-th calculating cycle is observed quantity, and by DR/ magnetic heading Integrated Navigation Algorithm models first state amount group is obtained Close, the lateral velocity with k-th calculating cycle obtains second as observed quantity by DR/ vehicle movement Integrated Navigation Algorithm models Quantity of state is combined；According to the system state equation of k-th calculating cycle, for the combination of first state amount and the combination of the second quantity of state Carry out closed loop Kalman's combined filter and obtain the first error correction values；According to first k-th calculating cycle of error correction values amendment Initial heading angle obtain target course, according to target course and the DR models of k-th calculating cycle, be calculated target Speed and target location.It can be seen that, by DR/ magnetic heading Integrated Navigation Algorithm models and DR/ vehicle movement Integrated Navigation Algorithm moulds Type is modified when satellite-signal fails to course angle, and then extrapolates target velocity and target location letter according to DR models Breath, realizes the vehicle positioning and navigation when satellite-signal is invalid, improves the accuracy of vehicle positioning.

In one example, when the navigation control device detect satellite-signal it is effective when, with+1 calculating cycle of kth Initial velocity and initial position be observed quantity, by the DR/ combinations of satellites navigation algorithm models of+1 calculating cycle of the kth Obtain the third state amount combination of+1 calculating cycle of the kth, the DR/ combinations of satellites navigation of+1 calculating cycle of the kth Algorithm model is set up according to the initial velocity and the initial position of+1 calculating cycle of the kth；

The navigation control device according to the Vehicular system state equation of+1 calculating cycle of the kth, for the kth The first state amount combination of+1 calculating cycle and the third state amount combination of+1 calculating cycle of the kth are carried out The closed loop Kalman combined filter, obtains second error correction values of+1 calculating cycle of the kth；

The navigation control device is according to the second error correction values amendment of+1 calculating cycle of the kth The initial heading angle of k+1 calculating cycle, obtains the target course of+1 calculating cycle of the kth, according to the kth+1 The DR models of+1 calculating cycle of the target course and the kth of calculating cycle, are calculated+1 calculating of the kth The target velocity in cycle and target location.

Wherein, the DR/ combinations of satellites navigation algorithm models of+1 calculating cycle of the kth are according to+1 meter of the kth The initial velocity and the initial position in calculation cycle and set up, including：

With the initial longitude of+1 calculating cycle of kth, the initial latitude of+1 calculating cycle of kth ,+1 calculating cycle of kth + 1 calculating cycle of initial east orientation speed and kth initial north orientation speed as observed quantity, set up+1 calculating cycle of kth Measurement equation, including third state amount combination Z_Sat,k+1、H_Sat,k+1、V_Sat,k+1；

The measurement equation of+1 calculating cycle of the kth is：

Z_Sat,k=H_Sat,kX_k+V_Sat,k

Wherein,

The 3rd measurement matrix H of+1 calculating cycle of kth_Sat,k+1For：

The 3rd measurement noise V of+1 calculating cycle of kth_Sat,k+1For：

In formula, λ_DR,k+1And L_DR,k+1The longitude and latitude of respectively described+1 calculating cycle of kth；λ_Sat,k+1、L_Sat,k+1、 v_eSat,k+1And v_nSat,k+1Longitude, latitude, the east orientation speed of the DVB positioning of respectively described+1 calculating cycle of kth And north orientation speed；N_eSat,k+1、N_nSat,k+1、M_eSat,k+1And M_nSat,k+1Respectively described+1 calculating cycle DVB of kth Longitude, latitude, east orientation speed and north orientation speed error that the location information calculated contains in itself.

Specifically, according to the Vehicular system state equation of+1 calculating cycle of the kth, for+1 calculating of the kth The first state amount combination in cycle and the third state amount combination of+1 calculating cycle of the kth carry out the closed loop Kalman's combined filter, obtains second error correction values of+1 calculating cycle of the kthIncluding：

Second error correction values of+1 calculating cycle of the kth are calculated by equation below

In formula,

Wherein：

Specifically ,+1 calculating of kth according to the second error correction values amendment of+1 calculating cycle of the kth The initial heading angle in cycle, obtains the target course of+1 calculating cycle of the kth, according to+1 calculating cycle of the kth + 1 calculating cycle of the target course and the kth DR models, be calculated the mesh of+1 calculating cycle of the kth The method of mark speed and target location as described by S103.

Therefore, the navigation locating method provided in this example realizes the vehicle location when satellite-signal is invalid Navigation, improves outside the accuracy of vehicle positioning, and equipment is interfered but non-in urban canyons, the overhead satellite-signal such as block Under failure environment, initial heading angle is modified using the second error correction values, improves satellite-signal and be interfered situation Under navigator fix accuracy.

It is above-mentioned mainly the scheme of the embodiment of the present invention to be described from the angle of equipment implementation procedure.May be appreciated It is that equipment it comprises and perform the corresponding hardware configuration of each function and/or software module to realize above-mentioned functions.Ability Field technique personnel should be readily appreciated that, with reference to the unit and algorithm steps of each example of the embodiments described herein description Suddenly, the present invention can be realized with the combining form of hardware or hardware and computer software.Certain function actually with hardware still Computer software drives the mode of hardware performing, depending on the application-specific and design constraint of technical scheme.Professional skill Art personnel can use different methods to realize described function to each specific application, but it is this realize it is not considered that It is beyond the scope of this invention.

The embodiment of the present invention can carry out the division of functional unit according to said method example to equipment, for example, can be right Each function is answered to divide each functional unit, it is also possible to which two or more functions are integrated in a processing unit. Above-mentioned integrated unit both can be realized in the form of hardware, it would however also be possible to employ the form of SFU software functional unit is realized.Need Illustrate, to the division of unit be schematic, only a kind of division of logic function in the embodiment of the present invention, it is actual to realize When can have other dividing mode.

In the case of using integrated unit, Fig. 2A shows that one kind of equipment involved in above-described embodiment may Navigation control device structural representation.Navigation control device 200 includes：Processing unit 202 and communication unit 203.Process Unit 202 is used to be controlled management to the action of navigation control device, and for example, processing unit 202 is for holding equipment execution Step S101 in Fig. 1 is to 104 and/or for other processes of techniques described herein.Communication unit 203 is used to support to move Communication between dynamic terminal and other equipment such as aeronautical satellite.Navigation control device can also include memory cell 201, for depositing The program code and data of storage equipment.

Wherein, processing unit 202 can be processor or controller, for example, can be central processing unit (Central Processing Unit, CPU), general processor, digital signal processor (Digital Signal Processor, DSP), Special IC (Application-Specific Integrated Circuit, ASIC), field programmable gate array It is (Field Programmable Gate Array, FPGA) or other PLDs, transistor logic, hard Part part or its any combination.What it can realize or perform with reference to described by the disclosure of invention various exemplary patrols Collect square frame, module and circuit.The processor can also be the combination for realizing computing function, such as comprising one or more micro- places Reason device combination, combination of DSP and microprocessor etc..Communication unit 203 can be communication interface, transceiver, transmission circuit etc., Wherein, communication interface is to be referred to as, and can include one or more interfaces.Memory cell 201 can be memory.

When processing unit 202 is processor, communication unit 203 is communication interface, when memory cell 401 is memory, this Equipment involved by inventive embodiments can be the navigation control device shown in Fig. 2 B.

Refering to shown in Fig. 2 B, the navigation control device 210 includes：Processor 212, communication interface 213, memory 211.Can Choosing, navigation control device 210 can also include bus 214.Wherein, communication interface 213, processor 212 and memory 211 Can be connected with each other by bus 214；Bus 214 can be Peripheral Component Interconnect standard (Peripheral Component Interconnect, abbreviation PCI) bus or EISA (Extended Industry Standard Architecture, abbreviation EISA) bus etc..The bus 214 can be divided into address bus, data/address bus, controlling bus etc.. For ease of representing, only represented with a thick line in Fig. 2 B, it is not intended that only one bus or a type of bus.

Navigation control device shown in above-mentioned Fig. 2A or Fig. 2 B is it can be appreciated that a kind of dress for navigation control device Put, the embodiment of the present invention is not limited.

The embodiment of the present invention is also disclosed a kind of computer-readable storage medium, wherein, the computer-readable storage medium can be stored with journey Sequence, including the part or all of step of any navigation locating method described in said method embodiment during the program performing Suddenly.

It should be noted that for aforesaid each method embodiment, in order to be briefly described, therefore it is all expressed as a series of Combination of actions, but those skilled in the art should know, the present invention do not limited by described sequence of movement because According to the present invention, some steps can adopt other orders or while carry out.Secondly, those skilled in the art also should know Know, embodiment described in this description belongs to preferred embodiment, involved action and module is not necessarily of the invention It is necessary.

In the above-described embodiments, the description to each embodiment all emphasizes particularly on different fields, without the portion described in detail in certain embodiment Point, may refer to the associated description of other embodiment.

In several embodiments disclosed in this invention, it should be understood that disclosed device, can be by another way Realize.For example, device embodiment described above is only schematic, such as division of described unit, is only one kind Division of logic function, can there is an other dividing mode when actually realizing, such as multiple units or component can with reference to or can To be integrated into another system, or some features can be ignored, or not perform.It is another, it is shown or discussed each other Coupling or direct-coupling or communication connection can be INDIRECT COUPLING or communication connection by some interfaces, device or unit, Can be electrical or other forms.

The unit as separating component explanation can be or may not be it is physically separate, it is aobvious as unit The part for showing can be or may not be physical location, you can with positioned at a place, or can also be distributed to multiple On NE.Some or all of unit therein can according to the actual needs be selected to realize the mesh of this embodiment scheme 's.

In addition, each functional unit in each embodiment of the invention can be integrated in a processing unit, it is also possible to It is that unit is individually physically present, it is also possible to which two or more units are integrated in a unit.Above-mentioned integrated list Unit both can be realized in the form of hardware, it would however also be possible to employ the form of SFU software functional unit is realized.

If the integrated unit is realized using in the form of SFU software functional unit and as independent production marketing or used When, during a computer-readable access to memory can be stored in.Based on such understanding, technical scheme substantially or Person say the part or technical scheme that prior art is contributed all or part can in the form of software product body Reveal and, the computer software product is stored in a memory, including some instructions are used so that a computer equipment (can be personal computer, server or network equipment etc.) performs all or part of each embodiment methods described of the invention Step.And aforesaid memory includes：USB flash disk, read-only storage (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), portable hard drive, magnetic disc or CD etc. are various can be with the medium of store program codes.

The embodiment of the present invention is described in detail above, specific case used herein to the principle of the present invention and Embodiment is set forth, and the explanation of above example is only intended to help and understands the method for the present invention and its core concept； Simultaneously for one of ordinary skill in the art, according to the thought of the present invention, can in specific embodiments and applications There is change part, in sum, this specification content should not be construed as limiting the invention.

Claims (25)

1. a kind of navigation locating method, it is characterised in that include：

When satellite-signal failure is detected, the magnetic heading angle with k-th calculating cycle is counted as observed quantity by described k-th The DR/ magnetic heading Integrated Navigation Algorithm models in calculation cycle obtain the first state amount combination of k-th calculating cycle, with described The lateral velocity of k-th calculating cycle is observed quantity, is calculated by the DR/ vehicle movements integrated navigation of k-th calculating cycle Method model obtains the second quantity of state combination of k-th calculating cycle, and the k is the integer more than 2；

According to the system state equation of k-th calculating cycle, for the first state amount of k-th calculating cycle Second quantity of state combination of combination and k-th calculating cycle carries out closed loop Kalman's combined filter, obtains the kth First error correction values of individual calculating cycle；

The initial heading of k-th calculating cycle according to the first error correction values amendment of k-th calculating cycle Angle, obtains the target course of k-th calculating cycle, according to the target course of k-th calculating cycle and The DR models of k-th calculating cycle, are calculated target velocity and the target location of k-th calculating cycle.

2. method according to claim 1, it is characterised in that the DR/ magnetic heading integrated navigations of k-th calculating cycle Algorithm model is according to the initial heading angle of k-th calculating cycle and the magnetic heading angle of k-th calculating cycle Set up, the magnetic heading angle of k-th calculating cycle be it is calculated according to Magnetic Sensor output data, described k-th The initial heading angle of calculating cycle is the angle of the target course according to -1 calculating cycle of kth and k-th calculating cycle acquisition Speed is calculated, and the DR/ vehicle movement Integrated Navigation Algorithm models of k-th calculating cycle are according to described k-th The lateral velocity of calculating cycle and set up, the lateral velocity of k-th calculating cycle is according to k-th calculating cycle Initial velocity and k-th calculating cycle initial heading angle it is calculated, the initial speed of k-th calculating cycle Degree is according to the initial heading angle of k-th calculating cycle, the target velocity of -1 calculating cycle of kth and described k-th The MEMS inertial sensor accelerometer output valve of calculating cycle is calculated；

The system state equation of k-th calculating cycle is set up according to the quantity of state of -1 calculating cycle of the kth；

The DR models of k-th calculating cycle are built according to the initial velocity and initial position of k-th calculating cycle Vertical, the initial position of k-th calculating cycle is -1 meter of initial velocity and kth according to k-th calculating cycle The target location in calculation cycle is calculated.

3. method according to claim 1, it is characterised in that methods described also includes：

When detecting satellite-signal and being effective, the initial velocity and initial position with+1 calculating cycle of kth passes through as observed quantity The DR/ combinations of satellites navigation algorithm models of+1 calculating cycle of the kth obtain the third state of+1 calculating cycle of the kth Amount combination, the DR/ combinations of satellites navigation algorithm models of+1 calculating cycle of the kth are according to+1 calculating cycle of the kth The initial velocity and the initial position and set up；

According to the system state equation of+1 calculating cycle of the kth, for first shape of+1 calculating cycle of the kth State amount is combined and the third state amount combination of+1 calculating cycle of the kth carries out the closed loop Kalman combined filter, Obtain second error correction values of+1 calculating cycle of the kth；

+ 1 calculating cycle of kth is initial according to the second error correction values amendment of+1 calculating cycle of the kth Course angle, obtains the target course of+1 calculating cycle of the kth, according to the target of+1 calculating cycle of the kth The DR models of+1 calculating cycle of course angle and the kth, are calculated the target velocity and mesh of+1 calculating cycle of the kth Cursor position.

4. method according to claim 1, it is characterised in that the first state amount group of k-th calculating cycle Conjunction includes:The magnetic heading angle combination state quantity measurement Z of k-th calculating cycle_{Mag, k}, the first measurement matrix H_Mag, and described First measurement noise V of k calculating cycle_{Mag, k}；

The Z is calculated based on equation below_{Mag, k}

Z_Mag,k=[Φ_DR,k-Φ_Mag,k]

The H is calculated based on equation below_Mag

H_Mag=[1 000000 0]

The V is calculated based on equation below_{Mag, k}

$V_{Mag,k}=\[{\mathrm{\Φ}}_{DR,k}-\∂{\mathrm{\Φ}}_k-{\mathrm{\Φ}}_{Mag,k}\]$

Wherein, Φ_DR,kFor k-th calculating cycle initial heading angle, Φ_{Mag, k}For the magnetic heading of k-th calculating cycle Angle,For the course angle error that k-th calculating cycle is obtained.

5. method according to claim 1, it is characterised in that the second quantity of state group of k-th calculating cycle Conjunction includes：The vehicle lateral speed state quantity measurement Z of k-th calculating cycle_{Virt, k}, the second amount of k-th calculating cycle Survey matrix H_{Virt, k}, and the second measurement noise V of k-th calculating cycle_{Virt, k}；

The Z is calculated based on equation below_{Virt, k}

$Z_{Virt,k}=\[\∂v_{eDR,k}{\mathrm{cos\Φ}}_{DR,k}-\∂v_{nDR,k}{\mathrm{sin\Φ}}_{DR,k}-v_{eDR,k}{\mathrm{sin\Φ}}_{DR,k}\×\∂{\mathrm{\Φ}}_k-v_{nDR,k}{\mathrm{cos\Φ}}_{DR,k}\×\∂{\mathrm{\Φ}}_k+{\mathrm{\δv}}_{x,k}\]$

The H is calculated based on equation below_{Virt, k}

H_Virt,k=[- ν_eDR,ksinΦ_DR,k-ν_nDR,kcosΦ_DR,k cosΦ_DR,k-sinΦ_DR,k 0 0 0 0 0 0]

The V is calculated based on equation below_{Virt, k}

V_Virt,k=[δ v_x,k]

Wherein,East orientation and north orientation speed error that respectively described k-th calculating cycle is obtained, v_eDR,kFor The initial east orientation speed of k-th calculating cycle；v_nDR,kFor the initial north orientation speed of k-th calculating cycle；δv_x,kFor Due to caused additional lateral velocity variations of breakking away during k-th calculating cycle vehicle movement.

6. method according to claim 3, it is characterised in that the third state amount combination of+1 calculating cycle of the kth Including：The combination state quantity measurement Z of the longitude of+1 calculating cycle of the kth, latitude, east orientation speed and north orientation speed_Sat,k+1, institute State the 3rd measurement matrix H of+1 calculating cycle of kth_Sat,k+1, and the 3rd measurement noise of+1 calculating cycle of the kth V_Sat,k+1；

The Z is calculated based on equation below_Sat,k+1

$Z_{Sat,k+1}=\left[\begin{array}{c}{\mathrm{\λ}}_{DR,k+1}-{\mathrm{\λ}}_{Sat,k+1}\\ L_{DR,k+1}-L_{Sat,k+1}\\ v_{eDR,k+1}-v_{eSat,k+1}\\ v_{nDR,k+1}-v_{nSat,k+1}\end{array}\right]$

The H is calculated based on equation below_Sat,k+1

$H_{Sat,k+1}=\left[\begin{array}{ccccccccc}0& 0& 0& R_n{\mathrm{cosL}}_{Sat,k+1}& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& R_m& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0& 0\end{array}\right]$

The V is calculated based on equation below_Sat,k+1

$V_{Sat,k+1}=\left[\begin{array}{c}{N}_{eSat,k+1}\\ N_{nSat,k+1}\\ M_{eSat,k+1}\\ M_{nSat,k+1}\end{array}\right]$

Wherein, λ_DR,k+1And L_DR,k+1The longitude and latitude of respectively described+1 calculating cycle of kth；λ_Sat,k+1、L_Sat,k+1、 v_eSat,k+1And v_nSat,k+1Longitude, latitude, the east orientation speed of the DVB positioning of respectively described+1 calculating cycle of kth And north orientation speed；R_mAnd R_nRespectively meridian circle radius and prime vertical radius；N_eSat,k+1、N_nSat,k+1、M_eSat,k+1And M_nSat,k+1 Longitude, latitude, east orientation that the location information that respectively described+1 calculating cycle DVB of kth is calculated contains in itself Speed and north orientation speed error.

7. method according to claim 1, it is characterised in that the system state equation of k-th calculating cycle is：

X_k=A_k,k-1X_k-1+G_k-1W_k-1

Wherein, A_k,k-1The state-transition matrix of k calculating cycle, X described in_kFor the system shape of k-th calculating cycle State amount,G_k-1For the system of -1 calculating cycle of the kth Noise matrix, W_k-1For the white noise random error vector of -1 calculating cycle of the kth,WithRespectively described Precision and latitude error that k calculating cycle is obtained, ε_rFor the single order Markov error of gyro, ε_bArbitrary constant error, ▽_x,k And ▽_y,kThe accelerometer dextrad and forward direction single order Markov error of respectively described k-th calculating cycle.

8. method according to claim 1, it is characterised in that the system mode according to k-th calculating cycle Equation, the first state amount combination and second shape of k-th calculating cycle for k-th calculating cycle The combination of state amount carries out the first error correction that closed loop Kalman's combined filter obtains k-th calculating cycle, including：

First error correction values of k-th calculating cycle are calculated by equation below

$P_{k|k-1}=A_{k,k-1}{P}_{k-1|k-1}{A}_{k,k-1}^T+G_{k-1}{Q}_{k-1}{G}_{k-1}^T$

$P_{k|k}=(I-K_k{H}_{Virt/Mag,k})P_{k|k-1}{(I-K_k{H}_{Virt/Mag,k})}^T+K_k{P}_{Virt/Mag,k-1}{K}_k^T$

$K_k=P_{k|k-1}{H}_{Virt/Mag,k}^T{(H_{Virt/Mag,k}{P}_{k|k-1}{H}_{Virt/Mag,k}^T+R_{Virt/Mag,k})}^{-1}$

${\hat{X}}_{k|k,1}=K_k{Z}_{Virt/Mag,k}$

Wherein,For the first error correction values of k-th calculating cycle,

9. method according to claim 3, it is characterised in that according to the system mode side of+1 calculating cycle of kth Journey, the described 3rd of the first state amount combination and+1 calculating cycle of the kth for+1 calculating cycle of the kth the Quantity of state combination carries out the second error correction values that the closed loop Kalman combined filter obtains+1 calculating cycle of the kth, Including：

Second error correction values of+1 calculating cycle of the kth are calculated by equation below

$P_{k+1|k}=A_{k+1,k}{P}_{k|k}{A}_{k+1,k}^T+G_k{Q}_k{G}_k^T$

$P_{k+1|k+1}=(I-K_{k+1}{H}_{Sat/Mag,k+1})P_{k+1|k}{(I-K_{k+1}{H}_{Sat/Mag,k+1})}^T+K_{k+1}{R}_{Sat/Mag,k}{K}_{k+1}^T$

$K_{k+1}=P_{k+1|k}{H}_{Sat/Mag,k+1}^T{(H_{Sat/Mag,k+1}{P}_{k+1|k}{H}_{Sat/Mag,k+1}^T+R_{Sat/Mag,k+1})}^{-1}$

${\hat{X}}_{k+1|k+1,2}=K_{k+1}{Z}_{Sat/Mag,k+1}$

Wherein,For second error correction values of+1 calculating cycle of the kth,

10. method according to claim 1, it is characterised in that described according to described the first of k-th calculating cycle The initial heading angle of k-th calculating cycle described in error correction values amendment is obtaining the bogey heading of k-th calculating cycle Angle, including：

The target course Φ ' of k-th calculating cycle is calculated by equation below_k,

${\mathrm{\Φ}}_k^{\′}={\mathrm{\Φ}}_{DR,k}-\∂{\mathrm{\Φ}}_k$

Wherein, the Φ '_kFor the target course of k-th calculating cycle.

11. methods according to claim 1, it is characterised in that the target according to k-th calculating cycle The DR models of course angle and k-th calculating cycle, are calculated target velocity and the target position of k-th calculating cycle Put, including：

Target east orientation speed v of k-th calculating cycle is calculated by equation below_{E, k}, k-th calculating cycle Target north orientation speed v_{N, k}, the target longitude λ of k-th calculating cycle_kWith the target latitude L of k-th calculating cycle_k,

f_e,k=f '_x,kcosΦ′_k+f′_y,ksinΦ′_k

f_n,k=-f '_x,ksinΦ′_k+f′_y,kcosΦ′_k

v_e,k=v_e,k-1+Tf_e,k

v_n,k=v_n,k-1+Tf_n,k

L_k=L_k-1+Tv_n,k/R_m

λ_k=λ_k-1+Tv_e,k/(R_ncosL_k)

Wherein, f '_{X, k}With f '_{Y, k}The dextrad of accelerometer output in respectively described k-th calculating cycle MEMS inertial sensor With the correction value of forward acceleration value, v_{E, k}For the target east orientation speed of k-th calculating cycle, v_{N, k}For described k-th meter The target north orientation speed in calculation cycle；f_e,kAnd f_n,kThe east orientation of respectively described k-th calculating cycle and the acceleration projection of north orientation； T is sensor sample time interval；L_kFor the target latitude of k-th calculating cycle, λ_kFor k-th calculating cycle Target longitude.

12. methods according to claim 11, it is characterised in that in k-th calculating cycle MEMS inertial sensor The dextrad of accelerometer output and the correction value f ' of forward acceleration value_{X, k}With f '_{Y, k}Pacified by the MEMS inertial sensor respectively Accelerate in k-th calculating cycle MEMS inertial sensor described in pitch angle deviation present in dress process and roll angle drift correction The dextrad and forward acceleration value gained of degree meter output.

13. a kind of equipment, it is characterised in that including processing unit and communication unit,

The processing unit is used for when satellite-signal failure is detected by the communication unit, with k-th calculating cycle Magnetic heading angle is observed quantity, and by the DR/ magnetic heading Integrated Navigation Algorithm models of k-th calculating cycle the kth is obtained Individual calculating cycle first state amount combination, the lateral velocity with k-th calculating cycle as observed quantity, by the kth The DR/ vehicle movement Integrated Navigation Algorithm models of individual calculating cycle obtain the second quantity of state combination of k-th calculating cycle, The k is the integer more than 2；And for according to the system state equation of k-th calculating cycle, for described k-th The first state amount combination of calculating cycle and second quantity of state combination of k-th calculating cycle carry out closed loop card Germania combined filter, obtains the first error correction values of k-th calculating cycle；And for according to described k-th calculating The initial heading angle of k-th calculating cycle described in the first error correction values amendment in cycle, obtains described k-th and calculates week The target course of phase, according to the target course and the DR moulds of k-th calculating cycle of k-th calculating cycle Type, is calculated target velocity and the target location of k-th calculating cycle.

14. equipment according to right 13, it is characterised in that the DR/ magnetic heading integrated navigation of k-th calculating cycle is calculated Method model is built according to the initial heading angle of k-th calculating cycle and the magnetic heading angle of k-th calculating cycle Vertical, the magnetic heading angle of k-th calculating cycle is, k-th meter calculated according to Magnetic Sensor output data The initial heading angle in calculation cycle is the angle speed of the target course according to -1 calculating cycle of kth and k-th calculating cycle acquisition Degree is calculated, and the DR/ vehicle movement Integrated Navigation Algorithm models of k-th calculating cycle are according to described k-th meter The lateral velocity in calculation cycle and set up, the lateral velocity of k-th calculating cycle is according to k-th calculating cycle The initial heading angle of initial velocity and k-th calculating cycle is calculated, the initial velocity of k-th calculating cycle It is according to the initial heading angle of k-th calculating cycle, the target velocity of -1 calculating cycle of kth and k-th meter The MEMS inertial sensor accelerometer output valve in calculation cycle is calculated；

The system state equation of k-th calculating cycle is set up according to the quantity of state of -1 calculating cycle of the kth；

The DR models of k-th calculating cycle are built according to the initial velocity and initial position of k-th calculating cycle Vertical, the initial position of k-th calculating cycle is -1 meter of initial velocity and kth according to k-th calculating cycle The target location in calculation cycle is calculated.

15. equipment according to claim 13, it is characterised in that the processing unit is additionally operable to when by the communication unit Unit detect satellite-signal it is effective when, the initial velocity and initial position with+1 calculating cycle of kth as observed quantity, by described The DR/ combinations of satellites navigation algorithm models of+1 calculating cycle of kth obtain the third state amount group of+1 calculating cycle of the kth Close, the DR/ combinations of satellites navigation algorithm models of+1 calculating cycle of the kth are the institutes according to+1 calculating cycle of the kth State initial velocity and the initial position and set up；

And for according to the system state equation of+1 calculating cycle of the kth, for the institute of+1 calculating cycle of the kth Stating the third state amount combination of the combination of first state amount and+1 calculating cycle of the kth carries out the closed loop Kalman group Filtering is closed, second error correction values of+1 calculating cycle of the kth are obtained；

And for+1 calculating week of kth described in the second error correction values amendment according to+1 calculating cycle of the kth The initial heading angle of phase, obtains the target course of+1 calculating cycle of the kth, according to+1 calculating cycle of the kth The DR models of+1 calculating cycle of the target course and the kth, are calculated the target of+1 calculating cycle of the kth Speed and target location.

16. equipment according to claim 13, it is characterised in that the first state amount of k-th calculating cycle Combination includes:The magnetic heading angle combination state quantity measurement Z of k-th calculating cycle_{Mag, k}, the first measurement matrix H_Mag, and it is described First measurement noise V of k-th calculating cycle_{Mag, k}；

The Z is calculated based on equation below_{Mag, k}

Z_Mag,k=[Φ_DR,k-Φ_Mag,k]

The H is calculated based on equation below_Mag

H_Mag=[1 000000 0]

The V is calculated based on equation below_{Mag, k}

$V_{Mag,k}=\[{\mathrm{\Φ}}_{DR,k}-\∂{\mathrm{\Φ}}_k-{\mathrm{\Φ}}_{Mag,k}\]$

Wherein, Φ_DR,kFor k-th calculating cycle initial heading angle, Φ_{Mag, k}For the magnetic heading of k-th calculating cycle Angle,For the course angle error that k-th calculating cycle is obtained.

17. equipment according to claim 13, it is characterised in that second quantity of state of k-th calculating cycle Combination includes：The vehicle lateral speed state quantity measurement Z of k-th calculating cycle_{Virt, k}, the second of k-th calculating cycle Measurement matrix H_{Virt, k}, and the second measurement noise V of k-th calculating cycle_{Virt, k}；

The Z is calculated based on equation below_{Virt, k}

$Z_{Virt,k}=\[\∂v_{eDR,k}{\mathrm{cos\Φ}}_{DR,k}-\∂v_{nDR,k}{\mathrm{sin\Φ}}_{DR,k}-v_{eDR,k}{\mathrm{sin\Φ}}_{DR,k}\×\∂{\mathrm{\Φ}}_k-v_{nDR,k}{\mathrm{cos\Φ}}_{DR,k}\×\∂{\mathrm{\Φ}}_k+{\mathrm{\δv}}_{x,k}\]$

The H is calculated based on equation below_{Virt, k}

H_Virt,k=[- ν_eDR,ksinΦ_DR,k-ν_nDR,kcosΦ_DR,k cosΦ_DR,k-sinΦ_DR,k 0 0 0 0 0 0]

The V is calculated based on equation below_{Virt, k}

V_Virt,k=[δ v_x,k]

Wherein,East orientation and north orientation speed error that respectively described k-th calculating cycle is obtained, v_eDR,kFor The initial east orientation speed of k-th calculating cycle；v_nDR,kFor the initial north orientation speed of k-th calculating cycle；δv_x,kFor Due to caused additional lateral velocity variations of breakking away during k-th calculating cycle vehicle movement.

18. equipment according to claim 15, it is characterised in that the third state amount group of+1 calculating cycle of the kth Conjunction includes：The combination state quantity measurement Z of the longitude of+1 calculating cycle of the kth, latitude, east orientation speed and north orientation speed_Sat,k+1, The 3rd measurement matrix H of+1 calculating cycle of the kth_Sat,k+1, and the 3rd measurement noise of+1 calculating cycle of the kth V_Sat,k+1；

The Z is calculated based on equation below_Sat,k+1

$Z_{Sat,k+1}=\left[\begin{array}{c}{\mathrm{\λ}}_{DR,k+1}-{\mathrm{\λ}}_{Sat,k+1}\\ L_{DR,k+1}-L_{Sat,k+1}\\ v_{eDR,k+1}-v_{eSat,k+1}\\ v_{nDR,k+1}-v_{nSat,k+1}\end{array}\right]$

The H is calculated based on equation below_Sat,k+1

$H_{Sat,k+1}=\left[\begin{array}{ccccccccc}0& 0& 0& R_n{\mathrm{cosL}}_{Sat,k+1}& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& R_m& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0& 0\end{array}\right]$

The V is calculated based on equation below_Sat,k+1

$V_{Sat,k+1}=\left[\begin{array}{c}{N}_{eSat,k+1}\\ N_{nSat,k+1}\\ M_{eSat,k+1}\\ M_{nSat,k+1}\end{array}\right]$

Wherein, λ_DR,k+1And L_DR,k+1The longitude and latitude of respectively described+1 calculating cycle of kth；λ_Sat,k+1、L_Sat,k+1、 v_eSat,k+1And v_nSat,k+1Longitude, latitude, the east orientation speed of the DVB positioning of respectively described+1 calculating cycle of kth And north orientation speed；R_mAnd R_nRespectively meridian circle radius and prime vertical radius；N_eSat,k+1、N_nSat,k+1、M_eSat,k+1And M_nSat,k+1 Longitude, latitude, east orientation that the location information that respectively described+1 calculating cycle DVB of kth is calculated contains in itself Speed and north orientation speed error.

19. equipment according to claim 13, it is characterised in that the system state equation of k-th calculating cycle is：

X_k=A_k,k-1X_k-1+G_k-1W_k-1

Wherein, A_k,k-1The state-transition matrix of k calculating cycle, X described in_kFor the system shape of k-th calculating cycle State amount,G_k-1For the system of -1 calculating cycle of the kth Noise matrix, W_k-1For the white noise random error vector of -1 calculating cycle of the kth,WithRespectively described Precision and latitude error that k calculating cycle is obtained, ε_rFor the single order Markov error of gyro, ε_bArbitrary constant error, ▽_x,k And ▽_y,kThe accelerometer dextrad and forward direction single order Markov error of respectively described k-th calculating cycle.

20. equipment according to claim 13, it is characterised in that the processing unit is based on according to described k-th The system state equation in calculation cycle, for the first state amount combination and described k-th calculating of k-th calculating cycle Second quantity of state combination in cycle carries out the first error that closed loop Kalman's combined filter obtains k-th calculating cycle Amendment aspect, the processing unit is additionally operable to：

First error correction values of k-th calculating cycle are calculated by equation below

$P_{k|k-1}=A_{k,k-1}{P}_{k-1|k-1}{A}_{k,k-1}^T+G_{k-1}{Q}_{k-1}{G}_{k-1}^T$

$P_{k|k}=(I-K_k{H}_{Virt/Mag,k})P_{k|k-1}{(I-K_k{H}_{Virt/Mag,k})}^T+K_k{P}_{Virt/Mag,k-1}{K}_k^T$

$K_k=P_{k|k-1}{H}_{Virt/Mag,k}^T{(H_{Virt/Mag,k}{P}_{k|k-1}{H}_{Virt/Mag,k}^T+R_{Virt/Mag,k})}^{-1}$

${\hat{X}}_{k|k,1}=K_k{Z}_{Virt/Mag,k}$

Wherein,For the first error correction values of k-th calculating cycle,

21. equipment according to claim 15, it is characterised in that the processing unit is for according to the kth+1 The system state equation of calculating cycle, the first state amount combination and the kth+1 for+1 calculating cycle of the kth The third state amount combination of individual calculating cycle carries out the closed loop Kalman combined filter and obtains+1 calculating week of the kth The second error correction values aspect of phase, the processing unit is additionally operable to：

Second error correction values of+1 calculating cycle of the kth are calculated by equation below

$P_{k+1|k}=A_{k+1,k}{P}_{k|k}{A}_{k+1,k}^T+G_k{Q}_k{G}_k^T$

$P_{k+1|k+1}=(I-K_{k+1}{H}_{Sat/Mag,k+1})P_{k+1|k}{(I-K_{k+1}{H}_{Sat/Mag,k+1})}^T+K_{k+1}{R}_{Sat/Mag,k}{K}_{k+1}^T$

$K_{k+1}=P_{k+1|k}{H}_{Sat/Mag,k+1}^T{(H_{Sat/Mag,k+1}{P}_{k+1|k}{H}_{Sat/Mag,k+1}^T+R_{Sat/Mag,k+1})}^{-1}$

${\hat{X}}_{k+1|k+1,2}=K_{k+1}{Z}_{Sat/Mag,k+1}$

Wherein,For second error correction values of+1 calculating cycle of the kth,

22. equipment according to claim 13, it is characterised in that the processing unit is for described according to the kth The initial heading angle of k-th calculating cycle described in the first error correction values amendment of individual calculating cycle is obtaining the kth The target course aspect of individual calculating cycle, the processing unit is additionally operable to：

The target course Φ ' of k-th calculating cycle is calculated by equation below_k,

${\mathrm{\Φ}}_k^{\′}={\mathrm{\Φ}}_{DR,k}-\∂\mathrm{\Φ}$

Wherein, the Φ '_kFor the target course of k-th calculating cycle.

23. equipment according to claim 13, it is characterised in that the processing unit is for described according to the kth The DR models of the target course of individual calculating cycle and k-th calculating cycle, are calculated described k-th and calculate week The target velocity of phase and target location aspect, the processing unit is additionally operable to：

Target east orientation speed V of k-th calculating cycle is calculated by equation below_{E, k}, k-th calculating cycle Target north orientation speed v_{N, k}, the target longitude λ of k-th calculating cycle_kWith the target latitude L of k-th calculating cycle_k,

f_e,k=f '_x,kcosΦ′_k+f′_y,ksinΦ′_k

f_n,k=-f '_x,ksinΦ′_k+f′_y,kcosΦ′_k

v_e,k=v_e,k-1+Tf_e,k

v_n,k=v_n,k-1+Tf_n,k

L_k=L_k-1+Tv_n,k/R_m

λ_k=λ_k-1+Tv_e,k/(R_ncosL_k)

Wherein, f '_{X, k}With f '_{Y, k}The dextrad of accelerometer output in respectively described k-th calculating cycle MEMS inertial sensor With the correction value of forward acceleration value, v_{E, k}For the target east orientation speed of k-th calculating cycle, v_{N, k}For described k-th meter The target north orientation speed in calculation cycle；f_e,kAnd f_n,kThe east orientation of respectively described k-th calculating cycle and the acceleration projection of north orientation； T is sensor sample time interval；L_kFor the target latitude of k-th calculating cycle, λ_kFor k-th calculating cycle Target longitude.

24. methods according to claim 23, it is characterised in that in k-th calculating cycle MEMS inertial sensor The dextrad of accelerometer output and the correction value f ' of forward acceleration value_{X, k}With f '_{Y, k}Pacified by the MEMS inertial sensor respectively Accelerate in k-th calculating cycle MEMS inertial sensor described in pitch angle deviation present in dress process and roll angle drift correction The dextrad and forward acceleration value gained of degree meter output.

25. a kind of equipment, it is characterised in that include：

Processor, memory, communication interface and communication bus, the processor, the memory and the communication interface pass through The communication bus connects and completes mutual communication；

The memory storage has executable program code, and the communication interface is used for radio communication；

The processor is used to call the executable program code in the memory, performs as claim 1-12 is arbitrary Method described by.