A path planning method for AUV performing underwater target positioning tasks based on improved artificial potential field method

Abstract

The present invention proposes a route planning method for an AUV to perform underwater target positioning tasks based on an improved artificial potential field method. The route planning method described in the present invention can solve the problem of an autonomous underwater vehicle (AUV) entering an area with a large positioning error when performing an underwater target positioning task. Step 1: The AUV travels under the action of the traditional artificial potential field method, receives the acoustic signal information of the target during the travel process, and obtains the time delay difference information required for the solution; Step 2: The position coordinates of the target are solved based on the obtained time delay difference information, and the positioning accuracy potential field and line-of-sight force are established through the target position coordinates; Step 3: The positioning accuracy potential field and line-of-sight force are added to the traditional artificial potential field method to obtain an improved artificial potential field method, and the improved artificial potential field method is used to plan the track for the AUV. The present invention has broad application prospects and can be used to search for black boxes of underwater crashed targets and to detect, reconnaissance, and monitor enemy underwater targets.

Description

Technical Field

[0001] The present invention belongs to the technical field of route planning, and in particular relates to a route planning method for an AUV to perform an underwater target positioning task based on an improved artificial potential field method. Background Art

[0002] Traditional artificial potential field methods, as a path planning method, can plan collision-free paths for AUVs. However, when an AUV is tasked with locating underwater targets, traditional artificial potential field methods only consider obstacle avoidance when planning routes. This can lead to excessive errors in the calculated target position when the AUV encounters areas with large positioning errors, making the calculated target position unreliable and the AUV payload inefficient. Therefore, traditional artificial potential field methods are not suitable for planning routes for AUVs tasked with underwater positioning. Summary of the Invention

[0003] The present invention aims to address the problems in the prior art by proposing a route planning method for an AUV performing underwater target positioning tasks based on an improved artificial potential field method. The method improves positioning accuracy and payload efficiency for AUVs performing underwater positioning tasks.

[0004] The present invention is achieved through the following technical solutions. The present invention proposes a route planning method for an AUV to perform underwater target positioning tasks based on an improved artificial potential field method. The route planning method includes the following steps:

[0005] Step 1: The AUV drives under the existing artificial potential field method, receives the target's acoustic signal information during driving, and obtains the time delay difference information required for solution;

[0006] Step 2: Calculate the target's position coordinates based on the obtained time delay difference information, and establish the positioning accuracy potential field and line-of-sight force through the target position coordinates;

[0007] Step 3: Add the positioning accuracy potential field and line-of-sight force to the existing artificial potential field method to obtain an improved artificial potential field method, and use the improved artificial potential field method to plan the trajectory for the AUV.

[0008] Furthermore, the specific process of step one is:

[0009] The default destination of AUV is , , let the position coordinates of AUV be expressed as , assuming that the motion space of the AUV is a two-dimensional space, according to the prior information of the underwater environment and the position information of the AUV itself, the gravitational potential field is expressed as:

[0010] (1)

[0011] Where, is the gain factor, is the position coordinate of the AUV navigation end point, is the distance from AUV to the end of navigation; gravity is the gravitational potential field The negative gradient force is The derivative of the distance between the AUV and the navigation end point is expressed as:

[0012] (2)

[0013] repulsive potential field The mathematical expression is:

[0014] (3)

[0015] Where, is the gain factor, is the position coordinate of AUV, is the position coordinate of the obstacle, is the distance between the AUV and the obstacle, is the maximum influence distance of the repulsive force; Repulsive potential field The negative gradient force is The derivative of the distance between the AUV and the obstacle is expressed as:

[0016] (4)

[0017] Total potential field function for:

[0018] (5)

[0019] The total force acting on the AUV Gravity and repulsion The vector sum is as follows:

[0020] (6)

[0021] The AUV is guided to navigate by gravity and repulsion. The arrival time of the acoustic signal received by the AUV during navigation is , the position of the AUV when it receives the sound signal is the positioning solution node, and the position coordinates of the positioning solution node are , , select two positioning solution nodes 、 , then the delay difference information is .

[0022] Furthermore, the specific process of step 2 is as follows:

[0023] The positioning accuracy potential field selects the method based on the time difference of arrival, namely the TDOA method, as the positioning solution algorithm. According to the obtained time delay difference information, the mathematical expression of the TDOA method is:

[0024] (7)

[0025] Where, Indicates the i The distance between the solution node and the target, c is the speed of sound; if the depth of the underwater target is known, at least three positioning solution nodes are required to solve the target position. The mathematical model of the TDOA method is:

[0026] (8)

[0027] Use the time delay difference information obtained in step 1 to calculate the target position using the TDOA method;

[0028] The positioning accuracy of the positioning algorithm is analyzed using the partial differential method, assuming that the error of each variable is random, the mean is 0, and the variables are independent of each other; the partial differential of each variable in the TDOA mathematical model is obtained, that is,

[0029] (9)

[0030] Write the above formula into matrix form:

[0031] (10)

[0032] From the above formula, we can get

[0033] (11)

[0034] In the formula , , , , , is a diagonal matrix, , , , , The mean square error of depth measurement, node coordinate solution, acoustic signal arrival time and sound speed measurement; , , , , is the partial derivative matrix of each variable;

[0035] It can be seen from formula (11) that the positioning accuracy is not only related to various input errors, but also to the position coordinates of the solution nodes. According to the calculated target position information, when the first two positioning solution nodes are known, the positioning accuracy potential field is as follows:

[0036] (12)

[0037] in, is the scale factor of the positioning accuracy potential field, is the trace of the matrix; under the action of the positioning accuracy potential field, the AUV moves towards the negative gradient direction of the positioning accuracy potential field, and the positioning accuracy force is:

[0038] (13)

[0039] The second-order partial derivative matrix in the formula is as follows:

[0040] (14)

[0041] (15)

[0042] (16)

[0043] (17)

[0044] (18)

[0045] (19)

[0046] The line-of-sight force is calculated by designing a circular route radius When the AUV is within the circular radius, it moves away from the target at the maximum radial speed under the effect of the line-of-sight force until it reaches the path radius. When the AUV is outside the circular radius, it approaches the target at the maximum radial speed under the action of the sight force until it reaches the route radius. is the circular motion with radius; the line-of-sight force is defined as follows:

[0047] (20)

[0048] (twenty one)

[0049] Where, is the scale factor of the sight force, is the designed AUV heading angle, is the unit vector of the heading angle, is the horizontal angle of the boresight, is the sonar's horizontal line-of-sight coverage angle, is the designed circular path radius, where is the coverage angle of the sonar vertical line of sight range, is the horizontal distance between the AUV and the target position.

[0050] Furthermore, the specific process of step three is:

[0051] After adding the positioning accuracy potential field and line-of-sight force to the existing artificial potential field method, an improved artificial potential field method is formed. The improved artificial potential field method is defined as follows:

[0052] (twenty two)

[0053] The navigation destination of the AUV is set as the calculated underwater target position coordinates, and the improved artificial potential field method is used to plan the trajectory for the AUV.

[0054] The beneficial effects of the present invention are:

[0055] When an AUV performs an underwater target positioning task, the method described in the present invention is used to plan the route of the AUV, taking obstacle avoidance into consideration. This can solve the problem of large positioning errors in the route planning process using the traditional artificial potential field method, thereby improving positioning accuracy, increasing the load efficiency of the AUV, and further improving the utilization efficiency of the AUV. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] Figure 1 Schematic diagram of the geometric configuration for AUV to perform underwater target positioning;

[0057] Figure 2 A flow chart of the method of the present invention;

[0058] Figure 3 A path diagram planned for the implementation of the present invention;

[0059] Figure 4 This is a positioning error diagram of an example implementation of the present invention. DETAILED DESCRIPTION

[0060] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. All other embodiments obtained by ordinary technicians in this field based on the embodiments of the present invention without making any creative efforts shall fall within the scope of protection of the present invention.

[0061] The geometric configuration of AUV for underwater target positioning is as follows: Figure 1As shown in FIG, a route is planned for the AUV by a route planning algorithm, and the AUV receives the arrival acoustic signal of the target during the driving process.

[0062] Combine Figure 2 The present invention proposes a path planning method for an AUV to perform underwater target positioning tasks based on an improved artificial potential field method. The path planning method comprises the following steps:

[0063] Step 1: After the AUV is launched, an artificial potential field is established according to the preset navigation destination to plan the path for the AUV. The AUV drives under the action of the existing artificial potential field method, receives the target's acoustic signal information during the driving process, and obtains the time delay difference information required for the solution;

[0064] Step 2: After the AUV is launched into the water and receives the sound arrival signal for a period of time, the Newton iteration method is used to solve the target's position coordinates based on the obtained time delay difference information, and the positioning accuracy potential field and line-of-sight force are established through the target position coordinates;

[0065] Step 3: Let the AUV's navigation destination be the calculated position coordinates of the underwater target, add the positioning accuracy potential field and the line-of-sight force to the existing artificial potential field method to obtain an improved artificial potential field method, and use the improved artificial potential field method to plan the AUV's trajectory.

[0066] The specific process of step one is:

[0067] The default destination of AUV is ( ), an artificial potential field is established, and the AUV navigates under the action of the artificial potential field and obtains the required time delay difference information based on the received acoustic signal information. Based on the prior information of the underwater environment and the AUV's own position information, a virtual potential field, namely the artificial potential field, is established in the space where the AUV is located. is the gravitational potential field, and the position coordinates of the AUV are expressed as , assuming that the motion space of AUV is a two-dimensional space, the gravitational potential field is expressed as:

[0068] (1)

[0069] Where, is the gain factor, is the position coordinate of the AUV navigation end point, is the distance from AUV to the end of navigation; gravity is the gravitational potential field The negative gradient force is The derivative of the distance between the AUV and the navigation end point is expressed as:

[0070] (2)

[0071] repulsive potential field The mathematical expression is:

[0072] (3)

[0073] Where, is the gain factor, is the position coordinate of AUV, is the position coordinate of the obstacle, is the distance between the AUV and the obstacle, is the maximum influence distance of the repulsive force; Repulsive potential field The negative gradient force is The derivative of the distance between the AUV and the obstacle is expressed as:

[0074] (4)

[0075] Total potential field function for:

[0076] (5)

[0077] The total force acting on the AUV Gravity and repulsion The vector sum is as follows:

[0078] (6)

[0079] The AUV is guided to navigate by gravity and repulsion. The arrival time of the acoustic signal received by the AUV during navigation is , the position of the AUV when it receives the sound signal is the positioning solution node, and the position coordinates of the positioning solution node are , , select two positioning solution nodes 、 , then the delay difference information is .

[0080] The specific process of step 2 is:

[0081] The positioning accuracy potential field selects the method based on the time difference of arrival, namely the TDOA method, as the positioning solution algorithm. According to the obtained time delay difference information, the mathematical expression of the TDOA method is:

[0082] (7)

[0083] Where, Indicates the iThe distance between the solution node and the target, c is the speed of sound; if the depth of the underwater target is known, at least three positioning solution nodes are required to solve the target position. The mathematical model of the TDOA method is:

[0084] (8)

[0085] Use the time delay difference information obtained in step 1 to calculate the target position using the TDOA method;

[0086] The positioning accuracy of the positioning algorithm is analyzed using the partial differential method, assuming that the error of each variable is random, the mean is 0, and the variables are independent of each other; the partial differential of each variable in the TDOA mathematical model is obtained, that is,

[0087] (9)

[0088] Write the above formula into matrix form:

[0089] (10)

[0090] From the above formula, we can get

[0091] (11)

[0092] In the formula , , , , , is a diagonal matrix, , , , , The mean square error of depth measurement, node coordinate solution, acoustic signal arrival time and sound speed measurement; , , , , is the partial derivative matrix of each variable;

[0093] It can be seen from formula (11) that the positioning accuracy is not only related to various input errors, but also to the position coordinates of the solution nodes. According to the calculated target position information, when the first two positioning solution nodes are known, the positioning accuracy potential field is as follows:

[0094] (12)

[0095] in, is the scale factor of the positioning accuracy potential field, is the trace of the matrix; under the action of the positioning accuracy potential field, the AUV moves towards the negative gradient direction of the positioning accuracy potential field, and the positioning accuracy force is:

[0096] (13)

[0097] The second-order partial derivative matrix in the formula is as follows:

[0098] (14)

[0099] (15)

[0100] (16)

[0101] (17)

[0102] (18)

[0103] (19)

[0104] In order to keep the target within the sonar's line of sight, the present invention establishes a line of sight force, which is achieved by designing a circular path radius. When the AUV is within the circular radius, it moves away from the target at the maximum radial speed under the effect of the line-of-sight force until it reaches the path radius. When the AUV is outside the circular radius, it approaches the target at the maximum radial speed under the action of the sight force until it reaches the route radius. is the circular motion with radius; the line-of-sight force is defined as follows:

[0105] (20)

[0106] (twenty one)

[0107] Where, is the scale factor of the sight force, is the designed AUV heading angle, is the unit vector of the heading angle, is the horizontal angle of the boresight, is the sonar's horizontal line-of-sight coverage angle, is the designed circular path radius, where is the coverage angle of the sonar vertical line of sight range, is the horizontal distance between the AUV and the target position.

[0108] The specific process of step three is:

[0109] After adding the positioning accuracy potential field and line-of-sight force to the existing artificial potential field method, an improved artificial potential field method is formed. The improved artificial potential field method is defined as follows:

[0110] (twenty two)

[0111] The navigation destination of the AUV is set as the calculated underwater target position coordinates, and the improved artificial potential field method is used to plan the trajectory for the AUV.

[0112] Example 1:

[0113] Environmental parameters are as follows: AUV initial position coordinates are m, the preset AUV navigation end point is m, the target position coordinates are m, the number of obstacles is 6, and the obstacle coordinates are [-420, 200] m, [-200, 280] m, [-300, 200] m, [-200, 200] m, [-40, 80] m, and [-70, 90] m. The obstacle radii are 30 m, 20 m, 20 m, 40 m, 30 m, and 20 m, respectively. The maximum obstacle impact distance is 5 m. The sonar horizontal line of sight coverage angle is 60 degrees, the sonar vertical line of sight coverage angle is 60 degrees, the equivalent sound speed is 1500 m / s, the delay measurement error standard deviation is 1 ms, the AUV position coordinate root mean square error is 2 m, the equivalent sound speed error standard deviation is 1.5 m / s, the depth measurement error standard deviation is 2 m, the target depth measurement error standard deviation is 2 m, and the acoustic beacon signal period is 1 second.

[0114] The improved artificial potential field method designed by the present invention is used to estimate the position of the target. The navigation path diagram of the AUV in the implementation example is as follows: Figure 3 As shown in the figure, the positioning error of the implementation example is as follows Figure 4 As shown in the figure, the average positioning error is 1.81m. It can be seen that the route planning method involved in the present invention can effectively estimate the position coordinates of the target and has high positioning accuracy. It effectively solves the problem of low AUV payload efficiency when using the traditional artificial potential field method to plan the trajectory for the AUV, which only considers obstacle avoidance and does not fully consider the payload efficiency of the AUV performing the positioning task.

[0115] The above is a detailed introduction to the route planning method for an AUV to perform underwater target positioning tasks based on an improved artificial potential field method proposed in the present invention. Specific examples are used in this article to illustrate the principles and implementation methods of the present invention. The description of the above embodiments is only used to help understand the method of the present invention and its core idea. At the same time, for those skilled in the art, according to the ideas of the present invention, there will be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as limiting the present invention.

Claims

1. A route planning method for an AUV performing underwater target positioning tasks based on an improved artificial potential field method, characterized by: The route planning method comprises the following steps: Step 1: The AUV drives under the existing artificial potential field method, receives the target's acoustic signal information during driving, and obtains the time delay difference information required for solution; Step 2: Calculate the target's position coordinates based on the obtained time delay difference information, and establish the positioning accuracy potential field and line-of-sight force through the target position coordinates; Step 3: Add the positioning accuracy potential field and the line-of-sight force to the existing artificial potential field method to obtain an improved artificial potential field method, and use the improved artificial potential field method to plan the trajectory for the AUV; The line-of-sight force is calculated by designing a circular route radius When the AUV is within the circular radius, it moves away from the target at the maximum radial speed under the effect of the line-of-sight force until it reaches the path radius. When the AUV is outside the circular radius, it approaches the target at the maximum radial speed under the action of the sight force until it reaches the route radius. is the circular motion with radius; the line-of-sight force is defined as follows: （20） （21） Where, is the scale factor of the sight force, is the designed AUV heading angle, is the unit vector of the heading angle, is the horizontal angle of the boresight, is the sonar's horizontal line-of-sight coverage angle, is the designed circular path radius, where is the coverage angle of the sonar vertical line of sight range, is the horizontal distance between the AUV and the target position.

2. The route planning method according to claim 1, characterized in that: The specific process of step one is: The default destination of AUV is , , let the position coordinates of AUV be expressed as , assuming that the motion space of the AUV is a two-dimensional space, according to the prior information of the underwater environment and the position information of the AUV itself, the gravitational potential field is expressed as: （1） Where, is the gain factor, is the position coordinate of the AUV navigation end point, is the distance from AUV to the end of navigation; gravity is the gravitational potential field The negative gradient force is The derivative of the distance between the AUV and the navigation end point is expressed as: （2） repulsive potential field The mathematical expression is: （3） Where, is the gain factor, is the position coordinate of AUV, is the position coordinate of the obstacle, is the distance between the AUV and the obstacle, is the maximum influence distance of the repulsive force; Repulsive potential field The negative gradient force is The derivative of the distance between the AUV and the obstacle is expressed as: （4） Total potential field function for: （5） The total force acting on the AUV Gravity and repulsion The vector sum is as follows: （6） The AUV is guided to navigate by gravity and repulsion. The arrival time of the acoustic signal received by the AUV during navigation is , the position of the AUV when it receives the sound signal is the positioning solution node, and the position coordinates of the positioning solution node are , , select two positioning solution nodes 、 , then the delay difference information is .

3. The route planning method according to claim 2, characterized in that: The specific process of step 2 is as follows: The positioning accuracy potential field selects the method based on the time difference of arrival, namely the TDOA method, as the positioning solution algorithm. According to the obtained time delay difference information, the mathematical expression of the TDOA method is: （7） Where, Indicates the i The distance between the solution node and the target, c is the speed of sound; if the depth of the underwater target is known, at least three positioning solution nodes are required to solve the target position. The mathematical model of the TDOA method is: （8） Use the time delay difference information obtained in step 1 to calculate the target position using the TDOA method; The positioning accuracy of the positioning algorithm is analyzed using the partial differential method, assuming that the error of each variable is random, the mean is 0, and the variables are independent of each other; the partial differential of each variable in the TDOA mathematical model is obtained, that is, （9） Write the above formula into matrix form: （10） From the above formula, we can get （11） In the formula , , , , , is a diagonal matrix, , , , , The mean square error of depth measurement, node coordinate solution, acoustic signal arrival time and sound speed measurement; , , , , is the partial derivative matrix of each variable; It can be seen from formula (11) that the positioning accuracy is not only related to various input errors, but also to the position coordinates of the solution nodes. According to the calculated target position information, when the first two positioning solution nodes are known, the positioning accuracy potential field is as follows: （12） in, is the scale factor of the positioning accuracy potential field, is the trace of the matrix; under the action of the positioning accuracy potential field, the AUV moves towards the negative gradient direction of the positioning accuracy potential field, and the positioning accuracy force is: （13） The second-order partial derivative matrix in the formula is as follows: （14） （15） （16） （17） （18） （19）。 4. The route planning method according to claim 3, characterized in that: The specific process of step three is: After adding the positioning accuracy potential field and line-of-sight force to the existing artificial potential field method, an improved artificial potential field method is formed. The improved artificial potential field method is defined as follows: （22） The navigation destination of the AUV is set as the calculated underwater target position coordinates, and the improved artificial potential field method is used to plan the trajectory for the AUV.

Images