A uuv underwater collision avoidance method applying a frustum method, storage medium and product
By constructing a visual cone model of an obstacle using the visual cone method and performing dynamic compensation, the problem of low intelligent level of underwater collision avoidance for UUVs was solved, and the autonomous collision avoidance capability of UUVs in three-dimensional space was improved.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- JIUJIANG BRANCH OF THE 707 RESEARCH INSTITUTE OF CHINA STATE SHIPBUILDING CORP LTD
- Filing Date
- 2023-06-13
- Publication Date
- 2026-07-10
AI Technical Summary
Existing technologies are insufficient to effectively improve the collision avoidance intelligence level of unmanned underwater vehicles (UUVs) during underwater navigation, especially their autonomous obstacle avoidance capabilities in dynamic and static obstacle environments.
The visual cone method is used to construct visual cone models of obstacles. By calculating the danger level of obstacles, obstacles that need to be avoided are selected, collision avoidance ray sets are generated, and relative motion compensation is performed on dynamic obstacles. The visual cone models of multiple obstacles are merged, and the optimal heading is selected to achieve collision avoidance.
This method enhances the autonomous collision avoidance capability of UUVs in underwater environments, improves the level of intelligence in collision avoidance, and is simple and has good real-time performance, making it valuable for practical applications.
Smart Images
Figure CN116679730B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of unmanned underwater vehicle control technology, and more specifically to a UUV underwater collision avoidance method, storage medium, and product using the cone-of-view method. Background Technology
[0002] When an unmanned underwater vehicle (UUV) encounters dynamic or static obstacles while navigating underwater, it needs to autonomously avoid the obstacles and complete its designated tasks.
[0003] The underwater space collision avoidance method for unmanned underwater vehicles (UUVs) requires a safety encounter assessment of unexpected obstacles and moving targets detected in real time by sensors, and the adoption of emergency collision avoidance maneuvers to form and improve autonomous navigation capabilities.
[0004] Therefore, how to improve the level of underwater collision avoidance intelligence of unmanned underwater vehicles is a problem that urgently needs to be solved by those skilled in the art. Summary of the Invention
[0005] In view of this, the present invention provides a UUV underwater collision avoidance method, storage medium and product using the cone method, which relates to an obstacle hazard judgment and automatic collision avoidance method for UUV underwater navigation, generating collision avoidance paths for avoiding dynamic and static obstacles in the underwater three-dimensional collision avoidance environment, thereby improving the intelligence level of UUV in the underwater environment.
[0006] To achieve the above objectives, the present invention adopts the following technical solution:
[0007] A UUV underwater collision avoidance method using the sight cone method includes the following steps:
[0008] Step 1: Collect the spatial position and velocity of obstacles, calculate the degree of danger of obstacles, and screen out collision avoidance obstacles that meet the set collision hazard conditions;
[0009] The collision avoidance risk is assessed based on the obstacle's spatial position and velocity, as well as the position and velocity of the UVV, to determine the timing of the avoidance maneuver. If multiple obstacles exist, the risk level of each obstacle is calculated based on its spatial position and velocity, and obstacles that meet the set collision risk conditions are selected.
[0010] Step 2: Based on the spatial position of the collision avoidance obstacles, construct the view cone model of each obstacle using the view cone method, and generate the collision avoidance ray set;
[0011] Treat each collision avoidance obstacle as a sphere. With the position of the UUV as the vertex of the cone, draw tangents to the sphere's surface to form a conical surface. The resulting cone is the view cone model, and the apex angle of the formed cone... Represented as:
[0012]
[0013] in, To avoid collisions with obstacles, the radius is expanded; The distance between the UUV and the obstacle to be avoided is calculated based on the position of the UUV and the spatial position of the obstacle to be avoided.
[0014] Enlarge the apex angle of the cone to avoid the cone's apex angle. Represented as:
[0015]
[0016] in, The set angular margin is a fixed constant; avoidance cone apex angle This is a set of rays, serving as a collision avoidance ray set;
[0017] Double the size of the apex to obtain a set of rays, and generate a collision avoidance ray set.
[0018] Step 3: Filter out dynamic obstacles based on their speed. If the obstacle speed is 0, it is a static obstacle; otherwise, it is a dynamic obstacle. Then, perform relative motion compensation on the view cone model based on the speed of the dynamic obstacles to obtain the corrected collision avoidance ray set.
[0019] Rotate the view cone model along the direction of the dynamic obstacle's movement to compensate for the angle. Then any vector in the view cone model All rotate to compensate for the angle along the direction of movement of the dynamic obstacle. , , , Let represent the three-dimensional coordinates of the vector, then the rotated vector is represented as:
[0020] ;
[0021] in, t represents the projection of the dynamic obstacle's velocity in three-dimensional coordinates, and t is the compensation parameter to be solved.
[0022] In a spatial coordinate system, by and Solving the equation for the spatial angle between them yields:
[0023]
[0024] in The value is obtained by solving for the relative velocity, let , , Square the above equation again to transform it into:
[0025]
[0026] To solve for the parameter t, we obtain a quadratic equation in one variable:
[0027]
[0028] Then, by taking the true value from the two t values based on the velocity direction of the dynamic obstacle, the rotationally compensated vector is obtained. .
[0029] Step 4: Merge the collision avoidance ray sets or modified collision avoidance ray sets corresponding to all collision avoidance obstacles to obtain the flight path set;
[0030] Take the union of the modified collision avoidance ray sets corresponding to all dynamic obstacles and the collision avoidance ray sets of other collision avoidance obstacles, and remove duplicate rays to establish the route set U;
[0031] There are overlapping, intersecting, or mutually exclusive relationships between the modified collision avoidance ray sets corresponding to several obstacles; for two overlapping modified collision avoidance ray sets, the union is taken, and only the outermost ray of the view cone model is considered; for two mutually exclusive modified collision avoidance ray sets, the union is taken, and the optimal ray is selected using a cost function; for two intersecting modified collision avoidance ray sets, the union is taken, and the intersecting rays are removed.
[0032] Represented as:
[0033]
[0034] The route set U is represented as:
[0035]
[0036] in, It represents the spatial angle between the i-th ray on the conical surface of the j-th view cone model and the center line, that is, the spatial angle between the i-th ray on the conical surface of the view cone model and the line connecting the j-th obstacle-UUV (the center line of the i-th view cone model); Let be the vertex angle of the j-th view frustum, and n be the total number of view frustums. Represents a vector in the view cone model;
[0037] Step 5: Select the commanded course from the course set using a cost function;
[0038] Constructing the cost function The cost of each ray in the path set is calculated and expressed as:
[0039]
[0040]
[0041] in, and These represent the yaw angle and pitch angle of the UUV, respectively. Indicates the concentrated ray of the flight path Yaw angle; Indicates the concentrated ray of the flight path The pitch angle; Concentrate rays for flight paths The cost observation matrix; Indicates the yaw angle and the concentrated ray of the UUV. The difference in yaw angle; Indicates the pitch angle and trajectory convergence ray of the UUV. The difference in pitch angles;
[0042] Minimizing the cost function to obtain the command heading is expressed as:
[0043]
[0044] in, The ray that minimizes the cost function when the flight path is concentrated; for Yaw angle; for The pitch angle.
[0045] As can be seen from the above technical solution, compared with the prior art, this invention discloses a UUV underwater collision avoidance method, storage medium, and product using the cone-of-view method. It relates to a three-dimensional space collision avoidance method for underwater unmanned vehicles (UVs). Based on the position and velocity factors of obstacles in three-dimensional space, a cone-of-view collision avoidance ray set is established. Dynamic compensation is performed on dynamic targets to correct the ray set. Cone-of-view fusion is then performed on multiple targets. Finally, the most easily implemented ray is selected from the fused cone-of-view ray set as the collision avoidance command course for the UUV, improving the automation level of the underwater unmanned vehicle. This invention enhances the autonomous collision avoidance capability of underwater unmanned vehicles. The method is simple, easy to implement, and has good real-time performance, possessing significant practical application value. It provides a highly practical collision avoidance method for autonomous navigation of underwater unmanned vehicles in three-dimensional space. Attached Figure Description
[0046] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.
[0047] Figure 1The attached figure is a main flowchart of the UUV underwater collision avoidance method using the sight cone method provided by the present invention;
[0048] Figure 2 The attached figure is a schematic diagram illustrating the principle of the three-dimensional frustum method provided by this invention;
[0049] Figure 3 The attached figure is a schematic diagram of the relative motion compensation angle provided by the present invention;
[0050] Figure 4 The attached figure is a schematic diagram of the relative motion compensation of the visual cone provided by the present invention;
[0051] Figure 5 The attached figure is a schematic diagram of the collision avoidance simulation results provided by the present invention. Detailed Implementation
[0052] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.
[0053] This invention discloses a three-dimensional collision avoidance method for underwater unmanned submersibles, such as... Figure 1 As shown, the process includes: 1. Assessing the collision avoidance risk level based on the obstacle's spatial position and speed, and the ship's position and speed, and determining the timing for collision avoidance; 2. Establishing a cone model based on the obstacle's position and scale, and generating a collision avoidance ray set; 3. Performing cone compensation on dynamic targets based on whether the obstacle is a dynamic target, and correcting the ray set; 4. Performing cone fusion on multiple targets, eliminating rays that do not meet the requirements, and obtaining a fused ray set; 5. Selecting the most easily implemented ray from the fused cone ray set as the UUV's collision avoidance command heading.
[0054] The embodiments of the present invention will be further described in detail below with reference to the accompanying drawings:
[0055] 1. Threat Target Screening
[0056] Multiple detected obstacles are screened based on their collision hazard, filtering out low-threat targets. During UUV navigation, the hazard level is calculated in real-time based on the obstacle's position and velocity information. Once the set collision hazard conditions are met, the collision avoidance algorithm is activated. Several factors influencing collision avoidance hazard are considered: closest horizontal encounter distance (DCPA), closest depth encounter distance (HCPA), and shortest encounter time (TCPA). Based on the motion characteristics in three-dimensional space, the collision hazard conditions are set as follows:
[0057]
[0058] Where d, h, and t are the set distance, depth, and encounter time thresholds. When an obstacle exceeds the set threshold, it is added to the threat list.
[0059] 2. Set up the view cone model according to the location of the obstacle.
[0060] The core of the cone method is based on maintaining a constant obstacle avoidance angle. In its specific implementation, the obstacle is considered as a sphere based on its spatial position and size. A tangent is drawn to the sphere's surface with the UUV's position as the center, forming a cone. The apex angle of the cone... Size:
[0061]
[0062] in The radius of the obstacle's expansion. The distance between the UUV and the obstacle is set, and the apex angle of the avoidance cone is also set. Set to:
[0063]
[0064] in The angular margin is set as a fixed constant. The apex angle is then doubled, resulting in a set of rays. To safely avoid obstacles, the UUV should follow one of these rays as its command heading. A diagram of the view cone is shown below. Figure 2 As shown. All tangents contained in the view cone model constitute the ray set, i.e., the collision avoidance ray set.
[0065] 3. Perform dynamic compensation for the view frustum of dynamic targets.
[0066] For collision avoidance of dynamic targets, the relative motion compensation of the visual cone is performed by combining the velocity of the UUV and the velocity of the obstacle, and a compensation angle is introduced. , The size should reflect the speed of the obstacle in the direction perpendicular to the current speed of the UUV. The calculation method is as follows Figure 3 As shown.
[0067] The specific calculation formula is as follows:
[0068]
[0069] in, Represents the velocity vector of a UUV. Represents vector acceleration. Indicates the speed of UUV, The magnitude of the velocity of the vector superposition.
[0070] Rotate the cone of vision along the direction of the obstacle's movement. The specific calculation of the view frustum coordinates in space is as follows:
[0071] For any vector in the view cone That is, the tangent from the apex of the enlarged view cone model to the sphere, which is then rotated and compensated along the direction of obstacle movement. Later obtained Then we have:
[0072]
[0073] make , , Square the above expression again to transform it into...
[0074]
[0075] To solve for the parameter t, we obtain a quadratic equation in one variable:
[0076]
[0077] Then, by taking the true value from the two t values based on the velocity direction, the rotationally compensated view cone vector is obtained. ,like Figure 4 As shown in the figure, the view frustum pointed to by the arrow is the compensated view frustum.
[0078] 4. Multi-target cone fusion
[0079] When the hazard levels of multiple targets simultaneously reach a threshold, the frustum method generates multiple frustums, which may overlap, intersect, or be mutually exclusive. A flight path set is then established. ,from The optimal route is selected to guide the UUV forward. For overlapping view cones, Take the union of the two view cones, considering only the outermost rays of the view cones. For mutually exclusive view cones, Take the union of the view cones, and select the optimal route from the union based on the cost function. For intersecting view cones, the intersection of the two view cones needs to be removed, i.e.:
[0080]
[0081] In the specific implementation of the algorithm, variables are introduced. , representing the spatial angle between the i-th ray and the line connecting the j-th obstacle-UUV, will Represented as:
[0082]
[0083] in Let be the vertex angle of the j-th view cone, and n be the total number of view cones.
[0084] 5. Selection of the optimal heading within the ray set:
[0085] Introducing variables And construct the cost function The cost of selecting the commanded course from the ray set:
[0086]
[0087]
[0088] and This indicates the yaw and pitch angles of the UUV.
[0089] The ray corresponding to the minimum cost function value is the command heading direction, i.e.:
[0090]
[0091] Collision avoidance results as follows Figure 5 As shown, Figure 5 (a, b, c, d) represent the same flight path planned within a space with identical obstacle distribution. The four figures demonstrate this from four different viewing angles. The figures show that the cone method can effectively select the timing for obstacle avoidance and assess the difficulty of achieving horizontal or vertical obstacle avoidance, allowing for obstacle avoidance in the appropriate direction.
[0092] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the apparatus disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the description is relatively simple; relevant parts can be referred to the method section.
[0093] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.
Claims
1. A UUV underwater collision avoidance method using the cone-of-view method, characterized in that, Includes the following steps: Step 1: Collect the spatial position and velocity of obstacles, calculate the degree of danger of obstacles, and screen out collision avoidance obstacles that meet the set collision hazard conditions; Step 2: Based on the spatial position of the collision avoidance obstacles, construct the view cone model of each obstacle using the view cone method, and generate the collision avoidance ray set; Step 3: Select dynamic obstacles based on their speeds and perform relative motion compensation on the view cone model based on their speeds to obtain the corrected collision avoidance ray set; Step 4: Merge the collision avoidance ray sets or modified collision avoidance ray sets corresponding to all collision avoidance obstacles to obtain the flight path set; Step 5: Select the commanded course from the course set using a cost function; The specific implementation process of step 2 is as follows: Each collision avoidance obstacle is defined as a sphere. Tangents are drawn to the sphere's surface using the UUV's position as the apex of the cone, forming a conical surface. This cone is the view cone model, and the apex angle of the resulting cone is... Represented as: in, To avoid collisions with obstacles, the radius is expanded; This refers to the distance between the UUV and the obstacle to be avoided. Enlarging the apex angle of the cone allows for avoidance of the cone's apex angle. Represented as: in, For setting angular margin; avoid cone apex angle This is a set of rays, serving as a collision avoidance ray set; The specific implementation process of step 3 is as follows: Rotate the view cone model along the direction of the dynamic obstacle's movement to compensate for the angle. Then any vector in the view cone model All rotate to compensate for the angle along the direction of movement of the dynamic obstacle. , , , Let represent the three-dimensional coordinates of the vector, then the rotated vector is represented as: ； in, t represents the projection of the dynamic obstacle's velocity in three-dimensional coordinates, and t is the compensation parameter to be solved. In a spatial coordinate system, by and The vector obtained by rotating the spatial angle between them is expressed as: in, This indicates the relative velocity between the UUV and the dynamic obstacle; Represents the velocity vector of a UUV. Indicates vector acceleration. Indicates the speed of UUV, Let be the magnitude of the velocity of the vector superposition; let , , Square the above equation again to transform it into: To solve for the parameter t, we obtain a quadratic equation in one variable: The true value is obtained by taking the velocity direction of the dynamic obstacle from two t values, resulting in the rotationally compensated vector. , thus obtaining the corrected collision avoidance ray set.
2. The UUV underwater collision avoidance method using the sight cone method according to claim 1, characterized in that, The specific implementation process of step 4 is as follows: Take the union of the modified collision avoidance ray sets corresponding to all dynamic obstacles and the collision avoidance ray sets of other obstacles, and remove duplicate rays to establish the path set U, represented as: in, This represents the set of corrected collision avoidance rays corresponding to all dynamic obstacles. This represents the collision avoidance ray set for other collision avoidance obstacles. This represents the spatial angle between the i-th ray on the conical surface of the j-th view cone model and the centerline; Let be the vertex angle of the j-th view frustum; n is the total number of view frustums. This represents a vector in the view frustum model.
3. The UUV underwater collision avoidance method using the sight cone method according to claim 1, characterized in that, The specific implementation process of step 5: Step 51: Construct the cost function The cost of each ray in the path set is calculated and expressed as: in, and These represent the yaw angle and pitch angle of the UUV, respectively. Indicates the concentrated ray of the flight path Yaw angle; Indicates the concentrated ray of the flight path The pitch angle; Concentrate rays for flight paths The cost observation matrix; Indicates the yaw angle and the concentrated ray of the UUV. The difference in yaw angle; Indicates the pitch angle and trajectory convergence ray of the UUV. The difference in pitch angles; Step 52: Minimize the cost function to obtain the command heading, expressed as: in, The ray that minimizes the cost function when the flight path is concentrated; for Yaw angle; for The pitch angle.
4. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the steps of the method described in any one of claims 1-3.
5. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by a processor, it implements the steps of the method described in any one of claims 1-3.