CN114646993A - A data fusion algorithm for precise positioning based on GNSS, vision and IMU - Google Patents

Abstract

The invention discloses a data fusion algorithm for accurate positioning based on GNSS, vision and IMU. The GNSS positioning technology is easily limited by external observation conditions, and satellite signal receiving capacity is sharply weakened in sheltered areas such as viaducts and urban canyons, so that the positioning and navigation requirements cannot be met. The scale problem cannot be solved by pure visual position estimation, and a matching relation cannot be established in a weak texture scene and a fast motion scene, so that tracking loss is easy to occur; the IMU can reflect dynamic changes in a short time (in milliseconds), but its cumulative error increases over long runs (in seconds). According to the invention, according to the complementarity among different sensor data sources, the global positioning precision of the GNSS is improved by using the local data sources (vision and IMU), the local accumulated error is eliminated by using the global data sources of the GNSS, the short-term high-precision global positioning is maintained by depending on the local vision and IMU data when the satellite signals are shielded, and the performance and the robustness of the positioning navigation system are integrally improved.

Description

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

technical field

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

Background technique

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

In recent years, with the rapid development of the location service industry, a single positioning system has been unable to meet users' needs for reliable positioning results. The GNSS global navigation satellite system can obtain positioning trajectories in the real world, but it is easily limited by external observation conditions. In blocked areas such as viaducts, urban canyons, underground garages and tunnels, the satellite signal receiving ability is sharply weakened, resulting in significant positioning accuracy. Falling or unable to resolve location information. Visual odometry uses each frame of camera image for pure visual position estimation, but the calculated positioning trajectory has an uncertain scaling ratio compared with the real world, and cannot establish matching in weak texture scenes and fast motion scenes Relationships, easy to track lost. Visual inertial odometry uses camera and IMU for position estimation, which has high accuracy, but will be affected by accumulated errors in long-term operation, resulting in continuous decline of accuracy, and its positioning trajectory has a heading angle with the trajectory in the real world. deviation. Due to the limitations of single-sensor localization methods and the complementarity of data sources among multiple sensors, high-precision and robust multi-source fusion localization technology has become an urgent scientific problem to be solved.

SUMMARY OF THE INVENTION

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

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

Step S1: Mount a GNSS satellite receiving device, a monocular camera and an IMU module on the mobile device, and collect GNSS signals, image data and IMU data during the moving process as the input 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 vision at the initial moment. scale, gravitational acceleration, system initial velocity;

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

Step S4: 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, and the system global position and local position at the initial moment are roughly calculated. The heading angle deviation between the coordinate system and the global coordinate system;

Step S5: Constrain the positioning estimation by constructing a tightly coupled global cost function of GNSS: if the number of satellites received at this moment is 0, the local positioning result is directly globalized; if the number of received satellites at this moment is greater than 0, the The raw data constructs a GNSS cost function to constrain the positioning estimation;

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

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.

Beneficial effects of the present invention:

(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 the outdoor scene of GNSS signal reception, the positioning method based on the SPP algorithm to calculate the unique coordinates of the receiver in the global coordinate system is not accurate enough to meet the needs of positioning and navigation in many scenarios.

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

(3) The method of constructing cost function constrained positioning estimation based on GNSS original data (including pseudorange, Doppler frequency shift, clock, etc.) proposed by the present invention, in the case that the number of receiving satellites is less than 4 and greater than 0, still The local accumulated error can be eliminated by using the GNSS global information, which can benefit the positioning estimation.

Description of drawings

Figure 1 is a flowchart of a data fusion algorithm for precise positioning based on GNSS, vision and IMU.

FIG. 2 is a schematic diagram of the relationship between system velocity and Doppler confidence in the present invention.

Detailed ways

The present invention will be further described below with reference to the accompanying drawings.

The invention discloses a data fusion algorithm for precise positioning based on GNSS, vision and IMU. The flow chart of the specific embodiment of the present invention is shown in Figure 1:

多普勒频移

时钟等信息，相机的输入数据是连续序列的图像帧，IMU的输入数据是一连串物体的线性加速度α以及旋转角速度ω信息。Step S1: Mount a GNSS satellite signal receiver, a monocular camera and an IMU module on the mobile device, and collect 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 during the moving process, as the whole process. Input to the positioning algorithm. The raw data of GNSS includes ephemeris, pseudorange

Doppler shift

Clock and other 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 rotational angular velocity ω information of the object.

Step S2: Initialize and align the local visual and IMU data: The front-end feature point tracking algorithm summarizes the image into a series of sparse feature points, and performs a purely visual pose estimation 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 data of vision and IMU are jointly calibrated, and the initial velocity, gravitational acceleration (marking the initial attitude) and visual scale of the system are roughly calculated.

Step S3: Perform nonlinear optimization on local data: construct a visual cost function and an IMU cost function for the key frames in the set sliding window. For the visual cost function, it is constructed from the reprojection error obtained by the beam adjustment method between two frames, and for the IMU cost function, it is constructed from the acceleration, angular velocity of the object between the two frames and the zero offset of the IMU.

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

Among them, r _L is the cost function of the local data:

r _G is the 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 represent the respective 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 can be obtained from the angle deviation between the local positioning trajectory and the global positioning trajectory.

Step S5: Constrain the positioning estimation by constructing a GNSS global cost function: 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 raw data of GNSS satellite signals, including pseudorange, Doppler frequency shift and clock information, are 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 shown in formula (4).

是一个四维向量，其中三个值为0，一个值为1，表示GNSS中特定的卫星导航系统，δt代表时钟偏置,

代表卫星的时钟误差，c代表光速,T与I分别表示对流层和电离层延迟，

表示伪距。多普勒频移反映了卫星在信号传播路径上相对于接收机的运动，其代价函数如式(5)。Among them, R _z (θ) represents the rotation matrix rotated by the angle θ around the z-axis of the ECI coordinate system, ω _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, e′ represents the ECEF coordinate system when the satellite sends the signal,

is a four-dimensional vector, where three values are 0 and one value is 1, which represents the specific satellite navigation system in GNSS, δt represents the clock offset,

represents the clock error of the satellite, c represents the speed of light, T and I represent the tropospheric and ionospheric delays, respectively,

represents the pseudorange. The Doppler frequency shift reflects the movement of the satellite relative to the receiver on the signal propagation path, and its cost function is shown in equation (5).

为卫星时钟误差漂移率，

为多普勒频移，

代表时钟漂移率。接收机解算卫星信号的过程与接收机的时钟偏置以及时钟漂移率相关,其构建的两个代价函数如式(6)。where v is the velocity, λ is the wavelength of the carrier signal, κ is the unit vector from the receiver to the satellite,

is the satellite clock error drift rate,

is the Doppler frequency shift,

represents the clock drift rate. The process of the receiver to solve the satellite signal is related to the receiver's clock offset and clock drift rate.

表示k帧与k-1帧的时间间隔。where δt represents the clock offset, 1 _m×n represents a matrix with m rows and n columns where all elements are 1,

Indicates the time interval between k frames and k-1 frames.

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 receiver is moving at a low speed, the signal-to-noise ratio of the Doppler frequency shift will decrease, resulting in a decrease in its constraint accuracy. As shown in Figure 2, the relationship between system velocity and Doppler confidence in this algorithm is shown. When the speed is less than v _min , 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. When the speed is between v _min and v _max , there is a linear relationship between the two, and when the speed increases above v _max , the confidence level does not change. v _min and v _max can be adjusted based on specific sensor kit characteristics and test conditions.

Step S7: Perform local and global tightly coupled joint nonlinear optimization: use the least squares method to 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, such as Formula (7) is shown.

为GNSS的紧耦合代价函数。根据实际需要，决定是否输出系统的姿态以及视觉的点云信息。Among them, the first item is the prior information brought by marginalization, r _B is the cost function of the IMU, r _C is the cost function of the camera,

is the tightly coupled cost function of GNSS. According to actual needs, decide whether to output the pose of the system and the visual point cloud information.

In a word, according to the above 7 steps, by integrating the information of GNSS, vision and IMU, the advantages of different sensors can be complemented, and finally the positioning and navigation system can achieve accurate positioning effect in the open outdoor scene, in the scene of satellite signal loss. It ensures short-term high-precision global positioning estimation, which improves the performance and robustness of the positioning and navigation system as a whole.

Claims (7)

1. the data fusion algorithm that carries out precise positioning based on GNSS, vision and IMU, is characterized in that, comprises the steps: Step S1: Mount a GNSS satellite receiving device, a monocular camera and an IMU module on the mobile device, and collect GNSS signals, image data and IMU data during the moving process as the input 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 vision at the initial moment. scale, gravitational acceleration, system initial velocity; Step S3: Perform nonlinear optimization on local data: construct a visual cost function and an IMU cost function for the key frames in the set sliding window, and obtain the local data positioning result by minimizing the overall cost function; Step S4: 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, and the system global position and local position at the initial moment are roughly calculated. The heading angle deviation between the coordinate system and the global coordinate system; Step S5: Constrain the positioning estimation by constructing a tightly coupled global cost function of GNSS: if the number of satellites received at this moment is 0, the local positioning result is directly globalized; if the number of received satellites at this moment is greater than 0, the The raw data constructs a GNSS cost function to constrain the positioning estimation; Step S6: adjusting the covariance matrix of the Doppler frequency shift cost function according to the current motion speed of the system, and changing the confidence level of the positioning estimation on the Doppler frequency shift information; 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. 2. the data fusion algorithm that carries out precise positioning based on GNSS, vision and IMU as claimed in claim 1, it is characterized in that, described step S1, the input of positioning algorithm comprises from the raw data of GNSS, the input data of camera, the input data of IMU. Input data data; GNSS raw data including ephemeris, pseudorange

Doppler shift

Clock and other 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 rotational angular velocity ω information of the object. 3. the data fusion algorithm that carries out precise positioning based on GNSS, vision and IMU as claimed in claim 1, is characterized in that, described step S1, collection frequency is that the GNSS signal of 10Hz, the image data of frame rate of 30fps and frequency are The 200Hz IMU data is used as the input for the entire positioning algorithm. 4. the data fusion algorithm that carries out precise positioning based on GNSS, vision and IMU as claimed in claim 1, is characterized in that, described step S4, based on the optimization method of loose coupling, as shown in formula (1);

Among them, r _L is the cost function of the local data:

r _G is the 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 subscripts in the upper right corner represent the local data source and global data respectively. Source respective positioning results, θ is the heading angle deviation between the local coordinate system and the global coordinate system, which is obtained by the angular deviation between the local positioning trajectory and the global positioning trajectory. 5. the data fusion algorithm that carries out precise positioning based on GNSS, vision and IMU as claimed in claim 1, is characterized in that, described step S5, if the number of satellites received at this moment is 0, then by making up local and global coordinate system If the number of satellites received at this moment is greater than 0, the raw data of GNSS satellite signals, including pseudorange, Doppler frequency shift and clock information, are used to construct a GNSS cost function Constrain the localization estimate. 6. the data fusion algorithm that carries out precise positioning based on GNSS, vision and IMU as claimed in claim 1, it is characterized in that, described step S6, when receiver is in low-speed motion state, the signal-to-noise of Doppler frequency shift When the speed is less than v _min , the confidence in the Doppler information is 0, that is, the Doppler frequency shift information provided by the satellite is not considered at all; the speed is between v _min and v _max . When the speed increases above v _max , the confidence level does not change; v _min and v _max are adjusted according to the specific sensor suite characteristics and test conditions. 7. the data fusion algorithm that carries out precise positioning based on GNSS, vision and IMU as claimed in claim 1, is characterized in that, described step S7, based on the nonlinear optimization method of tight coupling, as shown in formula (4);

Among them, the first item is the prior information brought by marginalization, r _B is the cost function of the IMU, r _C is the cost function of the camera,

is the tightly coupled cost function of GNSS; according to actual needs, decide whether to output the attitude of the system and the visual point cloud information.

Images