A Low-Cost Cooperative Localization and Mapping Method Based on Mutual Observation of Multiple Robots

Abstract

The present invention belongs to the technical field of intelligent navigation, and particularly relates to a low-cost collaborative mapping method based on mutual observation of multiple robots. It is applied to a multi-robot formation. Each robot in the formation is equipped with sensors such as a low-cost single-line lidar, a camera, and an IMU or a wheel speedometer, and the multiple robots communicate through a Wi-Fi local area network. Each robot in the robot formation combines the point cloud data of the single-line lidar with the IMU attitude angle data and acceleration data to achieve its own pose estimation and local map construction; furthermore, taking the key frame poses of each robot itself as vertices and the mutual observations between robots and the poses of other robots estimated by themselves as measurement edges, each constructs a pose graph optimization model, and then uses the results of graph optimization for loop detection to further optimize the pose of a single robot; on this basis, each robot performs pose transformation on the point cloud shared by other observed robots, and then incrementally performs map merging to achieve map information sharing among multiple robots, and all robots can independently construct an accurate global multi-beam point cloud map.

Description

Technical Field

[0001] The present invention belongs to the technical field of intelligent navigation, and particularly relates to a low-cost cooperative localization and mapping method based on mutual observation of multiple robots. Background Technique

[0002] With the rapid development of autonomous robot technologies such as intelligent control, environmental perception, and multi-sensor fusion, as well as their cross-research fields, multi-robot systems are increasingly widely used in various fields. The multi-robot cooperative localization technology is a necessary prerequisite for realizing the autonomous navigation of multi-robot systems. The commonly used localization method is to rely on various sensors (such as cameras, lidars, etc.) carried by the robots to perceive the external environment and determine their own positions, that is, simultaneous localization and mapping (SLAM). Currently, the existing mainstream SLAM algorithms mainly target single robots. However, in practical applications, a single robot often cannot handle some complex tasks, such as search and rescue, infrastructure inspection, home service, and logistics and transportation applications. Multi-robot cooperation is far more efficient than a single robot. For example, when exploring a large-scale unknown environment, the multi-robot cooperative SLAM scheme uses multiple robots to cooperate in exploring unknown areas, which can quickly collect information, improve the exploration speed, and expand the exploration range.

[0003] Currently, most multi-robot cooperative mapping methods rely on each robot to achieve its own localization, and then use methods based on feature matching or position recognition to calculate the relative poses between multiple robots. After optimizing the poses, the map merging is realized to construct a globally consistent map. However, the multi-robot cooperative mapping method based on feature matching or position recognition still has many deficiencies. First, when facing scenes with high similarity in the environment, the error rate of position recognition increases greatly, and the calculation results of the relative poses between robots are inaccurate. Second, in an environment with few features, it is difficult for robots to achieve encounter position recognition, which reduces the robustness of the algorithm. Third, such methods have high requirements for sensors, such as multi-line lidars, which will increase the sensor cost.

[0004] In the prior art, sensors such as UWB (Ultra-Wideband) and WiFi are used to obtain the relative distance information or azimuth information between robots. In such methods, each robot first realizes its own positioning, and then uses the relative distance information or azimuth information to calculate the relative pose between robots, and finally realizes the merging of maps. However, there are also some deficiencies in these methods that use the relative distance information or azimuth information between multiple robots. On the one hand, these methods assume that all robots can communicate in real time, and the communication bandwidth is sufficient to transmit key frame information, UWB ranging information, and map point information. However, in practical applications, these conditions may not be met. On the other hand, these methods require robots to keep observing each other all the time. For example, the method using WiFi fingerprint information requires the WiFi to be always connected, which affects the scalability and robustness of the system. Summary of the Invention

[0005] The object of the present invention is: in view of the problems existing in the prior art, such as inaccurate relative pose calculation, too high deployment cost, and unreliable in an environment with few features, a low-cost collaborative positioning and mapping method based on mutual observation of multiple robots is proposed, which solves the problems of pose collaborative optimization and map information sharing between multiple robots, and at the same time reduces the cost of sensors required for deployment on robots.

[0006] The technical solution of the present invention: In order to achieve the above object of the invention, according to the first aspect of the present invention, a low-cost collaborative mapping method based on mutual observation of multiple robots is proposed, which is applied to a multi-robot formation. Each robot in the formation is equipped with sensors such as a low-cost single-line lidar, a camera, and an IMU (Inertial Measurement Unit) or a wheel speedometer, etc. The multiple robots communicate through a WiFi local area network.

[0007] Each robot in the robot formation combines the point cloud data of the single-line lidar and the IMU attitude angle data and acceleration data to realize its own pose estimation and local map construction; furthermore, taking the key frame poses of each robot itself as vertices and the mutual observations between robots and the estimated poses of other robots as measurement edges, a pose graph optimization model is constructed respectively, and then the graph optimization result is used for loop detection to further optimize the pose of a single robot; on this basis, each robot performs a pose transformation on the point cloud shared by other observed robots, and then incrementally performs map merging to realize map information sharing between multiple robots, and all robots can independently construct an accurate global multi-beam point cloud map.

[0008] An embodiment of the present invention specifically includes the following steps:

[0009] Step 1: Each robot in the formation performs its own positioning and constructs a local map in real time. The local map is composed of key frames of the sensors;

[0010] Step 2: When different robots in the formation meet, use the sensors carried by themselves to observe each other, calculate the relative pose information of the observed robot relative to itself, and share the relative pose information with other robots in the formation;

[0011] Step 3: Each robot in the formation constructs a pose graph optimization model with the self-keyframe pose in Step 1 and the relative pose information calculated in Step 2 as constraints, uses the nonlinear optimization method to solve and obtain the optimized result of each robot's own pose, and updates the keyframe pose information in the local map;

[0012] Step 4: Use the pose optimization result in Step 3 to perform loop detection on the keyframe at the current moment of the robot and the local historical map. If a loop is detected, use the loop constraint information to construct a pose graph optimization model, further optimize the keyframe pose, and update the local map. If no loop is detected, keep the keyframe pose in the map unchanged.

[0013] Preferably, the range of the local historical map refers to the range within 1 meter of the relative distance of the robot.

[0014] Step 5: Merge the local maps of each robot in Step 4 to construct a global map, thereby realizing the sharing of map information among multiple robots.

[0015] In a possible embodiment, in Step 1, the method for the robot to perform self-localization and mapping adopts one of the Cartographer (mapping algorithm) algorithm and the gmapping (Lidar localization and mapping algorithm) algorithm.

[0016] In a possible embodiment, the sensors carried by the single-line Lidar, camera, and IMU or wheel speed sensor include a single-line Lidar, a camera, an IMU, or a wheel speed sensor.

[0017] In a possible embodiment, the ROS master-slave communication mechanism is used for communication. One of the robots is the ROS (Robot Operating System) master, and the other robots are ROS slaves. The ROS master centrally publishes messages to improve communication efficiency; the ROS master-slave architecture is easy to expand. When adding new robots, only need to configure them as slaves without large-scale modification of the existing system. When initializing, use the NTP (Network Time Protocol) service to synchronize the time of all robots to ensure that each robot has a consistent time reference in the distributed system, thereby avoiding data mismatch caused by time inconsistency.

[0018] In a possible embodiment, in step 2, when a monocular camera is adopted, an AprilTag (April label) graphic code is set on each robot in the formation; in step 2, the relative pose information of the observed robot relative to itself is deduced by observing and identifying the AprilTag graphic code carried by the robot using a monocular camera.

[0019] In a possible embodiment, in step 2, when a binocular camera is adopted, in step 2, the relative pose information of the observed robot relative to itself is deduced by using the yolo-v8 (object detection algorithm) neural network to perform object recognition on the images captured by the binocular camera.

[0020] In a possible embodiment, in step 3, the algorithms for solving the pose graph optimization model by the nonlinear optimization method include the Gauss-Newton method and the Levenberg-Marquardt method.

[0021] In a possible embodiment, in step 4, the methods for detecting loops include but are not limited to matching the current key frame point cloud with the local historical map, using the method based on radar point cloud data matching for loop closure detection, and using the method based on pose estimation for loop closure detection.

[0022] In a possible embodiment, in step 5, the methods for constructing the global point cloud map include but are not limited to performing pose transformation on the point clouds shared by other robots and incrementally merging the maps, and storing the point cloud map in an incremental k-dimensional tree and incrementally merging the maps.

[0023] The beneficial technical effects of the present invention:

[0024] Compared with the existing multi-robot collaborative simultaneous localization and mapping methods, the present invention has the following advantages:

[0025] First, for the multi-robot collaborative mapping method based on feature matching or position recognition, when facing a scene with high similarity in the environment, the error rate of position recognition greatly increases, and the calculation results of the relative poses between robots are inaccurate; while the present invention has no such limitation, and only needs to use a camera to perform mutual observation when different robots meet to obtain the accurate relative poses between robots.

[0026] Second, for the multi-robot collaborative mapping method based on feature matching or position recognition, in an environment with scarce features, it is difficult for robots to recognize the meeting positions, reducing the robustness of the algorithm; while the present invention uses the object recognition method to obtain the relative poses between robots, without being interfered by the environmental feature texture information.

[0027] Thirdly, the sensors required by existing methods for constructing a global three-dimensional point cloud map based on multi-robot collaborative SLAM generally require multi-line lidar, which will result in high sensor costs. While methods that only use low-cost single-line lidar can only construct a global two-dimensional grid map. In the method described in the present invention, each robot in the robot formation uses a combination of low-cost single-line lidar, camera, and IMU (inertial measurement unit) or wheel speed sensor. On this basis, the present invention realizes the sharing of map information among multiple robots, and all robots can independently construct an accurate global multi-beam point cloud map, effectively reducing the sensor cost. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] Figure 1 Schematic diagram of three robots in a preferred embodiment of the present invention using single-line lidar to collect environmental information;

[0029] Figure 2 Flowchart of mutual observation when two robots meet in a preferred embodiment of the present invention;

[0030] Figure 3 Schematic diagram of mutual observation when two robots meet in a preferred embodiment of the present invention;

[0031] Figure 4 Schematic diagram of pose graph optimization in a preferred embodiment of the present invention;

[0032] Figure 5 Schematic diagram of loop closure detection in a preferred embodiment of the present invention;

[0033] Figure 6 Schematic diagram of the global multi-beam point cloud map in a preferred embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0034] To make the objectives, technical solutions, and advantages of the present invention clearer and more understandable, the embodiments of the present invention are described in detail below. It should be noted that, without conflict, the embodiments in this application and the features in the embodiments can be combined arbitrarily with each other.

[0035] A low-cost collaborative mapping method based on mutual observation of multiple robots proposed by the present invention is applied to a multi-robot formation. Each robot in the formation is equipped with sensors such as a low-cost single-line lidar, a camera, and an IMU (inertial measurement unit) or a wheel speed sensor, and the multiple robots communicate through a WiFi local area network.

[0036] Each robot in the robot formation combines the point cloud data of the single-line lidar, the IMU attitude angle data, and the acceleration data to achieve its own pose estimation and local map construction; furthermore, taking the key frame poses of each robot itself as vertices and the mutual observations between robots and the poses of other robots estimated by themselves as measurement edges, a pose graph optimization model is constructed respectively, and then the loop closure detection is performed using the graph optimization results to further optimize the pose of a single robot; on this basis, each robot performs a pose transformation on the point cloud shared by other observed robots, and then incrementally merges the maps to achieve map information sharing among multiple robots, and all robots can independently construct an accurate global multi-beam point cloud map.

[0037] Embodiment 1:

[0038] Step 1. As Figure 1 shown, three robots are used to form a formation and move in an indoor environment with sparse features. Each robot in the formation is equipped with a low-cost single-line lidar, a monocular camera, and an IMU sensor to perform the SLAM task, and the Cartographer algorithm is used to generate point cloud key frames and estimate the positions and postures corresponding to the key frames. Communication between multiple robots is carried out through a WiFi local area network using the ROS master-slave communication mechanism. One of the robots is used as the ROS master, and the other robots are used as ROS slaves. When initializing, the NTP service is used to synchronize the time of all robots.

[0039] For the convenience of subsequent processing, in this embodiment, for the collected original laser point cloud data, the algorithm can remove invalid points by removing the point cloud data with abnormal intensity values, remove ground points by calculating geometric features, and correct the motion distortion of the lidar point cloud by combining IMU data.

[0040] Step 2. As Figure 2 shown, when different robots meet, they use monocular cameras to observe each other. In this example, each robot is equipped with an easily recognizable AprilTag label graphic code. After the robot uses the camera to collect image data, it performs image target recognition to obtain the position and direction of the observed graphic code relative to the camera. As Figure 6 shown, furthermore, the position and direction of the observed robot relative to the camera are deduced, and then the robots communicate with each other to share the measurement information of the mutual observations and their own positioning information, and finally the relative poses between multiple robots are obtained through calculation.

[0041] The specific steps are as follows:

[0042] When two robots meet and observe each other, the coordinates of robot β recognized by robot α using the monocular camera are recorded as [x1, y1, z1] T, record the coordinates of robot α recognized by robot β using a monocular camera as [x2, y2, z2] T , then there is the azimuth angle θ of robot α in the coordinate system of robot α α and the azimuth angle θ of robot α in the coordinate system of robot β β are as follows:

[0043]

[0044] The two robots share the results of target recognition through ROS communication. Through geometric relationships, the heading angle θ of robot β relative to robot α αβ and the heading angle θ of robot α relative to robot β βα are respectively:

[0045] θ αβ = π + θ β - θ α , θ βα = π + θ α - θ β

[0046] In summary, through the mutual observation between robots, the relative pose transformation matrix between the two robots can be obtained as follows:

[0047]

[0048] where R(θ αβ ) is the rotation matrix of robot β relative to robot α, R(θ βα ) is the rotation matrix of robot α relative to robot β, T αβ is the relative pose matrix of robot β relative to robot α, and T βα is the relative pose matrix of robot α relative to robot β.

[0049] Step 3: According to the results of Step 2, construct a pose graph optimization problem with the key frame poses of a single robot as vertices and the mutual observations between robots and the pose estimations of the single-robot SLAM algorithm as measurement edges, and use the Gauss-Newton method to solve the optimization problem.

[0050] The specific steps are as follows:

[0051] As Figure 3 , Figure 4 shown, taking robot α and robot β as examples, at the k-th key frame moment, robot α and robot β meet, and the poses of robot α and robot β are respectively recorded in the following forms:

[0052]

[0053] Denote the observation of robot β by robot α, i.e., the relative pose of robot β with respect to robot α as:

[0054]

[0055] At the k-th key frame, the pose T β′ (k) of robot β can be calculated based on the relative pose of robot β with respect to robot α, i.e.:

[0056] T β′ (k) = T α (k)T αβ (k)

[0057] From the single-robot localization odometry result of robot β, the transformation matrix between the poses T β (k), T β (k + 1) of its two consecutive key frames is:

[0058]

[0059] Based on the relative pose of robot β with respect to robot α, the pose transformation matrix between two consecutive key frames of robot β can be deduced as:

[0060]

[0061] Calculate the deviation Δ between the pose transformation matrices of two adjacent key frames of robot β obtained from the single-robot localization odometry of robot β and the mutual observation of robot α, as follows:

[0062]

[0063] Then transform Δ into a residual vector:

[0064]

[0065] Therefore, the pose graph optimization problem can be reduced to minimizing the sum of the residual vectors:

[0066]

[0067] where x represents all the poses to be optimized in the pose graph.

[0068] Finally, use the Gauss-Newton method to solve the above nonlinear least squares problem, and the specific steps are as follows:

[0069] The first step is to give the initial value x0, the error threshold E = 10 -6 and the upper limit K = 200 of the number of iterations;

[0070] Step 2. For the k-th iteration, calculate the current Jacobian matrix J(x) and the error

[0071] Step 3. Solve the equation:

[0072] J(x) T f(x)+J(x) T J(x)Δ=0

[0073] to obtain the increment Δ that minimizes ||f(x + Δ)|| 2 ;

[0074] Step 4. If the error is less than E or exceeds K, stop and output the pose result of the k-th iteration; otherwise, return to Step 2.

[0075] The pose optimization result obtained in this step is denoted as T 1 .

[0076] Step 4. As Figure 5 shown, perform loop closure detection. Use the pose optimization result T 1 of Step 3 to perform frame-to-map matching between the current key frame point cloud of the robot and the historical map, and further optimize the pose.

[0077] The specific steps are as follows:

[0078] First, extract the point cloud data within 1 meter of the current position of the robot from the historical map. Specifically: Assume the current position of the robot is X i =(x i , y i , z i ) T . Calculate the Euclidean distance between the position of each historical frame point cloud corresponding to the robot and the current position of the robot. Let the position of the robot corresponding to the j-th frame point cloud be X j =(x j , y j , z j ) T . Then the distance between the position of the robot corresponding to the j-th frame point cloud and the current position of the robot is

[0079]

[0080] If d ij > 1, do not process the j-th frame point cloud data; if d ij < 1, the j-th frame point cloud data is the point cloud data within 1 meter of the current position of the robot, and downsample this frame of point cloud data to reduce the computational load.

[0081] Then, the RANSAC (Random Sample Consensus) algorithm is used for preliminary matching to obtain a preliminary pose estimate.

[0082] Next, based on the initial registration, the ICP (Iterative Closest Point) algorithm is used for fine registration. The transformation between the two point clouds is iteratively adjusted to minimize the distance between the matching point pairs. In each iteration, first, the closest point pairs of each point cloud are found, and then the pose transformation is calculated to minimize the distance between these point pairs. The iteration ends when the distance between the point pairs is less than the threshold or the number of iterations reaches 200 times. If the distance between the point pairs is less than the threshold at the end, a closed loop is detected; otherwise, no closed loop is detected.

[0083] After detecting a closed loop, the relative transformation T between the current pose and the historical pose at which the closed loop is detected is output. now_his The relative transformation T obtained from the closed-loop detection now_his is added as a new edge for graph optimization, and then the graph optimization problem is solved again to obtain a further optimized robot pose T. 2

[0084] Step 5: Construct a global point cloud map. Taking robot β as an example, using the pose optimization result T in Step 4 2 and the relative pose calculation result T in Step 2 ×α , the point cloud P shared by robot α neighbor is subjected to a pose transformation and transformed into the global coordinate system of robot β. The specific formula is:

[0085] T 2 ·P world = T βα ·P neighbor

[0086] where T βα is the relative pose matrix of robot α with respect to robot β, P world is the coordinate of the point cloud in the global coordinate system of robot β, and P neighbor is the coordinate of the point cloud in the coordinate system of robot α.

[0087] Then, map merging is performed incrementally, and the new point cloud data is gradually added to the existing global map. The above steps are continuously executed in a loop until all robots complete their tasks. Through continuous acquisition, transformation, alignment, and merging, a complete and accurate environmental map is finally obtained, thereby realizing map information sharing among multiple robots, and all robots can independently construct an accurate global multi-beam point cloud map.

[0088] Example 2:

[0089] ​Step 1: Use a formation of three robots to move in an indoor environment with sparse features. Each robot in the formation uses a low-cost single-line lidar, a monocular camera, and a wheel speedometer to perform the SLAM task. Each robot in the formation uses the gmapping algorithm for its initial positioning, and uses a particle filter to process the single-line lidar point cloud data and wheel speedometer data to achieve simultaneous localization and mapping, generating the pose corresponding to the point cloud key frame and the estimated key frame. The multi-robots communicate with each other through a WiFi local area network using the ROS master-slave communication mechanism. One of the robots is the ROS master, and the other robots are ROS slaves. When initializing, use the NTP service to synchronize the time of all robots.

[0090] For the convenience of subsequent processing, in this embodiment, for the collected original lidar point cloud data, the algorithm can remove invalid points by removing the point cloud data with abnormal intensity values and remove ground points by calculating geometric features, and correct motion distortion using motion compensation.

[0091] Step 2: When different robots meet, they use monocular cameras to observe each other. In this example, each robot is equipped with an easily recognizable AprilTag label graphic code. After the robot uses the camera to collect image data, it performs image target recognition to obtain the position and orientation of the observed graphic code relative to the camera, and then calculates the position and orientation of the observed robot relative to the camera. Then the robots communicate with each other to share the measurement information of mutual observation and their own localization information, and finally obtain the relative pose between the multi-robots through calculation.

[0092] The specific steps are as follows:

[0093] When two robots meet and observe each other, record the coordinates of robot β recognized by robot α using a monocular camera as [x1, y1, z1] T , and record the coordinates of robot α recognized by robot β using a monocular camera as [x2, y2, z2] T , then the azimuth angle θ1 of robot β in the coordinate system of robot α and the azimuth angle θ2 of robot α in the coordinate system of robot β are as follows:

[0094]

[0095] The two robots share the results of target recognition through ROS communication. Through geometric relationships, it can be calculated that the heading angle θ of robot β relative to robot α αβ and the heading angle θ of robot α relative to robot β βα are respectively:

[0096] θ αβ = π + θ β - θ α , θβα = π + θ α -θ β

[0097] In summary, through the mutual observation between robots, the relative pose transformation matrix between two robots can be obtained as follows:

[0098]

[0099] Step 3: According to the result of Step 2, construct a pose graph optimization problem with the single-robot key-frame poses as vertices and the mutual observations between robots and the pose estimations of the single-robot SLAM algorithm as measurement edges, and use the Levenberg-Marquardt method to solve the optimization problem.

[0100] The specific steps are as follows:

[0101] Taking robot α and robot β as examples, at the k-th key-frame moment, robot α and robot β meet, and the poses of robot α and robot β are respectively denoted in the following forms:

[0102]

[0103] Denote the observation of robot α on robot β as:

[0104]

[0105] At the k-th key-frame moment, the pose of robot β can be calculated based on the observation of robot α on robot β, that is:

[0106] T β′ (k) = T α (k)T αβ (k)

[0107] From the single-robot localization odometer result of robot β, it can be known that the transformation matrix between the poses T β (k), T β (k + 1) of two consecutive key frames is:

[0108]

[0109] Based on the observation of robot α on robot β, the pose transformation matrix between two consecutive key frames of robot β can be deduced as:

[0110]

[0111] Calculate the deviation Δ between the pose transformation matrix of two adjacent key frames of robot β obtained by the single-robot localization odometer of robot β and the measurement of the mutual observation between robot α and robot β, and there is:

[0112]

[0113] Then transform Δ into a residual vector:

[0114]

[0115] Therefore, the pose graph optimization problem can be reduced to minimizing the sum of the residual vectors:

[0116]

[0117] where x represents all the poses to be optimized in the pose graph. Finally, the Levenberg-Marquardt method is used to solve the above non-linear least squares problem. The specific steps of the Levenberg-Marquardt method are as follows:

[0118] In the first step, given the initial state x0, initialize the trust region radius μ, set the coefficient matrix D as the identity matrix I, set the threshold ∈ of ρ, and the algorithm convergence error threshold E = 10 -6 ;

[0119] In the second step, for the k-th iteration, add a trust region based on the Gauss-Newton method:

[0120]

[0121] In the third step, calculate the approximation index ρ:

[0122]

[0123] According to the empirical value setting, if ρ > 0.75, then set μ = 2μ and jump to the fourth step;

[0124] If ρ < 0.25, then set μ = 0.5μ and jump to the fourth step;

[0125] If ρ > ∈, it is considered approximately feasible, solve for the increment Δ, and let x k+1 = x k + Δ

[0126] In the fourth step, calculate f(x k+1 ), if it is less than the threshold E, stop the iteration; otherwise return to the second step.

[0127] The pose optimization result obtained in this step is denoted as T 1 .

[0128] Step 4: Use the method based on radar point cloud data matching for loop closure detection. Use the pose optimization result T 1, the current key - frame point cloud of the robot is matched with the historical map. The ICP (Iterative Closest Point) algorithm is used to register the two sets of point - cloud data, calculate the transformation matrix, and further optimize the pose.

[0129] The specific steps are as follows:

[0130] First, extract the point - cloud data within 1 meter of the robot's current position from the historical map. Specifically: Assume the robot's current position is X i =(x i ,y i ,z i ) T , calculate the Euclidean distance between the position of each historical - frame point cloud corresponding to the robot and the robot's current position. Let the position of the robot corresponding to the j - th frame of point - cloud data be X j =(x j ,y j ,z j ) T , then the distance between the position of the robot corresponding to the j - th frame of point - cloud data and the robot's current position is

[0131]

[0132] If d ij >1, then do not process the j - th frame of point cloud; if d ij <1, then the j - th frame of point - cloud data is the point - cloud data within 1 meter of the robot's current position. Then use the ORB (Oriented fast and Rotated Brief) feature - extraction algorithm to extract the feature points in the j - th frame of point - cloud data.

[0133] Then use the RANSAC (Random Sample Consensus) algorithm for preliminary matching to obtain a preliminary pose estimate.

[0134] Next, based on the initial registration, use the ICP (Iterative Closest Point) algorithm for fine registration, iteratively adjusting the transformation between the two point clouds to minimize the distance between the matching feature - point pairs. In each iteration, first find the nearest feature - point pairs of each point cloud, and then calculate the pose transformation to minimize the distance between these feature - point pairs. Iterate until the distance between the feature - point pairs is less than the preset threshold, then a closed - loop is detected; otherwise, no closed - loop is detected.

[0135] After detecting a closed - loop, output the relative transformation T now_his between the current pose and the historical pose where the closed - loop is detected. The relative transformation T obtained from the closed - loop detectionnow_his Add new edges optimized for the graph, and solve the graph optimization problem again to obtain a further optimized robot pose T 2 .

[0136] Step 5: Construct a global point cloud map. Taking robot β as an example, use the pose optimization result T of Step 4 2 and the relative pose calculation result T of Step 2 βα , and transform the point cloud P shared by robot α neighbor to the global coordinate system of robot β. The specific formula is:

[0137] T 2 ·P world = T βα ·P neighbor

[0138] where T βα is the relative pose matrix of robot α with respect to robot β, and P world is the coordinate of the point cloud in the global coordinate system of robot β, and P neighbor is the coordinate of the point cloud in the coordinate system of robot α.

[0139] Then incrementally perform map merging, and gradually add the point cloud data after coordinate transformation to the existing global map. The above steps are continuously looped until all robots complete their tasks. Through continuous acquisition, transformation, alignment, and merging, a complete and accurate environmental map is finally obtained, thereby realizing map information sharing among multiple robots, and all robots can independently construct an accurate global multi-beam point cloud map.

[0140] Embodiment 3:

[0141] Step 1: Use three robots to form a formation and move in an indoor environment with sparse features. Each robot in the formation uses a low-cost single-line lidar and a binocular camera to perform the SLAM task, and uses the Cartographer algorithm to generate point cloud key frames and estimate the poses corresponding to the key frames. Communication between multiple robots is carried out through a WiFi local area network using the ROS master-slave communication mechanism. One of the robots is used as the ROS master, and the other robots are used as ROS slaves. When initializing, the NTP service is used to synchronize the time of all robots.

[0142] For the convenience of subsequent processing, in this embodiment, for the collected original laser point cloud data, the algorithm removes invalid points by removing point cloud data with abnormal intensity values and removes ground points by calculating geometric features, and corrects motion distortion using motion compensation.

[0143] Step 2: When different robots meet, they use binocular cameras to observe each other. Use yolo-v8 to identify the position and orientation of the observed robot relative to the camera. Then the robots communicate with each other to share the measurement information of mutual observation and their own positioning information. Finally, the relative poses between multiple robots are obtained through calculation.

[0144] The specific steps are as follows:

[0145] When two robots meet and use yolo-v8 to identify each other, record the coordinates of robot β identified by robot α using the binocular camera as [x1, y1, z1] T , and record the coordinates of robot α identified by robot β using the binocular camera as [x2, y2, z2] T , then the azimuth angles of robot β in the coordinate system of robot α and robot α in the coordinate system of robot β are as follows:

[0146]

[0147] The two robots share the results of target recognition through ROS communication. Through geometric relationships, the heading angle θ of robot β relative to robot α αβ and the heading angle θ of robot α relative to robot β βα are respectively:

[0148] θ αβ = π + θ β -θ α , θ βα = π + θ α -θ β

[0149] In summary, through the mutual observation between robots, the relative pose transformation matrix between the two robots can be obtained as follows:

[0150]

[0151] Step 3: According to the results of Step 2, construct a pose graph optimization problem with the key frame poses of single robots as vertices and the mutual observations between robots and the pose estimations of the single-robot SLAM algorithm as measurement edges, and use the gradient descent method to solve the optimization problem.

[0152] The specific steps are as follows:

[0153] Taking robot α and robot β as examples, at the k-th key frame moment, robot α and robot β meet, and record the poses of robot α and robot β in the following forms:

[0154]

[0155] Denote the observation of robot β by robot α as:

[0156]

[0157] At the k-th key frame, the pose of robot β can be calculated based on the observation of robot β by robot α, that is:

[0158] T β′ (k) = T α (k)T αβ (k)

[0159] From the single-robot localization odometer result of robot β, it can be known that the transformation matrix between the poses T β (k), T β (k + 1) of two consecutive key frames is:

[0160]

[0161] According to the observation of robot β by robot α, the pose transformation matrix between two consecutive key frames of robot β can be deduced as:

[0162]

[0163] Calculate the deviation Δ between the pose transformation matrix of two adjacent key frames of robot β obtained from the single-robot localization odometer of robot β and the mutual observation of robot α, as follows:

[0164]

[0165] Then convert Δ into a residual vector:

[0166]

[0167] Therefore, the pose graph optimization problem can be reduced to minimizing the sum of the residual vectors:

[0168]

[0169] where x represents all the poses to be optimized in the pose graph.

[0170] Finally, use the Gauss-Newton method to solve the above non-linear least squares problem. The specific steps are as follows:

[0171] Step 1, given the initial value x0, the error threshold E and the upper limit K of the number of iterations;

[0172] Step 2, for the k-th iteration, calculate the current Jacobian matrix J(x) and the error

[0173] Step 3, solve the equation:

[0174] J(x) T f(x) + J(x) T J(x)Δ = 0

[0175] Obtain the increment Δ that minimizes ||f(x + Δ)|| 2 ; the minimum increment Δ

[0176] In the fourth step, if the error is less than E or exceeds K, stop and output the pose result of the k-th iteration; otherwise, return to the second step.

[0177] The pose optimization result obtained in this step is denoted as T 1 .

[0178] Step 4: Use the method based on pose estimation for loop closure detection. The specific steps are as follows:

[0179] First, based on the current robot pose T 1 , quickly find the historical robot poses close to the current robot pose through the nearest neighbor search algorithm. These poses are the candidate loop poses of the robot.

[0180] Calculate the distance between the pose T 1 and the candidate historical robot poses. Specifically: Assume the robot position corresponding to the current pose is X i =(x i , y i , z i ) T , calculate the Euclidean distance between the position of the robot corresponding to each candidate historical robot pose and the position of the robot corresponding to the current pose. Let the position of the robot corresponding to the j-th candidate pose be X j =(x j , y j , z j ) T , then the distance between the position of the robot corresponding to the j-th candidate pose and the current position of the robot is

[0181]

[0182] If d ij < 0.01, it is considered that there is a loop between the current pose and the candidate historical robot pose.

[0183] After detecting the loop, output the relative transformation T now_his between the current pose and the historical pose detected by the loop closure. Add the relative transformation T now_his detected by the loop closure as a new edge of the graph optimization, and then solve the graph optimization problem again to obtain the further optimized robot pose T 2 .

[0184] Step 5: Construct a global point cloud map. Taking robot β as an example, use the pose optimization result T of Step 4 2 and the relative pose calculation result T of Step 2 βα to perform a pose transformation on the point cloud P shared by robot α neighbor and transform it to the global coordinate system of robot β. The specific formula is:

[0185] T 2 ·P world =T βα ·P neighbor

[0186] where T βα is the relative pose matrix of robot α with respect to robot β, and P world is the coordinate of the point cloud in the global coordinate system of robot β, and P neighbor is the coordinate of the point cloud in the coordinate system of robot α. Then incrementally perform map merging, gradually adding the new point cloud data to the existing global map. The above steps are continuously looped until all robots complete their tasks. Through continuous acquisition, transformation, alignment, and merging, a complete and accurate environmental map is finally obtained, thereby realizing map information sharing among multiple robots, and all robots can independently construct an accurate global multi-beam point cloud map.

[0187] Although the disclosed embodiments of the present invention are as above, the described content is only an embodiment adopted for facilitating the understanding of the present invention and is not intended to limit the present invention. Any person skilled in the art within the scope of the present invention can make any modifications and changes in the form and details of the implementation without departing from the spirit and scope disclosed by the present invention. However, the scope of patent protection of the present invention shall still be subject to the scope defined by the appended claims.

Claims

1. A low-cost collaborative mapping method based on multi-robot mutual observation, characterized in that: Applied to multi-robot formations, each robot in the formation is equipped with a sensor combination for mutual visual recognition and positioning and ranging. The robots communicate with each other through a wireless local area network, including the following steps: Step 1: Each robot in the formation performs its own positioning and builds a local map in real time. The local map consists of key frame poses; Step 2: When different robots in the formation meet, they use their own sensors to observe each other, and calculate the relative pose information of the observed robot relative to itself, and share the relative pose information with other robots in the formation; Step 3: Each robot in the formation uses its own key frame pose in step 1 and the relative pose obtained in step 2 to calculate the relative pose information of the observed robot relative to itself ...4: Each robot in the formation uses its own key frame pose in step 1 and the relative pose obtained in step 2 to calculate the relative pose information of the observed robot relative to itself; Step 5: Each robot in the formation uses its own key frame pose in step 1 and the relative pose obtained in step 2 to calculate the relative pose information of the observed robot relative to itself. Taking the pose information as a constraint, a pose graph optimization model is constructed, and the nonlinear optimization method is used to solve the pose optimization result of each robot, and the key frame pose information in the local map is updated; Step 4: Using the pose optimization result of step 3, the key frame of the robot at the current moment and the local historical map are subjected to loop detection. If a loop is detected, the pose graph optimization model is constructed using the loop constraint information, the key frame pose is further optimized, and the local map is updated. If no loop is detected, the key frame pose in the map is kept unchanged; Step 5: The local maps of each robot in step 4 are merged to construct a global map, thereby realizing map information sharing among multiple robots.

2. A low-cost collaborative mapping method based on multi-robot mutual observation according to claim 1, characterized in that: In step 1, the robot performs self-positioning and mapping by using one of the Cartographer algorithm and the gmapping algorithm.

3. The low-cost collaborative mapping method based on multi-robot mutual observation according to claim 1 is characterized in that: Each robot in the formation is equipped with a sensor combination for mutual visual recognition, positioning and ranging, including a single-line lidar, a camera, and an IMU or wheel speed meter sensor.

4. The low-cost collaborative mapping method based on multi-robot mutual observation according to claim 1 is characterized in that: Multiple robots communicate with each other using the ROS master-slave communication mechanism, with one robot as the ROS master and the others as ROS slaves. During initialization, the NTP service is used to synchronize the time of all robots.

5. The low-cost collaborative mapping method based on multi-robot mutual observation according to claim 3 is characterized in that: When a monocular camera is used, an AprilTag label graphic code is set on each robot in the formation; in the step 2, the relative posture information of the observed robot relative to itself is calculated by observing and identifying the AprilTag label graphic code carried by the robot using a monocular camera.

6. The low-cost collaborative mapping method based on multi-robot mutual observation according to claim 3 is characterized in that: When a binocular camera is used, in step 2, the relative position information of the observed robot relative to itself is calculated, and the target detection algorithm neural network is used to perform target recognition on the image taken by the binocular camera.

7. The low-cost collaborative mapping method based on multi-robot mutual observation according to claim 1 is characterized in that: In step 3, the algorithms for solving the pose graph optimization model by the nonlinear optimization method include the Gauss-Newton method and the Levenberg-Marquardt method.

8. The low-cost collaborative mapping method based on multi-robot mutual observation according to claim 1 is characterized in that: In step 4, the method for detecting loop closure includes but is not limited to matching the current key frame point cloud with the local historical map, performing closed loop detection using a method based on radar point cloud data matching, and performing closed loop detection using a method based on pose estimation.

9. The low-cost collaborative mapping method based on multi-robot mutual observation according to claim 1 is characterized in that: In step 5, the method for constructing a global point cloud map includes transforming the point clouds shared by other robots and incrementally merging the maps.

Images