CN106646569B

CN106646569B - Navigation positioning method and equipment

Info

Publication number: CN106646569B
Application number: CN201611034607.7A
Authority: CN
Inventors: 许建新; 田军; 熊智
Original assignee: Huawei Technologies Co Ltd
Current assignee: Haisi Technology Co ltd
Priority date: 2016-11-18
Filing date: 2016-11-18
Publication date: 2020-04-14
Anticipated expiration: 2036-11-18
Also published as: CN106646569A

Abstract

The embodiment of the invention discloses a navigation positioning method and equipment, which comprises the following steps: when the satellite signal failure is detected, a magnetic heading angle of a k-th calculation period is used as an observed quantity, a first state quantity combination is obtained through a DR/magnetic heading combined navigation algorithm model, and a second state quantity combination is obtained through the DR/vehicle motion combined navigation algorithm model by using a transverse speed of the k-th calculation period as the observed quantity; according to a system state equation of a kth calculation period, closed loop Kalman combination filtering is carried out on a first state quantity combination and a second state quantity combination to obtain a first error correction value; and correcting the initial course angle of the kth calculation period according to the first error correction value to obtain a target course angle, and calculating to obtain a target speed and a target position according to the target course angle and the DR model of the kth calculation period. By adopting the embodiment of the invention, the vehicle positioning navigation when the satellite signal is invalid is realized, and the accuracy of vehicle positioning is improved.

Description

Navigation positioning method and equipment

Technical Field

The invention relates to the technical field of vehicle navigation, in particular to a navigation positioning method and equipment.

Background

The vehicle-mounted navigation positioning technology is an important component of an intelligent traffic system and is also one of key technologies. The vehicle navigation requires that the position of the vehicle can be continuously, accurately determined in real time, and currently, the navigation modes available for the vehicle navigation technology are as follows: satellite navigation (e.g., global positioning system, beidou satellite navigation system), odometers, micro-electro-mechanical Systems (MEMS) sensors, magnetic sensors, map matching, and the like. The satellite navigation positioning precision is high, the technology is mature, the use is convenient, but the satellite navigation positioning device is easy to be shielded by high buildings, elevated frames, tunnels, underground garages and the like to fail; the Dead-reckoning (DR) algorithm for navigation and positioning by using the odometer and the MEMS sensor is a low-cost and autonomous vehicle navigation and positioning mode, has the advantages of strong anti-interference capability, capability of providing more accurate navigation parameters of a vehicle in a short time according to provided sensor data, accumulated errors of the DR algorithm along with time and unsuitability for long-time independent navigation; the map matching algorithm is mainly used as an auxiliary navigation means, and the navigation result is subjected to auxiliary correction by combining the road route condition in the digital map so as to improve the positioning accuracy, but has certain limitation.

At present, most of domestic and foreign vehicle navigation systems adopt a mode of combining satellite navigation and other positioning methods to realize navigation and positioning of vehicles, and the following two combination modes are mainly adopted: a map matching/satellite combined navigation system and a DR/satellite combined navigation system, but in the case of interference or even failure of satellite signals, the navigation error of the two combined navigation systems will be significantly increased or even unusable.

Disclosure of Invention

The embodiment of the invention discloses a navigation positioning method and equipment, aiming at realizing vehicle positioning navigation when a navigation signal is invalid and improving the accuracy of vehicle navigation positioning.

In a first aspect, an embodiment of the invention discloses a navigation positioning method. The method comprises the following steps:

when satellite signal failure is detected, taking a magnetic heading angle of a k-th calculation period as an observed quantity, obtaining a first state quantity combination of the k-th calculation period through a Dead-reckoning (DR) and magnetic heading combined navigation algorithm model of the k-th calculation period, taking a transverse speed of the k-th calculation period as the observed quantity, obtaining a second state quantity combination of the k-th calculation period through the DR and vehicle motion combined navigation algorithm model of the k-th calculation period, wherein k is an integer greater than 2;

according to the system state equation of the kth calculation period, carrying out closed-loop Kalman combination filtering on the first state quantity combination of the kth calculation period and the second state quantity combination of the kth calculation period to obtain a first error correction value of the kth calculation period;

correcting the initial course angle of the k-th calculation period according to the first error correction value of the k-th calculation period to obtain a target course angle of the k-th calculation period, and calculating to obtain a target speed and a target position of the k-th calculation period according to the target course angle of the k-th calculation period and a DR model of the k-th calculation period.

In view of the above, in the navigation and positioning method disclosed in the embodiment of the present invention, when a device detects that a satellite signal is invalid, a magnetic heading angle of a kth calculation period is taken as an observed quantity, a first state quantity combination is obtained through a DR/magnetic heading combined navigation algorithm model, and a second state quantity combination is obtained through the DR/vehicle motion combined navigation algorithm model with a lateral speed of the kth calculation period as the observed quantity; according to a system state equation of a kth calculation period, closed loop Kalman combination filtering is carried out on a first state quantity combination and a second state quantity combination to obtain a first error correction value; and correcting the initial course angle of the kth calculation period according to the first error correction value to obtain a target course angle, and calculating to obtain a target speed and a target position according to the target course angle and the DR model of the kth calculation period. Therefore, the course angle is corrected by the DR/magnetic course combined navigation algorithm model and the DR/vehicle motion combined navigation algorithm model when the satellite signal fails, and then the target speed and the target position information are calculated according to the DR model, so that the vehicle positioning navigation when the satellite signal fails is realized, and the vehicle positioning accuracy is improved.

In one possible design, the DR/magnetic heading combined navigation algorithm model of the kth calculation period is established according to the initial heading angle of the kth calculation period and the magnetic heading angle of the kth calculation period, the magnetic heading angle of the kth calculation period is calculated according to the output data of the magnetic sensor, the initial heading angle of the kth calculation period is calculated according to the target heading angle of the kth-1 calculation period and the angular velocity obtained in the kth calculation period, the DR/vehicle motion combined navigation algorithm model of the kth calculation period is established according to the lateral velocity of the kth calculation period, the lateral velocity of the kth calculation period is calculated according to the initial velocity of the kth calculation period and the initial heading angle of the kth calculation period, the initial speed of the k-th calculation period is calculated according to the initial course angle of the k-th calculation period, the target speed of the k-1 th calculation period and the output value of the MEMS inertial sensor accelerometer of the k-th calculation period;

the system state equation of the kth computing cycle is established according to the state quantity of the kth-1 computing cycle;

the DR model of the k-th calculation period is established according to the initial speed and the initial position of the k-th calculation period, and the initial position of the k-th calculation period is calculated according to the initial speed and the target position of the k-1-th calculation period.

In one possible design, the method further includes:

when the satellite signal is detected to be effective, taking the initial velocity and the initial position of the (k + 1) th calculation period as observed quantities, and obtaining a third state quantity combination of the (k + 1) th calculation period through a DR/satellite combined navigation algorithm model of the (k + 1) th calculation period, wherein the DR/satellite combined navigation algorithm model of the (k + 1) th calculation period is established according to the initial velocity and the initial position of the (k + 1) th calculation period;

according to the system state equation of the (k + 1) th calculation period, performing the closed-loop Kalman combined filtering on the first state quantity combination of the (k + 1) th calculation period and the third state quantity combination of the (k + 1) th calculation period to obtain a second error correction value of the (k + 1) th calculation period;

correcting the initial course angle of the (k + 1) th calculation period according to the second error correction value of the (k + 1) th calculation period to obtain a target course angle of the (k + 1) th calculation period, and calculating to obtain a target speed and a target position of the (k + 1) th calculation period according to the target course angle of the (k + 1) th calculation period and a DR model of the (k + 1) th calculation period.

Therefore, the navigation positioning method disclosed in the possible design not only realizes vehicle positioning navigation when the satellite signal is invalid and improves the accuracy of vehicle positioning, but also corrects the course angle by using the second error correction value in the non-failure environment where the satellite signal is interfered, such as urban canyon and overhead shielding, and improves the accuracy of navigation positioning when the satellite signal is interfered.

In one possible design, the first state quantity combination of the k-th calculation cycle includes a magnetic heading angle combination measurement state Z of the k-th calculation cycle_Mag，kThe first measurement matrix H_MagAnd a first measurement noise V of said k-th calculation cycle_Mag，k；

Calculating the Z based on the formula_Mag，k

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

Calculating the H based on the formula_Mag

H_Mag＝[1 0 0 0 0 0 0 0]

Calculating the V based on the formula_Mag，k

Wherein phi_DR,kCalculating a periodic initial course angle, Φ, for said kth_Mag，kFor the magnetic heading angle of the k-th calculation cycle,

the heading angle error obtained for the k-th calculation cycle.

In one possible design, the second state quantity combination for the kth computation cycle includes: the vehicle lateral velocity measurement state Z of the k-th calculation cycle_Virt，kA second measurement matrix H of the k-th calculation cycle_Virt，kAnd a second measurement noise V of said k-th calculation cycle_Virt，k；

Calculating the Z based on the formula_Virt，k

Calculating the H based on the formula_Virt，k

H_Virt,k＝[-ν_eDR,ksinΦ_DR,k-ν_nDR,kcosΦ_DR,kcosΦ_DR,k-sinΦ_DR,k0 0 0 0 0 0]

Calculating the V based on the formula_Virt，k

V_Virt,k＝[δv_x,k]

Wherein,

respectively east and north velocity errors, v, obtained in the k-th calculation cycle_eDR,kCalculating an initial east velocity for the kth calculation cycle; v. of_nDR,kCalculating an initial northbound speed for the kth cycle; delta v_x,kAn additional lateral velocity change due to side slip while the vehicle is in motion is calculated for the k-th cycle.

In one possible design, the third state quantity combination of the (k + 1) th calculation cycle includes: the combined measurement state Z of longitude, latitude, east speed and north speed of the k +1 th calculation cycle_Sat,k+1The third measurement matrix H of the k +1 th calculation cycle_Sat,k+1And a third measurement noise V of said (k + 1) th calculation cycle_Sat,k+1；

Calculating the Z based on the formula_Sat,k+1

Calculating the H based on the formula_Sat,k+1

Calculating the V based on the formula_Sat,k+1

Wherein λ is_DR,k+1And L_DR,k+1Longitude and latitude of the k +1 th calculation cycle, respectively; lambda [ alpha ]_Sat,k+1、L_Sat,k+1、v_eSat,k+1And v_nSat,k+1Respectively calculating longitude, latitude, east speed and north speed of the satellite receiver positioning in the (k + 1) th calculation period; r_mAnd R_nRespectively the radius of a meridian circle and the radius of a prime unit circle; n is a radical of_eSat,k+1、N_nSat,k+1、M_eSat,k+1And M_nSat,k+1The positioning information itself calculated for the (k + 1) th calculation cycle satellite receiver contains longitude, latitude, east-direction velocity and north-direction velocity errors.

As can be seen from the above, the navigation and positioning method disclosed in this possible design, in addition to realizing vehicle positioning navigation when the satellite signal is invalid and improving the accuracy of vehicle positioning, the device continuously updates the first state quantity combination, the second state quantity combination, and the third state quantity combination by the initial speed and the initial position updated in each calculation cycle, so that the first correction value is improved, the output accuracy of the second correction value is improved, and the continuous and reliable output of the navigation information in each calculation cycle is maintained.

In one possible design, the system state equation for the kth computation cycle is:

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

wherein A is_k,k-1For the state transition matrix of the k-th calculation cycle, X_kFor the system state quantity of the k-th calculation cycle,

G_k-1calculating a system noise matrix, W, for said k-1 th calculation cycle_k-1Calculating white for the k-1 th cycleThe noise is a random error vector that is,

and

respectively the precision and latitude error, epsilon, obtained for the k-th calculation cycle_rFirst order Markov error of gyro, ε_bThe error of the random constant is determined,

and

the right and forward first order markov errors of the accelerometer for the kth computation cycle, respectively.

In one possible design, the performing, according to the system state equation of the kth computation cycle, closed-loop kalman filtering on the first state quantity combination of the kth computation cycle and the second state quantity combination of the kth computation cycle to obtain a first error correction of the kth computation cycle includes:

calculating a first error correction value of the k-th calculation period by the following formula

Wherein the first error correction value for the k-th calculation cycle,

in one possible design, according to the system state equation of the (k + 1) th calculation cycle, performing the closed-loop kalman filtering combination on the first state quantity combination of the (k + 1) th calculation cycle and the third state quantity combination of the (k + 1) th calculation cycle to obtain a second error correction value of the (k + 1) th calculation cycle includes:

calculating a second error correction value of the (k + 1) th calculation cycle by the following formula

Wherein the second error correction value is the second error correction value of the (k + 1) th calculation cycle,

in one possible design, the correcting the initial course angle of the k-th calculation cycle according to the first error correction value of the k-th calculation cycle to obtain the target course angle of the k-th calculation cycle includes:

calculating to obtain a target heading angle phi 'of the kth calculation period through the following formula'_k，

Wherein, the phi'_kAnd calculating the target course angle of the k-th calculation period.

Therefore, the navigation positioning method disclosed in the possible design not only realizes vehicle positioning navigation when the satellite signal is invalid and improves the accuracy of vehicle positioning, but also corrects the course angle through the closed-loop combined Kalman filtering algorithm, and further improves the output accuracy of navigation positioning information.

In a possible design, the calculating the target speed and the target position in the kth calculation period according to the target heading angle in the kth calculation period and the DR model in the kth calculation period includes:

calculating the target east speed v of the k-th calculation period by the following formula_e，kSaid k-th calculated cycle target north velocity v_n，kTarget longitude λ of the k-th calculation cycle_kAnd a target latitude L of the k-th calculation 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，kAnd f'_y，kRespectively calculating the correction values of the right and forward acceleration values output by the accelerometer in the MEMS inertial sensor in the k-th calculation period, v_e，kCalculating a target east velocity, v, for the kth calculation cycle_n，kCalculating a target northbound speed for the kth cycle; f. of_e,kAnd f_n,kAcceleration projections in the east and north directions, respectively, of the kth computation cycle; t is a sensor sampling time interval; l is_kCalculating a target latitude, λ, for the kth calculation cycle_kA target longitude of the cycle is calculated for the kth.

In this possible design, the k-th calculation cycle is a correction value f 'of the right and forward acceleration values output by the accelerometer in the MEMS inertial sensor'_x，kAnd f'_y，kAnd correcting the right and forward acceleration values output by an accelerometer in the MEMS inertial sensor in the k-th calculation period according to the pitch angle deviation and the roll angle deviation existing in the installation process of the MEMS inertial sensor respectively.

Therefore, the navigation positioning method disclosed in the possible design not only realizes vehicle positioning navigation when satellite signals are invalid and improves the accuracy of vehicle positioning, but also improves the output accuracy of the horizontal accelerometer of the MEMS inertial sensor by correcting pitch angle deviation and roll angle deviation existing in the installation process of the low-accuracy MEMS inertial sensor, thereby improving the target speed and the target position output accuracy of navigation positioning.

In a second aspect, an embodiment of the present invention discloses an apparatus having a function of implementing a behavior of an apparatus in the above method design. The functions can be realized by hardware, and the functions can also be realized by executing corresponding software by hardware. The hardware or software includes one or more modules corresponding to the above-described functions.

In one possible design, the device includes a processor configured to enable the device to perform the corresponding functions in the above-described method. Further, the device may also include a receiver and a transmitter for communication between the device and other devices, such as navigation satellites. Further, the device may also include a memory for coupling with the processor that retains program instructions and data necessary for the device.

In a third aspect, an embodiment of the present invention discloses a computer-readable storage medium, where the computer-readable storage medium stores a program code for a computer device to execute, where the program code specifically includes an execution instruction, and the execution instruction is used to execute some or all of the steps described in any method of the first aspect of the embodiment of the present invention.

Drawings

In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, it is obvious that the drawings in the following description are only some embodiments of the present invention, and for those skilled in the art, other drawings can be obtained according to the drawings without creative efforts.

FIG. 1 is a schematic flow chart of a navigation positioning method according to an embodiment of the present invention;

FIG. 2A is a block diagram of functional units of a navigation control device according to an embodiment of the present invention;

fig. 2B is a schematic structural diagram of a navigation control device according to an embodiment of the present invention.

Detailed Description

The technical solution in the embodiment of the present invention is described below with reference to the drawings in the embodiment of the present invention.

Referring to fig. 1, fig. 1 is a schematic flow chart of a navigation and positioning method according to an embodiment of the present invention, where the method is applied to a navigation and positioning system, and the navigation and positioning system may be composed of a navigation display screen, a navigation control device, a satellite receiver, and sensors (such as a magnetic sensor, an inertial sensor (an accelerometer and a gyroscope), and the like) installed in a vehicle, as shown in the figure, the navigation and positioning method includes:

s101, when a navigation control device detects that a satellite signal is invalid, a magnetic heading angle of a k-th calculation period is used as an observed quantity, a first state quantity combination of the k-th calculation period is obtained through a Dead-reckoning (DR) and magnetic heading combined navigation algorithm model of the k-th calculation period, a transverse speed of the k-th calculation period is used as an observed quantity, and a second state quantity combination of the k-th calculation period is obtained through a DR and vehicle motion combined navigation algorithm model of the k-th calculation period;

wherein k is an integer greater than 2, the DR/magnetic heading combined navigation algorithm model of the k-th calculation period is established according to the initial heading angle of the k-th calculation period and the magnetic heading angle of the k-th calculation period, the magnetic heading angle of the k-th calculation period is calculated according to the output data of the magnetic sensor, the initial heading angle of the k-th calculation period is calculated according to the target heading angle of the k-1-th calculation period and the angular velocity obtained in the k-th calculation period, the DR/vehicle motion combined navigation algorithm model of the k-th calculation period is established according to the lateral velocity of the k-th calculation period, and the lateral velocity of the k-th calculation period is calculated according to the initial velocity of the k-th calculation period and the initial heading angle of the k-th calculation period, and the initial speed of the k calculation period is calculated according to the initial course angle of the k calculation period, the target speed of the k-1 calculation period and the output value of the MEMS inertial sensor accelerometer of the k calculation period.

Specifically, the DR/magnetic heading combined navigation algorithm model of the kth calculation period is established according to the initial heading angle of the kth calculation period and the magnetic heading angle of the kth calculation period, and includes:

firstly, the navigation control device calculates the magnetic heading angle phi of the k calculation period according to the following formula according to the output data of the magnetic sensor in the horizontal direction of the k calculation period_Mag，k：

In the formula, mag_x，kAnd mag_y，kOutput data of the magnetic sensors in the X axis and the Y axis are calculated for the k-th calculation cycle, respectively.

Meanwhile, the initial heading angle of the k-th calculation cycle and the heading angle information output by the magnetic sensor of the k-th calculation cycle can be respectively represented as:

Φ_Mag,k＝Φ_k-E_Mag,k

in the formula phi_DR,kFor the initial heading angle for the kth calculation cycle,

for course angle error obtained in the k-th calculation cycle, E_Mag，kFor the magnetic heading angle error, phi, obtained for the kth calculation cycle_kThe true value of the heading angle representing the k-th computation cycle.

Wherein, the recursion formula of the initial course angle of the kth calculation period is as follows:

Φ_DR,k＝Φ′_k-1-Tω_z,k

wherein the initial value of the initial course angle of the first calculation period is approximately equal to the magnetic course angle of the first calculation period, and the magnetic course angle of the first calculation period is obtained by a magnetic sensor of the navigation positioning system by phi'_k-1Calculate the target heading angle, ω, for the k-1 th calculation cycle_z,kCalculating angular velocity of a gyroscope output in the MEMS inertial sensor in a vertical direction for a kth cycle.

Secondly, the navigation control equipment establishes a k-th calculation period magnetic heading angle combined measurement state equation comprising a first state quantity combination Z_Mag，k、H_Mag、V_Mag，k；

Wherein, the magnetic heading angle combination measurement state equation in the kth calculation period is as follows:

wherein the first measurement matrix H_MagComprises the following steps:

H_Mag＝[1 0 0 0 0 0 0 0]

first measurement noise V of k-th calculation period_Mag，kComprises the following steps:

specifically, the DR/vehicle motion combined navigation algorithm model of the kth calculation cycle is established according to the lateral velocity of the kth calculation cycle, and includes:

firstly, the navigation control device establishes a measurement state equation model of the vehicle motion speed in the k-th calculation period:

initial east velocity v from the kth calculation cycle_eDR，kAnd an initial northbound velocity v_nDR，kCalculating the k-th calculation cycleThe vehicle lateral speed parameter of (a) is,

wherein v is_eDR，k，v_nDR，kThe east and north velocity errors obtained in the k-th calculation cycle are obtained respectively.

Secondly, the navigation control device performs minimum quantitative expansion on the transverse speed to obtain a linear measurement state equation:

thirdly, under the condition of actual vehicle motion, considering factors such as uneven road surface, cornering side slip and the like, the transverse velocity constraint equation of the k-th calculation period is as follows:

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

in the formula, v_x,kFor the true value of the lateral movement speed, δ v, of the k-th calculation cycle_x,kThe method is characterized in that the method represents the additional lateral velocity change caused by sideslip when the vehicle moves in the k-th calculation period, the additional lateral velocity change is modeled as white noise, the value size of the white noise is related to the specific motion characteristic of the vehicle, and when the vehicle runs at a high speed, the sideslip speed is high.

Finally, the navigation control device may construct a virtual lateral velocity measurement equation for the k-th calculation cycle according to the linearized lateral velocity measurement state equation and the lateral velocity constraint equation as follows:

so as to obtain a vehicle transverse speed measurement state equation of the k calculation period, including a second state quantity combination Z_Virt，k、H_Virt，k、V_Virt，k。

The vehicle lateral velocity measurement state equation of the kth calculation period is as follows:

Z_Virt,k＝H_Virt,kX_k+V_Virt,k

wherein, the second measurement matrix H of the k-th calculation cycle_Virt,kComprises the following steps:

second measurement noise V of k-th calculation period_Virt,kComprises the following steps:

V_Virt,k＝[δv_x,k]

s102, the navigation control equipment performs closed-loop Kalman combination filtering on the first state quantity combination of the kth calculation period and the second state quantity combination of the kth calculation period according to a system state equation of the kth calculation period to obtain a first error correction value of the kth calculation period;

wherein the system state equation is:

in the formula, a is a state transition matrix, G is a system noise matrix, and W is a white noise random error vector, wherein:

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

in the formula (f)_xAnd f_yRespectively obtaining a right acceleration value and a forward acceleration value output by an accelerometer of the MEMS inertial sensor in the DR model under the static condition; r_mAnd R_nRespectively being radius of meridian and Mao unitaryThe radius of the ring; l is_DRIs the initial latitude; v. of_eDRIs the initial east speed; omega_gzIs white noise of gyro, omega_rzDriving white noise, omega, for gyromagnet first order Markov_axAnd ω_ayDriving white noise for the accelerometer first order markov; t is_εAnd T_aThe first order markov correlation times of the gyroscope and accelerometer in the MEMS inertial sensor, respectively.

Wherein the system state equation is in terms of heading angle error

Horizontal velocity error

And

position error

And

first order Markov error epsilon of gyroscope_rAnd random constant error epsilon_bAnd accelerometer first order Markov error

And

establishing a system state equation as a system state quantity, wherein the system state quantity is as follows:

discretizing the system state equation to obtain a system state equation of a k-th calculation period:

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

wherein,

specifically, when the satellite signal fails, the navigation control device performs closed-loop kalman filtering on the first state quantity combination of the kth calculation period and the second state quantity combination of the kth calculation period according to the vehicle system state equation of the kth calculation period to obtain a first error correction value of the kth calculation period

The method comprises the following steps:

in the formula,

wherein:

the initial values of P, Q are related to the error parameters of the MEMS sensor.

S103, the navigation control equipment corrects the initial course angle of the k-th calculation period according to the first error correction value of the k-th calculation period to obtain a target course angle of the k-th calculation period, and calculates the target speed and the target position of the k-th calculation period according to the target course angle of the k-th calculation period and the DR model of the k-th calculation period.

The DR model of the k-th computing cycle is established according to the initial speed and the initial position of the k-th computing cycle, and the initial position of the k-th computing cycle is calculated according to the initial speed and the target position of the k-1-th computing cycle.

Firstly, the navigation control device corrects the initial course angle of the k-th calculation period according to the first error correction value of the k-th calculation period to obtain the target course angle of the k-th calculation period, and the method comprises the following steps:

of said first error correction values over said k-th calculation cycle

Calculating to obtain a target heading angle phi 'of the kth calculation period according to the formula'_k，

Secondly, the step of calculating by the navigation control device according to the target course angle of the kth calculation period and the DR model of the kth calculation period to obtain the target speed and the target position of the kth calculation period comprises:

establishing a DR model of the k-th calculation period according to the initial speed and the initial position of the k-th calculation period;

specifically, a northeast coordinate system is selected as a navigation coordinate system of the DR model in the kth calculation period, and a carrier coordinate system is selected as a front-right upper coordinate system;

calculating a target east speed v of the k-th calculation period by using the following DR model formula of the k-th calculation period_e，kSaid k-th calculated cycle target north velocity v_n,kTarget longitude λ of the k-th calculation cycle_kAnd a target latitude L of the k-th calculation 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,kAnd f'_y,kRespectively calculating the correction values of the right and forward acceleration values output by the accelerometer in the MEMS inertial sensor in the k-th calculation period, v_e,kCalculating a target east velocity, v, for the kth calculation cycle_n,kCalculating a target northbound speed for the kth cycle; f. of_e,kAnd f_n,kAcceleration projections in the east and north directions, respectively, of the kth computation cycle; t is a sensor sampling time interval; l is_kCalculating a target latitude, λ, for the kth calculation cycle_kA target longitude of the cycle is calculated for the kth.

Wherein, the k-th calculation cycle Micro Electro Mechanical Systems (MEMS) inertial sensor outputs correction values f 'of the acceleration values in the right direction and the forward direction output by the accelerometer'_x,kAnd f'_y,kCorrecting the right and forward acceleration values output by an accelerometer in the MEMS inertial sensor in the kth calculation period by the pitch angle deviation and the roll angle deviation existing in the installation process of the MEMS inertial sensor respectively, and obtaining the corrected values, wherein the correction comprises the following steps:

obtaining an accelerometer output f 'of a corrected k-th calculation period through the following formula'_x,kAnd f'_y,kComprises the following steps:

f'＝(C₁C₂)^-1f

wherein:

f′＝[f′_x,kf′_y,kf′_z,k]^T

f＝[f_x,kf_y,kf_z,k]^T

in the formula, delta theta is pitch angle deviation; delta gamma is the roll angle deviation; f. of_x,k，f_y,kAnd f_z,kThe accelerometer outputs are right, forward and up, respectively, before correction.

Wherein, the f_x,k，f_y,kAnd f_z,kFor the accelerometer output value obtained in the kth calculation cycle under the static condition, according to the acceleration projection relation, the Δ θ and Δ γ can be calculated as:

sinΔθ＝f_y,k/g

tanΔγ＝-f_x,k/f_z

wherein g is the acceleration of gravity.

Further, the first error correction value of the k-th calculation cycle

And also for correcting the following information:

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

in particular, a first error correction value over a k-th calculation cycle

In (1)

ε_b,ε_r，

For the k-th calculation cycle phi_DR,k，v_eDR,k，v_nDR,k，λ_DR,k，L_DR,k，ω_z,k，f_x,k，f_y,kCorrecting to obtain phi of k +1 th calculation period_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 of, usingIn the navigation positioning method of the (k + 1) th calculation period.

According to the navigation positioning method disclosed by the embodiment of the invention, when the navigation control equipment detects that a satellite signal is invalid, a magnetic heading angle of a k-th calculation period is taken as an observed quantity, a first state quantity combination is obtained through a DR/magnetic heading combined navigation algorithm model, and a second state quantity combination is obtained through the DR/vehicle motion combined navigation algorithm model with a transverse speed of the k-th calculation period as the observed quantity; according to a system state equation of a kth calculation period, closed loop Kalman combination filtering is carried out on a first state quantity combination and a second state quantity combination to obtain a first error correction value; and correcting the initial course angle of the kth calculation period according to the first error correction value to obtain a target course angle, and calculating to obtain a target speed and a target position according to the target course angle and the DR model of the kth calculation period. Therefore, the course angle is corrected by the DR/magnetic course combined navigation algorithm model and the DR/vehicle motion combined navigation algorithm model when the satellite signal fails, and then the target speed and the target position information are calculated according to the DR model, so that the vehicle positioning navigation when the satellite signal fails is realized, and the vehicle positioning accuracy is improved.

In one example, when the navigation control device detects that a satellite signal is valid, taking an initial speed and an initial position of a (k + 1) th calculation cycle as observed quantities, obtaining a third state quantity combination of the (k + 1) th calculation cycle through a DR/satellite combined navigation algorithm model of the (k + 1) th calculation cycle, wherein the DR/satellite combined navigation algorithm model of the (k + 1) th calculation cycle is established according to the initial speed and the initial position of the (k + 1) th calculation cycle;

the navigation control equipment performs closed-loop Kalman combined filtering on the first state quantity combination in the (k + 1) th calculation period and the third state quantity combination in the (k + 1) th calculation period according to a vehicle system state equation in the (k + 1) th calculation period to obtain a second error correction value in the (k + 1) th calculation period;

and the navigation control equipment corrects the initial course angle of the (k + 1) th calculation period according to the second error correction value of the (k + 1) th calculation period to obtain a target course angle of the (k + 1) th calculation period, and calculates to obtain the target speed and the target position of the (k + 1) th calculation period according to the target course angle of the (k + 1) th calculation period and the DR model of the (k + 1) th calculation period.

Wherein the DR/satellite combined navigation algorithm model of the (k + 1) th calculation cycle is established according to the initial velocity and the initial position of the (k + 1) th calculation cycle, and comprises:

taking the initial longitude of the (k + 1) th calculation cycle, the initial latitude of the (k + 1) th calculation cycle, the initial east velocity of the (k + 1) th calculation cycle and the initial north velocity of the (k + 1) th calculation cycle as observed quantities, establishing a measurement equation of the (k + 1) th calculation cycle, wherein the measurement equation comprises a third state quantity combination Z_Sat,k+1、H_Sat,k+1、V_Sat,k+1；

The measurement equation of the (k + 1) th calculation period is as follows:

Z_Sat,k＝H_Sat,kX_k+V_Sat,k

wherein,

third measurement matrix H of k +1 th calculation cycle_Sat,k+1Comprises the following steps:

third measurement noise V of k +1 th calculation period_Sat,k+1Comprises the following steps:

in the formula, λ_DR,k+1And L_DR,k+1Longitude and latitude of the k +1 th calculation cycle, respectively; lambda [ alpha ]_Sat,k+1、L_Sat,k+1、v_eSat,k+1And v_nSat,k+1Longitude of the satellite receiver position for the k +1 th calculation cycle, respectivelyLatitude, east speed, and north speed; n is a radical of_eSat,k+1、N_nSat,k+1、M_eSat,k+1And M_nSat,k+1The positioning information itself calculated for the (k + 1) th calculation cycle satellite receiver contains longitude, latitude, east-direction velocity and north-direction velocity errors.

Specifically, according to the vehicle system state equation of the (k + 1) th calculation cycle, the closed-loop kalman filtering is performed on the first state quantity combination of the (k + 1) th calculation cycle and the third state quantity combination of the (k + 1) th calculation cycle to obtain a second error correction value of the (k + 1) th calculation cycle

The method comprises the following steps:

In the formula,

wherein:

specifically, the method is described as S103, in which the initial heading angle of the (k + 1) th calculation cycle is corrected according to the second error correction value of the (k + 1) th calculation cycle to obtain the target heading angle of the (k + 1) th calculation cycle, and the target speed and the target position of the (k + 1) th calculation cycle are calculated according to the target heading angle of the (k + 1) th calculation cycle and the DR model of the (k + 1) th calculation cycle.

Therefore, the navigation and positioning method provided in this example realizes vehicle positioning and navigation when the satellite signal is invalid, improves accuracy of vehicle positioning, and corrects the initial course angle by using the second error correction value in the non-failure environment where the satellite signal is interfered, such as an urban canyon and an overhead shelter, thereby improving accuracy of navigation and positioning when the satellite signal is interfered.

The above description has introduced the solution of the embodiment of the present invention mainly from the perspective of the device implementation process. It will be appreciated that the apparatus, in order to carry out the above-described functions, comprises corresponding hardware structures and/or software modules for performing the respective functions. Those of skill in the art will readily appreciate that the present invention can be implemented in hardware or a combination of hardware and computer software, with the exemplary elements and algorithm steps described in connection with the embodiments disclosed herein. Whether a function is performed as hardware or computer software drives hardware depends upon the particular application and design constraints imposed on the solution. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present invention.

The embodiments of the present invention may perform functional unit division on the device according to the above method examples, for example, each functional unit may be divided corresponding to each function, or two or more functions may be integrated into one processing unit. The integrated unit can be realized in a form of hardware, and can also be realized in a form of a software functional unit. It should be noted that the division of the unit in the embodiment of the present invention is schematic, and is only a logic function division, and there may be another division manner in actual implementation.

In the case of an integrated unit, fig. 2A shows a schematic structural view of a possible navigation control device of the device referred to in the above-described embodiments. The navigation control apparatus 200 includes: a processing unit 202 and a communication unit 203. The processing unit 202 is used for controlling and managing the actions of the navigation control device, e.g. the processing unit 202 is used for supporting the device to perform steps S101 to 104 in fig. 1 and/or other processes for the techniques described herein. The communication unit 203 is used to support communication between the mobile terminal and other devices such as navigation satellites. The navigation control device may further comprise a storage unit 201 for storing program codes and data of the device.

The processing Unit 202 may be a Processor or a controller, and may be, for example, a Central Processing Unit (CPU), a general-purpose Processor, a Digital Signal Processor (DSP), an Application-Specific Integrated Circuit (ASIC), a Field Programmable Gate Array (FPGA) or other Programmable logic device, a transistor logic device, a hardware component, or any combination thereof. Which may implement or perform the various illustrative logical blocks, modules, and circuits described in connection with the disclosure. The processor may also be a combination of computing functions, e.g., comprising one or more microprocessors, DSPs, and microprocessors, among others. The communication unit 203 may be a communication interface, a transceiver circuit, etc., wherein the communication interface is a generic term and may include one or more interfaces. The storage unit 201 may be a memory.

When the processing unit 202 is a processor, the communication unit 203 is a communication interface, and the storage unit 401 is a memory, the apparatus according to the embodiment of the present invention may be the navigation control apparatus shown in fig. 2B.

Referring to fig. 2B, the navigation control apparatus 210 includes: processor 212, communication interface 213, memory 211. Optionally, the navigation control device 210 may also include a bus 214. Wherein the communication interface 213, the processor 212, and the memory 211 may be connected to each other through a bus 214; the bus 214 may be a Peripheral Component Interconnect (PCI) bus, an Extended Industry Standard Architecture (EISA) bus, or the like. The bus 214 may be divided into an address bus, a data bus, a control bus, etc. For ease of illustration, only one thick line is shown in FIG. 2B, but it is not intended that there be only one bus or one type of bus.

The navigation control device shown in fig. 2A or fig. 2B may also be understood as a device for a navigation control device, and the embodiment of the present invention is not limited thereto.

The embodiment of the invention also discloses a computer storage medium, wherein the computer storage medium can store a program, and the program comprises part or all of the steps of any one of the navigation positioning methods described in the method embodiments when being executed.

It should be noted that, for simplicity of description, the above-mentioned method embodiments are described as a series of acts or combination of acts, but those skilled in the art will recognize that the present invention is not limited by the order of acts, as some steps may occur in other orders or concurrently in accordance with the invention. Further, those skilled in the art should also appreciate that the embodiments described in the specification are preferred embodiments and that the acts and modules referred to are not necessarily required by the invention.

In the foregoing embodiments, the descriptions of the respective embodiments have respective emphasis, and for parts that are not described in detail in a certain embodiment, reference may be made to related descriptions of other embodiments.

In the embodiments disclosed in the present invention, it should be understood that the disclosed apparatus may be implemented in other ways. For example, the above-described embodiments of the apparatus are merely illustrative, and for example, the division of the units is only one type of division of logical functions, and there may be other divisions when actually implementing, for example, a plurality of units or components may be combined or may be integrated into another system, or some features may be omitted, or not implemented. In addition, the shown or discussed mutual coupling or direct coupling or communication connection may be an indirect coupling or communication connection of some interfaces, devices or units, and may be an electric or other form.

The units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of the embodiment.

In addition, functional units in the embodiments of the present invention may be integrated into one processing unit, or each unit may exist alone physically, or two or more units are integrated into one unit. The integrated unit can be realized in a form of hardware, and can also be realized in a form of a software functional unit.

The integrated unit, if implemented in the form of a software functional unit and sold or used as a stand-alone product, may be stored in a computer readable memory. Based on such understanding, the technical solution of the present invention may be embodied in the form of a software product, which is stored in a memory and includes several instructions for causing a computer device (which may be a personal computer, a server, a network device, or the like) to execute all or part of the steps of the method according to the embodiments of the present invention. And the aforementioned memory comprises: a U-disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a removable hard disk, a magnetic or optical disk, and other various media capable of storing program codes.

The above embodiments of the present invention are described in detail, and the principle and the implementation of the present invention are explained by applying specific embodiments, and the above description of the embodiments is only used to help understanding the method of the present invention and the core idea thereof; meanwhile, for a person skilled in the art, according to the idea of the present invention, there may be variations in the specific embodiments and the application scope, and in summary, the content of the present specification should not be construed as a limitation to the present invention.

Claims

1. A navigation positioning method is characterized by comprising the following steps:

when satellite signal failure is detected, taking a magnetic heading angle of a k-th calculation period as an observed quantity, obtaining a first state quantity combination of the k-th calculation period through a DR/magnetic heading combined navigation algorithm model of the k-th calculation period, taking a transverse speed of the k-th calculation period as the observed quantity, obtaining a second state quantity combination of the k-th calculation period through the DR/vehicle motion combined navigation algorithm model of the k-th calculation period, wherein k is an integer greater than 2;

2. The method according to claim 1, wherein the DR/magnetic heading combined navigation algorithm model of the k-th calculation cycle is established according to an initial heading angle of the k-th calculation cycle and a magnetic heading angle of the k-th calculation cycle, the magnetic heading angle of the k-th calculation cycle is calculated according to magnetic sensor output data, the initial heading angle of the k-th calculation cycle is calculated according to a target heading angle of a k-1-th calculation cycle and an angular velocity obtained in the k-th calculation cycle, the DR/vehicle motion combined navigation algorithm model of the k-th calculation cycle is established according to a lateral velocity of the k-th calculation cycle, the lateral velocity of the k-th calculation cycle is calculated according to the initial velocity of the k-th calculation cycle and the initial heading angle of the k-th calculation cycle, the initial speed of the k-th calculation period is calculated according to the initial course angle of the k-th calculation period, the target speed of the k-1 th calculation period and the output value of the MEMS inertial sensor accelerometer of the k-th calculation period;

3. The method of claim 1, further comprising:

4. The method of claim 1, wherein the first state quantity set of the k-th computation cycle comprises a magnetic heading angle set measured state Z of the k-th computation cycle_Mag，kThe first measurement matrix H_MagAnd a first measurement noise V of said k-th calculation cycle_Mag，k；

Calculating the Z based on the formula_Mag，k

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

Calculating the H based on the formula_Mag

H_Mag＝[1 0 0 0 0 0 0 0]

Calculating the V based on the formula_Mag，k

the heading angle error obtained for the k-th calculation cycle.

5. The method of claim 1, wherein the second state quantity combination for the k-th computation cycle comprises: the vehicle lateral velocity measurement state Z of the k-th calculation cycle_Virt，kA second measurement matrix H of the k-th calculation cycle_Virt，kAnd a second measurement noise V of said k-th calculation cycle_Virt，k；

Calculating the Z based on the formula_Virt，k

Calculating the H based on the formula_Virt，k

Calculating the V based on the formula_Virt，k

V_Virt,k＝[δv_x,k]

Wherein,

respectively east and north velocity errors, v, obtained in the k-th calculation cycle_eDR,kCalculating an initial east velocity for the kth calculation cycle; v. of_nDR,kCalculating an initial northbound speed for the kth cycle; delta v_x,kCalculating an additional lateral velocity change due to sideslip when the vehicle is in motion for said kth cycle, wherein Φ_DR,kCalculating a cycle initial heading angle for the kth,

the heading angle error obtained for the k-th calculation cycle.

6. The method according to claim 3, wherein the third state quantity combination of the (k + 1) th computation cycle comprises: the combined measurement state Z of longitude, latitude, east speed and north speed of the k +1 th calculation cycle_Sat,k+1The third measurement matrix H of the k +1 th calculation cycle_Sat,k+1And a third measurement noise V of said (k + 1) th calculation cycle_Sat,k+1；

Calculating the Z based on the formula_Sat,k+1

Based onThe following formula calculates the H_Sat,k+1

Calculating the V based on the formula_Sat,k+1

Wherein λ is_DR,k+1And L_DR,k+1Longitude and latitude of the k +1 th calculation cycle, respectively; lambda [ alpha ]_Sat,k+1、L_Sat,k+1、v_eSat,k+1And v_nSat,k+1Respectively calculating longitude, latitude, east speed and north speed of the satellite receiver positioning in the (k + 1) th calculation period; r_mAnd R_nRespectively the radius of a meridian circle and the radius of a prime unit circle; n is a radical of_eSat,k+1、N_nSat,k+1、M_eSat,k+1And M_nSat,k+1Longitude, latitude, east speed and north speed errors contained in the positioning information calculated by the satellite receiver in the (k + 1) th calculation period respectively;

7. The method of claim 1, wherein the system state equation for the kth computation cycle is:

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

wherein A is_k,k-1For the state transition matrix of the k-1 th calculation cycle, X_kFor the system state quantity of the k-th calculation cycle,

G_k-1for said system noise driving matrix of the k-1 th calculation cycle, W_k-1White noise for the k-1 th calculation cycleThe random error vector is then used to determine the error vector,

and

and

the right-direction and forward-direction first-order Markov errors of the accelerometer of the kth calculation period are respectively;

wherein,

for the course angle error obtained for the k-th calculation cycle,

the east and north velocity errors obtained in the k-th calculation cycle are obtained respectively.

8. The method according to claim 1, wherein the performing closed-loop kalman filtering on the first state quantity combination of the kth computation cycle and the second state quantity combination of the kth computation cycle according to the system state equation of the kth computation cycle to obtain a first error correction of the kth computation cycle comprises:

Wherein,

for the first error correction value of the k-th calculation cycle,

wherein A is_k,k-1For the state transition matrix of the k-1 th calculation cycle, G_k-1Driving a matrix for the system noise of the k-1 th calculation cycle;

wherein the second state quantity combination of the k-th computation cycle includes: the vehicle lateral velocity measurement state Z of the k-th calculation cycle_Virt，kA second measurement matrix H of the k-th calculation cycle_Virt，kAnd a second measurement noise V of said k-th calculation cycle_Virt，k；

Wherein the first state quantity combination of the k-th calculation period comprises a magnetic heading angle combination measurement state Z of the k-th calculation period_Mag，kA first measurement matrix H_MagThe first measurement noise V of the k-th calculation period_Mag，k。

9. The method according to claim 3, wherein the performing the closed-loop kalman filtering combination according to the system state equation of the (k + 1) th calculation cycle on the first state quantity combination of the (k + 1) th calculation cycle and the third state quantity combination of the (k + 1) th calculation cycle to obtain the second error correction value of the (k + 1) th calculation cycle comprises:

Wherein,

a second error correction value for the (k + 1) th calculation cycle,

wherein A is_k+1,kFor the state transition matrix of the k-th calculation cycle, G_kDriving a matrix for the system noise of the k-th computation cycle, Q_kFor the k-th calculation cycleThe system noise matrix of (2);

wherein the third state quantity combination of the (k + 1) th calculation cycle includes: the satellite measurement state Z of the (k + 1) th calculation cycle_Sat,k+1The third measurement matrix H of the k +1 th calculation cycle_Sat,k+1And a third measurement noise V of said (k + 1) th calculation cycle_Sat,k+1；

Wherein the first state quantity combination of the (k + 1) th calculation cycle comprises a magnetic heading angle combination measurement state Z of the (k + 1) th calculation cycle_Mag,k+1The first measurement matrix H of the (k + 1) th calculation cycle_Mag,k+1The first measurement noise V of the (k + 1) th calculation period_Mag,k+1。

10. The method of claim 1, wherein the correcting the initial heading angle for the kth computation cycle based on the first error correction value for the kth computation cycle to obtain the target heading angle for the kth computation cycle comprises:

Wherein, the phi'_kCalculating a target course angle of the k-th calculation period;

wherein phi_DR,kCalculating a cycle initial heading angle for the kth,

the heading angle error obtained for the k-th calculation cycle.

11. The method of claim 1, wherein calculating the target speed and the target position for the kth computation cycle according to the target heading angle for the kth computation cycle and the DR model for the kth computation cycle comprises:

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，kAnd f'_y，kRespectively calculating the correction values of the right and forward acceleration values output by the accelerometer in the MEMS inertial sensor in the k-th calculation period, v_e，kCalculating a target east velocity, v, for the kth calculation cycle_n，kCalculating a target northbound speed for the kth cycle; f. of_e,kAnd f_n,kAcceleration projections in the east and north directions, respectively, of the kth computation cycle; t is a sensor sampling time interval; l is_kCalculating a target latitude, λ, for the kth calculation cycle_kA target longitude for the kth computing cycle;

wherein R is_mAnd R_nRespectively the radius of a meridian circle and the radius of a prime unit circle;

wherein, phi'_kRepresenting a target heading angle for the k-th computation cycle.

12. The method of claim 11, wherein the correction values f 'of accelerometer output right and forward acceleration values in the kth computational cycle MEMS inertial sensor'_x，kAnd f'_y，kAnd correcting the right and forward acceleration values output by an accelerometer in the MEMS inertial sensor in the k-th calculation period according to the pitch angle deviation and the roll angle deviation existing in the installation process of the MEMS inertial sensor respectively.

13. A navigation and positioning device is characterized by comprising a processing unit and a communication unit,

the processing unit is used for obtaining a first state quantity combination of a k-th calculation period through a DR/magnetic heading combined navigation algorithm model of the k-th calculation period by taking a magnetic heading angle of the k-th calculation period as an observed quantity when the communication unit detects that a satellite signal fails, obtaining a second state quantity combination of the k-th calculation period through the DR/vehicle motion combined navigation algorithm model of the k-th calculation period by taking a transverse speed of the k-th calculation period as the observed quantity, wherein k is an integer greater than 2; the closed-loop Kalman combination filtering is carried out on the first state quantity combination of the kth computing period and the second state quantity combination of the kth computing period according to a system state equation of the kth computing period, and a first error correction value of the kth computing period is obtained; and the initial course angle of the k-th calculation period is corrected according to the first error correction value of the k-th calculation period to obtain a target course angle of the k-th calculation period, and the target speed and the target position of the k-th calculation period are calculated according to the target course angle of the k-th calculation period and the DR model of the k-th calculation period.

14. The apparatus according to claim 13, wherein the DR/MAGNETO RIGHT algorithm model of the kth computing cycle is built according to the initial heading angle of the kth computing cycle and the MAGNETO heading angle of the kth computing cycle, the MAGNETO heading angle of the kth computing cycle is computed according to the magnetic sensor output data, the initial heading angle of the kth computing cycle is computed according to the target heading angle of the kth computing cycle and the angular velocity obtained from the kth computing cycle, the DR/VEHICLE MOTION COMBINATION NAVIGARIGHT algorithm model of the kth computing cycle is built according to the lateral velocity of the kth computing cycle, the lateral velocity of the kth computing cycle is computed according to the initial velocity of the kth computing cycle and the initial heading angle of the kth computing cycle, the initial speed of the k-th calculation period is calculated according to the initial course angle of the k-th calculation period, the target speed of the k-1 th calculation period and the output value of the MEMS inertial sensor accelerometer of the k-th calculation period;

15. The apparatus according to claim 13, wherein the processing unit is further configured to obtain a third state quantity combination of the (k + 1) th computation cycle through a DR/satellite combined navigation algorithm model of the (k + 1) th computation cycle, using an initial velocity and an initial position of the (k + 1) th computation cycle as observed quantities when a satellite signal is detected to be valid through the communication unit, wherein the DR/satellite combined navigation algorithm model of the (k + 1) th computation cycle is established according to the initial velocity and the initial position of the (k + 1) th computation cycle;

the closed-loop Kalman combined filtering is performed on the first state quantity combination in the (k + 1) th calculation period and the third state quantity combination in the (k + 1) th calculation period according to a system state equation in the (k + 1) th calculation period to obtain a second error correction value in the (k + 1) th calculation period;

and the initial course angle of the (k + 1) th calculation cycle is corrected according to the second error correction value of the (k + 1) th calculation cycle to obtain a target course angle of the (k + 1) th calculation cycle, and the target speed and the target position of the (k + 1) th calculation cycle are calculated according to the target course angle of the (k + 1) th calculation cycle and the DR model of the (k + 1) th calculation cycle.

16. The apparatus of claim 13, wherein the first state quantity set of the k-th computation cycle comprises a magnetic heading angle set metrology state Z of the k-th computation cycle_Mag，kThe first measurement matrix H_MagAnd a first measurement noise V of said k-th calculation cycle_Mag，k；

Calculating the Z based on the formula_Mag，k

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

Calculating the H based on the formula_Mag

H_Mag＝[1 0 0 0 0 0 0 0]

Calculating the V based on the formula_Mag，k

the heading angle error obtained for the k-th calculation cycle.

17. The apparatus according to claim 13, wherein the second state quantity combination of the k-th computation cycle comprises: the vehicle lateral velocity measurement state Z of the k-th calculation cycle_Virt，kA second measurement matrix H of the k-th calculation cycle_Virt，kAnd a second measurement noise V of said k-th calculation cycle_Virt，k；

Calculating the Z based on the formula_Virt，k

Calculating the H based on the formula_Virt，k

Calculating the V based on the formula_Virt，k

V_Virt,k＝[δv_x,k]

Wherein,

respectively east and north velocity errors, v, obtained in the k-th calculation cycle_eDR,kCalculating an initial east velocity for the kth calculation cycle; v. of_nDR,kCalculating an initial northbound speed for the kth cycle; delta v_x,kCalculating an additional lateral velocity change due to sideslip when the vehicle is in motion for the kth cycle;

wherein phi_DRkCalculating a cycle initial heading angle for the kth,

the heading angle error obtained for the k-th calculation cycle.

18. The apparatus according to claim 15, wherein the third state quantity combination of the (k + 1) th computation cycle comprises: the combined measurement state Z of longitude, latitude, east speed and north speed of the k +1 th calculation cycle_Sat,k+1The third measurement matrix H of the k +1 th calculation cycle_Sat,k+1And a third measurement noise V of said (k + 1) th calculation cycle_Sat,k+1；

Calculating the Z based on the formula_Sat,k+1

Calculating the H based on the formula_Sat,k+1

Calculating the V based on the formula_Sat,k+1

19. The apparatus of claim 13, wherein the system state equation for the kth computation cycle is:

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

G_k-1for said system noise driving matrix of the k-1 th calculation cycle, W_k-1A white noise random error vector for the k-1 th calculation cycle,

and

and

wherein,

for the course angle error obtained for the k-th calculation cycle,

20. The apparatus according to claim 13, wherein the processing unit is further configured to, in terms of performing closed-loop kalman filtering on the first state quantity combination of the kth computation cycle and the second state quantity combination of the kth computation cycle according to the system state equation of the kth computation cycle to obtain a first error correction of the kth computation cycle, further:

the said formula is calculated byFirst error correction value of k-th calculation cycle

Wherein,

for the first error correction value of the k-th calculation cycle,

Wherein the first state quantity group of the k-th calculation cycleThe synthesis comprises the following steps: magnetic heading angle combined measurement state Z of kth calculation period_Mag，kA first measurement matrix H_MagThe first measurement noise V of the k-th calculation period_Mag，k。

21. The apparatus according to claim 15, wherein the processing unit is further configured to, in terms of performing the closed-loop kalman filtering on the first state quantity combination of the (k + 1) th computation cycle and the third state quantity combination of the (k + 1) th computation cycle according to the system state equation of the (k + 1) th computation cycle to obtain a second error correction value of the (k + 1) th computation cycle:

Wherein,

a second error correction value for the (k + 1) th calculation cycle,

wherein A is_k+1,kFor the state transition matrix of the k-th calculation cycle, G_kDriving a matrix for the system noise of the k-th computation cycle, Q_kCalculating a system noise matrix for the kth calculation cycle;

22. The apparatus of claim 13, wherein the processing unit, in being configured to correct the initial heading angle for the k-th computation cycle based on the first error correction value for the k-th computation cycle to obtain the target heading angle for the k-th computation cycle, is further configured to:

wherein phi_DR,kCalculating a cycle initial heading angle for the kth,

the heading angle error obtained for the k-th calculation cycle.

23. The apparatus of claim 13, wherein the processing unit is further configured to, in calculating the target speed and target position for the kth computation cycle based on the target heading angle for the kth computation cycle and a DR model for the kth computation cycle:

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)

24. The apparatus of claim 23, wherein the k-th calculation cycle MEMS inertial sensor is a correction value f 'of accelerometer output right and forward acceleration values'_x，kAnd f'_y，kAnd correcting the right and forward acceleration values output by an accelerometer in the MEMS inertial sensor in the k-th calculation period according to the pitch angle deviation and the roll angle deviation existing in the installation process of the MEMS inertial sensor respectively.

25. A navigational positioning device, comprising:

the system comprises a processor, a memory, a communication interface and a communication bus, wherein the processor, the memory and the communication interface are connected through the communication bus and complete mutual communication;

the memory stores executable program code, the communication interface is for wireless communication;

the processor is configured to call the executable program code in the memory to perform the method as described in any of claims 1-12.

26. A computer-readable storage medium, characterized in that the computer-readable storage medium stores a computer program which is executed by a processor to implement the method of any one of claims 1 to 12.