Real-time online experience map fusion method

Abstract

The invention relates to a real-time online experience map fusion method. According to the method, a master-slave mode multi-robot system based on ROS (Root Operating System) is established, the system is composed of a master robot and other slave robots, different robots respectively operate an experience map of a Fast-iBOW-RatSLAM construction environment, and the Fast-iBOW-RatSLAM is composed of three parts including local visual cells, pose cells and the experience map. The master robot receives visual information sensed by the slave robot, and data association among different robots is realized through overlapped region detection; after the overlapping area is detected, the master robot sends empirical nodes of the overlapping area to the slave robot, and relative pose estimation is carried out on an empirical map module of the slave robot; and finally, fusing the generated local map by using a proposed continuous fusion multi-map relaxation algorithm, and finally obtaining a globally consistent experience map. The map fusion algorithm provided by the invention can effectively improve the mapping efficiency, and has important theoretical significance and use value.

Description

Technical Field

[0001] The present invention belongs to the field of mobile robot navigation and positioning, and specifically relates to a real-time online empirical map fusion method for a system of multi-robot collaborative navigation and positioning in an unknown environment. Background Art

[0002] Simultaneous Localization and Mapping (SLAM) is a technology for a robot to use sensor information to represent the environment and estimate its own motion information in an unknown environment, and it is the basis for a robot to perform tasks in an unknown environment. Current SLAM systems can be divided into two types. One is the probability-based method, such as the Kalman filter algorithm, etc. The other way is to use the method of nonlinear optimization to construct the map. Although these methods have high mapping accuracy, they have the defects of large computational amount and large storage amount, and cannot meet the navigation requirements in a long-term and large-scale environment.

[0003] At the same time, a large number of studies focus on solving the spatial information encoding mechanism in the animal brain. Researchers use the navigation neural mechanism in the rodent brain to establish a brain-inspired navigation method, and RatSLAM is one of them. RatSLAM has the characteristics of small computational and storage amounts and is suitable for map construction in a large environment. However, the RatSLAM system uses the sum of absolute differences of pixel values after average pooling to measure the similarity between images, and needs to compare with the saved visual templates one by one to achieve position recognition or loop detection, with poor visual perception accuracy and low efficiency. Subsequently, Fast-iBOW-RatSLAM was proposed to solve the problems of poor visual perception accuracy and low efficiency of RatSLAM. The Fast-iBOW-RatSLAM algorithm extracts several key points from the input image in local visual cells, calculates the binary feature descriptor of each key point to represent the image, then uses an incremental binary search tree (Hamming distance embedding binary search tree, HBST) to store and retrieve image features, and uses a dynamic island mechanism to perform loop verification on the retrieved similar images, greatly improving the accuracy and efficiency of RatSLAM visual perception. However, when using a single robot to construct a map in a large environment, there are still problems of low mapping efficiency and low fault tolerance rate. Summary of the Invention

[0004] In view of the above problems, the present invention proposes a real-time online empirical map fusion method, which utilizes the characteristics of low computational and storage amounts of Fast-iBOW-RatSLAM and combines multi-robot collaborative mapping to improve the mapping efficiency and fault tolerance rate in large-scale environment mapping.

[0005] The technical solution adopted by the present invention to achieve the above object is as follows:

[0006] A real-time online experience map fusion method, comprising the following steps:

[0007] 1) Construct a master-slave multi-robot system based on ROS, and respectively sense environmental image information;

[0008] 2) The master robot uses the environmental image information to detect overlapping regions based on a binary search tree and sequence matching;

[0009] 3) Estimate the relative poses between different robots according to the overlapping regions;

[0010] 4) Use the graph relaxation algorithm for continuously fusing multiple maps to fuse the maps with unified poses to obtain a globally consistent experience map.

[0011] The master-slave multi-robot system based on ROS consists of a master robot and multiple slave robots. The coordinate system of the master robot is used as the main coordinate system, and the maps of the slave robots are transformed according to the coordinate system of the master robot to finally obtain a globally consistent experience map.

[0012] The step 2) includes the following steps:

[0013] 2.1) The master robot calculates the received image I B and all the sensed images of the master robot for similarity;

[0014] 2.2) Set a dynamic threshold τ, and use the image frames with similarity scores higher than the dynamic threshold as candidate key frames;

[0015] 2.3) Based on the candidate key frames, obtain the final matching key frames by means of sequence matching.

[0016] The similarity is:

[0017]

[0018] wherein, is the number of feature points matched between the image I B and N is the total number of feature points extracted from each image, and n is the number of sensed images of the master robot.

[0019] The dynamic threshold τ is:

[0020]

[0021] The step 2.3) includes the following steps:

[0022] 2.3.1) Calculate the key frame sequence set using the candidate key frames Among them, represents the i-th image sequence in the sequence set, that is, the k i to t i frame images. When the candidate key frame is in an existing continuous frame sequence, add the two frames before and after it to the sequence; otherwise, construct a new continuous frame sequence centered on the candidate key frame and combining the two frames before and after it;

[0023] 2.3.2) The matching score B between the image I and the continuous frame sequence Take the central image of the continuous frame sequence with the highest score in the sequence set as the final matching key frame, where the matching score is:

[0024]

[0025] 2.3.3) Construct the i-th matching pair and of the master robot A and the slave robot B i whose matching score is O The i-th matching pair is represented as i All the matching pairs are represented as {M

[0026] The specific steps of step 3) are as follows:

[0027] Take the coordinate system of the master robot A as the reference coordinate system, calculate the relative pose relationship of the slave robot relative to the master robot, and then unify the understanding of the environment by the two robots. For the i-th matching pair Obtain the rotation matrix and the position and of the corresponding node. Among them, SO(3) represents the rotation matrix in three-dimensional space, and calculate the relative rotation matrix of the slave robot relative to the master robot

[0028]

[0029] According to the relative rotation matrix and the positions and of the corresponding node, calculate the translation vector

[0030]

[0031] When the number of matching pairs increases by one, the average rotation matrix And the average translation vector Perform an update, and finally transform from the coordinate system B of the robot to the coordinate system A of the main robot.

[0032] The graph relaxation algorithm for continuously fusing multiple maps is specifically as follows:

[0033]

[0034]

[0035] Among them, Δp i represents the update amount of the physical pose corresponding to the i-th experience node e of the robot, N i represents the number of experience nodes pointing to e f represents the number of experience nodes pointed to by e i p t is the physical pose corresponding to the j-th node among all the experience nodes pointing to e i p j is the physical pose corresponding to node e i Δp i is the change amount between p i and p ij p i is the physical pose corresponding to the k-th node among all the experience nodes pointed to by e j Δp k is the change amount between p i and p ki p i γ is a confidence level with a value between 0 and 1, indicating the influence degree of the front and rear nodes of node e k on it. i represents the experience node constructed from robot B in the latest matching pair, represents the experience node constructed from robot B in the previous matching pair, represents the matching score of the matching pair corresponding to node O max represents the current maximum matching score, and its initial value is set to ν. When the matching score is greater than O max O max will be updated to

[0036]

[0037] Map fusion is specifically as follows: When the matching score of the experience node is the maximum, γ is considered to be 1 for map fusion. With continuous iteration, the experience nodes constructed from robot B are pulled to a new position by the nodes connected to it in front;​​

[0038] When the experience node has a matching score greater than the threshold γ but less than the current highest matching score O max , and the index of the new matching node differs from the index of the previous node by less than k, it is considered that continuous matching has occurred. The nodes between the two experience nodes will be locally optimized according to the continuous map fusion with γ = 0.5. At the same time, when performing local optimization, the information contained in each edge is updated to maintain the stability of the map.

[0039] The present invention has the following beneficial effects and advantages:

[0040] 1. The present invention uses the dynamic island mechanism and the idea of sequence matching to complete the overlapping area detection, realizing the data association between different robots. At the same time, this overlapping area detection method avoids false matching and better avoids the competition between similar images that are close in time, greatly improving the accuracy of position recognition.

[0041] 2. The present invention proposes a continuous fusion multi-map graph relaxation algorithm to realize the fusion of local maps and reduce the offset error caused by map construction by different robots. This method can fuse the local maps of different robots into a consistent global map, greatly improving the mapping efficiency while ensuring the map fusion accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Figure 1 Schematic diagram of the positioning and mapping system based on Fast-iBOW-RatSLAM of the present invention;

[0043] Figure 2 Frame diagram of the overlapping area detection algorithm of the present invention;

[0044] Figure 3 Schematic diagram of the relationship between local map nodes of the present invention;

[0045] Figure 4 Experimental result diagram of collaborative mapping of the multi-robot system of the present invention under the public dataset;

[0046] Figure 5 Experimental result diagram of collaborative mapping of the multi-robot system of the present invention in the real environment;

[0047] Figure 6 Experimental comparison diagram of the mapping performance of a single robot and collaborative mapping of multiple robots of the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0048] The present invention will be further described in detail below with reference to the drawings and embodiments.

[0049] The present invention discloses a real-time online experience map fusion method, which realizes overlapping area detection by using the idea of dynamic islands and sequence matching. After detecting the overlapping area, relative pose estimation is carried out, and finally the proposed continuous fusion multi-map graph relaxation algorithm is used to realize the fusion between two maps. A multi-robot system is constructed to demonstrate and verify this map fusion algorithm.

[0050] As Figure 1 shown, it is a collaborative mapping method framework based on Fast-iBOW-RatSLAM. Among them, robot A is regarded as the main robot and robot B is regarded as the slave robot. Each robot independently senses the external environment and constructs its own local experience map. The robot senses the external environment through a monocular camera and obtains its own motion information using an IMU. At the same time, the main robot will receive the visual image information of the slave robot for overlapping area detection. After detecting the overlapping area, the main robot will send the overlapping experience node information to the slave robot. Based on these overlapping experience nodes, the slave robot performs relative pose estimation and uses the continuous fusion multi-map graph relaxation algorithm for map fusion to make its map globally consistent with the map constructed by the main robot. The yellow arrows in the figure show the information interaction between the two robots. The present invention is based on the constructed multi-robot system and is divided into three parts: overlapping area detection, relative pose estimation, and map fusion in the local view cells.

[0051] The multi-robot system adopts a distributed master-slave multi-robot system, where one robot is the main robot and the rest are slave robots. The local view cells of the main robot receive the environmental image information published by the slave robots for overlapping area detection, return the experience node information of the overlapping part to the slave robots, and the slave robots perform relative pose estimation according to the returned information and finally realize map fusion.

[0052] Overlapping area detection in the local view cells, which is carried out in the local vision cell module of the main robot. As Figure 2 shown, it is a framework diagram of an overlapping area detection method using an incremental binary search tree and sequence matching. The green background part is carried out in the main robot A, and the blue background part is carried out in the slave robot B. The whole method includes the following steps:

[0053] Step 1: The main robot receives the images it senses, uses the ORB algorithm (Oriented FAST and Rotated BRIEF) to extract features from the scene information collected by the robot, calculates the BRIEF descriptor for each ORB feature point, generates a binary descriptor set for this image, and the number of feature points is N. Similarly, the main robot will also receive the environmental images sensed by the slave robots and perform feature point extraction.

[0054] Step 2: Construction of the binary search tree of the main robot. The main robot inputs the feature descriptors of the images it senses into the binary search tree for retrieval to determine whether there are images similar to the currently sensed image. If the similarity scores of all images in the binary search tree with the current image are less than the threshold τ 1 , the descriptor of the current image is inserted into the binary search tree to update the binary search tree. The calculation method of the similarity score between images is as follows:

[0055]

[0056] where n i,j is the number of feature points matched between the current image I i and the image I j in the binary search tree, N is the total number of feature points in each image, n is the number of images sensed by the main robot, and τ 1 is a dynamic threshold, which is the mean of the similarity scores of I i with the previous n frames, and depends on the frame rate of the image: the higher the frame rate, the larger n, and its calculation method is as follows:

[0057]

[0058] Step 3: Similarity score calculation. After the main robot receives the image I B from the slave robot and performs feature extraction, it inputs the binary descriptor of this image into the binary search tree for similarity score calculation. The similarity score calculation method is as follows:

[0059]

[0060] is the number of feature points matched between the image I B of the slave robot and the i-th visual template of the main robot , and N is the total number of feature points extracted from each image.

[0061] Step 4: Obtaining candidate key frames. Candidate key frames are obtained by setting a dynamic threshold τ. After obtaining the similarity scores, the visual images of the main robot with similarity scores higher than the dynamic threshold will be used as candidate key frames. The calculation method of the dynamic threshold is as follows:

[0062]

[0063] Using this dynamic threshold, candidate key frames I a′ can be screened out:

[0064]

[0065] Step 5: Obtain the final matching key frames by means of sequence matching. Calculate the key frame sequence set using the candidate key frames where represents the i-th image sequence in the sequence set, which represents the k i to t i frame images. When the candidate frame is in an existing continuous frame sequence, add the two frames before and after it to the sequence; otherwise, construct a new continuous frame sequence centered on the candidate frame and combining the two frames before and after it. Image I B and the continuous frame sequence The matching score is as follows:

[0066]

[0067] The central image of the continuous frame sequence with the highest score in the sequence set is considered the final matching image. Therefore, we can obtain the i-th matching pair constructed by robot A and robot B and Their matching score is O i . The i-th matching pair is denoted as All matching pairs are denoted as {M i | i = 1,..., n}.

[0068] The experience map and the relationships between nodes are as Figure 3 shown. The map consists of nodes and edges, represents the i-th 1 experience node of robot A. The edges connecting the nodes contain the relative position relationship and the construction time relationship, as represents the edge generated between nodes and in robot A. The detected matching pairs are connected by dashed lines in the figure. The i-th matching pair is denoted as The rotation matrix of the corresponding node can be obtained from the matching pair and the position and SO(3) refers to the rotation matrix in three-dimensional space. Therefore, we can calculate the relative rotation matrix of the robot relative to the main robot

[0069]

[0070] According to the rotation matrix and the positions of the corresponding nodes and the translation vector can be obtained

[0071]

[0072] The number of matching pairs increases by one, and the average rotation matrix and the average translation vector are updated once. Therefore, the average rotation matrix and the average translation vector are used to transform from the coordinate system {B} of the robot to the coordinate system {A} of the master robot.

[0073] After transforming the corresponding matching nodes of the slave robot to the coordinate system of the master robot, map fusion is achieved using the continuous fusion multi-map graph relaxation algorithm. The continuous fusion multi-map graph relaxation algorithm is as follows:

[0074]

[0075] where, Δp i represents the update amount of the physical pose corresponding to the i-th experience node e i of the slave robot, N f represents the number of experience nodes pointing to e i , N t represents the number of experience nodes pointed to by e i , p j is the physical pose corresponding to the j-th node among all the experience nodes pointing to e i , p i is the physical pose corresponding to the node e i , Δp ij is the change amount between p i and p j , p k is the physical pose corresponding to the k-th node among all the experience nodes pointed to by e i , Δp ki is the change amount between p i and p k , γ is considered as the confidence level with a value between 0 and 1, indicating the influence degree of the front and rear nodes of the node e i on it. represents the experience node constructed by robot B in the latest matching pair represents the experience node constructed by robot B in the previous matching pair. represents the matching score of the matching pair corresponding to the node , O max represents the current maximum matching score, and its initial value is set to ν. When the matching score is greater than O max , O maX will be updated to

[0076] The whole process can be divided into two parts.

[0077] (1) When the experience node The matching score When it is the largest, γ is considered to be 1 for map fusion. With continuous iteration, the experience nodes constructed by Robot B are pulled to new positions by the nodes connected to it in front.

[0078] (2) When the experience node The matching score is greater than the threshold γ but less than the current highest matching score O MAX , and the index of the new matching node differs from the index of the previous node by less than k, we consider that continuous matching has occurred. At this time, the nodes between these two experience nodes will be locally optimized according to the continuous map fusion with γ = 0.5. At the same time, during local optimization, the information contained in each edge is updated to keep the map stable.

[0079] Figure 4 shows the mapping results of the multi-robot system under the public dataset KITTI. Robot A is the main robot, and the constructed map is red, while the maps constructed by the slave robots are black. The stars represent the current positions of the robots. It can be seen that map fusion was performed at t = 19.55s. During the entire process of map construction, the map constructed by the main robot remains unchanged, and the maps of the slave robots will be unified with it. However, there will be deviations when different robots construct maps of the same environment. This can be seen in the figure at t = 29.32s. The continuous fusion multi-map graph relaxation algorithm helps reduce the deviation under the same path. In addition, from t = 84.40s to t = 114.35s, the slave robots will also use the graph relaxation algorithm for map optimization after detecting loop closures.

[0080] Figure 5 shows the comparison results between the mapping results of the map fusion method we proposed under the KITTI dataset and the ground truth of the dataset.

[0081] Figure 6 shows the mapping results of the multi-robot system in a real environment. Map fusion occurred at t = 50.64s, but a scenario with a higher matching score was detected at t = 125.32s and the map fusion was adjusted. The global map was completed at t = 146.48s.

[0082] The global experience map constructed by multiple robots basically coincides with the map constructed by a single robot, proving the effectiveness and feasibility of the algorithm proposed in this paper.

Claims

1. A real-time online empirical map fusion method, characterized in that: The following steps are involved: 1) Build a master-slave multi-robot system based on ROS and perceive environmental image information separately; 2) The main robot uses the environmental image information to detect overlapping areas based on binary search trees and sequence matching; 3) Estimating the relative poses of different robots based on the overlapping areas; 4) Using the graph relaxation algorithm of continuous fusion of multiple maps, different maps are fused after the posture is unified to obtain a globally consistent empirical map.

2. A real-time online experience map fusion method according to claim 1, characterized in that: The ROS-based master-slave multi-robot system consists of a master robot and multiple slave robots. The coordinate system of the master robot is used as the master coordinate system, and the maps of the slave robots are transformed according to the coordinate system of the master robot to finally obtain a globally consistent experience map.

3. A real-time online experience map fusion method according to claim 1, characterized in that: Step 2) The following steps are involved: 2.1) The master robot calculates the received image I according to the environmental image information perceived by the slave robot B All perception images of the main robot similarity; 2.2) Setting a dynamic threshold τ, and taking image frames with similarity scores higher than the dynamic threshold as candidate key frames; 2.3) Based on the candidate key frames, the final matching key frames are obtained by sequence matching.

4. A real-time online experience map fusion method according to claim 3, characterized in that: The similarity for: in, is image I B and The number of matched feature points, N is the total number of feature points extracted on each image, and n is the number of images perceived by the main robot.

5. A real-time online experience map fusion method according to claim 3, characterized in that: The dynamic threshold τ is:

6. A real-time online experience map fusion method according to claim 3, characterized in that: Step 2.3) The following steps are involved: 2.3.1) Use candidate key frames to calculate the key frame sequence set in, represents the i-th image sequence in the sequence set, that is, the k-th i to i Frame image, when the candidate key frame is in an existing continuous frame sequence, the two frames before and after it are added to the sequence; otherwise, a new continuous frame sequence is constructed with the candidate key frame as the center and the two frames before and after it combined; 2.3.2) Image I B With continuous frame sequence Matching score The central image of the continuous frame sequence with the highest score in the sequence set is taken as the final matching key frame, where the matching score for: 2.3.3) Construct the i-th matching pair of master robot A and slave robot B and Its matching score is O i , the i-th matching pair is represented as All matching pairs are represented as {M i |i=1,...,n}.

7. A real-time online experience map fusion method according to claim 1, characterized in that: The step 3) is specifically as follows: The coordinate system of the master robot A is used as the reference coordinate system to calculate the relative position and posture relationship of the slave robot with respect to the master robot, thereby unifying the two robots' cognition of the environment. Get the rotation matrix of the corresponding node and location and Among them, SO(3) represents the rotation matrix in three-dimensional space, and the relative rotation matrix of the slave robot relative to the master robot is calculated: According to the relative rotation matrix and the position of the corresponding node and Calculate the translation vector When the number of matching pairs increases by one, the average rotation matrix and the mean translation vector An update is performed, which will eventually transform the slave robot's coordinate system B to the master robot's coordinate system A.

8. A real-time online experience map fusion method according to claim 1, characterized in that: The graph relaxation algorithm for continuous fusion of multiple maps is specifically: Among them, Δp i Indicates the robot's i-th experience node e i The corresponding physical pose update amount, N f Indicates pointing to e i The number of experience nodes, N t Indicates e i The number of experience nodes pointed to, p j All points to e i The physical pose corresponding to the jth node in the empirical node, p i is node e i The corresponding physical pose, Δp ij Yes i With p j The change between k Yes i The kth node among all the experience nodes pointed to corresponds to the physical pose, Δp ki Yes i With p k The change between γ and γ is the confidence value between 0 and 1, indicating that the node e i The degree of influence of the previous and next nodes on it, represents the experience node constructed from robot B in the latest matching pair, represents the experience node constructed from robot B in the previous matching pair, Representation Node The matching score of the corresponding matching pair, O max Indicates the current maximum matching score. Its initial value is set to v. When the matching score Greater than O max When max will be updated to 9. A real-time online experience map fusion method according to claim 1 or 8, characterized in that: Map fusion is specifically as follows: When the experience node Matching score When γ is the largest, γ is considered to be 1 for map fusion. With continuous iterations, the experience nodes constructed by robot B are pulled to new positions by the nodes connected to it in front of it. When the experience node Matching score Greater than the threshold γ but less than the current highest matching score O max , and the index of the new matching node The index of the previous node When the difference is less than k, it is considered that continuous matching has occurred, and the nodes between the two experience nodes will be locally optimized according to the continuous map fusion with γ being 0.

5. At the same time, during local optimization, the information contained in each edge is updated to keep the map stable.

Images