Low-cost cooperative localization and mapping method based on mutual observation of multiple robots

Abstract

The 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. The method is applied to multi-robot formation, each robot in the formation carries a low-cost single-line laser radar, a camera, an I MU or a wheel speedometer and other sensors, and the multiple robots communicate through a Wi-Fi local area network. Each robot in the robot formation is combined with point cloud data of the single-line laser radar, I MU attitude angle data and acceleration data to realize pose estimation and local map construction of the robot; then taking the self key frame pose of each robot as a vertex and the self poses of mutual observation between the robots and estimation of other robots as measurement edges, respectively constructing pose graph optimization models, then performing closed-loop detection by using graph optimization results, and further optimizing the pose of the single robot; on this basis, each robot performs pose transformation on point clouds shared by other observed robots, and then performs map merging incrementally, so that map information sharing among multiple robots is realized, and all robots can independently construct an accurate global multi-harness point cloud map.

Description

Technical Field

[0001] The present invention belongs to the technical field of intelligent navigation, and in particular relates to a low-cost collaborative positioning and mapping method based on mutual observation of multiple robots. Background Art

[0002] With the rapid development of autonomous robot technologies and their cross-research fields such as intelligent control, environmental perception and multi-sensor fusion, multi-robot systems are increasingly being used in various fields. Multi-robot collaborative positioning technology is a necessary prerequisite for realizing autonomous navigation of multi-robot systems. The commonly used positioning method relies on various sensors (such as cameras, lidar, etc.) carried by the robot to perceive the external environment and determine its own position, that is, simultaneous localization and mapping (SLAM). The current mainstream SLAM algorithms are mainly aimed at single robots, but in practical applications, single robots are often unable to perform some complex tasks, such as search and rescue, infrastructure inspection, home services, and logistics and transportation applications. Multi-robot collaboration is far more efficient than a single robot. For example, when exploring large-scale unknown environments, multi-robot collaborative SLAM solutions use multiple robots to collaboratively explore unknown areas, which can quickly collect information, increase exploration speed, and expand the scope of exploration.

[0003] At present, most multi-robot collaborative mapping methods rely on each robot to achieve its own positioning, and then use methods based on feature matching or position recognition to realize the relative pose calculation between multiple robots, optimize the pose, and realize map merging to build a globally consistent map. However, multi-robot collaborative mapping methods based on feature matching or position recognition still have many shortcomings. First, when this method faces scenes with high similarity in the environment, the error rate of position recognition is greatly increased, and the relative pose calculation results between robots are inaccurate; second, in an environment with sparse features, it is difficult for robots to realize encounter position recognition, thereby reducing the robustness of the algorithm; third, this type of method has high requirements for sensors, such as multi-line lidar, which will increase the cost of sensors.

[0004] In the prior art, sensors such as UWB (ultra-wideband) and WiFi are used to obtain relative distance information or azimuth information between robots. In this type of method, each robot first realizes its own positioning, and then uses the relative distance information or azimuth information to calculate the relative posture between robots, and finally realizes the merging of maps. However, this type of method that uses relative distance information or azimuth information between multiple robots also has some shortcomings. 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, but in actual applications, these conditions may not be met; on the other hand, these methods require robots to keep observing each other at all times. For example, the method using WiFi fingerprint information requires WiFi to be connected at all times, which affects the scalability and robustness of the system. Summary of the invention

[0005] The purpose of the present invention is: in view of the problems existing in the prior art such as inaccurate relative pose calculation, high deployment cost, unreliability in environments with sparse features, etc., a low-cost collaborative positioning and mapping method based on mutual observation of multiple robots is proposed, which solves the problems of collaborative optimization of poses and sharing of map information among multiple robots, and at the same time reduces the cost of deploying the required sensors on the robots.

[0006] Technical solution of the present invention: In order to achieve the above-mentioned purpose 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 a low-cost single-line laser radar, a camera, and an IMU (inertial measurement unit) or a wheel speed meter and other sensors. 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 with the attitude angle data and acceleration data of the IMU to realize its own pose estimation and local map construction; then, with each robot's own key frame pose as the vertex, and the mutual observation between robots and the estimated pose of other robots as the measurement edges, each robot constructs a pose graph optimization model, and then uses the graph optimization result for closed-loop detection to further optimize the pose of the single robot; on this basis, each robot transforms the pose of the point cloud shared by other observed robots, and then incrementally merges the maps to realize map information sharing among multiple robots. All robots can independently build accurate global multi-beam point cloud maps.

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

[0009] Step 1: Each robot in the formation locates itself and builds a local map in real time. The local map consists of key frames of sensors.

[0010] Step 2: When different robots in the formation meet, they use their own sensors to observe each other, calculate the relative position information of the observed robot relative to itself, and share the relative position information with other robots in the formation;

[0011] Step 3: Each robot in the formation uses its own key frame pose in step 1 and the relative pose information inferred in step 2 as constraints to construct a pose graph optimization model, and uses a nonlinear optimization method to solve the pose optimization result of each robot, and updates the key frame pose information in the local map;

[0012] Step 4: Use the pose optimization result of step 3 to perform loop detection on the robot's current key frame and local history map. If a loop is detected, use the loop constraint information to build a pose graph optimization model, further optimize the key frame pose, and update the local map. If no loop is detected, keep the key frame pose in the map unchanged.

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

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

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

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

[0017] In one possible embodiment, the ROS master-slave communication mechanism is used for communication, with one robot as the ROS (Robot Operating System) master and the other robots as ROS slaves. The ROS master publishes messages centrally to improve communication efficiency. The ROS master-slave architecture is easy to expand. When adding a new robot, it only needs to be configured as a slave without making large-scale changes to the existing system. During initialization, the NTP (Network Time Protocol) service is used to synchronize the time of all robots to ensure that each robot has a consistent time reference in the distributed system, thereby avoiding data mismatches caused by time inconsistency.

[0018] In a possible embodiment, in step 2, when a monocular camera is used, an AprilTag label graphic code is set on each robot in the formation; in step 2, the relative posture information of the observed robot relative to itself is calculated by using a monocular camera to observe and identify the AprilTag label graphic code carried by the robot.

[0019] In one possible embodiment, in step 2, when a binocular camera is used, in step 2, the relative posture information of the observed robot relative to itself is calculated, and the image taken by the binocular camera is used for target recognition using the yolo-v8 (target detection algorithm) neural network.

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

[0021] In one possible embodiment, in step 4, the method for detecting loop closure includes but is not limited to matching the current key frame point cloud with a 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.

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

[0023] Beneficial technical effects of the present invention:

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

[0025] First, when the multi-robot collaborative mapping method based on feature matching or position recognition faces scenes with high similarity in the environment, the error rate of position recognition is greatly increased, and the relative posture calculation results between the robots are inaccurate; the present invention does not have this limitation, and only needs to use cameras to observe each other when different robots meet to obtain the accurate relative posture between the robots.

[0026] Second, in the multi-robot collaborative mapping method based on feature matching or position recognition, it is difficult for robots to identify the encounter position in an environment with sparse features, which reduces the robustness of the algorithm; while the present invention uses a target recognition method to obtain the relative posture between robots, which is not affected by the environmental feature texture information.

[0027] Third, the existing methods for constructing global three-dimensional point cloud maps based on multi-robot collaborative SLAM generally require multi-line laser radars, which will lead to very high sensor costs, while the method using only low-cost single-line laser radars can only construct global two-dimensional grid maps. In the method described in the present invention, each robot in the robot formation uses a low-cost single-line laser radar, a camera, and an IMU (inertial measurement unit) or a wheel speed meter sensor combination. On this basis, the present invention realizes the sharing of map information among multiple robots, and all robots can independently construct accurate global multi-line point cloud maps, effectively reducing sensor costs. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0029] Figure 2 A flow chart of two robots observing each other when they meet in a preferred embodiment of the present invention;

[0030] Figure 3 A schematic diagram of two robots observing each other when they 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 closed-loop detection in a preferred embodiment of the present invention;

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

[0034] In order to make the purpose, technical solution and advantages of the present invention more clear, the embodiments of the present invention are described in detail below. It should be noted that the embodiments and features in the embodiments of the present application can be combined with each other arbitrarily without conflict.

[0035] The present invention proposes a low-cost collaborative mapping method based on mutual observation among multiple robots, which is applied to a multi-robot formation. Each robot in the formation is equipped with a low-cost single-line laser radar, a camera, and an IMU (inertial measurement unit) or a wheel speed meter and other sensors. The multiple robots communicate with each other through a WiFi local area network.

[0036] Each robot in the robot formation combines the point cloud data of the single-line lidar with the attitude angle data and acceleration data of the IMU to realize its own pose estimation and local map construction; then, with each robot's own key frame pose as the vertex, and the mutual observation between robots and the estimated pose of other robots as the measurement edges, each robot constructs a pose graph optimization model, and then uses the graph optimization result for closed-loop detection to further optimize the pose of the single robot; on this basis, each robot transforms the pose of the point cloud shared by other observed robots, and then incrementally merges the maps to realize map information sharing among multiple robots. All robots can independently build accurate global multi-beam point cloud maps.

[0037] Embodiment 1:

[0038] Step 1: Figure 1 As shown in the figure, three robots are used to form a formation to move in an indoor environment with sparse features. Each robot in the formation is equipped with a low-cost single-line laser radar, a monocular camera, and an IMU sensor to perform SLAM tasks. The Cartographer algorithm is used to generate point cloud keyframes and estimate the position and posture corresponding to the keyframes. Multiple robots communicate through the 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. During initialization, the NTP service is used to synchronize the time of all robots.

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

[0040] Step 2: Figure 2 As shown in the figure, when different robots meet, they use monocular cameras to observe each other. In this example, each robot is equipped with an easy-to-identify 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 shown in the figure. Figure 6 As shown, the position and direction of the observed robot relative to the camera are calculated, and then the robots communicate with each other to share the measurement information of each other's observations and their own positioning information. Finally, the relative posture between multiple robots is obtained by calculation.

[0041] The specific steps are:

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

[0043]

[0044] The two robots share the target recognition results through ROS communication. The heading angle θ of robot β relative to robot α can be calculated through geometric relationship. αβ The heading angle θ of robot α relative to robot β βα They are:

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

[0046] In summary, through mutual observation between robots, the relative posture 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 α, T βα is the relative pose matrix of robot α relative to robot β.

[0049] Step 3. Based on the results of step 2, a pose graph optimization problem is constructed with the key frame poses of the single robot as vertices, mutual observations between robots and the pose estimation of the single robot SLAM algorithm as measurement edges, and the Gauss-Newton method is used to solve the optimization problem.

[0050] The specific steps are:

[0051] like Figure 3 , Figure 4 As shown, taking robot α and robot β as an example, at the kth key frame, robot α and robot β meet, and the positions of robot α and robot β are recorded as follows:

[0052]

[0053] The observation of robot α on robot β, that is, the relative position of robot β relative to robot α, is recorded as:

[0054]

[0055] At the kth key frame, the position T of robot β can be calculated based on the relative position of robot β to robot α. β′ (k) namely:

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

[0057] From the single robot positioning odometer results of robot β, we can know the pose T of two consecutive key frames β (k),T β The transformation matrix between (k+1) is:

[0058]

[0059] According to the relative posture of robot β relative to robot α, the posture transformation matrix between two consecutive key frames of robot β can be calculated as:

[0060]

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

[0062]

[0063] Then Δ is converted 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, the Gauss-Newton method is used to solve the above nonlinear least squares problem. The specific steps are as follows:

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

[0070] The second step is to find the current Jacobian matrix J(x) and error for the kth iteration.

[0071] The third step is to solve the equation:

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

[0073] We get the value such that ||f(x+Δ)|| 2 The smallest increment Δ;

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

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

[0076] Step 4: Figure 5 As shown, loop closure detection is performed. Use the pose optimization result T of step 3 1 , match the robot’s current key frame point cloud with the historical map to further optimize the posture.

[0077] The specific steps are:

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

[0079]

[0080] If d ij >1, the j-th frame point cloud data is not processed; if d ij <1, the j-th frame of point cloud data is the point cloud data within 1 meter of the robot's current position, and the point cloud data of this frame is downsampled to reduce the amount of calculation.

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

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

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

[0084] Step 5: Build a global point cloud map. Taking robot β as an example, use the pose optimization result T in step 4 2 And the relative pose calculation result T of step 2 ×α , the point cloud P shared by robot α neighbor Perform posture transformation to the global coordinate system of robot β. The specific formula is:

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

[0086] Among them, T βα is the relative pose matrix of robot α relative to robot β, P world is the coordinate of the point cloud in the global coordinate system of robot β, P neighbor is the coordinate of the point cloud in the robot's α coordinate system.

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

[0088] Embodiment 2:

[0089] 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 laser radar, a monocular camera, and a wheel speed meter to perform SLAM tasks. Each robot in the formation uses the gmapping algorithm to perform its own preliminary positioning, and uses a particle filter to process the single-line laser radar point cloud data and wheel speed meter data to achieve simultaneous positioning and mapping, generate point cloud key frames and estimate the pose corresponding to the key frames. Multiple robots communicate through the WiFi local area network using the ROS master-slave communication mechanism, with one robot as the ROS master and the other robots as ROS slaves. During initialization, the NTP service is used to synchronize the time of all robots.

[0090] To facilitate subsequent processing, in this embodiment, for the collected original laser point cloud data, the algorithm can remove invalid points by removing point cloud data with abnormal intensity values, 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 direction of the observed graphic code relative to the camera, and then calculates the position and direction of the observed robot relative to the camera. Then the robots communicate with each other to share the measurement information of each other's observation and their own positioning information. Finally, the relative posture between multiple robots is obtained by calculation.

[0092] The specific steps are:

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

[0094]

[0095] The two robots share the target recognition results through ROS communication. The heading angle θ of robot β relative to robot α can be calculated through geometric relationship. αβ The heading angle θ of robot α relative to robot β βα They are:

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

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

[0098]

[0099] Step 3. Based on the results of step 2, a pose graph optimization problem is constructed with the key frame poses of the single robot as vertices, mutual observations between robots and the pose estimates of the single robot SLAM algorithm as measurement edges, and the Levenberg-Marquardt method is used to solve the optimization problem.

[0100] The specific steps are:

[0101] Taking robot α and robot β as an example, at the kth key frame, robot α and robot β meet, and the positions of robot α and robot β are recorded as follows:

[0102]

[0103] The observation of robot α on robot β is recorded as:

[0104]

[0105] At the kth key frame, the position and posture 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 positioning odometer results of robot β, we can know the pose T of two consecutive key frames β (k),T β The transformation matrix between (k+1) is:

[0108]

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

[0110]

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

[0112]

[0113] Then Δ is converted 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 nonlinear least squares problem. The specific steps of the Levenberg-Marquardt method are as follows:

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

[0119] In the second step, for the kth iteration, a trust region is added based on the Gauss-Newton method:

[0120]

[0121] The third step is to calculate the approximate index ρ:

[0122]

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

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

[0125] If ρ>∈, the approximation is considered feasible, and the increment Δ is solved, and x k+1 =x k +Δ

[0126] Step 4: 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 recorded as T 1 .

[0128] Step 4: Use the radar point cloud data matching method to perform closed loop detection. Use the pose optimization result T of step 3 1, match the robot's current key frame point cloud and historical map frame by frame, use the ICP (Iterative Closest Point) algorithm to align the two sets of point cloud data, calculate the transformation matrix, and further optimize the posture.

[0129] The specific steps are:

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

[0131]

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

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

[0134] Then, based on the initial registration, the ICP (Iterative Closest Point) algorithm is used for fine registration, and the transformation between the two point clouds is iteratively adjusted to minimize the distance between the matching feature point pairs. In each iteration, the closest feature point pair of each point cloud is first found, and then the pose transformation is calculated to minimize the distance between these feature point pairs. If the distance between the feature point pairs is less than the preset threshold, a closed loop is detected, otherwise no closed loop is detected.

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

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

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

[0138] Among them, T βα is the relative pose matrix of robot α relative to robot β, P world is the coordinate of the point cloud in the global coordinate system of robot β, P neighbor is the coordinate of the point cloud in the robot's α coordinate system.

[0139] Then, the map is merged incrementally, and the point cloud data after the coordinate transformation is gradually added to the existing global map. The above steps are executed in a continuous cycle until all robots complete their tasks. Through continuous collection, transformation, alignment, and merging, a complete and accurate environment map is finally obtained, thereby realizing the sharing of map information among multiple robots, and all robots can independently build an accurate global multi-harness 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 laser radar and a binocular camera to perform SLAM tasks, and uses the Cartographer algorithm to generate point cloud keyframes and estimate the poses corresponding to the keyframes. Multiple robots communicate through the WiFi local area network 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.

[0142] To facilitate 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, 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, and use yolo-v8 to identify the position and direction of the observed robot relative to the camera. Then the robots communicate with each other to share the measurement information of each other's observation and their own positioning information. Finally, the relative posture between multiple robots is calculated.

[0144] The specific steps are:

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

[0146]

[0147] The two robots share the target recognition results through ROS communication. The heading angle θ of robot β relative to robot α can be calculated through geometric relationship. αβ The heading angle θ of robot α relative to robot β βα They are:

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

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

[0150]

[0151] Step 3. Based on the results of step 2, a pose graph optimization problem is constructed with the key frame poses of the single robot as vertices, mutual observations between robots and the pose estimation of the single robot SLAM algorithm as measurement edges, and the gradient descent method is used to solve the optimization problem.

[0152] The specific steps are:

[0153] Taking robot α and robot β as an example, at the kth key frame, robot α and robot β meet, and the positions of robot α and robot β are recorded as follows:

[0154]

[0155] The observation of robot α on robot β is recorded as:

[0156]

[0157] At the kth key frame, the position and posture of robot β can be calculated based on the observation of robot α on robot β, that is:

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

[0159] From the single robot positioning odometer results of robot β, we can know the pose T of two consecutive key frames β (k),T β The transformation matrix between (k+1) is:

[0160]

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

[0162]

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

[0164]

[0165] Then Δ is converted 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, the Gauss-Newton method is used to solve the above nonlinear least squares problem. The specific steps are as follows:

[0171] The first step is to give the initial value x0, the error threshold E and the upper limit of the number of iterations K;

[0172] The second step is to find the current Jacobian matrix J(x) and error for the kth iteration.

[0173] The third step is to solve the equation:

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

[0175] We get the value such that ||f(x+Δ)|| 2 The smallest increment Δ;

[0176] Step 4: If the error is less than E or exceeds K, stop and output the pose result of the kth iteration; otherwise, return to step 2.

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

[0178] Step 4: Use the pose estimation-based method to perform closed-loop detection. The specific steps are as follows:

[0179] First, according to the current robot posture T 1 , through the nearest neighbor search algorithm, the historical robot poses close to the current robot pose are quickly found. These poses are the candidate robot loop poses.

[0180] Calculate the pose T 1 The distance between the candidate historical robot pose and the robot position is: Assuming that the robot position corresponding to the current pose is X i =(x i ,y i ,z i ) T , calculate the Euclidean distance between the robot position corresponding to each candidate historical robot posture and the robot position corresponding to the current posture, and let the robot position corresponding to the jth candidate posture be X j =(x j ,y j ,z j ) T , then the distance between the robot position corresponding to the jth candidate pose and the robot’s current position is

[0181]

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

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

[0184] Step 5: Build a global point cloud map. Taking robot β as an example, use the pose optimization result T in step 4 2 And the relative pose calculation result T of step 2 βα , the point cloud P shared by robot α neighbor Perform posture transformation to the global coordinate system of robot β. The specific formula is:

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

[0186] Among them, T βα is the relative pose matrix of robot α relative to robot β, P world is the coordinate of the point cloud in the global coordinate system of robot β, P neighbor is the coordinate of the point cloud in the robot's α coordinate system. Then the map is merged incrementally, and the new point cloud data is gradually added to the existing global map. The above steps are executed in a continuous loop until all robots complete their tasks. Through continuous acquisition, transformation, alignment, and merging, a complete and accurate environment map is finally obtained, thereby realizing the sharing of map information among multiple robots, and all robots can independently build an accurate global multi-harness point cloud map.

[0187] Although the embodiments disclosed in the present invention are as above, the contents described are only embodiments adopted to facilitate understanding of the present invention and are not intended to limit the present invention. Any technician in the field to which the present invention belongs can make any modifications and changes in the form and details of implementation without departing from the spirit and scope disclosed in the present invention, but the patent protection scope of the present invention shall still be subject to the scope defined in the attached 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