Fusion positioning method, device, electronic device and storage medium

Abstract

This application belongs to the field of navigation and positioning technology, and discloses a fusion positioning method, device, electronic device and storage medium. The first pose data of the object to be positioned is obtained by jointly using a vision camera and an IMU module; the second pose data of the object to be positioned is obtained by jointly using the IMU module and the GNSS module; the weight value is determined according to the quality of the GNSS signal; the first pose data and the second pose data are weighted and fused to calculate the pose data measurement value of the object to be positioned; thus, an organic fusion of vision positioning technology, GNSS positioning technology and IMU positioning technology is realized, and the positioning accuracy in a complex environment with long-term occlusion of GNSS signals can be improved.

Description

Technical Field

[0001] The present application relates to the technical field of navigation and positioning, and in particular, to a fusion positioning method, device, electronic device, and storage medium. Background Art

[0002] Currently, when performing navigation and positioning, GNSS (Global Navigation Satellite System) technology and IMU (Inertial Measurement Unit) technology are mostly used for positioning. If GNSS technology is used alone for positioning, when there is GNSS signal occlusion, normal positioning cannot be carried out. If IMU technology is used alone for positioning, due to the phenomenon of cumulative error, when used for a long time, the positioning deviation is too large. Therefore, GNSS technology and IMU technology are often combined for fusion positioning (hereinafter referred to as GNSS / IMU fusion positioning technology). When the GNSS signal is interrupted briefly, positioning is performed through IMU technology. When the GNSS signal is restored, the cumulative error of the IMU is corrected according to the GNSS positioning result, thereby improving the positioning ability in complex environments.

[0003] However, when using this GNSS / IMU fusion positioning technology for navigation and positioning, in complex environments such as long tunnels and underground garages where the GNSS signal is occluded for a long time, since the GNSS signal is occluded for a long time, the IMU positioning module works alone for a long time for positioning, resulting in too large cumulative error, and further causing a large deviation in the positioning result. Therefore, this GNSS / IMU fusion positioning technology is not applicable to complex environments where the GNSS signal is occluded for a long time.

[0004] Currently, visual SLAM (Simultaneous Localization and Mapping) technology has been widely used in the field of indoor positioning. Using visual SLAM technology for navigation and positioning can effectively improve the positioning accuracy. However, there are problems of scale uncertainty and passivation during initialization.

[0005] Therefore, it is necessary to seek a fusion positioning method that combines the advantages of visual SLAM technology, GNSS technology, and IMU technology to improve the positioning accuracy in complex environments where the GNSS signal is occluded for a long time. Summary of the Invention

[0006] The purpose of the present application is to provide a fusion positioning method, device, electronic device, and storage medium, which can improve the positioning accuracy in complex environments where the GNSS signal is occluded for a long time.

[0007] In a first aspect, the present application provides a fusion positioning method, which is applied to a positioning system with a vision camera, an IMU module, and a GNSS module to position a target object, and includes the following steps:

[0008] A1. Combining the vision camera and the IMU module to obtain first pose data of the target object;

[0009] A2. Combining the IMU module and the GNSS module to obtain second pose data of the target object;

[0010] A3. Determining a weight value according to the quality of the GNSS signal;

[0011] A4. Performing weighted fusion calculation on the first pose data and the second pose data according to the weight value to obtain a pose data measurement value of the target object.

[0012] In this fusion positioning method, first, the first pose data is obtained by combining the vision camera and the IMU module respectively, and the second pose data is obtained by combining the IMU module and the GNSS module. Then, the weight value for fusing the first pose data and the second pose data is determined according to the quality of the GNSS signal. Finally, weighted fusion calculation is performed on the first pose data and the second pose data to obtain the final pose data. When the GNSS signal is blocked for a long time, a relatively accurate positioning result can still be obtained based on the vision camera and the IMU module. Compared with the existing GNSS / IMU fusion positioning technology, it has higher positioning accuracy in a complex environment with long-term occlusion of the GNSS signal.

[0013] Preferably, step A1 includes:

[0014] Obtaining multiple frames of images collected by the vision camera;

[0015] Extracting pixel coordinates of matching feature points of two adjacent frames of images according to the images;

[0016] Performing epipolar geometry solution according to the pixel coordinates of the matching feature points to calculate rough pose data of the target object;

[0017] Obtaining measurement data of the IMU module and performing pre-integration processing to obtain corresponding pre-integrated data; the measurement data includes accelerometer bias and gyroscope bias;

[0018] Performing depth estimation on the matching feature points through triangulation according to the pixel coordinates of the matching feature points and the pre-integrated data to obtain depth data of the matching feature points;

[0019] Generate state variables based on the rough pose data, the depth data, the measurement data of the IMU module, the pre-integrated data, the extrinsic parameters of the IMU module, and the extrinsic parameters of the vision camera, and perform non-linear optimization on the state variables to obtain the optimal estimated values of the state variables;

[0020] Extract the first pose data from the optimal estimated values of the state variables.

[0021] By using the pre-integrated data to estimate the depth of the matched feature points, the scale uncertainty and passivation problems existing in the initialization of visual positioning can be effectively avoided. By performing non-linear optimization on the state variables to obtain the first pose data, the accuracy of the first pose data can be improved.

[0022] Preferably, the state variables are:

[0023] ;

[0024] Among them, is the state variable, are the state estimation data corresponding to the 1st to the nth frames of images respectively. For the kth frame of image, there is: ; is the state estimation data corresponding to the kth frame of image, is the position in the rough pose data corresponding to the kth frame of image, is the attitude in the rough pose data corresponding to the kth frame of image, is the velocity corresponding to the kth frame of image, is the accelerometer bias corresponding to the kth frame of image, is the gyroscope bias corresponding to the kth frame of image;

[0025] Among them, ; is the extrinsic parameter matrix, is the extrinsic parameter of the vision camera, is the extrinsic parameter of the IMU module; is the depth data corresponding to the 1st to the nth frames of images;

[0026] The steps of performing non-linear optimization on the state variables to obtain the optimal estimated values of the state variables include:

[0027] Perform maximum a posteriori estimation according to the following model to obtain the optimal estimated values of the state variables:

[0028] ;

[0029] Among them, is the marginalized residual, is the Mahalanobis distance related to the covariance, is the Jacobian matrix;

[0030] wherein, is the IMU residual, is the IMU residual function, is the IMU measurement value, is the covariance matrix of the IMU pre-integration noise term;

[0031] wherein, is the visual reprojection error, is the visual reprojection error function, is the visual feature point coordinates, is the covariance matrix of the visual observation noise.

[0032] The above model is a tightly coupled model that comprehensively utilizes visual positioning data and IUM positioning data. Its advantage lies in the relatively accurate estimation result of the state variables of the pose, so as to obtain relatively accurate first pose data.

[0033] Preferably, step A2 includes:

[0034] Obtain the second pose data of the object to be located by using a GNSS / IMU fusion algorithm based on Kalman filtering.

[0035] Preferably, step A3 includes:

[0036] Obtain the third pose data of the object to be located measured by each satellite of the satellite positioning system;

[0037] Calculate the sum of squares of pseudorange residuals according to the third pose data;

[0038] Calculate the least squares residual statistic and the detection threshold according to the sum of squares of pseudorange residuals;

[0039] Compare the least squares residual statistic and the detection threshold to judge the quality of the GNSS signal;

[0040] Determine the weight value according to the judgment result of the quality of the GNSS signal.

[0041] Preferably, the step of comparing the least squares residual statistic and the detection threshold to judge the quality of the GNSS signal includes:

[0042] If , then it is determined that the quality of the GNSS signal is not good;

[0043] If , then it is determined that the quality of the GNSS signal is good;

[0044] If , it is determined that the quality of the GNSS signal is medium;

[0045] Wherein, is the least squares residual statistic, is the detection threshold value, is the standard deviation of the pseudorange residual;

[0046] The step of determining the weight value according to the judgment result of the quality of the GNSS signal includes:

[0047] If the quality of the GNSS signal is not good, then ;

[0048] If the quality of the GNSS signal is good, then ;

[0049] If the quality of the GNSS signal is medium, then

[0050] ;

[0051] is the weight value.

[0052] Preferably, step A4 includes:

[0053] Calculate the pose data measurement value of the object to be located according to the following formula:

[0054] ;

[0055] Wherein, is the pose data measurement value of the object to be located, is the first pose data, is the second pose data.

[0056] In a second aspect, the present application provides a fusion positioning device, which is applied to a positioning system having a vision camera, an IMU module, and a GNSS module, and is used to position an object to be located, including:

[0057] A first acquisition module, configured to jointly acquire the first pose data of the object to be located by the vision camera and the IMU module;

[0058] A second acquisition module, configured to jointly acquire the second pose data of the object to be located by the IMU module and the GNSS module;

[0059] A first calculation module, configured to determine a weight value according to the quality of the GNSS signal;

[0060] A second calculation module, configured to perform weighted fusion calculation on the first pose data and the second pose data according to the weight value to obtain a pose data measurement value of the object to be located.

[0061] This fusion positioning device first combines the visual camera and the IMU module to obtain the first pose data respectively, combines the IMU module and the GNSS module to obtain the second pose data, then determines the weight value when fusing the first pose data and the second pose data according to the quality of the GNSS signal, and finally performs weighted fusion calculation on the first pose data and the second pose data to obtain the final pose data. When the GNSS signal is blocked for a long time, it can still obtain a relatively accurate positioning result according to the visual camera and the IMU module. Compared with the existing GNSS / IMU fusion positioning technology, it has higher positioning accuracy in a complex environment with long-term occlusion of the GNSS signal.

[0062] In a third aspect, the present application provides an electronic device, including a processor and a memory. The memory stores computer-readable instructions. When the computer-readable instructions are executed by the processor, the steps in the fusion positioning method described above are run.

[0063] In a fourth aspect, the present application provides a storage medium, on which a computer program is stored. When the computer program is executed by a processor, the steps in the fusion positioning method described above are run.

[0064] Beneficial effects

[0065] The fusion positioning method, device, electronic device and storage medium provided by the present application obtain the first pose data of the object to be located by combining the visual camera and the IMU module; obtain the second pose data of the object to be located by combining the IMU module and the GNSS module; determine the weight value according to the quality of the GNSS signal; perform weighted fusion calculation on the first pose data and the second pose data according to the weight value to obtain a pose data measurement value of the object to be located; thereby realizing the organic fusion of visual positioning technology, GNSS positioning technology and IMU positioning technology, and improving the positioning accuracy in a complex environment with long-term occlusion of the GNSS signal.

[0066] Other features and advantages of the present application will be described in the subsequent specification, and part of them will become obvious from the specification, or be understood by implementing the embodiments of the present application. The objectives and other advantages of the present application can be realized and obtained by the structures specifically pointed out in the written specification and the drawings. Description of the drawings

[0067] Figure 1 It is a flowchart of a fusion positioning method provided by an embodiment of the present application.

[0068] Figure 2 This is a schematic structural diagram of the fusion positioning device provided by the embodiment of the present application.

[0069] Figure 3 This is a schematic structural diagram of the electronic device provided by the embodiment of the present application. Specific implementation manners

[0070] Next, the technical solutions in the embodiments of the present application will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. The components of the embodiments of the present application described and illustrated herein can be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of the present application provided in the drawings is not intended to limit the scope of the present application to be protected, but only represents the selected embodiments of the present application. All other embodiments obtained by those skilled in the art based on the embodiments of the present application without creative efforts fall within the scope of protection of the present application.

[0071] It should be noted that similar reference numerals and letters denote similar items in the following drawings. Therefore, once an item is defined in one drawing, it does not need to be further defined and explained in subsequent drawings. At the same time, in the description of the present application, the terms "first", "second", etc. are only used for differential description and cannot be understood as indicating or implying relative importance.

[0072] Please refer to Figure 1 , a fusion positioning method in some embodiments of the present application, which is applied to a positioning system with a vision camera, an IMU module, and a GNSS module to position a positioning object, includes the following steps:

[0073] A1. Jointly obtain the first pose data of the positioning object by the vision camera and the IMU module;

[0074] A2. Jointly obtain the second pose data of the positioning object by the IMU module and the GNSS module;

[0075] A3. Determine the weight value according to the quality of the GNSS signal;

[0076] A4. Perform weighted fusion calculation on the first pose data and the second pose data according to the weight value to obtain the pose data measurement value of the positioning object.

[0077] Wherein, the positioning object is a device equipped with this positioning system and movable, such as a mobile robot, a vehicle, a drone, a ship, etc.

[0078] This fusion positioning method first combines a vision camera and an IMU module respectively to obtain the first pose data, combines the IMU module and the GNSS module to obtain the second pose data, then determines the weight value when fusing the first pose data and the second pose data according to the quality of the GNSS signal, and finally performs weighted fusion calculation on the first pose data and the second pose data to obtain the final pose data. When the GNSS signal is blocked for a long time, it can still obtain a relatively accurate positioning result based on the vision camera and the IMU module. Compared with the GNSS / IMU fusion positioning technology in the prior art, in a complex environment with long-term occlusion of the GNSS signal, it has higher positioning accuracy. In addition, compared with the traditional visual SLAM positioning technology, the steps of SLAM mapping can be omitted, and the computational complexity is smaller. It can be seen that this fusion positioning method not only has high environmental adaptability, good positioning accuracy, but also relatively less computational complexity.

[0079] In this embodiment, step A1 includes:

[0080] Obtain multiple frames of images collected by the vision camera;

[0081] Extract the pixel coordinates of the matching feature points of two adjacent frames of images according to the images;

[0082] Perform epipolar geometry solution according to the pixel coordinates of the matching feature points, and calculate the rough pose data of the object to be positioned;

[0083] Obtain the measurement data of the IMU module and perform pre-integration processing to obtain the corresponding pre-integration data; the measurement data includes the accelerometer bias and the gyroscope bias;

[0084] Perform depth estimation on the matching feature points by the triangulation method according to the pixel coordinates of the matching feature points and the pre-integration data, and obtain the depth data of the matching feature points;

[0085] Generate state variables according to the rough pose data, depth data, measurement data of the IMU module, pre-integration data, external parameters of the IMU module and external parameters of the vision camera, and perform non-linear optimization processing on the state variables to obtain the optimal estimated value of the state variables;

[0086] Extract the first pose data from the optimal estimated value of the state variables.

[0087] By using the pre-integration data to perform depth estimation on the matching feature points, the scale uncertainty and passivation problems existing in the initialization of visual positioning can be effectively avoided. By performing non-linear optimization processing on the state variables and then obtaining the first pose data, the accuracy of the first pose data can be improved.

[0088] Among them, the feature points are the points where the image pixel values change drastically, and the matching feature points are the mutually matching feature points in two images. Existing image recognition methods can be used to extract the position data of the matching feature points from the images, and the specific methods are not limited here.

[0089] Among them, the process of solving the epipolar geometry is a prior art and will not be elaborated here. During the solving process, the depth is normalized. Therefore, there is a problem of scale uncertainty in the rough pose data and it cannot be directly used as the first pose data.

[0090] Among them, the step of estimating the depth of the feature points based on the pixel coordinates of the matching feature points and the pre-integrated data to obtain the depth data of the matching feature points specifically includes: First, pre-integrating the IMU data (i.e., the measurement data of the IMU module) can calculate the relative distance and relative pose between two frames of images, and then the distance between the matching feature point and the imaging plane can be obtained through triangulation.

[0091] In this embodiment, the state variables are:

[0092] ;

[0093] Among them, is the state variable, are the state estimation data corresponding to the 1st to the nth frames of images respectively (before generating the state variables, it is necessary to first perform time synchronization processing on the images collected by the vision camera and the measurement data collected by the IMU module. The specific time synchronization processing process is a prior art and will not be elaborated here; among them, the 1st to the nth frames of images here are the images within the sliding window. The larger the sliding window, the larger n is, indicating more constraint conditions and more accurate calculation results, but the larger the calculation amount. The size of n can be set according to actual needs. Generally, the window size range is 10 - 20, that is, n is generally 10 - 20). For the kth frame of image, there is:

[0094] ; is the state estimation data corresponding to the kth frame of image, is the position in the rough pose data corresponding to the kth frame of image (generally, the rough pose data corresponding to the kth frame of image is the rough pose data calculated through the (k - 1)th frame of image and the kth frame of image. For the 1st frame of image, the corresponding rough pose data can be calculated through the last frame of image in the previous window and this 1st frame of image), is the attitude in the rough pose data corresponding to the kth frame of image, is the velocity corresponding to the kth frame of image (obtained by pre-integrating the accelerometer bias, that is, the pre-integrated data includes velocity), is the accelerometer bias corresponding to the kth frame of image, is the gyroscope bias corresponding to the k-th frame image;

[0095] Among them, ; is the external parameter matrix, is the external parameter of the vision camera, is the external parameter of the IMU module; are the depth data corresponding to the 1st to the n-th frame images;

[0096] The steps of performing non-linear optimization processing on the state variables to obtain the optimal estimated values of the state variables include:

[0097] Perform maximum a posteriori estimation according to the following model (an existing maximum a posteriori estimation method can be used for calculation, which is not limited here) to obtain the optimal estimated values of the state variables:

[0098] ;

[0099] Among them, is the marginalized residual, is the Mahalanobis distance related to the covariance, is the Jacobian matrix;

[0100] Among them, is the IMU residual, is the IMU residual function, is the IMU measurement value, is the covariance matrix of the IMU pre-integration noise term;

[0101] Among them, is the visual reprojection error, is the visual reprojection error function, is the visual feature point, is the covariance matrix of the visual observation noise..

[0102] The above model is a tightly coupled model that comprehensively utilizes visual positioning data and IUM positioning data. Its advantage is that the estimation result of the pose state variable is relatively accurate, so that relatively accurate first pose data can be obtained.

[0103] Generally, represents the state estimation data (including rough pose data and IMU bias) corresponding to the image collected at the current moment. The pose of the object to be located included in the optimized in is the first pose data; thus, the steps of extracting the first pose data from the optimal estimated values of the state variables include: extracting the pose of the object to be located in the optimized from the optimal estimated values of the state variables to obtain the first pose data.

[0104] Among them, positioning by combining the IMU module and the GNSS module is a mature existing technology. In step A2, existing GNSS / IMU fusion algorithms can be used to obtain the second pose data of the object to be positioned. For example, in some embodiments, step A2 includes:

[0105] Obtaining the second pose data of the object to be positioned by using a GNSS / IMU fusion algorithm based on Kalman filtering.

[0106] Among them, the GNSS / IMU fusion algorithm based on Kalman filtering is an existing technology and will not be elaborated here. In practical applications, it is not limited to using the GNSS / IMU fusion algorithm based on Kalman filtering to obtain the second pose data.

[0107] In some embodiments, step A3 includes:

[0108] Obtaining the third pose data of the object to be positioned measured by each satellite of the satellite positioning system;

[0109] Calculating the sum of squares of pseudorange residuals according to the third pose data;

[0110] Calculating the least squares residual statistic and the detection threshold according to the sum of squares of pseudorange residuals;

[0111] Comparing the least squares residual statistic and the detection threshold to judge the quality of the GNSS signal;

[0112] Determining the weight value according to the judgment result of the quality of the GNSS signal.

[0113] Generally, the satellite positioning system includes multiple satellites, and each satellite can measure the third pose data of the object to be positioned respectively. Sometimes, some satellites may have faults or the signals of some satellites cannot be received by the object to be positioned. Here, obtaining the third pose data of the object to be positioned measured by each satellite of the satellite positioning system means obtaining the third pose data measured by the satellites whose signals can be received.

[0114] Actually, there is the following linear relationship between the GNSS pseudorange difference (that is, the difference between the actual pseudorange and the measured pseudorange value measured by the GNSS module) and the unit pose error of the object to be positioned:

[0115] (1);

[0116] ;

[0117] Among them, is the GNSS pseudorange difference, is the unit pose error of the object to be positioned, is the observation matrix, are the position data of the 1st to the mth satellites themselves, , , are the receiver positions, is the pseudorange, is the pseudorange noise that follows a normal distribution with zero mean.

[0118] According to the least squares principle, the optimal estimation solution is:

[0119] (2);

[0120] is the optimal estimation solution of, is the transpose matrix of; Combining formulas (1) and (2) gives:

[0121] ;

[0122] Then the predicted GNSS pseudorange difference is:

[0123] ;

[0124] is the predicted GNSS pseudorange difference;

[0125] Therefore, the pseudorange residual can be obtained as:

[0126]

[0127] is the pseudorange residual, is the identity matrix.

[0128] The sum of squares of the pseudorange residuals can be calculated according to the following formula:

[0129] ;

[0130] where, is the sum of squares of the pseudorange residuals, is the transpose matrix of.

[0131] where, follows a normal distribution with a mean of 0 and a variance of , so that follows a chi-square distribution with a degree of freedom of , that is .

[0132] At the system preset false alarm rate In the case of (which can be set according to actual needs), according to the chi-square distribution probability density function we can get:

[0133] (3);

[0134] Among them, is the correct alarm rate, is the standard deviation of the pseudorange residual, is related to the corresponding observation matrix, is the equivalent detection threshold value. Solving the above formula (3) can obtain the value of .

[0135] Among them, the least squares residual statistic can be calculated according to the following formula:

[0136] ; is the least squares residual statistic.

[0137] Among them, the detection threshold value can be calculated according to the following formula:

[0138] ; is the detection threshold value.

[0139] Preferably, the steps of comparing the least squares residual statistic and the detection threshold value to judge the quality of GNSS signals include:

[0140] If , it is determined that the quality of the GNSS signal is poor;

[0141] If , it is determined that the quality of the GNSS signal is good;

[0142] If , it is determined that the quality of the GNSS signal is medium;

[0143] According to the judgment result of the quality of the GNSS signal, the steps of determining the weight value include:

[0144] If the quality of the GNSS signal is poor, then ;

[0145] If the quality of the GNSS signal is good, then ;

[0146] If the quality of the GNSS signal is medium, then

[0147] ;

[0148] is the weight value.

[0149] Further, step A4 includes:

[0150] Calculating a measured value of the pose data of the object to be located according to the following formula:

[0151] ;

[0152] wherein, is the measured value of the pose data of the object to be located, is the first pose data, is the second pose data.

[0153] That is, when the quality of the GNSS signal is poor, the first pose data is used as the final pose positioning result; when the quality of the GNSS signal is good, the second pose data is used as the final pose positioning result; when the quality of the GNSS signal is medium (it is difficult to judge the quality of the GNSS signal at this time), the weighted sum of the first pose data and the second pose data is used as the final pose positioning result. By judging the quality of the GNSS signal in the above manner, the reliability is good.

[0154] As can be seen from the above, in this fusion positioning method, the first pose data of the object to be located is obtained by combining the vision camera and the IMU module; the second pose data of the object to be located is obtained by combining the IMU module and the GNSS module; the weight value is determined according to the quality of the GNSS signal; the first pose data and the second pose data are weighted and fused according to the weight value to obtain the measured value of the pose data of the object to be located; thus, the organic fusion of the vision positioning technology, the GNSS positioning technology and the IMU positioning technology is realized, and the positioning accuracy in a complex environment with long-term occlusion of the GNSS signal can be improved.

[0155] Referring to Figure 2 , the present application provides a fusion positioning device, which is applied to a positioning system with a vision camera, an IMU module and a GNSS module to locate an object to be located, and includes:

[0156] The first acquisition module 1 is used to jointly acquire the first pose data of the object to be located by the vision camera and the IMU module;

[0157] The second acquisition module 2 is used to jointly acquire the second pose data of the object to be located by the IMU module and the GNSS module;

[0158] The first calculation module 3 is used to determine the weight value according to the quality of the GNSS signal;

[0159] The second calculation module 4 is used to perform weighted fusion calculation on the first pose data and the second pose data according to the weight value to obtain the measured value of the pose data of the object to be located.

[0160] Among them, the object to be located is a movable device equipped with this positioning system, such as a mobile robot, a vehicle, a drone, a ship, etc.

[0161] This fusion positioning device first combines the visual camera and the IMU module to obtain the first pose data respectively, combines the IMU module and the GNSS module to obtain the second pose data, then determines the weight value when fusing the first pose data and the second pose data according to the quality of the GNSS signal, and finally performs weighted fusion calculation on the first pose data and the second pose data to obtain the final pose data. When the GNSS signal is blocked for a long time, it can still obtain a relatively accurate positioning result based on the visual camera and the IMU module. Compared with the existing GNSS / IMU fusion positioning technology, it has higher positioning accuracy in a complex environment with long-term occlusion of the GNSS signal.

[0162] In this embodiment, the first acquisition module 1 is used to execute when jointly acquiring the first pose data of the object to be located by the visual camera and the IMU module:

[0163] Acquire multiple frames of images collected by the visual camera;

[0164] Extract the pixel coordinates of the matching feature points of two adjacent frames of images according to the images;

[0165] Perform epipolar geometry solution according to the pixel coordinates of the matching feature points, and calculate the rough pose data of the object to be located;

[0166] Acquire the measurement data of the IMU module and perform pre-integration processing to obtain the corresponding pre-integration data; the measurement data includes the accelerometer bias and the gyroscope bias;

[0167] Perform depth estimation on the matching feature points by the triangulation method according to the pixel coordinates of the matching feature points and the pre-integration data to obtain the depth data of the matching feature points;

[0168] Generate state variables according to the rough pose data, depth data, measurement data of the IMU module, pre-integration data, external parameters of the IMU module and external parameters of the visual camera, and perform non-linear optimization processing on the state variables to obtain the optimal estimated value of the state variables;

[0169] Extract the first pose data from the optimal estimated value of the state variables.

[0170] By using the pre-integration data to perform depth estimation on the matching feature points, the scale uncertainty and passivation problems existing in the initialization of visual positioning can be effectively avoided. By performing non-linear optimization processing on the state variables and then obtaining the first pose data, the accuracy of the first pose data can be improved.

[0171] Among them, the feature points are the points where the image pixel values change drastically, and the matching feature points are the mutually matching feature points in two images. Existing image recognition methods can be used to extract the position data of the matching feature points from the images, and the specific methods are not limited herein.

[0172] Among them, the process of solving the epipolar geometry is a prior art and will not be described in detail herein. During the solving process, the depth is normalized. Therefore, there is a problem of scale uncertainty in the rough pose data and it cannot be directly used as the first pose data.

[0173] Among them, when the first acquisition module 1 estimates the depth of the feature points based on the position data of the feature points and the pre-integrated data to obtain the depth data of the feature points, it specifically performs: First, pre-integrate the IMU data (i.e., the measurement data of the IMU module) to calculate the relative distance and relative pose between two frames of images, and then the distance between the matching feature points and the imaging plane can be obtained through triangulation.

[0174] In this embodiment, the state estimation quantities include the pose and velocity of the object to be located, the accelerometer bias, and the gyroscope bias;

[0175] In this embodiment, the state variables are:

[0176] ;

[0177] Among them, is the state variable, are the state estimation data corresponding to the 1st to the nth frames of images respectively (before generating the state variables, it is necessary to first perform time synchronization processing on the images collected by the vision camera and the measurement data collected by the IMU module. The specific time synchronization processing process is a prior art and will not be described in detail herein; among them, the 1st to the nth frames of images here are the images within the sliding window. The larger the sliding window, the larger n is, indicating more constraint conditions and more accurate calculation results, but the larger the calculation amount. The size of n can be set according to actual needs. Generally, the window size range is 10 - 20, that is, n is generally 10 - 20). For the kth frame of image, there is:

[0178] ; is the state estimation data corresponding to the kth frame of image, is the position in the rough pose data corresponding to the kth frame of image (generally, the rough pose data corresponding to the kth frame of image is the rough pose data calculated from the (k - 1)th frame of image and the kth frame of image. For the 1st frame of image, the corresponding rough pose data can be calculated from the last frame of image in the previous window and this 1st frame of image), is the pose in the rough pose data corresponding to the kth frame of image, is the velocity corresponding to the k-th frame image (obtained by pre-integrating the accelerometer bias, i.e., the pre-integrated data includes velocity), is the accelerometer bias corresponding to the k-th frame image, is the gyroscope bias corresponding to the k-th frame image;

[0179] Among them, ; is the extrinsic parameter matrix, is the extrinsic parameter of the vision camera, is the extrinsic parameter of the IMU module; is the depth data corresponding to the 1st to the n-th frame images;

[0180] When the first acquisition module 1 performs non-linear optimization processing on the state variables to obtain the optimal estimated values of the state variables, it executes:

[0181] Perform maximum a posteriori estimation according to the following model (an existing maximum a posteriori estimation method can be used for calculation, which is not limited here) to obtain the optimal estimated values of the state variables:

[0182] ;

[0183] Among them, is the marginalized residual, is the Mahalanobis distance related to the covariance, is the Jacobian matrix;

[0184] Among them, is the IMU residual, is the IMU residual function, is the IMU measurement value, is the covariance matrix of the IMU pre-integration noise term;

[0185] Among them, is the visual reprojection error, is the visual reprojection error function, is the visual feature point, is the covariance matrix of the visual observation noise..

[0186] The above model is a tightly coupled model that comprehensively utilizes visual positioning data and IUM positioning data. Its advantage is that the estimation result of the pose state variable is relatively accurate, so that relatively accurate first pose data can be obtained.

[0187] Generally, represents the state estimation data corresponding to the image collected at the current moment (including rough pose data and IMU bias), and the optimized in The pose of the located object included is the first pose data; thus, when the first acquisition module 1 extracts the first pose data from the optimal estimated value of the state variable, it performs: extracting the optimized pose of the located object in from the optimal estimated value of the state variable to obtain the first pose data. of the located object to obtain the first pose data.

[0188] Among them, positioning by combining the IMU module and the GNSS module is a mature existing technology, and the second acquisition module 2 can adopt an existing GNSS / IMU fusion algorithm to obtain the second pose data of the located object. For example, in some embodiments, when the second acquisition module 2 is used to obtain the second pose data of the located object by combining the IMU module and the GNSS module, it performs:

[0189] Adopting a GNSS / IMU fusion algorithm based on Kalman filtering to obtain the second pose data of the located object.

[0190] Among them, the GNSS / IMU fusion algorithm based on Kalman filtering is an existing technology and will not be elaborated here. In practical applications, it is not limited to using the GNSS / IMU fusion algorithm based on Kalman filtering to obtain the second pose data.

[0191] In some embodiments, when the first calculation module 3 is used to determine the weight value according to the quality of the GNSS signal, it performs:

[0192] Obtaining the third pose data of the located object measured by each satellite of the satellite positioning system;

[0193] Calculating the sum of squares of pseudorange residuals according to the third pose data;

[0194] Calculating the least squares residual statistic and the detection threshold according to the sum of squares of pseudorange residuals;

[0195] Comparing the least squares residual statistic and the detection threshold to judge the quality of the GNSS signal;

[0196] Determining the weight value according to the judgment result of the quality of the GNSS signal.

[0197] Generally, the satellite positioning system includes multiple satellites, and each satellite can measure the third pose data of the located object respectively. Sometimes, some satellites may have faults or the signals of some satellites cannot be received by the located object. Here, obtaining the third pose data of the located object measured by each satellite of the satellite positioning system means obtaining the third pose data measured by the satellites whose signals can be received.

[0198] Actually, there is the following linear relationship between the GNSS pseudorange difference (that is, the difference between the actual pseudorange and the measured pseudorange value measured by the GNSS module) and the unit pose error of the located object:

[0199] (1);

[0200] ;

[0201] Wherein, is the GNSS pseudorange difference, is the unit pose error of the object to be located, is the observation matrix, are the own position data of the 1st to the mth satellites respectively, , , are the receiver positions respectively, is the pseudorange, is the pseudorange noise that follows a zero-mean normal distribution.

[0202] According to the least squares principle, the optimal estimation solution is:

[0203] (2);

[0204] is the optimal estimation solution of, is the transpose matrix of; Combining formulas (1) and (2) gives:

[0205] ;

[0206] Then the predicted GNSS pseudorange difference is:

[0207] ;

[0208] is the predicted GNSS pseudorange difference;

[0209] Therefore, the pseudorange residual can be obtained as:

[0210]

[0211] is the pseudorange residual.

[0212] The sum of squares of the pseudorange residuals can be calculated according to the following formula:

[0213] ;

[0214] Wherein, is the sum of squares of the pseudorange residuals, is the transpose matrix of.

[0215] Wherein, obeys a normal distribution with a mean of 0 and a variance of , so that obeys a chi-square distribution with a degree of freedom of , that is .

[0216] Under the condition that the system presets the false alarm rate (which can be set according to actual needs), according to the probability density function of the chi-square distribution it can be obtained that:

[0217] (3);

[0218] Among them, is the correct alarm probability, is the standard deviation of the pseudorange residual, is the observation matrix corresponding to , is the equivalent detection threshold value. Solving the above formula (3) can obtain the value of .

[0219] Among them, the least squares residual statistic can be calculated according to the following formula:

[0220] ; is the least squares residual statistic.

[0221] Among them, the detection threshold value can be calculated according to the following formula:

[0222] ; is the detection threshold value.

[0223] Preferably, when the first calculation module 3 compares the least squares residual statistic and the detection threshold value to judge the quality of the GNSS signal, it executes:

[0224] If , it is determined that the quality of the GNSS signal is not good;

[0225] If , it is determined that the quality of the GNSS signal is good;

[0226] If , it is determined that the quality of the GNSS signal is medium;

[0227] When the first calculation module 3 determines the weight value according to the judgment result of the quality of the GNSS signal, it executes:

[0228] If the quality of the GNSS signal is not good, then ;

[0229] If the quality of the GNSS signal is good, then ;

[0230] If the quality of the GNSS signal is medium, then

[0231] ;

[0232] are weight values.

[0233] Furthermore, the second calculation module 4 is used to perform the following when calculating the pose data measurement value of the object to be located by weighted fusion of the first pose data and the second pose data according to the weight value:

[0234] Calculate the pose data measurement value of the object to be located according to the following formula:

[0235] ;

[0236] where, is the pose data measurement value of the object to be located, is the first pose data, is the second pose data.

[0237] That is, when the quality of the GNSS signal is poor, the first pose data is used as the final pose positioning result. When the quality of the GNSS signal is good, the second pose data is used as the final pose positioning result. When the quality of the GNSS signal is medium (it is difficult to judge the quality of the GNSS signal at this time), the weighted sum of the first pose data and the second pose data is used as the final pose positioning result. By judging the quality of the GNSS signal in the above way, the reliability is good.

[0238] As can be seen from the above, the fusion positioning device obtains the first pose data of the object to be located by combining the vision camera and the IMU module; obtains the second pose data of the object to be located by combining the IMU module and the GNSS module; determines the weight value according to the quality of the GNSS signal; performs weighted fusion calculation on the first pose data and the second pose data according to the weight value to obtain the pose data measurement value of the object to be located; thus realizing the organic fusion of vision positioning technology, GNSS positioning technology and IMU positioning technology, and can improve the positioning accuracy in complex environments with long-term occlusion of GNSS signals.

[0239] Please refer to Figure 3 , Figure 3A schematic structural diagram of an electronic device provided by an embodiment of the present application. The present application provides an electronic device, including: a processor 301 and a memory 302. The processor 301 and the memory 302 are interconnected and communicate with each other through a communication bus 303 and / or other forms of connection mechanisms (not marked). The memory 302 stores a computer program executable by the processor 301. When the computing device runs, the processor 301 executes the computer program to execute the fusion positioning method in any optional implementation manner of the above embodiment to achieve the following functions: jointly obtaining first pose data of a target object by a vision camera and an IMU module; jointly obtaining second pose data of the target object by the IMU module and a GNSS module; determining a weight value according to the quality of the GNSS signal; and performing weighted fusion calculation on the first pose data and the second pose data according to the weight value to obtain a pose data measurement value of the target object.

[0240] An embodiment of the present application provides a storage medium. When a computer program is executed by a processor, it executes the fusion positioning method in any optional implementation manner of the above embodiment to achieve the following functions: jointly obtaining first pose data of a target object by a vision camera and an IMU module; jointly obtaining second pose data of the target object by the IMU module and a GNSS module; determining a weight value according to the quality of the GNSS signal; and performing weighted fusion calculation on the first pose data and the second pose data according to the weight value to obtain a pose data measurement value of the target object. Among them, the storage medium can be implemented by any type of volatile or non-volatile storage device or a combination thereof, such as static random access memory (SRAM for short), electrically erasable programmable read-only memory (EEPROM for short), erasable programmable read-only memory (EPROM for short), programmable read-only memory (PROM for short), read-only memory (ROM for short), magnetic memory, flash memory, a magnetic disk, or an optical disc.

[0241] In the embodiments provided in the present application, it should be understood that the disclosed devices and methods can be implemented in other ways. The device embodiments described above are merely illustrative. For example, the division of the units is only a logical function division. In actual implementation, there may be other division methods. For another example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the displayed or discussed coupling or direct coupling or communication connection between each other can be through some communication interfaces. The indirect coupling or communication connection of the devices or units can be in an electrical, mechanical or other form.

[0242] In addition, the units described as separate components may or may not be physically separated. The components displayed as units may or may not be physical units, that is, they can be located in one place, or they can be distributed to multiple network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0243] Furthermore, in each embodiment of the present application, the various functional modules can be integrated together to form an independent part, or each module can exist alone, or two or more modules can be integrated to form an independent part.

[0244] In this document, relational terms such as first and second are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any actual relationship or order between these entities or operations.

[0245] The above are only the embodiments of the present application and are not used to limit the protection scope of the present application. For those skilled in the art, the present application can have various changes and modifications. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims

1. A fusion positioning method, which is applied to a positioning system with a vision camera, an IMU module and a GNSS module to position a target object to be positioned, characterized in that Including the following steps: A1. Combine the visual camera and the IMU module to obtain the first pose data of the object to be located; A2. Combine the IMU module and the GNSS module to obtain the second pose data of the object to be located; A3. Determine the weight value according to the quality of the GNSS signal; A4. Perform weighted fusion calculation on the first pose data and the second pose data according to the weight value to obtain the measured value of the pose data of the object to be located; Step A3 includes: Obtain the third pose data of the object to be located measured by each satellite of the satellite positioning system; Calculate the sum of squares of pseudorange residuals according to the third pose data; Calculate the least squares residual statistic and the detection threshold according to the sum of squares of pseudorange residuals; Compare the least squares residual statistic and the detection threshold to judge the quality of the GNSS signal; Determine the weight value according to the judgment result of the quality of the GNSS signal; The step of comparing the least squares residual statistic and the detection threshold to judge the quality of the GNSS signal includes: If , it is determined that the quality of the GNSS signal is poor; If , it is determined that the quality of the GNSS signal is good; If , it is determined that the quality of the GNSS signal is medium; Among them, is the least squares residual statistic,[ is the detection threshold,[ is the standard deviation of the pseudorange residual; The step of determining the weight value according to the judgment result of the quality of the GNSS signal includes: If the quality of the GNSS signal is not good, then ; If the quality of the GNSS signal is good, then ; If the quality of the GNSS signal is medium, then ； is the weight value; Step A4 includes: Calculate the measured value of the pose data of the object to be located according to the following formula: ； Among them, is the measured value of the pose data of the object to be located, is the first pose data, is the second pose data.

2. The fusion positioning method according to claim 1, characterized in that, Step A1 includes: Obtain multiple frames of images collected by the visual camera; Extract the pixel coordinates of the matching feature points of two adjacent frames of images according to the images; Perform epipolar geometry solution according to the pixel coordinates of the matching feature points to calculate the rough pose data of the object to be located; Obtain the measurement data of the IMU module and perform pre-integration processing to obtain the corresponding pre-integration data; the measurement data includes the accelerometer bias and the gyroscope bias; Perform depth estimation on the matching feature points by triangulation according to the pixel coordinates of the matching feature points and the pre-integration data to obtain the depth data of the matching feature points; Generate a state variable according to the rough pose data, the depth data, the measurement data of the IMU module, the pre-integration data, the external parameters of the IMU module, and the external parameters of the visual camera, and perform non-linear optimization processing on the state variable to obtain the optimal estimated value of the state variable; Extract the first pose data from the optimal estimated value of the state variable.

3. The fusion positioning method according to claim 2, wherein The state variable is: ； Among them, is the state variable, are the state estimation data corresponding to the 1st to the nth frame images respectively. For the kth frame image, there is: ; is the state estimation data corresponding to the kth frame image, is the position in the rough pose data corresponding to the kth frame image, is the attitude in the rough pose data corresponding to the kth frame image, is the velocity corresponding to the kth frame image, is the accelerometer bias corresponding to the kth frame image, is the gyroscope bias corresponding to the kth frame image; Among them, ; is the external parameter matrix, is the external parameter of the visual camera, is the external parameter of the IMU module; are the depth data corresponding to the first to the nth frames of images; The step of performing non-linear optimization processing on the state variable to obtain the optimal estimated value of the state variable includes: Perform maximum a posteriori estimation according to the following model to obtain the optimal estimated value of the state variable: ； Among them, is the marginalized residual, is the Mahalanobis distance related to the covariance, is the Jacobian matrix; Among them, is the IMU residual, is the IMU residual function, is the IMU measurement value, is the covariance matrix of the IMU pre-integration noise term; Among them, is the visual reprojection error, is the visual reprojection error function, is the visual feature point, is the covariance matrix of the visual observation noise.

4. The fusion positioning method according to claim 1, wherein Step A2 includes: Adopt a GNSS / IMU fusion algorithm based on Kalman filter to obtain the second pose data of the object to be located.

5. A fusion positioning device is applied to a positioning system with a vision camera, an IMU module, and a GNSS module to position a target object to be positioned, characterized in that Including: The first acquisition module is used to combine the visual camera and the IMU module to obtain the first pose data of the object to be located; The second acquisition module is used to combine the IMU module and the GNSS module to obtain the second pose data of the object to be located; The first calculation module is used to determine the weight value according to the quality of the GNSS signal; A second calculation module, configured to perform weighted fusion calculation on the first pose data and the second pose data according to the weight value to obtain a pose data measurement value of the object to be located; When determining the weight value according to the quality of the GNSS signal, the first calculation module performs: Obtain third pose data of the object to be located measured by each satellite of the satellite positioning system; Calculate the sum of squares of pseudorange residuals according to the third pose data; Calculate the least squares residual statistic and the detection threshold according to the sum of squares of pseudorange residuals; Compare the least squares residual statistic and the detection threshold to determine the quality of the GNSS signal; Determine the weight value according to the determination result of the quality of the GNSS signal; The step of comparing the least squares residual statistic and the detection threshold to determine the quality of the GNSS signal includes: If , it is determined that the quality of the GNSS signal is poor; If , it is determined that the quality of the GNSS signal is good; If , it is determined that the quality of the GNSS signal is medium; wherein, is the least squares residual statistic, is the detection threshold, is the standard deviation of the pseudorange residual; The step of determining the weight value according to the determination result of the quality of the GNSS signal includes: If the quality of the GNSS signal is poor, then ; If the quality of the GNSS signal is good, then ; If the quality of the GNSS signal is medium, then ； is the weight value; Step A4 includes: Calculate the pose data measurement value of the object to be located according to the following formula: ； Among them, is the measurement value of the pose data of the object to be located, is the first pose data, is the second pose data.

6. An electronic device, characterized in that, Comprising a processor and a memory, the memory stores computer-readable instructions, and when the computer-readable instructions are executed by the processor, the steps in the fusion positioning method according to any one of claims 1-4 are run.

7. A storage medium, on which a computer program is stored, characterized in that, When the computer program is executed by the processor, the steps in the fusion positioning method according to any one of claims 1-4 are run.

Images