A Multi-Source Combined Navigation and Positioning Method in a Complex Environment

Abstract

The present invention discloses a multi-source integrated navigation and positioning method in a complex environment, belonging to the technical field of satellite positioning and navigation. In an urban environment, the signals of the Global Navigation Satellite System (GNSS) are easily reflected or blocked by surrounding buildings, and the positioning accuracy and reliability of the pure GNSS positioning in the urban environment will be significantly reduced. In the present invention, the carrier velocity respectively obtained by an on-line diagnosis system, Doppler frequency shift measurement, and inertial measurement unit is tightly coupled by using Kalman filtering, and a velocity estimation is output; the accuracy of pseudorange observation is improved by using the classical carrier-smoothed pseudorange algorithm, i.e., Hatch filtering, and the carrier position is solved. When the GNSS signal is unavailable, the velocity estimation output by the Kalman filter is used as an update substitute for the Hatch filtering, thereby ensuring the continuity of the carrier-smoothed pseudorange positioning and ultimately improving the positioning accuracy and reliability in the urban environment.

Description

Technical Field

[0001] The present invention relates to a navigation and positioning method, in particular to a multi-source integrated navigation and positioning method in a complex environment. Background Art

[0002] In recent years, accurate and reliable urban environment positioning has attracted extensive attention, such as autonomous driving, urban drones, etc. Such vehicles and aircraft need to operate in the low-altitude urban environment. In order to improve the navigation and positioning performance of the urban environment, a lot of research has been done on the integration of multi-source information such as the Global Navigation Satellite System (GNSS), Inertial Navigation System (INS), magnetic compass, high-precision electronic map, vision sensor, Light Detection And Ranging (LiDAR), altimeter, odometer, Inertial Measurement Unit (IMU), speed sensor, etc.

[0003] Kassas proposed an opportunity signal / digital map / IMU / GNSS combination method (Reference: Robust vehicular localization and map matching in urban environments through IMU, GNSS, and cellular signals), which combines cellular network signals, digital maps with traditional IMU / GNSS to improve the positioning accuracy in urban environments. However, this method requires the known position of cellular network base stations and the identification of the base stations corresponding to cellular network signals. Kim et al. (Reference: A baro-altimeter augmented INS / GPS navigation system for an uninhabited aerial vehicle) enhanced the INS / GPS integrated navigation system using a barometric altimeter, significantly improving the vertical positioning accuracy and reliability. However, the positioning error accumulates rapidly when the GPS signal is unavailable. Liu et al. proposed a pedestrian integrated navigation system (Reference: The Pedestrian Integrated Navigation System with Micro IMU / GPS / Magnetometer / Barometric Altimeter), which obtains information using IMU, GPS, magnetometer, and barometric altimeter, and gives the extended Kalman filter for pedestrian positioning. However, the magnetometer is easily affected by steel structures in the city. Jacques et al. proposed a vehicle navigation solution integrating low-cost mems inertial sensors and GPS (Reference: Low-Cost Three-Dimensional Navigation Solution for RISS / GPS Integration Using Mixture Particle Filter). This solution uses a mixture particle filter as a non-linear filter, combines a reduced inertial sensor system (RISS) with GPS to achieve a three-dimensional navigation solution, and adopts a loose coupling method, where RISS consists of two accelerometers, one gyroscope, and a vehicle odometer.Bondan constructed a combined navigation system through IMU, GPS, and OnBoard Diagnostics (OBD) sensors (reference: OBD-II Sensor Approaches for The IMU and GPS Based Apron Vehicle Positioning System), and used OBD speed to update the gradient Kalman filter; since OBD speed allows for the existence of absolute zero speed, especially when the carrier is stationary, this system can significantly reduce the drift effect. Su proposed a method for multi-sensor fusion positioning and attitude determination of GNSS / IMU and vision, etc. (reference: GR-SLAM: Vision-Based Sensor Fusion SLAM for Ground Robots on Complex Terrain), which improved the accuracy of feature point matching between adjacent two images and the solution accuracy of position and attitude, but this method is vulnerable to the influence of surrounding environments such as lighting, and the extraction and matching efficiency of feature points is low. Wen et al. proposed a multi-task collaborative integration method using the inter-range measurement of GNSS / camera / INS (reference: Multi-agent collaborative GNSS / camera / INS integration aided by inter-ranging for vehicular navigation in urban areas.), which improved the detection performance of the camera overlapping area, but due to the defects of vision positioning, it cannot be used in areas with poor meteorological conditions.

[0004] According to the analysis of existing literature, the simple combination of GNSS / IMU has the disadvantage of too fast accumulation of GNSS unavailable errors; the combination of GNSS and vision has the disadvantages of high computational load, inability to be used under poor meteorological conditions, and poor reliability. Using OBD to obtain speed is more accurate and can constrain the errors of GNSS / IMU in the traveling direction; the barometric altimeter can be used all-weather and has higher reliability in the vertical direction than GNSS. In order to combine the characteristics of urban environment use and consider cost factors, this patent proposes a combined navigation method of GNSS / IMU / OBD / altimeter in urban environment.

[0005] The technology relatively close to the present invention is to use the method of combining a vision system with a 3D urban model to identify urban blocks for positioning detection, but this method has more ground interference in the city, and the reliability of the system is not high, and it is more easily affected by other carriers, pedestrians, and other obstacles, etc., and the operability and practical significance are not great. Summary of the Invention

[0006] Objective of the Invention: The technical problem to be solved by the present invention is to provide a multi-source combined navigation and positioning method in a complex environment in view of the deficiencies of the prior art.

[0007] To solve the above technical problem, the present invention discloses a multi-source combined navigation and positioning method in a complex environment, including the following steps:

[0008] Step 1, Global Navigation Satellite System and multi-sensor measurement and observation. The multi-sensors include a Global Navigation Satellite System receiver, an inertial navigation system, an on-line diagnosis system, and a barometric altimeter;

[0009] Step 2, removing satellites with a carrier-to-noise ratio of the satellite signal lower than the threshold;

[0010] Step 3, Doppler frequency shift velocity measurement and barometric altitude calculation to obtain altitude information;

[0011] Step 4, velocity Kalman filtering, obtaining the velocity, mechanical alignment, and Doppler frequency shift velocity of the on-line diagnosis system, and outputting a velocity estimate to obtain attitude and velocity information;

[0012] Step 5, using Hatch filtering to obtain carrier-smoothed pseudorange and performing position calculation to obtain position information;

[0013] Step 6, inputting the position information obtained in Step 5, the altitude information obtained in Step 3, and the attitude and velocity information obtained in Step 4 into an unscented Kalman filter, performing error feedback on the mechanical alignment, and finally obtaining the position and velocity information of the carrier to complete combined navigation and positioning in an urban environment.

[0014] In the present invention, Step 1 includes:

[0015] Step 1-1, in an urban environment, obtaining the Doppler frequency shift measurement of the k-th satellite through the Global Navigation Satellite System vehicle receiver u and the reference station receiver r as and obtaining the carrier phase measurement as and obtaining the pseudorange observation as and obtaining the current satellite set, i.e., the number of satellites, and the carrier-to-noise ratio of the satellite signal;

[0016] Step 1-2, calculating the single difference of the carrier phase measurement:

[0017]

[0018] Calculating the double difference of the carrier phase measurement:

[0019]

[0020] Calculate the triple-difference of carrier phase measurements:

[0021]

[0022] Calculate the single-difference of pseudorange measurements:

[0023]

[0024] Calculate the double-difference of pseudorange measurements:

[0025]

[0026] Calculate the single-difference of Doppler frequency shift:

[0027]

[0028] Calculate the double-difference of Doppler frequency shift:

[0029]

[0030] Among them, Δ represents the difference in measurements between the vehicle u and the reference station r, and represents the difference in measurements between different satellites k and l;

[0031] In steps 1-3, obtain the ambient air pressure P(t) and temperature T0(t) through the barometric altimeter, and obtain the velocity of the vehicle relative to the vehicle coordinate system through the vehicle on-board diagnostic system The inertial measurement unit obtains the specific forces [f N f E f D and angular velocity of the vehicle in the north, east, and local directions.

[0032] In the present invention, step 2 includes:

[0033] Determine the satellite signal carrier-to-noise ratio threshold β according to historical observation data, compare the satellite signal carrier-to-noise ratios of all satellites with the threshold β, and save the satellites with satellite signal carrier-to-noise ratios greater than β in the set A1, and eliminate the satellites with satellite signal carrier-to-noise ratios lower than β.

[0034] In the present invention, step 3 includes:

[0035] Step 3-1, Doppler frequency shift velocity measurement, the method is as follows:

[0036]

[0037] In the present invention, the "·" above the parameter represents the derivative with respect to time t, the "^" represents the posteriori estimate, the "-" represents the priori estimate, and the "~" represents the measured value; is the vector from the vehicle u to the kth satellite at time t, is the vector difference from the vehicle u to the k-th satellite and the l-th satellite at time t, is derived with respect to time t, is the transformation matrix from the navigation coordinate system to ECEF, is the estimated velocity of the vehicle u in the navigation coordinate system, is the posterior estimate of the position of the vehicle u relative to the reference station r, is the Doppler triple difference measurement value, ε D (t) is the Doppler frequency shift noise, ▽Δε D (t) is the Doppler frequency shift noise triple difference, x u (t) is the vehicle position; Step 3-2, calculate the barometric altitude using the international standard atmosphere model, the method is as follows:

[0038]

[0039] h(t) = A(t) + N(t) + ε h (t)

[0040] where, A(t) is the barometric altitude, T0(t) is the absolute temperature at the local sea level, P0(t) is the barometric pressure at the local sea level, P(t) is the measured barometric pressure, γ, R and g are the corresponding physical constants, ε A (t) is the barometric altimeter error, N(t) is the geoid height, h(t) is the ellipsoidal height, ε h (t) is the ellipsoidal height error.

[0041] In the present invention, Step 4 includes:

[0042] Step 4-1, construct the state vector X as follows:

[0043]

[0044] where, is the velocity error relative to the navigation coordinate system, δV N 、δV E and δV D are the velocity errors in the north, east and down directions respectively; φ = [φ N φ E φ D is the attitude error represented by Euler angles, φ N 、φ E and φ D are the attitude errors in the north, east and down directions respectively; is the accelerometer bias, and are the biases in each direction in the vehicle coordinate system; ξ = [ξ x ξ yξ z is the gyroscope drift, ξ x , ξ y and ξ z are the gyroscope drifts in each direction in the vehicle coordinate system; is the offset of the on-line diagnostic system;

[0045] Step 4-2, construct the mechanical arrangement of the inertial measurement element, the method is as follows:

[0046]

[0047]

[0048]

[0049]

[0050]

[0051]

[0052]

[0053] where ω is the measurement noise of the inertial measurement element, R m and R t are the radius of curvature of the Earth's meridian and the equator respectively, h is the ellipsoidal height, and L is the latitude; [Ω N 0 Ω D is the angular rate of the Earth's rotation in the north, east, and local directions relative to the navigation coordinate system; [ρ N ρ E ρ D are the transfer angular rates in the north, east, and local directions respectively, [f N f E f D are the specific forces in the north, east, and local directions respectively, is the transfer matrix from the vehicle coordinate system to the navigation coordinate system;

[0054] Step 4-3, measurement error model, the method is as follows:

[0055] The Doppler frequency shift velocity measurement result is:

[0056]

[0057] The observed quantity z obtained by the OBD speed measurement OBD is:

[0058]

[0059] where, is the velocity of the carrier in the navigation coordinate system, is the posterior estimate of the carrier to the navigation coordinate system, and the measurement update equation is:

[0060] Z(t) = H(t)X(t) + ε Z (t)

[0061]

[0062]

[0063]

[0064] where r OBD , r D are the noise variances of the online diagnostic system and the Doppler frequency shift respectively, and ε Z (t) is the measurement error;

[0065] Step 4-4, velocity estimation output:

[0066] Determine whether the pseudorange and carrier phase measurements are available. When the number of available satellites cannot be used for positioning, use the velocity estimate of the velocity Kalman filter Calculate the alternative measurement ω KF (t) to perform alternative update on the carrier phase indirect measurement ω φ (t):

[0067]

[0068]

[0069] where x u is the carrier position, is the derivative of the user position at time t with respect to time, x r is the reference station position, is the triple difference of the carrier phase measurement; through the above alternative update, the error accumulation of the Hatch filter is controlled, and finally the velocity estimate and the alternative measurement ω KF (t) are output.

[0070] In the present invention, Step 5 includes:

[0071] Step 5-1, incremental state update:

[0072] Calculate the pseudorange indirect measurement:

[0073]

[0074] where is the triple difference of the carrier phase is the derivative of the measurement of with respect to time t, It is the triple difference of carrier phase measurement noise.

[0075] The indirect measurement of velocity estimation is obtained by velocity Kalman filtering:

[0076]

[0077]

[0078] Among them, is the transformation matrix from the navigation coordinate system to the geocentric earth-fixed coordinate system, and L and l are the latitude and longitude respectively;

[0079] Indirect measurement equation:

[0080]

[0081] Among them,

[0082] is the estimated incremental state vector, which is updated through , where is the prior estimate of the state vector, is the posterior estimate of the state vector;

[0083] Step 5-2, measurement update:

[0084] The indirect measurement of pseudorange is:

[0085]

[0086]

[0087] Among them is the measured value of the pseudorange observable, is the prior estimate of the position of the carrier relative to the reference station, and the indirect measurement is updated as:

[0088]

[0089] Among them,

[0090]

[0091]

[0092] is the column vector composed of.

[0093] Update the prior estimate to the posterior estimate:

[0094]

[0095] Among them, Z(t) is the Hatch filtering gain;

[0096] Step 5-3: Output the position x u (t) obtained after carrier-smoothed pseudorange solution.

[0097] In the present invention, Step 6 includes: inputting the position x u (t) obtained by carrier-smoothed pseudorange solution, the altitude A(t) obtained by barometric altimeter, and the attitude φ and velocity into the unscented Kalman filter, performing error feedback on the mechanical alignment, and finally obtaining the position and velocity information of the carrier to complete integrated navigation and positioning in an urban environment.

[0098] In the present invention, the physical constants in Step 3-2 are: lapse rate of temperature γ = -0.00649 K / m, molar gas constant R = 287.05 J·kg·K -1 and gravitational constant g = 9.80665 m / s 2 .

[0099] In Step 2 of the present invention, the satellite signal carrier-to-noise ratio threshold β is set to 40.

[0100] In the present invention, the method for obtaining the position of the carrier in Step 6 is:

[0101] x u (t + 1) = x u (t) + Δx u (t)

[0102] The method for obtaining velocity information is:

[0103] V u (t + 1) = V u (t) + ΔV u (t)

[0104] Beneficial effects:

[0105] 1. The commonly used vision positioning in existing multi-sensor fusion is unavailable under poor meteorological conditions and cannot achieve reliable all-weather navigation and positioning. The present invention improves the reliability of the integrated navigation system by using all-weather sensors: barometric altimeter, OBD, and IMU.

[0106] 2. The existing integrated navigation methods have low vertical accuracy in urban environments. In the present invention, the altitudes obtained by multiple carriers using barometric altimeters under the same barometric reference plane can well establish vertical intervals, which is very effective for the operation of carriers in an urban three-dimensional environment, especially for the operation of unmanned aerial vehicles, and can significantly improve the accuracy and reliability of vertical positioning.

[0107] 3. In the traditional integrated navigation method, when GNSS is unavailable, the estimation accuracy of the carrier position information will decrease significantly. The present invention uses velocity Kalman filtering to obtain the carrier velocity estimation, which is used as an alternative measurement for carrier-smoothed pseudorange when GNSS signals are unavailable, ensuring the continuity of the carrier-smoothed pseudorange, reducing the computational complexity, and ultimately improving the accuracy of integrated navigation positioning when GNSS is unavailable. Brief Description of the Drawings

[0108] The following further describes the present invention in detail with reference to the drawings and specific embodiments, and the above and / or other advantages of the present invention will become clearer.

[0109] Figure 1 It is a schematic flowchart of the present invention. Specific Embodiments

[0110] A multi-source integrated navigation and positioning method in a complex environment, as Figure 1 shown. First, obtain the measurement information of the GNSS receiver and multi-sensors, including the Doppler frequency shift, pseudorange, and carrier phase measurement provided by GNSS; the specific force and angular velocity obtained by the IMU; the carrier velocity provided by the OBD system; and the barometric altitude obtained by the barometric altimeter. Secondly, perform mechanical alignment on the IMU; perform fault detection on the GNSS measurement to extract available signals. Then, input the velocity measured by the Doppler frequency shift, the carrier velocity obtained by the OBD, the IMU velocity, attitude, and position information obtained by mechanical alignment into the Kalman filter to obtain the carrier velocity estimation; determine whether the carrier phase measurement and pseudorange measurement are available. When available, input them into the Hatch filter. When unavailable, use the velocity estimation of the Kalman filter to input and update the Hatch filter, and output the carrier-smoothed pseudorange measurement; use the obtained geoid height, environmental temperature, and barometric altitude of the barometric altimeter to calculate the ellipsoidal height. Next, use the carrier-smoothed pseudorange to calculate the carrier position, and input the position, the velocity measured by the Doppler frequency shift, the ellipsoidal height, the velocity, attitude, and position information obtained by mechanical alignment into the unscented Kalman filter to perform feedback correction on the mechanical alignment to obtain the carrier velocity, attitude, and position solved by this integrated navigation method. Specifically as follows:

[0111] Step 1, Global Navigation Satellite System and multi-sensor measurement and observation, where the multi-sensors include a Global Navigation Satellite System receiver, an Inertial Navigation System, an On-Board Diagnostic system, and a barometric altimeter;

[0112] Step 1-1, in an urban environment, obtain the Doppler frequency shift measurement of the k-th satellite through the GNSS carrier receiver u and the reference station receiver r as and Obtain the carrier phase measurement as and Obtain the pseudorange observation as and Obtain the current satellite set, i.e., the number of satellites, and the carrier-to-noise ratio of the satellite signals;

[0113] Step 1-2, calculate the single difference of carrier phase measurement:

[0114]

[0115] Calculate the double difference of carrier phase measurement:

[0116]

[0117] Calculate the triple difference of carrier phase measurement:

[0118]

[0119] Calculate the single difference of pseudorange measurement:

[0120]

[0121] Calculate the double difference of pseudorange measurement:

[0122]

[0123] Calculate the single difference of Doppler frequency shift:

[0124]

[0125] Calculate the double difference of Doppler frequency shift:

[0126]

[0127] where Δ is the difference of the measurement between the vehicle u and the reference station r, is the difference of the measurement between different satellites k, l;

[0128] Step 1-3, obtain the environmental pressure P(t) and temperature T0(t) through a barometric altimeter, and obtain the speed of the vehicle relative to the vehicle coordinate system through the vehicle on-board diagnostic system The inertial measurement element obtains the specific forces [f N f E f D and angular velocity of the vehicle in the north, east, and local directions.

[0129] Step 2, remove the satellites with the carrier-to-noise ratio of the satellite signals lower than the threshold; determine the carrier-to-noise ratio threshold β of the satellite signals according to the historical observation data, compare the carrier-to-noise ratio of all satellites with the threshold β, and save the satellites with the carrier-to-noise ratio of all satellites greater than β in the set A1, and remove all satellites with the carrier-to-noise ratio lower than β. The carrier-to-noise ratio threshold β of the satellite signals is set to 40.

[0130] Step 3: Doppler frequency shift velocity measurement and barometric altitude calculation to obtain altitude information;

[0131] Step 3-1: Doppler frequency shift velocity measurement, the method is as follows:

[0132]

[0133] Throughout the text, the "·" above the parameter represents the derivative with respect to time t, the "^" represents the posterior estimate, the "-" represents the prior estimate, and the "~" represents the measured value; is the vector from the vehicle u to the kth satellite at time t, is the difference between the vectors from the vehicle u to the kth satellite and the lth satellite at time t, is the derivative with respect to time t, is the transformation matrix from the navigation coordinate system to ECEF, is the velocity estimate of the vehicle u in the navigation coordinate system, is the posterior estimate of the position of the vehicle u relative to the reference station r, is the Doppler triple difference measurement value, ε D (t) is the Doppler frequency shift noise, is the Doppler frequency shift noise triple difference, x u (t) is the vehicle position;

[0134] Step 3-2: Calculate the barometric altitude using the international standard atmosphere model, the method is as follows:

[0135]

[0136] h(t) = A(t) + N(t) + ε h (t)

[0137] where A(t) is the barometric altitude, T0(t) is the absolute temperature at the local sea level, P0(t) is the barometric pressure at the local sea level, P(t) is the measured barometric pressure, γ, R, and g are the corresponding physical constants, ε A (t) is the barometric altimeter error, N(t) is the geoid height, h(t) is the ellipsoidal height, ε h (t) is the ellipsoidal height error.

[0138] where the lapse rate of temperature γ = -0.00649 K / m, the molar gas constant R = 287.05 J·kg·K -1 and the gravitational constant g = 9.80665 m / s 2 .

[0139] Step 4, velocity Kalman filtering, to obtain the velocity, mechanical arrangement, and Doppler shift velocity of the online diagnostic system, and output the velocity estimation to obtain the attitude and velocity information;

[0140] Step 4-1, construct the state vector X as follows:

[0141]

[0142] where, is the velocity error relative to the navigation coordinate system, δV N , δV E and δV D are the velocity errors in the north, east, and down directions respectively; φ = [φ N φ E φ D is the attitude error represented by Euler angles, φ N , φ E and φ D are the attitude errors in the north, east, and down directions respectively; is the accelerometer bias, and are the biases in each direction in the body coordinate system; ξ = [ξ x ξ y ξ z is the gyroscope drift, ξ x , ξ y and ξ z are the gyroscope drifts in each direction in the body coordinate system; is the offset of the online diagnostic system;

[0143] Step 4-2, construct the mechanical arrangement of the inertial measurement element, the method is as follows:

[0144]

[0145]

[0146]

[0147]

[0148]

[0149]

[0150]

[0151] where, ω is the measurement noise of the inertial measurement element, R m and R tThey are the earth's meridian and the equatorial radius of curvature respectively, h is the ellipsoid height, and L is the latitude; [Ω N 0 Ω D is the earth's angular rate of rotation with respect to the north, east, and local directions of the navigation coordinate system; [ρ N ρ E ρ D are the transfer angular rates in the north, east, and local directions respectively, [f N f E f D are the specific forces in the north, east, and local directions respectively, is the transfer matrix from the vehicle coordinate system to the navigation coordinate system;

[0152] Step 4-3, measurement error model, the method is as follows:

[0153] The Doppler frequency shift velocity measurement result is:

[0154]

[0155] The OBD speed measurement is:

[0156]

[0157] Where, is the speed of the vehicle in the navigation coordinate system, is the posterior estimate from the vehicle to the navigation coordinate system, and the measurement update equation is:

[0158] Z(t) = H(t)X(t) + ε Z (t)

[0159]

[0160]

[0161]

[0162] Where, r OBD 、r D are the noise variances of the on-board diagnostic system and the Doppler frequency shift respectively, and ε Z (t) is the measurement error;

[0163] Step 4-4, speed estimation output:

[0164] Determine whether the pseudorange and carrier phase measurements are available. When the number of available satellites cannot be used for positioning, use the speed estimate calculated by the speed Kalman filter to substitute the measurement ω KF (t) to perform substitution update on the carrier phase indirect measurement ω φ (t):

[0165]

[0166]

[0167] Among them, x u is the carrier position, is the derivative of the user position at time t with respect to time, x r is the reference station position, is the triple difference of carrier phase measurement; through the above substitution update, the error accumulation of the Hatch filter is controlled, and finally the speed estimate and the substitution measurement ω KF (t).

[0168] Step 5: Use the Hatch filter to obtain the carrier-smoothed pseudorange and perform position solution to obtain the position information;

[0169] Step 5-1: Incremental state update:

[0170] Calculate the indirect measurement of the pseudorange:

[0171]

[0172] where is the triple difference of the carrier phase derivative of the measurement with respect to time t, is the triple difference of the carrier phase measurement noise.

[0173] The indirect measurement of the speed estimate is obtained by the speed Kalman filter:

[0174]

[0175]

[0176] Among them, is the conversion matrix from the navigation coordinate system to the Earth-centered Earth-fixed coordinate system, and L and l are the latitude and longitude respectively;

[0177] Indirect measurement equation:

[0178]

[0179] Among them,

[0180] is the estimated incremental state vector, updated through where is the prior estimate of the state vector, is the posterior estimate of the state vector;

[0181] Step 5-2: Measurement update:

[0182] The indirect measurement of the pseudorange is as follows:

[0183]

[0184] where is the measured value of the pseudorange observation, is the prior estimate of the position of the carrier relative to the reference station, and the indirect measurement update is:

[0185]

[0186] where,

[0187] is the column vector formed by

[0188] Update the prior estimate to the posterior estimate:

[0189]

[0190] where Z(t) is the Hatch filter gain;

[0191] Step 5-3, output the position x u (t) obtained after carrier-smoothed pseudorange solution.

[0192] Step 6, input the position information obtained in Step 5, the altitude information obtained in Step 3, and the attitude and velocity information obtained in Step 4 into the unscented Kalman filter to perform error feedback on the mechanical alignment, and finally obtain the position and velocity information of the carrier to complete integrated navigation and positioning in an urban environment, specifically including: inputting the position x u (t) obtained by carrier-smoothed pseudorange solution, the altitude A(t) obtained by barometric altimeter solution, and the attitude φ and velocity obtained by mechanical alignment into the unscented Kalman filter to perform error feedback on the mechanical alignment, and finally obtain the position and velocity information of the carrier to complete integrated navigation and positioning in an urban environment.

[0193] The method for obtaining the position of the carrier is:

[0194] x u (t + 1) = x u (t) + Δx u (t)

[0195] The method for obtaining velocity information is:

[0196] V u (t + 1) = V u (t) + ΔV u (t)

[0197] The present invention provides an idea and method for a multi-source combined navigation and positioning method in a complex environment. There are many methods and ways to specifically implement this technical solution. The above is only the preferred embodiment of the present invention. It should be noted that for those of ordinary skill in the art of this technology, without departing from the principle of the present invention, several improvements and refinements can be made, and these improvements and refinements should also be regarded as the protection scope of the present invention. Each component not clearly defined in this embodiment can be implemented by using the prior art.

Claims

1. A multi-source combined navigation and positioning method in a complex environment, characterized in that It includes the following steps: Step 1, Global Navigation Satellite System and multi-sensor measurement and observation. The multi-sensors include a Global Navigation Satellite System receiver, an inertial navigation system, an on-line diagnosis system, and a barometric altimeter; Step 2, Remove satellites with a carrier-to-noise ratio of satellite signals lower than the threshold; Step 3, Doppler frequency shift velocity measurement and barometric altitude calculation to obtain altitude information; Step 4, Velocity Kalman filtering to obtain the velocity, mechanical alignment, and Doppler frequency shift velocity of the on-line diagnosis system, and output a velocity estimate to obtain attitude and velocity information; Step 5, Use Hatch filtering to obtain carrier-smoothed pseudorange and perform position calculation to obtain position information; Step 6, Input the position information obtained in Step 5, the altitude information obtained in Step 3, and the attitude and velocity information obtained in Step 4 into the unscented Kalman filter to perform error feedback on the mechanical alignment, and finally obtain the position and velocity information of the carrier to complete integrated navigation and positioning in an urban environment.

2. The multi-source combined navigation and positioning method in a complex environment according to claim 1, characterized in that, Step 1 includes: Step 1-1, in an urban environment, obtain the Doppler frequency shift measurement of the k-th satellite through the global navigation satellite system carrier u and the reference station r as and Obtain the carrier phase measurement as and Obtain the pseudo-range observation as and Obtain the current satellite set, i.e., the number of satellites, and the carrier-to-noise ratio of the satellite signal; Step 1-2, Calculate the single difference of carrier phase measurement: Calculate the double difference of carrier phase measurement: Calculate the triple difference of carrier phase measurement: Calculate the single difference of pseudorange measurement: Calculate the double difference of pseudorange measurement: Calculate the single difference of Doppler frequency shift: Calculate the double difference of Doppler frequency shift: where Δ is the difference of the measurements between the carrier u and the reference station r, and is the difference of the measurements between different satellites k and l; Steps 1-3: Obtain the ambient air pressure P(t) and temperature T0(t) through a barometric altimeter, and obtain the speed of the vehicle relative to the vehicle coordinate system through the on-vehicle diagnostic system The inertial measurement element obtains the specific forces [f N f E f D and angular velocity of the vehicle in the north, east, and local directions.

3. The multi-source combined navigation and positioning method in a complex environment according to claim 2, characterized in that Step 2 includes: Determine the satellite signal carrier-to-noise ratio threshold β according to historical observation data, compare the satellite signal carrier-to-noise ratios of all satellites with the threshold β, save the satellites with satellite signal carrier-to-noise ratios greater than β in set A1, and eliminate the satellites with satellite signal carrier-to-noise ratios lower than β.

4. A multi-source combined navigation and positioning method in a complex environment according to claim 3, characterized in that, Step 3 includes: Step 3-1, Doppler frequency shift velocity measurement, the method is as follows: Among them, is the vector from the vehicle u to the k-th satellite at time t, is the difference between the vector from the vehicle u to the k-th satellite and the l-th satellite at time t, is derived with respect to time t, is the transformation matrix from the navigation coordinate system to the Earth-centered Earth-fixed coordinate system, is the velocity estimate of the vehicle u in the navigation coordinate system, is the posterior estimate of the position of the vehicle u relative to the reference station r, is the triple-differenced Doppler measurement value, ε D (t) is the Doppler frequency shift noise, is the triple-differenced Doppler frequency shift noise, x u (t) is the vehicle position; Step 3-2, Calculate the barometric altitude using the international standard atmosphere model, the method is as follows: h(t) = A(t) + N(t) + ε h (t) where A(t) is the barometric altitude, T0(t) is the absolute temperature at the local sea level, P0(t) is the barometric pressure at the local sea level, P(t) is the measured barometric pressure, γ, R, and g are the corresponding physical constants, ε A (t) is the barometric altimeter error, N(t) is the geoid height, h(t) is the ellipsoidal height, ε h (t) is the ellipsoidal height error.

5. A multi-source combined navigation and positioning method in a complex environment according to claim 4, characterized in that Step 4 includes: Step 4-1, Construct the state vector X as follows: Among them, is the velocity error relative to the navigation coordinate system, δV N , δV E and δV D are the velocity errors in the north, east, and down directions respectively; φ = [φ N φ E φ D is the attitude error expressed using Euler angles, φ N , φ E and φ D are the attitude errors in the north, east, and down directions respectively; is the accelerometer zero bias, and are the zero biases in each direction in the vehicle coordinate system; ξ = [ξ x ξ y ξ z is the gyroscope drift, ξ x , ξ y and ξ z are the gyroscope drifts in each direction in the vehicle coordinate system; is the online diagnostic system offset; Step 4-2, Construct the mechanical alignment of the inertial measurement element, the method is as follows: where ω is the measurement noise of the inertial measurement element, R m and R t are the earth's meridian and equatorial radius of curvature respectively, h is the ellipsoidal height, and L is the latitude; [Ω N 0 Ω D is the earth's angular rate of rotation with respect to the north, east, and local directions of the navigation coordinate system; [ρ N ρ E ρ D are the transfer angular rates in the north, east, and local directions respectively, [f N f E f D are the specific forces in the north, east, and local directions respectively, is the transformation matrix from the vehicle coordinate system to the navigation coordinate system; Step 4-3, Measurement error model, the method is as follows: The result of Doppler frequency shift velocity measurement is: Observed quantity z obtained from OBD speed measurement is as follows OBD : wherein, is the velocity of the vehicle in the navigation coordinate system, is the posterior estimate from the vehicle to the navigation coordinate system, and the measurement update equation is: Z(t) = H(t)X(t) + ε Z (t) where r OBD and r D are the noise variances of the online diagnostic system and the Doppler shift, respectively; Step 4-4, Velocity estimate output: Determine whether the pseudorange and carrier phase measurements are available. When the number of available satellites cannot be used for positioning, use the velocity estimation of the velocity Kalman filter The calculated alternative measurement ω KF (t) for the indirect measurement of the carrier phase ω φ (t) for alternative update: where x u is the carrier position, is the derivative of the user position with respect to time at time t, and x r is the reference station position, is the triple difference of the carrier phase measurement; through the above substitution update, the error accumulation of the Hatch filter is controlled, and finally the speed estimate and the substitution measurement ω KF (t) are output.

6. The multi-source combined navigation and positioning method in a complex environment according to claim 5, wherein, Step 5 includes: Step 5-1, Incremental state update: Calculate the indirect measurement of pseudorange: where is the triple difference of carrier phase the derivative of the measurement with respect to time t, is the triple difference of carrier phase measurement noise; The indirect measurement of velocity estimate is obtained by velocity Kalman filtering: Among them, is the transformation matrix from the navigation coordinate system to the Earth-centered Earth-fixed coordinate system, where L and l are the latitude and longitude respectively; Indirect measurement equation: Among them, For incremental state vector estimation, updated by where is the prior estimate of the state vector, is the posterior estimate of the state vector; Step 5-2, Measurement update: The indirect measurement of pseudorange is: wherein is the measured value of the pseudorange observation, is the prior estimate of the position of the carrier relative to the reference station, and the indirect measurement update is: Wherein, is a column vector formed by; Update the prior estimate to the posterior estimate: Wherein, K(t) is the Hatch filtering gain; Step 5-3, output the position x obtained after carrier-smoothed pseudorange solution u (t).

7. A multi-source combined navigation and positioning method in a complex environment according to claim 6, characterized in that, Step 6 includes: the position x u (t) obtained by carrier smoothed pseudorange solution, the altitude A(t) obtained by barometric altimeter solution, and the attitude φ and velocity inputted into the unscented Kalman filter to perform error feedback on the mechanical alignment, and finally obtain the position and velocity information of the carrier, completing the integrated navigation and positioning in an urban environment.

8. A multi-source combined navigation and positioning method in a complex environment according to claim 7, characterized in that The physical constants described in Step 3-2 are: the lapse rate of temperature γ = -0.00649 K / m, the molar gas constant R = 287.05 J·kg·K -1 and the gravitational constant g = 9.80665 m / s 2 .

9. A multi-source combined navigation and positioning method in a complex environment according to claim 8, characterized in that, The satellite signal carrier-to-noise ratio threshold β described in Step 2 is set to 40.

10. A multi-source combined navigation and positioning method in a complex environment according to claim 9, characterized in that The method for obtaining the position of the carrier described in Step 6 is: x u (t + 1)=x u (t)+Δx u (t) The method for obtaining velocity information is: V u (t + 1)=V u (t)+ΔV u (t).

Images