A heterogeneous multi-robot autonomous collaborative exploration control method and system

Abstract

The present invention discloses a method and system for autonomous collaborative exploration control of heterogeneous multi-robots. The method first initializes the robots, obtains and preprocesses point cloud data and depth images, and performs scene mapping and positioning of each robot. Secondly, based on the preprocessed point cloud data and depth images, global target points and local target points are selected, and the local target points are type-matched according to the feature matching algorithm to obtain the local exploration path of the robot under the constraints of the global exploration path. Then, based on the communication situation, interactively guided path planning is performed between heterogeneous multi-robots. Finally, based on the local target points assigned to the heterogeneous multi-robots and the preprocessed point cloud data, a deep reinforcement learning model is used to select the optimal path to the local target point, and control instructions are output to complete the control operation of the heterogeneous multi-robots until all environmental information is explored. The present invention realizes efficient and accurate exploration of large, unknown and complex environments.

Description

Technical Field

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

[0002] Autonomous robotic exploration technology has garnered significant attention in numerous fields, including military reconnaissance and disaster relief. However, existing approaches to complex and changing environmental exploration often rely solely on a single type of multi-robot or single robot for autonomous exploration and human-robot interaction, resulting in difficulties in achieving accurate and efficient tasks. This is due to the relatively low exploration efficiency of individual robots and the functional limitations of single-type multi-robots. However, heterogeneous multi-robot autonomous collaborative exploration systems can significantly improve exploration efficiency and reliability. For example, task allocation can adaptively assign appropriate tasks to robots with corresponding functions based on sensor environmental information. For example, in exploration tasks, generating multiple exploration point categories (flat surfaces, stairs, and uneven surfaces) based on sensor information can help improve exploration efficiency. In complex real-world scenarios, such as earthquake rescue, guiding heterogeneous multi-robots to autonomously explore complex terrain through adaptive dynamic task allocation is crucial. For example, tasks involving stairs can be assigned to a quadruped robot dog, while tasks involving flat surfaces can be assigned to a wheeled intelligent vehicle. Summary of the Invention

[0003] The present invention addresses the deficiencies of the prior art and provides a method and system for autonomous collaborative exploration control of heterogeneous multi-robots. The system achieves overall collaborative control of heterogeneous multi-robots through multi-sensor data acquisition, point-to-point information communication transmission, top-level adaptive dynamic task allocation strategy, middle-level intelligent selection of navigation and obstacle avoidance paths, and bottom-level precise output of control instructions. It can achieve efficient and accurate exploration of large, unknown, and complex environments, and can be applied to various scenarios such as military reconnaissance and earthquake rescue. In order to solve the above technical problems, the technical solution of the present invention is as follows: the present invention provides a method for autonomous collaborative exploration of heterogeneous multi-robots. The method generates various types of exploration points according to an adaptive dynamic task allocation mechanism and dynamically allocates them to robots with corresponding functions. Each robot intelligently selects a navigation and obstacle avoidance path through a deep reinforcement learning algorithm, and determines whether information transmission is beneficial to overall exploration efficiency through exploration information. The robot autonomously decides to optimize the exploration path for pursuit to complete information exchange or maintain the original exploration plan, thereby achieving collaborative exploration tasks in unknown and complex environments.

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

[0005] Environmental data acquisition module, used to collect the depth image of each robot working area Idepth and lidar point cloud information D lidar , and at the same time perform feature extraction on point cloud data and normalize depth images to complete the preprocessing operation of sensor data.

[0006] Dynamic task allocation module is used to allocate tasks based on the pre-processed lidar point cloud data D pre-lidar and depth map I pre-depth The feature information of the robot is used to solve the unexplored edge area points in the global range and a series of viewpoints in the local range, and the category information of a series of viewpoints in the local range is judged according to multiple point cloud data of set types. The target local viewpoints to be explored are assigned to the heterogeneous multi-robots, and a global exploration path and a local exploration path are planned.

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

[0008] The pursuit optimization module is used to determine whether information transmission is beneficial to the overall exploration efficiency based on the time and exploration benefits of the global exploration path and the pursuit information interaction path planned by the calculated robot when communication is not possible, and to independently decide whether to maintain the original global exploration path plan or change to the pursuit information interaction path.

[0009] Navigation and obstacle avoidance module, used to obtain the assigned target local viewpoint and pre-processed lidar point cloud data D pre-lidar Information is collected to analyze the surrounding environment of heterogeneous multi-robots, and a deep reinforcement learning model is used to generate the optimal navigation and obstacle avoidance path to the target local viewpoint and output control instructions for the heterogeneous multi-robots.

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

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

[0012] Step 1: Initialize each robot (robot i, robot j, robot k...) at different nearby locations to ensure that each function is operating normally. At the same time, collect sensor data from the depth camera, lidar, and inertial measurement unit to obtain the point cloud data D of the scene. lidar and depth images, and perform normalized preprocessing operations.

[0013] Step 2: The pre-processed point cloud data Dpre-lidar And the inertial navigation data M obtained by the inertial measurement unit imu As the input of LIO-SAM (a simultaneous localization and mapping algorithm based on lidar inertial odometry), scene mapping and individual robot positioning are performed, and multiple point cloud scene maps are stitched together.

[0014] Step 3: Based on the pre-processed point cloud data D pre-lidar and depth map I pre-depth To extract the information of the edge area points of the global environment range as the global target point, a mathematical model of the gallery problem is constructed based on the environmental information 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, the selected local target point information is matched with multiple point cloud data of set types, and the local target points are dynamically assigned to robots with corresponding functions. Then, the traveling salesman problem is solved through the global target point to obtain the global exploration path of the robot. Then, the traveling salesman problem with a time window is solved to obtain the local exploration path of the robot under the constraints of the global exploration path.

[0016] Step 5: Based on the spliced ​​point cloud scene map, communication and interaction between heterogeneous multi-robots are carried out through wireless network cards. When communication is possible, 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 absence of communication, quantitatively analyze the time cost and exploration benefit of the robot's global exploration path and the pursuit information interaction path, and dynamically choose to maintain the original exploration strategy or switch to information interaction-oriented path planning.

[0018] Step 7: Based on the local target points assigned by heterogeneous multi-robots and the pre-processed point cloud data D pre-lidar , a deep reinforcement learning model is used to intelligently select the optimal navigation and obstacle avoidance path to the local target point and output control instructions for heterogeneous multi-robots.

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

[0020] Step 9: Repeat steps 1 to 8 until all environmental information has been explored and the robot returns to its initial position and stops.

[0021] As an example, in step 2, the LIO-SAM algorithm uses the pre-processed point cloud data D pre-lidar and inertial navigation data M imuTo achieve high-precision positioning and mapping of the robot, and to stitch multiple point cloud scene maps. The generation method is:

[0022] Step 2.1: Input the pre-processed LiDAR point cloud data D pre-lidar With inertial navigation data M imu .

[0023] Step 2.2: Using inertial navigation data M imu Pre-integration estimates the motion during the radar scan, 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 graph 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 cover the entire polygonal area. Use an octree to spatially segment the polygonal area. Optimize the raycasting algorithm to determine the visibility of the viewpoints and points within the area. Transform occlusion determination in the 3D scene into a line segment intersection problem. Combined with the RRT* (Rapidly Exploring Random Trees) path planning algorithm, ensure that the distance between selected viewpoints does not exceed the robot's maximum travel distance and that a feasible path exists.

[0031] Step 3.3: Encode the combination of candidate viewpoints in the candidate viewpoint set into chromosomes using binary coding. 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 condition based on the maximum number of iterations and the convergence threshold to obtain the optimal set of representative viewpoints, that is, the set of local target points.

[0032] Preferably, the feature matching algorithm in step 4 adopts a point cloud feature matching algorithm that combines NDT and ICP (normal distribution transform and iterative closest point), with local target point information and multiple point cloud data of set types as input. The specific steps are as follows:

[0033] Step 4.1. Obtain the point cloud information within a certain range around the local target point, that is, the source point cloud information, and read multiple point cloud data of set types. Apply the voxel grid downsampling algorithm to reduce the data volume, retain the geometric features while improving processing efficiency. Then, perform normal estimation based on the downsampled source point cloud information, calculate the normal vector by searching the neighborhood points to fit the plane, extract the FPFH (Fast Point Feature Histogram) feature to generate the global feature vector, and provide a geometric description for coarse alignment.

[0034] Step 4.2: Based on the global eigenvectors, a coarse registration of the point clouds is performed using the Normal Distribution Transform (NDT) algorithm. The source point cloud is iteratively transformed to approximate multiple target point clouds. The transformation parameters are optimized using the Newton method, and RANSAC (Random Sample Consensus Algorithm) is used to provide an initial pose estimate, achieving coarse registration with meter-level accuracy.

[0035] Step 4.3: Using the NDT results as the initial values, perform ICP iterations to find corresponding point pairs and solve for the optimal transformation using SVD (singular value decomposition). A descending search strategy is used to adjust the registration threshold for corresponding points from loose to strict, achieving centimeter-level precision registration.

[0036] Step 4.4: Dynamically adjust the NDT grid resolution and ICP search radius, iteratively achieving complementary optimization. Calculate mean square error and point cloud coverage to assess geometric consistency, and calculate the determinant of the transformation matrix to verify legitimacy. Monte Carlo sampling tests the stability of the results. Once they meet the requirements, the final transformation matrix is ​​output. Point cloud feature matching is achieved by multiplying the transformation matrix with the source point cloud information.

[0037] As a preferred method, in step 6, the time cost and exploration benefit of the robot's global exploration path and pursuit information interaction path are analyzed by a hierarchical quantitative calculation method. The calculation method is as follows:

[0038] First, based on the local exploration path and kinematic model of robot j (the pursued), the time it takes to reach each unexplored subspace is estimated, and a penalty mechanism is introduced to account for time window constraints. Next, a space-time graph is constructed, encompassing the current position of robot i (the pursuer) and the unexplored subspace of robot j's path. By solving a traveling salesman problem with time window constraints, a branch-and-bound algorithm is employed to find the optimal path for robot i to access a portion of robot j's global path and return. The cost of this path is then calculated, including the motion cost and a time synchronization penalty. Finally, robot i's current global path cost is compared with the pursuit cost plus the shared global path cost. If the latter is smaller, robot i executes the pursuit strategy, sending a request to robot j for a path. The optimal pursuit path is generated and executed, and information is exchanged at the encounter point, updating the global path plan.

[0039] For pursuit cost modeling and decision optimization, the parameters are as follows: i, j represent robot index (i is the pursuer, j is the pursued), S k represents the kth unexplored subspace (path node), t k Indicates reaching the unexplored subspace S k Time, V i represents the moving speed of robot i, d(S i ,S j ) represents the subspace S i to S j The Euclidean distance, π j represents the global path planning of robot j, [e k ,l k ] indicates that the subspace S is not explored k The effective access time window, e k is the left boundary of the time window, l k is the right boundary of the time window;

[0040] 1. Prediction of robot j’s arrival time in subspace:

[0041]

[0042] t jo =t o (Current moment) 2. Time window constraint penalty:

[0043]

[0044] 3. Total cost of pursuit path:

[0045]

[0046] The first expression shows the predicted time for robot j to reach the unexplored subspace, which is initialized with the current system time as the starting point t jo , marked as robot j leaves the initial subspace S o time, according to the moving speed V of robot j j Calculate from S k-1 to S k Moving time Iteratively calculate the movement time and add it to the departure time of the previous subspace. Finally, generate a sequence containing the arrival time of all subspaces [t j1 ,t j2 ,....,t jm ]. Where t jk represents the predicted time for robot j to reach the kth subspace, t jk-1 The time when robot j leaves the k-1th subspace, k represents the dynamic variable, and m represents the total number of subspaces.

[0047] The second expression shows whether the time for the robot to reach each subspace meets the preset time range. By imposing a penalty term, the robot is constrained to complete the task within the expected time. The specific penalty calculation method is: when robot j reaches subspace S k Time t jk Not in the time window [e k ,l k ], according to t jk The penalty value is calculated based on the distance from the time window boundary. δ is the penalty coefficient, which is used to adjust the penalty intensity. The larger the coefficient, the more likely the robot is to complete the task within the time window. min(|t jk -e k |,|t jk -l k |) then select t jk and the left boundary of the time window e k or right border k If the robot arrives within the time window, the penalty value is 0.

[0048] The third expression shows the quantitative index of the comprehensive cost paid in the process of pursuing the target. Through multi-dimensional modeling, the path execution cost, time synchronization cost, and return cost are converted into computable quantitative indicators. i-＞j The total cost of robot i chasing robot j is:

[0049] Path execution cost: Robot i starts from the current position and visits the subspaces in the path in sequence The total cost consumed, including Represents the subspace (the Pth subspace in the path) to transfer costs.

[0050] Time synchronization cost: the sum of the time difference between robot i’s arrival time in each subspace and robot j’s arrival time in the same subspace. β represents the time synchronization penalty coefficient, Respectively represent the subspaces reached by robots i and j prediction time.

[0051] Return cost: The cost of returning to the starting position after robot i completes the pursuit. λ represents the return path weight coefficient, which controls the importance of the return cost. represents the end point subspace of the pursuit path. S0 is the starting position of the robot. is the distance from the end point to the starting point.

[0052] As a preference, in step 7, the local target points and the pre-processed point cloud data D pre-lidar The input deep reinforcement learning model uses the PPO network, which is described as follows:

[0053] Input the local target point information position and the pre-processed lidar point cloud data D around the robot pre-lidar , for long-term goals, explore and avoid obstacles to select the optimal path, and the reward is defined by multiple reward constraints at the same time:

[0054] The first reward is R1 S p ,S r Represents the robot position and posture information respectively, P p ,P r Respectively represent the position information and pose information of the local target point to be explored, χ and γ are the corresponding weight coefficients. This formula shows that the strategy encourages the robot to move closer to the target point. As exploration progresses, the probability of the robot accurately reaching the target point increases.

[0055] The second reward is R2 D p Indicates the distance between the robot and the nearest obstacle according to the interval division, D safe represents the safety distance threshold, α, β, φ, t ex Denote the penalty coefficient, reward coefficient, exploration coefficient, and exploration progress time, respectively. This formula indicates that the strategy encourages the robot's trajectory to be away from obstacles. As the exploration progresses, the probability of the robot avoiding obstacles increases.

[0056] The third reward R3 is also for setting speed A vset , speed A vand angular velocity A a Certain constraints are also imposed, among which Ψ, θ, ρ, ξ, Δω, μ, They represent the speed reward coefficient, the angle between the robot and the target direction, the target speed penalty coefficient, the angular acceleration penalty coefficient, the angular acceleration, the angular velocity penalty coefficient, and the nonlinear penalty index, respectively, indicating that the strategy encourages the robot to move towards more efficient and smoother movements.

[0057]

[0058] Preferably, in step 8, the quadruped robot dog in the heterogeneous multi-robot receives the model output control instructions described as follows:

[0059] In complex scenarios, the quadruped robotic dog processes visual information in real time (after denoising, enhancement, and distortion correction) and inputs it into a lightweight visual encoder. The extracted spatial features are then fused with proprioception data (posture, joint angles) through an attention mechanism and then input into a deep reinforcement learning architecture. Finally, the gait parameters are mapped into 12D foot-end motor control signals (three joints per foot: hip, knee, and ankle) through an inverse kinematics solver, which are then sent to the underlying controller interface. The system combines Bezier curve trajectory planning and plantar force feedback mechanism to achieve adaptive movement in complex terrain.

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

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

[0062] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0063] Figure 1 Schematic diagram of a method flow in an embodiment of the present invention;

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

[0065] Figure 3 Show pictures for inter-robot communication mode;

[0066] Figure 4 A diagram showing reinforcement learning simulation training for smart cars;

[0067] Figure 5 This is a diagram showing the reinforcement learning simulation training of a four-legged robot dog;

[0068] Figure 6 Photos of the robot exploring in progress;

[0069] Figure 7 Simulation photos of robot exploration in progress;

[0070] Figure 8 This is a photo of the global point cloud map during the exploration process. DETAILED DESCRIPTION

[0071] It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments may be combined with each other.

[0072] In order to make the objectives, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below with reference to the accompanying drawings.

[0073] On the contrary, the present invention covers any alternatives, modifications, equivalents, and solutions that fall within the spirit and scope of the present invention as defined by the claims. Furthermore, to facilitate a better understanding of the present invention, certain specific details are described in detail below in the detailed description of the present invention. Those skilled in the art will be able to fully understand the present invention without these details.

[0074] Example 1

[0075] This embodiment provides a control method for a heterogeneous multi-robot autonomous collaborative exploration system. Figure 1 , the method comprising:

[0076] Step 1: Initialize each robot (robot i, robot j, robot k...) at different nearby locations to ensure that each function is functioning properly. At the same time, collect sensor data from the depth camera, lidar, and inertial measurement unit, and analyze the point cloud data D. lidar Perform preprocessing operations for feature extraction and depth image normalization. Figure 2 , Figure 2 Shows the point cloud data D around the robot lidar .

[0077] Step 2: The pre-processed lidar point cloud data D pre-lidar The attitude, velocity, acceleration and other data obtained from the inertial measurement unit are used as inputs to the LIO-SAM algorithm. The LIO-SAM algorithm uses a tight coupling method to deeply fuse the lidar data and inertial measurement data. By optimizing the matching between lidar frames and the pre-integration of the inertial measurement unit (IMU), it achieves high-precision scene mapping and robot positioning. After completing the construction of a single scene map, based on the positioning information of each robot, a graph optimization algorithm is used to stitch multiple scene maps together to eliminate errors between maps and build a complete and accurate global environment map, providing a reliable environmental reference for the robot's subsequent task execution. Figure 8 The generated global point cloud map photo shows that the interior is composed of point cloud data of two different colors, which are explored and constructed by two robots respectively.

[0078] Step 3, see Figure 3 The green square in is the global target point. Figure 3 The orange dots inside are a series of local representative viewpoints. These points are based on the pre-processed point cloud data D pre-lidar and depth map I pre-depth Based on the characteristic information of the environment, a topological graph algorithm is used to extract information on multiple boundary points of the environment. These boundary points can reflect the structural characteristics of the environment and serve as a series of global exploration target points. At the same time, a reasonable local area is set around each robot, and based on the environmental information within this area, a mathematical model of the gallery 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 fully reflect the local environmental characteristics. These viewpoints serve as local target points and provide a basis for the robot's local path planning.

[0079] Step 4, see Figure 4 and Figure 5 ,exist Figure 4 The green dots correspond to the exploration point categories on the flat road surface, which are assigned to the smart car for reinforcement learning training. Figure 5 The blue dots in the figure represent exploration points for rough terrain, assigned to the local areas of the quadruped robot. A feature matching algorithm analyzes and matches the characteristic information of each local target point, such as its geometry and texture, to classify it into different types. Based on the robot's functional characteristics, different types of local target points are dynamically assigned to the corresponding robot. After receiving the assigned local target point, each robot uses it as a node in the traveling salesman problem. Within the constraints of the global planning path, a genetic algorithm is used to solve the traveling salesman problem, deriving the optimal local planning path from its current position to traverse all assigned local target points.

[0080] Step 5: Use wireless network cards to communicate with heterogeneous multi-robots. When communication is possible, heterogeneous multi-robots share the positioning information output by the LIO-SAM algorithm and the planned global and local exploration paths. Figure 3 The large green circle in the middle indicates that the surrounding robots are within the communication range and are in a communicative state.

[0081] Step 6, see also Figure 3 The large red circle in the middle indicates that the robot has no other robots to communicate with and is not in a pursuit state. In the absence of communication, the robot j's arrival time in each subspace is predicted based on its global path and kinematic model, with a time window constraint penalty mechanism introduced. A spatiotemporal graph is constructed containing robot i's current position and robot j's path subspace. A branch-and-bound algorithm is used to solve the TSP problem with time window constraints, generating an optimal route for robot i to visit a portion of robot j's path from its current position and return. The total cost of this path (including motion cost and time synchronization penalty) is calculated and compared with the current global path cost of robot i. If the pursuit option has a lower cost, the pursuit strategy is executed: robot i sends a path request to robot j, generates an optimal pursuit path based on the returned data, and updates the global path plan after information exchange at the encounter point.

[0082] Step 7, see Figure 4 and Figure 5 , respectively showing the deep reinforcement learning of the smart car and the four-legged robot dog. Both are based on the assigned local target points and the pre-processed point cloud data D pre-lidar , uses a deep reinforcement learning model to intelligently select the optimal navigation obstacle avoidance path to the local target point and output the control instructions of heterogeneous multi-robots. For the long-term goal exploration process, the reward R all Defined by multiple reward constraints simultaneously:

[0083] The first reward is R1 S p ,S r Represents the robot position and posture information respectively, P p ,P r Respectively represent the position information and pose information of the local target point to be explored, χ and γ are the corresponding weight coefficients. This formula shows that the strategy encourages the robot to move closer to the target point. As exploration progresses, the probability of the robot accurately reaching the target point increases.

[0084] The second reward is R2 D p Indicates the distance between the robot and the nearest obstacle according to the interval division, D safe represents the safety distance threshold, α, β, φ, t exDenote the penalty coefficient, reward coefficient, exploration coefficient, and exploration progress time, respectively. This formula indicates that the strategy encourages the robot's trajectory to be away from obstacles. As the exploration progresses, the probability of the robot avoiding obstacles increases.

[0085] The third reward R3 is also for setting speed A vset , speed A v and angular velocity A a Certain constraints are also imposed, among which Ψ, θ, ρ, ξ, Δω, μ, They represent the speed reward coefficient, the angle between the robot and the target direction, the target speed penalty coefficient, the angular acceleration penalty coefficient, the angular acceleration, the angular velocity penalty coefficient, and the nonlinear penalty index, respectively, indicating that the strategy encourages the robot to move towards more efficient and smoother movements.

[0086]

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

[0088] Example 2

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

[0090] Environmental data acquisition module, used to collect the depth image of each robot working area I depth and lidar point cloud information D lidar , and at the same time perform feature extraction on point cloud data and normalize depth images to complete the preprocessing operation of sensor data.

[0091] Dynamic task allocation module is used to allocate tasks based on the pre-processed lidar point cloud data D pre-lidar and depth map I pre-depth The feature information of the robot is used to solve the unexplored edge area points in the global range and a series of viewpoints in the local range, and the category information of a series of viewpoints in the local range is judged according to multiple point cloud data of set types. The target local viewpoints to be explored are assigned to the heterogeneous multi-robots, and a global exploration path and a local exploration path are planned.

[0092] The point-to-point communication transmission module is used to directly assign different fixed static IP addresses to heterogeneous multiple robots, establish them on the same local area network, and share their own positioning information and their own planned global exploration paths within a certain communication range.

[0093] The pursuit optimization module is used to determine whether information transmission is beneficial to the overall exploration efficiency based on the time and exploration benefits of the global exploration path and the pursuit information interaction path planned by the calculated robot when communication is not possible, and to independently decide whether to maintain the original global exploration path plan or change to the pursuit information interaction path.

[0094] Navigation and obstacle avoidance module, used to obtain the assigned target local viewpoint and pre-processed lidar point cloud data D pre-lidar Information is collected to analyze the surrounding environment of heterogeneous multi-robots, and a deep reinforcement learning model is used to generate the optimal navigation and obstacle avoidance path to the target local viewpoint and output control instructions for the heterogeneous multi-robots.

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

[0096] The heterogeneous multi-robot motion control module includes a series of software algorithms driving the Yushu Technology quadrupedal robot dog, the Lunqu Technology Ackerman smart car, and the Blue Whale Chitu differential smart car. Each robot is equipped with a 16-line lidar, a depth camera, an inertial measurement unit, and a wireless network card. These are used to obtain environmental information within a certain range around each robot and to exchange and share information between robots.

[0097] Figure 2 The laser radar point cloud data of the scene around the robot at the beginning is input into the LIO-SAM mapping, dynamic task allocation module and deep reinforcement learning model after a series of pre-processing operations such as feature extraction. Figure 6 As shown in the figure, based on the pre-processed lidar point cloud data and depth image and other sensor data feature information, the unexplored edge area points in the global range and a series of viewpoints (local target points) in the local range are solved. Figure 6 The global unexplored points are green squares, and the local viewpoints of various types are orange dots. Figure 4 and Figure 5 As shown in the figure, the local target points of each type (corresponding to different colors) output by the dynamic task allocation module are assigned to the corresponding type of robot, and the sensor data is integrated to perform deep reinforcement learning navigation training under complex terrain. Figure 3 As shown in the figure, different communication modes are presented according to the position information and exploration path between each robot. The green circle indicates complete communication, the yellow circle indicates chasing for information exchange, and the red circle indicates no communication. During the operation of the system, the communication mode will change according to the processing time of the chasing optimization module. Figure 7 and Figure 8As shown in the figure, the laser radar point cloud map has been synchronously constructed during the multi-robot collaborative exploration process. When all the environmental information is obtained, it will return to the starting point ( Figure 7 (Small red dots appear in the middle).

[0098] The embodiments of the present invention are described in detail above with reference to the accompanying drawings, but the present invention is not limited to the described embodiments. It will be apparent to those skilled in the art that various changes, modifications, substitutions, and variations of these embodiments, including components, without departing from the principles and spirit of the present invention are still within the scope of protection of the present invention.

Claims

1. A heterogeneous multi-robot autonomous collaborative exploration control method, characterized by: The steps include: Step 1: Initialize each robot, obtain the point cloud data and depth image of this scene, and preprocess them; Step 2: Based on the pre-processed point cloud data and the inertial navigation data obtained by the inertial measurement unit, the scene is mapped and each robot is positioned, and multiple point cloud scene maps are stitched together; Step 3: Select global target points and local target points in the global environment range and local area respectively based on the pre-processed point cloud data and depth image; Step 4: Match the types of local target points according to the feature matching algorithm, and dynamically assign the local target points to robots with corresponding functions to obtain the local exploration path of the robot under the constraints of the global exploration path; Step 5: Based on the spliced ​​point cloud scene map and the communication conditions, communication interaction and interaction-oriented path planning are carried out among heterogeneous multi-robots. Step 6: Based on the local target points assigned by the heterogeneous multi-robots and the pre-processed point cloud data, the deep reinforcement learning model is used to select the optimal path to the local target point and output control instructions to complete the control operation of the heterogeneous multi-robots until all environmental information is explored and the robots return to their initial positions; The deep reinforcement learning model is described as follows: The input is the location of the local target point information and the pre-processed radar point cloud data around the robot. For the long-term target, the optimal path is selected by exploring and avoiding obstacles. The reward is defined by three reward constraints: The first reward is set based on the robot's position and pose information, and the position and pose information of the local target point to be explored, encouraging the robot to move towards the target point and position; The second reward is set based on the distance between the robot and the nearest obstacle, the safety distance threshold, and the exploration progress time. This encourages the robot's trajectory to be away from obstacles. As exploration progresses, the robot's probability of avoiding obstacles increases. The third reward sets constraints on set speed, velocity, and angular velocity, encouraging the robot to move towards efficient and smooth movement; The formulas for the three rewards are as follows: The first reward is R1 S p ,S r Represents the robot position and posture information respectively, P p ,P r Respectively represent the position information and pose information of the local target point to be explored, χ and γ are the corresponding weight coefficients; The second reward is R2 D p Indicates the distance between the robot and the nearest obstacle according to the interval division, D safe represents the safety distance threshold, α, β, φ, t ex Represent the penalty coefficient, reward coefficient, exploration coefficient, and exploration progress time respectively; The third reward R3 is also for setting speed A vset , speed A v and angular velocity A a Constraints are also made, where Ψ, θ, ρ, ξ, Δω, μ, They represent the speed reward coefficient, the angle between the robot and the target direction, the target speed penalty coefficient, the angular acceleration penalty coefficient, the angular acceleration, the angular velocity penalty coefficient, and the nonlinear penalty index respectively.

2. The method for autonomous collaborative exploration and control of heterogeneous multi-robots according to claim 1, characterized in that: In step 2, the specific implementation process of scene mapping is as follows: Step 2.1: Input pre-processed radar point cloud data and inertial navigation data, use inertial navigation data pre-integration to estimate the motion during the radar scan, dedistort the radar point cloud, use voxel grid filtering to reduce the amount of calculation, and perform feature extraction of edge points and plane points, and output a feature point cloud; Step 2.2: 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 closest point, and output the required factors in the factor graph; Step 2.3: Construct and optimize the factor graph. Generate the optimized robot trajectory and state estimation by solving the nonlinear least squares problem. Add loop constraints to the factor graph, re-optimize the robot trajectory, and finally output a globally consistent trajectory and map.

3. The method for autonomous collaborative exploration and control of heterogeneous multi-robots according to claim 2 is characterized in that: The generation of the global target point in step 3 is completed by generating boundary points based on a topological map method, and extracting information of edge area points of the global environment range as the global target point.

4. The method for autonomous collaborative exploration and control of heterogeneous multi-robots according to claim 3 is characterized in that: The specific process of selecting the local target point in step 3 is as follows: Step 3.1: Calculate the convex hull of the global unexplored boundary point set using the convex hull algorithm. Combined with the ear-cut method, decompose the non-convex region into multiple convex polygonal sub-regions. Use Boolean operations to remove obstacle regions. Simultaneously, use grid sampling and feature-based sampling methods within the polygonal region to generate a set of candidate viewpoints. Define the objective function as minimizing the number of viewpoints. Step 3.2: Construct constraints: Use an octree to spatially segment the polygonal region, optimize the ray detection algorithm to determine the visibility between the viewpoint and points within the region, transform the occlusion judgment in the 3D scene into a line segment intersection problem, and combine the Rapid Exploration Random Tree* (RRT*) path planning algorithm to ensure that the distance between the selected viewpoints does not exceed the robot's maximum movement distance and that a feasible path exists. Step 3.3: Encode the candidate viewpoint combinations in the candidate viewpoint set into chromosomes using binary coding. 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 method for autonomous collaborative exploration and control of heterogeneous multi-robots according to claim 4 is characterized in that: The specific implementation of step 4 is as follows: according to the feature matching algorithm, the local target point information is matched with a plurality of point cloud data of set types, and the local target points are dynamically assigned to robots with corresponding functions. Then, the traveling salesman problem is solved through the global target point to obtain the global exploration path of the robot. Then, the traveling salesman problem with a time window is solved to obtain the local exploration path of the robot under the constraints of the global exploration path.

6. The method for autonomous collaborative exploration and control of heterogeneous multi-robots according to claim 5, characterized in that: The feature matching algorithm described in step 4 uses a point cloud feature matching algorithm that combines normal distribution transform (NDT) with iterative closest point (ICP). The specific implementation is as follows: Step 4.1: Obtain the point cloud information around the local target point, i.e., the source point cloud information, and read multiple point cloud data of a specified type. Apply the voxel grid downsampling algorithm to reduce the data volume. Then, perform normal estimation based on the downsampled source point cloud information. Calculate the normal vector by searching the neighborhood points for a fitted plane, extract the fast point feature histogram (FPFH) features, and generate a global feature vector to provide a geometric description for coarse registration. Step 4.2: Based on the global eigenvectors, the normal distribution transform (NDT) algorithm is used to achieve coarse registration of the point cloud. The source point cloud is iteratively transformed to approximate multiple target point clouds. The transformation parameters are optimized using the Newton method, and the random sampling consensus algorithm (RANSAC) is used to provide an initial pose estimate, achieving coarse registration with meter-level accuracy. Step 4.3: Using the NDT results as the initial value, perform ICP iterations to find corresponding point pairs and solve the optimal transformation using singular value decomposition. A descending search strategy is used to adjust the registration threshold of corresponding points to achieve centimeter-level precision registration. Step 4.4: Dynamically adjust the NDT grid resolution and ICP search radius to iteratively achieve complementary optimization; test the stability of the results through Monte Carlo sampling, output the transformation matrix after meeting the standards, and achieve point cloud feature matching by multiplying the transformation matrix with the source point cloud information.

7. The method for autonomous collaborative exploration and control of heterogeneous multi-robots according to claim 6, characterized in that: The step 5 is implemented by comprising the following process: Step 5.1: Based on the spliced ​​point cloud scene map, the heterogeneous multi-robots communicate with each other through wireless network cards. When communication is possible, 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 absence of communication, quantitatively analyze the time cost and exploration benefit of the robot's global exploration path and the pursuit information interaction path, and dynamically choose to maintain the original exploration strategy or switch to information interaction-oriented path planning.

8. The method for autonomous collaborative exploration and control of heterogeneous multi-robots according to claim 7, characterized in that: The quantitative analysis of the time cost and exploration benefit of the robot's global exploration path and pursuit information interaction path is implemented as follows: For pursuit cost modeling and decision optimization, i, j represent robot indexes, i is the pursuer, and j is the pursued; S k represents the kth unexplored subspace, i.e., the path node; the time prediction of robot j reaching the unexplored subspace is initialized with the current system time as the starting point t jo , marked as robot j leaves the initial subspace S o time, according to the moving speed V of robot j j Calculate from S k-1 to S k The moving time is calculated iteratively; the moving time is added to the departure time of the previous subspace; finally, a sequence containing the arrival time of all subspaces is generated, where the element t jk represents the predicted time for robot j to reach the kth subspace, t jk-1 The time when robot j leaves the k-1th subspace; The mechanism of whether the time when the robot arrives at each subspace meets the preset time range is used to constrain the robot to complete the task within the expected time by imposing a penalty term. The penalty calculation method is: when robot j reaches subspace S k Time t jk Not in the time window [e k ,l k ], according to t jk The penalty value is calculated based on the distance from the time window boundary; δ is the penalty coefficient, which is used to adjust the penalty intensity. min(|t jk -e k |,|t jk -l k |) then select t jk and the left boundary of the time window e k or right border k For a closer distance, if the robot arrives within the time window, the penalty value is 0; The quantitative indicator of the comprehensive cost paid in the process of pursuing the target, through multi-dimensional modeling, converts the path execution cost, time synchronization cost, and return cost into computable quantitative indicators.

9. The method for autonomous collaborative exploration and control of heterogeneous multi-robots according to claim 8, characterized in that: The path execution cost, time synchronization cost, and return cost are as follows: The path execution cost is the robot i starting from the current position and visiting the subspace S in the path in sequence kP The total cost consumed; the time synchronization cost is the sum of the time difference between robot i’s arrival time in each subspace and robot j’s arrival time in the same subspace; the return cost is the cost of robot i returning to the starting position after completing the pursuit.

10. A heterogeneous multi-robot autonomous collaborative exploration control system, used to implement the heterogeneous multi-robot autonomous collaborative exploration control method according to any one of claims 1 to 9, characterized in that: Includes the following modules: Environmental data acquisition module, used to collect depth images and radar 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 and local viewpoints in the global scope based on the feature information of the pre-processed lidar point cloud data and depth map, assign the target local viewpoints to be explored to the heterogeneous multi-robots, and plan a global exploration path and a local exploration path; Point-to-point communication transmission module is used to directly assign different fixed static IP addresses to heterogeneous multiple robots, establish them on the same local area network, and share their own positioning information and global exploration paths within the communication range; The pursuit optimization module is used to determine whether information transmission is beneficial to overall exploration efficiency based on the time and exploration efficiency of the global exploration path and the pursuit information interaction path planned by the robot when communication is impossible, and independently decide whether 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 viewpoint and pre-processed LiDAR point cloud data information, analyze the surrounding environment of the heterogeneous multi-robot, and use the deep reinforcement learning model to generate the optimal navigation and obstacle avoidance path to the target local viewpoint and output the control instructions of the heterogeneous multi-robot; The heterogeneous multi-robot motion control module is used to obtain heterogeneous multi-robot control instructions and complete the control of the robots.

Images