A self-interference continuous elimination method for multi-unmanned ship unknown area cooperative exploration

Abstract

The application provides a kind of self-interference continuous elimination method for the cooperative exploration of multiple unmanned ships in unknown areas, belonging to the technical field of autonomous exploration, autonomous positioning and map construction of multiple unmanned ships. To solve the self-interference problem that occurs when multiple unmanned ships cooperatively explore unknown areas, a coordinate relationship established by SLAM and multiple unmanned ship map fusion is used in combination with the iterative closest point registration algorithm to propose a laser radar point cloud method for continuously processing dynamic obstacles, which eliminates the self-interference of multiple unmanned ships in the cooperative exploration of unknown areas. In addition, for the laser radar point cloud data generated by dynamic obstacles, random points are formed in the map, which leads to poor subsequent multi-map fusion results and ultimately fails the exploration task. Through the coordinate system conversion relationship between unmanned ships, the application calculates the occurrence time of the self-interference problem and the initial position of the dynamic obstacle, and uses the iterative closest point to continuously track the position of the dynamic obstacle, achieving the purpose of continuously eliminating the influence of self-interference.

Description

Technical Field

[0001] This invention relates to the field of autonomous exploration, autonomous positioning, and map building technology for multiple unmanned vessels, and more specifically, to a method for continuous elimination of self-interference in collaborative exploration of unknown areas by multiple unmanned vessels. Background Technology

[0002] With advancements in technology and understanding, human research has long since moved beyond the relatively simple Earth's surface, extending its reach to complex regions such as the deep ocean, underground, and outer space. These areas often possess harsh and unpredictable environments, posing significant challenges to human exploration efforts.

[0003] Unmanned surface vessels (USVs) can replace humans in completing some dangerous tasks, but their operation requires high-precision electronic maps. Therefore, many researchers have conducted extensive research on autonomous exploration algorithms for USVs. Autonomous exploration architectures using USVs equipped with SLAM modules and boundary exploration algorithms based on rapid extended random trees or image edge detection are relatively mature and have high practical value. However, the exploration efficiency of a single USV has an upper limit, so researchers have turned their attention to collaborative exploration using multiple USVs.

[0004] Compared to single-vehicle autonomous exploration, multi-vehicle autonomous exploration significantly improves exploration efficiency but also introduces obvious problems. During exploration, it is difficult for unmanned vessels to avoid appearing within each other's lidar detection range, potentially becoming dynamic obstacles. This leads to noise in the unmanned vessel's map building caused by dynamic obstacles, and in special cases, obstacles may even appear out of nowhere, causing self-interference problems.

[0005] Current methods for identifying dynamic obstacles require calculations of changes across multiple frames of probabilistic maps, resulting in significant computational demands. However, due to the limited computing resources and payload capacity of unmanned surface vessels (USVs), this massive computational load will significantly reduce the system's real-time performance, thereby adversely affecting the safety of the USV and the accuracy of the maps.

[0006] Therefore, this invention innovatively proposes a lightweight and low-power multi-unmanned vessel collaborative exploration method, which can continuously eliminate the self-interference problem in unmanned vessel exploration while ensuring system efficiency and stability. Summary of the Invention

[0007] Based on the aforementioned technical problem of self-interference in autonomous exploration by multiple unmanned vessels, this invention proposes a lightweight and low-power collaborative exploration method for multiple unmanned vessels. This method can continuously eliminate the self-interference problem in unmanned vessel exploration while ensuring system efficiency and stability. By continuously eliminating lidar point cloud data caused by dynamic obstacles, this invention aims to improve the success rate of exploration missions and build high-precision, high-quality maps.

[0008] The technical means employed in this invention are as follows:

[0009] A method for continuous self-interference elimination in collaborative exploration of unknown areas by multiple unmanned vessels includes:

[0010] S1. Obtain the coordinate transformation relationship between unmanned surface vessels (USVs) from the system's TF tree, and calculate the set of rotation matrices {R} between USVs. i} set of translation matrices {t i};

[0011] S2. Calculate the spacing {d} between the unmanned vessels using a translation matrix. i If {d} exists i} is less than the range ε, i.e., d i If the value is less than or equal to ε, a self-interference alarm will be triggered.

[0012] S3, using angle and lidar depth information with d i If the error is within the allowable range, it is considered that there is a self-interference problem, and the relevant unmanned vessel will enter the process of resolving the self-interference problem; otherwise, it is a false alarm, and the process will directly jump to step S6 to release radar data.

[0013] S4. After converting the lidar data to the Cartesian coordinate system, use DBSCAN to perform clustering processing on the lidar data.

[0014] S5. Use the ICP algorithm to obtain the matching relationship between the current frame of lidar data and the previous frame of lidar data, analyze the dynamic obstacle position relationship between the two frames of lidar data, and process the lidar point cloud of dynamic obstacles.

[0015] S6. Publish the LiDAR data before and after processing;

[0016] S7. If the exploration task is not completed, return to step S1 to continue exploring; otherwise, end the exploration task.

[0017] Further, step S1 specifically includes:

[0018] S11. The coordinate transformation relationship of the unmanned vessel is established using the SLAM module and the multi-map fusion module. The SLAM module calculates the coordinate relationship between the unmanned vessel and the unmanned vessel sub-map; the multi-map fusion module calculates the coordinate relationship between multiple unmanned vessel maps.

[0019] S12. Calculate the rotation matrix R between the unmanned vessels. The calculation formula is as follows:

[0020]

[0021] Among them, R z The rotation matrix represents the rotation angle ψ around the z-axis;

[0022] S13. Calculate the translation matrix t between the unmanned vessels. The calculation formula is as follows:

[0023] t=[Δx Δy 0] T

[0024] Where Δx and Δy represent the translation distances along the x-axis and y-axis, respectively.

[0025] Furthermore, in step S2, the range ε ​​is greater than the effective scanning range of the lidar.

[0026] Further, step S5 specifically includes:

[0027] S51. Use ICP to calculate the matching cluster of dynamic obstacles in two frames of LiDAR data, and calculate the center point coordinates of all elements in the cluster. The calculation formula is as follows:

[0028]

[0029]

[0030] S52. Obtain the coordinates of the center point of the dynamic obstacle in the previous frame. The coordinates of the center point of the dynamic obstacle in this frame are:

[0031] S53. Analyze the dynamic obstacle position relationship between two frames of LiDAR data, process the LiDAR point cloud of dynamic obstacles, if Dist(p,q)≤λ*V max If the dynamic obstacle matching is successful, then the dynamic obstacle matching is successful; where Dist(p,q) is the Euclidean distance between p and q, and V max This is the maximum speed of the unmanned vessel; λ should be slightly greater than 1 to avoid minor errors.

[0032] Further, in step S6:

[0033] Release the unprocessed lidar data to provide data for the dynamic path planning of unmanned vessels and ensure the safety of unmanned vessel movement;

[0034] The processed LiDAR data is published to eliminate LiDAR noise caused by self-interference and improve the quality of the constructed map.

[0035] Compared with the prior art, the present invention has the following advantages:

[0036] 1. The self-interference continuous elimination method for collaborative exploration of unknown areas by multiple unmanned vessels provided by this invention aims to improve the success rate of exploration missions and build high-precision, high-quality maps by continuously eliminating lidar point cloud data caused by dynamic obstacles.

[0037] 2. The self-interference continuous elimination method for collaborative exploration of unknown areas by multiple unmanned vessels provided by the present invention can not only continuously eliminate the self-interference problem in the exploration of unmanned vessels, but also ensure the efficiency and stability of the system.

[0038] Based on the above reasons, this invention can be widely applied in fields such as autonomous exploration, autonomous positioning, and map building of unmanned vessels. Attached Figure Description

[0039] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0040] Figure 1 This is a flowchart of the method of the present invention.

[0041] Figure 2 This is a simplified schematic diagram of a TF tree provided in an embodiment of the present invention.

[0042] Figure 3 This is a schematic diagram of dynamic obstacle tracking provided for an embodiment of the present invention. Detailed Implementation

[0043] It should be noted that, unless otherwise specified, the embodiments and features described in the present invention can be combined with each other. The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0044] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. The following description of at least one exemplary embodiment is merely illustrative and is in no way intended to limit the present invention or its application or use. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0045] It should be noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the scope of exemplary embodiments according to the invention. As used herein, the singular form is intended to include the plural form as well, unless the context clearly indicates otherwise. Furthermore, it should be understood that when the terms "comprising" and / or "including" are used in this specification, they indicate the presence of features, steps, operations, devices, components, and / or combinations thereof.

[0046] Unless otherwise specifically stated, the relative arrangement, numerical expressions, and values ​​of the components and steps described in these embodiments do not limit the scope of the invention. It should also be understood that, for ease of description, the dimensions of the various parts shown in the drawings are not drawn to actual scale. Techniques, methods, and devices known to those skilled in the art may not be discussed in detail, but where appropriate, such techniques, methods, and devices should be considered part of the specification. In all examples shown and discussed herein, any specific values ​​should be interpreted as merely exemplary and not as limitations. Therefore, other examples of exemplary embodiments may have different values. It should be noted that similar reference numerals and letters in the following figures denote similar items; therefore, once an item is defined in one figure, it need not be further discussed in subsequent figures.

[0047] In the description of this invention, it should be understood that the orientation or positional relationship indicated by directional terms such as "front, back, up, down, left, right", "horizontal, vertical, horizontal" and "top, bottom" is generally based on the orientation or positional relationship shown in the accompanying drawings, and is only for the convenience of describing this invention and simplifying the description. Unless otherwise stated, these directional terms do not indicate or imply that the device or element referred to must have a specific orientation or be constructed and operated in a specific orientation, and therefore should not be construed as a limitation on the scope of protection of this invention. The directional terms "inner" and "outer" refer to the inner and outer contours relative to the outline of each component itself.

[0048] For ease of description, spatial relative terms such as "above," "over," "on the upper surface of," "above," etc., are used herein to describe the spatial positional relationship of a device or feature as shown in the figures to other devices or features. It should be understood that spatial relative terms are intended to encompass different orientations in use or operation besides the orientation of the device as described in the figures. For example, if the device in the figures is inverted, a device described as "above" or "above" other devices or structures would subsequently be positioned as "below" or "under" other devices or structures. Thus, the exemplary term "above" can include both "above" and "below." The device may also be positioned in other different ways (rotated 90 degrees or in other orientations), and the spatial relative descriptions used herein will be interpreted accordingly.

[0049] Furthermore, it should be noted that the use of terms such as "first" and "second" to define components is merely for the purpose of distinguishing the corresponding components. Unless otherwise stated, the above terms have no special meaning and therefore should not be construed as limiting the scope of protection of this invention.

[0050] like Figure 1 As shown, this invention provides a method for continuous self-interference elimination in collaborative exploration of unknown areas by multiple unmanned vessels, including:

[0051] S1. Obtain the coordinate transformation relationship between unmanned surface vessels (USVs) from the system's TF tree, and calculate the set of rotation matrices {R} between USVs. i} set of translation matrices {t i};

[0052] S2. Calculate the spacing {d} between the unmanned vessels using a translation matrix. i If {d} exists i} is less than the range ε, i.e., d i If the value is less than or equal to ε, a self-interference alarm will be triggered.

[0053] S3, using angle and lidar depth information with d i If the error is within the allowable range, it is considered that there is a self-interference problem, and the relevant unmanned vessel will enter the process of resolving the self-interference problem; otherwise, it is a false alarm, and the process will directly jump to step S6 to release radar data.

[0054] S4. After converting the lidar data to the Cartesian coordinate system, use DBSCAN to perform clustering processing on the lidar data.

[0055] S5. Use the ICP algorithm to obtain the matching relationship between the current frame of lidar data and the previous frame of lidar data, analyze the dynamic obstacle position relationship between the two frames of lidar data, and process the lidar point cloud of dynamic obstacles.

[0056] S6. Publish the LiDAR data before and after processing;

[0057] S7. If the exploration task is not completed, return to step S1 to continue exploring; otherwise, end the exploration task.

[0058] In a specific implementation, as a preferred embodiment of the present invention, step S1 specifically includes:

[0059] S11. The coordinate transformation relationship of the unmanned vessel is established using the SLAM module and the multi-map fusion module. The SLAM module calculates the coordinate relationship between the unmanned vessel and the unmanned vessel sub-map; the multi-map fusion module calculates the coordinate relationship between multiple unmanned vessel maps.

[0060] S12. Calculate the rotation matrix R between the unmanned vessels. The calculation formula is as follows:

[0061]

[0062] Among them, R z The rotation matrix represents the rotation angle ψ around the z-axis;

[0063] S13. Calculate the translation matrix t between the unmanned vessels. The calculation formula is as follows:

[0064] t=[Δx Δy 0] T

[0065] Where Δx and Δy represent the translation distances along the x-axis and y-axis, respectively.

[0066] In this embodiment, as Figure 2 The diagram shows a simplified TF tree with multiple coordinate systems. Taking collaborative exploration by three unmanned surface vessels (USVs) as an example, `tb3_1_baselink` represents the coordinate system of USV 1, and `tb3_1_map` represents the map coordinate system constructed by the SLAM module of USV 1. The multi-map fusion module merges the sub-maps `tb3_1_map`, `tb3_2_map`, and `tb3_3_map` of the three USVs to obtain the world map and the world coordinate system transformation relationship. USVs can indirectly and quickly calculate their coordinate relationships with other USVs from this TF tree.

[0067] In a specific implementation, as a preferred embodiment of the present invention, in step S2, the range ε ​​is greater than the effective scanning range of the lidar.

[0068] In a specific implementation, as a preferred embodiment of the present invention, step S5 specifically includes:

[0069] S51. Use ICP to calculate the matching cluster of dynamic obstacles in two frames of LiDAR data, and calculate the center point coordinates of all elements in the cluster. The calculation formula is as follows:

[0070]

[0071]

[0072] S52. Obtain the coordinates of the center point of the dynamic obstacle in the previous frame. The coordinates of the center point of the dynamic obstacle in this frame are:

[0073] S53. Analyze the dynamic obstacle position relationship between two frames of LiDAR data, process the LiDAR point cloud of dynamic obstacles, if Dist(p,q)≤λ*V max If the dynamic obstacle matching is successful, then the dynamic obstacle matching is successful; where Dist(p,q) is the Euclidean distance between p and q, and V max This is the maximum speed of the unmanned vessel; λ should be slightly greater than 1 to avoid minor errors.

[0074] In this embodiment, as Figure 3 The image shown is a schematic diagram of dynamic obstacle point cloud matching in two frames of LiDAR data. Figure 3 In the image, P1, P2, and P3 represent the point cloud data of the dynamic obstacle in the previous frame of LiDAR data, and M... P Let P1, P2, and P3 be the coordinate centers of these three points, and Q1, Q2, and Q3 represent the point cloud data of the dynamic obstacle in the previous frame of LiDAR data. M Q Let M be the coordinate center of three points Q1, Q2, and Q3. Two frames of dynamic obstacle point clouds are matched using the ICP algorithm, and M is calculated. P With M Q The relationship between distance and speed of the unmanned vessel can be used to analyze the motion state of dynamic obstacles.

[0075] In a specific implementation, as a preferred embodiment of the present invention, in step S6:

[0076] Release the unprocessed lidar data to provide data for the dynamic path planning of unmanned vessels and ensure the safety of unmanned vessel movement;

[0077] The processed LiDAR data is published to eliminate LiDAR noise caused by self-interference and improve the quality of the constructed map.

[0078] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for continuous self-interference elimination in collaborative exploration of unknown areas by multiple unmanned vessels, characterized in that, include: S1. Obtain the coordinate transformation relationship between unmanned surface vessels (USVs) from the system's TF tree, and calculate the set of rotation matrices {R} between USVs. i } set of translation matrices {t i }; S2. Calculate the spacing {d} between the unmanned vessels using a translation matrix. i If {d} exists i } is less than the range ε, i.e., d i If the value is less than or equal to ε, a self-interference alarm will be triggered. S3, using angle and lidar depth information with d i If the error is within the allowable range, it is considered that there is a self-interference problem, and the relevant unmanned vessel will enter the process of solving the self-interference problem. Otherwise, if a false alarm is triggered, proceed directly to step S6 to release radar data; S4. After converting the lidar data to the Cartesian coordinate system, use DBSCAN to perform clustering processing on the lidar data. S5. Use the ICP algorithm to obtain the matching relationship between the current frame of lidar data and the previous frame of lidar data, analyze the dynamic obstacle position relationship between the two frames of lidar data, and process the lidar point cloud of dynamic obstacles. S6. Publish the LiDAR data before and after processing; S7. If the exploration task is not completed, return to step S1 to continue exploring; otherwise, end the exploration task.

2. The self-interference continuous elimination method for collaborative exploration of unknown areas by multiple unmanned vessels according to claim 1, characterized in that, Step S1 specifically includes: S11. The coordinate transformation relationship of the unmanned vessel is established using the SLAM module and the multi-map fusion module. The SLAM module calculates the coordinate relationship between the unmanned vessel and the unmanned vessel sub-map; the multi-map fusion module calculates the coordinate relationship between multiple unmanned vessel maps. S12. Calculate the rotation matrix R between the unmanned vessels. The calculation formula is as follows: Among them, R z The rotation matrix represents the rotation angle ψ around the z-axis; S13. Calculate the translation matrix t between the unmanned vessels. The calculation formula is as follows: t=[ΔxΔy 0] T Where Δx and Δy represent the translation distances along the x-axis and y-axis, respectively.

3. The self-interference continuous elimination method for collaborative exploration of unknown areas by multiple unmanned vessels according to claim 1, characterized in that, In step S2, the range ε ​​is greater than the effective scanning range of the lidar.

4. The method for continuous elimination of self-interference in collaborative exploration of unknown areas by multiple unmanned vessels according to claim 1, characterized in that, Step S5 specifically includes: S51. Use ICP to calculate the matching cluster of dynamic obstacles in two frames of LiDAR data, and calculate the center point coordinates of all elements in the cluster. The calculation formula is as follows: S52. Obtain the coordinates of the center point of the dynamic obstacle in the previous frame. The coordinates of the center point of the dynamic obstacle in this frame are: S53. Analyze the dynamic obstacle position relationship between two frames of LiDAR data, process the LiDAR point cloud of dynamic obstacles, if Dist(p,q)≤λ*V max If the dynamic obstacle matching is successful, then the dynamic obstacle matching is successful; where Dist(p,q) is the Euclidean distance between p and q, and V max This is the maximum speed of the unmanned vessel; λ should be slightly greater than 1 to avoid minor errors.

5. The method for continuous elimination of self-interference in collaborative exploration of unknown areas by multiple unmanned vessels according to claim 1, characterized in that, In step S6: Release the unprocessed lidar data to provide data for the dynamic path planning of unmanned vessels and ensure the safety of unmanned vessel movement; The processed LiDAR data is published to eliminate LiDAR noise caused by self-interference and improve the quality of the constructed map.

Images