UUV path planning method in underwater complex dynamic environment

Abstract

The invention provides a UUV path planning method in an underwater complex dynamic environment, and the method comprises the steps: detecting the moving parameters of a plurality of moving obstacles which are observed at present, and calculating frozen regions corresponding to the plurality of moving obstacles according to the moving parameters; setting a path planning framework of the UUV by defining a path node and a node speed, and setting a planning constraint condition of a frozen region on a planned path; based on a path planning framework and planning constraint conditions, calculating a plurality of safe paths by avoiding static obstacles and randomly and uniformly sampling nodes entering a frozen area again; and for each security path, calculating an optimization node through a reverse optimization algorithm to obtain a corresponding reverse optimization path, and selecting an optimal path according to the cost of the plurality of reverse optimization paths. According to the method, the planned path is optimized by combining the arrival time and the collision risk of the frozen region, and the real-time performance and safety of UUV path planning in an underwater complex dynamic scene are improved.

Description

Technical Field

[0001] The present application relates to the field of underwater unmanned vehicle control technology, and in particular to a UUV path planning method in a complex dynamic underwater environment. Background Art

[0002] As marine resources are increasingly gaining attention, the use of underwater unmanned vehicles (UUVs) for marine resource exploration has become increasingly popular. As a multifunctional intelligent underwater navigation device, UUVs have been fully developed in various fields. As UUVs develop in the direction of intelligence and the dynamic environmental conditions of the ocean become more complex, people have put forward higher and higher requirements for UUVs to achieve intelligent control and autonomous navigation. As the most important part of the UUV autonomous navigation system, the path planning effect of UUVs in the underwater dynamic environment will directly affect the UUV's ability to perform tasks.

[0003] When planning the path of UUV in related technologies, the influence of static obstacles such as underwater reefs is usually considered. However, as the underwater combat environment becomes increasingly complex, mobile obstacles with unknown movement intentions have a greater impact on the path planning of UUV. The path planning methods in related technologies generally do not consider the interaction between planning and mobile obstacles, and the possibility of potential collisions is relatively high. Even if the interaction with mobile obstacles is taken into account in some planning schemes, due to the cumbersome interaction methods and poor real-time interaction, the trajectory of mobile obstacles cannot be accurately predicted in a crowded underwater environment, and the probability of collision is still high. Summary of the invention

[0004] The present application aims to solve one of the technical problems in the related art at least to some extent.

[0005] To this end, the first purpose of this application is to propose a UUV path planning method in a complex dynamic underwater environment. The method optimizes the planned path by combining the arrival time and the collision risk of the frozen area, solving the problems of the existing dynamic path planning method, that is, the planning of moving obstacles with unknown intentions in a crowded underwater environment has cumbersome interaction methods, poor real-time performance, and does not fully consider the impact of the interaction between the planned path and obstacles on collision avoidance safety, thereby improving the real-time and safety of UUV path planning in complex dynamic underwater scenes.

[0006] The second object of this application is to propose a UUV path planning system in a complex underwater dynamic environment;

[0007] A third object of the present application is to provide a non-transitory computer-readable storage medium.

[0008] To achieve the above objectives, the first aspect of the present application is to propose a UUV path planning method in a complex underwater dynamic environment, the method comprising the following steps:

[0009] Detecting movement parameters of a plurality of moving obstacles currently observed, and calculating freezing areas corresponding to the plurality of moving obstacles according to the movement parameters;

[0010] By defining the path nodes and node speeds of the underwater unmanned vehicle UUV, setting the path planning framework of the UUV, and setting the planning constraints of the frozen area on the planned path;

[0011] Based on the path planning framework and the planning constraints, multiple safe paths are calculated by avoiding static obstacles and re-randomly and uniformly sampling nodes entering the frozen area;

[0012] For each of the safe paths, an optimization node is calculated by a reverse optimization algorithm to obtain a corresponding reverse optimization path, and an optimal path is selected from the multiple reverse optimization paths based on their costs, wherein a node speed corresponding to the optimization node is less than a node speed of the original node.

[0013] Optionally, in one embodiment of the present application, the method of avoiding static obstacles and re-randomly and uniformly sampling the nodes entering the frozen area to calculate multiple safe paths includes: determining a static planned path without considering the frozen area, the parameters of the static planned path include an orientation parameter from the starting position of the UUV to the target position; for the path node in the static planned path that collides with the static obstacle, reselecting an adjacent path node that does not intersect with the static obstacle to complete the static obstacle collision avoidance processing; determining whether the static planned path after completing the static obstacle collision avoidance processing has an intersection with the frozen area, and if there is an intersection, planning the multiple safe paths by random sampling, wherein planning the safe path includes: uniformly sampling on both sides of the orientation parameter to obtain multiple sampling points, and calculating the path nodes at the first moment and subsequent moments based on the speed and direction of each sampling point.

[0014] Optionally, in one embodiment of the present application, the calculation of the optimization node through the reverse optimization algorithm includes: selecting path nodes at three adjacent moments, and calculating three distance values ​​between the path nodes at the three adjacent moments; determining the maximum value of the three distance values, and determining the resampling area of ​​the alternative optimization node based on the maximum value and the three distance values; calculating the target number of the alternative optimization nodes based on the resampling area; determining the sampling constraints for uniformly sampling the target number of alternative optimization nodes within the resampling area; and calculating the optimization nodes corresponding to the intermediate nodes in the path nodes at the three adjacent moments according to the sampling constraints.

[0015] Optionally, in one embodiment of the present application, the calculating the frozen areas corresponding to the multiple moving obstacles according to the moving parameters includes: determining whether there is a corresponding connected graph satisfying the constraint condition for the multiple moving obstacles; and if there is a connected graph satisfying the constraint condition, calculating the frozen areas F corresponding to the multiple moving obstacles by the following formula:

[0016] F(x F ,R F )={||xx F || <R F}

[0017]

[0018] Among them, x F and R F are the center position and radius of the frozen area, p i is the position of the ith moving obstacle, n is the number of moving obstacles, and x is a two-dimensional vector representing the position within the frozen area.

[0019] Optionally, in one embodiment of the present application, after calculating the frozen areas corresponding to the multiple moving obstacles according to the moving parameters, the method further includes: indicating the threat level of the frozen area according to the density of the moving obstacles in the frozen area, wherein the density of the moving obstacles is calculated by the following formula:

[0020]

[0021] Among them, S F is the area of ​​the frozen region, and n is the number of moving obstacles constituting the frozen region.

[0022] Optionally, in an embodiment of the present application, after setting the planning constraint condition of the frozen area on the planned path, the method further includes: setting an evaluation function according to the distance between each planned path and the frozen area, wherein the evaluation function is expressed by the following formula:

[0023]

[0024] Among them, P s represents the planned path, N sub Indicates that from the planned path P s The number of nodes selected in is the number of frozen areas from time T-1 to time T, and t, k, and j are intermediate parameters used in the calculation.

[0025] Optionally, in one embodiment of the present application, selecting the optimal path from the multiple reverse optimization paths based on their costs includes: determining the arrival time corresponding to each of the reverse optimization paths, and selecting the shortest arrival time from the arrival times corresponding to the multiple reverse optimization paths; calculating the sum of the shortest arrival time and the tolerance time, and selecting an alternative optimization path whose arrival time is less than the sum from the multiple reverse optimization paths; calculating the cost of each of the alternative optimization paths through the evaluation function, and selecting the alternative optimization path with the lowest cost as the optimal path.

[0026] To achieve the above objectives, the second aspect of the present application also proposes a UUV path planning system in a complex underwater dynamic environment, including the following modules:

[0027] A calculation module, used to detect movement parameters of a plurality of moving obstacles currently observed, and calculate the freezing areas corresponding to the plurality of moving obstacles according to the movement parameters;

[0028] A setting module, used for setting a path planning framework of the underwater unmanned vehicle UUV by defining path nodes and node speeds of the UUV, and setting planning constraints of the frozen area on the planned path;

[0029] A planning module, configured to calculate a plurality of safe paths based on the path planning framework and the planning constraints by avoiding static obstacles and re-randomly and uniformly sampling nodes entering the frozen area;

[0030] The optimization module is used to calculate the optimization node for each of the safety paths through a reverse optimization algorithm to obtain a corresponding reverse optimization path, and select an optimal path from the multiple reverse optimization paths according to the costs of the multiple reverse optimization paths, wherein the node speed corresponding to the optimization node is less than the node speed of the original node.

[0031] In order to implement the above-mentioned embodiments, the third aspect embodiment of the present application also proposes a non-temporary computer-readable storage medium, on which a computer program is stored. When the computer program is executed by the processor, the UUV path planning method in the underwater complex dynamic environment in the above-mentioned first aspect embodiment is implemented.

[0032] The technical solution provided by the embodiment of the present application brings at least the following beneficial effects: the present application first calculates the frozen area according to the current moving parameters of multiple moving obstacles; then sets the path planning framework of the UUV by defining the path nodes and node speeds, and sets the planning constraints of the frozen area on the planned path; then, based on the path planning framework and planning constraints, multiple safe paths are calculated by avoiding static obstacles and re-randomly and uniformly sampling the nodes entering the frozen area; finally, for each safe path, the optimization node is calculated by the reverse optimization algorithm, and the optimal path is selected according to the cost of multiple reverse optimization paths. Thus, the present application integrates the long-term behavior planning and short-term motion planning of UUV guidance based on the path planning framework of social perception, proposes an efficient path planning framework to solve the interaction problem between path planning and frozen areas in dynamic environments, designs an optimization function of comprehensive arrival time and freezing risk, enhances the safety of the planned path, and establishes a path sampling strategy to avoid frozen areas. The optimal path is also calculated by the reverse optimization method, which improves the efficiency of UUV path planning in complex dynamic underwater environments. Therefore, the present application optimizes the planned path by combining the arrival time and the collision risk in the frozen area, thereby improving the real-time and safety of UUV path planning in complex underwater dynamic scenes, and facilitating the UUV to reach the target location safely and quickly.

[0033] Additional aspects and advantages of the present invention will be given in part in the following description and in part will be obvious from the following description, or will be learned through practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] The above and / or additional aspects and advantages of the present application will become apparent and easily understood from the following description of the embodiments in conjunction with the accompanying drawings, in which:

[0035] Figure 1 A flow chart of a UUV path planning method in an underwater complex dynamic environment proposed in an embodiment of the present application;

[0036] Figure 2 A schematic diagram of a freezing area proposed in an embodiment of the present application;

[0037] Figure 3 A flowchart of a safe path planning method proposed in an embodiment of the present application;

[0038] Figure 4A schematic diagram of a static planning path and static obstacle avoidance proposed in an embodiment of the present application;

[0039] Figure 5 A schematic diagram of the principle of reverse path optimization proposed in an embodiment of the present application;

[0040] Figure 6 A schematic diagram of a specific path planning result proposed in an embodiment of the present application;

[0041] Figure 7 This is a schematic diagram of the structure of a UUV path planning system in a complex underwater dynamic environment proposed in an embodiment of the present application. DETAILED DESCRIPTION

[0042] Embodiments of the present invention are described in detail below, examples of which are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are intended to be used to explain the present invention, and should not be construed as limiting the present invention.

[0043] It should be noted that this application studies the dynamic path planning problem for the groups of mobile obstacles faced by UUVs during navigation. According to the interaction mode between UUVs and mobile obstacles, the types of path planning research can be roughly divided into potential field-based path planning, velocity space-based path planning, learning-based path planning and social perception-based path planning.

[0044] In a related embodiment, a path planning method based on a potential field method is more common in collision avoidance applications. Furthermore, a potential field method based on a random reachable set can generate an accurate potential field for a mobile obstacle with uncertain behavioral intentions to plan the UUV path in a crowded environment. However, the potential field-based method does not consider the interaction between planning and mobile obstacles to avoid potential collisions.

[0045] The velocity space-based method selects a collision-free path by establishing a velocity constraint on the UUV through the vector velocity relationship between the UUV and the moving obstacle. However, when there are multiple moving obstacles around the UUV, the path planned by the velocity space method is relatively conservative.

[0046] Learning-based path planning methods are emerging methods relative to conservative planning methods. A navigation framework is constructed through methods such as reinforcement learning to generate collision-free paths in normal mode and recovery positions in navigation failure mode. However, the paths planned by this method are often short-term and unsafe in highly dynamic environments.

[0047] Therefore, although the velocity space and learning-based methods in the relevant embodiments take interaction and planning into consideration, accurate trajectory prediction of moving obstacles and real-time safe interaction in crowded environments remain urgent issues to be addressed.

[0048] To this end, this application proposes a UUV path planning method for underwater complex dynamic environments. This method is based on a socially perceived path planning framework, integrating long-term behavior planning and short-term motion planning for UUV guidance, and has the advantages of meeting the real-time and safety requirements of path planning in complex dynamic underwater scenes.

[0049] The following describes in detail a UUV path planning method and system in a complex underwater dynamic environment proposed in an embodiment of the present application with reference to the accompanying drawings.

[0050] Figure 1 This is a flow chart of a UUV path planning method in an underwater complex dynamic environment proposed in an embodiment of the present application, such as Figure 1 As shown, the method comprises the following steps:

[0051] Step S101, detecting movement parameters of a plurality of moving obstacles currently observed, and calculating freezing areas corresponding to the plurality of moving obstacles according to the movement parameters.

[0052] Specifically, this step calculates the frozen area. Since this application is aimed at complex scenes where a group of underwater obstacles are continuously moving, at a certain moment, multiple moving obstacles that are close to each other will form a frozen area, for example, Figure 2 Frozen area shown.

[0053] When planning the path of the UUV, the frozen area should be avoided. For example, if the UUV has entered the frozen area at the current moment due to the movement of the obstacle, it must stop sailing to avoid colliding with the obstacle. To this end, the present application first detects the movement parameters of multiple moving obstacles currently observed by the UUV, and determines whether these moving obstacles can form a frozen area based on these movement parameters. If so, the frozen areas corresponding to the multiple moving obstacles are calculated based on the movement parameters.

[0054] In one embodiment of the present application, the frozen areas corresponding to the multiple moving obstacles are calculated according to the moving parameters, including: determining whether there is a corresponding connected graph satisfying the constraint condition for the multiple moving obstacles; if there is a connected graph satisfying the constraint condition, calculating the frozen areas F corresponding to the multiple moving obstacles by the following formula:

[0055] F(x F ,R F )={||xx F ||· <R F}

[0056]

[0057] Among them, x F and R F are the center position and radius of the frozen area, p i is the position of the ith moving obstacle, n is the number of moving obstacles, and x is a two-dimensional vector representing the position within the frozen area.

[0058] Specifically, in this embodiment, the definition of the frozen area is first set. For a given set of moving obstacle positions p j ,j=[1,2,…,n],n≥2, if there exists a connected graph G=(V,E) that satisfies the constraints expressed by the following formula:

[0059]

[0060] It is determined that these multiple moving obstacles can form a frozen area. Where d(·) is the Euclidean distance function, R o and R r are the collision radius of the moving obstacle and UUV, p j is the position of the jth moving obstacle, and n is the number of moving obstacles.

[0061] Furthermore, the mobile obstacle group {p j The frozen area F can be calculated by the above calculation formula.

[0062] It should be noted that in this application, for the same parameter, when the values ​​of the subscript index and the superscript index are different, it means that the parameter is at different times or under different objects, which can be derived by combining the upper and lower deductions, and this application will not repeat them.

[0063] In this embodiment, the frozen area F calculated by the above calculation formula takes into account the collision between the UUV and the moving obstacle, but does not take into account the possible impact of the frozen area on the external area. For example, the frozen areas F1 and F2 with the same radius have different numbers of obstacles constituting the frozen areas, where n1>n2, and the threats to the UUV are different. Therefore, in one embodiment of the present application, after calculating the frozen areas corresponding to multiple moving obstacles according to the moving parameters, it also includes: indicating the threat level of the frozen area according to the density of the moving obstacles in the frozen area, where the density of the moving obstacles is calculated by the following formula:

[0064]

[0065] Among them, S F is the area of ​​the frozen region, and n is the number of moving obstacles constituting the frozen region.

[0066] Specifically, the present application uses the density of mobile obstacles in the frozen area to indicate the threat level of the frozen area, and the density of mobile obstacles in the frozen area is calculated by the above formula.

[0067] Step S102, by defining the path nodes and node speeds of the underwater unmanned vehicle UUV, setting the path planning framework of the UUV, and setting the planning constraints of the frozen area on the planned path.

[0068] Specifically, this step sets up a path planning framework. Since path planning is to generate an action strategy that limits the state and action of the UUV to a specified range, the goal of the strategy is to make the UUV reach the target location within a specified time T. g (·) minimum. To this end, this application first defines the node speed V T and path node P T To represent the planned path, T represents the time corresponding to each node.

[0069] As a possible implementation, the node speed can be calculated by the following formula:

[0070]

[0071] Among them, x k is the current position of the UUV, T≥1, T represents the sampling time of each behavior node, ΔT b is the sampling time interval.

[0072] In this example, it is required to sample N from the current position to the target position. b nodes, so that the planned path can be obtained Then the time to reach the target location is T g (·) can be calculated by the following formula:

[0073]

[0074] Among them, P g is the target position of the UUV, V g is from To P g The average speed.

[0075] Furthermore, the impact of the frozen area on path planning, that is, the planning constraints of the frozen area on the planned path, can be expressed by the following formula:

[0076]

[0077] Where L(·) represents P T-1 and P T The line segment between FT-1 represents the set of all frozen areas from time T-1 to time T, is the number of frozen areas from time T-1 to time T. According to the definition of the circular frozen area above, we can check whether the line segment L(·) intersects the frozen area F. T-1 .

[0078] Furthermore, based on the obtained frozen area F and planned path P s , an evaluation function needs to be designed to select the optimal path. The evaluation function should be designed based on the distance between the planned path and the frozen area.

[0079] In one embodiment of the present application, after setting the planning constraint condition of the frozen area on the planned path, it also includes: setting an evaluation function according to the distance between each planned path and the frozen area, wherein the evaluation function is represented by the following formula:

[0080]

[0081] Among them, P s represents the planned path, N sub Indicates that from the planned path P s The number of nodes selected in is the number of frozen areas from time T-1 to time T, and t, k, and j are intermediate parameters used in the calculation.

[0082] Therefore, from the calculation formula of the above evaluation function, it can be seen that when the planned path P s When far away from the frozen area, the evaluation function value is smaller, and the high-density frozen area is more dangerous to the UUV.

[0083] Step S103, based on the path planning framework and planning constraints, multiple safe paths are calculated by avoiding static obstacles and re-randomly and uniformly sampling the nodes entering the frozen area.

[0084] Specifically, this step is to plan a safe path. As mentioned above, in step S102, the planned path P is set. s This step will realize the specific details of calculating the planning path, so as to find a feasible planning path P without intersecting with the frozen area. s As a safe path.

[0085] It should be noted that in the path planning problem, it is necessary to set the UUV to a constant speed when there are no obstacles, because this method can save energy. In order to make the planned path as energy-efficient as possible, this application defines an ideal speed limit V const , and requires the node speed V T ≤V const .

[0086] In order to more clearly describe the specific implementation process of planning a safe path in the present application, a safe path planning method proposed in an embodiment of the present application is exemplified below. Figure 3 A flow chart of a safe path planning method proposed in an embodiment of the present application is shown in FIG. Figure 3 As shown, the method comprises the following steps:

[0087] Step S301 , determining a static planning path without considering the frozen area, wherein the parameters of the static planning path include a direction parameter from the starting position to the target position of the UUV.

[0088] Specifically, in order to search for the optimal path, first, find a static planning path P without considering the frozen area. static , the static planning path can be calculated by the following formula:

[0089]

[0090] Among them, θ0 is the orientation parameter from the starting position of the UUV to the target position, is the static planning path P static Number of nodes, if the static path end cannot reach P g ,but otherwise

[0091] Step S302: for the path node that collides with the static obstacle in the static planned path, reselect an adjacent path node that does not intersect with the static obstacle to complete the static obstacle collision avoidance process.

[0092] Specifically, we first consider the collision avoidance problem between the static planning path and the static obstacles in the environment. T-1 The generated node P T If a collision occurs with a static obstacle in the environment, a node P adjacent to the static obstacle is selected. T For example, a static planning path and static obstacle avoidance process can be as follows Figure 4 shown.

[0093] Step S303, determining whether there is an intersection between the static planned path after the static obstacle avoidance process and the frozen area, and if there is an intersection, planning multiple safe paths by random sampling.

[0094] Planning a safe path includes: uniformly sampling on both sides of the orientation parameter to obtain multiple sampling points, and calculating the path nodes at the first moment and the subsequent moments based on the speed and direction of each sampling point.

[0095] Specifically, in this embodiment, if there is no intersection between the static planned path after the static obstacle avoidance processing in step S302 and the frozen area F, the reverse optimization link in the subsequent step can be directly performed. If it is determined that there is an intersection between the static planned path after the processing and the frozen area, it is necessary to resample the nodes entering the frozen area in this step.

[0096] When resampling is specifically implemented, considering the non-convex constraints of the underwater environment, the frozen area will divide the space into different channels, so that the path nodes cannot cross these channels. In this embodiment, the random sampling method is used to select M b A safe path to search the navigation space.

[0097] As an example, to maintain the potential minimum arrival time, sample evenly on both sides of θ0 Then the node at time T = 1 is sampled according to the following formula:

[0098]

[0099] in, and are the speed and direction of the lth sampling point, E obst is a static obstacle in the environment, F0 is the frozen area at the corresponding moment, and L() is the line between the path points. Since the planned route should avoid the frozen area, the intersection of the planned route (linear area) and the frozen area should satisfy the empty set constraint.

[0100] Furthermore, if uniform sampling of the direction still cannot avoid intersecting with the frozen area at the current moment, the UUV may stop moving at the current moment and wait for the frozen area to disappear or move.

[0101] The sampling method of nodes at other moments is similar to that of the first moment mentioned above, and can be sampled according to the following formula:

[0102]

[0103] in, is from To P g The direction of rand is the maximum range of orientation change, F T-1 It is the frozen area at time T-1.

[0104] Step S104, for each safe path, the optimization node is calculated by the reverse optimization algorithm to obtain the corresponding reverse optimization path, and the optimal path is selected from the multiple reverse optimization paths according to the costs of the multiple reverse optimization paths, wherein the node speed corresponding to the optimization node is less than the node speed of the original node.

[0105] Specifically, this step performs reverse path optimization. The planned path obtained in the above step S103 is safe because it fully considers the impact of the frozen area composed of static obstacles and mobile obstacle groups in the underwater environment on the path safety, and ensures the safety of the planning results in the form of constraints. However, because the forward search structure is used when planning the safe path in step S103, the impact of the evolution of the frozen area at future moments on the planning results is not considered. Therefore, although the safe path obtained in step S103 is safe and can avoid collision risks, it is not optimal in terms of arrival time and other perspectives. For this reason, this step optimizes the M obtained in S103. b The safe paths are reversely optimized to obtain the final optimal path.

[0106] In order to obtain the optimal path under safety conditions, the present application adopts a reverse optimization method to find potential optimal path nodes, and the node speed corresponding to the optimized node is less than the node speed V of the original node before optimization. T .

[0107] In one embodiment of the present application, the optimization node is calculated by a reverse optimization algorithm, including: the first step is to select three path nodes at adjacent moments, and calculate three distance values ​​between the three path nodes at adjacent moments.

[0108] Specifically, the reverse optimization method selects three path nodes at adjacent moments [P T-1 ,P T ,P T+1 ]. For a given safe path P safe , whose length is The following three distances are calculated using the following formula:

[0109]

[0110] In the second step, the maximum value among the three distance values ​​is determined, and the resampling area of ​​the candidate optimization node is determined based on the maximum value and the three distance values.

[0111] Specifically, from the above content, we can know that the maximum value of the three distances in the above formula is d max =V const ΔT b , In addition, if the newly sampled node P T ' can reduce V T , then there is a relationship expressed by the following formula:

[0112] d(P T ′,P T-1 )≤d max

[0113]

[0114] At this time, the newly sampled point P T ′ also satisfies the aforementioned constraints and can be used as an alternative optimization node. And the original node P T The conditions that can be optimized are:

[0115]

[0116] Then the resampling area of ​​the candidate optimization node can be obtained as:

[0117]

[0118] Among them, e(·,·) represents the region with the first element as the center and the second element as the radius, and its area is ΔS min is the minimum area that can be sampled.

[0119] In the third step, the target number of candidate optimization nodes is calculated based on the resampled area.

[0120] Specifically, the number of candidate optimization path nodes can be calculated by the following formula:

[0121]

[0122] Among them, K op is the weight coefficient.

[0123] The fourth step is to determine the sampling constraints for uniformly sampling the target number of candidate optimization nodes in the resampling area.

[0124] Specifically, in order to obtain the optimal path node at time T, in S T Uniform sampling within the region nodes and are required to satisfy the constraints as shown in the following formula:

[0125]

[0126] in, and They are To P T+1 and P T-1 distance.

[0127] The fifth step is to calculate the optimized nodes corresponding to the intermediate nodes in the path nodes at three adjacent moments according to the sampling constraints.

[0128] Specifically, based on the above steps, P T Optimization Node The calculation method is as follows:

[0129]

[0130] Where, ΔD error is the permissible distance range.

[0131] Therefore, based on the reverse optimization method of this embodiment, there are two advantages after optimizing the safe path. One is that the optimized path shortens the arrival time, and the other is that the reverse optimization smoothes the planned path.

[0132] Furthermore, each safe path is reversely optimized in the above manner to obtain a corresponding reverse optimized path, and then an optimal path is selected from the multiple reverse optimized paths according to the calculated costs of the multiple reverse optimized paths.

[0133] In one embodiment of the present application, an optimal path is selected from multiple reverse optimization paths based on the costs of multiple reverse optimization paths, including: first determining the arrival time corresponding to each reverse optimization path, and selecting the shortest arrival time from the arrival times corresponding to the multiple reverse optimization paths; then calculating the sum of the shortest arrival time and the tolerance time, and selecting an alternative optimization path with an arrival time less than the sum from the multiple reverse optimization paths; finally, calculating the cost of each alternative optimization path through an evaluation function, and taking the alternative optimization path with the lowest cost as the optimal path.

[0134] Specifically, in this embodiment, from M b The path with the best arrival time is selected from the reverse optimized paths. The formula for calculating the shortest arrival time is as follows:

[0135]

[0136] It should be noted that, since different planned paths may have similar arrival times, in order to expand the search area for the optimal path, in this embodiment, b Select the path with arrival time less than The path is taken as the alternative optimization path, where ΔT error Tolerate time.

[0137] Furthermore, the cost of each path is calculated by the optimization function described in step S102, that is, the evaluation function J, and then by comparing the costs of the candidate optimization paths, the optimal path can be determined by the following formula:

[0138]

[0139] Therefore, the optimization process of the reverse path performed in this step is as follows: Figure 5 shown.

[0140] In summary, the UUV path planning method in the underwater complex dynamic environment of the embodiment of the present application first calculates the frozen area according to the current moving parameters of multiple moving obstacles; then sets the path planning framework of the UUV by defining the path nodes and node speeds, and sets the planning constraints of the frozen area on the planned path; then, based on the path planning framework and planning constraints, multiple safe paths are calculated by avoiding static obstacles and re-randomly and uniformly sampling the nodes entering the frozen area; finally, for each safe path, the optimization node is calculated by the reverse optimization algorithm, and the optimal path is selected according to the cost of multiple reverse optimization paths. Therefore, this method is based on the path planning framework of social perception, integrates the long-term behavior planning and short-term motion planning of UUV guidance, proposes an efficient path planning framework to solve the interaction problem between path planning and frozen areas in dynamic environments, designs an optimization function that integrates arrival time and freezing risk, enhances the safety of the planned path, and establishes a path sampling strategy to avoid frozen areas. The optimal path is also calculated by the reverse optimization method, which improves the efficiency of UUV path planning in the underwater complex dynamic environment. Therefore, this method optimizes the planned path by combining the arrival time and the collision risk of the frozen area, which improves the real-time and safety of UUV path planning in complex underwater dynamic scenes, and is conducive to the UUV reaching the target location safely and quickly.

[0141] Based on the above embodiments, in order to more intuitively describe the UUV path planning method in the underwater complex dynamic environment of the present application, the obstacle avoidance effect and the optimization effect of the arrival time of the planned path, a specific embodiment in actual application is described below.

[0142] In this embodiment, the effectiveness of the path planning method of the present application is verified by designing a simulation experiment. Under the condition of the same environmental data set, the path planning method of the present application is compared with the speed space method and the artificial potential field method in the related embodiments, and the results of controlling the movement of UUVs by paths planned by these three methods are compared.

[0143] Among them, the parameter value in the path planning scheme of this application is V const =1.50m / s, R o =0.25m, R r =0.41m, ΔT b =0.80s, N b =5. The path planning result of this embodiment is as follows Figure 6 The comparison of planning results of the three methods is shown in Table 1 below.

[0144] Table 1 Comparison of path planning methods

[0145] Average speed (m / s) Number of arrivals at the destination This method 1.28 45 Speed ​​Space Method 0.77 22 Artificial Potential Field Method 1.09 16

[0146] From Table 1, it can be seen that the path planning method of the present application has a higher success rate in reaching the destination after avoiding obstacles than the other two methods, and the driving speed is faster and the arrival time is shorter.

[0147] In order to implement the above embodiment, the present application also proposes a UUV path planning system in a complex underwater dynamic environment. Figure 7 This is a schematic diagram of the structure of a UUV path planning system in a complex underwater dynamic environment proposed in an embodiment of the present application. Figure 7 As shown, the system includes a calculation module 100 , a setting module 200 , a planning module 300 and an optimization module 400 .

[0148] The calculation module 100 is used to detect the movement parameters of the multiple moving obstacles currently observed, and calculate the freezing areas corresponding to the multiple moving obstacles according to the movement parameters.

[0149] The setting module 200 is used to set the path planning framework of the underwater unmanned vehicle UUV by defining the path nodes and node speeds of the UUV, and to set the planning constraints of the frozen area on the planned path.

[0150] The planning module 300 is used to calculate multiple safe paths based on the path planning framework and planning constraints by avoiding static obstacles and re-randomly and uniformly sampling the nodes entering the frozen area.

[0151] The optimization module 400 is used to calculate the optimization node for each safe path through the reverse optimization algorithm to obtain the corresponding reverse optimization path, and select the optimal path from multiple reverse optimization paths according to the costs of the multiple reverse optimization paths, wherein the node speed corresponding to the optimization node is less than the node speed of the original node.

[0152] Optionally, in one embodiment of the present application, the planning module 300 is specifically used to: determine a static planned path without considering the frozen area, the parameters of the static planned path include the orientation parameter from the starting position of the UUV to the target position; for the path node in the static planned path that collides with the static obstacle, reselect the adjacent path node that does not intersect with the static obstacle to complete the static obstacle collision avoidance processing; determine whether the static planned path after the static obstacle collision avoidance processing is completed has an intersection with the frozen area, and if there is an intersection, plan multiple safe paths by random sampling, wherein planning the safe path includes: uniformly sampling on both sides of the orientation parameter to obtain multiple sampling points, and based on the speed and direction of each sampling point, respectively calculating the path nodes at the first moment and subsequent moments.

[0153] Optionally, in one embodiment of the present application, the optimization module 400 is specifically used to: select path nodes at three adjacent moments, and calculate three distance values ​​between the path nodes at three adjacent moments; determine the maximum value of the three distance values, and determine the resampling area of ​​the alternative optimization node based on the maximum value and the three distance values; calculate the target number of alternative optimization nodes based on the resampling area; determine the sampling constraints for uniformly sampling the target number of alternative optimization nodes within the resampling area; and calculate the optimization nodes corresponding to the intermediate nodes in the path nodes at three adjacent moments according to the sampling constraints.

[0154] Optionally, in one embodiment of the present application, the calculation module 100 is specifically used to: determine whether there is a corresponding connected graph satisfying the constraint condition for the multiple moving obstacles; if there is a connected graph satisfying the constraint condition, calculate the frozen area F corresponding to the multiple moving obstacles by the following formula:

[0155] F(x F ,R F )={||xx F || <R F}

[0156]

[0157] Among them, x F and R F are the center position and radius of the frozen area, p i is the position of the ith moving obstacle, n is the number of moving obstacles, and x is a two-dimensional vector representing the position within the frozen area.

[0158] Optionally, in one embodiment of the present application, the calculation module 100 is further used to: indicate the threat level of the frozen area according to the density of the mobile obstacles in the frozen area, wherein the density of the mobile obstacles is calculated by the following formula:

[0159]

[0160] Among them, S F is the area of ​​the frozen region, and n is the number of moving obstacles constituting the frozen region.

[0161] Optionally, in one embodiment of the present application, the setting module 200 is further used to: set an evaluation function according to the distance between each planned path and the frozen area, wherein the evaluation function is represented by the following formula:

[0162]

[0163] Among them, P s represents the planned path, N subIndicates that from the planned path P s The number of nodes selected in is the number of frozen areas from time T-1 to time T, and t, k, and j are intermediate parameters used in the calculation.

[0164] Optionally, in one embodiment of the present application, the optimization module 400 is also used to: determine the arrival time corresponding to each reverse optimization path, and select the shortest arrival time from the arrival times corresponding to multiple reverse optimization paths; calculate the sum of the shortest arrival time and the tolerance time, and select an alternative optimization path with an arrival time less than the sum from multiple reverse optimization paths; calculate the cost of each alternative optimization path through an evaluation function, and select the alternative optimization path with the lowest cost as the optimal path.

[0165] It should be noted that the above explanation of the embodiment of the UUV path planning method in a complex underwater dynamic environment is also applicable to the system of this embodiment and will not be repeated here.

[0166] In summary, the UUV path planning system in the underwater complex dynamic environment of the embodiment of the present application is based on a socially perceived path planning framework, which integrates the long-term behavior planning and short-term motion planning of UUV guidance, and proposes an efficient path planning framework to solve the interaction problem between path planning and frozen areas in dynamic environments. An optimization function that integrates arrival time and freezing risk is designed to enhance the safety of the planned path, and a path sampling strategy is established to avoid frozen areas. The optimal path is also calculated using a reverse optimization method, which improves the efficiency of UUV path planning in underwater complex dynamic environments. As a result, the system optimizes the planned path in combination with the arrival time and the collision risk of the frozen area, improves the real-time and safety of UUV path planning in underwater complex dynamic scenes, and is conducive to the UUV reaching the target location safely and quickly.

[0167] In order to implement the above embodiments, the present application also proposes a non-temporary computer-readable storage medium on which a computer program is stored. When the computer program is executed by a processor, a UUV path planning method in an underwater complex dynamic environment as described in any of the above embodiments is implemented.

[0168] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" etc. 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 present application. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner. In addition, those skilled in the art may combine and combine the different embodiments or examples described in this specification and the features of the different embodiments or examples, without contradiction.

[0169] In addition, the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" and "second" may explicitly or implicitly include at least one of the features. In the description of this application, the meaning of "plurality" is at least two, such as two, three, etc., unless otherwise clearly and specifically defined.

[0170] Any process or method description in a flowchart or otherwise described herein may be understood to represent a module, fragment or portion of code comprising one or more executable instructions for implementing the steps of a custom logical function or process, and the scope of the preferred embodiments of the present application includes alternative implementations in which functions may not be performed in the order shown or discussed, including performing functions in a substantially simultaneous manner or in the reverse order depending on the functions involved, which should be understood by technicians in the technical field to which the embodiments of the present application belong.

[0171] The logic and / or steps represented in the flowchart or otherwise described herein, for example, can be considered as an ordered list of executable instructions for implementing logical functions, and can be embodied in any computer-readable medium for use by an instruction execution system, device or apparatus (such as a computer-based system, a system including a processor, or other system that can fetch instructions from an instruction execution system, device or apparatus and execute the instructions), or in combination with these instruction execution systems, devices or apparatuses. For the purpose of this specification, "computer-readable medium" can be any device that can contain, store, communicate, propagate or transmit a program for use by an instruction execution system, device or apparatus, or in combination with these instruction execution systems, devices or apparatuses. More specific examples of computer-readable media (a non-exhaustive list) include the following: an electrical connection with one or more wires (electronic device), a portable computer disk box (magnetic device), a random access memory (RAM), a read-only memory (ROM), an erasable and programmable read-only memory (EPROM or flash memory), a fiber optic device, and a portable compact disk read-only memory (CDROM). In addition, the computer-readable medium may even be paper or other suitable medium on which the program is printed, since the program may be obtained electronically, for example, by optically scanning the paper or other medium and then editing, interpreting or processing in other suitable ways if necessary, and then stored in a computer memory.

[0172] It should be understood that the various parts of the present application can be implemented by hardware, software, firmware or a combination thereof. In the above-mentioned embodiments, multiple steps or methods can be implemented by software or firmware stored in a memory and executed by a suitable instruction execution system. For example, if implemented by hardware, as in another embodiment, it can be implemented by any one of the following technologies known in the art or their combination: a discrete logic circuit having a logic gate circuit for implementing a logic function for a data signal, a dedicated integrated circuit having a suitable combination of logic gate circuits, a programmable gate array (PGA), a field programmable gate array (FPGA), etc.

[0173] A person skilled in the art may understand that all or part of the steps in the method for implementing the above-mentioned embodiment may be completed by instructing related hardware through a program, and the program may be stored in a computer-readable storage medium, which, when executed, includes one or a combination of the steps of the method embodiment.

[0174] In addition, each functional unit in each embodiment of the present application may be integrated into a processing module, or each unit may exist physically separately, or two or more units may be integrated into one module. The above-mentioned integrated module may be implemented in the form of hardware or in the form of a software functional module. If the integrated module is implemented in the form of a software functional module and sold or used as an independent product, it may also be stored in a computer-readable storage medium.

[0175] The storage medium mentioned above may be a read-only memory, a magnetic disk or an optical disk, etc. Although the embodiments of the present application have been shown and described above, it can be understood that the above embodiments are exemplary and cannot be understood as limiting the present application. A person of ordinary skill in the art may change, modify, replace and modify the above embodiments within the scope of the present application.

Claims

1. A UUV path planning method in a complex underwater dynamic environment, characterized in that: The following steps are involved: Detecting movement parameters of a plurality of moving obstacles currently observed, and calculating freezing areas corresponding to the plurality of moving obstacles according to the movement parameters; By defining the path nodes and node speeds of the underwater unmanned vehicle UUV, setting the path planning framework of the UUV, and setting the planning constraints of the frozen area on the planned path; Based on the path planning framework and the planning constraints, multiple safe paths are calculated by avoiding static obstacles and re-randomly and uniformly sampling nodes entering the frozen area; For each of the safe paths, an optimization node is calculated by a reverse optimization algorithm to obtain a corresponding reverse optimization path, and an optimal path is selected from the multiple reverse optimization paths based on their costs, wherein a node speed corresponding to the optimization node is less than a node speed of the original node.

2. The method according to claim 1, characterized in that: The method of calculating multiple safe paths by avoiding static obstacles and re-randomly and uniformly sampling the nodes entering the frozen area includes: Determining a static planning path without considering the frozen area, the parameters of the static planning path including a heading parameter from a starting position of the UUV to a target position; For the path node that collides with the static obstacle in the static planned path, reselect an adjacent path node that does not intersect with the static obstacle to complete the static obstacle collision avoidance process; Determine whether there is an intersection between the static planned path after completing the static obstacle avoidance processing and the frozen area. If there is an intersection, plan the multiple safe paths by random sampling, wherein planning the safe paths includes: uniformly sampling on both sides of the orientation parameter to obtain multiple sampling points, and based on the speed and direction of each of the sampling points, respectively calculate the path nodes at the first moment and the subsequent moments.

3. The method according to claim 1, characterized in that The step of calculating the optimization node by using the reverse optimization algorithm includes: Selecting three path nodes at adjacent moments, and calculating three distance values ​​between the three path nodes at adjacent moments; Determine a maximum value among the three distance values, and determine a resampling region of a candidate optimization node based on the maximum value and the three distance values; Calculating a target number of candidate optimization nodes based on the resampled region; Determine sampling constraint conditions for uniformly sampling the target number of candidate optimization nodes in the resampling area; According to the sampling constraint condition, the optimized node corresponding to the intermediate node in the path nodes at the three adjacent moments is calculated.

4. The method according to claim 1, characterized in that: The calculating, according to the movement parameters, the freezing areas corresponding to the plurality of moving obstacles comprises: Determine whether there is a corresponding connected graph satisfying the constraint condition for the multiple moving obstacles; In the case where there is a connected graph that satisfies the constraint condition, the frozen area F corresponding to the multiple moving obstacles is calculated by the following formula: F(x F ,R F )={||x-x F ||<R F } Among them, x F and R F are the center position and radius of the frozen area, p i is the position of the ith moving obstacle, n is the number of moving obstacles, and x is a two-dimensional vector representing the position within the frozen area.

5. The method according to claim 4, characterized in that After calculating the freezing areas corresponding to the plurality of moving obstacles according to the moving parameters, the method further includes: The threat level of the frozen area is represented according to the density of the mobile obstacles in the frozen area, wherein the density of the mobile obstacles is calculated by the following formula: Among them, S F is the area of ​​the frozen region, and n is the number of moving obstacles constituting the frozen region.

6. The method according to claim 5, characterized in that After setting the planning constraint condition of the frozen area on the planned path, the method further includes: An evaluation function is set according to the distance between each planned path and the frozen area, wherein the evaluation function is expressed by the following formula: Among them, P s represents the planned path, N sub Indicates that from the planned path P s The number of nodes selected in is the number of frozen areas from time T-1 to time T, and t, k, and j are intermediate parameters used in the calculation.

7. The method according to claim 6, characterized in that The selecting the optimal path from the multiple reverse optimization paths according to the costs of the multiple reverse optimization paths includes: Determine the arrival time corresponding to each of the reverse optimization paths, and select the shortest arrival time from the arrival times corresponding to the multiple reverse optimization paths; Calculate the sum of the shortest arrival time and the tolerance time, and select an alternative optimization path whose arrival time is less than the sum from the multiple reverse optimization paths; The cost of each of the candidate optimization paths is calculated using the evaluation function, and the candidate optimization path with the lowest cost is taken as the optimal path.

8. A UUV path planning system in a complex underwater dynamic environment, characterized in that: Includes the following modules: A calculation module, used to detect movement parameters of a plurality of moving obstacles currently observed, and calculate the freezing areas corresponding to the plurality of moving obstacles according to the movement parameters; A setting module, used for setting a path planning framework of the underwater unmanned vehicle UUV by defining path nodes and node speeds of the UUV, and setting planning constraints of the frozen area on the planned path; A planning module, configured to calculate a plurality of safe paths based on the path planning framework and the planning constraints by avoiding static obstacles and re-randomly and uniformly sampling nodes entering the frozen area; The optimization module is used to calculate the optimization node for each of the safety paths through a reverse optimization algorithm to obtain a corresponding reverse optimization path, and select an optimal path from the multiple reverse optimization paths according to the costs of the multiple reverse optimization paths, wherein the node speed corresponding to the optimization node is less than the node speed of the original node.

9. The system according to claim 8, characterized in that The planning module is specifically used for: Determining a static planning path without considering the frozen area, the parameters of the static planning path including a heading parameter from a starting position of the UUV to a target position; For the path node that collides with the static obstacle in the static planned path, reselect an adjacent path node that does not intersect with the static obstacle to complete the static obstacle collision avoidance process; Determine whether there is an intersection between the static planned path after completing the static obstacle avoidance processing and the frozen area. If there is an intersection, plan the multiple safe paths by random sampling, wherein planning the safe paths includes: uniformly sampling on both sides of the orientation parameter to obtain multiple sampling points, and based on the speed and direction of each of the sampling points, respectively calculate the path nodes at the first moment and the subsequent moments.

10. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, a UUV path planning method in an underwater complex dynamic environment is implemented as described in any one of claims 1-7.

Images