Underwater vehicle obstacle avoidance method and system, medium and product

Abstract

The invention relates to the technical field of ship navigation operation, and particularly discloses an obstacle avoidance method and system for an underwater vehicle, a medium and a product, and the method comprises the steps: obtaining navigation positioning information, static obstacle information and dynamic obstacle information of the underwater vehicle in real time; calculating a three-dimensional positive transverse distance between each static obstacle and the underwater vehicle; judging whether potential safety hazards exist or not; calculating evasion parameters; calculating a horizontal DCPA and a vertical DCPA between the dynamic obstacle and the underwater vehicle; judging whether potential safety hazards exist or not; calculating evasion parameters; and calculating a reference avoidance parameter according to the avoidance parameter of the static obstacle and the avoidance parameter of the dynamic obstacle. According to the method, danger prompts of static obstacles and dynamic obstacles can be given through comprehensive and real-time calculation, motion scheme selection for eliminating dangers is provided, the operation difficulty of navigation personnel can be effectively reduced, the error rate is reduced, and therefore the safety of underwater navigation is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of ship navigation operations, and in particular to a method, system, medium and product for avoiding obstacles for underwater vehicles. Background Art

[0002] When navigating underwater, underwater vehicles (UAVs) face limited vision and observation range, significantly shortening the range of their detection equipment compared to surface vessels, posing significant safety risks. During navigation, UAVs may encounter static obstacles such as islands, buoys, and sunken ships, as well as dynamic obstacles such as underwater moving objects. Predicting and assessing navigational situations in a timely manner is crucial to avoiding collision risks.

[0003] During traditional surface navigation, static obstructions are detected by determining the abeam distance and abeam time. Abeam distance refers to the distance between an object and the ship when it is abeamed. In nautical charting, it refers to the distance from the intersection of the object's bearing line and the track line when the object is abeamed. When a ship is sailing, if there are dangerous objects, reefs, shoals, sunken ships, etc., it is required to maintain sufficient abeam distance to safely pass through, and maintain sufficient abeam time for navigation personnel to respond to maneuvers. For underwater navigation, it is not enough to only consider the horizontal abeam distance; the shortest three-dimensional straight-line distance from the dangerous location point to the three-dimensional trajectory of the vehicle must also be considered.

[0004] During traditional surface navigation, the two most important parameters for assessing collision risk with dynamic obstructions are the distance to close point of approach (DCPA) and the time to close point of approach (TCPA). DCPA refers to the minimum distance between two ships on the horizontal plane while maintaining a constant course and speed. The safe distance approaching (SDA) is the range of distances within which collision can be avoided, i.e., the distance within which either the owning ship or the dynamic obstruction can safely pass. If a dynamic obstruction passes beyond the safe distance approaching, no collision avoidance action is required, and there is no collision risk for both parties; the original speed and course can be maintained. However, if a dynamic obstruction passes within the safe distance approaching, there is a collision risk, and collision avoidance action, such as adjusting course or turning, is required.

[0005] For underwater navigation, not only horizontal DCPA but also vertical DCPA must be considered, i.e., the closest approach distance in the vertical direction, and a vertical safe approach distance must be set. When determining whether there is a risk of collision, both horizontal and vertical DCPA must be considered comprehensively. If the horizontal DCPA exceeds the safe approach distance but the vertical DCPA does not, there is no danger. Only when both the horizontal and vertical DCPA exceed the safe approach distance does a danger arise and measures must be taken.

[0006] Existing methods are mostly applicable to surface navigation, and it is difficult to timely and accurately judge the collision risk and provide avoidance plans based on the navigation information of underwater vehicles, static obstacles and dynamic obstacles. Summary of the Invention

[0007] The present invention aims to address the aforementioned issues. To this end, the present invention provides an obstacle avoidance method, system, medium, and product for underwater vehicles. Based on the underwater vehicle's navigation and positioning information, as well as static and dynamic obstacle information, the present invention performs static obstacle detection (i.e., calculation of three-dimensional orthogonal and transverse parameters, static obstacle alarm determination, and reference information calculation), as well as dynamic obstacle detection (i.e., horizontal DCPA, vertical DCPA, and TCPA, dynamic obstacle alarm determination, and reference information calculation). Through comprehensive, real-time calculations, the present invention provides warnings of static and dynamic obstacles and offers the option of maneuvering to eliminate them. This method effectively reduces the operational difficulty and error rate for the operator, thereby improving underwater navigation safety.

[0008] The present invention provides a method for avoiding obstacles for underwater vehicles, and the technical solution adopted is as follows: comprising the following steps: S1: Real-time acquisition of underwater vehicle navigation and positioning information, static obstacle information, and dynamic obstacle information; S2: Calculate the three-dimensional vertical and horizontal distances between each static obstacle and the underwater vehicle based on the underwater vehicle navigation positioning information and the static obstacle information; S3: Determine whether there is a safety hazard based on the three-dimensional vertical and horizontal distances and the static obstruction safety distance; calculate the avoidance parameters for static obstructions that pose a safety hazard; S4: Calculate the horizontal DCPA and vertical DCPA between the dynamic obstacle and the underwater vehicle based on the underwater vehicle navigation positioning information and the dynamic obstacle information; S5: Determine whether there are safety hazards based on the horizontal DCPA, vertical DCPA, and the safe distance of dynamic obstructions; calculate the avoidance parameters for dynamic obstructions that pose safety hazards; S6: Calculate reference avoidance parameters based on the avoidance parameters of the static obstacle and the avoidance parameters of the dynamic obstacle; If the number of circumvention parameters is 0, refer to the default circumvention parameters. When the number of circumvention parameters is 1, the circumvention parameter is directly used as the reference circumvention parameter; When there are more than one avoidance parameter, if the avoidance parameter meets the heading consistency judgment standard, the heading avoidance parameter is calculated based on the optimal heading; if the avoidance parameter meets the heading consistency judgment standard, the heading avoidance parameter is calculated based on the optimal heading; if the avoidance parameter does not meet both the heading consistency judgment standard and the vertical speed consistency judgment standard, the hazard weight is calculated based on the avoidance parameter, and the avoidance parameter with the largest hazard weight is selected as the reference avoidance parameter; The reference avoidance parameters include heading avoidance parameters and vertical speed avoidance parameters.

[0009] Furthermore, step S2 includes: S21: Calculating the horizontal azimuth angle between the underwater vehicle and the static obstruction based on the underwater vehicle navigation positioning information and the static obstruction information; S22: Calculate the track azimuth angle based on the horizontal azimuth angle between the underwater vehicle and the static obstacle, and then calculate the horizontal azimuth time and horizontal azimuth distance; S23: Calculate the longitude, latitude, and depth of the three-dimensional vertical point based on the vertical time and horizontal vertical distance. The specific process is as follows: Calculate the longitude and latitude of the two-dimensional vertical point based on the horizontal vertical distance; The longitude and latitude of the three-dimensional abscissa point are the same as those of the two-dimensional abscissa point; The calculation formula for the depth of the three-dimensional positive horizontal point is: in, is the depth of the three-dimensional positive horizontal point, is the vertical velocity of the underwater vehicle, is the positive transverse time, is the depth of the underwater vehicle; S24: Calculate the three-dimensional transverse distance between the underwater vehicle and the three-dimensional transverse point.

[0010] Furthermore, in step S3, the static obstacle avoidance parameters include the optimal heading and its corresponding three-dimensional transverse distance, the optimal vertical speed and its corresponding three-dimensional transverse distance; The calculation process of the avoidance parameters of static obstacles is: S311: Select multiple headings according to the first interval value; S312: Calculate the three-dimensional vertical and horizontal distances corresponding to the static obstruction in each heading direction, and then determine whether there is a safety hazard. S313: From the headings without potential safety hazards, select the heading with the smallest difference from the track direction of the underwater vehicle as the optimal heading; S321: Selecting a plurality of vertical velocities according to the vertical velocity of the underwater vehicle and the second interval value; S322: Calculate the three-dimensional transverse distance and three-dimensional transverse time corresponding to the static obstruction based on each vertical velocity, and then determine whether there is a safety hazard. S323: From the vertical speeds that do not pose a safety hazard, select a vertical speed that has the smallest difference from the vertical speed of the underwater vehicle as the optimal vertical speed.

[0011] Furthermore, step S4 includes: S41: Calculating the horizontal azimuth and distance between the underwater vehicle and the dynamic obstacle based on the underwater vehicle navigation positioning information and the dynamic obstacle information; S42: Calculate horizontal DCPA and TCPA based on the horizontal azimuth and distance between the underwater vehicle and the dynamic obstacle; S43: Calculate vertical DCPA based on TCPA. The calculation formula is: in, For vertical DCPA, To find the absolute value, is the vertical speed of the dynamic obstacle, is the vertical velocity of the underwater vehicle, For TCPA, is the depth of the dynamic obstruction, is the depth of the underwater vehicle.

[0012] Furthermore, in step S5, the dynamic obstacle avoidance parameters include the optimal heading and its corresponding horizontal DCPA and vertical DCPA, the optimal vertical speed and its corresponding horizontal DCPA and vertical DCPA; The calculation process of the dynamic obstacle avoidance parameters is as follows: S511: Select multiple headings according to the first interval value; S512: Calculate the horizontal DCPA and vertical DCPA between the dynamic obstacle and the underwater vehicle in each heading direction, and then determine whether there is a safety hazard; S513: From the headings without potential safety hazards, select the heading with the smallest difference from the track direction of the underwater vehicle as the optimal heading; S521: Selecting a plurality of vertical velocities according to the vertical velocity of the underwater vehicle and the second interval value; S522: Calculate the horizontal DCPA and vertical DCPA between the dynamic obstruction and the underwater vehicle based on each vertical velocity, and then determine whether there is a safety hazard. S523: From the vertical speeds that do not pose a safety hazard, select a vertical speed that has the smallest difference from the vertical speed of the underwater vehicle as the optimal vertical speed.

[0013] Furthermore, the calculation formula for the danger weight of static obstructions is: in, is the hazard weight of the static obstruction, is the static obstacle safety distance, is the three-dimensional normal and transverse distance; The calculation formula of the hazard weight of dynamic obstructions is: in, is the horizontal hazard weight of the dynamic obstruction, is the vertical hazard weight of the dynamic obstruction, For horizontal DCPA, For vertical DCPA, is the horizontal safety distance of dynamic obstacles, It is the vertical safety distance of dynamic obstacles.

[0014] Furthermore, the calculation process of the heading avoidance parameter is: Obtaining an optimal heading value range according to the optimal heading, and selecting a plurality of headings from the optimal heading value range at intervals of a third interval value; Selecting a final optimal heading from the above multiple headings that eliminates safety hazards for both static and dynamic obstacles; if there are multiple final optimal headings, taking the average as the heading avoidance parameter; if there is only one final optimal heading, taking it directly as the heading avoidance parameter; if there is no final optimal heading, taking the midpoint of the optimal heading value range as the heading avoidance parameter; The calculation process of the vertical speed avoidance parameter is: Obtaining an optimal vertical speed value range according to the optimal vertical speed, and selecting a plurality of vertical speeds from the optimal vertical speed value range at intervals of a fourth interval value; The final optimal vertical speed that eliminates safety hazards for both static and dynamic obstacles is selected from the above multiple vertical speeds. If there are multiple final optimal vertical speeds, the average is taken as the vertical speed avoidance parameter. If there is only one final optimal vertical speed, it is directly used as the vertical speed avoidance parameter. If there is no final optimal vertical speed, the midpoint of the optimal vertical speed value range is taken as the heading avoidance parameter.

[0015] The present invention also provides an obstacle avoidance system for underwater vehicles, which adopts the following technical solution: comprising: a data acquisition module, a static obstacle parameter calculation module, a static obstacle avoidance parameter calculation module, a dynamic obstacle parameter calculation module, a dynamic obstacle avoidance parameter calculation module and a reference avoidance parameter calculation module, Data acquisition module, used to obtain underwater vehicle navigation and positioning information, static obstruction information and dynamic obstruction information in real time; A static obstacle parameter calculation module is used to calculate the three-dimensional vertical and horizontal distances between each static obstacle and the underwater vehicle based on the underwater vehicle navigation positioning information and the static obstacle information; The static obstruction avoidance parameter calculation module is used to determine whether there is a safety hazard based on the three-dimensional vertical and horizontal distances and the static obstruction safety distance; for static obstructions that pose a safety hazard, it calculates their avoidance parameters; A dynamic obstacle parameter calculation module is used to calculate the horizontal DCPA and vertical DCPA between the dynamic obstacle and the underwater vehicle based on the underwater vehicle navigation positioning information and the dynamic obstacle information; The dynamic obstacle avoidance parameter calculation module is used to determine whether there is a safety hazard based on the horizontal DCPA, vertical DCPA and the safe distance of the dynamic obstacle; for dynamic obstacles with safety hazards, it calculates their avoidance parameters; The reference avoidance parameter calculation module is used to calculate the reference avoidance parameter according to the avoidance parameter of the static obstacle and the avoidance parameter of the dynamic obstacle.

[0016] The present invention also provides a computer-readable storage medium having a computer program stored thereon, wherein the computer program implements the above method when executed by a processor.

[0017] The present invention also provides a computer program product, comprising a computer program, which implements the above method when executed by a processor.

[0018] The above one or more technical solutions in the embodiments of the present invention have at least one of the following technical effects: 1. This invention supports safety hazard assessment for one or more static and dynamic underwater obstructions. It calculates the three-dimensional orthographic parameters between the underwater vehicle and the static obstruction, as well as the horizontal and vertical DCPA between the underwater vehicle and the dynamic obstruction in real time, accurately assessing safety hazards and providing heading and vertical speed avoidance parameters for mariners to select and mitigate navigation risks.

[0019] 2. The present invention has the advantages of fast calculation speed, high accuracy, strong real-time performance, and the ability to provide warnings and reference prompts, thereby improving the automation level of underwater three-dimensional navigation safety detection, enabling navigation personnel to respond more quickly and accurately, and playing a role in improving navigation safety.

[0020] Additional aspects and advantages of the present invention will be set forth in part in the description which follows and, in part, will be obvious from the description which follows, or may be learned by practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0022] Figure 1 It is a flow chart of the method provided by the present invention.

[0023] Figure 2 It is a three-dimensional orthogonal and transverse parameter definition diagram provided by the present invention.

[0024] Figure 3 It is a structural block diagram of the system provided by the present invention.

[0025] Reference numerals: 1. Data acquisition module; 2. Static obstacle parameter calculation module; 3. Static obstacle avoidance parameter calculation module; 4. Dynamic obstacle parameter calculation module; 5. Dynamic obstacle avoidance parameter calculation module; 6. Reference avoidance parameter calculation module. DETAILED DESCRIPTION

[0026] To make the purpose, technical solutions and advantages of the present invention clearer, the technical solutions of the present invention will be clearly and completely described below in conjunction with the accompanying drawings of the present invention. Obviously, the embodiments described are part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention. The following embodiments are used to illustrate the present invention, but are not used to limit the scope of the present invention.

[0027] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" means that the specific features, structures or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the embodiment of the present invention. In this specification, the schematic representation of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures or characteristics described can be combined in any one or more embodiments or examples in a suitable manner. In addition, those skilled in the art can combine and combine different embodiments or examples described in this specification and features of different embodiments or examples without contradiction.

[0028] The following combination Figures 1 to 3 The present invention is further described in detail, and a method, system, medium and product for avoiding obstacles for underwater vehicles are described. In this embodiment, Figure 1 As shown, a method for avoiding obstacles for an underwater vehicle is provided, comprising the following steps: S1: Real-time acquisition of underwater vehicle navigation and positioning information, static obstacle information, and dynamic obstacle information.

[0029] The navigation and positioning information of the underwater vehicle is obtained through the navigation and positioning equipment on the underwater vehicle, including: longitude, latitude, depth, track direction, east speed, north speed and vertical speed.

[0030] Based on the underwater vehicle's location information (longitude, latitude, and depth), static obstructions within a threshold range are selected by browsing and picking from a nautical chart, or by setting a query range in the obstruction database. Static obstruction information is then obtained. This static obstruction information includes longitude, latitude, and depth. In this embodiment, the threshold is a horizontal distance of 5 nautical miles.

[0031] The detection equipment of the underwater vehicle can obtain dynamic obstacle information in real time, including longitude, latitude, depth, heading, east speed, north speed and vertical speed.

[0032] S2: Calculate the three-dimensional vertical and horizontal distances between each static obstacle and the underwater vehicle based on the underwater vehicle navigation positioning information and the static obstacle information.

[0033] For underwater navigation, we cannot only consider the horizontal transverse distance, but also the shortest three-dimensional straight-line distance from the dangerous location to the three-dimensional trajectory of the vehicle. The definition of the three-dimensional transverse parameter is as follows: Figure 2 As shown, Figure 2 In the figure, the red line d31 is the three-dimensional transverse distance, the red point is the three-dimensional transverse point, and the green point is the two-dimensional transverse point.

[0034] The specific steps include: S21: Calculate the horizontal azimuth angle between the underwater vehicle and the static obstruction based on the underwater vehicle navigation positioning information and the static obstruction information. The calculation process is as follows: in, is the longitude of the underwater vehicle, is the arc value corresponding to the longitude of the underwater vehicle, is pi, which is approximately 3.1415926535. is the latitude of the underwater vehicle, is the arc value corresponding to the latitude of the underwater vehicle, is the longitude of the jth static obstacle, is the arc value corresponding to the longitude of the jth static obstacle, is the latitude of the jth static obstacle, is the radian value corresponding to the latitude of the jth static obstacle, is the horizontal azimuth angle between the underwater vehicle and the static obstacle, is the two-dimensional inverse tangent function.

[0035] S22: Calculate the track azimuth angle based on the horizontal azimuth angle between the underwater vehicle and the static obstacle, and then calculate the horizontal transverse parameters, which include the transverse time and the horizontal transverse distance. The calculation formula is as follows: if , track azimuth for , if , track azimuth for , in, is the track direction of the underwater vehicle; Axis distance for ; in, is the straight-line distance between the underwater vehicle and the static obstacle on the horizontal plane; Positive horizontal time for ; in, is the eastward speed of the underwater vehicle, is the northward speed of the underwater vehicle; Horizontal transverse distance for .

[0036] S23: Calculate the longitude, latitude and depth of the three-dimensional vertical point based on the vertical time and horizontal vertical distance. The specific process is as follows: First, calculate the longitude and latitude of the two-dimensional vertical point based on the horizontal vertical distance. The calculation formula is as follows: in, is the equatorial radius, , is the arc value corresponding to the longitude of the two-dimensional positive horizontal point, is the arc value corresponding to the latitude of the two-dimensional positive horizontal point, is the longitude of the two-dimensional positive horizontal point, is the latitude of the two-dimensional positive horizontal point.

[0037] The longitude and latitude of the three-dimensional abscissa are the same as those of the two-dimensional abscissa, that is, , , is the longitude of the three-dimensional abscissa, is the latitude of the three-dimensional apogee.

[0038] The 3D positive transverse time is the same as the 2D positive transverse time.

[0039] Then, calculate the depth of the three-dimensional positive and horizontal points using the following formula: in, is the depth of the three-dimensional positive horizontal point, is the vertical velocity of the underwater vehicle, is the depth of the underwater vehicle.

[0040] S24: Calculate the three-dimensional transverse distance between the underwater vehicle and the three-dimensional transverse point.

[0041] The position information of the underwater vehicle and the static obstruction is converted from the longitude and latitude geographic coordinates to the xyz Cartesian coordinates, wherein the longitude and latitude geographic coordinates of the position information of the underwater vehicle ( , , ) converted to xyz Cartesian coordinates , the longitude and latitude geographic coordinates of the three-dimensional asymptotic point ( , , ) into xyz Cartesian coordinates ( , , ).

[0042] Calculate the 3D distance between the underwater vehicle and the 3D vertical point in the xyz Cartesian coordinate system , the calculation formula is: , in, is the x-axis coordinate of the underwater vehicle in the xyz Cartesian coordinate system, is the y-axis coordinate of the underwater vehicle in the xyz Cartesian coordinate system, is the z-axis coordinate of the underwater vehicle in the xyz Cartesian coordinate system, is the x-axis coordinate of the three-dimensional positive horizontal point in the xyz Cartesian coordinate system, is the y-axis coordinate of the three-dimensional positive horizontal point in the xyz Cartesian coordinate system, It is the z-axis coordinate of the three-dimensional positive horizontal point in the xyz Cartesian coordinate system.

[0043] For each static obstruction, calculate its corresponding 3D transverse distance and 3D transverse time through the above steps. For n static obstructions, calculate the corresponding n 3D transverse distances and n 3D transverse times.

[0044] S3: Based on the three-dimensional vertical and horizontal distances and the static obstruction safety distance, perform static obstruction alarm detection to determine whether there is a safety hazard; for static obstructions that pose a safety hazard, calculate their avoidance parameters.

[0045] The method for judging the safety hazards of static obstructions is as follows: Set the static obstacle safety distance , considering maneuvers such as turning and U-turn, the default setting size is 3 times the long axis size of the underwater vehicle.

[0046] Determine whether there is a safety hazard: If , there is a safety hazard and the heading or vertical speed needs to be adjusted. Otherwise, there is no safety hazard.

[0047] For static obstructions that pose a safety hazard, avoidance parameters must be calculated. Subsequently, combined with the results of dynamic obstruction detection, the aircraft will determine whether to adjust heading, vertical speed, or both, providing reference avoidance parameters. If a static obstruction does not pose a safety hazard, avoidance parameters do not need to be calculated. Each static obstruction that poses a safety hazard corresponds to a set of avoidance parameters.

[0048] The calculation process of the avoidance parameters of static obstacles is: S311: Select multiple headings based on a first interval value, that is, select headings from the entire rotatable range (360°). The first interval value is a value that is divisible by 360°, such as 0.5° or 1°. In this embodiment, the first interval value is 1°, resulting in 360 headings selected, namely 0°, 1°, 2°, ..., 359°.

[0049] S312: Traverse all headings, calculate the 3D transverse distance and 3D transverse time corresponding to the static obstruction for each heading, and then determine whether there is a safety hazard. This step utilizes the calculation method of step S2, using the heading as the horizontal azimuth between the underwater vehicle and the static obstruction. Based on 360 headings, 360 sets of 3D transverse parameters (3D transverse distance and 3D transverse time) are calculated. The presence of safety hazards is then determined for each heading using the aforementioned static obstruction safety hazard determination method, selecting headings that do not present safety hazards.

[0050] S313: From the headings that present no safety hazards, select the one with the smallest deviation from the underwater vehicle's track direction as the optimal heading. The optimal heading is closest to the track direction, exhibits the smallest rotation, and minimizes deviation from the route. This is the appropriate heading, and steering is performed to avoid the navigation hazard. The optimal heading and its corresponding three-dimensional forward and horizontal distance serve as part of the static obstacle avoidance parameters.

[0051] S321: Multiple vertical velocities are selected based on the vertical velocity of the underwater vehicle and a second interval value. Specifically, multiple vertical velocities are selected from a range around the vertical velocity of the underwater vehicle based on the second interval value. The range around the vertical velocity of the underwater vehicle is defined as a certain range centered around the vertical velocity of the underwater vehicle. The second interval value is set based on experience; in this embodiment, it is ±1 m / s. Five times are selected, resulting in the following 10 vertical velocities: , , , , , , , , , .

[0052] S322: Based on each vertical velocity, the corresponding 3D transverse distance and 3D transverse time of the static obstruction are calculated, and then a determination is made as to whether a safety hazard exists. This step utilizes the calculation method from step S2. Based on the 10 vertical velocities, 10 sets of 3D transverse parameters (3D transverse distance and 3D transverse time) are calculated. The presence of a safety hazard is then determined using the aforementioned safety hazard determination method, and vertical velocities that do not pose a safety hazard are selected.

[0053] S323: From the vertical speeds that present no safety hazards, select the one with the smallest difference from the underwater vehicle's vertical speed as the optimal vertical speed. When turning is inconvenient, such as when the formation mission requires a predetermined course that presents a danger, or when the horizontal space is narrow and difficult to turn in, such as a narrow waterway, the vertical speed can be adjusted to avoid the obstacle. The optimal vertical speed and its corresponding three-dimensional lateral distance serve as another component of the static obstacle avoidance parameters.

[0054] In this embodiment, both the optimal heading and the optimal vertical speed are calculated for use in the subsequent calculation of reference avoidance parameters.

[0055] S4: Based on the underwater vehicle navigation positioning information and the dynamic obstacle information, the horizontal DCPA, vertical DCPA and TCPA between the dynamic obstacle and the underwater vehicle are calculated. The specific process is as follows: S40: To conserve computing resources and speed up calculations, a dynamic obstruction screening range is set to filter dynamic obstructions. The dynamic obstruction screening thresholds include a horizontal radius and vertical distance, with a default horizontal radius of 50 nautical miles and a vertical distance of 200 meters. Dynamic obstructions within this range are subject to the following parameter calculations; those outside this range are considered to present no safety hazards. The latitude and longitude geographic coordinates of the underwater vehicle and the dynamic obstruction are converted to xyz Cartesian coordinates. The current horizontal and vertical distances between the underwater vehicle and the dynamic obstruction are then calculated. If the current horizontal distance is less than or equal to the horizontal radius, and the current vertical distance is less than or equal to the vertical distance, the dynamic obstruction is considered a potential safety hazard and the following parameter calculations are performed.

[0056] S41: Calculating the horizontal azimuth and distance between the underwater vehicle and the dynamic obstacle based on the underwater vehicle navigation and positioning information and the dynamic obstacle information.

[0057] Convert to radians: Convert to radians: Convert to radians: Convert to radians: in, is the longitude of the dth dynamic obstacle, is the arc value corresponding to the longitude of the dth dynamic obstacle, is the latitude of the dth dynamic obstacle, is the radian value corresponding to the latitude of the dth dynamic obstacle, is the horizontal azimuth angle between the underwater vehicle and the dth dynamic obstacle.

[0058] The latitude and longitude geographic coordinates of the dynamic obstruction ( , , ) converted to xyz Cartesian coordinates . is the longitude of the dynamic obstruction, is the latitude of the dynamic obstruction, is the depth of the dynamic obstruction, is the x-axis coordinate of the dynamic obstacle in the xyz Cartesian coordinate system, is the y-axis coordinate of the dynamic obstacle in the xyz Cartesian coordinate system, is the z-axis coordinate of the dynamic obstacle in the xyz Cartesian coordinate system.

[0059] in, is the distance between the underwater vehicle and the dynamic obstacle.

[0060] S42: Calculate the horizontal DCPA and TCPA based on the horizontal azimuth and distance between the underwater vehicle and the dynamic obstacle.

[0061] The calculation process of horizontal DCPA is: if , otherwise, in, is the total velocity of the underwater vehicle on the horizontal plane, is the total velocity of the dynamic obstacle on the horizontal plane, is the eastward speed of the dynamic obstacle, is the north speed of the dynamic obstacle, is the eastward component of the horizontal velocity of the dynamic obstacle, is the heading of the dynamic obstruction, is the north component of the horizontal velocity of the dynamic obstacle, is the eastward component of the underwater vehicle's horizontal velocity, is the north component of the underwater vehicle's horizontal velocity, is the eastward component of the relative velocity between the underwater vehicle and the dynamic obstacle, is the north component of the relative velocity between the underwater vehicle and the dynamic obstacle, is the horizontal relative motion speed between the underwater vehicle and the dynamic obstacle, is the relative motion angle between the underwater vehicle and the dynamic obstacle, For horizontal DCPA, To find the absolute value.

[0062] When it is not 0, the calculation formula of TCPA is: in, For TCPA.

[0063] When it is 0, the dynamic obstruction and the underwater vehicle are relatively stationary, the TCPA value is extremely large, there is no safety hazard, and no subsequent calculation is required.

[0064] S43: Calculate vertical DCPA based on TCPA. The calculation formula is: in, Indicates vertical DCPA, is the vertical speed of the dynamic obstacle.

[0065] S5: Based on the horizontal DCPA, vertical DCPA and the safe distance of dynamic obstructions, dynamic obstruction alarm detection is performed to determine whether there are safety hazards; for dynamic obstructions that pose safety hazards, their avoidance parameters are calculated.

[0066] The method for judging the safety hazards of dynamic obstructions is as follows: Set the horizontal safety distance of dynamic obstacles Vertical safety distance from dynamic obstacles According to the 1972 International Regulations for Preventing Collisions at Sea, and in accordance with the principle of early detection, early avoidance and early separation, The size is 7 times the size of the long axis of the underwater vehicle, The size is 1 times the long axis size of the underwater vehicle.

[0067] Determine whether there is a safety hazard: If ,and , there is a safety hazard and the heading or vertical speed needs to be adjusted. Otherwise, there is no safety hazard.

[0068] The calculation method of the avoidance parameters of dynamic obstacles is similar to that of static obstacles.

[0069] The calculation process of the dynamic obstacle avoidance parameters is as follows: S511: Select multiple headings according to the first interval value; S512: Calculate the horizontal DCPA and vertical DCPA between the dynamic obstacle and the underwater vehicle in each heading direction, and then determine whether there is a safety hazard; S513: From the headings without potential safety hazards, the heading with the smallest difference from the underwater vehicle's track direction is selected as the optimal heading; the optimal heading and its corresponding horizontal DCPA and vertical DCPA are used as part of the dynamic obstacle avoidance parameters.

[0070] S521: Selecting a plurality of vertical velocities according to the vertical velocity of the underwater vehicle and the second interval value; S522: Calculate the horizontal DCPA and vertical DCPA between the dynamic obstruction and the underwater vehicle based on each vertical velocity, and then determine whether there is a safety hazard. S523: From the vertical speeds that do not pose a safety hazard, the vertical speed that has the smallest difference from the vertical speed of the underwater vehicle is selected as the optimal vertical speed. The optimal vertical speed and its corresponding horizontal DCPA and vertical DCPA serve as another part of the dynamic obstacle avoidance parameters.

[0071] S6: Calculate reference avoidance parameters based on the static obstacle avoidance parameters and the dynamic obstacle avoidance parameters.

[0072] According to the calculation results of step S3 and step S5, the number of avoidance parameters may be 0, 1, or more than 1. The number of avoidance parameters is the same as the total number of static obstructions and dynamic obstructions that pose safety hazards.

[0073] If the number of circumvention parameters is 0, there is no security risk and no reference circumvention parameters are required. The reference circumvention parameters are default.

[0074] If there is only one avoidance parameter, that parameter is used directly as the reference avoidance parameter. Specifically, the optimal heading among the avoidance parameters is used as the heading avoidance parameter, and the optimal vertical speed is used as the vertical speed avoidance parameter. The reference avoidance parameters include the heading avoidance parameter and the vertical speed avoidance parameter.

[0075] When there are more than one avoidance parameter, consistency is required. Since the avoidance parameters include the optimal heading and optimal vertical speed, the consistency criteria also consist of two parts: the heading consistency criterion is that the optimal heading difference must be ≤1°, and the vertical speed consistency criterion is that the optimal vertical speed difference must be ≤1m / s.

[0076] If the avoidance parameters meet the heading consistency judgment standard, that is, the difference between the two optimal headings is ≤1°, in order to make the heading more accurate, the heading avoidance parameters are calculated based on the optimal heading.

[0077] The calculation process of the heading avoidance parameters is: An optimal heading value range is obtained according to the optimal heading, and multiple headings are selected from the optimal heading value range at intervals of a third interval value; in this embodiment, the third interval value is 0.1°; By calculating the three-dimensional transverse distance, horizontal DCPA, and vertical DCPA, it is determined whether static and dynamic obstacles pose safety hazards. The final optimal heading that eliminates safety hazards for both static and dynamic obstacles is selected from the above multiple headings. If there are multiple final optimal headings, the average is taken as the heading avoidance parameter. If there is only one final optimal heading, it is directly used as the heading avoidance parameter. If there is no final optimal heading (this situation generally does not occur), the midpoint of the optimal heading value range is taken as the heading avoidance parameter.

[0078] Since the optimal heading difference is ≤1°, the optimal heading range does not exceed 1°. Assuming there are two static obstacles and one dynamic obstacle, the corresponding optimal headings are 30°, 30°, and 31°, respectively. The optimal heading range is [30°, 31°]. Taking values ​​at 0.1° intervals, the resulting headings are 30.1°, 30.2°, 30.3°, 30.4°, 30.5°, 30.6°, 30.7°, 30.8°, and 30.9°. After calculation and safety hazard assessment, it was determined that headings of 30.3°, 30.4°, and 30.5° pose no safety hazards to either the two static obstacles or the one dynamic obstacle. Therefore, 30.3°, 30.4°, and 30.5° are the final optimal headings, and the average is 30.4°, which is the heading avoidance parameter. To ensure the real-time and simplicity of calculation, 0.1° is selected as the interval and only one round is calculated.

[0079] If the avoidance parameters meet the vertical speed consistency judgment standard, that is, the difference between the two optimal vertical speeds is ≤1m / s, in order to make the vertical speed more accurate, the vertical speed avoidance parameters are calculated based on the optimal vertical speed.

[0080] The calculation process of the vertical speed avoidance parameter is: An optimal vertical speed value range is obtained according to the optimal vertical speed, and a plurality of vertical speeds are selected from the optimal vertical speed value range at intervals of a fourth interval value; in this embodiment, the fourth interval value is 0.1; By calculating the three-dimensional transverse distance, horizontal DCPA, and vertical DCPA, it is determined whether static and dynamic obstacles pose safety hazards. The final optimal vertical speed that eliminates safety hazards for both static and dynamic obstacles is selected from the above multiple vertical speeds. If there are multiple final optimal vertical speeds, the average is taken as the vertical speed avoidance parameter. If there is only one final optimal vertical speed, it is directly used as the vertical speed avoidance parameter. If there is no final optimal vertical speed (this situation generally does not occur), the midpoint of the optimal vertical speed value range is taken as the vertical speed avoidance parameter.

[0081] Since the optimal vertical speed differs by ≤1 m / s, the optimal vertical speed range does not exceed 1 m / s. Assuming there are two static obstructions and one dynamic obstruction, the corresponding optimal vertical speeds are 1, 2, and 2, respectively. The optimal vertical speed range is [1, 2]. Using intervals of 0.1, the resulting vertical speeds are 1.1, 1.2, 1.3, 1.4, 1.5, 1.6, 1.7, 1.8, and 1.9. After calculation and safety hazard assessment, it was determined that at vertical speeds of 1.5 and 1.6, neither the two static obstructions nor the one dynamic obstruction presents a safety hazard. Therefore, 1.5 and 1.6 are the final optimal vertical speeds, and the average is 1.55, which is the vertical speed avoidance parameter. To ensure real-time calculation and simplicity, an interval of 0.1 is selected, and only one round of calculation is performed.

[0082] When the avoidance parameters meet both the heading consistency judgment criteria and the vertical speed consistency judgment criteria, the heading avoidance parameters and the vertical speed avoidance parameters are calculated separately.

[0083] If all optimal values ​​are the same, they are used directly as the heading avoidance parameter, and no further calculation is required. Similarly, if all optimal vertical speed values ​​are the same, they are used directly as the vertical speed avoidance parameter, and no further calculation is required.

[0084] If the avoidance parameters do not meet both the heading consistency judgment criteria and the vertical speed consistency judgment criteria, the hazard weight is calculated based on the avoidance parameters, and the avoidance parameter with the largest hazard weight is selected as the reference avoidance parameter.

[0085] The calculation formula for the hazard weight of static obstructions is: in, is the hazard weight of the static obstruction; The calculation formula of the hazard weight of dynamic obstructions is: in, is the horizontal hazard weight of the dynamic obstruction, is the vertical hazard weight of the dynamic obstruction.

[0086] Compare 、 and The larger the value, the more dangerous it is and the higher the priority.

[0087] This method performs real-time calculations based on the current underwater vehicle navigation and positioning information, providing reference avoidance parameters. After the underwater vehicle adjusts its navigation parameters according to the reference avoidance parameters, some static and dynamic obstructions that currently pose no safety hazards may become so. This will need to be taken into account in the next calculation using this method; it is not considered in this calculation.

[0088] This embodiment verifies the effectiveness of this method through the following experiments.

[0089] Assume that the underwater vehicle is 200m long, the static obstacle safety distance is 600m, the horizontal obstacle safety distance is 1400m, and the vertical obstacle safety distance is 200m.

[0090] The current position of the underwater vehicle is 110° longitude, 10° latitude, -100m altitude (negative below the horizontal plane), 90° track direction, 1kn horizontal speed, and -0.5m / s vertical speed (negative downward and positive upward).

[0091] Static obstacle A: longitude 110.003°, latitude 9.998°, altitude -170m; Static obstacle B: longitude 110.01°, latitude 9.998°, altitude -170m; Dynamic obstacle A: longitude 110.02°, latitude 10.012°, altitude: -200m, heading 270.1°, horizontal speed 2.1kn, vertical speed 0.3m / s; Dynamic obstacle B: longitude 110.05°, latitude 10.012°, altitude: -200m, heading 280.5°, horizontal speed 2.1kn, vertical speed 0.3m / s.

[0092] After calculation: The three-dimensional transverse distance of static obstacle A is 333.439654m, and the transverse time is 639s; The three-dimensional transverse distance of static obstacle B is 1019.777546m, and the transverse time is 2131s; The horizontal DCPA of dynamic obstacle A is 1329.885259m, the TCPA is 1374.000000s, and the vertical DCPA is 999.200000m; The horizontal DCPA of dynamic obstacle B is 1996.291623m, the TCPA is 3320.000000s, and the vertical DCPA is 2556.000000m.

[0093] According to the safety threshold, static obstruction B, dynamic obstruction A, and dynamic obstruction B do not pose a safety hazard, while static obstruction A does.

[0094] The following calculation is performed for the change of course of the static obstacle A. The calculation results are shown in Table 1.

[0095] Table 1 The calculation of the vertical speed change of the static obstacle A is carried out, and the calculation results are shown in Table 2.

[0096] Table 2 Tables 1 and 2 show that the optimal heading for static obstacle A is 91° and the optimal vertical speed is -1.5. The reference avoidance parameters are 91° and -1.5 m / s, respectively. Crew members can select these parameters based on their actual needs.

[0097] This embodiment also provides an obstacle avoidance system for underwater vehicles, such as Figure 3 As shown, the technical solution adopted is as follows: including: data acquisition module 1, static obstacle parameter calculation module 2, static obstacle avoidance parameter calculation module 3, dynamic obstacle parameter calculation module 4, dynamic obstacle avoidance parameter calculation module 5 and reference avoidance parameter calculation module 6.

[0098] Data acquisition module, used to obtain underwater vehicle navigation and positioning information, static obstruction information and dynamic obstruction information in real time; A static obstacle parameter calculation module is used to calculate the three-dimensional vertical and horizontal distances between each static obstacle and the underwater vehicle based on the underwater vehicle navigation positioning information and the static obstacle information; The static obstruction avoidance parameter calculation module is used to determine whether there is a safety hazard based on the three-dimensional vertical and horizontal distances and the static obstruction safety distance; for static obstructions that pose a safety hazard, it calculates their avoidance parameters; A dynamic obstacle parameter calculation module is used to calculate the horizontal DCPA and vertical DCPA between the dynamic obstacle and the underwater vehicle based on the underwater vehicle navigation positioning information and the dynamic obstacle information; The dynamic obstacle avoidance parameter calculation module is used to determine whether there is a safety hazard based on the horizontal DCPA, vertical DCPA and the safe distance of the dynamic obstacle; for dynamic obstacles with safety hazards, it calculates their avoidance parameters; The reference avoidance parameter calculation module is used to calculate the reference avoidance parameter according to the avoidance parameter of the static obstacle and the avoidance parameter of the dynamic obstacle.

[0099] The present invention also provides a computer-readable storage medium having a computer program stored thereon, wherein the computer program implements the above method when executed by a processor.

[0100] The present invention also provides a computer program product, comprising a computer program, which implements the above method when executed by a processor.

[0101] 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 make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the various embodiments of the present invention.

Claims

1. A method for avoiding obstacles for underwater vehicles, characterized in that: The following steps are involved: S1: Real-time acquisition of underwater vehicle navigation and positioning information, static obstacle information, and dynamic obstacle information; S2: Calculate the three-dimensional vertical and horizontal distances between each static obstacle and the underwater vehicle based on the underwater vehicle navigation positioning information and the static obstacle information; S3: Determine whether there is a safety hazard based on the three-dimensional vertical and horizontal distances and the static obstruction safety distance; calculate the avoidance parameters for static obstructions that pose a safety hazard; S4: Calculate the horizontal DCPA and vertical DCPA between the dynamic obstacle and the underwater vehicle based on the underwater vehicle navigation positioning information and the dynamic obstacle information; S5: Determine whether there are safety hazards based on the horizontal DCPA, vertical DCPA, and the safe distance of dynamic obstructions; calculate the avoidance parameters for dynamic obstructions that pose safety hazards; S6: Calculate reference avoidance parameters based on the avoidance parameters of the static obstacle and the avoidance parameters of the dynamic obstacle; If the number of circumvention parameters is 0, refer to the default circumvention parameters. When the number of circumvention parameters is 1, the circumvention parameter is directly used as the reference circumvention parameter; When there are more than one avoidance parameter, if the avoidance parameter meets the heading consistency judgment standard, the heading avoidance parameter is calculated based on the optimal heading; if the avoidance parameter meets the heading consistency judgment standard, the heading avoidance parameter is calculated based on the optimal heading; if the avoidance parameter does not meet both the heading consistency judgment standard and the vertical speed consistency judgment standard, the hazard weight is calculated based on the avoidance parameter, and the avoidance parameter with the largest hazard weight is selected as the reference avoidance parameter; The reference avoidance parameters include heading avoidance parameters and vertical speed avoidance parameters.

2. The method for avoiding obstacles for underwater vehicles according to claim 1, characterized in that: Step S2 includes: S21: Calculating the horizontal azimuth angle between the underwater vehicle and the static obstruction based on the underwater vehicle navigation positioning information and the static obstruction information; S22: Calculate the track azimuth angle based on the horizontal azimuth angle between the underwater vehicle and the static obstacle, and then calculate the horizontal azimuth time and horizontal azimuth distance; S23: Calculate the longitude, latitude, and depth of the three-dimensional vertical point based on the vertical time and horizontal vertical distance. The specific process is as follows: Calculate the longitude and latitude of the two-dimensional vertical point based on the horizontal vertical distance; The longitude and latitude of the three-dimensional abscissa point are the same as those of the two-dimensional abscissa point; The calculation formula for the depth of the three-dimensional positive horizontal point is: in, is the depth of the three-dimensional positive horizontal point, is the vertical velocity of the underwater vehicle, is the positive transverse time, is the depth of the underwater vehicle; S24: Calculate the three-dimensional transverse distance between the underwater vehicle and the three-dimensional transverse point.

3. The method for avoiding obstacles for underwater vehicles according to claim 2, characterized in that: In step S3, the static obstacle avoidance parameters include the optimal heading and its corresponding three-dimensional transverse distance, the optimal vertical speed and its corresponding three-dimensional transverse distance; The calculation process of the avoidance parameters of static obstacles is: S311: Select multiple headings according to the first interval value; S312: Calculate the three-dimensional vertical and horizontal distances corresponding to the static obstruction in each heading direction, and then determine whether there is a safety hazard. S313: From the headings without potential safety hazards, select the heading with the smallest difference from the track direction of the underwater vehicle as the optimal heading; S321: Selecting a plurality of vertical velocities according to the vertical velocity of the underwater vehicle and the second interval value; S322: Calculate the three-dimensional transverse distance and three-dimensional transverse time corresponding to the static obstruction based on each vertical velocity, and then determine whether there is a safety hazard. S323: From the vertical speeds that do not pose a safety hazard, select a vertical speed that has the smallest difference from the vertical speed of the underwater vehicle as the optimal vertical speed.

4. The method for avoiding obstacles for underwater vehicles according to claim 1, characterized in that: Step S4 includes: S41: Calculating the horizontal azimuth and distance between the underwater vehicle and the dynamic obstacle based on the underwater vehicle navigation positioning information and the dynamic obstacle information; S42: Calculate horizontal DCPA and TCPA based on the horizontal azimuth and distance between the underwater vehicle and the dynamic obstacle; S43: Calculate vertical DCPA based on TCPA. The calculation formula is: in, For vertical DCPA, To find the absolute value, is the vertical speed of the dynamic obstacle, is the vertical velocity of the underwater vehicle, For TCPA, is the depth of the dynamic obstruction, is the depth of the underwater vehicle.

5. The method for avoiding obstacles for underwater vehicles according to claim 4, characterized in that: In step S5, the dynamic obstacle avoidance parameters include the optimal heading and its corresponding horizontal DCPA and vertical DCPA, the optimal vertical speed and its corresponding horizontal DCPA and vertical DCPA; The calculation process of the dynamic obstacle avoidance parameters is as follows: S511: Select multiple headings according to the first interval value; S512: Calculate the horizontal DCPA and vertical DCPA between the dynamic obstacle and the underwater vehicle in each heading direction, and then determine whether there is a safety hazard; S513: From the headings without potential safety hazards, select the heading with the smallest difference from the track direction of the underwater vehicle as the optimal heading; S521: Selecting a plurality of vertical velocities according to the vertical velocity of the underwater vehicle and the second interval value; S522: Calculate the horizontal DCPA and vertical DCPA between the dynamic obstruction and the underwater vehicle based on each vertical velocity, and then determine whether there is a safety hazard. S523: From the vertical speeds that do not pose a safety hazard, select a vertical speed that has the smallest difference from the vertical speed of the underwater vehicle as the optimal vertical speed.

6. The method for avoiding obstacles for underwater vehicles according to claim 1, characterized in that: The calculation formula for the hazard weight of static obstructions is: in, is the hazard weight of the static obstruction, is the static obstacle safety distance, is the three-dimensional normal and transverse distance; The calculation formula of the hazard weight of dynamic obstructions is: in, is the horizontal hazard weight of the dynamic obstruction, is the vertical hazard weight of the dynamic obstruction, For horizontal DCPA, For vertical DCPA, is the horizontal safety distance of dynamic obstacles, It is the vertical safety distance of dynamic obstacles.

7. The method for avoiding obstacles for underwater vehicles according to claim 1, characterized in that: The calculation process of the heading avoidance parameters is: Obtaining an optimal heading value range according to the optimal heading, and selecting a plurality of headings from the optimal heading value range at intervals of a third interval value; Selecting a final optimal course from the above multiple courses so that both static obstacles and dynamic obstacles do not pose a safety hazard; If there are multiple final optimal headings, take the average value as the heading avoidance parameter; if there is only one final optimal heading, take it directly as the heading avoidance parameter; if there is no final optimal heading, take the midpoint value of the optimal heading value range as the heading avoidance parameter; The calculation process of the vertical speed avoidance parameter is: Obtaining an optimal vertical speed value range according to the optimal vertical speed, and selecting a plurality of vertical speeds from the optimal vertical speed value range at intervals of a fourth interval value; The final optimal vertical speed that eliminates safety hazards for both static and dynamic obstacles is selected from the above multiple vertical speeds. If there are multiple final optimal vertical speeds, the average is taken as the vertical speed avoidance parameter. If there is only one final optimal vertical speed, it is directly used as the vertical speed avoidance parameter. If there is no final optimal vertical speed, the midpoint of the optimal vertical speed value range is taken as the heading avoidance parameter.

8. An obstacle avoidance system for an underwater vehicle, characterized in that: The method for avoiding an obstacle for an underwater vehicle according to any one of claims 1 to 7 comprises: a data acquisition module, a static obstacle parameter calculation module, a static obstacle avoidance parameter calculation module, a dynamic obstacle parameter calculation module, a dynamic obstacle avoidance parameter calculation module, and a reference avoidance parameter calculation module. Data acquisition module, used to obtain underwater vehicle navigation and positioning information, static obstruction information and dynamic obstruction information in real time; A static obstacle parameter calculation module is used to calculate the three-dimensional vertical and horizontal distances between each static obstacle and the underwater vehicle based on the underwater vehicle navigation positioning information and the static obstacle information; The static obstruction avoidance parameter calculation module is used to determine whether there is a safety hazard based on the three-dimensional vertical and horizontal distances and the static obstruction safety distance; for static obstructions that pose a safety hazard, it calculates their avoidance parameters; A dynamic obstacle parameter calculation module is used to calculate the horizontal DCPA and vertical DCPA between the dynamic obstacle and the underwater vehicle based on the underwater vehicle navigation positioning information and the dynamic obstacle information; The dynamic obstacle avoidance parameter calculation module is used to determine whether there is a safety hazard based on the horizontal DCPA, vertical DCPA and the safe distance of the dynamic obstacle; for dynamic obstacles with safety hazards, it calculates their avoidance parameters; The reference avoidance parameter calculation module is used to calculate the reference avoidance parameter according to the avoidance parameter of the static obstacle and the avoidance parameter of the dynamic obstacle.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the obstacle avoidance method for an underwater vehicle according to any one of claims 1 to 7 is implemented.

10. A computer program product comprising a computer program, characterized in that When the computer program is executed by a processor, the obstacle avoidance method for an underwater vehicle according to any one of claims 1 to 7 is implemented.

Images