Key frame real-time positioning method assisted by stereo camera

Abstract

According to the stereo camera-assisted key frame real-time positioning method provided by the invention, local optimization is expanded into global optimization of multiple groups of key frames by carrying out two-time optimization positioning on stereo camera-assisted key frame data, so that on one hand, a global sub-optimal positioning solution is obtained, and the real-time positioning precision is improved; and on the other hand, the method can adapt to different types of stereo camera auxiliary scenes, and has good robustness. After the technical scheme of the invention is adopted, when the cyclic closure exists in the key frame sequence, a real-time processing effect with higher positioning precision can be provided; besides, the attitude after optimization of the current update frame is adopted in the pose graph, the optimization cost function is J (xnew), and the positioning quality is further improved by combining with the cyclic closed constraint. Moreover, the number of continuous adjacent key frames meeting the relocation hypothesis is not strictly limited, the algorithm adaptability is good, and the overall design is very reasonable and ingenious.

Description

Technical Field

[0001] The present invention relates to the technical field of stereo cameras, and in particular to a key frame real-time positioning method assisted by a stereo camera, which can be used for real-time positioning and map construction in a ground navigation process. Background Art

[0002] In recent years, information fusion technology combining visual and inertial sensors has become increasingly popular in robotic applications and can be used to accurately achieve real-time positioning and map construction during ground navigation. However, most current systems focus more on the use of visual odometry data and ignore the fusion of positioning and mapping. Such systems only maintain consistency in local areas and locate the motion position of the camera within a limited time frame, which has great limitations. Robotic navigation usually requires the construction of a global map and the possibility of positioning the agent through an absolute reference position.

[0003] Most of the current localization methods contain two layers. The first layer uses keyframes to estimate the camera position in a local area, and the second layer uses global constraints to optimize the motion trajectory and improve the estimation results. Typical representative algorithms of this type of method are ORB-SLAM (Raul Mur-Artal, JMM Montiel and Juan D Tardos. Orb-slam: a versatile and accurate monocular slam system. IEEE Transactions on Robotics, 2015.) and LSD-SLAM (Jakob Engel, Thomas Sch¨ops, and Daniel Cremers. Lsd-slam: Large-scale direct monocular slam. In European Conference on Computer Vision. 2014.). ORB-SLAM is a feature-based monocular SLAM that can provide real-time performance. It uses bundle adjustment and survival of the fittest methods to locate and build maps. ORB-SLAM features include loop closure detection and relocation. LSD-SLAM successfully uses direct (or featureless) SLAM for monocular systems. However, the positioning accuracy of these two sets of algorithms is still relatively low, and the processing effect is relatively poor. Therefore, it is necessary to propose a new positioning method with higher accuracy to solve the above technical problems. Summary of the invention

[0004] The present invention provides a key frame real-time positioning method assisted by a stereo camera. In order to achieve high-precision positioning of a robot system in ground navigation, a key data frame of a loop closure is used to perform global trajectory optimization. By updating the stereo camera posture in a saved map reference system, the map image and the trajectory are combined in a repositioning manner, thereby achieving suboptimal global real-time positioning optimization.

[0005] In order to achieve the above object, the present invention adopts the following technical solution: A stereo camera-assisted key frame real-time positioning method of the present invention specifically comprises the following steps:

[0006] S1, obtain the frame data frame for positioning of the robot system. If the frame data is not the key frame data, a new node is established based on the input measurement value and connected to the last pose node of the pose graph. The updated pose graph is used for step S7;

[0007] S2. If the frame data is a key frame, add the frame to the DBoW loop closure visual word bag to generate a corresponding DBoW index table;

[0008] S3, searching for a data frame match in the key frame that matches the global old map, and determining an index number corresponding to the frame;

[0009] S4, if there is no global matching data frame match, go to step S1 for operation;

[0010] S5. If there is a global matching data frame match, calculate the rotation relationship matrix tf of all key frames relative to match, and keep the relocation hypothesis corresponding to the new and old images

[0011] S6. If tf is an empty array, go to step S1;

[0012] S7. If tf is a non-empty array, add the edge corresponding to tf to the pose graph updated in step S1, and minimize the cost function J(x new ) is used as the optimization criterion to optimize the pose graph and generate a new map; the minimization cost function J(x new )for:

[0013]

[0014] in, is the kth frame stereo camera pose data currently updated, T i j is the pose transformation matrix of the j-th frame data relative to the i-th frame data, S new is a similar dataset to the current update frame;

[0015] S8. If the adjacent key frames all meet the relocation hypothesis, that is, and Then discard H j ,make

[0016] S9, if L consecutive key frames all meet the relocation hypothesis, that is, Then the relocation transformation matrix between the new and old maps is

[0017] S10, minimizing J(x) is used to optimize and obtain the suboptimal key frame relocation x, that is, the suboptimal key frame relocation x is obtained by optimizing the following cost function:

[0018]

[0019] in, is the last updated p-th frame stereo camera pose data, and S is a similar dataset of new and old map frames.

[0020] Compared with the prior art, the present invention has the following beneficial effects:

[0021] The present invention optimizes the positioning of the key frame data assisted by the stereo camera twice, and expands the local optimization into the global optimization of multiple groups of key frames. On the one hand, the global suboptimal positioning solution is obtained, which improves the real-time positioning accuracy. On the other hand, it can adapt to different types of stereo camera assisted scenes and has good robustness. After adopting the technical solution of the present invention, when there is a loop closure in the key frame sequence, it can provide a real-time processing effect with higher positioning accuracy; in addition, it adopts the posture after the optimization of the current update frame in the pose graph, and the optimization cost function is J(x new ), by combining the loop closure constraint, the positioning quality is further improved; moreover, there is no strict restriction on the number of consecutive adjacent key frames that meet the repositioning hypothesis, the algorithm has good adaptability, and its overall design is very reasonable and ingenious. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] Figure 1 It is a flow chart of the key frame real-time positioning method assisted by a stereo camera of the present invention. DETAILED DESCRIPTION

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

[0024] See also Figure 1 The present invention provides a stereo camera-assisted key frame real-time positioning method, which specifically comprises the following steps:

[0025] S1, obtain the frame data frame for positioning of the robot system. If the frame data is not the key frame data, a new node is established based on the input measurement value and connected to the last pose node of the pose graph. The updated pose graph is used for step S7;

[0026] S2. If the frame data is a key frame, continue the following operation to add the frame to the DBoW loop closure visual word bag to generate the corresponding DBoW index table;

[0027] S3, searching for a data frame match in the key frame that matches the global old map, and determining an index number corresponding to the frame;

[0028] S4, if there is no global matching data frame match, go to step S1 for operation;

[0029] S5. If there is a global matching data frame match, calculate the rotation relationship matrix tf of all key frames relative to match, and keep the relocation hypothesis corresponding to the new and old images

[0030] S6. If tf is an empty array, go to step S1;

[0031] S7. If tf is a non-empty array, add the edge corresponding to tf to the pose graph updated in step S1, and minimize the cost function J(x new ) is used as the optimization criterion to optimize the pose graph and generate a new map; the minimization cost function J(x new )for:

[0032]

[0033] in, is the kth frame stereo camera pose data currently updated, is the pose transformation matrix of the j-th frame data relative to the i-th frame data, S new is a similar dataset to the current update frame;

[0034] S8. If the adjacent key frames all meet the relocation hypothesis, that is, and Then discard H j ,make

[0035] S9, if L consecutive key frames all meet the relocation hypothesis, that is, Then the relocation transformation matrix between the new and old maps is

[0036] S10, minimizing J(x) is used to optimize and obtain the suboptimal key frame relocation x, that is, the suboptimal key frame relocation x is obtained by optimizing the following cost function:

[0037]

[0038] in, is the last updated p-th frame stereo camera pose data, and S is a similar dataset of new and old map frames.

[0039] The technical solution of the present invention will be clearly and completely described below in conjunction with specific embodiments. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0040] Example

[0041] The navigation environment is set to a 10m*10m field. After the system is initialized, the frame data frame used for positioning is obtained from the robot system with ground navigation function (including at least inertial navigation and stereo camera) at a rate of 20Hz. If the frame does not belong to a key frame, a new node is created according to the input measurement value of the frame, and the node is connected to the last pose node in the pose graph. The updated pose graph is used for positioning the current frame; if the frame data belongs to a key frame, the frame data is added to the DBoW loop visual word bag, and the corresponding DBoW index table is made. Find the data frame match that globally matches the old map in the key frame, and determine the index number corresponding to the frame according to the DBoW index table; if there is no globally matching data frame match, re-acquire the positioning frame data frame; if there is a globally matching data frame match, calculate the relative rotation relationship matrix tf between all key frames and match, and save the repositioning hypothesis corresponding to the new map and the old map If tf is an empty array, that is, tf does not exist, then re-acquire the positioning frame data frame; if tf is not empty, add the edge corresponding to tf to the pose graph updated in the current frame to minimize the cost function J(x new ) optimizes the pose graph and generates a new map. Merge the assumptions that satisfy the relocalization between adjacent keyframes. If the relocalization assumptions are satisfied between consecutive L = 5 adjacent keyframes, that is, Then the relocation transformation matrix between the new and old maps is defined as The suboptimal keyframe relocalization x is obtained by minimizing J(x).

[0042] The number of consecutive adjacent key frames L is defined according to the changes in the actual ground scene. If there are more moving objects in the scene, the set L value should be reduced, otherwise, the set L value should be increased. The following table shows the positioning accuracy comparison between the key frame real-time positioning algorithm and the ORB-SLAM and LSD-SLAM algorithms in the two-dimensional space of the ground environment. It can be seen from the table that the positioning accuracy of the algorithm of the present invention is significantly better than that of the ORB- and LSD-SLAM algorithms.

[0043]

[0044] It should be noted that the above specific implementation is exemplary. Under the guidance of the present invention, those skilled in the art can make various improvements or changes based on the above embodiments, and these improvements or changes should fall within the protection scope of the present invention.

[0045] Those skilled in the art should understand that the above detailed description is only for the purpose of explaining the present invention, and is not intended to limit the scope of the present invention. The protection scope of the present invention is defined by the claims and their equivalents.

Claims

1. A stereo camera-assisted key frame real-time positioning method, characterized in that: The following steps are involved: S1, obtain the frame data frame for positioning of the robot system. If the frame data is not the key frame data, a new node is established based on the input measurement value and connected to the last pose node of the pose graph. The updated pose graph is used for step S7; S2. If the frame data is a key frame, add the frame to the DBoW loop closure visual word bag to generate a corresponding DBoW index table; S3, searching for a data frame match in the key frame that matches the global old map, and determining an index number corresponding to the frame; S4, if there is no global matching data frame match, go to step S1 for operation; S5. If there is a global matching data frame match, calculate the rotation relationship matrix tf of all key frames relative to match, and keep the relocation hypothesis corresponding to the new and old images S6. If tf is an empty array, go to step S1; S7. If tf is a non-empty array, add the edge corresponding to tf to the pose graph updated in step S1, and minimize the cost function J(x new ) as the optimization criterion to optimize the pose graph and generate a new map; S8. If the adjacent key frames all meet the relocation hypothesis, that is, and Then discard H j ,make S9, if L consecutive key frames all meet the relocation hypothesis, that is, Then the relocation transformation matrix between the new and old maps is S10, obtain the suboptimal key frame relocation x by minimizing J(x).

2. The method for real-time key frame positioning assisted by a stereo camera according to claim 1, characterized in that: In step S7, the minimization cost function J(x new )for: in, is the kth frame stereo camera pose data currently updated, is the pose transformation matrix of the j-th frame data relative to the i-th frame data, S new is a similar dataset for the current update frame.

3. The key frame real-time positioning method assisted by a stereo camera according to claim 1, characterized in that: In step S10, the suboptimal key frame relocation x is obtained by optimizing using the following cost function: in, is the last updated p-th frame stereo camera pose data, and S is a similar dataset of new and old map frames.

Images