A target search method for AUVs combining RRT with artificial potential field method
By combining RRT and artificial potential field method, the problem of target unreachability of AUV in three-dimensional dense obstacle environment is solved, realizing efficient target search of AUV in complex environment and providing a new search planning method.
Patent Information
- Application Number
- CN202210692482.6
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-06-17
- Publication Date
- 2024-11-05
- Estimated Expiration
- 2042-06-17
AI Technical Summary
Existing technologies suffer from target unreachability issues in AUV target search in complex 3D environments, especially in environments with dense obstacles. Artificial potential field methods are prone to falling into a state of gravitational-repulsive equilibrium, causing the AUV to stagnate.
By combining the RRT and artificial potential field method, the search area is segmented by the grid method, sonar is used to find the target, and the RRT algorithm is applied to escape the stagnant position and plan an effective path by combining the search decision function and path planning algorithm.
It improves the target accessibility of AUVs in three-dimensional dense obstacle environments, provides a new approach to search planning in complex environments, and is innovative and practically applicable.
Smart Images

Figure CN114879706B_ABST
Abstract
Description
Technical Field
[0001] The invention belongs to the technical field of autonomous control of unmanned systems, and in particular relates to an AUV target search method combining RRT with an artificial potential field method. Background Art
[0002] Autonomous underwater vehicles (AUVs) are powerful tools for humans to explore unknown ocean environments. In the future, the development space of AUV technology will continue to expand. With the emergence of related research hotspots, research limitations will become more prominent. Topics such as path planning, emergency obstacle avoidance, and target search of AUVs in unknown environments are all hot topics at present. Among them, how to enable AUVs to search for targets scientifically and efficiently in a three-dimensional complex environment and complete corresponding tasks is a very important topic.
[0003] At present, some scholars have proposed efficient search algorithms in two-dimensional environments, but have not expanded the search dimension and have not considered the three-dimensional effectiveness of the algorithm; some scholars have conducted research on searches in three-dimensional environments, but have simply studied target search algorithms and have not linked the search methods to the constraints of the AUV itself, resulting in poor algorithm practicality; some scholars have proposed using heterogeneous vehicles for collaborative searches, such as collaborative searches between surface vehicles and underwater vehicles, which increases the cost and complexity of the search.
[0004] The ocean environment in real missions is three-dimensional and often has dense obstacles. No scholar has yet proposed a corresponding method for the AUV search path planning problem in complex environments. In an environment with a large number of obstacles and close distances between targets, the artificial potential field method is used for search path planning, which is prone to fall into a state of gravitational repulsion equilibrium, causing the AUV to stagnate. Summary of the invention
[0005] The purpose of the present invention is to overcome the problem of unreachable targets in the search process of the artificial potential field method in complex environments, and to provide an AUV target search method combining RRT with the artificial potential field method.
[0006] An AUV target search method combining RRT and artificial potential field method comprises the following steps:
[0007] Step 1: Use the grid method to segment the underwater environment of the AUV search area, and divide the search area into M*N*K grid units. The grid set is t={(i,j,k)|i=1,2...M; j=1,2,...N; k=1,2,...K}, and each grid represents the i-th row and j-th column in the k-water depth cost matrix;
[0008] Step 2: The AUV searches for the target through sonar detection. If there is a target within the sonar detection range, execute step 5; if there is no target within the sonar detection range, update the environmental information around the AUV and execute step 3:
[0009] Step 3: Combine the updated environmental information to solve the search decision function and obtain the best search point to be searched next;
[0010] Search decision function:
[0011] f(a,t)=I B (a,t+1)+I D (a,t+1)
[0012] Among them, I B (a,t+1) is the search task benefit, I B (a,t+1)=k 1 dis(S n ,t)+k 2 turn(S n ,t);dis(S n ,t) represents the distance from the current position of the AUV to the candidate search point S at time t n The path cost between turn(S n ,t) represents the distance from the current position of the AUV to the candidate search point S at time t n The steering angle cost between
[0013]
[0014]
[0015]
[0016] Among them, τ is the path cost coefficient; (x n ,y n ,z n ) is the candidate search point S n The coordinates of the AUV; α is the set AUV steering angle boundary value; β n Indicates that the AUV turns from the current heading to the candidate search point S n Steering angle; k w is a constant;
[0017] I D (a,t+1) is the regional traversal benefit, Where b∈φ(a) represents the grid b in the area with the AUV current position as the center and the AUV detection range as the radius; B(b,t) represents the traversal degree of grid b at time t;
[0018]
[0019] Among them, V b1 (t) represents the volume size that has been searched in grid b at time t; V b Indicates the volume of grid b;
[0020] Step 4: Determine whether the best search point to be searched in the next step is the feasible area of the AUV; if it is not the feasible area, return to step 3 and solve again; otherwise, execute step 5;
[0021] Step 5: Plan the path to make the AUV navigate to the best search point:
[0022] Step 6: Determine whether the target has been completely found. If the target has been completely found, the AUV stops moving and the search mission ends; if the target has not been completely found, return to step 2.
[0023] Furthermore, in step 5, an artificial potential field algorithm is used for path planning, specifically:
[0024] The gravitational potential energy function set at the target point is:
[0025]
[0026] F att = -grad(U att )=k(XX t )
[0027] U att represents the gravitational potential energy around the target point, F att is the gravitational force of the target point on the AUV, k is the gravitational potential field constant, X = (x, y, z), X t =(x t ,y t ,z t ) are the AUV and target coordinates respectively;
[0028] The repulsive potential energy function near the obstacle is:
[0029]
[0030]
[0031] U rep is the repulsive potential energy near the obstacle; F rep is the repulsive force on the AUV from the obstacle; η is the repulsive potential field coefficient; ρ is the straight-line distance between the AUV and the obstacle; ρ 0 It is the farthest range of the repulsive effect of obstacles;
[0032] The AUV plans its path under the action of gravitational repulsion. The resultant force on the AUV is:
[0033]
[0034] Furthermore, in order to prevent the traditional artificial potential field method from being in an unreachable state when facing the target in a dense obstacle environment, the RRT algorithm is applied to jump out of the current stagnant position; the stagnant position of the AUV is selected as the root node p_init of the random tree, and the root node is randomly sampled in the underwater state space as the parent node to obtain p_rand. The Euclidean distance between each node on the tree and the random sampling point p_rand is solved to find the node p_near closest to p_rand on the random tree, and then p_near is expanded in the direction of the random sampling point p_rand with a step size r, and a new node p_new is obtained. If the line connecting p_near and p_new passes the collision avoidance detection, that is, there is no obstacle between the two points, the new node p_new and the line are added to the random tree, otherwise the sampling point will be reselected for expansion and detection;
[0035] The above steps of random sampling and node expansion are repeated until the generated new random tree contains the target node p_goal or when the generated random tree reaches a preset designated area or the number of samplings has reached the upper limit, the tree nodes from the root node to the target point on the random tree are connected to form the planned path.
[0036] Furthermore, in step 2, 360° scanning sonar is used to obtain spherical scanning data to achieve omnidirectional target detection by AUV.
[0037] (x m -x a ) 2 +(y m -y a ) 2 +(z m -z a ) 2 ≤R 2
[0038] If the coordinates are (x m ,y m ,z m ) satisfies the above formula, it means that the target is detected by the sonar; where (x a ,y a ,z a ) represents the coordinates of the AUV; R is the radius of sonar detection.
[0039] The beneficial effects of the present invention are:
[0040] The present invention aims at the planning limitation problem of the artificial potential field algorithm in dense obstacle environment, and utilizes the randomness characteristics of the RRT algorithm itself to solve the problem of unreachable targets in the AUV search process of the artificial potential field method. The combination of the two improves the target accessibility of the AUV in a three-dimensional dense obstacle environment. The present invention not only solves the search planning problem in a three-dimensional dense obstacle environment, but also provides a new idea for planning in a more complex environment. It is innovative and practically applicable and can be used in actual environments. BRIEF DESCRIPTION OF THE DRAWINGS
[0041] Figure 1 It is a flow chart of the present invention.
[0042] Figure 2 This is a three-dimensional environment model diagram.
[0043] Figure 3 This is a sonar detection map.
[0044] Figure 4 is the path cost graph.
[0045] Figure 5 is the steering angle cost map.
[0046] Figure 6 Schematic diagram of the limitations of the artificial potential field.
[0047] Figure 7 Schematic diagram of unreachable target in artificial potential field.
[0048] FIG8( a ) is a schematic diagram of the target unreachable search angle 1 introducing the RRT concept.
[0049] FIG8( b ) is a schematic diagram of the target unreachable search angle 2 which introduces the RRT concept. DETAILED DESCRIPTION
[0050] The present invention is further described below in conjunction with the accompanying drawings.
[0051] The invention relates to an artificial potential field target search method introducing the RRT concept, which can be used to solve the target search problem of an autonomous underwater vehicle (AUV) under its own constraints and belongs to the technical field of autonomous control of unmanned systems.
[0052] The purpose of the present invention is to overcome the problem of unreachable targets in the search process of complex environments by artificial potential field methods, and to provide an AUV target search method combining RRT with the artificial potential field method. This method can allow the AUV to jump out of the stagnant state when it is trapped in a gravitational repulsion equilibrium state and navigate to the target, thereby realizing multi-target search in a three-dimensional dense obstacle environment.
[0053] The present invention is implemented using two functional parts. The first part is to search for target points with unknown positions. By establishing a search decision function, the optimal search point is found within the search range, guiding the AUV to achieve efficient target search. The second part is to use the improved artificial potential field method to plan a path for the AUV from the current position to the position to be searched. If the artificial potential field method cannot plan an effective path, the RRT algorithm is used to plan a path to jump out of the current stagnant position. The following are the specific steps to implement the present invention:
[0054] The search area is divided into M*N*K grid cells, and the grid set is used as the cost matrix of the AUV track:
[0055] t={(i,j,k)|i=1,2...M; j=1,2,...N; k=1,2,...K}
[0056] In the formula, the i, j, k values represent the row number, column number, and depth number of each grid cell.
[0057] The AUV searches for targets through sonar detection. The sonar selected is the SeaKing Hammerhead mechanical scanning sonar from the British Tritech (Tritech International Limited). This sonar has dual frequency and high resolution and can scan the surrounding environment 360°. The horizontal beam angle is 1.5° and the vertical beam angle is 40°. The mathematical model of the sonar is:
[0058] (xx 0 ) 2 +(yy 0 ) 2 +(zz 0 ) 2 ≤R 2
[0059] In the formula, (x 0 ,y 0 ,z 0 ) represents the coordinates of the AUV at a certain moment, and R represents the detection radius of the spherical sonar.
[0060] In order to reduce the navigation distance of AUV, reduce energy consumption and improve search efficiency, the path cost dis(k+1) is established according to the distance between AUV and each search point. The path cost formula is:
[0061]
[0062]
[0063] dis(k+1)=τ·d i
[0064] In the formula, (xa ,y a ,z a ) represents the coordinates of the AUV, (x 1 ,y 1 ,z 1 ) and (x 2 ,y 2 ,z 2 ) represent the selectable search points within the detection range.
[0065] In order to comprehensively consider the relationship between steering loss and search efficiency, the angle at which the AUV turns to each search point is included in the evaluation system, and the steering angle cost turn(k+1) is established. Combined with the actual situation, the inertia weight w is introduced, and the steering angle boundary value α is set for the AUV. That is, the search points exceeding this boundary value have a steering cost, and the steering cost is greater when the boundary value is exceeded. The steering angle cost formula is:
[0066]
[0067]
[0068] Where w is the weight coefficient, β is the steering angle of the AUV, α is the boundary value of the AUV steering angle, and k is a constant.
[0069] Create a region traversal map:
[0070]
[0071] In the formula, B a (k) represents the degree of traversal of grid a at time k, V1 represents the volume size that has been searched in grid a, and V represents the volume size of grid a.
[0072] A search decision function is established by comprehensively considering uncertainty, search cost (path cost and steering angle cost) and area traversal. The AUV obtains the best search node by judging and solving the search decision function, which serves as the key search area of the AUV at the next moment, and then uses the improved RRT algorithm to plan the search path.
[0073] Create a search decision function:
[0074] f(a,k)=I B (a,k+1)+I D (a,k+1)
[0075] Among them, I B (a, k+1) represents the sum of the path cost and steering angle cost of the selectable search points centered on grid a at time k; I D(a, k+1) represents the traversal of the area with grid a as the center and AUV detection range as the radius at time k.
[0076] Solve the decision function to get the best search point. Apply the artificial potential field algorithm to plan the path to the best search point:
[0077] The gravitational potential energy function at the target point is:
[0078]
[0079] U att represents the gravitational potential energy around the target point, k is the gravitational potential field constant, X = (x, y, z) is the AUV coordinate, X t =(x t ,y t ,z t ) are the target coordinates.
[0080] The negative gradient of the potential energy function is the force acting on the AUV. It can be seen that the gravitational force on the AUV from the end point is:
[0081] F att = -grad(U att )=k(XX t )
[0082] The repulsive potential energy function near the obstacle is:
[0083]
[0084] U rep is the repulsive potential energy near the obstacle, η is the repulsive potential field coefficient, ρ is the straight-line distance between the AUV and the obstacle, and ρ 0 It is the farthest range that the repulsive effect of an obstacle can radiate.
[0085] The repulsive force exerted on the AUV by obstacles is:
[0086]
[0087] The net force on the AUV is:
[0088]
[0089] In order to prevent the traditional artificial potential field method from being in an unreachable state when facing the target in a dense obstacle environment, the RRT algorithm is used to jump out of the current stagnant position:
[0090] The stagnant position of the AUV is selected as the root node p_init of the random tree. The root node is used as the parent node to randomly sample p_rand in the underwater state space. The Euclidean distance between each node on the tree and the random sampling point p_rand is solved to find the node p_near closest to p_rand on the random tree. Then p_near is expanded in the direction of the random sampling point p_rand with a step size r, and a new node p_new is obtained. If the line connecting p_near and p_new passes the collision avoidance detection, that is, there is no obstacle between the two points, the new node p_new and the line are added to the random tree, otherwise the sampling point will be reselected for expansion and detection. The above-mentioned random sampling and node expansion steps are performed until the generated new random tree contains the target node p_goal or when the generated random tree reaches the pre-set designated area or the number of samplings has reached the upper limit. The tree nodes from the root node to the target point on the random tree are connected to form the planned path. The RRT expansion process is as follows Figure 3 shown.
[0091] Embodiment 1:
[0092] An AUV target search method combining RRT and artificial potential field method comprises the following steps:
[0093] Step 1: Use the grid method to segment the underwater environment of the AUV search area, and divide the search area into M*N*K grid units. The grid set is t={(i,j,k)|i=1,2...M; j=1,2,...N; k=1,2,...K}, and each grid represents the i-th row and j-th column in the k-water depth cost matrix;
[0094] Step 2: The AUV searches for the target through sonar detection. If there is a target within the sonar detection range, execute step 5; if there is no target within the sonar detection range, update the environmental information around the AUV and execute step 3:
[0095] Use 360° scanning sonar to obtain spherical scanning data, enabling AUV to detect targets in all directions
[0096] (x m -x a ) 2 +(y m -y a ) 2 +(z m -z a ) 2 ≤R 2
[0097] If the coordinates are (x m ,y m ,z m) satisfies the above formula, it means that the target is detected by the sonar; where (x a ,y a ,z a ) represents the coordinates of the AUV; R is the radius of sonar detection.
[0098] Step 3: Combine the updated environmental information to solve the search decision function and obtain the best search point to be searched in the next step; Search decision function:
[0099] f(a,t)=I B (a,t+1)+I D (a,t+1)
[0100] Among them, I B (a,t+1) is the search task benefit, I B (a,t+1)=k 1 dis(S n ,t)+k 2 turn(S n ,t);dis(S n ,t) represents the distance from the current position of the AUV to the candidate search point S at time t n The path cost between turn(S n ,t) represents the distance from the current position of the AUV to the candidate search point S at time t n The steering angle cost between
[0101]
[0102]
[0103]
[0104] Among them, τ is the path cost coefficient; (x n ,y n ,z n ) is the candidate search point S n The coordinates of the AUV; α is the set AUV steering angle boundary value; β n Indicates that the AUV turns from the current heading to the candidate search point S n Steering angle; k w is a constant;
[0105] I D (a,t+1) is the regional traversal benefit, Where b∈φ(a) represents the grid b in the area with the AUV current position as the center and the AUV detection range as the radius; B(b,t) represents the traversal degree of grid b at time t;
[0106]
[0107] Among them, V b1 (t) represents the volume size that has been searched in grid b at time t; V b Indicates the volume of grid b;
[0108] Step 4: Determine whether the best search point to be searched in the next step is the feasible area of the AUV; if it is not the feasible area, return to step 3 and solve again; otherwise, execute step 5;
[0109] Step 5: Plan the path to make the AUV navigate to the best search point:
[0110] The gravitational potential energy function set at the target point is:
[0111]
[0112] F att = -grad(U att )=k(XX t )
[0113] U att represents the gravitational potential energy around the target point, F att is the gravitational force of the target point on the AUV, k is the gravitational potential field constant, X = (x, y, z), X t =(x t ,y t ,z t ) are the AUV and target coordinates respectively;
[0114] The repulsive potential energy function near the obstacle is:
[0115]
[0116]
[0117] U rep is the repulsive potential energy near the obstacle; F rep is the repulsive force on the AUV from the obstacle; η is the repulsive potential field coefficient; ρ is the straight-line distance between the AUV and the obstacle; ρ 0 It is the farthest range of the repulsive effect of obstacles;
[0118] The AUV plans its path under the action of gravitational repulsion. The resultant force on the AUV is:
[0119]
[0120] In order to prevent the traditional artificial potential field method from being in an unreachable state when facing the target in a dense obstacle environment, the RRT algorithm is applied to jump out of the current stagnant position; the stagnant position of the AUV is selected as the root node p_init of the random tree, and the root node is randomly sampled in the underwater state space as the parent node to obtain p_rand. The Euclidean distance between each node on the tree and the random sampling point p_rand is solved to find the node p_near closest to p_rand on the random tree, and then p_near is expanded in the direction of the random sampling point p_rand with a step size r, and a new node p_new is obtained. If the line connecting p_near and p_new passes the collision avoidance test, that is, there is no obstacle between the two points, the new node p_new and the line are added to the random tree, otherwise the sampling point will be reselected for expansion and detection;
[0121] Repeat the above steps of random sampling and node expansion until the generated new random tree contains the target node p_goal or when the generated random tree reaches the preset designated area or the number of samplings has reached the upper limit, connect the tree nodes from the root node to the target point on the random tree to form the planned path;
[0122] Step 6: Determine whether the target has been completely found. If the target has been completely found, the AUV stops moving and the search mission ends; if the target has not been completely found, return to step 2.
[0123] Embodiment 2:
[0124] Combination Figure 1 The flowchart takes the dense obstacle search process in a three-dimensional environment as an example to describe the artificial potential field planning method that introduces the RRT idea in detail:
[0125] AUV detects targets in a three-dimensional dense environment. Targets within the sonar detection range indicate that the target has been found. The next step is to confirm at the target point. This confirmation process uses the artificial potential field method to plan the search confirmation path; if there is no target within the detection range, the environmental information is updated and a search decision function is established. The search decision function consists of path cost, steering angle cost and area traversal data. The path cost is expressed by length, the steering angle cost is expressed by the AUV bow deflection angle, and the area traversal is expressed by the ratio of the sonar detection volume to the environmental grid volume. The position to be searched should meet the characteristics of minimum path cost and steering angle cost and maximum area traversal. The artificial potential field method is then used to reach the position to be searched. In a dense obstacle environment, during the target confirmation process, the repulsive force between multiple obstacles will cause the AUV to stagnate. To avoid this state, the idea of RRT random sampling is introduced when the AUV falls into a stagnant state, so as to jump out of the current stagnation and plan a path to the target point. After successfully finding the path, the AUV search task can be continued.
[0126] Combination Figure 1 The search process of the AUV in a three-dimensional dense obstacle environment in the present invention has the following specific steps:
[0127] Step 1: Establish an ocean environment model and initialize the number of AUV positions and target information in the ocean environment:
[0128] The grid method is used to segment the underwater environment. It is assumed that the underwater environment is an M*N*K rectangular ocean area with unknown number and position of targets in the area, and an AUV performing target search mission.
[0129] The navigation of the AUV in the water is described by a grid set, which is t = {(i, j, k) | i = 1, 2...M; j = 1, 2,...N; k = 1, 2,...K}, where each grid represents the i-th row and j-th column in the k-water depth cost matrix, as shown in the attached figure. Figure 2 As shown in the figure, the grid area that the AUV has already detected returns a value of 1; the grid area that has not been detected by the AUV returns a value of 0. At the same time, the grid map is used to represent the existence of the target. If a target is found in grid a during the detection process, the target probability of the grid becomes 1. If there is no target in the grid, the target probability of the grid becomes 0.
[0130] Step 2: Search for the target through sonar detection. If there is a target, proceed to step 5. If there is no target, update the environmental information around the AUV and proceed to step 3:
[0131] Selecting a 360° scanning sonar to obtain spherical scanning data can realize all-round target detection by the AUV. To ensure that the AUV can detect environmental information above, below, on the left, and on the right, the present invention selects to use two sonars, which are placed above and below the AUV respectively. Figure 2 Shown is the range detected by the sonar.
[0132] (xx 0 ) 2 +(yy 0 ) 2 +(zz 0 ) 2 ≤R 2
[0133] If the coordinates meet the above conditions, it means that the target is detected, where the AUV coordinates are (x 0 ,y 0 ,z 0 ), the target coordinates are (x 1 ,y 1 ,z 1 ), R is the distance at which the target can be clearly detected.
[0134] Step 3: Combine the updated environment information to determine the next location to search. This process requires the establishment of a search decision function, which involves factors such as path cost, steering angle cost, and area traversal degree:
[0135] When energy is limited, the search process should avoid searching long paths as much as possible. There may be multiple other targets in the same length path. The path cost dis means that when facing multiple optional search paths, the shorter the distance between the AUV and the search point, the lower the energy consumption. Otherwise, the greater the energy consumption, the greater the path cost. Figure 4 As shown in the figure, if the AUV chooses to search Searchpoint2, the path cost is obviously greater than Search point1.
[0136] The AUV coordinates are (x a ,y a ,z a ), assuming that the coordinates of the search points that can be selected within the detection range are (x 1 ,y 1 ,z 1 ) and (x 2 ,y 2 ,z 2 ). Path cost formula:
[0137]
[0138]
[0139] dis(k+1)=τ·d i
[0140] Where d1 and d2 are the distances between AUV and search point 1 and search point 2, respectively, and τ is the path cost coefficient.
[0141] Combined with Figure 5 Describe the steering angle cost. k represents the navigation speed direction of AUV at time k, V k+1 Indicates the navigation speed direction of the AUV at time k+1, such as Figure 5 As shown in the figure, multiple possible navigation directions of the AUV at the next moment are listed. The specific selection of the navigation direction should take into account the relationship between the turning loss and the search efficiency.
[0142] The present invention introduces a steering angle weight coefficient and a steering angle boundary value, and the steering angle cost increases as the angle boundary value exceeds the increase:
[0143]
[0144]
[0145] Where turn(k+1) is the turning cost of the AUV at time k+1.
[0146] The area traversal degree indicates the degree to which the area has been searched and traversed. The degree to which each grid in the environment has been traversed at different times is different and is updated as the search time increases. a (k) represents the traversal degree of grid a at time k. Considering that all grids in the environment have not been traversed at the initial time, we set B a (0)=0,a∈Ω.
[0147] Ergodic degree update formula:
[0148]
[0149] Where V1 represents the volume that has been searched in grid a, and V represents the volume of grid a.
[0150] Search mission benefits:
[0151] I B (a,k+1)=k1·dis(S i ,k)+k2·turn(S i ,k)
[0152] In the formula, dis(S i ,k) represents the distance from the current position to the candidate search point S at time k iThe path cost between turn(S i ,k) represents k
[0153] From the current position to the candidate search point S i The steering angle cost between .
[0154] Area traversal benefits:
[0155]
[0156] Considering the uncertainty benefit, search task benefit and area traversal degree, the search decision function is defined as f(a,k), which represents the search decision value of grid a at time k.
[0157] Search decision function:
[0158] f(a,k)=I B (a,k+1)+I D (a,k+1)
[0159] Solve and get the position to be searched at the next moment.
[0160] Step 4: Determine whether the search position is a feasible area for the AUV. If not, return to step 3 and solve again until the search position meets the conditions.
[0161] Step 5: Apply the artificial potential field algorithm to the path planning process of the AUV to the location to be searched:
[0162] The gravitational potential energy function set at the target point is:
[0163]
[0164] F att = -grad(U att )=k(XX t )
[0165] U att represents the gravitational potential energy around the target point, F att is the gravitational force of the target point on the AUV, k is the gravitational potential field constant, X = (x, y, z), X t =(x t ,y t ,z t ) are the AUV and target coordinates respectively.
[0166] The repulsive potential energy function near the obstacle is:
[0167]
[0168]
[0169] U rep is the repulsive potential energy near the obstacle, F rep is the repulsive force on the AUV from the obstacle, η is the repulsive potential field coefficient, ρ is the straight-line distance between the AUV and the obstacle, and ρ 0 This is the maximum range of the repulsion effect of an obstacle.
[0170] The AUV plans its path under the action of gravitational repulsion. The resultant force on the AUV is:
[0171]
[0172] Step 6: In the case of dense obstacles, the AUV is stagnant. The RRT (Rapidly Exploring Random Trees) algorithm is introduced to randomly search for sampling points in space:
[0173] The stagnant position of the AUV is taken as the root node p_init of the random tree. The root node is sampled in space to obtain a random sampling point p_rand. The Euclidean distance between each node on the tree and the random sampling point p_rand is solved, and the node p_near closest to p_rand is found. Then p_near is expanded in the direction of the random sampling point p_rand with a step size r, and a new node p_new is obtained. The above random sampling and node expansion steps are repeated until the generated new random tree contains the target node p_goal. The tree nodes from the root node to the target point on the random tree are connected to form the planned path.
[0174] Step 7: Determine whether the target has been completely found. If the target has been completely found, the AUV stops moving and the search mission ends; if the target has not been completely found, return to step 2 to continue.
[0175] In order to verify the effectiveness of the RRT concept introduced in this invention, a three-dimensional dense obstacle environment is constructed. Figure 7 The blue curve in the middle represents the path planned by the artificial potential field, and the red broken line in Figure 8 represents the introduction of the RRT algorithm for path planning after the artificial potential field fails to reach the target, which proves the effectiveness of this method.
[0176] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.
Claims
1. An AUV target search method combining RRT and artificial potential field method, characterized in that: The following steps are involved: Step 1: Use the grid method to segment the underwater environment of the AUV search area, and divide the search area into M*N*K grid units. The grid set is t={(i,j,k)|i=1,2...M; j=1,2,...N; k=1,2,...K}, and each grid represents the i-th row and j-th column in the k-water depth cost matrix; Step 2: The AUV searches for the target through sonar detection. If there is a target within the sonar detection range, execute step 5; if there is no target within the sonar detection range, update the environmental information around the AUV and execute step 3: Step 3: Combine the updated environmental information to solve the search decision function and obtain the best search point to be searched next; Search decision function: f(a,t)=I B (a,t+1)+I D (a,t+1) Among them, I B (a,t+1) is the search task benefit, I B (a,t+1)=k1dis(S n ,t)+k2turn(S n ,t);dis(S n ,t) represents the distance from the current position of the AUV to the candidate search point S at time t n The path cost between turn(S n ,t) represents the distance from the current position of the AUV to the candidate search point S at time t n The steering angle cost between Among them, τ is the path cost coefficient; (x n ,y n ,z n ) is the candidate search point S n The coordinates of the AUV; α is the set AUV steering angle boundary value; β n Indicates that the AUV turns from the current heading to the candidate search point S n Steering angle; k w is a constant; I D (a,t+1) is the regional traversal benefit, Where b∈φ(a) represents the grid b in the area with the AUV current position as the center and the AUV detection range as the radius; B(b,t) represents the traversal degree of grid b at time t; Among them, V b1 (t) represents the volume size that has been searched in grid b at time t; V b Indicates the volume of grid b; Step 4: Determine whether the best search point to be searched in the next step is the feasible area of the AUV; if it is not the feasible area, return to step 3 and solve again; otherwise, execute step 5; Step 5: Plan the path to make the AUV navigate to the best search point: Step 6: Determine whether the target has been completely found. If the target has been completely found, the AUV stops moving and the search mission ends; if the target has not been completely found, return to step 2.
2. The AUV target search method combining RRT and artificial potential field method according to claim 1, characterized in that: In step 5, an artificial potential field algorithm is used for path planning, specifically: The gravitational potential energy function set at the target point is: F att =-grad(U att )=k(X-X t ) U att represents the gravitational potential energy around the target point, F att is the gravitational force of the target point on the AUV, k is the gravitational potential field constant, X = (x, y, z), X t =(x t ,y t ,z t ) are the AUV and target coordinates respectively; The repulsive potential energy function near the obstacle is: U rep is the repulsive potential energy near the obstacle; F rep The AUV is subject to repulsive force from obstacles; η is the repulsive potential field coefficient; ρ is the straight-line distance between the AUV and the obstacle; ρ0 is the farthest range of the repulsive effect of the obstacle; The AUV plans its path under the action of gravitational repulsion. The resultant force on the AUV is:
3. The AUV target search method combining RRT and artificial potential field method according to claim 2, characterized in that: In order to prevent the traditional artificial potential field method from being in an unreachable state when facing the target in a dense obstacle environment, the RRT algorithm is applied to jump out of the current stagnant position; the stagnant position of the AUV is selected as the root node p_init of the random tree, and the root node is randomly sampled in the underwater state space as the parent node to obtain p_rand. The Euclidean distance between each node on the tree and the random sampling point p_rand is solved to find the node p_near closest to p_rand on the random tree, and then p_near is expanded in the direction of the random sampling point p_rand with a step size r, and a new node p_new is obtained. If the line connecting p_near and p_new passes the collision avoidance test, that is, there is no obstacle between the two points, the new node p_new and the line are added to the random tree, otherwise the sampling point will be reselected for expansion and detection; The above steps of random sampling and node expansion are repeated until the generated new random tree contains the target node p_goal or when the generated random tree reaches a preset designated area or the number of samplings has reached the upper limit, the tree nodes from the root node to the target point on the random tree are connected to form the planned path.
4. The AUV target search method combining RRT and artificial potential field method according to claim 1, characterized in that: In step 2, 360° scanning sonar is used to obtain spherical scanning data to achieve omnidirectional target detection by AUV (x m -x a ) 2 +(y m -y a ) 2 +(z m -z a ) 2 ≤R 2 If the coordinates are (x m ,y m ,z m ) satisfies the above formula, it means that the target is detected by the sonar; where (x a ,y a ,z a ) represents the coordinates of the AUV; R is the radius of sonar detection.
