A multi-robot cooperative composition method

Abstract

The application discloses a kind of multi-robot cooperative mapping methods, each robot local all maintain a local topological map consisting of key image frame pose, and the spatial position of feature point observed by key frame observation and the descriptor corresponding to feature point composition.In the process of robot movement, the map is constantly updated, and the incremental part of the map is sent to the central server.On the server, the position re-identification algorithm based on feature point matching is used to detect the coincidence part of each robot map, and the maps with intersection are merged.Each robot pose is optimized first, and then the optimized pose is fed back to each robot.Finally, the optimized robot pose is used as prior information to optimize the map point coordinates.The method has the advantages of saving communication bandwidth, improving cooperative mapping accuracy, and improving the positioning accuracy of each robot.The method can obtain a global map through online cooperation of multi-robot platform.

Description

Technical Field

[0001] The present invention belongs to the field of unmanned system navigation technology, and specifically, is a method for real-time online mutual positioning and map construction of multiple robots in collaboration. Background Art

[0002] Robot swarms are widely used in surveillance, target detection, environmental monitoring and other fields due to their low cost and high mobility. However, the endurance, movement speed and detection field of a single robot are limited, and it is impossible to explore a large area in a short time. Therefore, multi-robot systems are used to work together.

[0003] Patent CN 109559277 A proposes a multi-UAV collaborative map construction method for data sharing. This patent adaptively fuses maps based on changes in the local maps constructed by the UAVs in an order that is most conducive to improving data sharing. However, its front-end odometer uses only visual image information and cannot integrate other sensor data, resulting in robustness in complex scenes and high-light-ratio environments. Furthermore, this method packages and sends map information during data transmission, which places a heavy burden on communications and causes significant delays in the repositioning of both the server and each robot terminal. Summary of the Invention

[0004] The present invention aims to provide a multi-robot collaborative mapping method that utilizes visual information acquired by each robot platform to identify overlapping portions of local maps. This method enables collaborative online mutual localization and map fusion among multiple robots, addressing the relocalization and map fusion issues encountered during multi-robot collaborative map construction under limited communication bandwidth.

[0005] The technical solution for achieving the purpose of the present invention is: a multi-robot collaborative mapping method, which divides the multi-robot system into robot nodes and a central server. In the system framework of multi-robot collaborative synchronous positioning and composition, the robot nodes are equipped with photoelectric image sensors, IMUs, and satellite positioning. Each robot locally maintains a local topological map consisting of the key image frame pose, the spatial position of the feature points observed in the key frame, and the descriptors corresponding to the feature points. During the movement of the robot, the map is continuously updated, and the incremental part of the map is sent to the central server. On the server, a position re-identification algorithm based on feature point matching is used to detect the overlapping parts of each robot map, and the maps with intersections are merged. First, the pose of each robot is optimized, and then the optimized pose is fed back to each robot. Finally, the coordinates of the map points are optimized using the optimized robot pose as prior information.

[0006] The specific implementation steps are as follows:

[0007] Step (1): Each robot node is equipped with a photoelectric image sensor and IMU as standard, and some are equipped with satellite positioning. The robot's own posture results obtained by the visual inertial odometer and the satellite positioning data are sent to the central server in the form of key frame data packets.

[0008] Step (2): Each time the server receives a keyframe data packet, it stitches the map point information and posture information into a map to be fused, and compares the image feature information with the image feature information previously received from each robot. Position re-identification is performed. If a similar scene occurs, that is, the robot represented by the keyframe has passed by a location previously passed by another robot, or the robot has passed by a location previously passed by itself, then step 3 is performed. Otherwise, the image feature data is stored in the bag-of-words model and the next data packet is awaited.

[0009] Step (3) calculates the pose transformation relationship between the two key frames to obtain the initial transformation matrix. Then, the map points of the local map to be fused are projected onto the current key frame Ka through this transformation matrix. Then, the guided matching method is used to obtain more matching point pairs and perform nonlinear optimization to obtain an accurate transformation matrix.

[0010] Step (4) transforms the map points of the matched map to be fused to the map coordinates of the current key frame, eliminating duplicate map points. Then, the key frames of the two maps are fused to generate a new map.

[0011] Step (5): Optimize the pose graph of the entire merged map.

[0012] Step (6): Send the optimized robot positions and their relative positions back to each robot.

[0013] Step (7): Use the optimized robot posture as prior information to optimize the coordinates of the map points in the fused large map.

[0014] Furthermore, in the step (1), the robot's own posture result data obtained by the visual inertial odometry is aligned with the timestamp to form a "key frame data packet", which contains a frame image and all satellite positioning data and IMU data from the time the image is taken to the previous frame image; a topological map is constructed based on the extracted feature points, feature points are obtained through feature detection, and the robot's motion estimation is obtained through triangulation solution. After eliminating outliers, the key frame posture and map point coordinates obtained by the motion estimation, as well as the observation relationship between them, are saved as a local topological map; and the key frame data packet is sent to the central server via wireless communication.

[0015] Furthermore, in step (2), the key frame K sent by a single robot aThe feature descriptor of the image information in the image is used to find out whether there are any key frames of other robots received previously that are similar to K a Matched; key frame K on the matched b Search for as many map points as possible in the adjacent common view key frames to generate the best map point correspondence. When the number of matched point pairs exceeds the threshold, it is considered that the map M a 、M b There are overlapping parts between them, which need to be merged; if a new matching relationship is generated later, it will be combined with the map M a 、M b The fused large map is further fused.

[0016] Furthermore, in step (3), the map M b The feature points corresponding to the map points are projected onto K a In the process, the reprojection error of the feature points of the two maps is optimized nonlinearly to obtain the relative pose transformation matrix between the two key frames.

[0017] Compared with existing technologies, this invention has the following significant advantages: (1) It uses feature points to extract and describe image information, and uses a topological map to simultaneously describe spatial position and observation relationships, saving communication bandwidth; (2) It can improve the positioning accuracy of each robot while also improving collaborative mapping accuracy; and (3) In the collaborative positioning process, it integrates sensor information from IMU and satellite positioning, which is conducive to improving the positioning accuracy of each robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] Figure 1 This is a flow chart of a multi-robot collaborative mapping method of the present invention.

[0019] Figure 2 It is a structural diagram of the graph optimization problem for solving the robot posture in an embodiment of the present invention.

[0020] Figure 3 It is a structural diagram of a graph optimization problem for solving map point coordinates in an embodiment of the present invention.

[0021] Figure 4 It is the local map constructed by robot a in the embodiment of the present invention.

[0022] Figure 5 It is the local map constructed by the robot b in the embodiment of the present invention.

[0023] Figure 6 It is a global map obtained by merging and optimizing the two maps in the embodiment of the present invention. DETAILED DESCRIPTION

[0024] The method of the present invention addresses the problem of low efficiency and poor map accuracy of single robots facing large-scale map exploration tasks due to limitations in movement ability and range. It proposes a real-time synchronous positioning and map construction framework based on the fusion of multiple sensors such as visual inertia. In this framework, each robot locally maintains a local topological map consisting of the pose of key image frames, the spatial positions of feature points observed in the key frames, and the descriptors corresponding to the feature points. During the movement of the robot, the map is continuously updated, and the incremental part of the map is sent to the central server. On the server, a position re-identification algorithm based on feature point matching is used to detect the overlapping parts of the maps of each robot, and the maps with intersections are merged. The pose of each robot is first optimized, and then the optimized pose is fed back to each robot. Finally, the coordinates of the map points are optimized using the optimized robot pose as prior information.

[0025] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0026] Take the collaborative composition of two robots as an example. Robot A is equipped with a monocular camera, IMU, and GNSS satellite positioning, while robot B is equipped with a monocular camera and IMU.

[0027] like Figure 1 As shown, the specific steps of the present invention are as follows:

[0028] Step 1: The robot node locally maintains an odometry based on multivariate data fusion and a local topological map. This topological map describes the relationship between map points with significant features and the robot's poses at different times, as well as the relative position between map points and the robot. The connections between keyframes are described using a weighted vector graph structure, known as a "spanning tree." Multimodal sensor data is timestamped to form a "keyframe data packet," which contains a single image and all satellite positioning and IMU data from the moment the image was captured to the previous frame. A topological map is constructed based on feature point extraction. Feature points are obtained through feature detection, and the robot's motion estimate is derived through triangulation. After removing outliers, the keyframe poses and map point coordinates derived from the motion estimate, as well as the observed relationships between them, are stored as a local topological map. The keyframe data packet is then transmitted to a central server via wireless communication.

[0029] Step 2: Keyframe K sent by a single robot a The feature descriptor of the image information in the image is used to find out whether there is a key frame that matches it before through the DBoW2 algorithm. b Search as many map points as possible in the adjacent common view key frames to generate the best map point to map point correspondence. When the number of matched point pairs exceeds the threshold (set to a total of 250 in the embodiment), it is considered that the map M a 、Mb There are overlapping parts that need to be merged.

[0030] Step 3: Map M b The feature points corresponding to the map points are projected onto K a In the process, the reprojection error of the feature points of the two maps is optimized nonlinearly to obtain the relative pose transformation matrix between the two key frames.

[0031] Step 4: Fuse the two maps to be fused and merge the spanning tree and co-map.

[0032] Step 4.1: Using the estimated transformation matrix To guide the matching. Use a Sim3 transformation in is a 3×3 rotation transformation matrix, is the translation transformation vector, c ab is the scale transformation factor, which describes the change relationship between the two map point clouds. The error equation is established by matching the map points, and the rotation matrix, translation vector and scale transformation factor are solved respectively.

[0033] Step 4.2: Fusion map. Using pose Put M b All keyframes and map points in M ​​are projected onto a Detect duplicate map points;

[0034] Step 4.3: Put M b and M a All keyframes and map points in the new map M m At the same time, put M b and M a The spanning tree and cograph are combined into M m Spanning tree and cograph graph of .

[0035] Step 5: For the fused large map M m Optimize the robot pose in .

[0036] Step 5.1: For the newly generated map M m Construct a graph optimization problem. Nodes represent poses, and edges represent relationships between poses. Build an observation model for each sensor, where the visual-inertial data information is as follows:

[0037] χ=[x0,x1,...,x n ,λ1,...,λ m ] T

[0038]

[0039] where x i represents the position, attitude, velocity, and bias of the IMU accelerometer and gyroscope of the robot in the world coordinate system at time i, n is the number of key frames, m is the number of map points, and λ m is the inverse depth of the feature point.

[0040] Optimize all state variables by minimizing the sum of all error terms:

[0041]

[0042] ρ is the Huber robust kernel function, B is the set of IMU measurements in the sliding window, r b is the residual of IMU, F is the set of feature points in the sliding window, r f is the visual reprojection error. The pre-integration results of the IMU sensor data between key frames are: Represent position, velocity and attitude respectively. The IMU pre-integration error is defined as:

[0043]

[0044] Where: Δt is the time interval between two adjacent frames, [·] xyz Indicates that only the three-dimensional vector consisting of the imaginary part (x, y, z) of the quaternion is taken.

[0045] For a point P in space i =(X i ,Y i ,Z i ), whose pixel coordinate is u i , then the reprojection error of the point feature can be defined as:

[0046]

[0047] where z i P i The depth of , K is the camera intrinsic parameter, (·)^ represents the antisymmetric matrix of the vector.

[0048] Step 5.2: GPS positioning point on map M a After the coordinates are converted, robot a can receive the GPS positioning frame and obtain a position constraint: That is, the position change between two GPS positioning and the position change residual between the two corresponding frames calculated in step 5.1 should be minimized. Figure 2 :

[0049] After iterative optimization, the robot's position on the fused map at each key frame is finally obtained. and T ib Where R is a 3×3 rotation transformation matrix and t is a translation transformation vector.

[0050] Step 6: The most recent pose T a m and T b m Sent to robot a and robot b. On robot a, the corresponding key frame pose T a n With T a m The transformation relationship between Current pose The calculation of robot b is similar.

[0051] Step 7: Build the graph optimization model as shown in the attached Figure 3 .

[0052] The lth feature is observed for the first time in the i-th image, and the residual of the feature observation in the j-th image is defined as:

[0053]

[0054]

[0055]

[0056] in is the first observation of the lth feature appearing in the i-th image. is the observation of the same feature in the jth image. It is a back-projection function that converts the pixel position into a unit vector using the camera's intrinsic parameters. Since the degree of freedom of the visual residual is 2, we project the residual vector onto the tangent plane. Since b1 and b2 are on the tangent plane Two arbitrarily chosen orthogonal bases on , based on this condition, a set of b1, b2 can be found. is the standard covariance of fixed length in tangent space. The optimized map point coordinates can be obtained by solving the graph optimization problem.

[0057] Simulation experiment

[0058] In order to illustrate the effectiveness of the algorithm of the present invention and fully demonstrate that the method has the function of collaborative path planning, the following experiments were completed:

[0059] (1) Experimental initial conditions and parameter settings

[0060] In the simulation experiment, robot a carries a camera, IMU, and GPS module, and robot b carries a camera and IMU. The frame rate of the camera is 20 Hz, and the frequency of the IMU is 200 Hz.

[0061] Robot A and Robot B start from the same position and explore the same spatial area according to their own different trajectories. The robots communicate with the server using a wireless network.

[0062] (2) Analysis of experimental results

[0063] Attachment Figure 4 is the local map obtained by robot a, Figure 5 is the local map obtained by robot b, Figure 6 It is the global map obtained after server fusion.

Claims

1. A multi-robot collaborative mapping method, characterized by: The multi-robot system is divided into robot nodes and a central server. In the system framework of multi-robot collaborative synchronous positioning and composition, the robot nodes are equipped with photoelectric image sensors, IMUs, and satellite positioning. Each robot maintains a local topological map composed of the key image frame pose, the spatial position of feature points obtained by key frame observations, and the descriptors corresponding to the feature points. During the robot movement, the map is continuously updated, and the incremental part of the map is sent to the central server. On the server, a position re-identification algorithm based on feature point matching is used to detect the overlapping parts of each robot map, and the intersecting maps are merged. The pose of each robot is first optimized, and then the optimized pose is fed back to each robot. Finally, the coordinates of the map points are optimized using the optimized robot pose as prior information. The specific implementation steps are as follows: Step (1): Each robot node is equipped with a photoelectric image sensor and IMU as standard, and some are equipped with satellite positioning. The robot's own posture results obtained by the visual inertial odometer, the coordinates of the map points and the read satellite positioning data are sent to the central server in the form of key frame data packets; Step (2): Each time the server receives a keyframe data packet, it splices the map point information and posture information therein into a map to be fused, compares the image feature information therein with the image feature information previously received from each robot, and performs position re-identification; if a similar scene appears, that is, the robot represented by the keyframe passes by the position that another robot has passed by before, or the robot passes by the position that it has passed by before, then execute step 3; otherwise, the image feature data is stored in the BOW word bag model and waits for the next data packet; Step (3) calculates the pose transformation relationship between the two key frames to obtain the initial transformation matrix, and then projects the map points of the local map to be fused onto the current key frame Ka through this transformation matrix. Then, the guided matching method is used to obtain more matching point pairs and perform nonlinear optimization to obtain an accurate transformation matrix. Step (4): transform the map points of the matched map to be fused to the map coordinates of the current key frame, eliminating duplicate map points; then fuse the key frames of the two maps to generate a new map; Step (5), optimizing the pose graph of the entire merged map; Step (6), sending the optimized robot positions and their relative positions back to each robot; Step (7): Use the optimized robot posture as prior information to optimize the coordinates of the map points in the fused large map.

2. The multi-robot collaborative mapping method according to claim 1, characterized in that: In the step (1), the robot's own posture result data obtained by the visual inertial odometry is aligned with the timestamp to form a "key frame data packet", which contains a frame image and all satellite positioning data and IMU data from the time the image is taken to the previous frame image; a topological map is constructed based on the extracted feature points, feature points are obtained through feature detection, and the robot's motion estimation is obtained through triangulation solution. After eliminating outliers, the key frame posture and map point coordinates obtained by the motion estimation, as well as the observation relationship between them, are saved as a local topological map; and the key frame data packet is sent to the central server via wireless communication.

3. The multi-robot collaborative mapping method according to claim 1, characterized in that: In step (2), the key frame K sent by a single robot a The feature descriptor of the image information in the image is used to find out whether there are any key frames of other robots received previously that are similar to K a Matched; key frame K on the matched b Search for as many map points as possible in the adjacent common view key frames to generate the best map point correspondence. When the number of matched point pairs exceeds the threshold, it is considered that the map M a 、M b There are overlapping parts between them, which need to be merged; If a new matching relationship is generated later, it will be matched with the map M a 、M b The fused large map is further fused.

4. The multi-robot collaborative mapping method according to claim 1, characterized in that: In the step (3), map M b The feature points corresponding to the map points are projected onto K a In the process, the reprojection error of the feature points of the two maps is optimized nonlinearly to obtain the relative pose transformation matrix T between the two key frames. kakb .

Images