A pipeline environment SLAM method based on multi-robot collaboration

Abstract

The present invention relates to a pipeline environment SLAM method based on multi-robot collaboration, belonging to the field of robot simultaneous positioning and mapping technology. The present invention adopts a robot collaboration mode, forms two robots into a front and rear queue in a pipeline environment, designs a unique periodic peristalsis mode as a collaboration strategy between the two robots, and constructs a multi-robot posture constraint factor to constrain the relative posture of the robots, so that the robot queues can provide references to each other when traveling in the pipeline, and utilizes factor graph optimization to fuse information of the sensors of the two robots to achieve high-quality self-posture correction, thereby obtaining higher positioning accuracy. The present invention solves the positioning and three-dimensional reconstruction technology problems in special scenarios such as laser degradation and the absence of obvious feature points, overcomes the shortcomings of the existing SLAM method in pipeline environments due to the high similarity of the environment, and can complete SLAM tasks in spaces with high environmental similarity characteristics such as pipelines and tunnels.

Description

Technical Field

[0001] The present invention relates to a pipeline environment SLAM method based on multi-robot collaboration, belonging to the technical field of robot simultaneous localization and mapping (SLAM). Background Art

[0002] With the rapid development of SLAM technology, mainstream SLAM techniques currently rely on laser and vision fusion strategies. However, in specialized scenarios such as underground pipelines and tunnels, problems such as a lack of geometric structure, a lack of obvious feature points in the surrounding environment, laser radar degradation, loss of longitudinal constraints, and the absence of GPS signals within the pipeline make laser or vision SLAM difficult to apply. For example, when a robot navigates a long, straight pipeline, the high structural similarity of the pipelines means that the laser radar measurements are consistent throughout the pipeline and there are no directional constraints, making it impossible for the robot to locate its own position. Similarly, in such highly similar scenarios, visual sensors struggle to acquire useful feature points or reference points, so relying on visual sensors can also lead to positioning failures. Existing research has proposed two solutions to this degradation problem caused by high environmental similarity. One approach involves determining the direction and degree of laser degradation and filtering or compensating for the degraded information during the fusion process, thereby alleviating the positioning error caused by this factor. However, due to the difficulty of accurately filtering or compensating for degraded information in real-world environments, the effectiveness of these methods falls short of meeting practical requirements. Another approach is to primarily integrate a high-precision differential odometer and inertial navigation sensor (IMU), supplemented by other sensors. To ensure that positioning results are not sensitive to the surrounding environment, photoelectric speed sensor odometers or inertial measurement units (IMUs) are typically used for dead reckoning. However, in pipeline environments, tire slippage is common due to the influence of the conveying medium and the pipeline walls, causing a sharp increase in system errors. Furthermore, for a single robot, due to the lack of a reference, the cumulative error will gradually increase as the system runs longer, causing the robot's estimated position to exceed the pipeline's constraints, resulting in low overall robustness and reliability. Clearly, there is currently a lack of a very effective solution to the SLAM problem in pipeline environments. Summary of the Invention

[0003] The technical problem to be solved by the present invention is to provide a pipeline environment SLAM method based on multi-robot collaboration, which can effectively complete SLAM tasks in spaces with high environmental similarity characteristics such as pipelines and tunnels, overcoming the shortcomings of the existing SLAM method in pipeline environments due to the high similarity of the environment.

[0004] The technical solution of the present invention is: a pipeline environment SLAM method based on multi-robot collaboration, the specific steps are:

[0005] Step 1: In a pipeline environment, two robots are arranged into a front-to-back queue. The front robot is named Leader, and the back robot is named Follower. The specific configuration of the robot queue is as follows:

[0006] Step 1.1: The leader and followers communicate using broadcasting to exchange data information.

[0007] Step 1.2: Each robot is equipped with a high-precision wheel odometer and inertial navigation sensor to perform basic positioning of its own movement;

[0008] Step 1.3: The leader uses an ultrasonic sensor to observe the follower and obtain the relative distance between the leader and the follower. The leader receives the observation value and sends it to the follower in real time.

[0009] Step 1.4: Considering that the inlet and outlet locations of the pipeline may receive GPS signals, both robots are equipped with GPS receivers to further reduce the cumulative error by using the GPS signals at the inlet and outlet locations.

[0010] Step 1.5: To achieve 3D reconstruction of the pipeline environment, the Leader is equipped with a 16-line LiDAR, which uses its own fused pose to perform point cloud projection and complete the 3D reconstruction of the pipeline.

[0011] Step 2: The robot fleet uses a periodic peristaltic collaborative strategy to move along the pipeline axis. Each peristaltic cycle is divided into two stages: extension and contraction.

[0012] Stage 1 is the stretching stage, in which the leader is the mover and the follower is the reference. The leader moves forward to the maximum stretching distance D. max After stopping, the Follower remains stationary;

[0013] Stage 2 is the contraction stage. At this time, the leader role is converted to the reference role, and the follower role is converted to the active role. The leader is stationary, and the follower moves toward the leader to the minimum contraction distance D. min ;

[0014] In order to ensure the reliability of communication between the two robots during movement and reduce the impact of unexpected interference on the formation, the maximum reach distance D needs to be adjusted according to the actual hardware parameters. max Adjust and limit, especially when it is at the bend of the pipeline, D max Need to be reduced appropriately.

[0015] Step 3: Use factor graph optimization to fuse the multi-sensor information on the Leader and Follower robots to obtain the robot pose and complete the three-dimensional reconstruction of the pipeline. The overall factor graph is composed of two factor graphs, ares1 and ares2, which represent the factor graphs of the two robots, Leader (blue) and Follower (green), respectively. Each creeping collaboration cycle corresponds to a local factor graph, which includes the extension phase and the contraction phase. In the entire creeping collaboration cycle, first, the wheel odometer and IMU data of the actor are fused to obtain its estimated pose. Then, the relative translation between the estimated pose of the actor and the pose of the reference is calculated. And combined with the ultrasonic observation value to form the ultrasonic range factor. Finally, the ultrasonic range factor is used as the multi-robot posture constraint factor and added to the factor graph of the actor for information fusion. The estimated posture of the actor is corrected, while the reference actor remains motionless and its posture does not need to be corrected. In this way, in a collaborative cycle, since the Leader and Follower alternately become the actor and the reference actor, both the Leader and the Follower have the opportunity to correct their own posture to obtain higher positioning accuracy. The multi-line lidar carried by the robot Leader uses the fused accurate posture information to perform point cloud projection to generate a three-dimensional map of the pipeline.

[0016] In the Step 2, the ultrasonic sensor is deployed at the rear end of the Leader and points to the Follower, and is used to observe the Follower to obtain the relative distance between the Leader and the Follower. However, during the implementation of the periodic peristaltic collaboration strategy, the robot queue does not always move in a straight line. When the Leader changes its direction of movement, the Follower may exceed its measurement range, resulting in no ultrasonic measurement signal or measuring other objects, making the ultrasonic observation unreliable, thereby increasing the system error and causing positioning errors. In order to solve this problem, first, a pan-tilt system is installed for the Leader's ultrasonic sensor, and then the pan-tilt system is rotated and controlled by solving the deflection angle of the Follower in the ultrasonic pan-tilt system coordinate system, so that the detection direction of the ultrasonic sensor always points to the center point of the Follower to ensure the reliability of the ultrasonic observation. The specific steps of the solution are as follows:

[0017] Step 2.1.1: Set the world coordinate system to {W}-O W X W Y W , the ultrasonic PTZ coordinate system on Leader is {U}-O U X U Y U , the transformation matrix of {U} relative to {W} is The homogeneous coordinates of the Follower's two-dimensional plane position under {W} are W P F =[x w y w 1] T ;

[0018] Then the homogeneous coordinates of Follower's position under {U} are U P F =[x U y U 1] r It can be obtained by formula (1);

[0019]

[0020] Step 2.1.2: Assume that the position relationship of Follower relative to the {U} coordinate system on Leader is:

[0021]

[0022] Step 2.1.3: Define clockwise rotation as positive and counterclockwise rotation as negative. When correcting the ultrasonic detection direction, the angle θ that the gimbal needs to rotate can be determined by formula (3):

[0023]

[0024] In Step 2, in a complex environment, when the Leader changes direction as the navigator, in order to prevent the Follower from losing track and causing the robot queue to be disrupted, it is necessary to solve the Follower's following problem. The specific steps are as follows:

[0025] Step 2.2.1: Define the circular area around the leader as the leader's following domain. From the previous creeping collaboration strategy, we know that the radius of the leader's following domain is D. min , the follower's following problem is essentially to determine the speed and direction of the follower at each moment, so that the follower can quickly reach the edge of the leader's following domain. Assume that at time t, the two-dimensional plane position coordinates of the leader and follower under {W} are and Then the speed of the Follower center at time t is Calculated by formula (4):

[0026]

[0027] Where k p is a positive proportionality coefficient, indicating It is proportional to the distance between the leader and the follower, and can be selected through experiments based on the robot's motion parameters. When the distance between the leader and the follower is far, The value is larger, as the distance between Leader and Follower decreases, The value gradually decreases. When the Follower reaches the edge of the Leader's follow-up area, A value of 0 can prevent collisions between robots.

[0028] Step 2.2.2: When the Follower moves toward the Leader, it needs to continuously adjust the magnitude and direction of its angular velocity to correct the direction of movement so that the Follower can continue to approach the Leader. γ represents the angle between the line connecting the Follower and the Leader and the horizontal coordinate of the world coordinate system {W}, and β represents the angle between the forward direction of the Follower and the horizontal coordinate of the world coordinate system {W}. At time t, the angular velocity of the Follower is It can be calculated by formula (5):

[0029]

[0030] Where kω is a positive proportional coefficient, indicating It is proportional to the deviation angle (γ-β) from the Follower to the Leader and can be selected through experiments based on the robot motion parameters. When the deviation angle from the Follower to the Leader is large, The larger the value, the smaller the deviation angle between the Follower and the Leader. The value gradually decreases. When the Follower points to the Leader, the deviation angle is 0. The value is 0;

[0031] Step 2.2.3: and By inputting the wheeled robot's motion equation, we can obtain the motion control parameters of the Follower robot's left and right wheel motors at each moment, and control the Follower to reach the destination at the expected speed and direction.

[0032] In the Step 3, the factors involved are mainly four factors: wheel odometer factor, IMU pre-integration factor, GPS factor, multi-robot posture constraint factor (ultrasonic range factor). Among them, the multi-robot posture constraint factor is used to constrain the posture between the active and the reference, and is a key factor for the present invention to achieve high-quality robot posture correction. The main idea is to solve the relative translation between the estimated posture of the active and the posture of the reference, and combine it with the relative distance observation value between them to form a multi-robot posture constraint factor. Considering that when robots collaborate, the relative distance between robots is affected by factors such as communication, and is affected by D max Limitations. For this type of short-distance relative position measurement, ultrasonic sensors offer the advantages of being small and inexpensive. While maintaining high measurement accuracy, they are also insensitive to ambient light and electromagnetic fields. Therefore, an ultrasonic sensor mounted on the Leader is used to obtain the relative distance between the active and reference objects, forming a multi-robot pose constraint factor, called the ultrasonic range factor, as follows:

[0033] Assuming that at a certain time t, the relative translation between the active person and the reference person is obtained by solving d t , the ultrasonic observation value is r t , then the optimization objective function of the ultrasonic range factor at time t is shown in formula (6):

[0034] f range =argmin|d t -r t | (6)

[0035] The ultrasonic range factor is inserted into the actor factor graph at the key frame, and the actor's posture is continuously corrected with the reference actor as the reference point through factor graph optimization.

[0036] In Step 3, the specific steps for factor graph optimization are as follows (taking Leader as an example, the same applies to Follower):

[0037] Step 3.1.1: In the factor graph solution, maximize the product of all factors to obtain the state with the highest probability, as shown in formulas (7) and (8):

[0038]

[0039]

[0040] Step 3.1.2: Usually, the noise of the sensor conforms to the Gaussian distribution, so all factors are expressed as exponential models:

[0041]

[0042] Step 3.1.3: Assuming that the observation value of each sensor is Z, the error function of each sensor can be abstractly expressed as:

[0043]

[0044] Step 3.1.4: The solution of the factor graph can be converted into a nonlinear least squares problem for solution:

[0045]

[0046] In Step 3, when the factor graph works, the Odom frequency is used as the main working frequency to process each key frame. The specific steps are as follows:

[0047] Step 3.2.1: In the stretching phase, the leader is the mover and the follower is the reference. The leader moves forward and changes its position; the reference follower remains stationary and its position remains unchanged.

[0048] During this process, the Follower and Leader obtain their respective Odom current frame pose, current frame timestamp and previous frame timestamp. The pose information is used as a priori factor to construct their respective factor graphs. The current frame timestamp and the previous frame timestamp are used to perform time alignment processing on the sensor data. The reference Follower is in a stationary state and sends its buffered pose information to the active Leader. The active (Leader) uses the IMU data pre-integration between the current frame timestamp and the previous frame timestamp as a constraint to generate an IMU factor insertion factor graph. At the same time, the pose information released by the reference Follower is used as a reference point to solve the relative displacement between its own Odom pose and the reference Follower pose. The ultrasonic range factor is composed of the ultrasonic measurement value and the relative translation amount and inserted into its own factor graph for optimization and solution, and the final pose information is output.

[0049] Step 3.2.2: Similarly, in the contraction phase, the follower plays the role of the mover and the leader plays the role of the reference. The mover, the follower, moves forward and changes its position; the reference, the leader, remains stationary and its position remains unchanged.

[0050] During this process, the Follower and Leader obtain their respective Odom current frame pose, current frame timestamp, and previous frame timestamp. The pose information is used as a priori factors to construct their respective factor graphs. The current frame timestamp and previous frame timestamp are used to time-align the data of each sensor. The reference Leader is in a stationary state and sends its buffered own pose information and ultrasonic measurement values ​​to the active Follower. The active Follower uses the IMU data pre-integration between the current frame timestamp and the previous frame timestamp as a constraint to generate an IMU factor insertion factor graph. At the same time, using the pose information released by the reference Leader as a reference point, it solves the relative displacement between its own Odom pose and the reference Leader pose, and uses the ultrasonic measurement value and relative translation to form the ultrasonic range factor, which is inserted into its own factor graph for optimization and solution, and outputs the final pose information.

[0051] Step 3.2.3: When the robot receives the GPS signal at the inlet and outlet of the pipeline, the GPS signal is added as a special priori factor into the factor graph for fusion, thereby minimizing the cumulative error.

[0052] The beneficial effects of the present invention are:

[0053] 1. The present invention forms a front-to-back queue of two robots in a pipeline environment, designs a unique periodic peristalsis method as a cooperation strategy between the two robots, and constructs a multi-robot posture constraint factor (ultrasonic range factor) to constrain the relative posture of the robots, so that the robot queues can provide references to each other when moving in the pipeline and achieve high-quality self-posture correction, thereby obtaining higher positioning accuracy, overcoming the shortcomings of the existing SLAM method in pipeline environments due to the high similarity of the environment.

[0054] 2. The present invention is applicable to wheeled robots and can effectively complete SLAM tasks in spaces with high environmental similarity characteristics such as pipelines and tunnels.

[0055] 3. Due to the advantages of multi-robot collaboration, the present invention is not easily affected by the environment. In addition to its applicability in ordinary environments such as rooms, the present invention can achieve better performance and results in various pipeline environments with highly similar characteristics. Moreover, as the pipeline scale increases and the operation time increases, the present invention can still maintain a good SLAM effect, with the advantages of controllable cumulative error, high reliability, and strong robustness. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] Figure 1 is a schematic diagram of a multi-robot queue of the present invention;

[0057] Figure 2This is a schematic diagram of the robot queue periodic creeping collaboration strategy of the present invention;

[0058] Figure 3 It is a schematic diagram of the overall factor structure of the present invention;

[0059] Figure 4 is a local factor graph of the present invention;

[0060] Figure 5 It is a position relationship diagram of the ultrasonic pan-tilt coordinate system {U} of the present invention relative to the robot Follower;

[0061] Figure 6 It is a schematic diagram of the robot following model of the present invention;

[0062] Figure 7 This is a model diagram of the robot following control algorithm of the present invention;

[0063] Figure 8 It is a flow chart of the multi-sensor information fusion algorithm of the present invention;

[0064] Figure 9 is a schematic diagram of a room model of the present invention;

[0065] Figure 10 This is the SLAM test result of this method under the common environment of the present invention;

[0066] Figure 11 It is a simplified pipeline model diagram of the present invention;

[0067] Figure 12 is the LIO-SAM test result of the present invention;

[0068] Figure 13 It is the LIO-SAM error details of the present invention;

[0069] Figure 14 is the FAST-LIO2.0 test result of the present invention;

[0070] Figure 15 This is the SLAM test result of IMU fusion Odom strategy under the simple pipeline of the present invention;

[0071] Figure 16 This is the SLAM test result of this method under the simple pipeline of the present invention;

[0072] Figure 17 This is a graph showing the SLAM test results of the method and IMU fusion Odom strategy in a complex urban pipeline environment of the present invention;

[0073] Figure 18 It is the EVO evaluation result of the present invention;

[0074] Figure 19 This is a comparison chart of the SLAM results of the present invention and the error analysis of the IMU fusion Odom strategy. DETAILED DESCRIPTION

[0075] The technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. In the absence of conflict, the embodiments in this application and the features in the embodiments can be combined with each other. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0076] It should be noted that if the embodiments of the present invention involve directional indications (such as up, down, left, right, front, back, etc.), the directional indications are only used to explain the relative position relationship, movement status, etc. between the various components under a certain specific posture (as shown in the accompanying drawings). If the specific posture changes, the directional indications will also change accordingly.

[0077] In addition, if there are descriptions involving "first", "second", etc. in the embodiments of the present invention, the descriptions of "first", "second", etc. are only for descriptive purposes and cannot be understood as indicating or suggesting their relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" and "second" may explicitly or implicitly include at least one of such features. In addition, the meaning of "and / or" appearing throughout the text includes three parallel schemes. Taking "A and / or B" as an example, it includes scheme A, or scheme B, or schemes in which A and B are satisfied at the same time. In addition, "multiple" refers to more than two. In addition, the technical solutions between the various embodiments can be combined with each other, but it must be based on the ability of ordinary technicians in this field to implement. When the combination of technical solutions is mutually contradictory or cannot be implemented, it should be deemed that such a combination of technical solutions does not exist.

[0078] Example 1: A pipeline environment SLAM method based on multi-robot collaboration, the specific steps are as follows:

[0079] Step 1: In the pipeline environment, two robots are arranged into a front-to-back queue. The front robot is named Leader and the back robot is named Follower. Figure 1 As shown, the specific configuration of the robot queue is as follows:

[0080] Step 1.1: The Leader and Follower communicate using broadcasting to exchange data information.

[0081] Step 1.2: Each robot is equipped with a high-precision wheel odometer and inertial navigation sensor to perform basic positioning of its own movement.

[0082] Step 1.3: The leader is equipped with an ultrasonic sensor to observe the follower and obtain the observation value of the relative distance between the leader and the follower. The leader receives the observation value and sends it to the follower in real time.

[0083] Step 1.4: Considering that the inlet and outlet locations of the pipeline may receive GPS signals, both robots are equipped with GPS receivers. The GPS signals at the inlet and outlet locations can be used to further reduce the cumulative error.

[0084] Step 1.5: To achieve 3D reconstruction of the pipeline environment, the Leader is equipped with a 16-line laser radar that uses its own fused posture to perform point cloud projection and complete the 3D reconstruction of the pipeline.

[0085] Step 2: The robot fleet uses a periodic peristaltic cooperative strategy to move along the pipeline axis. Each peristaltic cycle is divided into two stages: expansion and contraction. For ease of description, the previous behavior is used as an example, as shown in the attached figure. Figure 2 Stage 1 is the stretching stage, where the leader is the mover and the follower is the reference. The leader moves forward to the maximum stretch distance D. max Then it stops and the Follower remains motionless. Stage 2 is the contraction stage, at which the Leader role is converted to the reference role and the Follower role is converted to the active role. The Leader remains motionless and the Follower moves towards the Leader to the minimum contraction distance D. min In order to ensure the reliability of communication between the two robots during movement and reduce the impact of unexpected interference on the formation, the maximum extension distance D needs to be adjusted according to the actual hardware parameters. max Adjust and limit, especially when it is at the bend of the pipeline, D max Need to be reduced appropriately.

[0086] Step 3: Use factor graph optimization to fuse the multi-sensor information on the Leader and Follower robots, obtain the robot pose and complete the pipeline 3D reconstruction. The overall factor graph consists of two factor graphs ares1 and ares2, representing the factor graphs of the two robots Leader (blue) and Follower (green), respectively, as shown in the attached figure. Figure 3 Each peristaltic collaboration cycle corresponds to a local factor graph, which includes two parts: the expansion phase and the contraction phase, as shown in the attached figure. Figure 4As shown in the figure, during the entire peristaltic collaboration cycle, the wheel odometry and IMU data of the active robot are first fused to obtain its estimated pose. Then, the relative translation between the active robot's estimated pose and the reference robot's pose is calculated and combined with the ultrasonic observations to form the ultrasonic range factor. Finally, the ultrasonic range factor is added to the active robot's factor graph as a multi-robot pose constraint factor for information fusion, correcting the active robot's estimated pose. The reference robot remains stationary and its pose does not require correction. Thus, during a collaboration cycle, as the leader and follower alternately serve as the active and reference robots, both have the opportunity to correct their own poses, achieving higher positioning accuracy. The multi-line lidar on the leader robot uses the fused accurate pose information to perform point cloud projection, generating a three-dimensional map of the pipeline.

[0087] In the Step 2, the ultrasonic sensor is deployed at the rear end of the Leader and points to the Follower, and is used to observe the Follower to obtain the relative distance between the Leader and the Follower. However, during the implementation of the periodic peristaltic collaboration strategy, the robot queue does not always move in a straight line. When the Leader changes its direction of movement, the Follower may exceed its measurement range, resulting in no ultrasonic measurement signal or measuring other objects, making the ultrasonic observation unreliable, thereby increasing the system error and causing positioning errors. In order to solve this problem, first, a pan-tilt system is installed for the Leader's ultrasonic sensor. Then, the pan-tilt system is rotated and controlled by solving the deflection angle of the Follower in the ultrasonic pan-tilt system coordinate system, so that the detection direction of the ultrasonic sensor always points to the center point of the Follower, ensuring the reliability of the ultrasonic observation. The specific steps of the solution are as follows:

[0088] Step 2.1.1: Set the world coordinate system to {W}-O W X W Y W , the ultrasonic PTZ coordinate system on Leader is {U}-O U X U Y U , the transformation matrix of {U} relative to {W} is The homogeneous coordinates of the Follower's two-dimensional plane position under {W} are W P F =[x w y w 1] T .

[0089] Then the homogeneous coordinates of Follower's position under {U} are U P F =[xU y U 1] T It can be obtained by formula (1).

[0090]

[0091] Step 2.1.2: Assume that the position relationship of Follower relative to Leader's {U} coordinate system is as shown in the attached figure. Figure 5 As shown in the figure, α can be calculated by formula (2).

[0092]

[0093] Step 2.1.3: Define clockwise rotation as positive and counterclockwise rotation as negative. When correcting the ultrasonic detection direction, the angle θ that the gimbal needs to rotate can be determined by formula (3).

[0094]

[0095] In Step 2, in a complex environment, when the Leader changes direction as the navigator, in order to prevent the Follower from losing track and causing the robot queue to be disrupted, the Follower's following problem needs to be solved. The specific steps are:

[0096] Step 2.2.1: Define the circular area around the leader as the leader's following domain. From the previous creeping collaboration strategy, we know that the radius of the leader's following domain is D. min The essence of the follower's following problem is to determine the speed and direction of the follower at each moment, so that the follower can quickly reach the edge of the leader's following domain. Assume that at time t, the two-dimensional plane position coordinates of the leader and follower under {W} are and Then the speed of the Follower center at time t is Calculated by formula (4).

[0097]

[0098] Where k p is a positive proportionality coefficient, indicating It is proportional to the distance between the leader and the follower and can be selected through experiments based on the robot motion parameters. The value is larger; as the distance between Leader and Follower decreases, The value gradually decreases; when the Follower reaches the edge of the Leader's follow domain, A value of 0 can avoid collisions between robots.

[0099] Step 2.2.2: When the Follower moves towards the Leader, it needs to continuously adjust the magnitude and direction of the angular velocity to correct the direction of movement so that the Follower can continue to get closer to the Leader. Figure 6 As shown in the figure, γ represents the angle between the line connecting the Follower and the Leader and the horizontal coordinate of the world coordinate system {W}, and β represents the angle between the forward direction of the Follower and the horizontal coordinate of the world coordinate system {W}. At time t, the angular velocity of the Follower is It can be calculated by formula (5).

[0100]

[0101] Where kω is a positive proportional coefficient, indicating It is proportional to the deviation angle (γ-β) from the Follower to the Leader and can be selected through experiments based on the robot motion parameters. When the deviation angle from the Follower to the Leader is large, The value is larger; as the deviation angle from the Follower to the Leader decreases, The value gradually decreases; when the Follower points to the Leader, the deviation angle is 0. The value is 0.

[0102] Step 2.2.3: and By inputting the wheeled robot motion equation, we can obtain the motion control parameters of the left and right wheel motors of the Follower robot at each moment, and control the Follower to reach the destination at the expected speed and direction. The control algorithm model is shown in the attached figure. Figure 7 shown.

[0103] In Step 3, the factors involved are mainly the wheel odometer factor, the IMU pre-integration factor, the GPS factor, and the multi-robot posture constraint factor (ultrasonic range factor). The details are as follows:

[0104] (1) The wheel odometry factor is used as the odom prior factor of the two robots for factor graph optimization. The wheel odometry contains information on both position (position and attitude) and velocity (forward speed and angular velocity), which is obtained by a high-precision photoelectric encoder.

[0105] (2) Introducing IMU pre-integration can avoid the occurrence of repeated integration problems. The IMU pre-integration factor is obtained by integrating the IMU measurements between adjacent keyframes. It is inserted into the factor graph as a constraint between two keyframes and optimized together with the odom prior factor.

[0106] (3) The GPS factor is obtained by the GPS receiver at the inlet and outlet of the pipeline and is inserted into the key frame at the inlet and outlet of the pipeline as a special priori factor to be jointly optimized with other factors.

[0107] (4) The multi-robot posture constraint factor is used to constrain the posture between the active and reference objects, and is a key factor in achieving high-quality robot posture correction in the present invention. The main idea is to solve the relative translation between the estimated posture of the active and reference objects, and combine it with the relative distance observation value between them to form the multi-robot posture constraint factor. Considering that when robots collaborate, the relative distance between robots is affected by factors such as communication, and is affected by D max Limitations. For this type of short-distance relative position measurement, ultrasonic sensors offer the advantages of small size and low cost. While maintaining high measurement accuracy, they are also insensitive to ambient light and electromagnetic fields. Therefore, an ultrasonic sensor mounted on the Leader is used to obtain the relative distance between the active and reference objects, forming a multi-robot pose constraint factor, called the ultrasonic range factor. Its calculation method is as follows:

[0108] Assuming that at a certain time t, the relative translation between the active person and the reference person is obtained by solving d t , the ultrasonic observation value is r t , then the optimization objective function of the ultrasonic range factor at time t is shown in formula (6):

[0109] f range =argmin|d t -r t | (6)

[0110] The ultrasonic range factor is inserted into the actor factor graph at the key frame, and the actor's posture is continuously corrected with the reference actor as the reference point through factor graph optimization.

[0111] In Step 3, the specific steps of factor graph optimization solution (taking Leader as an example, the same applies to Follower) are as follows:

[0112] Step 3.1.1: In the factor graph solution, maximize the product of all factors to obtain the state with the highest probability, as shown in formula (7) (8):

[0113]

[0114]

[0115] Step 3.1.2: Usually, the noise of the sensor conforms to the Gaussian distribution, so all factors are expressed as exponential models:

[0116]

[0117] Step 3.1.3: Assuming that the observation value of each sensor is Z, the error function of each sensor can be abstractly expressed as:

[0118]

[0119] Step 3.1.4: The solution of the factor graph can be converted into a nonlinear least squares problem for solution:

[0120]

[0121] In Step 3, when the factor graph works, the Odom frequency is used as the main working frequency. The algorithm flow for processing each key frame is as shown in the attached figure. Figure 8 The specific steps are as follows:

[0122] Step 3.2.1: During the extension phase, the Leader acts as the active element, and the Follower acts as the reference element. The active element (Leader) moves forward, changing its position; the reference element (Follower) remains stationary, maintaining its position. During this process, the Follower and Leader obtain their respective Odom current frame position, current frame timestamp, and previous frame timestamp. The pose information is used as a priori factor to construct their respective factor graphs, and the current frame timestamp and previous frame timestamp are used to time-align the sensor data. The reference element (Follower) is stationary and sends its buffered pose information to the active element (Leader). The leader uses the IMU data pre-integration between the current frame timestamp and the previous frame timestamp as a constraint to generate the IMU factor insertion factor graph. At the same time, it uses the posture information released by the follower as the reference point to solve the relative displacement between its own Odom posture and the follower posture. The ultrasonic range factor composed of the ultrasonic measurement value and the relative translation is inserted into its own factor graph for optimization and solution, and the final posture information is output.

[0123] Step 3.2.2: Similarly, in the contraction phase, the role of the Follower is the active one, and the role of the Leader is the reference one. The active one (Follower) moves forward and its posture changes; the reference one (Leader) remains stationary and its posture remains unchanged. During this process, the Follower and Leader obtain their respective Odom current frame posture, current frame timestamp, and previous frame timestamp. The posture information is used as a priori factor to construct their respective factor graphs, and the current frame timestamp and previous frame timestamp are used to perform time alignment processing on the sensor data. The reference one (Leader) is in a stationary state and sends its buffered posture information and ultrasonic measurement values ​​to the active one (Follower). The follower uses the IMU data pre-integration between the current frame timestamp and the previous frame timestamp as a constraint to generate the IMU factor insertion factor graph. At the same time, it uses the posture information released by the leader as a reference point to solve the relative displacement between its own Odom posture and the leader's posture. The ultrasonic range factor composed of the ultrasonic measurement value and the relative translation is inserted into its own factor graph for optimization and solution, and the final posture information is output.

[0124] Step 3.2.3: When the robot receives the GPS signal at the inlet and outlet of the pipeline, the GPS signal is added as a special priori factor into the factor graph for fusion, thereby minimizing the cumulative error.

[0125] In order to further illustrate the beneficial effects of the method of the present invention, this embodiment was experimentally analyzed through computer simulation. The experimental environment is ubuntu20.04 and ROS (Noetic) system. The robot has a cylindrical body with a diameter of 150mm and a height of 70mm. The robot tire has a diameter of 70mm and a width of 30mm. Compared with the Follower, the Leader additionally publishes ultrasonic topics and lidar topics for fusion and mapping, and the sensor data is recorded under Gazebo simulation conditions. The experiment consists of four parts. The first part is a SLAM test in a normal environment to verify the applicability of this method to a normal environment. The second part is a SLAM test comparison in an ordinary pipeline environment. The third part is a SLAM test in a complex urban pipeline environment. Verify the applicability of this method to larger-scale and more complex urban pipeline environments. The fourth part is the EVO evaluation analysis. Using the SLAM algorithm evaluation tool EVO, the SLAM results of this method in a complex urban pipeline environment are quantitatively evaluated and analyzed. The details are as follows:

[0126] (1) SLAM test in normal environment

[0127] The general environment is a room with many objects with obvious geometric features. The room model is as shown in the attached figure. Figure 9 shown.

[0128] The two robots are arranged into a queue as described above, and the environmental data in the room are collected according to the periodic creeping collaborative strategy. In order to simulate the fusion effect in real situations, a certain amount of Gaussian noise is added to the sensors of the leader and follower respectively, and the robot wheels are simulated to slip in corners and walls. The effect of this method on multi-robot collaborative SLAM in a normal environment is shown in the attached figure. Figure 10 As shown. Figure 10 As can be seen from the figure, this method can successfully complete the positioning of the room itself and the 3D reconstruction of the room environment under normal conditions. It has a high degree of restoration of the normal environment and reflects the room's geometric features and depth information. This result verifies that this method can complete SLAM tasks in normal environments and has applicability.

[0129] (2) Comparison of SLAM tests in common pipeline environments

[0130] For the sake of representativeness, the common pipeline environment is a pipeline with a long straight pipe, a four-way pipe, and a 90-degree elbow, which includes a variety of typical pipeline structures. The model is drawn in SoildWorks and converted into a URDF model, which is then imported into the Gazebo simulation environment for testing. The pipeline model is shown in the attached figure. Figure 11 shown.

[0131] First, the feasibility of the current mainstream LIO-SAM method was tested in a simple pipeline. After changing the configuration file and performing relevant calibration, the test results are shown in the attached figure. Figure 12 After entering the pipeline, the LIO-SAM system issues a warning that there are not enough feature points, and a system error occurs after running for a period of time. The specific content is as shown in the attached Figure 13 As shown in the following figure, the error message indicates that the error is caused by insufficient variable constraints. Mathematically, the system is in an underdetermined state. Figure 12 , Attachment Figure 13 The following conclusions can be drawn: the laser matching in LIO-SAM is not applicable to laser-degraded scenarios such as long corridors or pipelines. In addition, since there are no obvious geometric features in the pipeline, the results of the lidar scan are consistent everywhere, resulting in the robot being unable to determine its own position and causing the SLAM task to fail.

[0132] Then, a feasibility test of FAST-LIO2.0 was conducted under a simple pipeline. FAST-LIO2.0 is more complete than FAST-LIO. On its basis, an incremental kd-Tree (ikd-Tree) is proposed, which enables kd-Tree to incrementally delete or add point clouds. Thanks to ikd-Tree, FAST-LIO2.0 no longer extracts edge features and plane features like LOAM, but directly aligns each 3D point with the map. Therefore, it can run more stably in some scenarios where manual feature extraction is difficult. The specific test results are shown in the attached figure. Figure 14 During the operation of FAST-LIO2.0, the attached Figure 14 As can be seen from (a), compared with LIO-SAM, the system can reconstruct part of the map but the restoration degree is not high, the effect is extremely poor, and as the running time increases, the point cloud map begins to distort. Figure 14 (b) shows an alarm indicating no valid points until the system stops running. Although FAST-LIO2.0 can run stably in some environments where manual feature extraction is difficult, it is not well suited for environments with high similarity and almost no geometric features, such as pipelines, especially long straight pipelines. FAST-LIO2.0 may fail the SLAM task due to insufficient feature points.

[0133] For single-robot SLAM, we control it to enter the pipeline equipped with IMU and odometer to extract environmental information. The simulation method is the same as that in normal environment. Gaussian noise is added and wheel slippage is simulated at the bend. Using traditional methods, we integrate odom and IMU strategies and optimize the factor graph to form the point cloud projection as shown in the attached figure. Figure 15 shown.

[0134] The simulation strategy of this invention is the same as that in ordinary environments. After adding Gaussian noise to the sensor, the simulation is carried out to simulate the tire slipping situation at the corner. Unlike a single robot, the leader and follower constantly correct their own postures by the posture of each other in a collaborative cycle. The final fusion effect is shown in the attached figure. Figure 16As shown. Thanks to the advantage of the two robots constantly correcting their own postures through each other during the collaboration cycle, this method can operate stably in ordinary pipeline environments and achieve good results in completing SLAM tasks. Especially in locations such as bends where the robot wheels are simulated to slip, the error caused by the slipping of a single robot's wheel odometer can be effectively corrected, and there are no problems such as map ghosting and excessive noise points. After the three-dimensional reconstruction of the pipeline model, the pipeline structure is highly restored, and the direction and depth information of the pipeline are clearly visible. The test results show that this method benefits from the advantages of multi-robot collaboration. The two robots can alternately use each other as a reference during the creeping collaboration cycle to perform high-quality corrections on their own postures, significantly reducing the error impact of the robot's wheel odometer slipping, allowing the lidar to perform point cloud projection in the correct posture. The results do not show problems such as map ghosting and excessive noise points, and the robot posture is more accurate.

[0135] SLAM testing in a real urban simulated pipeline environment

[0136] The pipeline model used in the experiment comes from a city gas pipeline distribution map. After modeling the local area through Soildworks, it is converted into a urdf model and imported into the Gazebo simulation environment. The pipeline model is as shown in the attached figure. Figure 17 As shown in (a). The pipeline model parameters are as follows:

[0137] Pipeline type: long straight pipe (2260m), tee (1), cross (2), multi-angle elbow (9)

[0138] Total length of pipeline: about 4000m

[0139] Pipe inner diameter: 1016mm

[0140] In the SLAM test results, the effect of using IMU fusion Odom strategy for a single robot is shown in the attached Figure 17 (b) shows the effect of this method after fusion. Figure 17 (c) is shown. Figure 17 As can be seen from the results of (b), in the SLAM results of a single robot using the IMU fusion Odom strategy for a real city simulation pipeline, there are a lot of noise points and three map ghosting in the 3D map, which leads to serious deformation of the local results. Figure 17 In (c), the SLAM results of this method are free of noticeable noise and map ghosting, and the overall result is superior to the single-robot SLAM results. It also surpasses the IMU-fused Odom strategy in both robustness and reliability. Test results show that as the pipeline scale and runtime increase, this method maintains good SLAM performance, demonstrating advantages such as controllable cumulative error, high confidence, and strong robustness.

[0141] EVO Review Analysis

[0142] The SLAM evaluation tool EVO was used to perform error analysis on the TUM format trajectory data saved during the SLAM test of this method in a complex urban pipeline environment. The compared trajectory is the final Leader fusion output result, and the data is in TUM format. The SLAM evaluation tool EVO was used to analyze the results. The trajectory intuitive diagram and error information intuitive diagram are shown in the attached figure. Figure 18 shown.

[0143] After EVO evaluation, it can be seen that Figure 18 (a) is a direct comparison of the trajectory. The true value and the fusion effect are basically consistent. From the direct information of the trajectory error in (b) and the absolute trajectory error histogram in (d), we can see that the maximum error of the SLAM method in the pipeline is 0.713m and the average error is 0.047m. After analysis, the maximum error occurs at the corner with a larger arc. The wheel may have slipped, but by comparison Figure 18 From the pipeline 3D map, there is no map distortion or ghosting at the bend. Compared with the pipeline inner diameter of 1016mm and the total length of 4000m, the average error is within a reasonable range. Figure 17 As can be seen from the pipeline restoration degree in (c), this method can excellently complete the SLAM task in the real urban simulated pipeline with high pipeline restoration degree and small error, and can overcome the map ghosting problem caused by tire slippage at turns.

[0144] Comparing the IMU fusion Odom strategy, the results after unified comparison using the EVO evaluation tool are shown in the attached Figure 19 The results show that the maximum error of the IMU fusion Odom strategy is about 1.54m, compared with the Figure 17 Judging from the SLAM result diagram in (b), the map ghosting problem occurs at the bend and there are a lot of noise points. The reason is that the tire may slip at the bend, resulting in this situation. The maximum absolute error is more than 2.2 times that of this method, and the average value is about 2.5 times that of this method.

[0145] The above describes the specific embodiments of the present invention in detail with reference to the accompanying drawings. However, the present invention is not limited to the above embodiments. Various changes can be made within the knowledge of ordinary technicians in this field without departing from the scope of the present invention.

Claims

1. A pipeline environment SLAM method based on multi-robot collaboration, characterized by: Step 1: In a pipeline environment, two robots are arranged into a front-to-back queue. The front robot is named Leader, and the back robot is named Follower. The specific configuration of the robot queue is as follows: Step 1.1: The leader and followers communicate using broadcasting to exchange data information. Step 1.2: Each robot is equipped with a high-precision wheel odometer and inertial navigation sensor to perform basic positioning of its own movement; Step 1.3: The leader uses an ultrasonic sensor to observe the follower and obtain the relative distance between the leader and the follower. The leader receives the observation value and sends it to the follower in real time. Step 1.4: Both robots are equipped with GPS receivers to use GPS signals at the entry and exit locations to further reduce the cumulative error; Step 1.5: The Leader is equipped with a 16-line laser radar, which uses its own fused pose to perform point cloud projection and complete the 3D reconstruction of the pipeline. Step 2: The robot fleet uses a periodic peristaltic collaborative strategy to move along the pipeline axis. Each peristaltic cycle is divided into two stages: extension and contraction. Stage 1 is the stretching stage, in which the leader is the mover and the follower is the reference. The leader moves forward to the maximum stretching distance D. max After stopping, Follower remains stationary; Stage 2 is the contraction stage. At this time, the leader role is converted to the reference role, and the follower role is converted to the active role. The leader is stationary, and the follower moves toward the leader to the minimum contraction distance D. min ; Step 3: Factor graph optimization is used to fuse the multi-sensor information on the leader and follower robots to obtain the robot pose and complete the three-dimensional reconstruction of the pipeline. The overall factor graph consists of two factor graphs, ares1 and ares2, which represent the factor graphs of the two robots, the leader and follower, respectively. Each creeping collaboration cycle corresponds to a local factor graph, which includes two parts: the extension phase and the contraction phase. During the entire creeping collaboration cycle, first, the wheel odometer and IMU data of the active robot are fused to obtain its estimated pose. Then, the relative translation between the estimated pose of the active robot and the pose of the reference robot is calculated and combined with the ultrasonic observation value to form the ultrasonic range factor. Finally, the ultrasonic range factor is used as a multi-robot pose constraint factor and added to the factor graph of the active robot for information fusion. The estimated pose of the active robot is corrected, while the reference robot remains stationary and its pose does not need to be corrected. The multi-line lidar carried by the leader robot uses the fused accurate pose information to perform point cloud projection to generate a three-dimensional map of the pipeline.

2. The pipeline environment SLAM method based on multi-robot collaboration according to claim 1 is characterized in that: In Step 2, first, a gimbal is installed for the Leader's ultrasonic sensor. Then, the gimbal is rotated by calculating the Follower's deflection angle in the ultrasonic gimbal coordinate system, so that the ultrasonic sensor detection direction always points to the Follower's center point, ensuring the reliability of ultrasonic observation. The specific steps of the calculation are as follows: Step 2.1.1: Set the world coordinate system to {W}-O W X W Y W , the ultrasonic PTZ coordinate system on Leader is {U}-O u X U Y U , the transformation matrix of {U} relative to {W} is The homogeneous coordinates of the Follower's two-dimensional plane position under {W} are W P F =[x w y w 1] T ; Then the homogeneous coordinates of Follower's position under {U} are U P F =[x U y U 1] T It can be obtained by formula (1); Step 2.1.2: Assume that the position relationship of Follower relative to the {U} coordinate system on Leader is: Step 2.1.3: Define clockwise rotation as positive and counterclockwise rotation as negative. When correcting the ultrasonic detection direction, the angle θ that the gimbal needs to rotate can be determined by formula (3):

3. The pipeline environment SLAM method based on multi-robot collaboration according to claim 1 is characterized in that: In Step 2, in a complex environment, when the Leader changes direction as the navigator, in order to prevent the Follower from losing track and causing the robot queue to be disrupted, it is necessary to solve the Follower's following problem. The specific steps are as follows: Step 2.2.1: Define the circular area around the leader as the leader's following domain. From the previous creeping collaboration strategy, we know that the radius of the leader's following domain is D. min , let the two-dimensional plane position coordinates of Leader and Follower under {W} at time t be and Then the speed of the Follower center at time t is Calculated by formula (4): Where k p is a positive proportionality coefficient, indicating Proportional to the distance between the leader and the follower. When the distance between the leader and the follower is far, The value is larger, as the distance between Leader and Follower decreases, The value gradually decreases. When the Follower reaches the edge of the Leader's follow-up area, The value is 0; Step 2.2.2: When the Follower moves toward the Leader, it needs to continuously adjust the magnitude and direction of its angular velocity to correct the direction of movement so that the Follower can continue to approach the Leader. γ represents the angle between the line connecting the Follower and the Leader and the horizontal coordinate of the world coordinate system {W}, and β represents the angle between the forward direction of the Follower and the horizontal coordinate of the world coordinate system {W}. At time t, the angular velocity of the Follower is It can be calculated by formula (5): Where k ω is a positive proportionality coefficient, indicating It is proportional to the deviation angle (γ-β) from the Follower to the Leader. When the deviation angle from the Follower to the Leader is large, The larger the value, the smaller the deviation angle between the Follower and the Leader. The value gradually decreases. When the Follower points to the Leader, the deviation angle is 0. The value is 0; Step 2.2.3: and By inputting the wheeled robot's motion equation, we can obtain the motion control parameters of the Follower robot's left and right wheel motors at each moment, and control the Follower to reach the destination at the expected speed and direction.

4. The pipeline environment SLAM method based on multi-robot collaboration according to claim 1, characterized in that: In Step 3, the ultrasonic sensor mounted on the Leader is used to obtain the relative distance observation value between the active and reference objects, which constitutes the multi-robot posture constraint factor, called the ultrasonic range factor, and its method is as follows: Assuming that at a certain time t, the relative translation between the active person and the reference person is obtained by solving d t , the ultrasonic observation value is r t , then the optimization objective function of the ultrasonic range factor at time t is shown in formula (6): f range =argmin|d t -r t | (6) The ultrasonic range factor is inserted into the actor factor graph at the key frame, and the actor's posture is continuously corrected with the reference actor as the reference point through factor graph optimization.

5. The pipeline environment SLAM method based on multi-robot collaboration according to claim 1, characterized in that: In Step 3, the specific steps of factor graph optimization solution are: Step 3.1.1: In the factor graph solution, maximize the product of all factors to obtain the state with the highest probability, as shown in formulas (7) and (8): Step 3.1.2: Express all factors as exponential models: Step 3.1.3: Assuming that the observation value of each sensor is Z, the error function of each sensor can be abstractly expressed as: Step 3.1.4: The solution of the factor graph is converted into a nonlinear least squares problem:

6. The pipeline environment SLAM method based on multi-robot collaboration according to claim 1, characterized in that: In Step 3, when the factor graph works, the Odom frequency is used as the main working frequency to process each key frame. The specific steps are as follows: Step 3.2.1: In the stretching phase, the leader is the mover and the follower is the reference. The leader moves forward and changes its position; the reference follower remains stationary and its position remains unchanged. During this process, the Follower and Leader obtain their respective Odom current frame pose, current frame timestamp and previous frame timestamp. The pose information is used as a priori factor to construct their respective factor graphs. The current frame timestamp and the previous frame timestamp are used to perform time alignment processing on the sensor data. The reference Follower is in a stationary state and sends its buffered pose information to the active Leader. The active (Leader) uses the IMU data pre-integration between the current frame timestamp and the previous frame timestamp as a constraint to generate an IMU factor insertion factor graph. At the same time, the pose information released by the reference Follower is used as a reference point to solve the relative displacement between its own Odom pose and the reference Follower pose. The ultrasonic range factor is composed of the ultrasonic measurement value and the relative translation amount and inserted into its own factor graph for optimization and solution, and the final pose information is output. Step 3.2.2: Similarly, in the contraction phase, the follower plays the role of the mover and the leader plays the role of the reference. The mover, the follower, moves forward and changes its position; the reference, the leader, remains stationary and its position remains unchanged. During this process, Follower and Leader obtain their own Odom current frame pose, current frame timestamp and previous frame timestamp, and the pose information is used as a priori factor to construct their own factor graphs. The current frame timestamp and the previous frame timestamp are used to perform time alignment processing on the sensor data. The reference leader is in a stationary state and sends its buffered own pose information and ultrasonic measurement value to the active follower. The active follower uses the IMU data pre-integration between the current frame timestamp and the previous frame timestamp as a constraint to generate an IMU factor insertion factor graph. At the same time, the pose information released by the reference leader is used as a reference point to solve the relative displacement between its own Odom pose and the reference leader pose. The ultrasonic measurement value and the relative translation are used to form the ultrasonic range factor, which is inserted into its own factor graph for optimization and solution, and the final pose information is output. Step 3.2.3: When the robot receives the GPS signal at the inlet and outlet of the pipeline, the GPS signal is added as a special priori factor into the factor graph for fusion, thereby minimizing the cumulative error.

Images