A three-dimensional route monitoring method and system

Abstract

The present invention relates to the technical field of ship navigation operations, and provides a three-dimensional route monitoring method and system. The method includes determining the current segment based on the horizontal and vertical distances between two waypoints in the segment; calculating the horizontal deviation distance and the intersection point of the underwater vehicle and the current segment, and calculating the depth deviation distance and the three-dimensional deviation distance based on the intersection point; detecting whether there is an obstacle hazard using a fan-shaped detection azimuth interval method; obtaining the current water depth based on grid terrain data or nautical chart data, and calculating the true water depth based on tidal data and the current water depth; calculating the depth of the underwater vehicle from the water bottom based on the true water depth, and detecting whether there is a depth hazard based on the depth from the water bottom. The present invention solves the problem that traditional two-dimensional segment judgment cannot adapt to the three-dimensional underwater environment. It enables the vehicle to accurately identify the current segment even in complex sea conditions, grasp the true water depth of the current position in real time, effectively avoid dangers such as collisions with submarine mountains, and improve the safety of deep-sea navigation.

Description

Technical Field

[0001] The present invention relates to the technical field of ship navigation operations, and in particular to a three-dimensional route monitoring method and system. Background Art

[0002] Before setting sail, underwater vehicles need to develop a three-dimensional route plan based on their mission destination, utilizing nautical charts and ocean environment information. This route plan is crucial for ensuring the safe passage of underwater vehicles to their destination. However, during actual navigation, the vehicle may yaw due to factors such as internal ocean waves, geomagnetic anomalies, and errors in its own propulsion system. The underwater compass will inevitably deviate from the planned route by a certain distance, known as the yaw distance. Once the yaw distance exceeds the safety threshold, the vehicle will not only deviate from the intended target point, but may also enter dangerous areas, such as being caught in strong currents or colliding with unknown obstacles, leading to equipment damage or even mission failure. Therefore, accurately calculating yaw in three dimensions in real time is crucial for ensuring navigation safety.

[0003] Underwater vehicles commonly use sonar systems to detect obstacles, emitting sound waves and receiving reflected signals to construct an image of the environment. However, for route planning, traditional nautical chart data has a long update cycle, making it difficult to reflect real-time changes in ocean topography, such as submarine landslides and newly formed reefs. Marine environmental models also lack the ability to predict sudden extreme hydrological conditions, such as turbidity currents triggered by submarine earthquakes, significantly compromising the reliability of planned routes. Regarding yaw calculation, positioning methods that rely on inertial navigation systems accumulate errors over time, while satellite signals cannot be directly used underwater. Acoustic positioning technology is affected by the characteristics of seawater, resulting in signal propagation delays and distortion, making it difficult to achieve high-precision, real-time yaw measurements. For obstacle detection, sonar is limited by its operating frequency and detection angle, resulting in blind spots. Furthermore, in complex hydrological conditions, the signal is susceptible to interference, reducing the accuracy of obstacle identification. For depth monitoring, a single depth sensor failure or calibration error can lead to misjudgment, threatening vehicle safety. Summary of the Invention

[0004] The present invention aims to solve at least one of the technical problems existing in the related art. To this end, the present invention provides a three-dimensional route monitoring method and system that automatically calculates the three-dimensional current flight segment, calculates the three-dimensional deviation distance, and detects obstacles and water depth hazards.

[0005] The present invention provides a three-dimensional route monitoring method, comprising:

[0006] S1: Determine whether the current segment automatically switches to the next segment based on the horizontal and vertical distances between the two waypoints of the segment, and determine the current segment based on whether the current segment automatically switches to the next segment;

[0007] S2: Calculate the horizontal yaw distance of the underwater vehicle based on the position information of the two waypoints of the current segment, calculate the intersection point of the underwater vehicle with the current segment, and calculate the depth yaw distance of the underwater vehicle and the three-dimensional yaw distance of the underwater vehicle based on the intersection point of the underwater vehicle with the current segment and the horizontal yaw distance;

[0008] S3: Determine the detection range and detection frequency based on the heading and three-dimensional speed of the underwater vehicle, and detect whether there is an obstacle hazard using a fan-shaped detection azimuth interval method;

[0009] S4: obtaining the current water depth based on the grid terrain data or the nautical chart data, obtaining the tide data, and calculating the true water depth based on the tide data and the current water depth;

[0010] S5: Calculate the depth of the underwater vehicle from the water bottom according to the current water depth and the true water depth, and detect whether there is a depth hazard according to the depth of the underwater vehicle from the water bottom;

[0011] S6: Complete the three-dimensional route monitoring of the underwater vehicle according to the horizontal deviation distance, depth deviation distance, three-dimensional deviation distance, obstacle danger and depth danger of the underwater vehicle.

[0012] Furthermore, step S1 includes:

[0013] S11; Calculate the horizontal distance and vertical distance between two waypoints according to the latitude and longitude of the two waypoints of the flight segment;

[0014] S12: determining the current flight segment based on whether the current flight segment automatically switches to the next flight segment;

[0015] S121: If the horizontal distance is greater than or equal to the vertical distance, determine whether to automatically switch to the next route segment in the horizontal direction;

[0016] Setting a first minimum distance for all waypoints on the segment except the first waypoint, and calculating the distance between the underwater vehicle and all waypoints;

[0017] If the distance between the underwater vehicle and three consecutive waypoints is less than or equal to the minimum distance, the route segment it is in will automatically switch to the next route segment;

[0018] If the distance between the underwater vehicle and multiple waypoints is greater than the minimum distance, the underwater vehicle is judged to see whether it has exceeded the perpendicular line between the next waypoint and the current heading. If it has exceeded the perpendicular line three times in a row, it will automatically switch to the next route segment.

[0019] S122: If the horizontal distance is less than the vertical distance, determine in the vertical direction whether the current route segment is automatically switched to the next route segment;

[0020] S13: Determine the depth range of the current segment based on the depth value of each waypoint;

[0021] Determine whether the depth of the underwater vehicle is within the depth range of the current segment. If it exceeds the depth range of the current segment, it will automatically switch to the next segment.

[0022] Furthermore, in step S2, calculating the horizontal deviation distance of the underwater vehicle based on two adjacent waypoints of the current segment includes:

[0023] S211: Select the current flight segment Waypoints and Waypoint, calculate the Waypoints and The horizontal distance and horizontal bearing of the waypoint, obtain the first horizontal distance and the first horizontal bearing;

[0024] S212: Calculate the underwater vehicle and The horizontal distance and horizontal bearing of the waypoint are used to obtain the second horizontal distance and the second horizontal bearing;

[0025] S213: Convert the first horizontal orientation into radians, calculate the sine, and multiply the result by the first horizontal distance to obtain the x-coordinate of the first vector. Convert the first horizontal orientation into radians, calculate the cosine, and multiply the result by the first horizontal distance to obtain the y-coordinate of the first vector.

[0026] S214: Convert the second horizontal direction into radians, calculate the sine, and multiply the result by the second horizontal distance to obtain the x-coordinate of the second vector. Convert the second horizontal direction into radians, calculate the cosine, and multiply the result by the second horizontal distance to obtain the y-coordinate of the second vector.

[0027] S215: multiplying the x-coordinate of the second vector by the dot product of the first vector and the second vector and the ratio of the dot product of the second vector to its own dot product to obtain the x-coordinate of the third vector, and multiplying the y-coordinate of the second vector by the dot product of the first vector and the second vector and the ratio of the dot product of the second vector to its own dot product to obtain the y-coordinate of the third vector;

[0028] S216: Subtract the x coordinate of the first vector from the x coordinate of the third vector to obtain the x coordinate of the fourth vector; subtract the y coordinate of the first vector from the y coordinate of the third vector to obtain the y coordinate of the fourth vector;

[0029] S217: Calculate the square root of the sum of the squares of the x-coordinate of the fourth vector and the y-coordinate of the fourth vector to obtain the horizontal yaw distance of the underwater vehicle.

[0030] Furthermore, in step S2, calculating the intersection point between the underwater vehicle and the current segment, and calculating the depth deviation distance of the underwater vehicle based on the intersection point between the underwater vehicle and the current segment and the horizontal deviation distance includes:

[0031] S221: Calculate the horizontal distance and horizontal bearing between the underwater vehicle and the intersection point of the current segment;

[0032] The horizontal bearing of the intersection point is the first horizontal bearing plus or the first horizontal bearing plus ;

[0033] The horizontal distance of the intersection point is the horizontal yaw distance of the underwater vehicle;

[0034] S222: Calculate the longitude and latitude of the intersection point based on the horizontal distance and horizontal orientation of the intersection point;

[0035] The calculation expression of the intersection latitude is:

[0036]

[0037] in, is the latitude of the intersection, Underwater vehicle longitude, is the horizontal yaw distance of the underwater vehicle, is the radius, is the underwater vehicle latitude, is the horizontal orientation of the intersection point, To calculate the inverse tangent function of two variables,

[0038] The calculation expression of the intersection longitude is:

[0039]

[0040] in, is the longitude of the intersection point;

[0041] S223: Calculate the depth difference of the current segment according to the depths of the two waypoints, divide the depth difference of the current segment by the horizontal distance of the current segment, and then multiply it by the horizontal distance from the current segment to the intersection point to obtain the distance from the intersection point to the first waypoint. The distance difference between the waypoints will be The depth of the waypoint is the same as the intersection point to the Subtract the distance difference between the waypoints to obtain the depth of the intersection;

[0042] S224: Subtract the depth of the underwater vehicle from the depth of the intersection point to obtain the depth deviation distance.

[0043] Furthermore, in step S2, calculating the three-dimensional yaw distance of the underwater vehicle includes:

[0044] Converting the underwater vehicle position information and the intersection information of the underwater vehicle and the current route segment into rectangular coordinates, wherein the intersection information of the underwater vehicle and the current route segment includes the longitude of the intersection point, the latitude of the intersection point, and the depth of the intersection point;

[0045] In rectangular coordinates, the three-dimensional distance between the underwater vehicle and the intersection of the current segment and the underwater vehicle is calculated to obtain the three-dimensional yaw distance of the underwater vehicle.

[0046] Furthermore, step S3 includes:

[0047] S31: Determine the detection range and detection frequency according to the heading and three-dimensional speed of the underwater vehicle;

[0048] The detection range is a fan-shaped column, the horizontal direction is fan-shaped, and the angle is the left and right of the current heading. Degrees, sector radius According to the horizontal speed and time of the underwater vehicle Decide;

[0049] The vertical detection distance is , , is the vertical component of the three-dimensional velocity, and the detection direction is the direction of the vertical velocity;

[0050] Set the speed threshold. When the three-dimensional speed is greater than or equal to the speed threshold, the detection frequency is Seconds / times;

[0051] When the three-dimensional speed is less than the speed threshold, the detection frequency is Seconds / times, ;

[0052] S32: Detect whether there is an obstacle hazard by using a sector detection azimuth interval method;

[0053] Calculate the The horizontal distance between the obstacle and the underwater vehicle and horizontal orientation ;

[0054] like , then there is no obstacle hazard;

[0055] like , then compare The horizontal position of the obstacle and the underwater vehicle ;

[0056] Calculate the sector detection interval according to the current heading and obtain the sector starting angle and the fan end angle ;

[0057] like or , then there is no obstacle hazard;

[0058] like , determine whether the obstacle is in the vertical speed direction;

[0059] If it is not in the vertical speed direction, there is no obstacle hazard;

[0060] If it is in the vertical speed direction, the depth of the obstruction is relatively and the current depth of the underwater vehicle ;

[0061] like or , then If an obstacle is within the vertical detection range, there is an obstacle hazard and an alarm is required;

[0062] Otherwise, The obstacle is not within the vertical detection range and there is no obstacle hazard;

[0063] S33: Repeat step S32 to complete the detection of all obstacles.

[0064] Furthermore, in step S4,

[0065] Obtaining chart data includes:

[0066] Set the second minimum distance;

[0067] The longitude difference between the sounding point on the nautical chart and the current position is less than 0.5, and the latitude difference between the sounding point on the nautical chart and the current position is less than 0.5;

[0068] If the distance between the water depth point on the nautical chart and the current position is less than or equal to the second minimum distance, the water depth value of the water depth point on the nautical chart is the water depth of the current position;

[0069] If the distance between the sounding point on the chart and the current position is greater than the second minimum distance, the grid terrain data is used;

[0070] Obtaining grid terrain data includes:

[0071] Set the third minimum distance;

[0072] Select four points on the grid terrain data based on the latitude and longitude of the current location and the resolution of the terrain data;

[0073] Select the point closest to the current position as point P1;

[0074] If the distance between point P1 and the current position is less than or equal to the minimum distance, the water depth at the current position is equal to the water depth at point P1;

[0075] If the distance between point P1 and the current position is greater than the minimum distance, bilinear interpolation is performed to obtain the water depth at the current position.

[0076] Furthermore, obtaining tide data includes:

[0077] Set the fourth minimum distance;

[0078] Select four points on the grid terrain data based on the latitude and longitude of the current location and the resolution of the terrain data;

[0079] Select the point closest to the current position as point Q1;

[0080] If the distance between point Q1 and the current position is less than or equal to the fourth minimum distance, the water depth at the current position is equal to the water depth at point Q1;

[0081] If the distance between point Q1 and the current position is greater than the fourth minimum distance, bilinear interpolation is performed to obtain tidal data.

[0082] Furthermore, in step S5,

[0083] Superimposing the current water depth obtained from the tide data and the grid data or superimposing the current water depth obtained from the tide data and the chart data to obtain the true water depth;

[0084] Subtract the actual water depth from the current depth of the underwater vehicle to obtain the depth from the water bottom;

[0085] If the depth from the bottom of the water is greater than the safe distance, there is no danger;

[0086] If the depth from the bottom of the water is less than or equal to the safe distance, there is danger and an alarm is required.

[0087] The present invention further provides a three-dimensional route monitoring system for executing the three-dimensional route monitoring method described above, comprising:

[0088] a current segment determination module, which determines whether the current segment automatically switches to the next segment based on the horizontal and vertical distances between two waypoints of the segment, and determines the current segment based on whether the current segment automatically switches to the next segment;

[0089] a yaw distance calculation module, which calculates the horizontal yaw distance of the underwater vehicle based on the position information of two waypoints in the current segment, calculates the intersection point of the underwater vehicle with the current segment, and calculates the depth yaw distance of the underwater vehicle and the three-dimensional yaw distance of the underwater vehicle based on the intersection point of the underwater vehicle with the current segment;

[0090] An obstacle detection module, which determines a detection range and frequency based on the underwater vehicle's heading and three-dimensional speed, and detects whether there are obstacle hazards using a sector detection azimuth interval method;

[0091] a real water depth acquisition module, which obtains the current water depth based on grid terrain data or nautical chart data, obtains tidal data, and calculates the real water depth based on the tidal data and the current water depth;

[0092] a depth hazard detection module, which calculates the depth of the underwater vehicle from the water bottom based on the current water depth and the actual water depth, and detects whether there is a depth hazard based on the depth of the underwater vehicle from the water bottom;

[0093] A three-dimensional route monitoring module is provided for completing three-dimensional route monitoring of the underwater vehicle according to the horizontal deviation distance, depth deviation distance, three-dimensional deviation distance, obstacle danger and depth danger of the underwater vehicle.

[0094] The above one or more technical solutions in the embodiments of the present invention have at least one of the following technical effects:

[0095] The present invention can grasp the degree of deviation in three dimensions of longitude, latitude and depth in real time, facilitating timely course correction and reducing the risk of deviation from the destination or hitting the bottom;

[0096] Rapidly locate real dangerous targets in complex underwater environments, reduce false alarms, and improve the vehicle's response to sudden obstacles;

[0097] It realizes the dynamic integration of terrain, water depth and tidal influences during underwater navigation, so that the vehicle can grasp the real water depth of the current position in real time, and automatically alarms according to the safety distance threshold, effectively avoiding collisions with submarine mountains and other dangers, thereby improving the safety of deep-sea navigation.

[0098] 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

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

[0100] Figure 1 It is a flow chart of a three-dimensional route monitoring method provided by the present invention.

[0101] Figure 2 It is a structural schematic diagram of a three-dimensional route monitoring system provided by the present invention.

[0102] Reference numerals:

[0103] 101. Current segment determination module; 102. Off-course distance calculation module; 103. Obstacle detection module; 104. Real water depth acquisition module; 105. Depth hazard detection module; 106. Three-dimensional route monitoring module. DETAILED DESCRIPTION

[0104] To make the purpose, technical solutions and advantages of the present invention clearer, the technical solutions in the present invention will be clearly and completely described below. 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.

[0105] In the description of the embodiments of the present invention, it should be noted that the terms "first", "second" and "third" are used for descriptive purposes only and should not be understood as indicating or implying relative importance. 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, materials 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, materials 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 the different embodiments or examples described in this specification and the features of the different embodiments or examples, unless they are contradictory.

[0106] The following combination Figure 1 and Figure 2 A three-dimensional route monitoring method and system of the present invention are described.

[0107] Underwater vehicles are equipped with navigation and positioning equipment, which provides real-time information on their current position (longitude, latitude, depth), heading, and three-dimensional speed. Before voyage, thematic data on obstacles, gridded terrain, and tides are prepared for the navigation area. Obstacle data and discrete depth data can also be extracted from nautical charts. A three-dimensional planned route consists of a series of sequentially arranged three-dimensional waypoints (longitude, latitude, depth). The line segment between two adjacent waypoints is called a leg. During navigation monitoring, the current leg and next waypoint are determined based on the vehicle's current position. The deviation between the current leg and the current position, as well as the bearing and distance to the next waypoint, are calculated.

[0108] like Figure 1 As shown, a three-dimensional route monitoring method includes:

[0109] S1: Determine whether the current segment automatically switches to the next segment based on the horizontal and vertical distances between the two waypoints of the segment, and determine the current segment based on whether the current segment automatically switches to the next segment;

[0110] S11: Calculate the horizontal distance and vertical distance between two waypoints according to their latitude and longitude;

[0111] Assume that the current flight segment Waypoints , ,in, For the the longitude of the waypoint, For the the latitude of the waypoint, For the Depth of waypoint, Waypoints , , For the the longitude of the waypoint, For the the latitude of the waypoint, For the The calculation expression of the depth and horizontal distance of the waypoint is:

[0112]

[0113] in, is the first horizontal distance, is the radius of the Earth, To find the square root function, To calculate the arc tangent function of two variables;

[0114] S12: determining the current flight segment based on whether the current flight segment automatically switches to the next flight segment;

[0115] S121: If the horizontal distance is greater than or equal to the vertical distance, determine in the horizontal direction whether the current route segment is automatically switched to the next route segment;

[0116] Setting a first minimum distance for all waypoints on the segment except the first waypoint, and calculating the distance between the underwater vehicle and all waypoints;

[0117] If the distance between the underwater vehicle and three consecutive waypoints is less than or equal to the minimum distance, the route segment it is in will automatically switch to the next route segment;

[0118] If the distance between the underwater vehicle and multiple waypoints is greater than the minimum distance, the underwater vehicle is judged to see whether it has exceeded the perpendicular line between the next waypoint and the current heading. If it has exceeded the perpendicular line three times in a row, it will automatically switch to the next route segment.

[0119] S122: If the horizontal distance is less than the vertical distance, whether the current segment automatically switches to the next segment in the vertical direction;

[0120] S13: Determine the depth range of the current segment based on the depth value of each waypoint;

[0121] Determine whether the depth of the underwater vehicle is within the depth range of the current segment. If it exceeds the depth range of the current segment, it will automatically switch to the next segment.

[0122] S2: Calculate the horizontal yaw distance of the underwater vehicle based on the position information of the two waypoints of the current segment, calculate the intersection point of the underwater vehicle with the current segment, and calculate the depth yaw distance of the underwater vehicle and the three-dimensional yaw distance of the underwater vehicle based on the intersection point of the underwater vehicle with the current segment and the horizontal yaw distance;

[0123] Current location of underwater vehicle , , is the current longitude of the underwater vehicle, is the current latitude of the underwater vehicle, is the depth of the underwater vehicle's current position;

[0124] S211: Select the current flight segment Waypoints and Waypoint, calculate the Waypoints and The horizontal distance and horizontal bearing of the waypoint, obtain the first horizontal distance and the first horizontal bearing;

[0125] The calculation expression of the first horizontal distance is:

[0126]

[0127] in, is the first horizontal distance, is the radius of the Earth, To find the square root function;

[0128] The calculation expression of the first horizontal orientation is:

[0129]

[0130] in, is the first horizontal orientation, Calculates the arctangent function of two variables.

[0131] S212: Calculate the underwater vehicle and The horizontal distance and horizontal bearing of the waypoint are used to obtain the second horizontal distance and the second horizontal bearing;

[0132] The calculation expression of the second horizontal orientation is:

[0133]

[0134] in, is the second horizontal orientation;

[0135] The calculation expression of the second horizontal distance is:

[0136]

[0137] in, is the second horizontal distance.

[0138] S213: Convert the first horizontal orientation into radians, calculate the sine, and then multiply it by the first horizontal distance to obtain the x-coordinate of the first vector. Convert the first horizontal orientation into radians, calculate the cosine, and then multiply it by the first horizontal distance to obtain the y-coordinate of the first vector. The calculation expression is:

[0139]

[0140]

[0141] in, is the x-coordinate of the first vector, is the y coordinate of the first vector, is the first vector;

[0142] S214: Convert the second horizontal direction into radians, calculate the sine, and multiply the result by the second horizontal distance to obtain the x-coordinate of the second vector. Convert the second horizontal direction into radians, calculate the cosine, and multiply the result by the second horizontal distance to obtain the y-coordinate of the second vector.

[0143] The calculation expression of the second vector is:

[0144]

[0145]

[0146] in, is the x-coordinate of the second vector, is the y coordinate of the second vector, is the second vector.

[0147] S215: multiplying the x-coordinate of the second vector by the dot product of the first vector and the second vector and the ratio of the dot product of the second vector to its own dot product to obtain the x-coordinate of the third vector, and multiplying the y-coordinate of the second vector by the dot product of the first vector and the second vector and the ratio of the dot product of the second vector to its own dot product to obtain the y-coordinate of the third vector;

[0148] The calculation expression of the third vector is:

[0149]

[0150]

[0151] in, is the x-coordinate of the third vector, is the y coordinate of the third vector, is the third vector.

[0152] S216: Subtract the x coordinate of the first vector from the x coordinate of the third vector to obtain the x coordinate of the fourth vector; subtract the y coordinate of the first vector from the y coordinate of the third vector to obtain the y coordinate of the fourth vector;

[0153] The calculation expression of the fourth vector is:

[0154]

[0155]

[0156] in, is the x-coordinate of the fourth vector, is the y coordinate of the fourth vector, is the fourth vector.

[0157] S217: Calculate the square root of the sum of the squares of the x-coordinate of the fourth vector and the y-coordinate of the fourth vector to obtain the horizontal yaw distance of the underwater vehicle;

[0158] The calculation expression of horizontal yaw distance is:

[0159]

[0160] in, is the horizontal yaw distance.

[0161] Calculating the intersection point of the underwater vehicle and the current segment, and calculating the depth deviation distance of the underwater vehicle according to the intersection point of the underwater vehicle and the current segment and the horizontal deviation distance include:

[0162] S221: Calculate the horizontal distance and horizontal bearing from the underwater vehicle to the intersection of the current segment;

[0163] The intersection point from the underwater vehicle to the current segment is , ,in, is the longitude of the intersection, is the latitude of the intersection, The depth of the intersection,

[0164] The horizontal bearing of the intersection point is the first horizontal bearing plus or the first horizontal bearing plus , the calculation expression is:

[0165] or

[0166] in, is the horizontal bearing of the intersection point;

[0167] The horizontal distance of the intersection point is the horizontal yaw distance of the underwater vehicle, and the calculation expression is:

[0168]

[0169] in, is the horizontal distance of the intersection point;

[0170] S222: Calculate the longitude and latitude of the intersection point based on the horizontal distance and horizontal orientation of the intersection point;

[0171] The calculation expression of the intersection latitude is:

[0172]

[0173] in, is the latitude of the intersection, Underwater vehicle longitude, is the horizontal yaw distance of the underwater vehicle, is the radius, is the underwater vehicle latitude, is the horizontal orientation of the intersection point, To calculate the arc tangent function of two variables;

[0174] The calculation expression of the intersection longitude is:

[0175]

[0176] in, is the longitude of the intersection point.

[0177] S223: Calculate the depth difference of the current segment according to the depths of the two waypoints, divide the depth difference of the current segment by the horizontal distance of the current segment, and then multiply it by the horizontal distance from the current segment to the intersection point to obtain the distance from the intersection point to the first waypoint. The distance difference between the waypoints will be The depth of the waypoint is the same as the intersection point to the Subtract the distance difference between the waypoints to obtain the depth of the intersection;

[0178] Depth difference between the two ends of the current segment for:

[0179]

[0180] Intersection to Distance difference between waypoints The calculation expression is:

[0181]

[0182] in, is the horizontal distance of the current segment, , is the horizontal distance from the current segment to the intersection point;

[0183] The calculation expression of the depth of the intersection is:

[0184]

[0185] in, is the depth of the intersection.

[0186] S224: Subtract the depth of the underwater vehicle from the depth of the intersection point to obtain the depth deviation distance. The calculation expression of the depth deviation distance is:

[0187]

[0188] in, is the depth yaw distance;

[0189] Calculating the three-dimensional yaw distance of an underwater vehicle includes:

[0190] S231: Convert the underwater vehicle position information and the intersection point information into rectangular coordinates;

[0191] S232: The calculation expression of the three-dimensional yaw distance is:

[0192]

[0193] in, is the three-dimensional yaw distance, To find the square root function, is the x-coordinate of the intersection point, is the x-coordinate of the underwater vehicle, is the y coordinate of the intersection point, is the y coordinate of the underwater vehicle, is the z coordinate of the intersection point, is the z coordinate of the underwater vehicle.

[0194] Convert the position information into rectangular coordinates as follows:

[0195] Equatorial radius :

[0196] Polar radius :

[0197] The calculation expression of flattening is:

[0198]

[0199] in, is the flatness rate;

[0200] The calculation expression of eccentricity is:

[0201]

[0202] in, is the eccentricity;

[0203] The calculation expression of the curvature radius of the unitary circle of the reference ellipsoid is:

[0204] in, is the radius of curvature of the reference ellipsoid;

[0205]

[0206]

[0207]

[0208] is the x coordinate in the rectangular coordinate system, is the y coordinate in the rectangular coordinate system, is the z coordinate in the rectangular coordinate system, is the depth information, is the latitude information, Longitude information.

[0209] S3: Determine the detection range and detection frequency based on the heading and three-dimensional speed of the underwater vehicle, and detect whether there is an obstacle hazard using a fan-shaped detection azimuth interval method;

[0210] S31: Determine the detection range and detection frequency according to the heading and three-dimensional speed of the underwater vehicle;

[0211] The detection range is a fan-shaped column, the horizontal direction is fan-shaped, and the angle is the left and right of the current heading. degrees, the fan radius is determined by the horizontal speed of the underwater vehicle and the time Determine the calculation expression:

[0212]

[0213] in, is the sector radius, is the horizontal velocity, , for Directional speed, for Directional speed For time;

[0214] In some specific embodiments of the present invention, , The default is 10 minutes. and The specific details can be adjusted according to actual conditions.

[0215] The vertical detection distance is , , is the vertical component of the three-dimensional velocity, and the detection direction is the direction of the vertical velocity;

[0216] Set the speed threshold. When the three-dimensional speed is greater than or equal to the speed threshold, the detection frequency is Seconds / times;

[0217] When the three-dimensional speed is less than the speed threshold, the detection frequency is Seconds / times, ;

[0218] In some specific embodiments of the present invention, , .

[0219] S32: Detect whether there is an obstacle hazard by using a sector detection azimuth interval method;

[0220] Known obstructions , ,in, For the The longitude of the obstacle, For the The latitude of the obstacle, For the the depth of the obstacle;

[0221] Calculate the The horizontal distance between the obstacle and the underwater vehicle and horizontal orientation ,

[0222] like , then there is no obstacle hazard;

[0223] like , then compare The horizontal position of the obstacle and the underwater vehicle ,

[0224] Calculate the sector detection interval according to the current heading and obtain the sector starting angle and the fan end angle ; The calculation expression of the sector detection interval is:

[0225] ,like Less than , ;

[0226] ,like Greater than , ;

[0227] in, is the heading of the underwater vehicle, is the starting angle of the sector, is the sector ending angle;

[0228] like or , then there is no obstacle hazard;

[0229] like , determine whether the obstacle is in the vertical speed direction,

[0230] If it is not in the vertical speed direction, there is no obstacle hazard;

[0231] If it is in the vertical speed direction, the depth of the obstruction is relatively and the current depth of the underwater vehicle ,

[0232] like or , then If an obstacle is within the vertical detection range, there is an obstacle hazard and an alarm is required;

[0233] Otherwise, The obstacle is not within the vertical detection range and there is no obstacle hazard.

[0234] The vertical speed is positive when it is downward. If the vertical speed is positive, the depth of the obstacle is determined. Is it greater than the current depth of the underwater vehicle? If it is greater than, it means it is below the underwater vehicle and in the vertical speed direction. If it is less than, it means it is above the underwater vehicle and not in the vertical speed direction.

[0235] If the vertical speed is negative, determine the depth of the obstruction Is it less than the current depth of the underwater vehicle? If it is less than , it means it is above the underwater vehicle and in the vertical speed direction. If it is greater than , it means it is below the underwater vehicle and not in the vertical speed direction.

[0236] Repeat step S32 to complete the detection of all obstacles. If the vertical speed direction is toward the horizontal plane and the vertical detection distance exceeds the horizontal plane, only the horizontal plane is detected.

[0237] S4: Obtain the current water depth based on the nautical chart data and the grid terrain data, obtain the tide data, and calculate the true water depth based on the tide data and the current water depth;

[0238] Get the water depth value at the current location. The water depth information comes from the uniform grid terrain data and the discrete water depth point data in the nautical chart. At the same time, it is necessary to consider the tidal data of the specific location and superimpose them to obtain the current true water depth value.

[0239] Since chart data is updated more promptly, chart depth data is used first.

[0240] S41: Acquisition of chart data;

[0241] Set the second minimum distance;

[0242] The longitude difference between the sounding point on the nautical chart and the current position is less than 0.5, and the latitude difference between the sounding point on the nautical chart and the current position is less than 0.5;

[0243] If the distance between the water depth point on the nautical chart and the current position is less than or equal to the second minimum distance, the water depth value of the water depth point on the nautical chart is the water depth of the current position;

[0244] If the distance between the sounding point on the chart and the current position is greater than the second minimum distance, the grid terrain data is used;

[0245] In some specific embodiments of the present invention, there are many soundings points on the nautical chart, and the soundings points far away from the current position are of little reference significance, so the soundings points within 0.5° of latitude and longitude are selected for use.

[0246] Obtaining grid terrain data includes:

[0247] Set the third minimum distance;

[0248] Select four points on the grid terrain data based on the latitude and longitude of the current location and the resolution of the terrain data;

[0249] Select the point closest to the current position as point P1;

[0250] If the distance between point P1 and the current position is less than or equal to the minimum distance, the water depth at the current position is equal to the water depth at point P1;

[0251] If the distance between point P1 and the current position is greater than the minimum distance, bilinear interpolation is performed to obtain the water depth at the current position.

[0252] In some specific embodiments of the present invention, it is assumed that the resolution of the terrain data is 0.25′, the longitude and latitude of the current position are (E110°10.03′, N10°10.03′), and the four points within the resolution range are: P1 (E110°10′, N10°10′), P2 (E110°10.25′, N10°10′), P3 (E110°10′, N10°9.75′), and P4 (E110°10.25′, N10°9.75′). The distances between the four points and the current position are calculated, and the water depth data point closest to the current position should be point P1.

[0253] Set the minimum distance. If the distance between point P1 and the current position is less than the minimum distance, the water depth at the current position is considered equal to the water depth at point P1. If the distance between point P1 and the current position is greater than the minimum distance, interpolation is required.

[0254] The interpolation process is as follows:

[0255] Based on the four points P1, P2, P3, and P4, bilinear interpolation is performed to convert P1, P2, P3, and P4 into rectangular coordinates, which are 、 、 、 , the corresponding water depth is 、 、 、 , the current point location information is , , the corresponding water depth is , the calculation expression is:

[0256] ;

[0257] S43: Acquiring tide data includes:

[0258] Set the fourth minimum distance,

[0259] According to the latitude and longitude of the current location and the resolution of the terrain data, select four points on the grid terrain data.

[0260] Select the point closest to the current position as point Q1;

[0261] If the distance between point Q1 and the current position is less than or equal to the fourth minimum distance, the water depth at the current position is equal to the water depth at point Q1;

[0262] If the distance between point Q1 and the current position is greater than the fourth minimum distance, bilinear interpolation is performed to obtain tide data;

[0263] In some specific embodiments of the present invention, since the tidal information of points far away from the current position is of little reference significance, only the position points within the resolution range of the current position are searched, the distances between the four points and the current position are calculated, and the point closest to the current position is selected.

[0264] S44: superimposing the tide value and the grid topographic water depth value or superimposing the tide value and the chart water depth value to obtain the true water depth.

[0265] The current location's chart depth value, grid terrain depth value, and tide value must be unified to a unified depth reference. After conversion to the unified depth reference, the depth value and tide value are superimposed to obtain the actual water depth at that time.

[0266] S5: Calculate the depth of the underwater vehicle from the water bottom according to the current water depth and the true water depth, and detect whether there is a depth hazard according to the depth of the underwater vehicle from the water bottom;

[0267] S51: Subtract the actual water depth from the current depth of the underwater vehicle to obtain the depth from the water bottom. The calculation expression is:

[0268]

[0269] is the depth from the bottom of the water, is the true water depth, is the current depth of the underwater vehicle;

[0270] S52: If the depth to the bottom of the water is greater than the safe distance , then there is no danger;

[0271] If the depth from the bottom of the water is less than or equal to the safe distance, there is danger and an alarm is required.

[0272] When sailing underwater, there should be enough safety distance between the current position and the depth of the water bottom to avoid dangers such as collision with submarine mountains.

[0273] In some specific embodiments of the present invention, , which can be adjusted according to needs.

[0274] S6: Complete the three-dimensional route monitoring of the underwater vehicle based on the three-dimensional deviation distance, obstacle danger and depth danger of the underwater vehicle.

[0275] The three-dimensional yaw distance is used to ensure that the underwater vehicle does not deviate from the navigation plan, the obstacle hazard is used to ensure that the underwater vehicle does not collide with obstacles, and the depth hazard is used to effectively avoid collisions with seabed mountains and other hazards, thereby completing the three-dimensional route monitoring of the underwater vehicle from multiple angles.

[0276] like Figure 2 As shown, a three-dimensional route monitoring system is used to perform a three-dimensional route monitoring method, including:

[0277] The current segment determination module 101 determines whether the current segment automatically switches to the next segment based on the horizontal distance and vertical distance between two waypoints of the segment, and determines the current segment based on whether the current segment automatically switches to the next segment;

[0278] The yaw distance calculation module 102 calculates the horizontal yaw distance of the underwater vehicle based on the position information of the two waypoints of the current segment, calculates the intersection point of the underwater vehicle with the current segment, and calculates the depth yaw distance of the underwater vehicle and the three-dimensional yaw distance of the underwater vehicle based on the intersection point of the underwater vehicle with the current segment;

[0279] The obstacle detection module 103 determines the detection range and detection frequency according to the heading and three-dimensional speed of the underwater vehicle, and detects whether there is an obstacle hazard through a fan-shaped detection azimuth interval method;

[0280] The real water depth acquisition module 104 obtains the current water depth based on the grid terrain data or the nautical chart data, obtains the tidal data, and calculates the real water depth based on the tidal data and the current water depth;

[0281] The depth danger detection module 105 calculates the depth of the underwater vehicle from the water bottom according to the current water depth and the actual water depth, and detects whether there is a depth danger according to the depth of the underwater vehicle from the water bottom;

[0282] The three-dimensional route monitoring module 106 completes the three-dimensional route monitoring of the underwater vehicle according to the horizontal deviation distance, depth deviation distance, three-dimensional deviation distance, obstacle danger and depth danger of the underwater vehicle.

[0283] Through the coordinated operation of the above modules, the system can monitor the degree of deviation in longitude, latitude, and depth in real time, facilitating timely course corrections and reducing the risk of straying from the destination or hitting the bottom. It can also quickly locate real dangerous targets in complex underwater environments, reducing false alarms and improving the vehicle's ability to respond to sudden obstacles. It also achieves dynamic integration of the effects of terrain, water depth, and tides during underwater navigation, enabling the vehicle to grasp the actual water depth at its current location in real time. Combined with the safety distance threshold, it automatically issues alarms, effectively avoiding collisions with submarine mountains and other hazards, and improving the safety of deep-sea navigation.

[0284] 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 three-dimensional route monitoring method, characterized in that: include: S1: Determine whether the current segment automatically switches to the next segment based on the horizontal and vertical distances between the two waypoints of the segment, and determine the current segment based on whether the current segment automatically switches to the next segment; S2: Calculate the horizontal yaw distance of the underwater vehicle based on the position information of the two waypoints of the current segment, calculate the intersection point of the underwater vehicle with the current segment, and calculate the depth yaw distance of the underwater vehicle and the three-dimensional yaw distance of the underwater vehicle based on the intersection point of the underwater vehicle with the current segment and the horizontal yaw distance; S3: Determine the detection range and detection frequency based on the heading and three-dimensional speed of the underwater vehicle, and detect whether there is an obstacle hazard using a fan-shaped detection azimuth interval method; S4: obtaining the current water depth based on the grid terrain data or the nautical chart data, obtaining the tide data, and calculating the true water depth based on the tide data and the current water depth; S5: Calculate the depth of the underwater vehicle from the water bottom according to the current water depth and the true water depth, and detect whether there is a depth hazard according to the depth of the underwater vehicle from the water bottom; S6: Complete the three-dimensional route monitoring of the underwater vehicle according to the horizontal deviation distance, depth deviation distance, three-dimensional deviation distance, obstacle danger and depth danger of the underwater vehicle.

2. A three-dimensional route monitoring method according to claim 1, characterized in that: Step S1 includes: S11; Calculate the horizontal distance and vertical distance between two waypoints according to the latitude and longitude of the two waypoints of the flight segment; S12: determining the current flight segment based on whether the current flight segment automatically switches to the next flight segment; S121: If the horizontal distance is greater than or equal to the vertical distance, determine whether to automatically switch to the next route segment in the horizontal direction; Setting a first minimum distance for all waypoints on the segment except the first waypoint, and calculating the distance between the underwater vehicle and all waypoints; If the distance between the underwater vehicle and three consecutive waypoints is less than or equal to the minimum distance, the route segment it is in will automatically switch to the next route segment; If the distance between the underwater vehicle and multiple waypoints is greater than the minimum distance, the underwater vehicle is judged to see whether it has exceeded the perpendicular line between the next waypoint and the current heading. If it has exceeded the perpendicular line three times in a row, it will automatically switch to the next route segment. S122: If the horizontal distance is less than the vertical distance, determine in the vertical direction whether the current route segment is automatically switched to the next route segment; S13: Determine the depth range of the current segment based on the depth value of each waypoint; Determine whether the depth of the underwater vehicle is within the depth range of the current segment. If it exceeds the depth range of the current segment, it will automatically switch to the next segment.

3. A three-dimensional route monitoring method according to claim 1, characterized in that: In step S2, the calculation of the horizontal deviation distance of the underwater vehicle based on two adjacent waypoints of the current segment includes: S211: Select the current flight segment Waypoints and Waypoint, calculate the Waypoints and The horizontal distance and horizontal bearing of the waypoint, obtain the first horizontal distance and the first horizontal bearing; S212: Calculate the underwater vehicle and the The horizontal distance and horizontal bearing of the waypoint are used to obtain the second horizontal distance and the second horizontal bearing; S213: Convert the first horizontal orientation into radians, calculate the sine, and multiply the result by the first horizontal distance to obtain the x-coordinate of the first vector. Convert the first horizontal orientation into radians, calculate the cosine, and multiply the result by the first horizontal distance to obtain the y-coordinate of the first vector. S214: Convert the second horizontal direction into radians, calculate the sine, and multiply the result by the second horizontal distance to obtain the x-coordinate of the second vector. Convert the second horizontal direction into radians, calculate the cosine, and multiply the result by the second horizontal distance to obtain the y-coordinate of the second vector. S215: multiplying the x-coordinate of the second vector by the dot product of the first vector and the second vector and the ratio of the dot product of the second vector to its own dot product to obtain the x-coordinate of the third vector, and multiplying the y-coordinate of the second vector by the dot product of the first vector and the second vector and the ratio of the dot product of the second vector to its own dot product to obtain the y-coordinate of the third vector; S216: Subtract the x coordinate of the first vector from the x coordinate of the third vector to obtain the x coordinate of the fourth vector; subtract the y coordinate of the first vector from the y coordinate of the third vector to obtain the y coordinate of the fourth vector; S217: Calculate the square root of the sum of the squares of the x-coordinate of the fourth vector and the y-coordinate of the fourth vector to obtain the horizontal yaw distance of the underwater vehicle.

4. A three-dimensional route monitoring method according to claim 1, characterized in that: In step S2, the intersection point between the underwater vehicle and the current segment is calculated, and the depth deviation distance of the underwater vehicle is calculated based on the intersection point between the underwater vehicle and the current segment and the horizontal deviation distance, including: S221: Calculate the horizontal distance and horizontal bearing between the underwater vehicle and the intersection point of the current segment; The horizontal bearing of the intersection point is the first horizontal bearing plus or the first horizontal bearing plus ; The horizontal distance of the intersection point is the horizontal yaw distance of the underwater vehicle; S222: Calculate the longitude and latitude of the intersection point based on the horizontal distance and horizontal orientation of the intersection point; The calculation expression of the intersection latitude is: in, is the latitude of the intersection, Underwater vehicle longitude, is the horizontal yaw distance of the underwater vehicle, is the radius, is the underwater vehicle latitude, is the horizontal orientation of the intersection point, To calculate the inverse tangent function of two variables, The calculation expression of the intersection longitude is: in, is the longitude of the intersection point; S223: Calculate the depth difference of the current segment according to the depths of the two waypoints, divide the depth difference of the current segment by the horizontal distance of the current segment, and then multiply it by the horizontal distance from the current segment to the intersection point to obtain the distance from the intersection point to the first waypoint. The distance difference between the waypoints will be The depth of the waypoint is the same as the intersection point to the Subtract the distance difference between the waypoints to obtain the depth of the intersection; S224: Subtract the depth of the underwater vehicle from the depth of the intersection point to obtain the depth deviation distance.

5. A three-dimensional route monitoring method according to claim 4, characterized in that: In step S2, the calculation of the three-dimensional yaw distance of the underwater vehicle includes: Converting the underwater vehicle position information and the intersection information of the underwater vehicle and the current route segment into rectangular coordinates, wherein the intersection information of the underwater vehicle and the current route segment includes the longitude of the intersection point, the latitude of the intersection point, and the depth of the intersection point; In rectangular coordinates, the three-dimensional distance between the underwater vehicle and the intersection of the current segment and the underwater vehicle is calculated to obtain the three-dimensional yaw distance of the underwater vehicle.

6. A three-dimensional route monitoring method according to claim 1, characterized in that: The S3 steps include: S31: Determine the detection range and detection frequency according to the heading and three-dimensional speed of the underwater vehicle; The detection range is a fan-shaped column, the horizontal direction is fan-shaped, and the angle is the left and right of the current heading. Degrees, sector radius According to the horizontal speed and time of the underwater vehicle Decide; The vertical detection distance is , , It is the vertical component of the three-dimensional velocity, and the detection direction is the direction of the vertical velocity; Set the speed threshold. When the three-dimensional speed is greater than or equal to the speed threshold, the detection frequency is Seconds / times; When the three-dimensional speed is less than the speed threshold, the detection frequency is Seconds / times, ; S32: Detect whether there is an obstacle hazard by using a sector detection azimuth interval method; Calculate the The horizontal distance between the obstacle and the underwater vehicle and horizontal orientation ; like , then there is no obstacle hazard; like , then compare The horizontal position of the obstacle and the underwater vehicle ; Calculate the sector detection interval according to the current heading and obtain the sector starting angle and the fan end angle ; like or , then there is no obstacle hazard; like , determine whether the obstacle is in the vertical speed direction; If it is not in the vertical speed direction, there is no obstacle hazard; If it is in the vertical speed direction, the depth of the obstruction is relatively and the current depth of the underwater vehicle ; like or , then If an obstacle is within the vertical detection range, there is an obstacle hazard and an alarm is required; Otherwise, The obstacle is not within the vertical detection range and there is no obstacle hazard; S33: Repeat step S32 to complete the detection of all obstacles.

7. A three-dimensional route monitoring method according to claim 1, characterized in that: In step S4, Obtaining chart data includes: Set the second minimum distance; The longitude difference between the sounding point on the nautical chart and the current position is less than 0.5, and the latitude difference between the sounding point on the nautical chart and the current position is less than 0.5; If the distance between the water depth point on the nautical chart and the current position is less than or equal to the second minimum distance, the water depth value of the water depth point on the nautical chart is the water depth of the current position; If the distance between the sounding point on the chart and the current position is greater than the second minimum distance, the grid terrain data is used; Obtaining grid terrain data includes: Set the third minimum distance; Select four points on the grid terrain data based on the latitude and longitude of the current location and the resolution of the terrain data; Select the point closest to the current position as point P1; If the distance between point P1 and the current position is less than or equal to the minimum distance, the water depth at the current position is equal to the water depth at point P1; If the distance between point P1 and the current position is greater than the minimum distance, bilinear interpolation is performed to obtain the water depth at the current position.

8. A three-dimensional route monitoring method according to claim 7, characterized in that: Obtaining tide data includes: Set the fourth minimum distance; Select four points on the grid terrain data based on the latitude and longitude of the current location and the resolution of the terrain data; Select the point closest to the current position as point Q1; If the distance between point Q1 and the current position is less than or equal to the fourth minimum distance, the water depth at the current position is equal to the water depth at point Q1; If the distance between point Q1 and the current position is greater than the fourth minimum distance, bilinear interpolation is performed to obtain tidal data.

9. A three-dimensional route monitoring method according to claim 8, characterized in that: In step S5, Superimposing the current water depth obtained from the tide data and the grid data or superimposing the current water depth obtained from the tide data and the chart data to obtain the true water depth; Subtract the actual water depth from the current depth of the underwater vehicle to obtain the depth from the bottom of the water. If the depth from the bottom of the water is greater than the safe distance, there is no danger; If the depth from the bottom of the water is less than or equal to the safe distance, there is danger and an alarm is required.

10. A three-dimensional route monitoring system, characterized in that: A method for performing a three-dimensional route monitoring method according to any one of claims 1 to 9, comprising: a current segment determination module, which determines whether the current segment automatically switches to the next segment based on the horizontal and vertical distances between two waypoints of the segment, and determines the current segment based on whether the current segment automatically switches to the next segment; a yaw distance calculation module, which calculates the horizontal yaw distance of the underwater vehicle based on the position information of two waypoints in the current segment, calculates the intersection point of the underwater vehicle with the current segment, and calculates the depth yaw distance of the underwater vehicle and the three-dimensional yaw distance of the underwater vehicle based on the intersection point of the underwater vehicle with the current segment; An obstacle detection module, which determines a detection range and frequency based on the underwater vehicle's heading and three-dimensional speed, and detects whether there are obstacle hazards using a sector detection azimuth interval method; a real water depth acquisition module, which obtains the current water depth based on grid terrain data or nautical chart data, obtains tidal data, and calculates the real water depth based on the tidal data and the current water depth; a depth hazard detection module, which calculates the depth of the underwater vehicle from the water bottom based on the current water depth and the actual water depth, and detects whether there is a depth hazard based on the depth of the underwater vehicle from the water bottom; A three-dimensional route monitoring module is provided for completing three-dimensional route monitoring of the underwater vehicle according to the horizontal deviation distance, depth deviation distance, three-dimensional deviation distance, obstacle danger and depth danger of the underwater vehicle.

Images