Obstruction avoidance method, system, medium and product for underwater vehicle

Abstract

The application relates to the technical field of ship navigation operation, and particularly discloses a navigation-obstructing-object-avoiding method and system of an underwater vehicle, a medium and a product, which comprises the following steps: acquiring navigation positioning information of the underwater vehicle, static navigation-obstructing-object information and dynamic navigation-obstructing-object information in real time; calculating three-dimensional normal transverse distances between each static navigation-obstructing object and the underwater vehicle; judging whether there is a safety hazard; calculating an avoiding parameter; calculating horizontal DCPA and vertical DCPA between the dynamic navigation-obstructing object and the underwater vehicle; judging whether there is a safety hazard; calculating an avoiding parameter; and calculating a reference avoiding parameter according to the avoiding parameter of the static navigation-obstructing object and the avoiding parameter of the dynamic navigation-obstructing object. The application can give a dangerous prompt of static obstacles and dynamic obstacles through comprehensive and real-time calculation, and provide a movement scheme selection for eliminating the danger, so that the operation difficulty of the navigation personnel can be effectively reduced, the error rate can be reduced, and the safety of underwater navigation can be improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of ship navigation, and in particular to a method and system for avoiding navigation hazards for underwater vehicles, a medium and a product. BACKGROUND

[0002] When an underwater vehicle is navigating underwater, the line of sight is affected and the observation range is limited, and the detection device has a much shorter effective distance than a surface vehicle, which brings great hidden dangers to navigation safety. During the navigation of an underwater vehicle, it may encounter static obstacles such as islands, buoys, and sunken ships, and may also encounter dynamic obstacles such as underwater moving targets. In order to avoid collision hazards, it is necessary to predict and judge the navigation situation in a timely manner.

[0003] When navigating on the surface, the detection of static navigation hazards is realized by judging the abeam distance and the abeam time. The abeam distance refers to the distance between the ship and the object when the object is abeam, and in chart work, it refers to the distance from the intersection of the abeam position line and the track line to the object. When a ship is navigating, if there are dangerous objects, reefs, shoals, sunken ships, etc., it is required to maintain sufficient abeam distance to safely pass through, and sufficient abeam time is provided for the navigation personnel to make a response. For underwater navigation, only the horizontal abeam distance is considered, and the shortest three-dimensional space straight line distance from the dangerous position point to the three-dimensional track line of the vehicle is also considered.

[0004] When navigating on the surface, for dynamic navigation hazards, the two most important parameters for evaluating the risk of collision are the distance to the closest point of approach (DCPA) between the two ships and the time to the closest point of approach (TCPA). DCPA refers to the minimum distance that can be reached by two ships on the horizontal plane while maintaining the same heading and speed. The safe distance approaching (SDA) is a distance range that can be passed through to avoid collision, i.e., the distance that can be safely passed through between the ship and the dynamic navigation hazard. When the dynamic navigation hazard passes through a place beyond the safe distance approaching, the ship does not need to take any action to avoid collision, and there is no danger of collision between the two parties, and the original speed and heading can be maintained; when the dynamic navigation hazard passes through a place within the safe distance approaching, there is a risk of collision, and it is necessary to take action to avoid collision, such as adjusting the heading to turn.

[0005] For underwater navigation, not only the horizontal DCPA, but also the vertical DCPA, i.e. the closest meeting distance in the vertical direction, is considered, and the vertical safe meeting distance is set. When judging whether there is a collision danger, the horizontal DCPA and the vertical DCPA are considered comprehensively, when the horizontal DCPA exceeds the safe meeting distance and the vertical DCPA does not exceed the vertical safe meeting distance, no danger occurs, when the horizontal DCPA and the vertical DCPA both exceed the safe meeting distance, danger occurs, and measures need to be taken.

[0006] The existing method is more suitable for surface navigation, and it is difficult to accurately judge the collision risk in time according to the navigation information of the underwater vehicle, the information of the static navigation obstacle and the dynamic navigation obstacle, and give an avoidance scheme. SUMMARY

[0007] The present application aims to solve the above problems. To this end, the present application provides a navigation obstacle avoidance method, system, medium and product for underwater vehicles, which performs static navigation obstacle detection, i.e. three-dimensional cross parameter calculation, static navigation obstacle alarm judgment and reference information calculation, and dynamic navigation obstacle detection, i.e. horizontal DCPA, vertical DCPA and TCPA, dynamic navigation obstacle alarm judgment and reference information calculation, according to the navigation positioning information of the underwater vehicle, the static navigation obstacle information and the dynamic navigation obstacle information. The present application can give a dangerous prompt of static and dynamic obstacles through comprehensive and real-time calculation, and provide a motion scheme selection to eliminate the danger, which can effectively reduce the difficulty of operation of the navigation personnel and the error rate, thereby improving the safety of underwater navigation.

[0008] The present application provides a navigation obstacle avoidance method for underwater vehicles, which adopts the following technical solutions:

[0009] S1: Real-time acquisition of underwater vehicle navigation positioning information, static navigation obstacle information and dynamic navigation obstacle information;

[0010] S2: According to the underwater vehicle navigation positioning information and the static navigation obstacle information, the three-dimensional cross distance between each static navigation obstacle and the underwater vehicle is calculated;

[0011] S3: According to the three-dimensional cross distance and the static navigation obstacle safety distance, it is judged whether there is a safety hazard, and for the static navigation obstacle with a safety hazard, the avoidance parameter is calculated;

[0012] S4: According to the underwater vehicle navigation positioning information and the dynamic navigation obstacle information, the horizontal DCPA and the vertical DCPA between the dynamic navigation obstacle and the underwater vehicle are calculated;

[0013] S5: judging whether there is a safety hazard according to the horizontal DCPA, the vertical DCPA and the dynamic obstacle safety distance; calculating the avoidance parameter of the dynamic obstacle with a safety hazard;

[0014] S6: calculating the reference avoidance parameter according to the avoidance parameter of the static obstacle and the avoidance parameter of the dynamic obstacle;

[0015] When the number of avoidance parameters is 0, the reference avoidance parameter is default;

[0016] When the number of avoidance parameters is 1, the avoidance parameter is directly taken as the reference avoidance parameter;

[0017] When the number of avoidance parameters is more than 1, if the avoidance parameter meets the heading consistency judgment standard, the heading avoidance parameter is calculated according to the optimal heading; if the avoidance parameter meets the heading consistency judgment standard, the heading avoidance parameter is calculated according to the optimal heading; if the avoidance parameter does not meet the heading consistency judgment standard and the vertical speed consistency judgment standard at the same time, the danger weight is calculated according to the avoidance parameter, and the avoidance parameter with the largest danger weight is taken as the reference avoidance parameter;

[0018] The reference avoidance parameter includes the heading avoidance parameter and the vertical speed avoidance parameter.

[0019] Further, step S2 includes:

[0020] S21: calculating the horizontal azimuth angle between the underwater vehicle and the static obstacle according to the underwater vehicle navigation positioning information and the static obstacle information;

[0021] S22: calculating the track azimuth angle according to the calculated horizontal azimuth angle between the underwater vehicle and the static obstacle, and then calculating the time to cross and the horizontal distance to cross;

[0022] S23: calculating the longitude, latitude and depth of the three-dimensional cross point according to the time to cross and the horizontal distance to cross; the specific process is as follows:

[0023] The longitude and latitude of the two-dimensional cross point are calculated according to the horizontal distance to cross;

[0024] The longitude and latitude of the three-dimensional cross point are the same as those of the two-dimensional cross point;

[0025] The calculation formula of the depth of the three-dimensional cross point is:

[0026]

[0027] wherein, is the depth of the three-dimensional cross point, is the vertical speed of the underwater vehicle, is the time to cross, the depth of the underwater vehicle;

[0028] S24: calculating the three-dimensional abeam distance between the underwater vehicle and the three-dimensional abeam point.

[0029] Further, in step S3, the avoidance parameters of the static obstacle include the optimal heading and its corresponding three-dimensional abeam distance, the optimal vertical speed and its corresponding three-dimensional abeam distance.

[0030] The calculation process of the avoidance parameters of the static obstacle is as follows:

[0031] S311: selecting multiple headings according to the first interval value;

[0032] S312: calculating the three-dimensional abeam distance corresponding to the static obstacle on each heading, and then judging whether there is a safety hazard;

[0033] S313: selecting the heading with the smallest difference from the heading direction of the underwater vehicle from the headings without safety hazards as the optimal heading.

[0034] S321: selecting multiple vertical speeds according to the vertical speed of the underwater vehicle and the second interval value;

[0035] S322: calculating the three-dimensional abeam distance and the three-dimensional abeam time corresponding to the static obstacle according to each vertical speed, and then judging whether there is a safety hazard;

[0036] S323: selecting the vertical speed with the smallest difference from the vertical speed of the underwater vehicle from the vertical speeds without safety hazards as the optimal vertical speed.

[0037] Further, step S4 includes:

[0038] S41: calculating the horizontal bearing angle and distance between the underwater vehicle and the dynamic obstacle according to the underwater vehicle navigation positioning information and the dynamic obstacle information;

[0039] S42: calculating the horizontal DCPA and TCPA according to the horizontal bearing angle and distance between the underwater vehicle and the dynamic obstacle;

[0040] S43: calculating the vertical DCPA according to the TCPA, and the calculation formula is:

[0041]

[0042] wherein, the vertical DCPA, the absolute value, the vertical speed of the dynamic obstacle, the vertical speed of the underwater vehicle, the TCPA, a depth of the dynamic hazard, a depth of the underwater vehicle.

[0043] Further, in step S5, the avoidance parameters of the dynamic hazard include an optimal heading and its corresponding horizontal DCPA and vertical DCPA, an optimal vertical speed and its corresponding horizontal DCPA and vertical DCPA;

[0044] The calculation process of the avoidance parameters of the dynamic hazard is as follows:

[0045] S511: selecting multiple headings according to the first interval value;

[0046] S512: calculating the horizontal DCPA and the vertical DCPA between the dynamic hazard and the underwater vehicle on each heading, and then judging whether there is a safety hazard;

[0047] S513: selecting, from the headings without safety hazards, a heading with the smallest difference from the heading of the underwater vehicle as the optimal heading;

[0048] S521: selecting multiple vertical speeds according to the vertical speed of the underwater vehicle and the second interval value;

[0049] S522: calculating the horizontal DCPA and the vertical DCPA between the dynamic hazard and the underwater vehicle according to each vertical speed, and then judging whether there is a safety hazard;

[0050] S523: selecting, from the vertical speeds without safety hazards, a vertical speed with the smallest difference from the vertical speed of the underwater vehicle as the optimal vertical speed.

[0051] Further, the calculation formula of the danger weight of the static hazard is:

[0052]

[0053] wherein, the danger weight of the static hazard, the safety distance of the static hazard, the three-dimensional positive lateral distance;

[0054] The calculation formula of the danger weight of the dynamic hazard is:

[0055]

[0056]

[0057] wherein, the horizontal danger weight of the dynamic hazard, the vertical danger weight of the dynamic hazard, a horizontal DCPA, a vertical DCPA, a dynamic obstacle horizontal safety distance, a dynamic obstacle vertical safety distance.

[0058] Further, the calculation process of the heading avoidance parameter is as follows:

[0059] According to the optimal heading, an optimal heading value range is obtained, and a plurality of headings are selected from the optimal heading value range with a third interval value as an interval;

[0060] From the above plurality of headings, a final optimal heading is selected, which does not have safety hazards for static obstacles and dynamic obstacles; if the final optimal heading is multiple, the average value is taken as the heading avoidance parameter; if the final optimal heading is one, it is directly taken as the heading avoidance parameter; if there is no final optimal heading, the midpoint value of the optimal heading value range is taken as the heading avoidance parameter;

[0061] The calculation process of the vertical velocity avoidance parameter is as follows:

[0062] According to the optimal vertical velocity, an optimal vertical velocity value range is obtained, and a plurality of vertical velocities are selected from the optimal vertical velocity value range with a fourth interval value as an interval;

[0063] From the above plurality of vertical velocities, a final optimal vertical velocity is selected, which does not have safety hazards for static obstacles and dynamic obstacles; if the final optimal vertical velocity is multiple, the average value is taken as the vertical velocity avoidance parameter; if the final optimal vertical velocity is one, it is directly taken as the vertical velocity avoidance parameter; if there is no final optimal vertical velocity, the midpoint value of the optimal vertical velocity value range is taken as the vertical velocity avoidance parameter.

[0064] The application also provides an obstacle avoidance system of an underwater vehicle, which adopts the technical scheme as follows: 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,

[0065] The data acquisition module is used for acquiring the underwater vehicle navigation positioning information, the static obstacle information and the dynamic obstacle information in real time.

[0066] The static obstacle parameter calculation module is used for calculating the three-dimensional positive lateral distance between each static obstacle and the underwater vehicle according to the underwater vehicle navigation positioning information and the static obstacle information.

[0067] The static obstacle avoidance parameter calculation module is used for judging whether there is a safety hazard according to the three-dimensional positive lateral distance and the static obstacle safety distance; for the static obstacle with a safety hazard, the avoidance parameter is calculated.

[0068] a dynamic obstacle parameter calculation module, configured to calculate horizontal DCPA and vertical DCPA between the underwater vehicle and the dynamic obstacle according to the underwater vehicle navigation positioning information and the dynamic obstacle information;

[0069] a dynamic obstacle avoidance parameter calculation module, configured to determine whether there is a safety hazard according to the horizontal DCPA, the vertical DCPA and the dynamic obstacle safety distance, and calculate avoidance parameters of the dynamic obstacle with the safety hazard;

[0070] a reference avoidance parameter calculation module, configured to calculate reference avoidance parameters according to the avoidance parameters of the static obstacle and the avoidance parameters of the dynamic obstacle.

[0071] The application further provides a computer readable storage medium, which has a computer program stored thereon, and the computer program is executed by a processor to implement the method.

[0072] The application further provides a computer program product, which comprises a computer program, and the computer program is executed by a processor to implement the method.

[0073] The one or more technical solutions in the embodiments of the application have at least one of the following technical effects:

[0074] 1. The application supports safety hazard determination of one or more static obstacles and dynamic obstacles under water. The application calculates three-dimensional positive transverse parameters between the underwater vehicle and the static obstacle, and horizontal DCPA and vertical DCPA between the underwater vehicle and the dynamic obstacle in real time, accurately determines the safety hazard, and gives avoidance parameters of the heading and vertical speed for the mariners to select and avoid the navigation risk.

[0075] 2. The application has the advantages of fast calculation speed, high accuracy, strong real-time performance, and giving alarm and reference prompt, improves the automation level of underwater three-dimensional navigation safety detection, makes the navigation personnel more rapid and accurate in response, and plays a role in improving the navigation safety.

[0076] Additional aspects and advantages of the application will be described in part in the description which follows, will become apparent from the description, or will be learned by practice of the application. BRIEF DESCRIPTION OF DRAWINGS

[0077] In order to more clearly illustrate the technical solutions in the application or the prior art, the following will briefly introduce the drawings needed to be used in the embodiments or the prior art description. Obviously, the drawings in the following description are some embodiments of the application, and for those skilled in the art, other drawings can also be obtained without creative labor on the basis of these drawings.

[0078] Figure 1 is a flow chart of the method provided by the present application.

[0079] Figure 2 is a three-dimensional positive lateral parameter definition diagram provided by the present application.

[0080] Figure 3 is a structural block diagram of the system provided by the present application.

[0081] Reference signs:

[0082] 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, and reference avoidance parameter calculation module. DETAILED DESCRIPTION

[0083] In order to make the objects, technical solutions and advantages of the present application clearer, the technical solutions in the present application will be described clearly and completely below in combination with the drawings in the present application. Obviously, the described embodiments are some embodiments of the present application, but not all the embodiments. Based on the embodiments in the present application, all the other embodiments obtained by those skilled in the art without creative labor fall within the scope of protection of the present application. The following embodiments are used to illustrate the present application, but cannot be used to limit the scope of the present application.

[0084] In the description of the present application, the description of the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" means that the specific features, structures or characteristics described in combination with the embodiment or example are contained in at least one embodiment or example of the present application. In the present application, the illustrative description of the above terms is not necessarily for 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, the skilled in the art can combine and combine the different embodiments or examples described in the present application and the features of the different embodiments or examples without contradiction.

[0085] The following will be described in combination with Figures 1 to 3 The present application is further described in detail, and a navigation obstacle avoidance method, system, medium and product of an underwater vehicle are described:

[0086] In the present embodiment, as Figure 1 shown, a navigation obstacle avoidance method of an underwater vehicle is provided, comprising the following steps:

[0087] S1: Real-time acquisition of underwater vehicle navigation positioning information, static obstacle information and dynamic obstacle information.

[0088] The navigation positioning information of the underwater vehicle is obtained by a navigation positioning device on the underwater vehicle, including longitude, latitude, depth, track direction, eastward speed, northward speed and vertical speed.

[0089] According to the position information (longitude, latitude and depth) of the underwater vehicle, static navigation-obstructing objects within a threshold range are selected by viewing and picking from a chart or by setting a query range to query from a navigation-obstructing object database, and static navigation-obstructing object information is obtained, including longitude, latitude and depth. In this embodiment, the threshold is 5 nautical miles in horizontal distance.

[0090] The dynamic navigation-obstructing object information is obtained in real time by a detection device of the underwater vehicle, including longitude, latitude, depth, heading, eastward speed, northward speed and vertical speed.

[0091] S2: According to the navigation positioning information of the underwater vehicle and the static navigation-obstructing object information, the three-dimensional abeam distance between each static navigation-obstructing object and the underwater vehicle is calculated.

[0092] For underwater navigation, only the horizontal abeam distance should be considered, and the shortest three-dimensional spatial straight line distance from the dangerous position point to the three-dimensional trajectory line of the underwater vehicle should be considered. The definition of the three-dimensional abeam parameter is shown in Figure 2 Figure 2 In the figure, the red line d31 is the three-dimensional abeam distance, the red point is the three-dimensional abeam point, and the green point is the two-dimensional abeam point.

[0093] Specifically, the following steps are included:

[0094] S21: According to the navigation positioning information of the underwater vehicle and the static navigation-obstructing object information, the horizontal azimuth angle between the underwater vehicle and the static navigation-obstructing object is calculated, and the calculation process is as follows:

[0095]

[0096]

[0097]

[0098]

[0099]

[0100] wherein, is the longitude of the underwater vehicle, is the radian value corresponding to the longitude of the underwater vehicle, is the value of pi, which is about 3.1415926535, is the latitude of the underwater vehicle, is the radian value corresponding to the latitude of the underwater vehicle,​ longitude of the jth static obstacle, radian value corresponding to the longitude of the jth static obstacle, latitude of the jth static obstacle, radian value corresponding to the latitude of the jth static obstacle, horizontal azimuth angle between the underwater vehicle and the static obstacle, two-dimensional inverse tangent function.

[0101] S22: Calculate the track heading azimuth angle according to the calculation of the horizontal azimuth angle between the underwater vehicle and the static obstacle, and then calculate the horizontal crossing parameters, including the crossing time and the horizontal crossing distance. The calculation formula is as follows:

[0102] If the track heading azimuth angle is ,

[0103] If the track heading azimuth angle is ,

[0104] wherein, track heading of the underwater vehicle;

[0105] crossing point distance is ;

[0106] wherein, straight-line distance between the underwater vehicle and the static obstacle in the horizontal plane;

[0107] crossing time is ;

[0108] wherein, eastward speed of the underwater vehicle, northward speed of the underwater vehicle;

[0109] horizontal crossing distance is .

[0110] S23: Calculate the longitude, latitude and depth of the three-dimensional crossing point according to the crossing time and the horizontal crossing distance. The specific process is as follows:

[0111] First, calculate the longitude and latitude of the two-dimensional crossing point according to the horizontal crossing distance, and the calculation formula is as follows:

[0112]

[0113]

[0114]

[0115] wherein, is the equatorial radius, , is the radian value corresponding to the longitude of the two-dimensional prime vertical point, is the radian value corresponding to the latitude of the two-dimensional prime vertical point, is the longitude of the two-dimensional prime vertical point, is the latitude of the two-dimensional prime vertical point.

[0116] The longitude and latitude of the three-dimensional prime vertical point are the same as those of the two-dimensional prime vertical point, i.e. , , is the longitude of the three-dimensional prime vertical point, is the latitude of the three-dimensional prime vertical point.

[0117] The three-dimensional prime vertical time is the same as the two-dimensional prime vertical time.

[0118] Then, the depth of the three-dimensional prime vertical point is calculated, and the calculation formula is:

[0119]

[0120] wherein, is the depth of the three-dimensional prime vertical point, is the vertical velocity of the underwater vehicle, is the depth of the underwater vehicle.

[0121] S24: Calculate the three-dimensional prime vertical distance between the underwater vehicle and the three-dimensional prime vertical point.

[0122] Convert the position information of the underwater vehicle and the static navigation hazard 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 , , are converted to the xyz Cartesian coordinates , the longitude and latitude geographic coordinates of the three-dimensional prime vertical point , , are converted to the xyz Cartesian coordinates , , .

[0123] In the xyz Cartesian coordinate system, calculate the three-dimensional prime vertical distance between the underwater vehicle and the three-dimensional prime vertical point , and the calculation formula is:

[0124] ,

[0125] 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.

[0126] 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.

[0127] 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.

[0128] The method for judging the safety hazards of static obstructions is as follows:

[0129] 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.

[0130] 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.

[0131] 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.

[0132] The calculation process of the avoidance parameters of static obstacles is:

[0133] 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°.

[0134] S312: traversing all headings, calculating the three-dimensional crossing distance and the three-dimensional crossing time corresponding to the static obstacle for each heading, and then judging whether there is a safety hazard. This step uses the calculation method of step S2, taking the heading as the horizontal position angle between the underwater vehicle and the static obstacle. According to 360 headings, 360 groups of three-dimensional crossing parameters (three-dimensional crossing distance and three-dimensional crossing time) are calculated, and then whether there is a safety hazard is judged based on the above-mentioned safety hazard judgment method, and the heading without safety hazard is selected.

[0135] S313: selecting the heading with the smallest difference from the track direction of the underwater vehicle from the heading without safety hazard as the optimal heading. The optimal heading is closest to the track direction, with the smallest rotation and the smallest deviation from the course. As a suitable heading, it can avoid navigation danger. The optimal heading and its corresponding three-dimensional crossing distance are part of the avoidance parameters of the static obstacle.

[0136] S321: selecting multiple vertical velocities according to the vertical velocity of the underwater vehicle and the second interval value, i.e. selecting multiple vertical velocities from the vertical velocity of the underwater vehicle according to the second interval value. The vertical velocity of the underwater vehicle is a certain range with the vertical velocity of the underwater vehicle as the center point. The second interval value is set according to experience, which is ±1m / s in this embodiment, and 5 times are taken, so 10 vertical velocities are selected, which are: , , , , , , , , , .

[0137] S322: calculating the three-dimensional crossing distance and the three-dimensional crossing time corresponding to the static obstacle according to each vertical velocity, and then judging whether there is a safety hazard. This step uses the calculation method of step S2. According to 10 vertical velocities, 10 groups of three-dimensional crossing parameters (three-dimensional crossing distance and three-dimensional crossing time) are calculated, and then whether there is a safety hazard is judged based on the above-mentioned safety hazard judgment method, and the vertical velocity without safety hazard is selected.

[0138] S323: Select the vertical velocity with the least difference from the vertical velocity of the underwater vehicle as the optimal vertical velocity from the vertical velocities without safety hazards. When it is inconvenient to turn, for example, according to the requirements of the formation task, it must be driven according to the predetermined heading, and there is a danger on the heading, or the horizontal space is narrow such as a narrow waterway, etc., it can be avoided by adjusting the vertical velocity. The optimal vertical velocity and the corresponding three-dimensional positive lateral distance are another part of the avoidance parameters of the static obstacle.

[0139] This embodiment calculates both the optimal heading and the optimal vertical velocity, which are used in the subsequent calculation of the reference avoidance parameters.

[0140] S4: According to 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:

[0141] S40: In order to save computing resources and speed up the calculation, the dynamic obstacle screening range is set to screen the dynamic obstacle. The dynamic obstacle screening threshold includes horizontal radius and vertical distance, and the default is that the horizontal radius is 50 nautical miles and the vertical distance is 200 m. Only the dynamic obstacle within the distance range is calculated for the following parameters, and the dynamic obstacle outside the distance range is considered to have no safety hazards. The latitude and longitude geographic coordinates of the underwater vehicle and the dynamic obstacle are converted into xyz Cartesian coordinates, and then the current horizontal distance and the current vertical distance between the underwater vehicle and the dynamic obstacle are 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, it is considered that the dynamic obstacle may have safety hazards, and the following parameter calculation is performed.

[0142] S41: According to the underwater vehicle navigation positioning information and the dynamic obstacle information, the horizontal azimuth and distance between the underwater vehicle and the dynamic obstacle are calculated.

[0143] Convert to radian unit:

[0144] Convert to radian unit:

[0145] Convert to radian unit:

[0146] Convert to radian unit:

[0147]

[0148] wherein, is the longitude of the dth dynamic obstacle, is the radian value corresponding to the longitude of the dth dynamic obstacle, is the latitude of the dth dynamic obstacle, the radian value corresponding to the latitude of the dth dynamic obstacle, the horizontal azimuth angle between the underwater vehicle and the dth dynamic obstacle.

[0149] convert the geographic coordinates of longitude and latitude of the dynamic obstacle into xyz Cartesian coordinates . the longitude of the dynamic obstacle, the latitude of the dynamic obstacle, the depth of the dynamic obstacle, the x-axis coordinate of the dynamic obstacle in the xyz Cartesian coordinate system, the y-axis coordinate of the dynamic obstacle in the xyz Cartesian coordinate system, the z-axis coordinate of the dynamic obstacle in the xyz Cartesian coordinate system.

[0150]

[0151] wherein, the distance between the underwater vehicle and the dynamic obstacle.

[0152] S42: calculate the horizontal DCPA and TCPA according to the horizontal azimuth angle and the distance between the underwater vehicle and the dynamic obstacle.

[0153] The calculation process of the horizontal DCPA is as follows:

[0154]

[0155]

[0156]

[0157]

[0158]

[0159]

[0160]

[0161]

[0162]

[0163] If ,

[0164] Otherwise, ​​

[0165]

[0166] wherein, is the resultant velocity of the underwater vehicle on the horizontal plane, is the resultant velocity of the dynamic obstacle on the horizontal plane, is the east velocity of the dynamic obstacle, is the north velocity of the dynamic obstacle, is the east component of the horizontal velocity of the dynamic obstacle, is the heading of the dynamic obstacle, is the north component of the horizontal velocity of the dynamic obstacle, is the east component of the horizontal velocity of the underwater vehicle, is the north component of the horizontal velocity of the underwater vehicle, is the east component of the relative motion velocity between the underwater vehicle and the dynamic obstacle, is the north component of the relative motion velocity between the underwater vehicle and the dynamic obstacle, is the horizontal relative motion velocity between the underwater vehicle and the dynamic obstacle, is the relative motion direction angle between the underwater vehicle and the dynamic obstacle, is the horizontal DCPA, is the absolute value.

[0167] When the horizontal DCPA is not 0, the calculation formula of the TCPA is:

[0168]

[0169] wherein, is the TCPA.

[0170] When the horizontal DCPA is 0, the dynamic obstacle is relatively static with the underwater vehicle, the value of the TCPA is extremely large, there is no safety hazard, and no further calculation is needed.

[0171] S43: According to the TCPA, the vertical DCPA is calculated, and the calculation formula is:

[0172]

[0173] wherein, represents the vertical DCPA, is the vertical velocity of the dynamic obstacle.

[0174] S5: According to the horizontal DCPA, the vertical DCPA and the safety distance of the dynamic obstacle, the dynamic obstacle alarm detection is performed to determine whether there is a safety hazard; for the dynamic obstacle with a safety hazard, the avoidance parameter is calculated.

[0175] The judgment method of the safety risk of the dynamic obstacle is:

[0176] The horizontal safety distance of the dynamic obstacle is set and the vertical safety distance of the dynamic obstacle According to the International Regulations for Preventing Collisions at Sea, 1972, the principle of early detection, early avoidance and early separation is followed, and The size is 7 times the size of the long axis of the underwater vehicle, The size is 1 times the size of the long axis of the underwater vehicle.

[0177] Judge whether there is a safety risk: if , and , there is a safety risk, and the heading or vertical speed needs to be adjusted. Otherwise, there is no safety risk.

[0178] The calculation method of the avoidance parameter of the dynamic obstacle is similar to the calculation method of the avoidance parameter of the static obstacle.

[0179] The calculation process of the avoidance parameter of the dynamic obstacle is:

[0180] S511: Select multiple headings according to the first interval value;

[0181] S512: Calculate the horizontal DCPA and vertical DCPA between the dynamic obstacle and the underwater vehicle on each heading, and then judge whether there is a safety risk;

[0182] S513: From the headings without safety risks, select the heading with the smallest difference from the heading of the underwater vehicle as the optimal heading; the optimal heading and its corresponding horizontal DCPA and vertical DCPA are part of the avoidance parameter of the dynamic obstacle.

[0183] S521: Select multiple vertical speeds according to the vertical speed of the underwater vehicle and the second interval value;

[0184] S522: According to each vertical speed, calculate the horizontal DCPA and vertical DCPA between the dynamic obstacle and the underwater vehicle, and then judge whether there is a safety risk;

[0185] S523: From the vertical speeds without safety risks, select the vertical speed with the smallest difference from the vertical speed of the underwater vehicle as the optimal vertical speed. The optimal vertical speed and its corresponding horizontal DCPA and vertical DCPA are another part of the avoidance parameter of the dynamic obstacle.

[0186] S6: According to the avoidance parameter of the static obstacle and the avoidance parameter of the dynamic obstacle, calculate the reference avoidance parameter.

[0187] According to the calculation results of step S3 and step S5, the number of avoidance parameters can be 0, 1 or more than 1, and the number of avoidance parameters is the same as the total number of static and dynamic obstacles with potential safety hazards.

[0188] When the number of avoidance parameters is 0, there is no safety hazard, and there is no need to provide a reference avoidance parameter, and the reference avoidance parameter is default.

[0189] When the number of avoidance parameters is 1, the avoidance parameter is directly used as the reference avoidance parameter. That is, the optimal heading in the avoidance parameter is used as the heading avoidance parameter, and the optimal vertical velocity is used as the vertical velocity avoidance parameter. The reference avoidance parameter includes the heading avoidance parameter and the vertical velocity avoidance parameter.

[0190] When the number of avoidance parameters is more than 1, consistency judgment is needed. Since the avoidance parameters include optimal headings and optimal vertical velocities, the consistency judgment criteria also include two parts. The heading consistency judgment criterion is that the difference between the optimal headings is ≤1°, and the vertical velocity consistency judgment criterion is that the difference between the optimal vertical velocities is ≤1 m / s.

[0191] If the avoidance parameters meet the heading consistency judgment criterion, that is, the difference between the optimal headings is ≤1°, in order to make the heading more accurate, the heading avoidance parameter is calculated according to the optimal heading.

[0192] The calculation process of the heading avoidance parameter is as follows:

[0193] The optimal heading value range is obtained according to the optimal heading, and a plurality of headings are selected from the optimal heading value range with a third interval value as an interval. In this embodiment, the third interval value is 0.1°.

[0194] The three-dimensional positive lateral distance, horizontal DCPA and vertical DCPA are calculated to determine whether the static and dynamic obstacles have potential safety hazards. The final optimal heading is selected from the above plurality of headings, which makes the static and dynamic obstacles have no potential safety hazards. If the final optimal heading is more than one, the average value is taken as the heading avoidance parameter. If the final optimal heading is one, it is directly taken as the heading avoidance parameter. If there is no final optimal heading (this situation generally does not occur), the midpoint value of the optimal heading value range is taken as the heading avoidance parameter.

[0195] Since the optimal heading difference is less than or equal to 1°, the optimal heading value range will not exceed 1°. Assuming that there are two static obstacles and one dynamic obstacle, the corresponding optimal headings are 30°, 30° and 31°, and the optimal heading value range is [30°, 31°]. Taking 0.1° as the interval, the multiple headings obtained 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 determination, when the heading is 30.3°, 30.4° or 30.5°, there is no safety hazard for the two static obstacles and the one dynamic obstacle, and therefore 30.3°, 30.4° and 30.5° are the final optimal headings, and the average value of 30.3°, 30.4° and 30.5° is 30.4°, which is the heading avoidance parameter. In order to ensure the real-time and simplicity of the calculation, 0.1° is selected as the interval, and only one round of calculation is performed.

[0196] If the avoidance parameter meets the vertical velocity consistency judgment standard, i.e., the difference between each two optimal vertical velocities is less than or equal to 1 m / s, in order to make the vertical velocity more accurate, the vertical velocity avoidance parameter is calculated according to the optimal vertical velocity.

[0197] The calculation process of the vertical velocity avoidance parameter is as follows:

[0198] The optimal vertical velocity value range is obtained according to the optimal vertical velocity, and multiple vertical velocities are selected from the optimal vertical velocity value range with the fourth interval value as the interval. In this embodiment, the fourth interval value is 0.1.

[0199] The three-dimensional positive lateral distance, horizontal DCPA and vertical DCPA are calculated to determine whether the static obstacle and the dynamic obstacle exist safety hazards. The final optimal vertical velocity is selected from the above multiple vertical velocities, which makes the static obstacle and the dynamic obstacle both have no safety hazards. If there are multiple final optimal vertical velocities, the average value is taken as the vertical velocity avoidance parameter. If there is only one final optimal vertical velocity, it is directly taken as the vertical velocity avoidance parameter. If there is no final optimal vertical velocity (this situation generally does not occur), the midpoint value of the optimal vertical velocity value range is taken as the vertical velocity avoidance parameter.

[0200] 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.

[0201] 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.

[0202] 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.

[0203] 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.

[0204] The calculation formula for the hazard weight of static obstructions is:

[0205]

[0206] in, is the hazard weight of the static obstruction;

[0207] The calculation formula of the hazard weight of dynamic obstructions is:

[0208]

[0209]

[0210] in, is the horizontal hazard weight of the dynamic obstruction, is the vertical hazard weight of the dynamic obstruction.

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

[0212] The method provides reference avoidance parameters according to the current underwater vehicle navigation positioning information and the like. After the underwater vehicle adjusts the navigation parameters according to the reference avoidance parameters, some static obstacles and dynamic obstacles that do not currently have safety hazards may become safety hazards. This needs to be considered in the next calculation using the method, and is not considered in this calculation.

[0213] The embodiment verifies the effectiveness of the method through the following test.

[0214] It is assumed that the underwater vehicle is 200 m long, the static obstacle safety distance is 600 m, the dynamic obstacle horizontal safety distance is 1400 m, and the dynamic obstacle vertical safety distance is 200 m.

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

[0216] Static obstacle A: longitude 110.003°, latitude 9.998°, height -170 m;

[0217] Static obstacle B: longitude 110.01°, latitude 9.998°, height -170 m;

[0218] Dynamic obstacle A: longitude 110.02°, latitude 10.012°, height -200 m, heading 270.1°, horizontal speed 2.1 kn, and vertical speed 0.3 m / s;

[0219] Dynamic obstacle B: longitude 110.05°, latitude 10.012°, height -200 m, heading 280.5°, horizontal speed 2.1 kn, and vertical speed 0.3 m / s.

[0220] After calculation:

[0221] The three-dimensional positive lateral distance of static obstacle A is 333.439654 m, and the positive lateral time is 639 s;

[0222] The three-dimensional positive lateral distance of static obstacle B is 1019.777546 m, and the positive lateral time is 2131 s;

[0223] The horizontal DCPA of dynamic obstacle A is 1329.885259 m, the TCPA is 1374.000000 s, and the vertical DCPA is 999.200000 m;

[0224] The horizontal DCPA of the dynamic obstacle B is 1996.291623 m, the TCPA is 3320.000000 s, and the vertical DCPA is 2556.000000 m.

[0225] According to the safety threshold judgment, the static obstacle A, the dynamic obstacle A and the dynamic obstacle B do not have safety hazards, and the static obstacle B has a safety hazard.

[0226] The following calculates the change of the heading of the static obstacle A, and the calculation results are shown in Table 1.

[0227] Table 1

[0228]

[0229] The following calculates the change of the vertical velocity of the static obstacle A, and the calculation results are shown in Table 2.

[0230] Table 2

[0231]

[0232] As shown in Table 1 and Table 2, the optimal heading corresponding to the static obstacle A is 91°, and the optimal vertical velocity is -1.5. In the reference avoidance parameter, the heading avoidance parameter is 91°, and the vertical velocity avoidance parameter is -1.5 m / s. The crew can select according to actual needs.

[0233] The embodiment also provides an obstacle avoidance system of an underwater vehicle, as shown in Figure 3 The technical scheme adopted is as follows: comprising a data acquisition module 1, a static obstacle parameter calculation module 2, a static obstacle avoidance parameter calculation module 3, a dynamic obstacle parameter calculation module 4, a dynamic obstacle avoidance parameter calculation module 5 and a reference avoidance parameter calculation module 6.

[0234] The data acquisition module is used for acquiring the underwater vehicle navigation positioning information, the static obstacle information and the dynamic obstacle information in real time.

[0235] The static obstacle parameter calculation module is used for calculating the three-dimensional cross range between each static obstacle and the underwater vehicle according to the underwater vehicle navigation positioning information and the static obstacle information.

[0236] The static obstacle avoidance parameter calculation module is used for judging whether there is a safety hazard according to the three-dimensional cross range and the static obstacle safety distance, and calculating the avoidance parameter of the static obstacle with a safety hazard.

[0237] a dynamic obstacle parameter calculation module, configured to calculate horizontal DCPA and vertical DCPA between the underwater vehicle and the dynamic obstacle according to the underwater vehicle navigation positioning information and the dynamic obstacle information;

[0238] a dynamic obstacle avoidance parameter calculation module, configured to determine whether there is a safety hazard according to the horizontal DCPA, the vertical DCPA and the dynamic obstacle safety distance, and calculate avoidance parameters of the dynamic obstacle with the safety hazard;

[0239] a reference avoidance parameter calculation module, configured to calculate reference avoidance parameters according to the avoidance parameters of the static obstacle and the avoidance parameters of the dynamic obstacle.

[0240] The application further provides a computer readable storage medium, which stores a computer program, and the computer program is executed by a processor to realize the method.

[0241] The application further provides a computer program product, which comprises a computer program, and the computer program is executed by a processor to realize the method.

[0242] Finally, it should be noted that: the above examples are only used to illustrate the technical solutions of the present application, and not to limit them; although the present application has been described in detail with reference to the foregoing examples, those skilled in the art should understand that: it can still modify the technical solutions recorded in the foregoing examples, or make equivalent replacement for part of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present application.

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 vertical speed consistency judgment standard, the vertical speed avoidance parameter is calculated based on the optimal vertical speed; 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