Real-time obstacle detection and avoidance method and device for underwater vehicle based on foresight sonar

Abstract

The embodiment of the invention provides an underwater vehicle real-time obstacle detection and avoidance method and device based on a foresight sonar, and the method comprises the steps: obtaining a foresight sonar image in real time, and extracting key obstacle information from the foresight sonar image; establishing an obstacle situation map based on the extracted key obstacle information, and establishing a global situation map; and integrating the obstacle situation map and the global situation map to perform real-time obstacle avoidance and route dynamic planning of the underwater vehicle. The embodiment of the invention can be used for the underwater vehicle to realize autonomous obstacle recognition and avoidance, so that the underwater vehicle can autonomously execute tasks in a complex and unknown water environment.

Description

Technical Field

[0001] This document relates to the field of computer technology, and more particularly to a method and device for real-time obstacle detection and avoidance of a submersible based on forward-looking sonar. Background Art

[0002] Autonomous obstacle avoidance is one of the key capabilities of underwater unmanned vehicles to carry out complex tasks. The current research on autonomous obstacle avoidance faces problems such as low robustness of obstacle recognition and positioning, poor real-time performance of complex planning algorithms, inability to effectively cope with complex ocean currents and weak maneuverability of vehicles to achieve high-quality obstacle avoidance. Summary of the invention

[0003] The purpose of the present invention is to provide a real-time obstacle detection and avoidance method and device for a submersible based on forward-looking sonar, aiming to solve the above-mentioned problems in the prior art.

[0004] The present invention provides a real-time obstacle detection and avoidance method for a submersible based on forward-looking sonar, comprising:

[0005] Acquire forward-looking sonar images in real time, and extract key obstacle information from the forward-looking sonar images;

[0006] Establishing an obstacle situation map based on the extracted key obstacle information and establishing a global situation map;

[0007] The obstacle situation map and the global situation map are combined to perform real-time obstacle avoidance and dynamic route planning for the submersible.

[0008] The present invention provides a real-time obstacle detection and avoidance device for a submersible based on forward-looking sonar, comprising:

[0009] An extraction module, used to acquire a forward-looking sonar image in real time and extract key obstacle information from the forward-looking sonar image;

[0010] An establishment module is used to establish an obstacle situation map based on the extracted key obstacle information and establish a global situation map;

[0011] The planning module is used to integrate the obstacle situation map and the global situation map to perform real-time obstacle avoidance and dynamic route planning for the submersible.

[0012] An embodiment of the present invention also provides an electronic device, comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the computer program, when executed by the processor, implements the steps of the above-mentioned method for real-time obstacle detection and avoidance of a submersible based on forward-looking sonar.

[0013] An embodiment of the present invention also provides a computer-readable storage medium, on which a program for implementing information transmission is stored. When the program is executed by a processor, the steps of the above-mentioned real-time obstacle detection and avoidance method for a submersible based on forward-looking sonar are implemented.

[0014] The embodiment of the present invention provides a real-time obstacle detection and avoidance method for a submersible based on forward-looking sonar, including real-time acquisition, extraction, positioning, tracking of obstacle images, establishment of situation maps, global planning and local re-planning, and integration of statistical learning and target tracking obstacle detection methods and local online planning based on dynamic situation maps, forming a complete set of autonomous obstacle avoidance systems. Obstacles are detected and located by forward-looking sonar, environmental situation models are established, and routes are planned online in real time. While achieving the purpose of collision avoidance, the route is as close to the global mission route as possible. The above technical solution can be used for submersibles to realize autonomous obstacle recognition and avoidance, thereby enabling submersibles to autonomously perform tasks in complex and unknown water environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] In order to more clearly illustrate one or more embodiments of this specification or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments recorded in this specification. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0016] Figure 1 is a flow chart of a method for real-time obstacle detection and avoidance of a submersible based on forward-looking sonar according to an embodiment of the present invention;

[0017] Figure 2 is a detailed flow chart of a real-time obstacle detection and avoidance method for a submersible based on forward-looking sonar according to an embodiment of the present invention;

[0018] Figure 3 is an operation flow chart of the method of an embodiment of the present invention when it is executed on a submersible;

[0019] Figure 4 is a schematic diagram of a real-time obstacle detection and avoidance device for a submersible based on forward-looking sonar according to an embodiment of the present invention;

[0020] Figure 5 is a schematic diagram of an electronic device according to an embodiment of the present invention. DETAILED DESCRIPTION

[0021] In order to enable those skilled in the art to better understand the technical solutions in one or more embodiments of this specification, the following will be combined with the drawings in one or more embodiments of this specification to clearly and completely describe the technical solutions in one or more embodiments of this specification. Obviously, the described embodiments are only part of the embodiments of this specification, not all of the embodiments. Based on one or more embodiments of this specification, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of this document.

[0022] Method Embodiment

[0023] According to an embodiment of the present invention, a real-time obstacle detection and avoidance method for a submersible based on forward-looking sonar is provided. Figure 1 is a flow chart of a method for real-time obstacle detection and avoidance of a submersible based on forward-looking sonar according to an embodiment of the present invention, such as Figure 1 As shown, the real-time obstacle detection and avoidance method for a submersible based on forward-looking sonar according to an embodiment of the present invention specifically includes:

[0024] Step S101, acquiring a forward-looking sonar image in real time, and extracting key obstacle information from the forward-looking sonar image; specifically comprising:

[0025] Performing statistics on the grayscale information of the original image of the sonar image to obtain a grayscale histogram;

[0026] The grayscale histogram is segmented by obstacles, the arc noise of strong obstacles is eliminated, and an image that retains the obstacle characteristics to the greatest extent and eliminates noise is selected. Finally, the visual attention mechanism is used to extract the final key obstacle information.

[0027] Step S102, establishing an obstacle situation map based on the extracted key obstacle information, and establishing a global situation map; specifically includes:

[0028] Based on the extracted key obstacle information, the obstacle situation map is established in the submersible body coordinate system, wherein the submersible body coordinate system takes the center of the submersible as the origin, the positive direction of the x-axis points to the bow of the submersible, the positive direction of the y-axis points to the right side of the submersible, and the positive direction of the z-axis points downward and perpendicular to the xy plane, which is a right-handed coordinate system. The global situation map is established in the navigation coordinate system, wherein the navigation coordinate system adopts the global geographic coordinate system, and the positions of all obstacles, mission targets, and submersibles are converted into values ​​of the global geographic coordinate system. The global situation map is a scatter plot, and the position of each scatter point represents the coordinate of the obstacle, mission target, or the center point of the submersible in the global geographic coordinate system.

[0029] Step S103, comprehensively analyzing the obstacle situation map and the global situation map, and performing real-time obstacle avoidance and route dynamic planning for the submersible. Specifically, it includes:

[0030] According to the global situation map, the optimal estimate is made when the autonomous planning of the submersible falls into a local unsolvable state, and a current optimal path is autonomously planned when the overall mission goal changes. According to the position information, movement speed and direction information of all obstacles in the threat circle of the submersible relative to the submersible in the obstacle situation map, the submersible performs fast local path planning with minimized overhead to achieve fast obstacle avoidance, and plans the subsequent movement trajectory of the obstacle according to the current position and speed of the moving obstacle, and updates the movement trajectory in real time, so that the submersible can avoid long-distance dynamic obstacles in advance. According to the subsequent movement trajectory of the dynamic obstacle, it is judged whether it is a dynamic strong threat target, and the judgment result is sent to the top-level mission planning to guide the defense of the submersible.

[0031] The above technical solution of the embodiment of the present invention is described in detail below in conjunction with the accompanying drawings.

[0032] like Figure 2 As shown, the steps of the present invention include (1) acquiring sonar images in real time; (2) extracting key obstacles; (3) establishing an obstacle situation map; (4) establishing a global situation map; and (5) re-planning.

[0033] Forward-looking multi-beam imaging sonar is one of the most important payloads for underwater target identification and obstacle detection. Affected by the water environment, the characteristics of sound wave transmission and the noise of the equipment itself, the images of targets and obstacles in the forward-looking multi-beam sonar have low resolution, irregular shapes, incomplete boundaries, blurred boundaries, and high noise. This poses a great challenge to correctly separating obstacles, accurately estimating the location of obstacles, and obtaining an accurate environmental situation map. In addition, since the obstacle point is displayed based on the sonar reflection intensity, the absorption or penetration of sound by certain parts of the obstacle causes the obstacle to be discrete, which in turn leads to discontinuous display of the same obstacle.

[0034] In this regard, the extraction of key obstacles (2) consists of three parts: segmenting obstacles, segmenting noise, and enhancing obstacles. The specific method is to consider that the image obtained from the forward-looking sonar is a grayscale image, the background is a dark spot, and the noise and obstacles are bright spots. The appearance of noise is often irregular. At the same time, due to the interference between the beams of the sonar equipment, arc noise will be formed at the distance where the strong obstacle is located. The obstacle is relatively stable, but it often has "tailing" and "shadow" areas and unstable shapes or discontinuous imaging characteristics. Assuming that the image Pic0 is the original image, the grayscale information of Pic0 is statistically analyzed to obtain the grayscale histogram Pic1. According to the characteristics of Pic1, Pic0 is segmented for the first time. Specifically, when Pic1 has obvious bright and dark peaks, troughs, and peaks, the grayscale value of the trough is used as the threshold G. When there are multiple peaks, troughs, and peaks, only the rightmost trough is considered. When Pic1 is relatively flat as a whole and there are no obvious peaks, troughs, and peaks, the threshold G=200 is selected. Points greater than G in Pic0 are reserved as obstacle points, and the rest are assigned 0 to obtain the obstacle distribution range map Pic2. Points less than G in Pic0 are reserved as background points, and the rest are assigned 0 to obtain the background distribution range map Pic3. Scan Pic2 radially in the arc direction, remove all arc noise to obtain Pic4, and perform median filtering on Pic3 to obtain Pic5. Add Pic4 and Pic5 to obtain Pic6. At this time, Pic6 is an image that retains obstacle characteristics to the greatest extent and removes noise.

[0035] Furthermore, Pic6 is adaptively connected to its nearest neighbor to obtain Pic7, which realizes the integrity of obstacle target segmentation and solves the problem of obstacle discontinuity.

[0036] Furthermore, in order to correctly segment the real position of the obstacle, for Pic7, an obstacle segmentation algorithm that integrates the visual attention mechanism and the frame difference method is used to perceive the obstacle as a whole. For special obstacle targets, a machine learning method is used to track and locate the obstacle targets, and finally the obstacle type, location, dynamic and static parameters and other information are obtained for the establishment of a situation map.

[0037] The obstacle situation diagram (3) is established in the submersible body coordinate system.

[0038] The coordinate system of the submersible body takes the center of the submersible as the origin, the positive direction of the x-axis points to the bow of the submersible, the positive direction of the y-axis points to the right side of the submersible, and the positive direction of the z-axis points downward and perpendicular to the xy plane. It is a right-handed coordinate system. First, according to the sonar map, obtain the obstacle relative to the position of the sonar (d, θ), d is the distance, and θ is the angle. Assuming that the installation position of the sonar is (xs, ys, zs), the representation position of the obstacle in the situation map is:

[0039] xobs=xs+d*cosθ, yobs=ys+d*sinθ, zobs=zs. A scatter plot is made based on (xobs, yobs, zobs), and the center point of the scatter plot is the submersible.

[0040] It should be noted that the scattered point here is not a data point, but a circle. The center point of the circle is the obstacle center value, and the circle radius represents the threat level and obstacle size. Each circle is divided into two areas, and the dark area refers to the direction of movement.

[0041] The global situation map (4) is established in the navigation coordinate system. The navigation coordinate system adopts the global geographic coordinate system. All obstacles, mission objectives, and the positions of the submersible are converted into the values ​​of the global geographic coordinate system. The global situation map is a scatter plot.

[0042] It should be noted that the scatter plot here is a real scatter plot, and the position of each scatter point represents the coordinates of the obstacle, mission target or the center point of the submersible in the global geographic coordinate system.

[0043] Replanning (5) includes local replanning and global replanning. In fact, planning is reflected in the entire task. Figure 3 This is a simplified flow chart of the operation of this method when it is executed on a submersible. For a submersible, there are multiple subsystems such as mission planning, navigation, and control, corresponding to multiple algorithms. This method is implemented on a submersible and needs to be integrated among the many subsystems of the submersible. Here, it is mainly integrated with the path planning in mission planning.

[0044] like Figure 3 As shown in the figure, after the submersible enters the water and is powered on, the command and control station will bind tasks for the submersible. The tasks may be water body detection, target scanning in a certain area, or reconnaissance of a certain destination. Different tasks require different bound information. Facing an unknown environment, the initial global situation map only has the starting point and target point or target area, and the obstacle situation map is a blank map. With the detection of the forward-looking sonar, obstacle information is continuously added to the obstacle situation map and the global situation map, collectively referred to as the environmental situation map.

[0045] In summary, with the help of the technical solution of the embodiment of the present invention, a dynamic low-complexity global situation and local situation map is established, which can cope with multiple underwater obstacles and dynamic obstacles, and combined with motion estimation, achieve rapid obstacle avoidance while achieving global optimization. The technical solution of the embodiment of the present invention involves an algorithm with low time overhead, low hardware requirements, low support for software libraries, and can be quickly integrated on unmanned platforms.

[0046] Device Example 1

[0047] According to an embodiment of the present invention, a real-time obstacle detection and avoidance device for a submersible based on forward-looking sonar is provided. Figure 4is a schematic diagram of a real-time obstacle detection and avoidance device for a submersible based on forward-looking sonar according to an embodiment of the present invention, Figure 4 As shown, the real-time obstacle detection and avoidance device for a submersible based on forward-looking sonar according to an embodiment of the present invention specifically includes:

[0048] The extraction module 40 is used to obtain the forward-looking sonar image in real time and extract key obstacle information from the forward-looking sonar image; specifically, it is used to:

[0049] Performing statistics on the grayscale information of the original image of the sonar image to obtain a grayscale histogram;

[0050] By adopting the histogram segmentation method, the grayscale histogram is segmented to remove the arc noise of strong obstacles, and an image that retains the obstacle characteristics to the greatest extent and removes the noise is selected. Finally, the visual attention mechanism method is used to extract the final key obstacle information;

[0051] The establishment module 42 is used to establish an obstacle situation map based on the extracted key obstacle information and establish a global situation map; specifically, it is used to:

[0052] Based on the extracted key obstacle information, the obstacle situation map is established in the submersible body coordinate system, wherein the submersible body coordinate system takes the center of the submersible as the origin, the positive direction of the x-axis points to the bow of the submersible, the positive direction of the y-axis points to the right side of the submersible, and the positive direction of the z-axis points downward and is perpendicular to the xy plane, which is a right-handed coordinate system;

[0053] The global situation map is established in a navigation coordinate system, wherein the navigation coordinate system adopts the global geographic coordinate system, and the positions of all obstacles, mission objectives, and submersibles are converted into values ​​of the global geographic coordinate system. The global situation map is a scatter plot, and the position of each scatter point represents the coordinates of the center point of the obstacle, mission objective, or submersible in the global geographic coordinate system.

[0054] The planning module 44 is used to perform real-time obstacle avoidance and route dynamic planning for the submersible by integrating the obstacle situation map and the global situation map. Specifically used for:

[0055] According to the global situation map, the optimal estimate is made when the autonomous planning of the submersible falls into a local unsolvable state, and a current optimal path is autonomously planned when the overall mission goal changes. According to the position information, movement speed and direction information of all obstacles in the threat circle of the submersible relative to the submersible in the obstacle situation map, the submersible performs fast local path planning with minimized overhead to achieve fast obstacle avoidance, and plans the subsequent movement trajectory of the obstacle according to the current position and speed of the moving obstacle, and updates the movement trajectory in real time, so that the submersible can avoid long-distance dynamic obstacles in advance. According to the subsequent movement trajectory of the dynamic obstacle, it is judged whether it is a dynamic strong threat target, and the judgment result is sent to the top-level mission planning to guide the defense of the submersible.

[0056] The embodiment of the present invention is a device embodiment corresponding to the above-mentioned method embodiment. The specific operations of each module can be understood by referring to the description of the method embodiment, which will not be repeated here.

[0057] Device Example 2

[0058] An embodiment of the present invention provides an electronic device, such as Figure 5 As shown, it includes: a memory 50, a processor 52, and a computer program stored in the memory 50 and executable on the processor 52. When the computer program is executed by the processor 52, the steps described in the method embodiment are implemented.

[0059] Device Example 3

[0060] An embodiment of the present invention provides a computer-readable storage medium, on which a program for implementing information transmission is stored. When the program is executed by the processor 52, the steps described in the method embodiment are implemented.

[0061] The computer-readable storage medium in this embodiment includes, but is not limited to, ROM, RAM, magnetic disk or optical disk, etc.

[0062] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements 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 real-time obstacle detection and avoidance method for a submersible based on forward-looking sonar, characterized in that: include: Acquire forward-looking sonar images in real time, and extract key obstacle information from the forward-looking sonar images; Establishing an obstacle situation map based on the extracted key obstacle information and establishing a global situation map; The obstacle situation map and the global situation map are combined to perform real-time obstacle avoidance and dynamic route planning for the submersible.

2. The method according to claim 1, characterized in that Extracting key obstacle information from the sonar image specifically includes: Performing statistics on the grayscale information of the original image of the sonar image to obtain a grayscale histogram; The grayscale histogram is segmented by obstacles, and the arc noise of strong obstacles is eliminated to obtain an image that retains the obstacle characteristics to the greatest extent and eliminates noise. Finally, the visual attention mechanism is used to extract the final key obstacle information.

3. The method according to claim 1, characterized in that Establishing an obstacle situation map based on the extracted key obstacle information specifically includes: Based on the extracted key obstacle information, the obstacle situation map is established in the submersible body coordinate system, wherein the submersible body coordinate system takes the center of the submersible as the origin, the positive direction of the x-axis points to the bow of the submersible, the positive direction of the y-axis points to the right side of the submersible, and the positive direction of the z-axis points downward and perpendicular to the xy plane, which is a right-handed coordinate system.

4. The method according to claim 1, characterized in that Establishing a global situation map specifically includes: The global situation map is established in a navigation coordinate system, wherein the navigation coordinate system adopts the global geographic coordinate system, and the positions of all obstacles, mission objectives, and submersibles are converted into values ​​of the global geographic coordinate system. The global situation map is a scatter plot, and the position of each scatter point represents the coordinates of the center point of the obstacle, mission objective, or submersible in the global geographic coordinate system.

5. The method according to claim 1, characterized in that Comprehensively performing real-time obstacle avoidance and route dynamic planning of the submersible based on the obstacle situation map and the global situation map specifically includes: According to the global situation map, the optimal estimate is made when the autonomous planning of the submersible falls into a local unsolvable state, and a current optimal path is autonomously planned when the overall mission goal changes. According to the position information, movement speed and direction information of all obstacles in the threat circle of the submersible relative to the submersible in the obstacle situation map, the submersible performs fast local path planning with minimized overhead to achieve fast obstacle avoidance, and plans the subsequent movement trajectory of the obstacle according to the current position and speed of the moving obstacle, and updates the movement trajectory in real time, so that the submersible can avoid long-distance dynamic obstacles in advance. According to the subsequent movement trajectory of the dynamic obstacle, it is judged whether it is a dynamic strong threat target, and the judgment result is sent to the top-level mission planning to guide the defense of the submersible.

6. A real-time obstacle detection and avoidance device for a submersible based on forward-looking sonar, characterized in that: include: An extraction module, used to acquire a forward-looking sonar image in real time and extract key obstacle information from the forward-looking sonar image; An establishment module is used to establish an obstacle situation map based on the extracted key obstacle information and establish a global situation map; The planning module is used to integrate the obstacle situation map and the global situation map to perform real-time obstacle avoidance and dynamic route planning for the submersible.

7. The device according to claim 6, characterized in that The extraction module is specifically used for: Performing statistics on the grayscale information of the original image of the sonar image to obtain a grayscale histogram; The grayscale histogram is segmented by obstacles, and the arc noise of strong obstacles is eliminated to obtain an image that retains the obstacle characteristics to the greatest extent and eliminates the noise. Finally, the visual attention mechanism is used to extract the final key obstacle information. The establishment module is specifically used for: Based on the extracted key obstacle information, the obstacle situation map is established in the submersible body coordinate system, wherein the submersible body coordinate system takes the center of the submersible as the origin, the positive direction of the x-axis points to the bow of the submersible, the positive direction of the y-axis points to the right side of the submersible, and the positive direction of the z-axis points downward and is perpendicular to the xy plane, which is a right-handed coordinate system; The global situation map is established in a navigation coordinate system, wherein the navigation coordinate system adopts the global geographic coordinate system, and the positions of all obstacles, mission objectives, and submersibles are converted into values ​​of the global geographic coordinate system. The global situation map is a scatter plot, and the position of each scatter point represents the coordinates of the center point of the obstacle, mission objective, or submersible in the global geographic coordinate system.

8. The device according to claim 6, characterized in that The planning module is specifically used for: According to the global situation map, the optimal estimate is made when the autonomous planning of the submersible falls into a local unsolvable state, and a current optimal path is autonomously planned when the overall mission goal changes. According to the position information, movement speed and direction information of all obstacles in the threat circle of the submersible relative to the submersible in the obstacle situation map, the submersible performs fast local path planning with minimized overhead to achieve fast obstacle avoidance, and plans the subsequent movement trajectory of the obstacle according to the current position and speed of the moving obstacle, and updates the movement trajectory in real time, so that the submersible can avoid long-distance dynamic obstacles in advance. According to the subsequent movement trajectory of the dynamic obstacle, it is judged whether it is a dynamic strong threat target, and the judgment result is sent to the top-level mission planning to guide the defense of the submersible.

9. An electronic device, characterized in that: include: A memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the computer program, when executed by the processor, implements the steps of the real-time obstacle detection and avoidance method for a submersible based on forward-looking sonar as described in any one of claims 1 to 5.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores an implementation program for information transmission, and when the program is executed by the processor, the steps of the real-time obstacle detection and avoidance method for a submersible based on forward-looking sonar as described in any one of claims 1 to 5 are implemented.

Images