Heterogeneous multi-robot autonomous collaborative exploration control method and system

Abstract

The invention discloses a heterogeneous multi-robot autonomous collaborative exploration control method and system. The method comprises the following steps: firstly, initializing robots, acquiring point cloud data and depth images, preprocessing the point cloud data and the depth images, and carrying out scene mapping and robot positioning; secondly, selecting global target points and local target points according to the preprocessed point cloud data and the depth image, and performing type matching on the local target points according to a feature matching algorithm to obtain a local exploration path of the robot under the constraint of a global exploration path; and then according to the communication condition, path planning of interaction guidance among the heterogeneous multiple robots is carried out. And finally, according to the local target points distributed by the heterogeneous multiple robots and the preprocessed point cloud data, a deep reinforcement learning model is adopted to select an optimal path to the local target points, a control instruction is output, and control operation on the heterogeneous multiple robots is completed until all environment information is explored. According to the invention, efficient and accurate exploration of a large unknown complex environment is realized.

Description

Technical Field

[0001] The present invention relates to the technical field of artificial intelligence and autonomous exploration of robots, and specifically refers to a heterogeneous multi-robot autonomous collaborative exploration control method and system. Background Art

[0002] The technology of autonomous exploration of robots has attracted much attention in many fields such as military reconnaissance and disaster rescue. However, in the existing exploration tasks in complex and changeable environments, most of them only use a single type of multi-robot or a single robot for autonomous exploration and human-machine interaction exploration, but it is difficult to complete the tasks accurately and efficiently. This is because the exploration efficiency of a single robot is relatively low, and a single type of multi-robot has functional limitations. The heterogeneous multi-robot autonomous collaborative exploration system can significantly improve the exploration efficiency and reliability. For example, task allocation: adaptively allocate appropriate tasks to robots with corresponding functions according to sensor environment information. For example, in the exploration task, judging the generated multiple exploration point categories (plane, ladder, uneven road surface) according to sensor information helps to improve the exploration efficiency. In real-world complex scenarios such as earthquake rescue, it is of great significance to guide heterogeneous multi-robots to complete the task of autonomous exploration of complex terrains through adaptive dynamic task allocation. For example, allocating tasks with steps to quadruped robot dogs and flat ground to wheeled intelligent vehicles, etc. Summary of the Invention

[0003] In view of the deficiencies of the prior art, the present invention provides a heterogeneous multi-robot autonomous collaborative exploration control method and system. The system completes the overall collaborative control of heterogeneous multi-robots through multi-sensor data acquisition, point-to-point information communication and transmission, a top-level adaptive dynamic task allocation strategy, a middle-level intelligent selection of navigation and obstacle avoidance paths, and a bottom-level accurate output of control instructions. It can realize the efficient and accurate exploration of large unknown complex environments and can be applied to various scenarios such as military reconnaissance and earthquake rescue. To solve the above technical problems, the technical solution of the present invention is as follows: The present invention provides a heterogeneous multi-robot autonomous collaborative exploration method, which generates exploration points of each type according to an adaptive dynamic task allocation mechanism and dynamically allocates them to corresponding functional robots. Each robot intelligently selects a navigation and obstacle avoidance path through a deep reinforcement learning algorithm, and judges whether information transmission is beneficial to the overall exploration efficiency through exploration information, and optimizes the exploration path through independent decision-making to complete information exchange or maintain the original exploration plan, so as to realize the collaborative exploration task of unknown complex environments.

[0004] A heterogeneous multi-robot autonomous collaborative exploration control system includes an environmental data acquisition module, a dynamic task allocation module, a navigation and obstacle avoidance module, a pursuit and optimization module, a point-to-point communication and transmission module, and a heterogeneous multi-robot motion control module.

[0005] The environmental data acquisition module is used to collect depth images of the working area of each robot and lidar point cloud information , and at the same time, feature extraction is performed on the point cloud data and normalization processing is performed on the depth image to complete the preprocessing operation of the sensing data.

[0006] The dynamic task allocation module is used to, according to the lidar point cloud data after preprocessing and the depth map feature information, solve the points in the unexplored edge area within the global range and a series of viewpoints within the local range, and judge the category information of a series of viewpoints within the local range according to multiple sets of point cloud data of set types, and allocate the required target local viewpoints to be explored to heterogeneous multi-robots and plan a global exploration path and a local exploration path.

[0007] The point-to-point communication transmission module is used to directly assign different fixed static IP addresses to heterogeneous multi-robots, establish them under the same local area network, and at the same time share their own positioning information and the globally planned exploration path within a certain communication range.

[0008] The pursuit optimization module is used to, when communication is not possible, judge whether information transmission is beneficial to the overall exploration efficiency according to the time and exploration benefits of the globally planned exploration path and the pursuit information interaction path planned by the robot, and independently choose to maintain the original globally planned exploration path plan or change it to the pursuit information interaction path.

[0009] The navigation and obstacle avoidance module is used to obtain the assigned target local viewpoints and the lidar point cloud data after preprocessing information, analyze the surrounding environment of the heterogeneous multi-robots, and use a deep reinforcement learning model to generate the optimal navigation and obstacle avoidance path to the target local viewpoints and output the control instructions of the heterogeneous multi-robots.

[0010] The heterogeneous multi-robot motion control module is used to obtain the control instructions of the heterogeneous multi-robots output by the model. The differential intelligent vehicle receives the motor differential information, the Ackermann intelligent vehicle receives the front wheel steering information and the rear wheel drive information, and the quadruped robot dog receives the joint angle information of each motor.

[0011] The present invention also provides a control method for the heterogeneous multi-robot autonomous cooperative exploration system, including the following steps:

[0012] Step 1: Initialize each robot (robot i, robot j, robot k...) at different adjacent positions to ensure the normal operation of each function. At the same time, collect the sensing data of the depth camera, lidar, and inertial measurement unit, and obtain the point cloud data and depth image of this scene, and perform the preprocessing operation of normalization.

[0013] Step 2: The preprocessed point cloud data and the inertial navigation data obtained by the inertial measurement unit As the input of LIO-SAM (a simultaneous localization and mapping algorithm based on lidar inertial odometry), it is used for scene mapping and the positioning of each robot, and the stitching of multiple point cloud scene maps is performed.

[0014] Step 3: According to the preprocessed point cloud data and the depth map to extract the information of the edge area points in the global environment range as global target points, and based on the environmental information, a mathematical model of the gallery problem is constructed in the local area set around the robot to select a series of representative viewpoints as local target points.

[0015] Step 4: According to the feature matching algorithm, match the information of the selected local target points with multiple types of point cloud data that have been set, and dynamically allocate the local target points to the robots with corresponding functions. Then, solve the traveling salesman problem through the global target points to obtain the global exploration path of the robots, and then solve the traveling salesman problem with time windows to obtain the local exploration path of the robots under the constraint of the global exploration path.

[0016] Step 5: Based on the stitched point cloud scene map, communication and interaction between heterogeneous multi-robots are carried out through a wireless network card. In the case of communicability, the heterogeneous multi-robots share the positioning information output by the LIO-SAM algorithm and the planned global and local exploration paths.

[0017] Step 6: In the case of non-communicability, quantitatively analyze the time cost and exploration benefits of the global exploration path of the robots and the pursuit information interaction path, and dynamically select to maintain the original exploration strategy or switch to the path planning oriented by information interaction.

[0018] Step 7: According to the local target points assigned to the heterogeneous multi-robots and the preprocessed point cloud data , use a deep reinforcement learning model to intelligently select the optimal navigation and obstacle avoidance path to the local target points and output the control instructions for the heterogeneous multi-robots.

[0019] Step 8: The heterogeneous multi-robots obtain the corresponding control instructions from the model output and send them to the underlying drive controller to complete the control operation of the heterogeneous multi-robots.

[0020] Step 9: Repeat Step 1 to Step 8 until all environmental information has been explored and the robots return to the initial position and stop.

[0021] Preferably, in Step 2, the LIO-SAM algorithm uses the preprocessed point cloud data and the inertial navigation data to achieve high-precision positioning and mapping of the robots, and perform the stitching of multiple point cloud scene maps. The generation method is as follows:

[0022] Step 2.1: Input pre-processed LiDAR point cloud data With inertial navigation data .

[0023] Step 2.2: Using inertial navigation data Pre-integration estimates the motion during radar scanning, dedistorts the radar point cloud, uses voxel grid filtering to reduce the amount of calculation, and performs feature extraction of edge points and plane points to output a feature point cloud.

[0024] Step 2.3: Match the edge points and plane points of the feature point cloud with the features of the map, solve the relative pose of the current frame and the map by iterating the nearest point, and output the required factors in the factor graph.

[0025] Step 2.4: Construct and optimize the factor graph, use the Levenberg-Marquardt algorithm in the GTSAM library to solve the nonlinear least squares problem, and generate the optimized robot trajectory and state estimation.

[0026] Step 2.5: Add loop constraints to the factor graph, reoptimize the robot trajectory, and finally output a globally consistent trajectory and map.

[0027] Preferably, the generation of the global exploration target points in step 3 is accomplished by generating boundary points based on a topological map method.

[0028] Preferably, the method for solving a series of local viewpoints based on constructing a mathematical model of the gallery problem in step 3 adopts a genetic algorithm for model solving and viewpoint selection, and the specific steps are as follows:

[0029] Step 3.1. Calculate the convex hull of the global unexplored boundary point set through the Graham scanning algorithm (convex hull algorithm), decompose the non-convex area into multiple convex polygonal sub-areas by combining the ear cutting method, and use Boolean operations to eliminate obstacle areas. At the same time, use grid sampling and feature-based sampling methods in the polygonal area to generate a set of candidate viewpoints. Define the objective function as minimizing the number of viewpoints so that any point in the polygon can be "seen" by the selected viewpoint.

[0030] Step 3.2: Construct constraints to ensure that the selected viewpoints can cover the entire polygonal area. Use the octree to perform spatial segmentation on the polygonal area, optimize the ray detection algorithm to determine the visibility of the viewpoints and points in the area, and transform the occlusion judgment in the three-dimensional scene into a line segment intersection problem. At the same time, combine the RRT * (Rapid Exploration Random Tree) path planning algorithm to ensure that the distance between the selected viewpoints does not exceed the maximum movement distance of the robot and there is a feasible path.

[0031] Step 3.3: Encode the combinations of candidate viewpoints in the candidate viewpoint set as chromosomes. Use binary encoding to encode the candidate viewpoint combinations as chromosomes. Design a comprehensive fitness function that includes coverage and the number of viewpoints. Continuously evolve the population through roulette wheel selection, single-point crossover, and probabilistic mutation operations. Determine the termination conditions based on the maximum number of iterations and the convergence threshold, and obtain the optimal set of representative viewpoints, which is the set of local target points.

[0032] Preferably, the feature matching algorithm in Step 4 uses a point cloud feature matching algorithm that combines NDT + ICP (Normal Distribution Transform + Iterative Closest Point), with the local target point information and multiple sets of point cloud data of set types as inputs. The specific steps are as follows:

[0033] Step 4.1: Obtain the point cloud information within a certain range around the local target point, i.e., the source point cloud information, and read multiple sets of point cloud data of set types. Apply the voxel grid downsampling algorithm to reduce the data volume, retain geometric features while improving processing efficiency, and then perform normal estimation based on the downsampled source point cloud information. Calculate the normal vector by fitting a plane through searching for neighboring points, and extract FPFH (Fast Point Feature Histogram) features to generate a global feature vector, providing a geometric description for coarse registration.

[0034] Step 4.2: Based on the global feature vector, achieve coarse registration of the point cloud through the Normal Distribution Transform (NDT) algorithm, and approximate the source point cloud to multiple target type point clouds through iterative transformation. Use the Newton method to optimize the transformation parameters, and combine RANSAC (Random Sample Consensus algorithm) to provide an initial pose estimate to achieve coarse registration with meter-level accuracy.

[0035] Step 4.3: Use the NDT result as the initial value, and ICP iteratively searches for corresponding point pairs and solves the optimal transformation through SVD (Singular Value Decomposition). Adopt a gradually decreasing search strategy to adjust the registration threshold of the corresponding points from loose to strict to achieve centimeter-level fine registration.

[0036] Step 4.4: Dynamically adjust the NDT grid resolution and the ICP search radius, and iteratively achieve complementary optimization. Calculate the mean square error and the point cloud coverage rate to evaluate geometric consistency, and calculate the determinant of the transformation matrix to verify its legality. Use Monte Carlo sampling to test the result stability. After meeting the standards, output the final transformation matrix, and achieve point cloud feature matching by multiplying the transformation matrix with the source point cloud information.

[0037] Preferably, in Step 6, a hierarchical quantization calculation method is used to analyze the time cost and exploration benefit of the robot's global exploration path and the pursuit information interaction path. The calculation method is as follows:

[0038] First, based on the local exploration path and kinematic model of robot j (the pursued), predict the time it takes for it to reach each unexplored subspace, and introduce a penalty mechanism considering the time window constraint. Then, construct a spatio-temporal graph containing the current position of robot i (the pursuer) and the unexplored subspaces of robot j's path. By solving the traveling salesman problem with time window constraints and using the branch and bound algorithm, find the optimal path for robot i to visit part of robot j's global path from the current position and return. Then, calculate the cost of this path, including the motion cost and the time synchronization penalty term. Finally, compare the current global path cost of robot i with the pursuit cost plus the shared global path cost. If the latter is smaller, robot i executes the pursuit strategy, sends a request to robot j to obtain the path, generates and executes the optimal pursuit path, exchanges information at the meeting point, and updates the global path planning.

[0039] For the pursuit cost modeling and decision optimization, the meanings of the various parameters are as follows: i and j represent robot indices (i is the pursuer and j is the pursued), represents the k-th unexplored subspace (path node), represents the time to reach the unexplored subspace represents the moving speed of robot i, represents the subspace to Euclidean distance, represents the global path planning of robot j, represents the unexplored subspace effective access time window, is the left boundary of the time window, is the right boundary of the time window;

[0040] 1. Prediction of the time for robot j to reach the subspace:

[0041]

[0042]

[0043] 2. Penalty for time window constraint:

[0044]

[0045] 3. Total cost of the pursuit path:

[0046]

[0047] The first expression shows the prediction of the time for robot j to reach the unexplored subspace, and its initialization takes the current system time as the starting point , marked as robot j leaving the initial subspace​ At the time according to the moving speed of robot j Calculate from to The moving time . Iteratively calculate the moving time and accumulate it to the leaving time of the previous subspace. Finally, generate a sequence containing the arrival times of all subspaces . Among them represents the predicted time for robot j to reach the k-th subspace, the time when robot j leaves the (k - 1)-th subspace, k represents a dynamic variable, and m represents the total number of subspaces.

[0048] The second expression shows the mechanism for whether the time for the robot to reach each subspace meets the preset time range, and restricts the robot to complete the task according to the expected time by imposing a penalty term. The specific penalty calculation method is: when the time for robot j to reach subspace is not within the time window , calculate the penalty value according to the distance between and the boundary of the time window. is the penalty coefficient used to adjust the penalty intensity. The larger this coefficient, the more inclined the robot is to complete the task within the time window. Then select the closer distance to the left boundary or the right boundary of the time window. If the robot arrives within the time window, the penalty value is 0.

[0049] The third expression shows the quantitative index of the comprehensive cost paid during the process of chasing the target. Through multi-dimensional modeling, convert the path execution cost, time synchronization cost, and return cost multi-objectives into computable quantitative indexes. represents the total cost for robot i to chase robot j:

[0050] Path execution cost: The total cost consumed by robot i starting from the current position and sequentially visiting the subspaces in the path , among which represents the transfer cost from subspace (the P-th subspace in the path) to .

[0051] Time synchronization cost: The sum of the time differences between the time when robot i arrives at each subspace and the time when robot j arrives at the same subspace. represents the time synchronization penalty coefficient, respectively represent the predicted times for robot i and j to reach subspace .

[0052] Return cost: The cost for robot i to return to the starting position after completing the pursuit. Represents the return path weight coefficient, which controls the importance of the return cost. Represents the end subspace of the pursuit path. Then it is the starting position of the robot. Is the distance from the end point to the starting point.

[0053] Preferably, in step 7, the local target point and the preprocessed point cloud data The input deep reinforcement learning model uses a PPO network, which is specifically described as follows:

[0054] Input the local target point information position and the preprocessed lidar point cloud data around the robot , explore obstacle avoidance for long-term goals and select the optimal path. The reward is defined by multiple rewards simultaneously constrained:

[0055] The first reward Among them respectively represent the position and pose information of the robot, respectively represent the position information and pose information of the local target point to be explored, , and are all corresponding weight coefficients. This formula indicates that the policy encourages the robot to approach the target point in terms of direction and position. As the exploration progresses, the probability of the robot accurately reaching the target point becomes higher and higher.

[0056] The second reward Among them represents the distance between the robot and the nearest obstacle processed according to the interval division, represents the safety distance threshold, respectively represent the penalty coefficient, reward coefficient, exploration coefficient, and exploration progress time. This formula indicates that the policy encourages the robot's trajectory to deviate from the obstacle. As the exploration progresses, the probability of the robot avoiding obstacles also increases accordingly.

[0057] The third reward At the same time, certain constraints are also imposed on the set speed , speed and angular velocity . Among them respectively represent the speed reward coefficient, the angle between the robot's orientation and the target direction, the target speed penalty coefficient, the angular acceleration penalty coefficient, the angular acceleration, the angular velocity penalty coefficient, and the non-linear penalty exponent, indicating that the policy encourages the robot to move towards a more efficient and smooth motion.

[0058]

[0059]

[0060]

[0061] Preferably, the description of the control instruction received by the quadruped robot dog in the heterogeneous multi-robot in step 8 is as follows:

[0062] In complex scenarios, the quadruped robot dog processes visual information in real time (after denoising, enhancement, and distortion correction), inputs it into a lightweight visual encoder, fuses the extracted spatial features with the body perception data (pose, joint angles) through an attention mechanism, and then inputs them into a deep reinforcement learning architecture. Finally, through an inverse kinematics solver, the gait parameters are mapped into 12D foot-end motor control signals (3 joints per foot: hip joint, knee joint, ankle joint), sent to the underlying controller interface, and combined with Bezier curve trajectory planning and sole force feedback mechanism to achieve adaptive movement on complex terrains.

[0063] The present invention has the following characteristics and beneficial effects:

[0064] By adopting the above technical solution, an adaptive dynamic task allocation mechanism is used to analyze the unknown environment to achieve a rational allocation of the exploration targets required, and the characteristics of each heterogeneous robot are fully utilized. In the navigation and obstacle avoidance module, a deep reinforcement learning algorithm is used to complete the end-to-end navigation and obstacle avoidance function, and the pursuit optimization module is used to efficiently update the exploration path. Finally, the overall system is realized through the heterogeneous multi-robot motion control module. The system adopts a modular design with clear structural levels and has important theoretical value and practical significance. The present invention accurately realizes the collaborative autonomous exploration of heterogeneous multi-robots in unknown complex environments and can be applied to various scenarios such as earthquake rescue and military reconnaissance. BRIEF DESCRIPTION OF THE DRAWINGS

[0065] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for the description of the embodiments or the prior art. Obviously, the drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0066] Figure 1 It is a schematic flowchart of the method according to the embodiment of the present invention;

[0067] Figure 2 It is a photo of the radar point cloud information around the robot at the beginning;

[0068] Figure 3 It is a picture showing the communication mode between robots;

[0069] Figure 4It is a demonstration diagram for the simulation training of reinforcement learning of an intelligent vehicle;

[0070] Figure 5 It is a demonstration diagram for the simulation training of reinforcement learning of a quadruped robot dog;

[0071] Figure 6 It is a physical photo during the exploration of a robot;

[0072] Figure 7 It is a simulation photo during the exploration of a robot;

[0073] Figure 8 It is a photo of the global point cloud map during the exploration process. Detailed implementation manners

[0074] It should be noted that, without conflict, the embodiments in the present invention and the features in the embodiments can be combined with each other.

[0075] In order to make the objectives, technical solutions and advantages of the present invention clearer, the present invention will be further described in detail below with reference to the accompanying drawings.

[0076] On the contrary, the present invention covers any alternatives, modifications, equivalent methods and solutions made within the essence and scope of the present invention defined by the claims. Further, in order to enable the public to have a better understanding of the present invention, in the following detailed description of the present invention, some specific details are described in detail. Those skilled in the art can fully understand the present invention without the description of these details.

[0077] Embodiment 1

[0078] This embodiment provides a control method for a heterogeneous multi-robot autonomous collaborative exploration system. Please refer to Figure 1 . The method includes:

[0079] Step 1: Initialize each robot (robot i, robot j, robot k...) at different adjacent positions to ensure the normal operation of each function. At the same time, collect the sensing data of the depth camera, lidar and inertial measurement unit, and perform preprocessing operations of feature extraction on the point cloud data and normalization of the depth image. Please refer to Figure 2 Figure 2 which shows the point cloud data around the robot .

[0080] Step 2: After the preprocessing, the lidar point cloud data ​The attitude, velocity, acceleration and other data obtained by the inertial measurement unit are used as the input of the LIO-SAM algorithm. The LIO-SAM algorithm uses a tightly coupled method to deeply fuse lidar data and inertial measurement data. By optimizing the inter-frame matching of lidar and the pre-integration of the inertial measurement unit (IMU), high-precision scene mapping and robot self-localization are achieved. After completing the construction of a single scene map, based on the positioning information of each robot, a graph optimization algorithm is used to splice multiple scene maps, eliminate the errors between maps, and construct a complete and accurate global environment map, providing a reliable environment reference for the subsequent tasks of the robot. Please see Figure 8 For the generated global point cloud map photo, it can be seen that the interior consists of point cloud data of two different colors, which are explored and constructed by two different robots respectively.

[0081] Step 3, please see Figure 3 The green square in Figure 3 is the global target point, and the orange dots inside are a series of representative viewpoints in the local area. These points are based on the preprocessed point cloud data and depth map feature information. Through the topological graph algorithm, multiple boundary point information of the environment is extracted. These boundary points can reflect the structural characteristics of the environment and are used as a series of global exploration target points. At the same time, a reasonable local area is set around each robot. Based on the environmental information within this area, a mathematical model of the traveling salesman problem is constructed. This model aims to maximize the acquisition and coverage of environmental information. Through optimization calculations, a series of representative viewpoints are selected from the local area. These viewpoints can comprehensively reflect the local environmental characteristics and are used as local target points, providing a basis for the local path planning of the robot.

[0082] Step 4, please see Figure 4 and Figure 5 , in Figure 4 the green dots correspond to the exploration point category of the flat road surface and are assigned to the intelligent vehicle for reinforcement learning training. In Figure 5 the blue dots are the exploration point category of the rough road surface and are assigned to the local part of the quadruped robot dog. By using the feature matching algorithm, the feature information of the current local target points, such as geometric shape, texture, etc., is analyzed and matched, and they are divided into different types. According to the functional characteristics of the robot, different types of local target points are dynamically assigned to the corresponding functional robots. After each robot receives the assigned local target points, the local target points are used as the nodes of the traveling salesman problem. Under the constraint of the global planned path, the traveling salesman problem is solved by the genetic algorithm to obtain the optimal local planned path starting from the current position and traversing all the assigned local target points.

[0083] Step 5: Conduct communication and interaction among heterogeneous multi-robots through a wireless network card. In the case of communicability, the heterogeneous multi-robots share the positioning information output by the LIO-SAM algorithm and the planned global and local exploration paths. Please refer to Figure 3 The green large circles in

[0084] indicate that the surrounding robots are within the communication range and are in a communicable state. Figure 3 Step 6: Similarly, please refer to

[0085] In Figure 4 and Figure 5 , the deep reinforcement learning situations of the intelligent vehicle and the quadruped robot dog are respectively shown. Both are based on the allocated local target points and the preprocessed point cloud data and use the deep reinforcement learning model to intelligently select the optimal navigation and obstacle avoidance path to the local target point and output the control instructions for heterogeneous multi-robots. During the exploration process for long-term goals, the reward is defined by the simultaneous constraints of multiple rewards:

[0086] The first reward in respectively represent the robot's position and pose information, respectively represent the position and pose information of the local target point to be explored, , and are all corresponding weight coefficients. This formula indicates that the policy encourages the robot to approach the target point in terms of direction and position. As the exploration progresses, the probability of the robot accurately reaching the target point becomes higher and higher.

[0087] The second reward in represents the distance between the robot and the nearest obstacle processed according to the interval division, represents the safety distance threshold, respectively represent the penalty coefficient, reward coefficient, exploration coefficient, and exploration progress time. This formula indicates that the policy encourages the robot's trajectory to deviate from obstacles. As exploration progresses, the probability of the robot avoiding obstacles increases.

[0088] The third reward At the same time, for the set speed , speed and angular velocity certain constraints are also imposed, where respectively represent the speed reward coefficient, the angle between the robot's orientation and the target direction, the target speed penalty coefficient, the angular acceleration penalty coefficient, the angular acceleration, the angular velocity penalty coefficient, and the non - linear penalty exponent, indicating that the policy encourages the robot to move towards a more efficient and smooth motion.

[0089]

[0090]

[0091]

[0092] Step 8, The heterogeneous multi - robot obtains the corresponding control instructions from the output of the deep reinforcement learning model and issues them to the underlying drive controller to complete the control operation of the heterogeneous multi - robot.

[0093] Embodiment 2

[0094] This embodiment provides a heterogeneous multi - robot collaborative autonomous exploration system, including the following modules:

[0095] The environmental data acquisition module is used to collect the depth images and lidar point cloud information of each robot's working area, and at the same time, perform feature extraction on the point cloud data and normalization processing on the depth images to complete the pre - processing operation of the sensing data.

[0096] The dynamic task allocation module is used to solve the unexplored edge region points in the global range and a series of viewpoints in the local range according to the feature information of the pre - processed lidar point cloud data and depth map , and judge the category information of a series of viewpoints in the local range according to the point cloud data of multiple set types, and allocate the target local viewpoints to be explored to the heterogeneous multi - robots and plan a global exploration path and a local exploration path.

[0097] The point - to - point communication transmission module is used to directly assign different fixed static IP addresses to the heterogeneous multi - robots, establish them under the same local area network, and at the same time share their own positioning information and the globally planned exploration path within a certain communication range.

[0098] The pursuit optimization module is used to, when communication is unavailable, determine whether information transmission is beneficial to the overall exploration efficiency based on the time and exploration benefits of the global exploration path planned by the computer robot and the pursuit information interaction path, and autonomously choose to maintain the original global exploration path plan or change to the pursuit information interaction path.

[0099] The navigation and obstacle avoidance module is used to obtain the assigned target local viewpoints and the lidar point cloud data after preprocessing information, analyze the surrounding environment of the heterogeneous multi-robot, and use a deep reinforcement learning model to generate the optimal navigation and obstacle avoidance path to the target local viewpoints and output the control instructions for the heterogeneous multi-robot.

[0100] The heterogeneous multi-robot motion control module is used to obtain the control instructions for the heterogeneous multi-robot output by the model. The differential intelligent vehicle receives the motor differential information, the Ackermann intelligent vehicle receives the front wheel steering information and the rear wheel drive information, and the quadruped robot dog receives the joint angle information of each motor.

[0101] The heterogeneous multi-robot motion control module includes a series of software algorithm drivers for Unitree quadruped robot dog, Lunqu Ackermann intelligent vehicle and Blue Whale's Chitu differential intelligent vehicle. Each robot is equipped with a 16-line lidar, a depth camera, an inertial measurement unit and a wireless network card. It is used to obtain the environmental information within a certain range around each robot and the information interaction and sharing between robots.

[0102] Figure 2 The lidar point cloud data of the scene around the robot at the beginning, after a series of preprocessing operations such as feature extraction, is input into the LIO-SAM mapping, dynamic task allocation module and deep reinforcement learning model. As Figure 6 shown, according to the feature information of the preprocessed lidar point cloud data and sensor data such as depth images, the unexplored edge area points in the global range and a series of viewpoints (local target points) in the local range are solved. In Figure 6 the global unexplored points are green squares, and the series of local viewpoints of various types are orange small dots. As Figure 4 and Figure 5 shown, according to the local target points of various types (corresponding to different colors) output by the dynamic task allocation module, they are assigned to the corresponding type of robot, and the sensor data is fused for deep reinforcement learning navigation training under complex terrains. As Figure 3 shown, different communication modes are presented according to the position information and exploration paths among the robots. The green circle represents full communication, the yellow circle represents the pursuit for information interaction, and the red circle represents unavailable communication. During the operation of the system, the communication mode will change according to the processing moment of the pursuit optimization module. As Figure 7 and Figure 8As shown, it is the lidar point cloud map that has been synchronously constructed during the multi-robot collaborative exploration process. After all the environmental information is obtained, it will return to the starting point ( Figure 7 presented as a small red dot in

[0103] The above has described the embodiments of the present invention in detail with reference to the accompanying drawings, but the present invention is not limited to the described embodiments. For those skilled in the art, without departing from the principle and spirit of the present invention, various changes, modifications, substitutions, and variations to these embodiments including components still fall within the protection scope of the present invention.

Claims

1. A heterogeneous multi-robot autonomous collaborative exploration control method, characterized in that, It includes the following steps: Step 1: Initialize each robot, obtain the point cloud data and depth image in this scenario, and perform preprocessing; Step 2: Based on the preprocessed point cloud data and the inertial navigation data obtained by the inertial measurement unit, perform scene mapping and positioning of each robot, and splice multiple point cloud scene maps; Step 3: According to the preprocessed point cloud data and depth image, select global target points and local target points in the global environment range and local area respectively; Step 4: Match the types of local target points according to the feature matching algorithm, and dynamically allocate local target points to robots with corresponding functions to obtain the local exploration paths of robots under the constraint of the global exploration path; Step 5: Based on the spliced point cloud scene map, perform communication interaction and interaction-oriented path planning among heterogeneous multi-robots according to the communication situation; Step 6: According to the local target points allocated to heterogeneous multi-robots and the preprocessed point cloud data, use the deep reinforcement learning model to select the optimal path to the local target points, and output control instructions to complete the control operation of heterogeneous multi-robots until all environmental information is explored and the robots return to the initial position.

2. The heterogeneous multi-robot autonomous collaborative exploration control method according to claim 1, characterized in that In the said Step 2, the specific implementation process of scene mapping is as follows: Step 2.1: Input the preprocessed radar point cloud data and inertial navigation data, use the inertial navigation data pre-integration to estimate the motion during radar scanning, de-distort the radar point cloud, use voxel grid filtering to reduce the calculation amount, and perform feature extraction of edge points and plane points, and output the feature point cloud; Step 2.2: Perform feature matching between the edge points and plane points of the feature point cloud and the map, solve the relative pose between the current frame and the map through the iterative closest point, and output the factors required in the factor graph; Step 2.3: Perform factor graph construction and optimization, generate the optimized robot trajectory and state estimation by solving the nonlinear least squares problem; and add loop closure constraints to the factor graph, re-optimize the robot trajectory, and finally output the globally consistent trajectory and map.

3. The heterogeneous multi-robot autonomous collaborative exploration control method according to claim 2, wherein, The generation of the global target points in the said Step 3 is completed by generating boundary points through a method based on the topological graph, and extracting the information of the edge region points in the global environment range as the global target points.

4. The heterogeneous multi-robot autonomous collaborative exploration control method according to claim 3, characterized in that, The specific process of selecting local target points in the said Step 3 is as follows: Step 3.1: Calculate the convex hull of the set of global unexplored boundary points through the convex hull algorithm, decompose the non-convex region into multiple convex polygon sub-regions by combining the ear cutting method, and use Boolean operations to remove the obstacle regions. At the same time, use grid sampling and feature-based sampling methods in the polygon region to generate a candidate viewpoint set, and define the objective function as minimizing the number of viewpoints; Step 3.2: Construct constraint conditions: Use an octree to perform spatial segmentation on the polygon region, optimize the ray detection algorithm to judge the visibility between viewpoints and points in the region, transform the occlusion judgment in the three-dimensional scene into a line segment intersection problem, and at the same time combine the rapidly-exploring random tree RRT* path planning algorithm to ensure that the distance between the selected viewpoints does not exceed the maximum moving distance of the robot and there is a feasible path; Step 3.3: Encode the candidate viewpoints in the candidate viewpoint set into chromosomes. The candidate viewpoint combinations are encoded into chromosomes using binary encoding. Through a comprehensive fitness function that includes coverage and the number of viewpoints, the population is continuously evolved through roulette wheel selection, single-point crossover, and probabilistic mutation operations to obtain the optimal set of representative viewpoints, that is, the set of local target points.

5. The heterogeneous multi-robot autonomous collaborative exploration control method according to claim 4, characterized in that The specific implementation of Step 4 is as follows: According to the feature matching algorithm, match the local target point information with multiple sets of point cloud data of set types, and dynamically allocate the local target points to robots with corresponding functions. Then, solve the traveling salesman problem through the global target points to obtain the global exploration path of the robot, and then solve the traveling salesman problem with time windows to obtain the local exploration path of the robot under the constraint of the global exploration path.

6. The heterogeneous multi-robot autonomous collaborative exploration control method according to claim 5, characterized in that, The feature matching algorithm described in Step 4 uses a point cloud feature matching algorithm that combines the normal distribution transform (NDT) and the iterative closest point (ICP), and the specific implementation is as follows: Step 4.1: Obtain the point cloud information around the local target points, that is, the source point cloud information, and read multiple sets of point cloud data of set types. Apply the voxel grid downsampling algorithm to reduce the data volume, and then perform normal estimation based on the downsampled source point cloud information. Calculate the normal vector by fitting a plane through searching for neighboring points, and extract the fast point feature histogram (FPFH) features to generate a global feature vector, providing a geometric description for coarse registration. Step 4.2: Based on the global feature vector, achieve coarse registration of the point cloud through the NDT algorithm. Approximate the source point cloud to multiple target type point clouds through iterative transformation; use the Newton method to optimize the transformation parameters, and combine the random sample consensus (RANSAC) algorithm to provide an initial pose estimate to achieve coarse registration with meter-level accuracy. Step 4.3: Use the NDT result as the initial value, and the ICP iteratively searches for corresponding point pairs, and solves the optimal transformation through singular value decomposition; adopt a gradually decreasing search strategy to adjust the registration threshold of the corresponding points to achieve centimeter-level fine registration. Step 4.4: Dynamically adjust the NDT grid resolution and the ICP search radius, and iteratively achieve complementary optimization; test the result stability through Monte Carlo sampling. After meeting the standard, output the transformation matrix, and perform point cloud feature matching by multiplying the transformation matrix with the source point cloud information.

7. The heterogeneous multi-robot autonomous collaborative exploration control method according to claim 6, wherein The specific implementation of Step 5 includes the following processes: Step 5.1: Based on the stitched point cloud scene map, conduct communication and interaction between heterogeneous multi-robots through a wireless network card. In the case of communicability, the heterogeneous multi-robots share the positioning information output by the LIO-SAM algorithm and the planned global and local exploration paths. Step 5.2: In the case of non-communicability, quantitatively analyze the time cost and exploration benefits of the global exploration path of the robot and the chasing information interaction path, and dynamically select to maintain the original exploration strategy or switch to the information interaction-oriented path planning.

8. A heterogeneous multi-robot autonomous cooperative exploration control method according to claim 7, characterized in that The quantitative analysis of the time cost and exploration benefits of the global exploration path of the robot and the chasing information interaction path is implemented as follows: For pursuit cost modeling and decision optimization, i and j represent robot indices, where i is the pursuer and j is the pursued; represents the k-th unexplored subspace, i.e., the path node; the time prediction for robot j to reach the unexplored subspace is initialized with the current system time as the starting point , marked as the time when robot j leaves the initial subspace According to the moving speed of robot j calculate the moving time from to ; iteratively calculate the moving time and accumulate it to the leaving time of the previous subspace; finally, generate a sequence containing the arrival times of all subspaces, where the element represents the predicted time for robot j to reach the k-th subspace, the time when robot j leaves the (k - 1)-th subspace; The mechanism for whether the time for the robot to reach each subspace meets the preset time range imposes a penalty term to constrain the robot to complete the task as expected. The penalty calculation method is as follows: When robot j reaches subspace at time not within the time window , the penalty value is calculated according to the distance from the time window boundary; is the penalty coefficient, used to adjust the penalty intensity. Then select the closer distance to the left boundary or the right boundary of the time window. If the robot arrives within the time window, the penalty value is 0. The quantitative index of the comprehensive cost paid during the pursuit of the target. Through multi-dimensional modeling, multi-objectives such as path execution cost, time synchronization cost, and return cost are transformed into computable quantitative indexes.

9. The heterogeneous multi-robot autonomous collaborative exploration control method according to claim 8, characterized in that The path execution cost, time synchronization cost, and return cost are specifically as follows: The path execution cost is the total cost consumed by robot i starting from the current position and sequentially visiting the sub-spaces in the path. The time synchronization cost is the total sum of the time differences between the time when robot i reaches each sub-space and the time when robot j reaches the same sub-space; the return cost is the cost for robot i to return to the starting position after completing the pursuit.

10. The heterogeneous multi-robot autonomous collaborative exploration control method according to claim 9, characterized in that The deep reinforcement learning model is specifically described as follows: Input the local target point information position and the preprocessed lidar point cloud data around the robot. For long-term goals, explore obstacle avoidance and select the optimal path. The reward is defined by the simultaneous constraints of three rewards: The first reward is set based on the robot's position and pose information, the position and pose information of the local target point to be explored, encouraging the robot to approach the target point in terms of direction and position; The second reward is set according to the distance between the robot and the nearest obstacle processed by interval division, the safety distance threshold, and the exploration progress time, encouraging the robot's trajectory to deviate from the obstacle. As the exploration progresses, the probability of the robot avoiding obstacles increases; The third reward constrains the set speed, speed, and angular velocity, encouraging the robot to develop towards efficient and smooth movement.

11. A heterogeneous multi-robot autonomous collaborative exploration control system for implementing the heterogeneous multi-robot autonomous collaborative exploration control method according to any one of claims 1 to 10, characterized in that, It includes the following modules: The environmental data acquisition module is used to acquire the depth image and lidar point cloud information of each robot's working area and perform normalization processing at the same time; The dynamic task allocation module is used to solve the unexplored edge area points in the global range and the viewpoints in the local range based on the feature information of the preprocessed lidar point cloud data and depth map, allocate the required target local viewpoints to be explored to heterogeneous multi-robots, and plan a global exploration path and a local exploration path; The point-to-point communication transmission module is used to directly assign different fixed static IP addresses to heterogeneous multi-robots, establish them under the same local area network, and share their own positioning information and global exploration path within the communication range; The pursuit optimization module is used to judge whether information transmission is beneficial to the overall exploration efficiency based on the time and exploration benefit of calculating the global exploration path planned by the robot and the pursuit information interaction path when communication is unavailable, and autonomously choose to maintain the original global exploration path plan or change to the pursuit information interaction path; The navigation and obstacle avoidance module is used to obtain the assigned target local viewpoints and the preprocessed lidar point cloud data information, analyze the surrounding environment of heterogeneous multi-robots, and use the deep reinforcement learning model to generate the optimal navigation and obstacle avoidance path to the target local viewpoints and output the control instructions of heterogeneous multi-robots; The heterogeneous multi-robot motion control module is used to obtain the control instructions of heterogeneous multi-robots and complete the control of the robots.

Images