CN114545475B - A multi-source integrated navigation and positioning method in complex environments - Google Patents
A multi-source integrated navigation and positioning method in complex environments Download PDFInfo
- Publication number
- CN114545475B CN114545475B CN202210043088.XA CN202210043088A CN114545475B CN 114545475 B CN114545475 B CN 114545475B CN 202210043088 A CN202210043088 A CN 202210043088A CN 114545475 B CN114545475 B CN 114545475B
- Authority
- CN
- China
- Prior art keywords
- carrier
- measurement
- positioning
- navigation
- coordinate system
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
- G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
本发明公开了一种复杂环境下的多源组合导航定位方法，属于卫星定位导航的技术领域。城市环境下全球导航卫星系统信号容易被周边建筑物反射或遮挡，单纯的全球导航卫星系统定位在城市环境中的精度及可靠性会显著降低。本发明通过在线诊断系统、多普勒频移测量、惯性测量元件分别获取的载体速度利用卡尔曼滤波进行紧耦合，并输出速度估计量；通过经典的载波平滑伪距算法Hatch滤波改进伪距观测的精度，并解算载体位置，当全球导航卫星系统信号不可用时，卡尔曼滤波器输出的速度估计作为Hatch滤波的更新替代，从而保证了载波平滑伪距定位的连续性，最终提高了城市环境的定位精度及可靠性。
The present invention discloses a multi-source combined navigation positioning method in a complex environment, and belongs to the technical field of satellite positioning and navigation. In an urban environment, global navigation satellite system signals are easily reflected or blocked by surrounding buildings, and the accuracy and reliability of simple global navigation satellite system positioning in an urban environment will be significantly reduced. The present invention uses Kalman filtering to tightly couple the carrier speeds obtained by an online diagnostic system, Doppler frequency shift measurement, and inertial measurement elements, and outputs a speed estimate; the accuracy of pseudorange observations is improved by using the classic carrier smoothing pseudorange algorithm Hatch filtering, and the carrier position is solved. When the global navigation satellite system signal is unavailable, the speed estimate output by the Kalman filter is used as an update replacement for the Hatch filter, thereby ensuring the continuity of carrier smoothing pseudorange positioning, and ultimately improving the positioning accuracy and reliability in the urban environment.
Description
Technical Field
The invention relates to a navigation positioning method, in particular to a multi-source combined navigation positioning method in a complex environment.
Background
Accurate and reliable positioning of urban environments has attracted considerable attention over the years, such as autopilots, urban drones, etc., where such carriers, aircraft are required to operate in low-altitude urban environments. In order to improve navigation positioning performance in urban environments, integration of multi-source information such as global navigation satellite system (Global Navigation SATELLITE SYSTEM, GNSS) and inertial navigation system (Inertial Navigation System, INS), magnetic compass, high-precision electronic map, vision sensor, light Detection and ranging (Light Detection AND RANGING, LIDAR), altimeter, odometer, inertial measurement unit (Inertial Measurement Unit, IMU), speed sensor, etc. has been studied.
Kassas proposes a opportunistic signal/digital map/IMU/GNSS combined method, (refer to ：Robust vehicular localization and map matching in urban environments through IMU,GNSS,and cellular signals),, which combines a cellular network signal, a digital map and a traditional IMU/GNSS, so as to improve the positioning accuracy in urban environment, but the method needs to ensure that the position of a cellular network base station is known and identify the base station corresponding to the cellular network signal, kim and the like are used for an unmanned aerial vehicle (refer to A baro-ALTIMETER AUGMENTED INS/GPS navigation system for an uninhabited AERIAL VEHICLE), an INS/GPS combined navigation system is enhanced by a barometer, the vertical positioning accuracy and reliability are obviously improved, and the positioning error accumulation is faster when a GPS signal is not available. Liu et al have proposed a pedestrian integrated Navigation system (see: THE PEDESTRIAN INTEGRATED Navigation SYSTEM WITH Micro IMU/GPS/Magnetometer/Barometric Altimeter) that uses an IMU, GPS, magnetometer and barometric altimeter to obtain information and extended Kalman filtering to locate pedestrians, but the magnetometer is easily affected by the steel structure in the city. Jacques et al propose a low cost mems inertial sensor integrated with GPS vehicle navigation solution (reference ：Low-Cost Three-Dimensional Navigation Solution for RISS/GPS Integration Using Mixture Particle Filter), which uses hybrid particle filtering as a nonlinear filter, a simple inertial sensing system (Reduced Inertial Sensor System, RISS) combined with GPS to implement a three-dimensional navigation solution, and a loose coupling approach, where RISS consists of two accelerometers, one gyroscope and a carrier odometer. bondan through IMU, The GPS and on-line diagnostic system (On Board Diagnostics, OBD) sensors construct a set of integrated navigation systems (ref: OBD-II Sensor Approaches for The IMU and GPS Based Apron Vehicle Positioning System) and update gradient Kalman filtering using OBD speed, which significantly reduces drift effects due to the OBD speed allowing the presence of absolute zero speed, especially in the case of stationary carriers. Su proposes a method for fusing, positioning and attitude determination of multiple sensors such as GNSS/IMU and Vision (refer to GR-SLAM: vision-Based Sensor Fusion SLAM for Ground Robots on Complex Terrain), accuracy of matching of feature points of two adjacent images and resolving accuracy of position and attitude are improved, but the method is easily affected by surrounding environments such as illumination, and the extraction and matching efficiency of the feature points is low. Wen et al propose a coordinated integration method of inter-range measurement with GNSS/camera/INS (reference ：Multi-agent collaborative GNSS/camera/INS integration aided by inter-ranging for vehicular navigation in urban areas.), improves the detection performance of the overlapping region of cameras, but is not applicable in areas with poor weather environment due to visual localization defects).
According to the analysis of the prior literature, the simple GNSS/IMU combination has the defect of too fast accumulation of the unavailable errors of the GNSS, and the GNSS and vision combination has the defects of higher calculation load, incapability of use under the condition of poor meteorological conditions and poor reliability. The method has the advantages that the OBD acquisition speed is more accurate, errors of the GNSS/IMU in the running direction can be restrained, the barometric altimeter can be used in all weather, and the reliability in the vertical direction is higher than that of the GNSS. In order to combine the use characteristics of urban environment and consider the cost factor, the patent proposes a GNSS/IMU/OBD/altimeter combined navigation method in urban environment.
The technology which is relatively close to the technology of the invention is to perform positioning detection by combining a vision system with a 3D city model to identify the city block, but the method has low reliability due to more ground interference in the city and is easy to influence other carriers, pedestrians and other obstacles, and has low operability and practicability.
Disclosure of Invention
Aiming at the defects of the prior art, the invention provides a multi-source combined navigation positioning method in a complex environment.
In order to solve the technical problems, the invention discloses a multi-source combined navigation positioning method in a complex environment, which comprises the following steps:
Step 1, a global navigation satellite system and a multi-sensor measurement observation, wherein the multi-sensor comprises a global navigation satellite system receiver, an inertial navigation system, an online diagnosis system and an barometric altimeter;
step 2, removing satellites with satellite signal carrier-to-noise ratios lower than a threshold value;
step 3, doppler frequency shift speed measurement and air pressure height calculation are carried out, and height information is obtained;
Step4, velocity Kalman filtering, namely acquiring the velocity, mechanical arrangement and Doppler frequency shift velocity of the online diagnosis system, and outputting velocity estimation to obtain attitude and velocity information;
step 5, acquiring a carrier smooth pseudo-range by utilizing a Hatch filter, and performing position calculation to obtain position information;
And 6, inputting the position information obtained in the step 5 and the height information obtained in the step 3 into unscented Kalman filtering, performing error feedback on mechanical arrangement, and finally obtaining the position and the speed information of the carrier to finish the integrated navigation positioning in the urban environment.
In the present invention, step1 includes:
step 1-1, in urban environment, obtaining Doppler frequency shift measurement of the kth satellite through a global navigation satellite system carrier receiver u and a reference station receiver r as follows AndAcquiring carrier phase measurements asAndObtaining the pseudo-range observed quantity asAndAcquiring the current satellite set, namely the number of satellites, and the carrier-to-noise ratio of satellite signals;
Step 1-2, calculating carrier phase measurement single difference:
calculating carrier phase measurement double differences:
calculating carrier phase measurement three differences:
calculating a pseudo-range measurement single difference:
calculating a pseudo-range measurement double difference:
Calculating Doppler frequency shift single difference:
Calculating Doppler frequency shift double difference:
Where delta is the difference between the measurements of the carrier u and the reference station r, To differential measurements between different satellites k, l;
step 1-3, acquiring the ambient air pressure P (T) and the temperature T 0 (T) through an air pressure altimeter, and acquiring the speed of the carrier relative to a carrier coordinate system through a carrier online diagnosis system The inertial measurement unit obtains the specific force [ f N fE fD ] and the angular velocity of the carrier in the north, east and ground directions.
In the present invention, step2 includes:
And determining a satellite signal carrier-to-noise ratio threshold beta according to the historical observation data, comparing the satellite signal carrier-to-noise ratios of all satellites with the threshold beta, storing all satellites with satellite signal carrier-to-noise ratios greater than beta in the A1 set, and eliminating all satellites with satellite signal carrier-to-noise ratios lower than beta.
In the present invention, step 3 includes:
step 3-1, doppler frequency shift speed measurement, the method is as follows:
In the invention, "-" above the parameter represents derivation for time t, "-" represents posterior estimation, "-" represents prior estimation, "-" represents measured value; Vectors for the t-time carrier u through the kth satellite, The difference between vectors of the carrier u to the kth satellite and the first satellite at the time t,Is thatThe derivative is derived for the time t,For navigating the coordinate system to the ECEF transformation matrix,For the velocity estimation of the carrier u in the navigation coordinate system,For a posterior estimate of the position of the carrier u relative to the reference station r,For Doppler three-difference measurement, epsilon D (t) is Doppler frequency shift noise, delta epsilon D (t) is Doppler frequency shift noise three-difference, x u (t) is carrier position, and step 3-2, calculating the barometric pressure altitude by using an international standard atmosphere model, wherein the method comprises the following steps:
h(t)=A(t)+N(t)+εh(t)
Wherein A (T) is air pressure height, T 0 (T) is absolute temperature of local sea level, P 0 (T) is air pressure of local sea level, P (T) is actual measured air pressure, gamma, R and g are corresponding physical constants, epsilon A (T) is air pressure altimeter error, N (T) is ground level height, h (T) is ellipsoid height, epsilon h (T) is ellipsoid height error.
In the present invention, step 4 includes:
Step 4-1, construct state vector X as follows:
Wherein, the Delta V N、δVE and delta V D are respectively north, east and earth speed errors, phi= [ phi N φE φD ] is an attitude error expressed by Euler angles, phi N、φE and phi D are respectively north, east and earth attitude errors; for the zero-bias of the accelerometer, AndIs zero offset of each direction under the carrier coordinate system, and ζ= [ ζ x ξy ξz ] is gyroscope drift, and ζ x、ξy and ζ z are gyroscope drift of each direction under the carrier coordinate system; for online diagnostic system offset;
Step 4-2, constructing an inertial measurement unit mechanical arrangement, wherein the method comprises the following steps:
Wherein ω is the inertial measurement unit measurement noise, R m and R t are the earth meridian and equatorial radius of curvature, h is the ellipsoidal height, L is the latitude, [ omega N 0 ΩD ] is the earth rotation angular velocity relative to the navigation coordinate system in each of the north, east and earth directions, [ ρ N ρE ρD ] is the transfer angular velocity in each of the north, east and earth directions, [ f N fE fD ] is the specific force in each of the north, east and earth directions, A transfer matrix from the carrier coordinate system to the navigation coordinate system;
And 4-3, measuring an error model, wherein the method comprises the following steps:
the Doppler frequency shift speed measurement result is as follows:
From OBD speed measurements The observed quantity z OBD obtained is:
Wherein, the For the speed of the carrier in the navigational coordinate system,For posterior estimation of the carrier to the navigation coordinate system, the measurement update equation is:
Z(t)=H(t)X(t)+εZ(t)
Wherein r OBD、rD is the noise variance of the on-line diagnostic system and the Doppler shift, and ε Z (t) is the measurement error;
step 4-4, speed estimation output:
Determining whether pseudorange and carrier phase measurements are available, and when the number of available satellites cannot be used for positioning, velocity estimation using velocity Kalman filtering The calculated surrogate measurement ω KF (t) performs a surrogate update on the carrier phase indirect measurement ω φ (t):
Wherein x u is the carrier position, Deriving time for the user position at time t, x r is the reference station position,The error accumulation of the Hatch filter is controlled by the substitution update to finally output the speed estimationAnd an alternative measurement ω KF (t).
In the present invention, step5 includes:
step 5-1, incremental status update:
Calculating pseudo-range indirect measurement:
Wherein is carrier phase three difference Is a derivative of the measurement of (c) with respect to time t,Noise third difference is measured for carrier phase.
Indirect measurement of velocity estimation, obtained by velocity kalman filtering:
Wherein, the The matrix is a conversion matrix from a navigation coordinate system to a geocentric and geodetic fixed coordinate system, L, l is latitude and longitude respectively;
indirect measurement equation:
Wherein, the
For incremental state vector estimation byPerforming an update in whichFor an a priori estimation of the state vector,A posterior estimate of the state vector;
Step 5-2, measurement update:
the pseudo-range indirect measurement is as follows:
Wherein the method comprises the steps of As a measurement of the observed quantity of the pseudo-range,For a priori estimation of the position of the carrier relative to the reference station, the indirect measurement is updated as:
Wherein, the
Is thatA column vector is constructed.
Updating the a priori estimates to a posterior estimate:
wherein Z (t) is a Hatch filter gain;
And 5-3, outputting a position x u (t) obtained after the carrier smoothing pseudo-range is solved.
In the invention, step 6 comprises calculating the position x u (t) of the carrier smooth pseudo range, calculating the height A (t) of the air pressure altitude, and mechanically arranging the acquired gesture phi and speedAnd (3) inputting unscented Kalman filtering, performing error feedback on mechanical arrangement, and finally obtaining position and speed information of the carrier to finish integrated navigation positioning in urban environment.
In the invention, the physical constants in the step 3-2 are a temperature reduction rate gamma= -0.00649K/m, a molar gas constant R= 287.05 J.kg.K -1 and a gravity constant g=9.80665 m/s 2.
The satellite signal carrier-to-noise ratio threshold beta in the step 2 is set to 40.
In the invention, the method for acquiring the position of the carrier in the step 6 is as follows:
xu(t+1)＝xu(t)+Δxu(t)
the method for acquiring the speed information comprises the following steps:
Vu(t+1)＝Vu(t)+ΔVu(t)
The beneficial effects are that:
1. The conventional visual positioning in the multi-sensor fusion is not available under the condition of poor meteorological conditions, and the all-weather reliable navigation positioning cannot be realized. The invention improves the reliability of the integrated navigation system by using all-weather sensors, namely an air pressure altimeter, an OBD and an IMU.
2. The existing integrated navigation method has lower vertical precision in urban environment. According to the invention, the vertical interval can be well established by the heights of the plurality of carriers obtained by the barometric altimeter under the same barometric reference plane, so that the method is very effective for carrier operation in urban three-dimensional environment, especially for operation of unmanned aircraft, and can remarkably improve the accuracy and reliability of vertical positioning.
3. When the conventional integrated navigation method is unavailable, the estimation accuracy of the carrier position information is significantly reduced. According to the invention, the carrier speed estimation is obtained by utilizing the speed Kalman filtering, and the carrier speed estimation is used as the alternative measurement of the carrier smooth pseudo range when the GNSS signal is unavailable, so that the continuity of the carrier smooth pseudo range is ensured, the calculation complexity is reduced, and the precision of the integrated navigation positioning when the GNSS signal is unavailable is finally improved.
Drawings
The foregoing and/or other advantages of the invention will become more apparent from the following detailed description of the invention when taken in conjunction with the accompanying drawings and detailed description.
FIG. 1 is a schematic flow chart of the present invention.
Detailed Description
A multi-source integrated navigation positioning method under a complex environment is shown in figure 1, firstly, measurement information of a GNSS receiver and a multi-sensor is obtained, wherein the measurement information comprises Doppler frequency shift, pseudo range and carrier phase measurement provided by the GNSS, specific force and angular velocity obtained by an IMU, carrier velocity provided by an OBD system and barometric altitude obtained by a barometric altimeter. And secondly, mechanically arranging the IMU, and carrying out fault detection on GNSS measurement to extract available signals. Then, the speed measured by Doppler frequency shift, the carrier speed obtained by OBD, the IMU speed obtained by mechanical arrangement, the gesture and the position information are input into Kalman filtering to obtain the speed estimation of the carrier, whether carrier phase measurement and pseudo-range measurement are available or not is judged, when the carrier phase measurement and the pseudo-range measurement are available, the Hatch filtering is input into the speed estimation input of Kalman filtering to update the carrier phase measurement and the pseudo-range measurement is output, and the ellipsoidal height is calculated by using the obtained geodetic reference surface height, the ambient temperature and the air pressure height of the air pressure altimeter. And then, calculating the carrier position by using the carrier smoothing pseudo range, inputting the position, the Doppler frequency shift measured speed, the ellipsoid height, the mechanical arrangement acquired speed, posture and position information into unscented Kalman filtering, and carrying out feedback correction on the mechanical arrangement to acquire the carrier speed, posture and position calculated by the integrated navigation method. The method comprises the following steps:
Step 1, a global navigation satellite system and a multi-sensor measurement observation, wherein the multi-sensor comprises a global navigation satellite system receiver, an inertial navigation system, an online diagnosis system and an barometric altimeter;
step 1-1, in urban environment, obtaining Doppler frequency shift measurement of the kth satellite through a global navigation satellite system carrier receiver u and a reference station receiver r as follows AndAcquiring carrier phase measurements asAndObtaining the pseudo-range observed quantity asAndAcquiring the current satellite set, namely the number of satellites, and the carrier-to-noise ratio of satellite signals;
Step 1-2, calculating carrier phase measurement single difference:
calculating carrier phase measurement double differences:
calculating carrier phase measurement three differences:
calculating a pseudo-range measurement single difference:
calculating a pseudo-range measurement double difference:
Calculating Doppler frequency shift single difference:
Calculating Doppler frequency shift double difference:
Where delta is the difference between the measurements of the carrier u and the reference station r, To differential measurements between different satellites k, l;
step 1-3, acquiring the ambient air pressure P (T) and the temperature T 0 (T) through an air pressure altimeter, and acquiring the speed of the carrier relative to a carrier coordinate system through a carrier online diagnosis system The inertial measurement unit obtains the specific force [ f N fE fD ] and the angular velocity of the carrier in the north, east and ground directions.
And step 2, removing satellites with the satellite signal carrier-to-noise ratios lower than the threshold value, determining a satellite signal carrier-to-noise ratio threshold value beta according to historical observation data, comparing the satellite signal carrier-to-noise ratios of all satellites with the threshold value beta, storing all satellites with the satellite signal carrier-to-noise ratios higher than the threshold value beta in an A1 set, and removing all satellites with the satellite signal carrier-to-noise ratios lower than the threshold value beta. Wherein the satellite signal carrier-to-noise ratio threshold β is set to 40.
Step 3, doppler frequency shift speed measurement and air pressure height calculation are carried out, and height information is obtained;
step 3-1, doppler frequency shift speed measurement, the method is as follows:
throughout, "·" above the parameter represents derivation for time t, "ζ" represents posterior estimate, "-" represents prior estimate, "-" represents measured value; Vectors for the t-time carrier u through the kth satellite, The difference between vectors of the carrier u to the kth satellite and the first satellite at the time t,Is thatThe derivative is derived for the time t,For navigating the coordinate system to the ECEF transformation matrix,For the velocity estimation of the carrier u in the navigation coordinate system,For a posterior estimate of the position of the carrier u relative to the reference station r,Epsilon D (t) is the Doppler shift noise for Doppler tri-difference measurements,X u (t) is the carrier position, which is the Doppler shift noise three difference;
and 3-2, calculating the air pressure height by using an international standard atmosphere model, wherein the method comprises the following steps:
h(t)=A(t)+N(t)+εh(t)
Wherein A (T) is air pressure height, T 0 (T) is absolute temperature of local sea level, P 0 (T) is air pressure of local sea level, P (T) is actual measured air pressure, gamma, R and g are corresponding physical constants, epsilon A (T) is air pressure altimeter error, N (T) is ground level height, h (T) is ellipsoid height, epsilon h (T) is ellipsoid height error.
Wherein the temperature reduction rate gamma= -0.00649K/m, the molar gas constant r= 287.05 j.kg.k -1 and the gravitational constant g=9.80665 m/s 2.
Step4, velocity Kalman filtering, namely acquiring the velocity, mechanical arrangement and Doppler frequency shift velocity of the online diagnosis system, and outputting velocity estimation to obtain attitude and velocity information;
Step 4-1, construct state vector X as follows:
Wherein, the Delta V N、δVE and delta V D are respectively north, east and earth speed errors, phi= [ phi N φE φD ] is an attitude error expressed by Euler angles, phi N、φE and phi D are respectively north, east and earth attitude errors; for the zero-bias of the accelerometer, AndIs zero offset of each direction under the carrier coordinate system, and ζ= [ ζ x ξy ξz ] is gyroscope drift, and ζ x、ξy and ζ z are gyroscope drift of each direction under the carrier coordinate system; for online diagnostic system offset;
Step 4-2, constructing an inertial measurement unit mechanical arrangement, wherein the method comprises the following steps:
Wherein ω is the inertial measurement unit measurement noise, R m and R t are the earth meridian and equatorial radius of curvature, h is the ellipsoidal height, L is the latitude, [ omega N 0 ΩD ] is the earth rotation angular velocity relative to the navigation coordinate system in each of the north, east and earth directions, [ ρ N ρE ρD ] is the transfer angular velocity in each of the north, east and earth directions, [ f N fE fD ] is the specific force in each of the north, east and earth directions, A transfer matrix from the carrier coordinate system to the navigation coordinate system;
And 4-3, measuring an error model, wherein the method comprises the following steps:
the Doppler frequency shift speed measurement result is as follows:
OBD speed measurements were:
Wherein, the For the speed of the carrier in the navigational coordinate system,For posterior estimation of the carrier to the navigation coordinate system, the measurement update equation is:
Z(t)=H(t)X(t)+εZ(t)
Wherein r OBD、rD is the noise variance of the on-line diagnostic system and the Doppler shift, and ε Z (t) is the measurement error;
step 4-4, speed estimation output:
Determining whether pseudorange and carrier phase measurements are available, and when the number of available satellites cannot be used for positioning, velocity estimation using velocity Kalman filtering The calculated surrogate measurement ω KF (t) performs a surrogate update on the carrier phase indirect measurement ω φ (t):
Wherein x u is the carrier position, Deriving time for the user position at time t, x r is the reference station position,The error accumulation of the Hatch filter is controlled by the substitution update to finally output the speed estimationAnd an alternative measurement ω KF (t).
Step 5, acquiring a carrier smooth pseudo-range by utilizing a Hatch filter, and performing position calculation to obtain position information;
step 5-1, incremental status update:
Calculating pseudo-range indirect measurement:
Wherein is carrier phase three difference Is a derivative of the measurement of (c) with respect to time t,Noise third difference is measured for carrier phase.
Indirect measurement of velocity estimation, obtained by velocity kalman filtering:
Wherein, the The matrix is a conversion matrix from a navigation coordinate system to a geocentric and geodetic fixed coordinate system, L, l is latitude and longitude respectively;
indirect measurement equation:
Wherein, the
For incremental state vector estimation byPerforming an update in whichFor an a priori estimation of the state vector,A posterior estimate of the state vector;
Step 5-2, measurement update:
the pseudo-range indirect measurement is as follows:
Wherein the method comprises the steps of As a measurement of the observed quantity of the pseudo-range,For a priori estimation of the position of the carrier relative to the reference station, the indirect measurement is updated as:
Wherein, the
Is thatA column vector is constructed.
Updating the a priori estimates to a posterior estimate:
wherein Z (t) is a Hatch filter gain;
And 5-3, outputting a position x u (t) obtained after the carrier smoothing pseudo-range is solved.
Step 6, inputting the position information obtained in step 5, the height information obtained in step 3 and the gesture and speed information obtained in step 4 into unscented Kalman filtering, performing error feedback on mechanical arrangement, finally obtaining the position and speed information of a carrier, and completing combined navigation positioning in urban environment, wherein the method specifically comprises the steps of smoothing the position x u (t) of a carrier, calculating the height A (t) of air pressure height, and mechanically arranging the acquired gesture phi and speedAnd (3) inputting unscented Kalman filtering, performing error feedback on mechanical arrangement, and finally obtaining position and speed information of the carrier to finish integrated navigation positioning in urban environment.
The method for acquiring the position of the carrier comprises the following steps:
xu(t+1)＝xu(t)+Δxu(t)
the method for acquiring the speed information comprises the following steps:
Vu(t+1)＝Vu(t)+ΔVu(t)
the invention provides a thought and a method for realizing a multisource combined navigation positioning method in a complex environment, and the method and the way for realizing the technical scheme are a plurality of methods, and the above is only a preferred embodiment of the invention, and it should be pointed out that a plurality of improvements and modifications can be made by one of ordinary skill in the art without departing from the principle of the invention, and the improvements and modifications are also considered as the protection scope of the invention. The components not explicitly described in this embodiment can be implemented by using the prior art.
Claims (10)
1. The multi-source integrated navigation positioning method in the complex environment is characterized by comprising the following steps:
Step 1, a global navigation satellite system and a multi-sensor measurement observation, wherein the multi-sensor comprises a global navigation satellite system receiver, an inertial navigation system, an online diagnosis system and an barometric altimeter;
step 2, removing satellites with satellite signal carrier-to-noise ratios lower than a threshold value;
step 3, doppler frequency shift speed measurement and air pressure height calculation are carried out, and height information is obtained;
Step4, velocity Kalman filtering, namely acquiring the velocity, mechanical arrangement and Doppler frequency shift velocity of the online diagnosis system, and outputting velocity estimation to obtain attitude and velocity information;
step 5, acquiring a carrier smooth pseudo-range by utilizing a Hatch filter, and performing position calculation to obtain position information;
And 6, inputting the position information obtained in the step 5 and the height information obtained in the step 3 into unscented Kalman filtering, performing error feedback on mechanical arrangement, and finally obtaining the position and the speed information of the carrier to finish the integrated navigation positioning in the urban environment.
2. The method for multi-source integrated navigation and positioning in a complex environment according to claim 1, wherein step 1 comprises:
Step 1-1, in urban environment, obtaining Doppler frequency shift measurement of the kth satellite through the global navigation satellite system carrier u and the reference station r as AndAcquiring carrier phase measurements asAndObtaining the pseudo-range observed quantity asAndAcquiring the current satellite set, namely the number of satellites, and the carrier-to-noise ratio of satellite signals;
Step 1-2, calculating carrier phase measurement single difference:
calculating carrier phase measurement double differences:
calculating carrier phase measurement three differences:
calculating a pseudo-range measurement single difference:
calculating a pseudo-range measurement double difference:
Calculating Doppler frequency shift single difference:
Calculating Doppler frequency shift double difference:
Where delta is the difference between the measurements of the carrier u and the reference station r, For differentiating measurements between different satellites k, l;
step 1-3, acquiring the ambient air pressure P (T) and the temperature T 0 (T) through an air pressure altimeter, and acquiring the speed of the carrier relative to a carrier coordinate system through a carrier online diagnosis system The inertial measurement unit obtains the specific force [ f N fEfD ] and the angular velocity of the carrier in the north, east and ground directions.
3. The method for multi-source integrated navigation and positioning in a complex environment according to claim 2, wherein step 2 comprises:
And determining a satellite signal carrier-to-noise ratio threshold beta according to the historical observation data, comparing the satellite signal carrier-to-noise ratios of all satellites with the threshold beta, storing all satellites with satellite signal carrier-to-noise ratios greater than beta in the A1 set, and eliminating all satellites with satellite signal carrier-to-noise ratios lower than beta.
4. A multi-source integrated navigation positioning method in a complex environment according to claim 3, wherein step 3 comprises:
step 3-1, doppler frequency shift speed measurement, the method is as follows:
Wherein, the Vectors for the t-time carrier u through the kth satellite,The difference between vectors of the carrier u to the kth satellite and the first satellite at the time t,Is thatThe derivative is derived for the time t,For the transformation matrix of the navigation coordinate system to the geocentric and geodetic fixed coordinate system,For the velocity estimation of the carrier u in the navigation coordinate system,For a posterior estimate of the position of the carrier u relative to the reference station r,Epsilon D (t) is the Doppler shift noise for Doppler tri-difference measurements,X u (t) is the carrier position, which is the Doppler shift noise three difference;
and 3-2, calculating the air pressure height by using an international standard atmosphere model, wherein the method comprises the following steps:
h(t)=A(t)+N(t)+εh(t)
Wherein A (T) is air pressure height, T 0 (T) is absolute temperature of local sea level, P 0 (T) is air pressure of local sea level, P (T) is actual measured air pressure, gamma, R and g are corresponding physical constants, epsilon A (T) is air pressure altimeter error, N (T) is ground level height, h (T) is ellipsoid height, epsilon h (T) is ellipsoid height error.
5. The method for multi-source integrated navigation and positioning in a complex environment according to claim 4, wherein step 4 comprises:
Step 4-1, construct state vector X as follows:
Wherein, the Delta V N、δVE and delta V D are respectively north, east and earth speed errors, phi= [ phi N φE φD ] is an attitude error expressed by Euler angles, phi N、φE and phi D are respectively north, east and earth attitude errors; for the zero-bias of the accelerometer, AndIs zero offset of each direction under the carrier coordinate system, and ζ= [ ζ x ξy ξz ] is gyroscope drift, and ζ x、ξy and ζ z are gyroscope drift of each direction under the carrier coordinate system; for online diagnostic system offset;
Step 4-2, constructing an inertial measurement unit mechanical arrangement, wherein the method comprises the following steps:
Wherein ω is the inertial measurement unit measurement noise, R m and R t are the earth meridian and equatorial radius of curvature, h is the ellipsoidal height, L is the latitude, [ omega N 0 ΩD ] is the earth rotation angular velocity relative to the navigation coordinate system in each of the north, east and earth directions, [ ρ N ρE ρD ] is the transfer angular velocity in each of the north, east and earth directions, [ f N fE fD ] is the specific force in each of the north, east and earth directions, A transfer matrix from the carrier coordinate system to the navigation coordinate system;
And 4-3, measuring an error model, wherein the method comprises the following steps:
the Doppler frequency shift speed measurement result is as follows:
From OBD speed measurements The observed quantity z OBD obtained is:
Wherein, the For the speed of the carrier in the navigational coordinate system,For posterior estimation of the carrier to the navigation coordinate system, the measurement update equation is:
Z(t)=H(t)X(t)+εZ(t)
Wherein r OBD、rD is the noise variance of the on-line diagnostic system and the Doppler shift, respectively;
step 4-4, speed estimation output:
Determining whether pseudorange and carrier phase measurements are available, and when the number of available satellites cannot be used for positioning, velocity estimation using velocity Kalman filtering The calculated surrogate measurement ω KF (t) performs a surrogate update on the carrier phase indirect measurement ω φ (t):
Wherein x u is the carrier position, Deriving time for the user position at time t, x r is the reference station position,The error accumulation of the Hatch filter is controlled by the substitution update to finally output the speed estimationAnd an alternative measurement ω KF (t).
6. The method for multi-source integrated navigation and positioning in a complex environment according to claim 5, wherein step 5 comprises:
step 5-1, incremental status update:
Calculating pseudo-range indirect measurement:
Wherein is carrier phase three difference Is a derivative of the measurement of (c) with respect to time t,Measuring a noise third difference for the carrier phase;
indirect measurement of velocity estimation, obtained by velocity kalman filtering:
Wherein, the The matrix is a conversion matrix from a navigation coordinate system to a geocentric and geodetic fixed coordinate system, L, l is latitude and longitude respectively;
indirect measurement equation:
Wherein, the
For incremental state vector estimation byPerforming an update in whichFor an a priori estimation of the state vector,A posterior estimate of the state vector;
Step 5-2, measurement update:
the pseudo-range indirect measurement is as follows:
Wherein the method comprises the steps of As a measurement of the observed quantity of the pseudo-range,For a priori estimation of the position of the carrier relative to the reference station, the indirect measurement is updated as:
Wherein, the
Is thatA column vector is formed;
Updating the a priori estimates to a posterior estimate:
Wherein K (t) is a Hatch filter gain;
And 5-3, outputting a position x u (t) obtained after the carrier smoothing pseudo-range is solved.
7. The method of claim 6, wherein step 6 includes calculating the position x u (t) of the carrier smooth pseudo range, the altitude A (t) of the barometric altitude calculation, and mechanically arranging the acquired pose phi and velocityAnd (3) inputting unscented Kalman filtering, performing error feedback on mechanical arrangement, and finally obtaining position and speed information of the carrier to finish integrated navigation positioning in urban environment.
8. The method for multi-source integrated navigation and positioning under a complex environment according to claim 7, wherein the physical constants in the step 3-2 are a temperature reduction rate gamma= -0.00649K/m, a molar gas constant R= 287.05 j.kg.k -1 and a gravity constant g=9.80665 m/s 2.
9. The method for integrated navigation and positioning in a complex environment according to claim 8, wherein the satellite signal carrier-to-noise ratio threshold β in step 2 is set to 40.
10. The method for multi-source integrated navigation positioning under a complex environment according to claim 9, wherein the method for obtaining the position of the carrier in step 6 is as follows:
xu(t+1)＝xu(t)+Δxu(t)
the method for acquiring the speed information comprises the following steps:
Vu(t+1)＝Vu(t)+ΔVu(t)。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210043088.XA CN114545475B (en) | 2022-01-14 | 2022-01-14 | A multi-source integrated navigation and positioning method in complex environments |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210043088.XA CN114545475B (en) | 2022-01-14 | 2022-01-14 | A multi-source integrated navigation and positioning method in complex environments |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114545475A CN114545475A (en) | 2022-05-27 |
| CN114545475B true CN114545475B (en) | 2025-07-04 |
Family
ID=81671111
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210043088.XA Active CN114545475B (en) | 2022-01-14 | 2022-01-14 | A multi-source integrated navigation and positioning method in complex environments |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114545475B (en) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115616641B (en) * | 2022-12-16 | 2023-03-17 | 南京航空航天大学 | A high-precision positioning method for integrated navigation in urban canyons based on particle filter |
| CN119738853A (en) * | 2024-04-30 | 2025-04-01 | 中山大学 | A smart phone positioning method, system, device and medium |
| CN118688839A (en) * | 2024-06-19 | 2024-09-24 | 南京理工大学 | A multi-level joint enhanced positioning method and system assisted by a low-orbit satellite constellation |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102445200A (en) * | 2011-09-30 | 2012-05-09 | 南京理工大学 | Microminiature personal combined navigation system and navigation positioning method thereof |
| CN103487822A (en) * | 2013-09-27 | 2014-01-01 | 南京理工大学 | BD/DNS/IMU autonomous integrated navigation system and method thereof |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9030356B2 (en) * | 2009-09-29 | 2015-05-12 | Texas Instruments Incorporated | Positioning system receiver sensor system coupled with measurement data output |
| US20110238308A1 (en) * | 2010-03-26 | 2011-09-29 | Isaac Thomas Miller | Pedal navigation using leo signals and body-mounted sensors |
| US8756001B2 (en) * | 2011-02-28 | 2014-06-17 | Trusted Positioning Inc. | Method and apparatus for improved navigation of a moving platform |
| WO2012167367A1 (en) * | 2011-06-09 | 2012-12-13 | Trusted Positioning Inc. | Method and apparatus for real-time positioning and navigation of a moving platform |
| CN111149141A (en) * | 2017-09-04 | 2020-05-12 | Nng软件开发和商业有限责任公司 | Method and apparatus for collecting and using sensor data from vehicles |
| US11350245B2 (en) * | 2020-05-22 | 2022-05-31 | Qualcomm Incorporated | Time-domain processing for positioning signals |
-
2022
- 2022-01-14 CN CN202210043088.XA patent/CN114545475B/en active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102445200A (en) * | 2011-09-30 | 2012-05-09 | 南京理工大学 | Microminiature personal combined navigation system and navigation positioning method thereof |
| CN103487822A (en) * | 2013-09-27 | 2014-01-01 | 南京理工大学 | BD/DNS/IMU autonomous integrated navigation system and method thereof |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114545475A (en) | 2022-05-27 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN114545475B (en) | A multi-source integrated navigation and positioning method in complex environments | |
| Sun et al. | Robust IMU/GPS/VO integration for vehicle navigation in GNSS degraded urban areas | |
| CN111156994B (en) | A loose integrated navigation method of INS/DR&GNSS based on MEMS inertial components | |
| Garratt et al. | Integration of GPS/INS/vision sensors to navigate unmanned aerial vehicles | |
| EP2133662B1 (en) | Methods and system of navigation using terrain features | |
| CN111288984B (en) | Multi-vehicle joint absolute positioning method based on Internet of vehicles | |
| CN102829785B (en) | Air vehicle full-parameter navigation method based on sequence image and reference image matching | |
| CN112505737B (en) | GNSS/INS integrated navigation method | |
| CN101858748B (en) | Fault-tolerance autonomous navigation method of multi-sensor of high-altitude long-endurance unmanned plane | |
| CN108051866B (en) | Based on strap down inertial navigation/GPS combination subsidiary level angular movement isolation Gravimetric Method | |
| CN107656301A (en) | A kind of vehicle positioning method based on Multi-source Information Fusion | |
| CN113203418A (en) | GNSSINS visual fusion positioning method and system based on sequential Kalman filtering | |
| CN113252033A (en) | Positioning method, positioning system and robot based on multi-sensor fusion | |
| CN109916394A (en) | Combined navigation algorithm fusing optical flow position and speed information | |
| CN115523920B (en) | A seamless positioning method based on visual-inertial GNSS tight coupling | |
| CN110849360B (en) | Distributed relative navigation method for multi-machine collaborative formation flight | |
| CN106405670A (en) | Gravity anomaly data processing method applicable to strapdown marine gravimeter | |
| CN106405592B (en) | Vehicle-mounted Beidou carrier phase cycle slips detection and restorative procedure and system | |
| CN115327588A (en) | A high-precision positioning method for unmanned automatic operation special vehicles based on network RTK | |
| CN108613675B (en) | Low-cost unmanned aerial vehicle mobile measurement method and system | |
| Meguro et al. | Low-cost lane-level positioning in urban area using optimized long time series GNSS and IMU data | |
| CN116558512A (en) | GNSS and vehicle-mounted sensor fusion positioning method and system based on factor graph | |
| CN115479605A (en) | High-altitude long-endurance unmanned aerial vehicle autonomous navigation method based on space target directional observation | |
| Wen et al. | 3D LiDAR aided GNSS real-time kinematic positioning | |
| Klein et al. | LiDAR and INS fusion in periods of GPS outages for mobile laser scanning mapping systems |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |