A data fusion algorithm for precise positioning based on GNSS, vision and IMU

Abstract

The present invention discloses a data fusion algorithm for precise positioning based on GNSS, vision and IMU. GNSS positioning technology is easily restricted by external observation conditions. In obstructed areas such as viaducts and urban canyons, the satellite signal receiving ability is sharply weakened, and it cannot meet the needs of positioning and navigation. Purely visual position estimation cannot solve the scale problem, and cannot establish a matching relationship in weak texture scenes and fast motion scenes, and is prone to tracking loss; IMU can reflect dynamic changes in a short time (milliseconds), but its cumulative error will continue to increase under long-term operation (seconds). Based on the complementarity between different sensor data sources, the present invention uses local data sources (vision and IMU) to improve the global positioning accuracy of GNSS, uses the GNSS global data source to eliminate local cumulative errors, and relies on local vision and IMU data to maintain short-term high-precision global positioning when satellite signals are blocked, thereby improving the performance and robustness of the positioning and navigation system as a whole.

Description

Technical Field

[0001] The present invention relates to the field of navigation and positioning, and in particular to a navigation and positioning system based on GNSS, vision and IMU. Background Art

[0002] High-precision and high-robust positioning technology is crucial to national security, artificial intelligence, the Internet of Things and other high-tech industries. After continuous research and development in the industry, a variety of different positioning systems have been produced, including GNSS global navigation satellite system, INS inertial navigation system, VO visual odometer, CNS astronomical navigation system, etc.

[0003] In recent years, with the rapid development of the location service industry, a single positioning system can no longer meet the needs of users for reliable positioning results. The GNSS global navigation satellite system can obtain positioning trajectories in the real world, but it is easily restricted by external observation conditions. In obstructed areas such as viaducts, urban canyons, underground garages and tunnels, the satellite signal reception capability is sharply weakened, resulting in a significant decrease in positioning accuracy or the inability to resolve positioning information. The visual odometer uses each frame of the camera for pure visual position estimation, but the positioning trajectory it solves has an uncertain scale scaling ratio compared to the real world, and it cannot establish a matching relationship in weak texture scenes and fast motion scenes, and it is easy to lose tracking. The visual inertial odometer uses a camera and an IMU for position estimation, which has high accuracy, but in long-term operation, it will be affected by the cumulative error, resulting in a continuous decrease in accuracy, and there is a heading angle deviation between its positioning trajectory and the trajectory in the real world. Due to the limitations of the single-sensor positioning method and the complementarity of the data sources between multiple sensors, high-precision and high-robust multi-source fusion positioning technology has become a scientific problem that needs to be solved urgently. Summary of the invention

[0004] The purpose of the present invention is to improve the positioning performance of a GNSS navigation and positioning system and to solve the problem of GNSS satellite signal loss in complex scenarios.

[0005] The present invention proposes a data fusion algorithm for precise positioning based on GNSS, vision and IMU, and the implementation process includes:

[0006] Step S1: A GNSS satellite receiving device, a monocular camera and an IMU module are mounted on the mobile device, and GNSS signals, image data and IMU data are collected during movement as inputs of the entire positioning algorithm;

[0007] Step S2: Initialize and align the local vision and IMU data; perform front-end visual tracking and SFM visual pose solution on the image data, pre-integrate the IMU data, and then align the data of the two to roughly solve the visual scale, gravity acceleration, and initial speed of the system at the initial moment;

[0008] Step S3: Perform nonlinear optimization on the local data: construct a visual cost function and an IMU cost function for the key frames in the set sliding window, and obtain the positioning result of the local data by minimizing the overall cost function;

[0009] Step S4: Perform loosely coupled joint initialization of local and global data sources: Based on nonlinear optimization technology, the local positioning result is integrated with the global positioning result of GNSS based on the SPP algorithm to roughly calculate the global position of the system at the initial moment and the heading angle deviation between the local coordinate system and the global coordinate system;

[0010] Step S5: construct a tightly coupled global cost function of GNSS to constrain the positioning estimation: if the number of satellites received at this moment is 0, then the local positioning result is directly globalized; if the number of satellites received at this moment is greater than 0, then the GNSS cost function is constructed using the original data of the GNSS satellite signal to constrain the positioning estimation;

[0011] Step S6: adjusting the covariance matrix of the Doppler frequency shift cost function according to the current movement speed of the system, and changing the confidence level of the positioning estimation on the Doppler frequency shift information;

[0012] Step S7: Perform local and global tightly coupled joint nonlinear optimization: solve the final global positioning estimate of the system by minimizing the overall cost function composed of visual constraints, IMU constraints and GNSS raw data constraints.

[0013] Beneficial effects of the present invention:

[0014] (1) The present invention improves the positioning performance of the navigation and positioning system by integrating the global data source of GNSS with the high-precision visual and IMU local data sources. In outdoor scenarios where GNSS signals are received, the positioning method based on the SPP algorithm to solve the unique coordinates of the receiver in the global coordinate system is not accurate enough and cannot meet the needs of positioning and navigation in many scenarios.

[0015] (2) The data fusion algorithm based on GNSS, vision and IMU for precise positioning proposed in the present invention enables the system to seamlessly provide short-term high-precision global positioning estimates in various scenarios where the GNSS satellite signal is completely lost. When the satellite signal is recaptured, the system positioning mode is seamlessly switched, and the local accumulated error is eliminated using the GNSS global information to continuously obtain high-precision global positioning.

[0016] (3) The method proposed in the present invention for constructing a cost function constrained positioning estimation based on GNSS raw data (including pseudorange, Doppler frequency shift, clock, etc.) can still use GNSS global information to eliminate local accumulated errors and maximize positioning estimation benefits when the number of received satellites is less than 4 and greater than 0. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 Flowchart of the data fusion algorithm for precise positioning based on GNSS, vision and IMU.

[0018] Figure 2 It is a schematic diagram of the relationship between system speed and Doppler confidence in the present invention. DETAILED DESCRIPTION

[0019] The present invention will be further described below in conjunction with the accompanying drawings.

[0020] The present invention discloses a data fusion algorithm for accurate positioning based on GNSS, vision and IMU. The specific implementation flow chart of the present invention is as follows: Figure 1 As shown:

[0021] Step S1: The mobile device is equipped with a GNSS satellite signal receiver, a monocular camera and an IMU module. During the movement, GNSS signals with a frequency of 10 Hz, image data with a frame rate of 30 fps and IMU data with a frequency of 200 Hz are collected as inputs to the entire positioning algorithm. The raw GNSS data includes ephemeris, pseudorange and Doppler shift The input data of the camera is a continuous sequence of image frames, and the input data of the IMU is a series of linear acceleration α and rotation angular velocity ω information of the object.

[0022] Step S2: Initialize and align the local visual and IMU data: The image is summarized into a series of sparse feature points through the front-end feature point tracking algorithm, and a purely visual pose estimation is performed based on SFM. Since the sampling frequency of the IMU is higher than that of the camera, the input data of the IMU is pre-integrated and aligned with each frame of the camera in time. Then the visual and IMU data are jointly calibrated to roughly solve the initial velocity, gravity acceleration (marking the initial posture) and visual scale of the system.

[0023] Step S3: Perform nonlinear optimization on local data: construct visual cost function and IMU cost function for key frames in the set sliding window. The visual cost function is constructed by the reprojection error obtained by bundle adjustment between two frames, and the IMU cost function is constructed by the acceleration, angular velocity and zero bias of the object between two frames.

[0024] Step S4: Perform loosely coupled joint initialization of local and global data sources: Based on nonlinear optimization technology, the local positioning results are fused with the global positioning results of GNSS based on the SPP algorithm in a loosely coupled manner to roughly calculate the system global position at the initial moment and the heading angle deviation between the local coordinate system and the global coordinate system, as shown in formula (1).

[0025]

[0026] Among them, r L is the cost function for local data:

[0027]

[0028] r G is a loosely coupled global data cost function:

[0029]

[0030] Among them, the subscript k represents the kth frame, δx represents the cost in the x direction, δy represents the cost in the y direction, and x and y are the positioning coordinates that need to be optimized. The local and global in the upper right corner represent the positioning results of the local data source and the global data source respectively, and θ is the heading angle deviation between the local coordinate system and the global coordinate system, which can be obtained by the angle deviation between the local positioning trajectory and the global positioning trajectory.

[0031] Step S5: Construct a GNSS global cost function to constrain positioning estimation: If the number of satellites received at this moment is 0, the local positioning result is globalized by compensating for the heading angle deviation between the local and global coordinate systems. If the number of satellites received at this moment is greater than 0, the original data of the GNSS satellite signal, including pseudorange, Doppler frequency shift and clock information, is used to construct a GNSS cost function to constrain the positioning estimation. Among them: the pseudorange represents the distance between the receiver and the satellite, and its cost function is as shown in formula (4).

[0032]

[0033] Among them, R z (θ) represents the rotation matrix of the ECI coordinate system rotated by an angle of θ around the z-axis, ω E represents the angular velocity of the Earth's rotation, t f is the flight time of the GNSS satellite signal, p represents the position coordinate, s represents the satellite, r represents the receiver, E represents the ECEF coordinate system when the receiver receives the signal, and e′ represents the ECEF coordinate system when the satellite sends the signal. is a four-dimensional vector with three values ​​of 0 and one value of 1, indicating a specific satellite navigation system in GNSS, δt represents the clock bias, represents the satellite clock error, c represents the speed of light, T and I represent the tropospheric and ionospheric delays respectively, Represents pseudorange. Doppler shift reflects the motion of the satellite relative to the receiver on the signal propagation path, and its cost function is shown in formula (5).

[0034]

[0035] Where v represents the velocity, λ represents the wavelength of the carrier signal, and κ represents the unit vector from the receiver to the satellite. is the satellite clock error drift rate, is the Doppler shift, Represents the clock drift rate. The receiver's process of solving satellite signals is related to the receiver's clock bias and clock drift rate. The two cost functions constructed are shown in formula (6).

[0036]

[0037]

[0038] Where, δt represents the clock bias, 1 m×n represents a matrix with m rows and n columns where all elements are 1. Indicates the time interval between frame k and frame k-1.

[0039] Step S6: Adjust the covariance matrix of the Doppler frequency shift cost function according to the current speed of the system to change the confidence level of the positioning estimate on the Doppler frequency shift information. When the receiver is in a low-speed motion state, the signal-to-noise ratio of the Doppler frequency shift will decrease, resulting in a decrease in its constraint accuracy. Figure 2 As shown in the figure, the relationship between system velocity and Doppler confidence in this algorithm is demonstrated. min When the speed is v, the confidence level of the Doppler information is 0, that is, the Doppler frequency shift information provided by the satellite is not considered at all. min to v max When the speed increases to v, the two are in a linear relationship. max When the confidence level is above , the confidence level will not change. min With v max It can be adjusted according to the specific sensor kit characteristics and test conditions.

[0040] Step S7: Perform local and global tightly coupled joint nonlinear optimization: Use the least squares method to minimize the overall cost function composed of visual constraints, IMU constraints, and GNSS raw data constraints to solve the final global positioning estimate of the system, as shown in formula (7).

[0041]

[0042] Among them, the first one is the prior information brought by marginalization, r B is the cost function of IMU, r C is the camera cost function, is the tightly coupled cost function of GNSS. According to actual needs, it is decided whether to output the system attitude and visual point cloud information.

[0043] In summary, according to the above 7 steps, by integrating the information of GNSS, vision and IMU, the advantages of different sensors can be complementary, and finally the positioning and navigation system can achieve accurate positioning in open outdoor scenes, and ensure short-term high-precision global positioning estimation in the scenario of satellite signal loss, which improves the performance and robustness of the positioning and navigation system as a whole.

Claims

1. A data fusion algorithm for precise positioning based on GNSS, vision and IMU, characterized by: The steps include: Step S1: A GNSS satellite receiving device, a monocular camera and an IMU module are mounted on the mobile device, and GNSS signals, image data and IMU data are collected during movement as inputs of the entire positioning algorithm; Step S2: Initialize and align the local vision and IMU data; Perform front-end visual tracking and SFM visual pose solution on the image data, pre-integrate the IMU data, and then align the two data to roughly solve the visual scale, gravity acceleration, and initial speed of the system at the initial moment; Step S3: Perform nonlinear optimization on the local data: construct a visual cost function and an IMU cost function for the key frames in the set sliding window, and obtain the positioning result of the local data by minimizing the overall cost function; Step S4: Perform loosely coupled joint initialization of local and global data sources: Based on nonlinear optimization technology, the local positioning result is integrated with the global positioning result of GNSS based on the SPP algorithm to roughly calculate the global position of the system at the initial moment and the heading angle deviation between the local coordinate system and the global coordinate system; Step S5: construct a tightly coupled global cost function constrained positioning estimation of GNSS: if the number of satellites received at this moment is 0, the local positioning result is globalized by compensating for the heading angle deviation between the local and global coordinate systems; If the number of satellites received at this moment is greater than 0, the original data of the GNSS satellite signal, including pseudorange, Doppler frequency shift and clock information, is used to construct a GNSS cost function to constrain the positioning estimate; where: The pseudorange represents the distance between the receiver and the satellite, and its cost function is as follows: Among them, R z (θ) represents the rotation matrix of the ECI coordinate system rotated by an angle of θ around the z-axis, ω E represents the angular velocity of the Earth's rotation, t f is the flight time of the GNSS satellite signal, p represents the position coordinate, s represents the satellite, r represents the receiver, E represents the ECEF coordinate system when the receiver receives the signal, and e′ represents the ECEF coordinate system when the satellite sends the signal. is a four-dimensional vector with three values ​​of 0 and one value of 1, indicating a specific satellite navigation system in GNSS, δt represents the clock bias, represents the satellite clock error, c represents the speed of light, T and I represent the tropospheric and ionospheric delays respectively, represents pseudorange; The Doppler shift reflects the movement of the satellite relative to the receiver on the signal propagation path, and its cost function is as follows: Where v represents the velocity, λ represents the wavelength of the carrier signal, and κ represents the unit vector from the receiver to the satellite. is the satellite clock error drift rate, is the Doppler shift, represents the clock drift rate; The process of the receiver calculating the satellite signal is related to the receiver's clock bias and clock drift rate. The two cost functions constructed are as follows: Where, δt represents the clock bias, 1 m×n represents a matrix with m rows and n columns where all elements are 1. Indicates the time interval between frame k and frame k-1; Step S6: adjust the covariance matrix of the Doppler frequency shift cost function according to the current motion speed of the system, and change the confidence level of the positioning estimation on the Doppler frequency shift information; when the speed is less than v min When the speed is v min to v max When the speed increases to v max When the value is above , the confidence level will not change; Step S7: Perform local and global tightly coupled joint nonlinear optimization: By minimizing the overall cost function composed of visual constraints, IMU constraints and GNSS raw data constraints, the final global positioning estimate of the system is solved, as shown in formula (7): Among them, the first term is the prior information brought by marginalization, rB is the cost function of IMU, and r C is the camera cost function, is the tightly coupled cost function of GNSS.

2. The data fusion algorithm for precise positioning based on GNSS, vision and IMU as claimed in claim 1, characterized in that: In step S1, the input of the positioning algorithm includes raw data from GNSS, input data from the camera, and input data from the IMU; the raw data from the GNSS includes ephemeris, pseudorange, Doppler shift Clock information, the input data of the camera is a continuous sequence of image frames, and the input data of the IMU is a series of linear acceleration α and rotation angular velocity ω information of the object.

3. The data fusion algorithm for precise positioning based on GNSS, vision and IMU as claimed in claim 1, characterized in that: In step S1, GNSS signals with a frequency of 10 Hz, image data with a frame rate of 30 fps, and IMU data with a frequency of 200 Hz are collected as inputs of the entire positioning algorithm.

4. The data fusion algorithm for precise positioning based on GNSS, vision and IMU as claimed in claim 1, characterized in that: The step S4 is based on a loosely coupled optimization method, as shown in formula (1); Among them, r L is the cost function for local data: r G is a loosely coupled global data cost function: Among them, the subscript k represents the kth frame, δx represents the cost in the x direction, δy represents the cost in the y direction, and x and y are the positioning coordinates that need to be optimized; the local and global in the upper right corner respectively represent the positioning results of the local data source and the global data source, and θ is the heading angle deviation between the local coordinate system and the global coordinate system, which is obtained by the angle deviation between the local positioning trajectory and the global positioning trajectory.

Images