An all-round automatic obstacle avoidance unmanned boat on and under water

Abstract

An unmanned vessel capable of all-around automatic obstacle avoidance both above and below the surface belongs to the field of route planning technology and comprises a multimodal sensing array, a multibeam echo sounder, and a fusion engine. The multimodal sensing array comprises multiple horizontal millimeter-level radars, a stereo millimeter-level radar, a SWIR laser imaging device, and a polarization camera installed based on the hull coordinate system. The multimodal sensing array is used to initiate SWIR laser emission in the detection area and receive reflections to generate a target outline image. The polarization camera is used to collect a polarization map based on obstacle points in the target outline image and fuse and filter the polarization map with the millimeter-wave echo to obtain a point set with clutter suppressed. The multibeam echo sounder is activated in the direction of the point set to detect and identify underwater terrain protrusions and output a three-dimensional point cloud of the shallow water area. The fusion engine calculates and plans the route based on the three-dimensional obstacle point cloud and the dynamic window DAW, generating an autonomous route with all-around obstacle avoidance, thereby reducing the probability of the unmanned vessel losing control and running aground.

Description

Technical Field

[0001] The present invention belongs to the technical field of route planning, and more specifically, relates to an unmanned boat that automatically avoids obstacles in all directions above and below the water. Background Art

[0002] Unmanned boats' obstacle avoidance is primarily based on horizontal radar waves. They rely on radar waves to determine where obstacles are located on the surface and then replan their autonomous route to avoid them. This method has a drawback: if the underwater terrain has shallow protrusions, such as rocks, the unmanned boat can easily run aground. Alternatively, if there are obstructions in the air ahead, the unmanned boat can easily lose its positioning signal, potentially causing it to lose control.

[0003] Existing technologies, on the one hand, use radar waves at relatively high frequencies (30GHz to 300GHz) during intelligent navigation, resulting in very short wavelengths (millimeter-level), making them susceptible to atmospheric conditions. In particular, signal strength rapidly decays in rain, snow, and dense fog, significantly reducing detection range and accuracy. For example, heavy fog and rainstorms are common in coastal unmanned vessel scenarios. When using millimeter radar, the radar signal is significantly weakened when penetrating rain and fog. Small obstacles (such as small buoys and floating debris) that could originally be detected hundreds of meters away are reduced to tens of meters or even less in rain and fog. This can easily lead to delayed obstacle detection and potentially delayed or inaccurate obstacle avoidance decisions. On the other hand, millimeter radar waves have short wavelengths and extremely high resolution. While they can effectively detect smaller targets, they are also highly susceptible to interference from surface clutter (such as ripples, water vapor layers, and bubbles), making target recognition difficult. For example, when an unmanned vessel navigates lakes or rivers, especially in summer, the surface waves frequently generate numerous tiny bubbles and ripples. Millimeter-wave radar waves can easily misinterpret these minute surface disturbances as real obstacles. This misjudgment can frequently trigger obstacle avoidance maneuvers, causing the vessel to constantly adjust its path and impacting navigation efficiency. Summary of the Invention

[0004] In order to solve the deficiencies in the prior art, the purpose of the present invention is to solve the above-mentioned defects and further propose an unmanned boat with all-round automatic obstacle avoidance on the water and underwater.

[0005] The present invention adopts the following technical solutions.

[0006] The present invention discloses an unmanned boat capable of all-around automatic obstacle avoidance on and under water, comprising a multi-modal sensing array, a multi-beam echo sounder, and a fusion engine;

[0007] The multimodal sensing array is composed of multiple horizontal millimeter-level radars, three-dimensional millimeter-level radars, SWIR laser imaging equipment and polarization cameras installed based on the hull coordinate system;

[0008] The multimodal sensing array is used to initiate SWIR laser emission in the detection area and receive reflections to generate a target contour image;

[0009] The polarization camera is used to collect a polarization map according to the obstacle points in the target outline image, and fuse and filter the polarization map with the millimeter wave echo to obtain a point set after clutter suppression;

[0010] activating the multi-beam echo sounder in the direction of the point set to detect and identify underwater terrain protrusions to output a three-dimensional point cloud of the shallow water area;

[0011] The fusion engine calculates and plans the route based on the three-dimensional obstacle point cloud and the dynamic window DAW.

[0012] Furthermore, the multimodal sensing array is used to initiate SWIR laser emission in the detection area and receive reflections to generate a target contour image, including the following steps:

[0013] Starting the horizontal millimeter-level radar to perform preliminary detection of the detection area, so as to obtain, from the preliminary detection, first distances and radar cross sections between each horizontal millimeter-level radar and obstacles in the detection area, and calculating a signal-to-noise ratio based on the first distances and radar cross sections;

[0014] A parameter tuple is defined for each SWIR laser imaging device in the multimodal sensing array to construct SWIR emission parameters; the parameter tuple is a subset of the SWIR emission parameters, and is composed of the laser power, beam divergence angle, and beam pointing angle linearly mapped to the signal-to-noise ratio.

[0015] Furthermore, the multimodal sensing array is used to initiate SWIR laser emission in the detection area and receive reflections to generate a target contour image, and further includes the following steps:

[0016] Starting the SWIR laser imaging devices to observe the detection area based on the SWIR emission parameters to obtain original images from each SWIR laser imaging device;

[0017] Performing Gaussian filtering and normalization processing on the original image to obtain a preprocessed SWIR image;

[0018] Binarizing the SWIR image according to a set threshold to obtain a binary image;

[0019] Connected domain analysis is performed on the binary image to retain the largest connected region in the binary image as the target contour, and to generate the target contour image.

[0020] Furthermore, the polarization camera is used to collect a polarization map according to the obstacle points in the target outline image, and fuse and filter the polarization map with the millimeter wave echo to obtain a point set after clutter suppression, including the following steps:

[0021] Scanning all pixel coordinates in the target contour image to extract contour pixel coordinates, and constructing a contour pixel set based on the contour pixel coordinates;

[0022] The coordinates of each contour pixel in the contour pixel set are mapped to a ground plane point in the hull coordinate system to output a preliminary point set.

[0023] Furthermore, the polarization camera is used to collect a polarization map according to the obstacle points in the target outline image, and fuse and filter the polarization map with the millimeter wave echo to obtain a point set after clutter suppression, further comprising the following steps:

[0024] Calculating the polarization degree corresponding to each contour pixel coordinate, and assigning the polarization degree to each corresponding ground plane point in the preliminary point set, so as to output a secondary point set carrying the polarization degree;

[0025] All ground plane points in the secondary point set are traversed, and ground plane points whose polarization degrees do not exceed a first threshold are retained to eliminate clutter, and the clutter-suppressed point set is output.

[0026] Furthermore, the multi-beam echo sounder is activated in the direction of the point set to detect and identify underwater terrain protrusions to output a three-dimensional point cloud of the shallow water area, including the following steps:

[0027] configuring multi-beam sonar parameters within the directional range covered by the point set, and constructing a sonar parameter tuple set based on the multi-beam sonar parameters; the multi-beam sonar parameters include the number of beams, the tilt angle of the center of each beam, the transmission pulse frequency, the sound speed, and the underwater absorption coefficient;

[0028] For each beam, an acoustic pulse is transmitted at the transmitting pulse frequency toward the center of each beam at an inclined angle, and corresponding echoes are received and the round trip time of each beam is recorded, and an echo time set is output through multiple averaging.

[0029] Furthermore, the multi-beam echo sounder is activated in the direction of the point set to detect and identify underwater terrain protrusions to output a three-dimensional point cloud of a shallow water area, further comprising the following steps:

[0030] Converting the round trip time of each beam in the echo time set into water depth, and correcting the ranging error of the acoustic wave attenuation of each beam by a preset correction coefficient, and outputting a sounding point set;

[0031] Determine the hull position, sonar installation height, and beam angle of each beam based on the sounding point set, and convert the beam angle of each beam and the corresponding water depth into a spatial point, so as to retain the spatial point whose water depth does not exceed the preset safe draft height;

[0032] By integrating the spatial points where the water depth does not exceed the preset safe draft height, the sounding point set, the hull position and the sonar installation height, a three-dimensional point cloud of the shallow water area is output.

[0033] Furthermore, the fusion engine calculates and plans a route based on the three-dimensional obstacle point cloud and the dynamic window DAW, including the following steps:

[0034] Merging the clutter-suppressed point set with the shallow water area three-dimensional point cloud to generate the three-dimensional obstacle point cloud;

[0035] Calculate the grid index corresponding to each point in the three-dimensional obstacle point cloud to output an occupied grid set; the occupied grid set includes the grid resolution and grid range boundary of each grid.

[0036] Furthermore, the fusion engine calculates and plans the route based on the three-dimensional obstacle point cloud and the dynamic window DAW, and further includes the following steps:

[0037] Calculating a second distance between each grid in the occupied grid set and the nearest obstacle corresponding to each grid, and setting a risk radius based on the second distance to construct a cost function, and outputting a cost grid and a range;

[0038] Based on the Euclidean distance between adjacent grids in the cost function, a path grid sequence is determined by a heuristic function to output a grid path.

[0039] Furthermore, the fusion engine calculates and plans the route based on the three-dimensional obstacle point cloud and the dynamic window DAW, and further includes the following steps:

[0040] Converting the grid index into spatial coordinates to generate a coordinate sequence;

[0041] A three-dimensional cubic spline interpolation is applied to the coordinate sequence to generate a continuous curve, and based on the continuous curve, the re-planned planned route is output; the planned route is a three-dimensional path for the unmanned ship to travel.

[0042] The beneficial effects of the present invention are that, compared with the prior art, the present invention has the following advantages:

[0043] (1) The present invention installs a multimodal sensing array on the hull coordinate system to provide a hardware foundation for all-round (horizontal, aerial, and underwater clutter) detection during the intelligent driving of the unmanned ship.

[0044] (2) Based on the detection parameters output by the multimodal sensing array, the present invention starts SWIR laser emission and receives reflections to generate a target contour image, thus solving the problem of shortened detection range of millimeter-level radar in heavy fog or rainstorm.

[0045] (3) The present invention uses a polarization camera to collect polarization images and fuses and filters them with millimeter-level echoes to eliminate false alarms from short-range clutter such as ripples on the water surface and multiple bubbles. Ultimately, it generates an autonomous route that takes into account all-round obstacle avoidance in the horizontal, air, and underwater directions, reducing the probability of the unmanned ship losing control and running aground. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] Figure 1 This is a schematic diagram of the overall architecture of an all-round automatic obstacle avoidance unmanned boat provided by the present invention;

[0047] Figure 2 It is a schematic diagram of the detection and obstacle avoidance effect of an all-round automatic obstacle avoidance unmanned boat on and under water provided by the present invention. DETAILED DESCRIPTION

[0048] The present application will be further described below in conjunction with the accompanying drawings. The following embodiments are only used to more clearly illustrate the technical solutions of the present invention and are not intended to limit the scope of protection of the present application.

[0049] like Figure 1 As shown, in one embodiment, an all-round automatic obstacle avoidance unmanned boat on the water and underwater includes a multi-modal sensing array, a multi-beam echo sounder, and a fusion engine. The multi-modal sensing array is a plurality of horizontal millimeter-level radars, a three-dimensional millimeter-level radar, a SWIR laser imaging device, and a polarization camera installed based on the hull coordinate system. The multi-modal sensing array is used to start SWIR laser emission in the detection area and receive reflections to generate a target contour image. The polarization camera is used to collect a polarization map based on the obstacle points in the target contour image, and fuse and filter the polarization map with the millimeter-wave echo to obtain a point set after clutter suppression. The multi-beam echo sounder is started in the direction of the point set to detect and identify underwater terrain protrusions to output a three-dimensional point cloud of the shallow water area. The fusion engine calculates and plans the route based on the three-dimensional obstacle point cloud and the dynamic window DAW.

[0050] It should be noted that the fusion engine is a sensor fusion engine, that is, a fusion engine of a multimodal sensing array and a multibeam echo sounder. It is used to integrate the detection data of the multimodal sensing array and the multibeam echo sounder to achieve simultaneous localization and mapping (SLAM), improve navigation capabilities in complex environments, reduce noise through heterogeneous sensor fusion, estimate the status of occlusion or dynamic obstacles, and improve the accuracy and redundancy of environmental perception.

[0051] Combine Figure 2As shown, in a specific embodiment, the present invention provides an all-round automatic obstacle avoidance unmanned boat on and under water, comprising steps 1 to 5:

[0052] Step 1: Sensor array installation.

[0053] Specifically, N horizontal millimeter wave radars are installed on the ship coordinate system O. , M three-dimensional millimeter-wave radars , P SWIR laser imaging devices and Q polarization cameras , forming a multi-modal sensing array, providing the hardware foundation for all-round (horizontal, aerial, and underwater clutter) detection.

[0054] In some embodiments, the present invention provides an omnidirectional automatic obstacle avoidance unmanned vessel for use above and below the surface, wherein a multimodal sensing array is used to initiate SWIR laser emission in a detection area and receive reflections to generate a target outline image, including the following steps:

[0055] Step S110: Start the horizontal millimeter-level radar to perform preliminary detection of the detection area, so as to obtain the first distance and radar cross section between each horizontal millimeter-level radar and the obstacles in the detection area from the preliminary detection, and calculate the signal-to-noise ratio based on the first distance and radar cross section.

[0056] Step S120: Define a parameter tuple for each SWIR laser imaging device in the multimodal sensing array to construct SWIR emission parameters. The parameter tuple is a subset of the SWIR emission parameters, consisting of laser power, beam divergence angle, and beam pointing angle linearly mapped to the signal-to-noise ratio.

[0057] In some embodiments, the present invention provides an omnidirectional automatic obstacle avoidance unmanned vessel for use above and below the surface, wherein the multimodal sensing array is used to initiate SWIR laser emission in a detection area and receive reflections to generate a target contour image, further comprising the following steps:

[0058] Step S130 : starting the SWIR laser imaging devices to observe the detection area based on the SWIR emission parameters to obtain original images from each SWIR laser imaging device.

[0059] Step S140 , performing Gaussian filtering and normalization processing on the original image to obtain a pre-processed SWIR image.

[0060] Step S150 , binarizing the SWIR image according to a set threshold to obtain a binary image.

[0061] Step S160 , performing connected domain analysis on the binary image to retain the largest connected region in the binary image as the target contour, and generating a target contour image.

[0062] Combine Figure 2 As shown, in a specific embodiment, the present invention provides an all-round automatic obstacle avoidance unmanned boat on the water and underwater, step 2, SWIR active imaging compensation.

[0063] Step 2: The detection area where the radar is severely attenuated in bad weather in step 1 , start SWIR laser emission (λ=1000~1700nm) and receive reflection , generating target outline images, solving the problem of shortened detection range of millimeter-wave radar in heavy fog / heavy rain.

[0064] The following steps are involved:

[0065] Step 2.1: Determine the direction to be compensated .

[0066] Specifically, for each horizontal millimeter wave radar unit i , Obstacle distance obtained from preliminary radar detection (m) and radar cross section (square meters).

[0067] Calculating the signal-to-noise ratio :

[0068]

[0069] in, (10-100W) is the transmission power, 、 are the transmitting and receiving antenna gains, (meter) is the radar wavelength, , , is the system equivalent temperature, B (Hz) is the receiving bandwidth, F (≥1) is the noise factor, is the rain attenuation coefficient, in m -1 , e is a constant, t represents the time point, and the threshold is set (such as 10dB), take .

[0070] Step 2.2, construct SWIR emission parameters .

[0071] Specifically, for each millimeter wave radar , define the parameter tuple :

[0072]

[0073] in, (0.1-5W) is the laser power of millimeter wave radar j, given by and The linear mapping is: , (0.5°~5°) is the beam divergence angle, , , is the beam pointing angle.

[0074] Step 2.3, Acquire and preprocess SWIR images .

[0075] Specifically, emit laser and receive original image :

[0076]

[0077] in, is the pixel coordinate, For system response, (range 0-1) is the target reflectivity, (0.6-0.95) is the atmospheric transmittance, is Gaussian noise. After that, the original image is Gaussian filtered and normalized to obtain a standardized image .

[0078] Step 2.4, extract target contour .

[0079] Specifically, according to the threshold (0.4-0.6) Binarization:

[0080] ,but ,otherwise, .

[0081] Afterwards, the binary image is subjected to connected domain analysis, and only the largest connected region is retained as the target contour, and a binary contour image is output.

[0082] In some embodiments, the present invention provides an omnidirectional automatic obstacle avoidance unmanned vessel on and under water, wherein a polarization camera is used to collect a polarization map based on obstacle points in a target outline image, and fuse and filter the polarization map with millimeter wave echoes to obtain a point set after clutter suppression, including the following steps:

[0083] Step S210 , scanning all pixel coordinates in the target contour image to extract contour pixel coordinates, and constructing a contour pixel set based on the contour pixel coordinates.

[0084] Step S220 , mapping the coordinates of each contour pixel in the contour pixel set to a ground plane point in the hull coordinate system to output a preliminary point set.

[0085] In some embodiments, the present invention provides an omnidirectional automatic obstacle avoidance unmanned vessel on and under water, wherein a polarization camera is used to collect a polarization map based on obstacle points in a target outline image, and fuse and filter the polarization map with millimeter wave echoes to obtain a point set after clutter suppression, further comprising the following steps:

[0086] Step S230 , calculating the polarization degree corresponding to each contour pixel coordinate, and assigning the polarization degree to each corresponding ground plane point in the preliminary point set, so as to output a secondary point set carrying the polarization degree.

[0087] Step S240 traverses all ground plane points in the secondary point set, and retains ground plane points whose polarization degree does not exceed the first threshold to eliminate clutter, and outputs a point set after clutter suppression.

[0088] Combine Figure 2 As shown, in a specific embodiment, the present invention provides an all-round automatic obstacle avoidance unmanned boat on the water and underwater, step 3, surface clutter suppression.

[0089] Step 3: The suspected obstacle point output from step 2 or step 1 , using a polarization camera Collect polarization images and with millimeter wave echo Fusion filtering to obtain the point set after clutter suppression , in order to eliminate false alarms of short-range clutter such as ripples and foam on the water surface.

[0090] The following steps are involved:

[0091] Step 3.1, extract the contour pixel set U.

[0092] Specifically, scan all pixel coordinates, when When the corresponding coordinate pair Add to the set U, where , K is the total number of contour pixels.

[0093] Step 3.2, image pixels are mapped to the ground plane .

[0094] Specifically, for each Map to the ground plane point (X, Y, Z axis coordinates) of the hull coordinate system according to the following formula ( , , ):

[0095]

[0096]

[0097]

[0098] in, is the camera installation height, 、 is the camera's intrinsic focal length, principal point coordinates 、 .

[0099] Step 3.3, calculate the degree of polarization .

[0100] Specifically, for each pixel Calculate the corresponding polarization degree to output the polarization degree Point set .

[0101] Step 3.4, clutter identification and purification generation .

[0102] Specifically, traverse the point set All records in the table retain polarization degrees that do not exceed the set threshold The points are removed, the water surface mirror reflection clutter with high polarization degree is eliminated, and the point set after clutter suppression is output .

[0103] In some embodiments, the present invention provides an omnidirectional automatic obstacle avoidance unmanned vessel for use above and below the surface, which activates a multi-beam echo sounder in the direction of a point set to detect and identify underwater terrain protrusions and output a three-dimensional point cloud of a shallow water area, including the following steps:

[0104] Step S310: configuring multi-beam sonar parameters within the directional range covered by the point set, and constructing a sonar parameter tuple set based on the multi-beam sonar parameters; the multi-beam sonar parameters include the number of beams, the tilt angle of the center of each beam, the sending pulse frequency, the speed of sound, and the underwater absorption coefficient.

[0105] Step S320 , transmitting an acoustic pulse at a transmission pulse frequency to the center of each beam at an inclination angle, receiving corresponding echoes and recording the round trip time of each beam, and outputting an echo time set by averaging multiple times.

[0106] In some embodiments, the present invention provides an omnidirectional automatic obstacle avoidance unmanned vessel for use above and below the surface, which activates a multi-beam echo sounder in the direction of a point set to detect and identify underwater terrain protrusions to output a three-dimensional point cloud of a shallow water area, and further includes the following steps:

[0107] Step S330: convert the round trip time of each beam in the echo time set into water depth, and correct the ranging error of the acoustic wave attenuation of each beam by a preset correction coefficient, and output a sounding point set.

[0108] Step S340, based on the sounding point set, the hull position, the sonar installation height and the beam angle of each beam are determined, and the beam angle of each beam and the corresponding water depth are converted into spatial points to retain the spatial points whose water depth does not exceed the preset safe draft height.

[0109] Step S350: Output a three-dimensional point cloud of the shallow water area by integrating the spatial points whose water depth does not exceed the preset safe draft height, the sounding point set, the hull position, and the sonar installation height.

[0110] Combine Figure 2 As shown, in a specific embodiment, the present invention provides an all-round automatic obstacle avoidance unmanned boat on the water and underwater, step 4, multi-beam underwater depth and ranging.

[0111] Step 4: After clutter suppression, point set The multi-beam echo sounder is activated in the direction to measure the depth and identify underwater terrain protrusions (such as rocks) to prevent the unmanned ship from running aground due to shallow protrusions.

[0112] The following steps are involved:

[0113] Step 4.1, configure the multibeam sonar parameters.

[0114] Specifically, after clutter suppression, the point set Configure multi-beam sonar parameters, including the number of beams, within the covered direction range (Usually 64 to 256 beams), the tilt angle of each beam center (evenly distributed within ±(30°~60°)), sending pulse frequency (20~200kHz), sound speed c (1400~1550 m / s, obtained by on-site temperature, salinity and pressure measurements), underwater absorption coefficient α (0.1~1.0 dB / km, preset according to the environment), and output a sonar parameter tuple set consisting of the configured multi-beam sonar parameters :

[0115]

[0116] Step 4.2, sonar emission and echo time acquisition.

[0117] Specifically, for each beam, an acoustic pulse is transmitted at a pulse frequency and tilted toward the center of the beam, and the echo is received and the round-trip time is recorded. :

[0118]

[0119] Where M is the number of repeated pulses (10~20 times), Indicates the mth measurement time.

[0120] At the same time, multiple averages are used to reduce multipath and noise in repeated pulses, and the echo time set T is output:

[0121]

[0122] Step 4.3, calculate the depth measurement results for each beam.

[0123] Specifically, convert the round trip time into water depth , while correcting the ranging error caused by sound wave attenuation :

[0124]

[0125] Where β is the correction coefficient, which takes the value [0.1, 0.5], reflecting the influence of absorption on the time delay measurement. The output sounding point set D = { (k, ) | k=1… }.

[0126] Step 4.4, generate 3D underwater terrain map .

[0127] Specifically, based on the sounding point set D and the hull position coordinates , sonar installation height Range [0.5m, 1.5m] and beam center tilt angle , convert the depth of each beam into a spatial point:

[0128] ;

[0129] (along course, no side deviation);

[0130] .

[0131] Only points shallower than the safe draft are retained. = ship draft + safety margin, usually 0.5-1.0 m, and the final output is the 3D point cloud of the shallow water area:

[0132]

[0133] in, They respectively represent the X, Y, and Z axis coordinates of the corresponding point m in the three-dimensional point cloud of the shallow water area.

[0134] In some embodiments, the present invention provides an omnidirectional automatic obstacle avoidance unmanned vessel on and under water, wherein a fusion engine calculates and plans a route based on a three-dimensional obstacle point cloud and a dynamic window DAW, including the following steps:

[0135] Step S410 : merging the clutter-suppressed point set with the shallow water area three-dimensional point cloud to generate a three-dimensional obstacle point cloud.

[0136] Step S420 , calculating the grid index corresponding to each point in the three-dimensional obstacle point cloud to output an occupied grid set; the occupied grid set includes the grid resolution and grid range boundary of each grid.

[0137] In some embodiments, the present invention provides an omnidirectional automatic obstacle avoidance unmanned vessel on and under water, wherein a fusion engine calculates and plans a route based on a three-dimensional obstacle point cloud and a dynamic window DAW, further comprising the following steps:

[0138] Step S430 , calculating a second distance between each grid in the occupied grid set and the nearest obstacle corresponding to each grid, and setting a risk radius based on the second distance to construct a cost function, and outputting a cost grid and a range.

[0139] Step S440 : Based on the Euclidean distance between adjacent grids in the cost function, a path grid sequence is determined by a heuristic function to output a grid path.

[0140] In some embodiments, the present invention provides an omnidirectional automatic obstacle avoidance unmanned vessel on and under water, wherein a fusion engine calculates and plans a route based on a three-dimensional obstacle point cloud and a dynamic window DAW, further comprising the following steps:

[0141] Step S450: Convert the grid index into space coordinates to generate a coordinate sequence.

[0142] In step S460, three-dimensional cubic spline interpolation is applied to the coordinate sequence to generate a continuous curve, and based on the continuous curve, a re-planned route is output; the planned route is a three-dimensional path for the unmanned ship to travel.

[0143] Combine Figure 2 As shown, in a specific embodiment, the present invention provides an all-round automatic obstacle avoidance unmanned boat on the water and underwater, step 5, multimodal data fusion and three-dimensional path replanning.

[0144] Step 5: 、 、 、 、 The fusion engine is input to realize the three-dimensional obstacle point cloud, and a new route is obtained based on the three-dimensional obstacle point cloud and the dynamic window method (DWA). Ultimately, an autonomous route is generated that takes into account all-round obstacle avoidance horizontally, in the air, and underwater, solving the problem of ship loss of control and grounding.

[0145] The following steps are involved:

[0146] Step 5.1: Construct a 3D obstacle point cloud.

[0147] Specifically, the water surface obstacle point set Set of raised dots with underwater shallow Merge to get the omnidirectional 3D obstacle point cloud (the union of the two):

[0148]

[0149] Where n=1,...,N, N is and The sum of K in the middle.

[0150] Step 5.2: Build a 3D grid index.

[0151] Specifically, calculate the grid index corresponding to each point in the omnidirectional 3D obstacle point cloud:

[0152]

[0153]

[0154]

[0155] in, is the grid resolution, 、 、 These are the minimum values ​​of the grid's range boundaries on the X, Y, and Z axes. 、 、 They are the indexes of the X, Y, and Z axes respectively. Represents the index calculation function. The final output is given by 、 、 Together they form a collection of occupancy grids.

[0156] Step 5.3, calculate the grid cost C.

[0157] Specifically, for each grid, the distance to the nearest obstacle grid is calculated, and the risk radius is set to construct the cost function:

[0158]

[0159] in, (0.1m) represents the geometric occupied area, and the final output cost grid .

[0160] Step 5.4, three-dimensional path search.

[0161] Specifically, define the cost function :

[0162]

[0163] in, represents the Euclidean distance between adjacent grids, is the heuristic function used to record the path grid sequence from the starting point to the end point, is the core cost function, represents the cumulative true cost from the starting point s to the current node n, represents the heuristic estimated cost from the current node n to the end point g, is the grid cost of node n obtained in step 5.3.

[0164] Step 5.5: Path smoothing and three-dimensional trajectory generation to plan the route.

[0165] Specifically, convert the grid index to spatial coordinates:

[0166]

[0167]

[0168]

[0169] For the X, Y, and Z axis coordinate sequence ( , , ) Apply three-dimensional cubic spline interpolation to generate a continuous curve , t represents the time point, represents the planned route at time t, are the coordinate sequences ( , , ) The planned route coordinates, 、 、 are the grid indexes corresponding to each point in the omnidirectional three-dimensional obstacle point cloud of the planned route.

[0170] The present disclosure may be a system, method and / or computer program product. The computer program product may include a computer-readable storage medium carrying computer-readable program instructions for causing a processor to implement various aspects of the present disclosure.

[0171] A computer-readable storage medium can be a tangible device that can hold and store instructions for use by an instruction execution device. A computer-readable storage medium can be, for example, but not limited to, an electrical storage device, a magnetic storage device, an optical storage device, an electromagnetic storage device, a semiconductor storage device, or any suitable combination thereof. More specific examples (a non-exhaustive list) of computer-readable storage media include: a portable computer disk, a hard disk, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), static random access memory (SRAM), a portable compact disc read-only memory (CD-ROM), a digital versatile disk (DVD), a memory stick, a floppy disk, a mechanical encoding device, such as a punched card or raised structure in a groove on which instructions are stored, and any suitable combination thereof. As used herein, a computer-readable storage medium is not to be construed as a transient signal per se, such as a radio wave or other freely propagating electromagnetic wave, an electromagnetic wave propagating through a waveguide or other transmission medium (e.g., a light pulse passing through a fiber optic cable), or an electrical signal transmitted through an electrical wire.

[0172] The computer-readable program instructions described herein can be downloaded from a computer-readable storage medium to each computing / processing device, or downloaded to an external computer or external storage device via a network, such as the Internet, a local area network, a wide area network, and / or a wireless network. The network can include copper transmission cables, fiber optic transmission, wireless transmission, routers, firewalls, switches, gateway computers, and / or edge servers. The network adapter card or network interface in each computing / processing device receives the computer-readable program instructions from the network and forwards the computer-readable program instructions to be stored in the computer-readable storage medium in each computing / processing device.

[0173] The computer program instructions for performing the operations of the present disclosure may be assembly instructions, instruction set architecture (ISA) instructions, machine instructions, machine-dependent instructions, microcode, firmware instructions, state setting data, or source code or object code written in any combination of one or more programming languages, including object-oriented programming languages ​​such as Smalltalk, C++, and conventional procedural programming languages ​​such as "C" or similar programming languages. The computer-readable program instructions may be executed entirely on the user's computer, partially on the user's computer, as a stand-alone software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In the case of a remote computer, the remote computer may be connected to the user's computer via any type of network, including a local area network (LAN) or a wide area network (WAN), or may be connected to an external computer (e.g., via the Internet using an Internet service provider). In some embodiments, the state information of the computer-readable program instructions is used to personalize an electronic circuit, such as a programmable logic circuit, a field programmable gate array (FPGA), or a programmable logic array (PLA), so that the electronic circuit can execute the computer-readable program instructions, thereby implementing various aspects of the present disclosure.

[0174] Various aspects of the present disclosure are described herein with reference to flowcharts and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the present disclosure. It should be understood that each block of the flowcharts and / or block diagrams, and combinations of blocks in the flowcharts and / or block diagrams, can be implemented by computer-readable program instructions.

[0175] These computer-readable program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, or other programmable data processing device, thereby producing a machine, so that when these instructions are executed by the processor of the computer or other programmable data processing device, a device is generated that implements the functions / actions specified in one or more blocks in the flowchart and / or block diagram. These computer-readable program instructions can also be stored in a computer-readable storage medium, where these instructions cause the computer, programmable data processing device, and / or other device to operate in a specific manner. Thus, the computer-readable medium storing the instructions comprises an article of manufacture that includes instructions for implementing various aspects of the functions / actions specified in one or more blocks in the flowchart and / or block diagram.

[0176] Computer-readable program instructions may also be loaded onto a computer, other programmable data processing apparatus, or other device so that a series of operational steps are performed on the computer, other programmable data processing apparatus, or other device to produce a computer-implemented process, thereby causing the instructions executed on the computer, other programmable data processing apparatus, or other device to implement the functions / actions specified in one or more blocks in the flowchart and / or block diagram.

[0177] The flow charts and block diagrams in the accompanying drawings show the possible architecture, functions and operations of the systems, methods and computer program products according to multiple embodiments of the present disclosure. In this regard, each box in the flow chart or block diagram can represent a part of a module, program segment or instruction, and the part of the module, program segment or instruction contains one or more executable instructions for realizing the prescribed logical function. In some alternative implementations, the functions marked in the box can also occur in a sequence different from that marked in the accompanying drawings. For example, two consecutive boxes can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram and / or flow chart, and the combination of the boxes in the block diagram and / or flow chart can be implemented by a dedicated hardware-based system that performs the prescribed function or action, or can be implemented by a combination of dedicated hardware and computer instructions.

[0178] 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 above embodiments, ordinary technicians in the field should understand that the specific implementation methods of the present invention can still be modified or replaced by equivalents. Any modification or equivalent replacement that does not depart from the spirit and scope of the present invention should be covered by the scope of protection of the claims of the present invention.

Claims

1. An all-round automatic obstacle avoidance unmanned boat on and under water, characterized by: Includes multimodal sensing arrays, multibeam echo sounders, and fusion engines; The multimodal sensing array is composed of multiple horizontal millimeter-level radars, three-dimensional millimeter-level radars, SWIR laser imaging equipment and polarization cameras installed based on the hull coordinate system; The multimodal sensing array is used to initiate SWIR laser emission in the detection area and receive reflections to generate a target contour image; The polarization camera is used to collect a polarization map according to the obstacle points in the target outline image, and fuse and filter the polarization map with the millimeter wave echo to obtain a point set after clutter suppression; activating the multi-beam echo sounder in the direction of the point set to detect and identify underwater terrain protrusions to output a three-dimensional point cloud of the shallow water area; The fusion engine calculates and plans the route based on the three-dimensional obstacle point cloud and the dynamic window DAW; The multimodal sensing array is used to initiate SWIR laser emission in a detection area and receive reflections to generate a target contour image, including the following steps: Starting the horizontal millimeter-level radar to perform preliminary detection of the detection area, so as to obtain, from the preliminary detection, first distances and radar cross sections between each horizontal millimeter-level radar and obstacles in the detection area, and calculating a signal-to-noise ratio based on the first distances and radar cross sections; A parameter tuple is defined for each SWIR laser imaging device in the multimodal sensing array to construct SWIR emission parameters; the parameter tuple is a subset of the SWIR emission parameters, and is composed of the laser power, beam divergence angle, and beam pointing angle linearly mapped to the signal-to-noise ratio.

2. The omnidirectional automatic obstacle avoidance unmanned boat according to claim 1 is characterized in that: The multimodal sensing array is used to initiate SWIR laser emission in a detection area and receive reflections to generate a target contour image, and further includes the following steps: Starting the SWIR laser imaging devices to observe the detection area based on the SWIR emission parameters to obtain original images from each SWIR laser imaging device; Performing Gaussian filtering and normalization processing on the original image to obtain a preprocessed SWIR image; Binarizing the SWIR image according to a set threshold to obtain a binary image; Connected domain analysis is performed on the binary image to retain the largest connected region in the binary image as the target contour, and to generate the target contour image.

3. The omnidirectional automatic obstacle avoidance unmanned boat according to claim 2 is characterized in that: The polarization camera is used to collect a polarization map according to the obstacle points in the target outline image, and fuse and filter the polarization map with the millimeter wave echo to obtain a point set after clutter suppression, including the following steps: Scanning all pixel coordinates in the target contour image to extract contour pixel coordinates, and constructing a contour pixel set based on the contour pixel coordinates; The coordinates of each contour pixel in the contour pixel set are mapped to a ground plane point in the hull coordinate system to output a preliminary point set.

4. The omnidirectional automatic obstacle avoidance unmanned boat according to claim 3 is characterized in that: The polarization camera is used to collect a polarization map according to the obstacle points in the target outline image, and fuse and filter the polarization map with the millimeter wave echo to obtain a point set after clutter suppression, and further includes the following steps: Calculating the polarization degree corresponding to each contour pixel coordinate, and assigning the polarization degree to each corresponding ground plane point in the preliminary point set, so as to output a secondary point set carrying the polarization degree; All ground plane points in the secondary point set are traversed, and ground plane points whose polarization degrees do not exceed a first threshold are retained to eliminate clutter, and the clutter-suppressed point set is output.

5. The unmanned boat capable of all-around automatic obstacle avoidance on and under water according to claim 4 is characterized in that: Starting the multi-beam echo sounder in the direction of the point set to detect and identify underwater terrain protrusions to output a three-dimensional point cloud of a shallow water area, comprising the following steps: configuring multi-beam sonar parameters within the directional range covered by the point set, and constructing a sonar parameter tuple set based on the multi-beam sonar parameters; the multi-beam sonar parameters include the number of beams, the tilt angle of the center of each beam, the transmission pulse frequency, the sound speed, and the underwater absorption coefficient; For each beam, an acoustic pulse is transmitted at the transmitting pulse frequency toward the center of each beam at an inclined angle, and corresponding echoes are received and the round trip time of each beam is recorded, and an echo time set is output through multiple averaging.

6. The omnidirectional automatic obstacle avoidance unmanned boat according to claim 5 is characterized in that: The multi-beam echo sounder is activated in the direction of the point set to detect and identify underwater terrain protrusions to output a three-dimensional point cloud of the shallow water area, further comprising the following steps: Converting the round trip time of each beam in the echo time set into water depth, and correcting the ranging error of the acoustic wave attenuation of each beam by a preset correction coefficient, and outputting a sounding point set; Determine the hull position, sonar installation height, and beam angle of each beam based on the sounding point set, and convert the beam angle of each beam and the corresponding water depth into a spatial point, so as to retain the spatial point whose water depth does not exceed the preset safe draft height; By integrating the spatial points where the water depth does not exceed the preset safe draft height, the sounding point set, the hull position and the sonar installation height, a three-dimensional point cloud of the shallow water area is output.

7. The unmanned boat capable of all-around automatic obstacle avoidance on and under water according to claim 6, characterized in that: The fusion engine calculates and plans a route based on the three-dimensional obstacle point cloud and the dynamic window DAW, including the following steps: Merging the clutter-suppressed point set with the shallow water area three-dimensional point cloud to generate the three-dimensional obstacle point cloud; Calculate the grid index corresponding to each point in the three-dimensional obstacle point cloud to output an occupied grid set; the occupied grid set includes the grid resolution and grid range boundary of each grid.

8. The unmanned boat capable of all-around automatic obstacle avoidance on and under water according to claim 7, characterized in that: The fusion engine calculates and plans the route based on the three-dimensional obstacle point cloud and the dynamic window DAW, and further includes the following steps: Calculating a second distance between each grid in the occupied grid set and the nearest obstacle corresponding to each grid, and setting a risk radius based on the second distance to construct a cost function, and outputting a cost grid and a range; Based on the Euclidean distance between adjacent grids in the cost function, a path grid sequence is determined by a heuristic function to output a grid path.

9. The omnidirectional automatic obstacle avoidance unmanned boat according to claim 7 is characterized in that: The fusion engine calculates and plans the route based on the three-dimensional obstacle point cloud and the dynamic window DAW, and further includes the following steps: Converting the grid index into spatial coordinates to generate a coordinate sequence; A three-dimensional cubic spline interpolation is applied to the coordinate sequence to generate a continuous curve, and based on the continuous curve, the re-planned planned route is output; the planned route is a three-dimensional path for the unmanned ship to travel.

Images