A multi-robot path planning method and device

By combining lidar and camera data in a multi-robot system, using artificial potential field and RRT algorithm to optimize path planning, the problem of low path planning efficiency of multi-robots in unknown environments is solved, and efficient and accurate map fusion and smooth paths are achieved. Build.

CN115586767BActive Publication Date: 2024-11-22江西禾信鑫信息技术有限公司
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202210686058.0
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2022-06-16
Publication Date
2024-11-22
Estimated Expiration
2042-06-16

AI Technical Summary

Technical Problem

Existing multi-robot path planning methods are inefficient in unknown environments and are prone to repeated routes. Especially in map fusion scenarios without initial poses, it is difficult to quickly build accurate maps and plan smooth paths.

Method used

By obtaining data from lidar and cameras, building raster maps and analyzing pose information, combining artificial potential fields and RRT algorithms for path planning, using cartography and ORBSLAM3 framework to accelerate map construction and pose positioning, optimize path correction and map correction, Ensure path smoothness and accuracy.

Benefits of technology

It significantly accelerates the matching speed and exploration efficiency of maps, improves the accuracy of map fusion, and avoids obstacles. It is suitable for map fusion without initial poses, making the path formed smoother and more efficient.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN115586767B_ABST
    Figure CN115586767B_ABST
Patent Text Reader

Abstract

The present invention provides a multi-robot path planning method and device, belonging to the field of robot control, the method comprises: obtaining multiple laser data from laser radars arranged on multiple robots, and obtaining multiple visual images from cameras arranged on each robot; respectively constructing maps for multiple laser data to obtain grid maps; respectively analyzing the posture information of multiple visual images to obtain posture information; respectively constructing paths in grid maps according to the posture information to obtain robot paths; respectively performing map correction analysis on each grid map to obtain a corrected map. The present invention speeds up the matching speed of sub-graphs, improves the fusion accuracy of maps, speeds up the exploration efficiency, makes the exploration path as smooth as possible, avoids touching obstacles, and is suitable for map fusion without initial posture.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention mainly relates to the field of robot control technology, and in particular to a multi-robot path planning method and device. Background Art

[0002] Multi-robot space exploration in unknown environments has a wide range of application scenarios, including transportation, medical care, industry, rescue, etc. For example, in the event of an earthquake, robots can help rescuers assess the damage inside a building. In this case, it is important to make the assessment as quickly as possible. Compared with a single robot, multi-robot collaborative exploration of an unknown environment and building a global map takes less time and is more efficient; the collaboration between multiple robots can make the system operation more fault-tolerant, reliable, and stable.

[0003] In a multi-robot exploration system, map fusion is a critical part. Map fusion can be divided into two types: 1) Map fusion with initial pose. 2) Map fusion without initial pose. Fusion without initial pose is more difficult. It mainly estimates relative pose through transmitted data and then merges the maps. The transmitted data includes: 1) Location descriptor, which is a compact code that describes a location. Location recognition (PR) extracts and matches descriptors to find the same location between robots. 2) Sensor data, which is used to calculate the relative pose between robots based on the matched same locations.

[0004] Commonly used path planning methods include A*, RRT, D* algorithms, etc., but these methods are usually not efficient and will result in too many repeated routes in complex environments, greatly reducing the exploration efficiency. Summary of the invention

[0005] The technical problem to be solved by the present invention is to provide a multi-robot path planning method and device in view of the deficiencies in the prior art.

[0006] The technical solution of the present invention to solve the above technical problem is as follows: a multi-robot path planning method, comprising the following steps:

[0007] Obtaining a plurality of laser data corresponding to each of the robots from laser radars disposed on the plurality of robots, and obtaining a plurality of visual images corresponding to each of the robots from cameras disposed on the plurality of robots;

[0008] Mapping is performed on the plurality of laser data corresponding to each of the robots to obtain a grid map corresponding to each of the robots;

[0009] Analyzing the position and posture information of the multiple visual images corresponding to each of the robots respectively to obtain the position and posture information corresponding to each of the robots;

[0010] According to the posture information corresponding to each of the robots, paths are constructed in the grid maps corresponding to each of the robots to obtain robot paths corresponding to each of the robots;

[0011] Performing map correction analysis on each of the grid maps to obtain a corrected map corresponding to each of the robots;

[0012] Perform path correction analysis on each of the robot paths respectively to obtain a corrected robot path corresponding to each of the robots, and control each of the robots to move along the corrected robot path corresponding to each of the robots in a corrected map corresponding to each of the robots.

[0013] Another technical solution of the present invention to solve the above technical problem is as follows: a multi-robot path planning device, comprising:

[0014] A data acquisition module, used to obtain a plurality of laser data corresponding to each of the robots from laser radars arranged on the plurality of robots, and to obtain a plurality of visual images corresponding to each of the robots from cameras arranged on the plurality of robots;

[0015] A map construction module, used to construct a map for each of the plurality of laser data corresponding to each of the robots, to obtain a grid map corresponding to each of the robots;

[0016] A posture information analysis module, used to analyze the posture information of the multiple visual images corresponding to each of the robots, to obtain the posture information corresponding to each of the robots;

[0017] A path construction module, used to construct paths in the grid maps corresponding to the robots according to the posture information corresponding to the robots, so as to obtain robot paths corresponding to the robots;

[0018] A map correction analysis module, used to perform map correction analysis on each of the grid maps to obtain a corrected map corresponding to each of the robots;

[0019] The planning result acquisition module is used to perform path correction analysis on each robot path respectively, obtain a corrected robot path corresponding to each robot, and control each robot to move along the corrected robot path corresponding to each robot in a corrected map corresponding to each robot.

[0020] Another technical solution of the present invention to solve the above-mentioned technical problem is as follows: a multi-robot path planning device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the multi-robot path planning method as described above is implemented.

[0021] Another technical solution of the present invention to solve the above technical problem is as follows: a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the multi-robot path planning method as described above is implemented.

[0022] The beneficial effects of the present invention are as follows: a grid map is obtained by respectively constructing maps of multiple laser data, posture information of multiple visual images is respectively analyzed to obtain posture information, a robot path is obtained by respectively constructing paths in the grid map according to the posture information, a corrected map is obtained by respectively performing map correction analysis on the grid map, a corrected robot path is obtained by respectively performing path correction analysis on the robot path, and the robot is controlled to move along the corrected robot path in the corrected map, thereby accelerating the matching speed of sub-maps, improving the map fusion accuracy, accelerating the exploration efficiency, making the exploration path as smooth as possible, avoiding touching obstacles, and being suitable for map fusion without initial posture. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] Figure 1 A schematic diagram of a multi-robot path planning method provided by an embodiment of the present invention;

[0024] Figure 2 A module block diagram of a multi-robot path planning device provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0025] The principles and features of the present invention are described below in conjunction with the accompanying drawings. The examples given are only used to explain the present invention and are not used to limit the scope of the present invention.

[0026] Figure 1 A schematic flow chart of a multi-robot path planning method provided in an embodiment of the present invention.

[0027] like Figure 1 As shown, a multi-robot path planning method includes the following steps:

[0028] Obtaining a plurality of laser data corresponding to each of the robots from laser radars disposed on the plurality of robots, and obtaining a plurality of visual images corresponding to each of the robots from cameras disposed on the plurality of robots;

[0029] Mapping is performed on the plurality of laser data corresponding to each of the robots to obtain a grid map corresponding to each of the robots;

[0030] Analyzing the position and posture information of the multiple visual images corresponding to each of the robots respectively to obtain the position and posture information corresponding to each of the robots;

[0031] According to the posture information corresponding to each of the robots, paths are constructed in the grid maps corresponding to each of the robots to obtain robot paths corresponding to each of the robots;

[0032] Performing map correction analysis on each of the grid maps to obtain a corrected map corresponding to each of the robots;

[0033] Perform path correction analysis on each of the robot paths respectively to obtain a corrected robot path corresponding to each of the robots, and control each of the robots to move along the corrected robot path corresponding to each of the robots in a corrected map corresponding to each of the robots.

[0034] In the above embodiment, a grid map is obtained by respectively constructing maps of multiple laser data, the posture information of multiple visual images is respectively analyzed to obtain posture information, a path is respectively constructed in the grid map according to the posture information to obtain a robot path, the map correction analysis of the grid map is respectively performed to obtain a corrected map, the path correction analysis of the robot path is respectively performed to obtain a corrected robot path, and the robot is controlled to move along the corrected robot path in the corrected map, which speeds up the matching speed of the sub-maps, improves the map fusion accuracy, speeds up the exploration efficiency, makes the exploration path as smooth as possible, avoids touching obstacles, and is suitable for map fusion without initial posture.

[0035] Optionally, as an embodiment of the present invention, the process of respectively constructing a map for the plurality of laser data corresponding to each of the robots to obtain a grid map corresponding to each of the robots includes:

[0036] Based on the cartography framework, maps are constructed for the multiple laser data corresponding to each of the robots to obtain a grid map corresponding to each of the robots.

[0037] It should be understood that each robot performs SLAM of an unknown environment through cartography.

[0038] It should be understood that the cartography framework is an open source framework released by Google in 2016. The framework can access data from 2D laser, 3D laser, odometer, and IMU sensors, and output 2D maps or 3D maps.

[0039] Specifically, when cartography is building a map, it collects laser data to form a scan, i.e., a laser point cloud map, and a sub-map is created by several consecutive scans, which is composed of a probability grid of 5 cm*5 cm in size [P min ,P max ], when the sub-graph is created, the grid probability is less than P min Indicates that there is no obstacle at this point. min With P max Between means unknown, greater than P max It means that there is an obstacle at this point, and the entire map (i.e., the grid map) is composed of sub-maps connected one by one.

[0040] Specifically, the grid map divides the environment into several cells of a certain size, each of which is represented by a square. The side length of the square represents the resolution of the grid map. The state of each cell is calculated based on the posterior probability of the observation value of the robot sensor, and is represented by a number between [0,1]. The closer to 0, the greater the possibility that the cell is not occupied by an obstacle in the environment, and the closer to 1, the greater the possibility that the cell is occupied. Figure 1 It is generally used as a priori map for path planning, navigation, obstacle avoidance and other tasks. At this time, the grid map is divided into three states: occupied (indicating that there are obstacles in the real environment position corresponding to the grid), idle (indicating that there are no obstacles in the real environment position corresponding to the grid, that is, it is an area where the robot can walk), and unknown (indicating that the grid has not been observed by the sensor carried by the robot and its occupancy status is unknown).

[0041] In the above embodiment, a grid map is obtained by constructing maps of multiple laser data based on the cartography framework, which provides basic data for subsequent data processing, speeds up sub-map matching, and improves map fusion accuracy.

[0042] Optionally, as an embodiment of the present invention, the process of analyzing the posture information of the multiple visual images corresponding to each of the robots to obtain the posture information corresponding to each of the robots includes:

[0043] Based on the ORBSLAM3 framework, feature extraction is performed on the multiple visual images corresponding to each of the robots to obtain multiple visual feature maps corresponding to each of the robots;

[0044] Performing frame-to-frame matching on a plurality of visual feature maps corresponding to each of the robots to obtain a plurality of matched feature maps corresponding to each of the robots;

[0045] Obtaining a visual odometer corresponding to each of the matched feature maps respectively through each of the matched feature maps;

[0046] The multiple visual odometers corresponding to each of the robots are positioned respectively to obtain the position and posture information corresponding to each of the robots.

[0047] It should be understood that ORBSLAM3 is used to calculate visual odometry for positioning.

[0048] It should be understood that the ORBSLAM3 framework, namely ORB_SLAM3, is the first visual and visual inertial navigation system that can perform data association for short-term, medium-term and long-term data. It can run without drift in a known map environment, where mixed map data association - this allows us to perform map matching and BA optimization, which also achieves the purpose of building a map and then accurately positioning in the map.

[0049] The process of obtaining the visual odometer corresponding to each of the matched feature maps respectively through each of the matched feature maps can be understood as: extracting feature points of the image, estimating the camera motion and scene structure between the two frames through matching, and realizing a visual odometer between the two frames, specifically:

[0050] Select a more representative point from the image, which remains unchanged after a small change in the camera's perspective. In this way, we can find these points in the images of the previous and next frames, and then estimate the camera's pose. The feature points of an image can be simply understood as the more significant points in the image, such as contour points, bright spots in darker areas, dark spots in brighter areas, etc. The feature points are composed of two parts: key points and descriptors. The key points indicate the position of the feature points in the image, and the descriptors are usually a vector that describes the information around the key points in a certain artificially designed way. The descriptor designer is based on the principle that features with similar appearance should have similar descriptors. Commonly used methods include SIFT, SURF, ORB, etc.

[0051] It should be understood that the inter-frame matching, i.e. point cloud registration, can actually be understood as: obtaining a perfect coordinate transformation through calculation, and integrating the point cloud data at different perspectives into a specified coordinate system through rigid transformations such as rotation and translation.

[0052] It should be understood that on the real robot, we use ORBSLAM3 visual odometry to locate the robot and provide pose information to cartography for mapping.

[0053] In the above embodiment, the posture information of multiple visual images is analyzed to obtain the posture information, which improves the accuracy of robot path planning, speeds up exploration efficiency, and is suitable for map fusion without initial posture.

[0054] Optionally, as an embodiment of the present invention, the grid map includes a plurality of points to be identified and a plurality of obstacle points, and the process of constructing paths in the grid maps corresponding to the respective robots according to the posture information corresponding to the respective robots to obtain robot paths corresponding to the respective robots includes:

[0055] Verify one by one whether there is only one preset boundary threshold among the multiple adjacent points of the point to be identified. If the verification is successful, the point to be identified is used as a boundary point, thereby obtaining multiple boundary points corresponding to each of the robots, wherein the adjacent points are the points to be identified or obstacle points adjacent to the currently verified point to be identified;

[0056] Constructing multiple boundaries corresponding to each of the robots through multiple boundary points corresponding to each of the robots, and taking the center point of the boundary as the target point, thereby obtaining multiple target points corresponding to each of the robots;

[0057] The artificial potential field method is used to construct paths for multiple target points corresponding to each of the robots, multiple obstacle points corresponding to each of the robots, and posture information corresponding to each of the robots, so as to obtain robot paths corresponding to each of the robots.

[0058] Preferably, the preset boundary threshold may be -1.

[0059] It should be understood that the center point of the boundary is the center point of a line segment, and each boundary has only one center point.

[0060] It should be understood that if the verification is unsuccessful, no further processing is required.

[0061] It should be understood that after the unknown boundary is found, the centroid of the boundary is calculated and set as the target point.

[0062] It should be understood that an artificial potential field is added to the RRT rapidly expanding random tree algorithm to generate a new PFRRT path planning algorithm, and the algorithm is used to plan the path from the robot to the target point.

[0063] Specifically, the artificial potential field method is the idea of ​​using potential field for obstacle avoidance and path planning proposed by Khatib in 1986.

[50] , namely the artificial potential field method (APF). It comes from the concept of field in physics, which regards the movement of an object as the result of the interaction of two forces. Therefore, in this method, the robot is modeled as a particle affected by attraction and repulsion. In this idea, the robot will first create a potential field in the moving environment, denoted as U. This force field is divided into two parts: 1. The gravitational field generated by the target point, pointing to the target point. 2. The repulsive field generated by the obstacle, away from the obstacle.

[0064] The obstacle is represented by the repulsive field U rep , the goal is to attract the field U att Under the combined effect of attractive and repulsive forces, the robot moves in the direction of the combined force and reaches the target area safely, i.e. without any collision.

[0065] When the robot is traveling in the potential field, it is always affected by forces of different directions and magnitudes. When the robot is closer to an obstacle, the repulsive force is greater, and vice versa. When the robot is closer to the target point, the attractive force is smaller, and vice versa.

[0066] The force of the robot in the potential field is the combined effect of repulsion and gravity. The robot moves in a reasonable direction. The total potential field function is:

[0067] U t =U att +U rep (4-8)

[0068] In the above formula, U att represents the gravitational field, U rep represents the repulsive field, U t represents the total potential field of the robot's environment in the potential field. The resultant force it receives is expressed as:

[0069] F t =F att +F rep (4-9)

[0070] In the above formula, F att represents the gravitational force on the robot, F rep represents the repulsive force on the robot. Assume that the robot’s position in the map is x, x g Represents the coordinate position of the target point, then the potential field function of gravity is as follows:

[0071]

[0072] Where K a Represents the gravitational field coefficient, which is a constant. At this moment, the negative gradient expression of the gravitational function is:

[0073]

[0074] According to the above formula, the closer the robot is to the target point, the smaller the gravity is, and the farther away from the target point, the greater the gravity is, and the gravity is 0 when the robot reaches the target point. The definition of the repulsion function is as follows:

[0075]

[0076] In the above formula, K r is the repulsive field coefficient, which is also a constant. ρ is the distance from the robot to the obstacle. ρ0 represents the minimum influence distance of the potential field. When the distance ρ from the robot to the obstacle is greater than the influence distance ρ0 of the potential field, the repulsive force is 0, and the robot will not be affected by the repulsive force, but only by the gravitational force. The negative gradient expression of the repulsive function is as follows:

[0077]

[0078] When a robot is navigating, it is generally subject to one gravitational force and multiple repulsive forces, and the sum of the potential fields is as follows:

[0079] U t =U att +∑U rep (4-14)

[0080] Then the total force on the robot is:

[0081] F t =F att +∑F rep (4-15)

[0082] For PFRRT, we add an artificial potential field based on the RRT algorithm, and use the artificial force field to guide the selection of child nodes. Before selecting a random point, add a potential field function APF(x rand ), as shown in Algorithm 1, x rand is the sampling point set, and then calculate the minimum distance d between the current point and the obstacle min , the minimum distance and the effective distance d of repulsion obs (When the distance exceeds d obs There is no repulsion) for comparison, if d min ≤d obs , a repulsive force will be generated, otherwise the repulsive force is 0, and then through the gravitational function f att () and repulsion function f rep () Extract the gravitational vector between the current point and the target point. After obtaining the gravitational vector, the minimum increment λ will be added and pointed to the direction of the resultant force, as shown in the following formula:

[0083]

[0084] Finally, the optimized sample set x is returned.rand , through this function, the points in the sampling space are moved closer to the target point when the repulsion force allows, thereby generating a new sample x prand .

[0085] Specifically, the grid map will generate many boundaries, which are located at the junction of the explored space and the unexplored space. When the robot moves to the boundary, it will be able to explore the environment in the unknown area and update the map information. We set the center of the boundary as the target point as the robot's moving target.

[0086] Specifically, on the basis of the RRT algorithm, an artificial potential field is added to guide the selection of child nodes. Before selecting a random point, a potential field function is added to extract the gravitational vector between the current point and the target point through the gravitational function, and calculate the repulsive vector of the obstacle to the target point, and then calculate the final sum vector, and apply the sum vector to the random point to make it closer to the target point and away from the obstacle, so that the formed path is less curved and smoother.

[0087] Specifically, the specific calculation process of the child node is as follows:

[0088]

[0089]

[0090] In the algorithm, V is the vertex set, E is the edge set, ε is the growth step, C is the state space, N is the number of iterations, and G(V,E) is the graph composed of V and E. When initialized, the random tree T contains only one node: the root node x_init. First, the Sample function randomly selects a sampling point x_rand from the state space; then the Nreast function selects a node x_nreast closest to x_rand from the random tree; finally, the NewNode function obtains a new node x_new by extending a distance ε from x_nreast to x_rand. If x_new collides with an obstacle, this growth is abandoned, otherwise q_new is added to the V point set, and the edge (x_nearest, x_new) is added to the edge set E. When used for path planning, the above steps will be repeated until the distance between x_nreast and the target point x_gaol is less than a threshold, which means that the random tree has reached the target point and the algorithm returns success.

[0091] In the above embodiment, the robot path is obtained by constructing the path in the grid map according to the posture information, so that the formed path has less curvature and is smoother, the number of iterations is reduced, and the establishment of the map is accelerated.

[0092] Optionally, as an embodiment of the present invention, the process of performing map correction analysis on each of the grid maps to obtain a corrected map corresponding to each of the robots includes:

[0093] Using a spatial distribution sampling algorithm to divide each of the grid maps, respectively, to obtain a plurality of NDT detection units corresponding to each of the robots;

[0094] Constructing an initial training model, and performing training analysis on the initial training model according to a plurality of NDT detection units corresponding to each of the robots, to obtain a training model corresponding to each of the robots;

[0095] Positioning each NDT detection unit corresponding to each robot by using each training model, obtaining a plurality of sub-graph descriptors corresponding to each robot, and storing all the sub-graph descriptors in a pre-established sub-graph descriptor database;

[0096] According to the subgraph descriptor database, a plurality of subgraph descriptors corresponding to each of the robots are screened and analyzed to obtain a subgraph candidate queue corresponding to each of the robots;

[0097] Performing target image transformation matrix analysis on the sub-image candidate queues corresponding to the respective robots to obtain target image transformation matrices corresponding to the respective robots;

[0098] The corrected map corresponding to each of the robots is obtained through the target image transformation matrix corresponding to each of the robots.

[0099] It should be understood that after sampling by the spatial sampler (ie, the spatial distribution sampling algorithm), the map descriptor (ie, the sub-map descriptor) is generated using the 2D-NDT-Transformer network and stored in a public sub-map repository (ie, the sub-map descriptor database).

[0100] It should be understood that the training model locates each of the NDT detection units respectively to obtain a sub-image descriptor corresponding to each of the NDT detection units.

[0101] Specifically, the spatial distribution sampling method G is used to generate uniform NDT detection units, and the formula is as follows:

[0102]

[0103] Among them, m s , m represents the map (ie, the grid map), s represents its size (ie, the size of the grid map), and G refers to an NDT unit of size k.

[0104] It should be understood that the spatial sampling distribution samples the input map of size s (i.e., the grid map) into a fixed number of points k, and retains the original map information. The sub-map is converted into Then it is sent into the 2D-NDT-Transformer network to obtain the sub-graph descriptor.

[0105] In the above embodiment, the map correction analysis of each grid map is performed to obtain the corrected map, which can retain the original map information, speed up the exploration efficiency, and realize the map fusion without initial pose.

[0106] Optionally, as an embodiment of the present invention, the process of constructing the initial training model, respectively training and analyzing the initial training model according to a plurality of NDT detection units corresponding to each of the robots, to obtain the training model corresponding to each of the robots includes:

[0107] Constructing an NDT-Transformer point cloud network, and performing dimensionality reduction processing on the NDT-Transformer point cloud network to obtain a point cloud network after dimensionality reduction;

[0108] respectively judging whether the radius of each of the NDT detection units is equal to a preset judgment threshold, and if so, taking the NDT detection unit as a similar subgraph; if not, taking the NDT detection unit as a dissimilar subgraph, until all the NDT detection units are judged, thereby obtaining a plurality of similar subgraphs corresponding to each of the robots and a plurality of dissimilar subgraphs corresponding to each of the robots;

[0109] The reduced-dimensional point cloud network is trained according to a plurality of NDT detection units corresponding to each of the robots, a plurality of similar subgraphs corresponding to each of the robots, and a plurality of dissimilar subgraphs corresponding to each of the robots to obtain a training model corresponding to each of the robots.

[0110] It should be understood that the NDT-Transformer point cloud network is used for real-time and large-scale place recognition using 3D point clouds. Specifically, a 3D normal distribution transform (NDT) representation is used to compress the original, dense 3D point cloud into a probability distribution (NDT unit) to provide a geometric shape description. Then a new NDT-Transformer network learns a global descriptor from a set of 3D NDT unit representations. Benefiting from the NDT representation and the NDT-Transformer network, the learned global descriptor is enriched with geometric and contextual information. Finally, descriptor retrieval is achieved using a query database for place recognition.

[0111] It should be understood that NDT-Transformer (i.e., the NDT-Transformer point cloud network) is a point cloud network used for 3D scene positioning. The input layer of the network is changed to 2D, and 2D indoor data (i.e., the NDT detection unit) is collected and divided into sub-graphs (i.e., the NDT detection unit), the similar sub-graphs, and the triplets of the dissimilar sub-graphs are trained to generate a 2D-NDT-Transformer model suitable for 2D grid maps (i.e., the reduced-dimensionality point cloud network), which is used for map matching between multiple robots.

[0112] In the above embodiment, the training model is obtained by respectively analyzing the training of the initial training model by multiple NDT detection units, which can retain the original map information, speed up the exploration efficiency, and realize map fusion without initial pose.

[0113] Optionally, as an embodiment of the present invention, the process of screening and analyzing the multiple subgraph descriptors corresponding to the respective robots according to the subgraph descriptor database to obtain the subgraph candidate queues corresponding to the respective robots includes:

[0114] Comparing the Euclidean distances of the subgraph descriptors according to the subgraph descriptors in the subgraph descriptor database to obtain descriptor distances corresponding to the subgraph descriptors;

[0115] If the descriptor distance is less than a preset distance threshold, the corresponding subgraph descriptor in the subgraph descriptor database is used as a subgraph candidate descriptor and stored in a pre-established subgraph candidate queue, thereby obtaining a subgraph candidate queue corresponding to each of the robots.

[0116] Preferably, the preset distance threshold may be 0.275.

[0117] It should be understood that after the new sub-map generates a map descriptor (i.e., the sub-map descriptor), it is compared one by one with other robot sub-maps in the sub-map warehouse (i.e., the sub-map descriptors in the sub-map descriptor database). If it is less than a set threshold (i.e., the preset distance threshold), it is considered to be a nearby sub-map (i.e., the sub-map candidate descriptor) and is added to the sub-map candidate queue.

[0118] Specifically, the descriptors of the newly generated sub-graph are compared with those of the stored sub-graph. represents the distance between subgraph i of robot α and subgraph j of robot β, α i Subgraph i, β representing robot α j Represents the subgraph j of robot β, where a d is set th (d th=0.275) threshold, select subgraphs below the threshold to add to the candidate queue, and filter out subgraphs above the threshold.

[0119] In the above embodiment, a subgraph candidate queue is obtained by screening and analyzing a plurality of subgraph descriptors according to the subgraph descriptor database, and nearby subgraphs can be selected, thereby filtering out some useless subgraphs.

[0120] Optionally, as an embodiment of the present invention, the process of performing target image transformation matrix analysis on the sub-image candidate queues corresponding to the respective robots to obtain the target image transformation matrix corresponding to the respective robots includes:

[0121] Using the AKAZE algorithm to extract features from the sub-image candidate descriptors in each of the sub-image candidate queues, respectively, to obtain feature points corresponding to each of the sub-image candidate descriptors;

[0122] Using the OpenCV tool to perform image matching on multiple feature points corresponding to each of the robots, to obtain an original image transformation matrix corresponding to each of the robots, a scale factor corresponding to each of the robots, and an initial confidence level corresponding to each of the robots;

[0123] Each of the proportional factors and the initial confidence corresponding to each of the robots is normalized by the first formula to obtain the target confidence corresponding to each of the robots. The first formula is:

[0124]

[0125] in, is the target confidence, is the scale factor, is the initial confidence;

[0126] The original image transformation matrix corresponding to each of the robots is screened according to each of the target confidences to obtain a target image transformation matrix corresponding to each of the robots.

[0127] It should be understood that the AKAZE algorithm, i.e., the AKAZE feature algorithm, is an improved version of the SIFT feature algorithm, but does not use Gaussian blur to construct the scale space because Gaussian blur has the disadvantage of losing edge information. Instead, nonlinear diffusion filtering is used to construct the scale space, thereby retaining more edge features of the image.

[0128] It should be understood that the OpenCV tool is a cross-platform computer vision and machine learning software library released under the Apache 2.0 license (open source), which can run on Linux, Windows, Android and Mac OS operating systems. It is lightweight and efficient - consisting of a series of C functions and a small number of C++ classes, while providing interfaces for languages ​​such as Python, Ruby, and MATLAB, and implementing many common algorithms in image processing and computer vision.

[0129] It should be understood that AKAZE feature extraction is performed on the new sub-image and all sub-images in the sub-image candidate queue (i.e., the sub-image candidate descriptors in the sub-image candidate queue), and their confidence is calculated, and the most similar sub-image is selected as the same sub-image, and pose transformation and map fusion are performed based on this sub-image.

[0130] It should be understood that the AKAZE algorithm in OpenCV extracts feature points from the sub-image, and then AffineBestOf2NearestMatcher() is used to obtain the image transformation (i.e., the original image transformation matrix), the scaling factor and the confidence level of the image (i.e. the initial confidence level). At the same time, the sub-images should be of the same size, so the scale factor is used for normalization to obtain the final image transformation (i.e., the target image transformation matrix), the final confidence becomes (i.e., the target confidence), the calculation formula is as follows:

[0131]

[0132] The confidence threshold c is set in the experiment th (c th = 0.5), if the final confidence is greater than the threshold, it is considered to be the same location. Then the relative position of the robot is used to fuse the map.

[0133] In the above embodiment, the target image transformation matrix of the sub-image candidate queue is analyzed respectively to obtain the target image transformation matrix, which speeds up the sub-image matching speed, improves the accuracy of map fusion, and improves the efficiency of map exploration.

[0134] Optionally, as an embodiment of the present invention, the robot path includes distances from multiple robots to points, and the process of performing path correction analysis on each of the robot paths to obtain a corrected robot path corresponding to each of the robots includes:

[0135] The repulsive potential field values ​​of the distances from the multiple robots corresponding to the respective robots to the point are calculated by the second formula to obtain multiple repulsive potential field values ​​corresponding to the respective robots. The second formula is:

[0136]

[0137] Among them, P r (α,i) is the repulsive potential field value, d s is the laser radar range value, r d (α,i) is the distance from the robot to the point, α is the robot number, and i is the point in the grid map;

[0138] The paths of the robots are corrected according to the multiple repulsive potential field values ​​corresponding to the robots, so as to obtain the corrected robot paths corresponding to the robots.

[0139] It should be understood that, through the artificial potential field method, the robot directly generates a repulsive force, so that the robot does not explore the same area.

[0140] Specifically, multiple robots should explore different directions, so a repulsive force is introduced and added to each robot. The repulsive potential field of robot α at point i is shown in the formula:

[0141]

[0142] Among them, d s represents the laser sensor range, r d (α,i) is the distance from the robot to the point, k r is the repulsion coefficient. Through this formula, when the robots are about to meet, they will feel the repulsion between each other and explore other exploration points.

[0143] In the above embodiment, the repulsive potential field values ​​of the distances from multiple robots to the point are calculated by the second formula to obtain multiple repulsive potential field values, and the path of each robot is corrected according to the multiple repulsive potential field values ​​to obtain the corrected robot path, thereby avoiding the problem of too many repeated routes in complex environments and reducing the exploration efficiency, thereby improving the efficiency of map exploration.

[0144] Optionally, as another embodiment of the present invention, when the number of boundary points is 0, the robot has completed the exploration.

[0145] Optionally, as another embodiment of the present invention, the present invention uses NDT-Transformer to construct a map fusion framework, which enables multiple robots to perform pose estimation and map fusion through subgraphs shared between robots; and uses the PFRRT path planning algorithm based on artificial potential field and RRT to reduce the number of iterations and speed up map establishment.

[0146] Optionally, as another embodiment of the present invention, the technical problems to be solved by the present invention are: 1. For the map fusion problem without initial pose, a sub-graph-based map fusion method is provided to accelerate the sub-graph matching speed and improve the map fusion accuracy. 2. A collaborative method between multiple robots is provided to enable them to explore different areas and accelerate the exploration efficiency. 3. A path planning method is provided to make the exploration path as smooth as possible to avoid touching obstacles.

[0147] Optionally, as another embodiment of the present invention, the beneficial effects of the present invention are: a map fusion framework is constructed through 2D-NDT-Transformer, and through the subgraph shared between robots, multiple robots can perform pose estimation and map fusion. In the RRT path planning algorithm, an artificial potential field is added to make its random points more orderly, the tree has fewer branches, the number of iterations is reduced, and the path curve is smoother, thereby speeding up the establishment of the map.

[0148] Optionally, as another embodiment of the present invention, the present invention combines the 2D-NDT-Transformer network with the cartography subgraph to construct a map fusion framework, and at the same time, adds an artificial potential field on the basis of RRT to form a new PFRRT path planning algorithm. Combining these two points, a multi-robot exploration system in an unknown environment is proposed. The ordinary RRT path planning algorithm has more branches and a longer path; while the PFRRT algorithm, after adding an artificial potential field, has fewer path branches and a smoother path, which speeds up the exploration efficiency. It can be seen that the present invention has the function of exploring the environment in an unknown environment, and at the same time, the maps of multiple robots can be perfectly integrated to improve the efficiency of exploration.

[0149] Figure 2 A module block diagram of a multi-robot path planning device provided in an embodiment of the present invention.

[0150] Optionally, as another embodiment of the present invention, Figure 2 As shown, a multi-robot path planning device comprises:

[0151] A data acquisition module, used to obtain a plurality of laser data corresponding to each of the robots from laser radars arranged on the plurality of robots, and to obtain a plurality of visual images corresponding to each of the robots from cameras arranged on the plurality of robots;

[0152] A map construction module, used to construct a map for each of the plurality of laser data corresponding to each of the robots, to obtain a grid map corresponding to each of the robots;

[0153] A posture information analysis module, used to analyze the posture information of the multiple visual images corresponding to each of the robots, to obtain the posture information corresponding to each of the robots;

[0154] A path construction module, used to construct paths in the grid maps corresponding to the robots according to the posture information corresponding to the robots, so as to obtain robot paths corresponding to the robots;

[0155] A map correction analysis module, used to perform map correction analysis on each of the grid maps to obtain a corrected map corresponding to each of the robots;

[0156] The planning result acquisition module is used to perform path correction analysis on each robot path respectively, obtain a corrected robot path corresponding to each robot, and control each robot to move along the corrected robot path corresponding to each robot in a corrected map corresponding to each robot.

[0157] Optionally, another embodiment of the present invention provides a multi-robot path planning device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, the multi-robot path planning method described above is implemented. The device may be a computer or other device.

[0158] Optionally, another embodiment of the present invention provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the multi-robot path planning method as described above is implemented.

[0159] It should be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device.

[0160] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working processes of the above-described devices and units can refer to the corresponding processes in the aforementioned method embodiments and will not be repeated here.

[0161] In the several embodiments provided in this application, it should be understood that the disclosed devices and methods can be implemented in other ways. For example, the device embodiments described above are only schematic, for example, the division of units is only a logical function division, and there may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed.

[0162] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the embodiments of the present invention.

[0163] In addition, each functional unit in each embodiment of the present invention may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

[0164] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention is essentially or the part that contributes to the prior art, or all or part of the technical solution can be embodied in the form of a software product. The computer software product is stored in a storage medium, including several instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the methods of each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk and other media that can store program codes.

[0165] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims

1. A multi-robot path planning method, characterized in that: The steps include: Obtaining a plurality of laser data corresponding to each of the robots from laser radars disposed on the plurality of robots, and obtaining a plurality of visual images corresponding to each of the robots from cameras disposed on the plurality of robots; Mapping is performed on the plurality of laser data corresponding to each of the robots to obtain a grid map corresponding to each of the robots; Analyzing the position and posture information of the multiple visual images corresponding to each of the robots respectively to obtain the position and posture information corresponding to each of the robots; According to the posture information corresponding to each of the robots, paths are constructed in the grid maps corresponding to each of the robots to obtain robot paths corresponding to each of the robots; Performing map correction analysis on each of the grid maps to obtain a corrected map corresponding to each of the robots; Performing path correction analysis on each of the robot paths respectively to obtain a corrected robot path corresponding to each of the robots, and controlling each of the robots to move along the corrected robot path corresponding to each of the robots in a corrected map corresponding to each of the robots; The process of performing map correction analysis on each of the grid maps to obtain a corrected map corresponding to each of the robots comprises: Using a spatial distribution sampling algorithm to divide each of the grid maps, respectively, to obtain a plurality of NDT detection units corresponding to each of the robots; Constructing an initial training model, and performing training analysis on the initial training model according to a plurality of NDT detection units corresponding to each of the robots, to obtain a training model corresponding to each of the robots; Positioning each NDT detection unit corresponding to each robot by using each training model, obtaining a plurality of sub-graph descriptors corresponding to each robot, and storing all the sub-graph descriptors in a pre-established sub-graph descriptor database; According to the subgraph descriptor database, a plurality of subgraph descriptors corresponding to each of the robots are screened and analyzed to obtain a subgraph candidate queue corresponding to each of the robots; Performing target image transformation matrix analysis on the sub-image candidate queues corresponding to the respective robots to obtain target image transformation matrices corresponding to the respective robots; Obtaining a corrected map corresponding to each of the robots through a target image transformation matrix corresponding to each of the robots; The robot path includes distances from multiple robots to points, and the process of performing path correction analysis on each of the robot paths to obtain a corrected robot path corresponding to each of the robots includes: The repulsive potential field values ​​of the distances from the multiple robots corresponding to the respective robots to the point are calculated by the second formula to obtain multiple repulsive potential field values ​​corresponding to the respective robots. The second formula is: Among them, P r (α,i) is the repulsive potential field value, d s is the laser radar range value, r d (α,i) is the distance from the robot to the point, α is the robot number, and i is the point in the grid map; The paths of the robots are corrected according to the multiple repulsive potential field values ​​corresponding to the robots, so as to obtain the corrected robot paths corresponding to the robots.

2. The multi-robot path planning method according to claim 1, characterized in that: The process of constructing a map for each of the plurality of laser data corresponding to each of the robots to obtain a grid map corresponding to each of the robots comprises: Based on the cartography framework, maps are constructed for the multiple laser data corresponding to each of the robots to obtain a grid map corresponding to each of the robots.

3. The multi-robot path planning method according to claim 1, characterized in that: The process of analyzing the posture information of the multiple visual images corresponding to each of the robots to obtain the posture information corresponding to each of the robots comprises: Based on the ORBSLAM3 framework, feature extraction is performed on the multiple visual images corresponding to each of the robots to obtain multiple visual feature maps corresponding to each of the robots; Performing frame-to-frame matching on a plurality of visual feature maps corresponding to each of the robots to obtain a plurality of matched feature maps corresponding to each of the robots; Obtaining a visual odometer corresponding to each of the matched feature maps respectively through each of the matched feature maps; The multiple visual odometers corresponding to each of the robots are positioned respectively to obtain the position and posture information corresponding to each of the robots.

4. The multi-robot path planning method according to claim 1, characterized in that: The grid map includes a plurality of points to be identified and a plurality of obstacle points. The process of constructing paths in the grid maps corresponding to the robots according to the posture information corresponding to the robots to obtain the robot paths corresponding to the robots includes: Verify one by one whether there is only one preset boundary threshold among the multiple adjacent points of the point to be identified. If the verification is successful, the point to be identified is used as a boundary point, thereby obtaining multiple boundary points corresponding to each of the robots, wherein the adjacent points are the points to be identified or obstacle points adjacent to the currently verified point to be identified; Constructing multiple boundaries corresponding to each of the robots through multiple boundary points corresponding to each of the robots, and taking the center point of the boundary as the target point, thereby obtaining multiple target points corresponding to each of the robots; The artificial potential field method is used to construct paths for multiple target points corresponding to each of the robots, multiple obstacle points corresponding to each of the robots, and posture information corresponding to each of the robots, so as to obtain robot paths corresponding to each of the robots.

5. The multi-robot path planning method according to claim 1, characterized in that: The process of constructing the initial training model and performing training analysis on the initial training model according to the multiple NDT detection units corresponding to the respective robots to obtain the training model corresponding to the respective robots includes: Constructing an NDT-Transformer point cloud network, and performing dimensionality reduction processing on the NDT-Transformer point cloud network to obtain a point cloud network after dimensionality reduction; respectively judging whether the radius of each of the NDT detection units is equal to a preset judgment threshold, and if so, taking the NDT detection unit as a similar subgraph; if not, taking the NDT detection unit as a dissimilar subgraph, until all the NDT detection units are judged, thereby obtaining a plurality of similar subgraphs corresponding to each of the robots and a plurality of dissimilar subgraphs corresponding to each of the robots; The reduced-dimensional point cloud network is trained according to a plurality of NDT detection units corresponding to each of the robots, a plurality of similar subgraphs corresponding to each of the robots, and a plurality of dissimilar subgraphs corresponding to each of the robots to obtain a training model corresponding to each of the robots.

6. The multi-robot path planning method according to claim 1, characterized in that: The process of screening and analyzing the multiple subgraph descriptors corresponding to the respective robots according to the subgraph descriptor database to obtain the subgraph candidate queues corresponding to the respective robots comprises: Comparing the Euclidean distances of the subgraph descriptors according to the subgraph descriptors in the subgraph descriptor database to obtain descriptor distances corresponding to the subgraph descriptors; If the descriptor distance is less than a preset distance threshold, the corresponding subgraph descriptor in the subgraph descriptor database is used as a subgraph candidate descriptor and stored in a pre-established subgraph candidate queue, thereby obtaining a subgraph candidate queue corresponding to each of the robots.

7. The multi-robot path planning method according to claim 6, characterized in that: The process of respectively analyzing the target image transformation matrix of the sub-image candidate queues corresponding to the respective robots to obtain the target image transformation matrix corresponding to the respective robots comprises: Using the AKAZE algorithm to extract features from the sub-image candidate descriptors in each of the sub-image candidate queues, respectively, to obtain feature points corresponding to each of the sub-image candidate descriptors; Using the OpenCV tool to perform image matching on multiple feature points corresponding to each of the robots, to obtain an original image transformation matrix corresponding to each of the robots, a scale factor corresponding to each of the robots, and an initial confidence level corresponding to each of the robots; Each of the proportional factors and the initial confidence corresponding to each of the robots is normalized by the first formula to obtain the target confidence corresponding to each of the robots. The first formula is: in, is the target confidence, is the scale factor, is the initial confidence; The original image transformation matrix corresponding to each of the robots is screened according to each of the target confidences to obtain a target image transformation matrix corresponding to each of the robots.

8. A multi-robot path planning device, characterized in that: include: A data acquisition module, used to obtain a plurality of laser data corresponding to each of the robots from laser radars arranged on the plurality of robots, and to obtain a plurality of visual images corresponding to each of the robots from cameras arranged on the plurality of robots; A map construction module, used to construct a map for each of the plurality of laser data corresponding to each of the robots, to obtain a grid map corresponding to each of the robots; A posture information analysis module, used to analyze the posture information of the multiple visual images corresponding to each of the robots, to obtain the posture information corresponding to each of the robots; A path construction module, used to construct paths in the grid maps corresponding to the robots according to the posture information corresponding to the robots, so as to obtain robot paths corresponding to the robots; A map correction analysis module, used to perform map correction analysis on each of the grid maps to obtain a corrected map corresponding to each of the robots; A planning result acquisition module, used to perform path correction analysis on each of the robot paths, obtain a corrected robot path corresponding to each of the robots, and control each of the robots to move along the corrected robot path corresponding to each of the robots in a corrected map corresponding to each of the robots; The map correction analysis module is specifically used for: Using a spatial distribution sampling algorithm to divide each of the grid maps, respectively, to obtain a plurality of NDT detection units corresponding to each of the robots; Constructing an initial training model, and performing training analysis on the initial training model according to a plurality of NDT detection units corresponding to each of the robots, to obtain a training model corresponding to each of the robots; Positioning each NDT detection unit corresponding to each robot by using each training model, obtaining a plurality of sub-graph descriptors corresponding to each robot, and storing all the sub-graph descriptors in a pre-established sub-graph descriptor database; According to the subgraph descriptor database, a plurality of subgraph descriptors corresponding to each of the robots are screened and analyzed to obtain a subgraph candidate queue corresponding to each of the robots; Performing target image transformation matrix analysis on the sub-image candidate queues corresponding to the respective robots to obtain target image transformation matrices corresponding to the respective robots; Obtaining a corrected map corresponding to each of the robots through a target image transformation matrix corresponding to each of the robots; The robot path includes distances from multiple robots to points. In the planning result acquisition module, the process of performing path correction analysis on each of the robot paths to obtain a corrected robot path corresponding to each of the robots includes: The repulsive potential field values ​​of the distances from the multiple robots corresponding to the respective robots to the point are calculated by the second formula to obtain multiple repulsive potential field values ​​corresponding to the respective robots. The second formula is: Among them, P r (α,i) is the repulsive potential field value, d s is the laser radar range value, r d (α,i) is the distance from the robot to the point, α is the robot number, and i is the point in the grid map; The paths of the robots are corrected according to the multiple repulsive potential field values ​​corresponding to the robots, so as to obtain the corrected robot paths corresponding to the robots.