CN112461228B - A secondary loop-closing detection and positioning method based on IMU and vision in a similar environment - Google Patents

Abstract

The invention discloses a secondary loop detection positioning method based on IMU and vision in a similar environment, which comprises the following steps: step one, calibrating and synchronizing parameters of a binocular camera and an IMU; step two, extracting image features and matching the image features; thirdly, pose estimation and movement track formation; step four, loop detection; step five, a secondary loop detection mechanism; and step six, repositioning. The invention can carry out rough comparison constraint on the pose of the current image frame through the IMU pose information, namely, the current position direction of the IMU is compared with the position direction of the primary closed-loop image, and the direction consistency constraint pre-judgment is carried out, so that the problem that the surrounding scenes are similar, the current position of the IMU cannot be the same position and the surrounding scenes are wrong according to the image similarity and the image similarity can be judged to be closed-loop is prevented; the orientation error caused by similar images in similar environments is prevented through the direction prejudgment of the IMU; and after repositioning, updating the high-precision pose for correcting the accumulated drift error of the IMU, and improving the robustness of the secondary loop detection positioning method.

Description

A secondary loop-closing detection and positioning method based on IMU and vision in a similar environment

technical field

The invention relates to a secondary loop detection and positioning method, in particular to a secondary loop detection and positioning method based on IMU and vision in a similar environment.

Background technique

UAVs are developing in the direction of intelligent UAVs and intelligent systems, and the ability of autonomous positioning and navigation determines the level of intelligence of UAVs. Unmanned aerial vehicle (UAV) puts forward an urgent demand for its autonomous positioning ability in the fields of military reconnaissance, power inspection, geological exploration, forest fire prevention, etc., especially with the emergence of satellite navigation signal interference and deception technology, UAV requires It has completely autonomous positioning and navigation capabilities to get rid of the dependence on satellite navigation signals. Vision and inertial (Inertial measurement unit, IMU for short) combined SLAM (Simultaneous Localization And Mapping, simultaneous positioning and mapping) provides a new technical support and effective way for the realization of autonomous positioning and navigation without GPS signals. Through the complementarity of two different sensors, the IMU can perform error correction and compensation on the visual positioning information, which improves the real-time positioning capability of the UAV. However, as time goes by, the IMU itself also has accumulated drift errors after long-term operation. If no effective measures are taken, the UAV will still fail to locate after a long time. At the same time, in the vision and IMU combined positioning method, the existing back-end loop detection positioning method optimizes the pose of the previous related image frames by judging whether the current position is a previously visited environment area, and there is a problem of misjudgment. This is because there are a large number of similar environments in the actual environment, such as groups of buildings with similar appearance, rolling sand dunes, and dense forest environments, which will cause loopback detection in a non-GPS environment due to a large number of similar images in similar environments. It is difficult to judge the closed loop, which leads to the fact that it is a loop, but the algorithm judges that it is not a loop, or it is not actually a loop. The algorithm judgment is a misjudgment of the loop, resulting in a wrong loop, and the positioning fails. Therefore, in a similar environment without GPS, the key problem that needs to be solved urgently is how to prevent error loopback due to a large number of similar environment pictures, resulting in positioning failure.

Contents of the invention

The technical problem to be solved by the present invention is to provide a secondary loop detection and positioning method based on IMU and vision in a similar environment, which can correct the visual cumulative error and IMU cumulative drift error in a similar environment without GPS, and correct loop positioning Purpose.

To achieve the above object, the present invention provides the following technical solutions:

A secondary loop detection and positioning method based on IMU and vision in a similar environment, comprising the following steps:

Step 1, binocular camera and IMU parameter calibration and synchronization:

Unify the time stamps of the binocular camera and the IMU, and calibrate the parameters of the binocular camera and the IMU, including the start-up delay time of the binocular camera, the IMU start-up delay time, the zero bias of the accelerometer and the zero bias of the gyroscope (zero bias, the input is zero , that is, when the IMU does not move, the output is not an initial value of zero), synchronize the time of the binocular camera and the IMU, and read the image obtained by the binocular camera and the IMU pose information;

Step 2, image feature extraction and image matching:

Detect the corner point in the image acquired by the binocular camera as a feature point, describe the image around the corner point, form a feature descriptor, use the Hamming distance to match the feature descriptor in two consecutive images, and filter the feature matching points Yes, in case of mis-match;

Step 3, pose estimation and trajectory formation:

By matching the 3D information of multiple sets of feature matching point pairs, the least square method is used to calculate the rotation vector matrix R and translation vector t that make the error square sum of the feature matching point pairs reach a minimum value, and the pose of the drone is obtained. and moving map point trajectories;

When the camera moves too fast and the image is blurred and the positioning information is lost, the current binocular camera pose information is obtained by integrating the IMU pose information under the unified time stamp, and the associated map points are updated;

Step 4, loopback detection:

According to the similarity of the two images, it is judged whether to return to the passing position to determine the loop detection relationship, and the similarity between image frames is measured by building a tree clustering database to determine whether the position should be consistent and the loop is closed; for image matching The extracted feature points and feature descriptions are clustered to form a tree branch structure to quickly screen image features, output candidate images, and reduce similarity judgment time; calculate non-connected but adjacent key points that have common feature classes with the current image The class vector similarity of the frame is used to determine whether to loop back;

Step 5, secondary loopback detection mechanism:

After the closed-loop confirmation in the above steps, the closed-loop is not performed first, but the current image frame pose is roughly compared and constrained by the IMU pose information. Judgment to reconfirm the correctness of the loopback and form a secondary detection mechanism;

Step six, relocation:

Through the error results confirmed by the loop closure, the global nonlinear optimization is used to evenly distribute the loop closure error to all key image frames, so as to optimize and update the pose and associated map points of all key image frames in the world coordinate system, and obtain weight Positioning; at the same time, differentially process the updated current map point to correct the accumulated drift error of the current IMU.

Preferably, the calculation formula of the least squares method in the step 3 is:

Where R is the rotation vector matrix, t is the translation vector matrix, p _i is the feature point in the current image, is the feature point corresponding to the match between the previous image and p _i .

Preferably, the calculation formula of the class vector similarity in the step 4 is:

Among them, v ₁ and v ₂ are feature class vectors of the two images.

Beneficial effects of the present invention:

1. The visual image acquisition frequency used in the present invention is low, and the IMU acquisition frequency is high. After synchronizing the time stamp, when the initial IMU has no high cumulative drift error, the IMU has more accurate positioning information than the visual image, and the IMU information can be used to correct the visual accumulation error;

2. Through the combined IMU, the present invention can obtain the current pose information by integrating the IMU pose information under the synchronous time stamp when the camera moves too fast and the image is blurred and the positioning information is lost, so as to update the associated map points and make up for the visual The problem of positioning failure caused by image failure;

3. The secondary loopback detection and positioning method provided by the present invention uses the IMU pose information to roughly compare and constrain the pose of the current image frame, that is, compares the current position direction of the IMU with the position direction of the primary closed-loop image, and performs direction consistency constraint pre-judgment , to prevent the surrounding scenes from being similar, such as the similarity between buildings. According to the similarity of the image, it can be judged as a closed loop, but according to the current actual position of the IMU, it is impossible to be the same position at all, so the problem of looping back is wrong, that is, through the direction prediction of the IMU to prevent the similarity Positioning errors caused by similar images in the environment;

4. The high-precision pose updated after the relocation of the present invention is used to correct the accumulated drift error of the IMU, and improves the robustness of the secondary loop detection and positioning method based on the IMU and vision.

Description of drawings

FIG. 1 is a flow chart of the secondary loop detection and positioning method based on IMU and vision in a similar environment of the present invention.

Detailed ways

The following will clearly and completely describe the technical solutions in the embodiments of the present invention with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only some, not all, embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by persons of ordinary skill in the art without making creative efforts belong to the protection scope of the present invention.

Please refer to Fig. 1, an embodiment provided by the present invention: a secondary loop detection and positioning method based on IMU and vision in a similar environment, including the following steps:

Step 1, binocular camera and IMU parameter calibration and synchronization:

Unify the time stamps of the binocular camera and the IMU, and calibrate the parameters of the binocular camera and the IMU, including the start-up delay time of the binocular camera, the IMU start-up delay time, the zero bias of the accelerometer and the zero bias of the gyroscope (zero bias, the input is zero , that is, when the IMU does not move, the initial value is not zero), synchronize the binocular camera and IMU time, and read the binocular camera image and IMU pose information;

Step 2, image feature extraction and image matching:

Detect the corner point in the image as a feature point, describe the image around the corner point, form a feature descriptor, use the Hamming distance to match the feature descriptors in two consecutive images, and filter the feature matching point pairs to prevent errors match;

Step 3, pose estimation and trajectory formation:

By matching the 3D information of multiple sets of feature matching point pairs, the least squares method is used to calculate the rotation vector matrix R and the translation vector t that make the error square sum of feature matching point pairs reach a minimum value, and obtain the pose of the binocular camera and moving map point trajectories;

When the camera moves too fast and the image is blurred and the positioning information is lost, the current double-sided camera pose information is obtained by integrating the IMU pose information under the unified time stamp, and the associated map points are updated;

Step 4, loopback detection:

According to the similarity of the two images, it is judged whether to return to the passing position to determine the loop detection relationship, and the similarity between image frames is measured by building a tree clustering database to determine whether the position should be consistent and the loop is closed; for image matching The extracted feature points and feature descriptions are clustered to form a tree branch structure to quickly filter image features, output candidate images, reduce similarity judgment time, and calculate non-connected but adjacent keys that have common feature classes with the current image. The class vector similarity of the frame is used to determine whether to loop back;

Step 5, secondary loopback detection mechanism:

After the closed-loop confirmation in the above steps, the closed-loop is not performed first, but the current image frame pose is roughly compared and constrained by the IMU pose information. Judgment to reconfirm the correctness of the loopback and form a secondary detection mechanism;

Step six, relocation:

Through the error results confirmed by the loop closure, the global nonlinear optimization is used to evenly distribute the loop closure error to all key image frames, so as to optimize and update the pose and associated map points of all key image frames in the world coordinate system, and obtain weight Positioning; at the same time, differentially process the updated current map point to correct the accumulated drift error of the current IMU.

In this embodiment, the calculation formula of the least squares method in the step 3 is:

Where R is the rotation vector matrix, t is the translation vector matrix, p _i is the feature point in the current image, is the feature point corresponding to the match between the previous image and p _i .

In this embodiment, the calculation formula of the class vector similarity in the step 4 is:

Among them, v ₁ and v ₂ are feature class vectors of the two images.

To sum up, the embodiment of the present invention provides a secondary loop detection and positioning method based on IMU and vision in a similar environment. The pose of the current image frame is roughly compared and constrained by the pose information of the IMU, that is, the current position and direction of the IMU Compared with the position and direction of a closed-loop image, the direction consistency constraint is pre-judged to prevent the surrounding scenes from being similar, such as buildings with similar similarities. According to the similarity of the image, it can be judged as a closed loop, but according to the current actual position of the IMU, it is impossible to be the same position at all. The problem of error loopback is to prevent positioning errors caused by similar images in similar environments through the direction prediction of the IMU; the high-precision pose updated after relocation is used to correct the accumulated drift error of the IMU, which improves the two-dimensional image based on IMU and vision. Robustness of secondary loop detection localization methods.

It will be apparent to those skilled in the art that the invention is not limited to the details of the above-described exemplary embodiments, but that the invention can be embodied in other specific forms without departing from the spirit or essential characteristics of the invention. Accordingly, the embodiments should be regarded in all points of view as exemplary and not restrictive, the scope of the invention being defined by the appended claims rather than the foregoing description, and it is therefore intended that the scope of the invention be defined by the appended claims rather than by the foregoing description. All changes within the meaning and range of equivalents of the elements are embraced in the present invention. Any reference sign in a claim should not be construed as limiting the claim concerned.

Claims (4)

1. The secondary loop detection positioning method based on IMU and vision in similar environment is characterized by comprising the following steps:

step one, calibrating and synchronizing parameters of a binocular camera and an IMU:

unifying time stamps of the binocular camera and the IMU, calibrating parameters of the binocular camera and the IMU, including binocular camera start delay time, IMU start delay time, accelerometer zero bias and gyroscope zero bias, synchronizing the binocular camera and the IMU time, and reading images acquired by the binocular camera and IMU pose information;

step two, image feature extraction and image matching:

detecting corner points in images acquired by a binocular camera as characteristic points, describing images around the corner points to form characteristic descriptors, matching the characteristic descriptors in two continuous images by using a Hamming distance, and screening characteristic matching point pairs to prevent mismatching;

thirdly, pose estimation and movement track formation:

calculating a rotation vector matrix R and a translation vector t which enable the square sum of errors of the feature matching point pairs to reach a minimum value by using a least square method through 3D information of the matched multiple groups of feature matching point pairs, and obtaining the pose of the binocular camera and the locus of the moving map point;

when the camera moves too fast to cause image blurring and positioning information is lost, acquiring pose information of the current binocular camera through integrating IMU pose information under a unified timestamp, and updating associated map points;

step four, loop detection:

judging whether to return to the passing position according to the similarity of the two images so as to determine a loop detection relation, and measuring the similarity between image frames in a mode of constructing a tree cluster database so as to judge whether the positions are consistent so as to loop; clustering the feature points and feature descriptions extracted during image matching to form a crotch structure, so as to rapidly screen image features, output candidate images and reduce similarity judging time; calculating the similarity of class vectors of non-connected but adjacent key frames with common feature classes of the current image, and judging whether to loop;

step five, a secondary loop detection mechanism:

after the closed loop is confirmed in the steps, the closed loop is not firstly carried out, but the pose of the current image frame is roughly compared and restrained through IMU pose information, namely, the current position direction of the IMU is compared with the position direction of the primary closed loop image, and the direction consistency constraint pre-judgment is carried out, so that the correctness of the loop is confirmed again, and a secondary detection mechanism is formed;

step six, repositioning:

through the error result after loop confirmation, global nonlinear optimization is adopted, loop errors are evenly distributed to all key image frames, so that pose of all key image frames under a world coordinate system and associated map points are optimized and updated, and repositioning is obtained; and meanwhile, differentiating the updated current map point to correct the accumulated drift error of the current IMU.

2. The method for detecting and positioning secondary loop based on IMU and vision according to claim 1, wherein the condition of selecting the feature matching point pair in the second step is that the hamming distance of the descriptor is less than twice the minimum distance.

3. The method for detecting and positioning the secondary loop based on the IMU and the vision in the similar environment according to claim 1, wherein the calculation formula of the least squares method in the third step is:

wherein R is a rotation vector matrix, t is a translation vector matrix, and p _i As a feature point in the current image,

for the last picture and p _i Matching the corresponding feature points.

4. The method according to claim 1, wherein the formula for calculating the similarity of the class vectors in the fourth step is:

wherein v is ₁ 、v ₂ Is a feature class vector of two images.

Images