A local path planning method for an underwater autonomous robot

Abstract

The application is a kind of local path planning method for underwater autonomous robot. It includes the following steps: using global path planning A* algorithm as known three-dimensional global reference path; setting space sampling parameters, establishing discrete sampling space of waypoints; finding current global path matching point from global reference path according to the positioning coordinates of current robot, finding local global terminal point; carrying out hierarchical processing on sampling space, screening the last layer of sampling space points, finding the nearest local terminal point to local global terminal point; defining multiple cost functions according to discrete sampling space and obstacle information, using analytic hierarchy process to weight and sum multiple cost function values; calculating the cost value between each layer of discrete sampling space waypoints, solving the reference waypoints in each layer of discrete sampling space with the minimum cost from current position to local terminal point based on dynamic programming algorithm. The application is simple and efficient, and is free from the influence of underwater autonomous robot perception limitation on existing path planning algorithm.

Description

Technical Field

[0001] This invention belongs to the field of path planning, specifically relating to a local path planning method for an underwater autonomous robot. Background Technology

[0002] Underwater autonomous robots are robots capable of performing various tasks in underwater environments, possessing high application value and development potential. However, the underwater environment is complex and ever-changing, and autonomous navigation and obstacle avoidance remain significant challenges in their application. Local path planning, as a crucial method for solving the autonomous navigation and obstacle avoidance problems of underwater autonomous robots, plays a vital role in improving their obstacle avoidance capabilities and ensuring their safety and stability.

[0003] Traditional robots typically rely on sensors such as vision and lasers for obstacle detection, then use local path planning algorithms to achieve autonomous navigation and obstacle avoidance. Currently, commonly used methods in the field of robot local path planning include the Dynamic Window Approach (DWA) and the Timed Elastic Band (TEB) algorithm. While these methods can achieve basic autonomous navigation and obstacle avoidance, they still have many problems, such as unreasonable path planning and ineffective obstacle avoidance, especially in complex underwater environments with dense obstacles. Furthermore, these algorithms require the robot to have complete knowledge of obstacle information within its 360-degree radius. However, the sensing range of sensors used in underwater autonomous robots is usually limited to a fixed opening angle, resulting in limited perception. Therefore, conventional DWA and TEB algorithms are not suitable for path planning research in underwater autonomous robots. Summary of the Invention

[0004] The purpose of this invention is to provide a local path planning method for underwater autonomous robots based on discrete spatial sampling and dynamic programming algorithms. This method can not only effectively realize the autonomous navigation and obstacle avoidance functions of underwater autonomous robots, but also get rid of the adverse effects of limited underwater obstacle perception on local path planning. It is simple, efficient and has better real-time performance.

[0005] The technical solution to achieve the objective of this invention is: a local path planning method for an underwater autonomous robot, comprising the following steps:

[0006] S1: Obtain the three-dimensional global reference path using the global path planning A* algorithm;

[0007] S2: Based on the positioning and attitude quaternion information of the underwater autonomous robot, set the spatial sampling parameters and establish a waypoint discrete sampling space based on the robot's global coordinate system;

[0008] S3: Transform the obstacle position coordinates sensed by the sonar into coordinates relative to the robot's global coordinate system using the robot's current position coordinates and attitude quaternions;

[0009] S4: Based on the current robot's positioning coordinates, find the current global path matching point on the global reference path obtained in step S1, and find the local global endpoint from the position of the discrete sampling space established in step S2.

[0010] S5: Perform layered processing on the discrete sampling space established in step S2, filter the last layer of sampling space points, and find the local planning endpoint that is closest to the local global endpoint in step S4 according to the proximity principle.

[0011] S6: Based on the discrete sampling space coordinates of step S2, the global coordinates of obstacles sensed by the sonar in step S3, the local and global endpoint coordinates obtained in step S4, and the local planning endpoint obtained in step S5, define the obstacle avoidance cost function, the cost function for reaching the local and global endpoint, and the path length cost function. Use the analytic hierarchy process (AHP) to perform a weighted summation of the cost function values.

[0012] S7: Calculate the cost between waypoints in each layer of the discrete sampling space, that is, calculate the value of the weighted sum of the cost function values ​​in step S6, and solve the reference waypoints in each layer of the discrete sampling space with the minimum cost from the current position to the local planning endpoint based on the dynamic programming algorithm.

[0013] S8: Local path planning is performed by constructing a path that connects reference waypoints using curves.

[0014] Furthermore, the spatial sampling parameters set in step S2 include: longitudinal sampling distance L, number of longitudinal sampling points ln, longitudinal sampling interval Dx, lateral sampling distance W, number of lateral sampling points wn, lateral sampling interval Dy, vertical sampling distance H, number of vertical sampling points hn, and vertical sampling interval Dz.

[0015] Furthermore, step S2, which establishes the waypoint discrete sampling space based on the robot's global coordinate system, specifically involves:

[0016] Based on the relationship between line segments and points, that is, for n points there are n-1 line segments, the longitudinal sampling parameters satisfy the following relationship:

[0017]

[0018] Similarly, the following relationship applies to the lateral sampling parameters:

[0019]

[0020] Similarly, the vertical sampling parameters satisfy the following relationship:

[0021]

[0022] S represents the longitudinal distance from the robot's current planned starting point to the sampling space, and is set to S = Dx;

[0023] The position coordinates of the waypoint discrete sampling space relative to the robot coordinate system are denoted as follows:

[0024] The longitudinal sampling interval Dx and the longitudinal distance S from the robot's current position to the sampling space are expressed as follows:

[0025]

[0026] Obtained from the transverse sampling interval Dy

[0027]

[0028] Similarly Obtained from the vertical sampling interval Dz

[0029]

[0030] Meanwhile, the points in this discrete sampling space Transform from the robot coordinate system to the global coordinate system.

[0031] Furthermore, step S3 specifically includes:

[0032] The transformation relationship between the robot's position coordinates in the robot coordinate system and its position coordinates in the global coordinate system is obtained from the robot's pose quaternions, as shown in the following formula.

[0033]

[0034] In the above formula, p′ obs p′ represents a pure imaginary quaternion composed of obstacle coordinates relative to the global coordinate system. obs =[0,obs X ,obs Y ,obs Z ]; obs X obs Y obs Z Let represent the position coordinates of the detected obstacle relative to the global coordinate system; q represents the unit quaternion of the robot's current pose, denoted as q = [ω, x, y, z]; p obsp represents a pure imaginary quaternion consisting of waypoint coordinates in a discrete sampling space relative to the robot's coordinate system. obs =[0,obs x ,obs y ,obs z ]; obs x obs y obs z These represent the position coordinates of the detected obstacle relative to the robot's coordinate system; q -1 The unit inverse quaternion representing the robot's current position and pose is denoted as q. -1 = [ω, -x, -y, -z]; △p represents the imaginary quaternion formed by the robot's current position, denoted as △p = [0, pos] X ,pos Y ,pos Z ], where pos X ,pos Y ,pos Z This represents the robot's spatial position coordinates relative to the global coordinate system.

[0035] Furthermore, step S5 specifically includes the following steps:

[0036] S51: Search layer for determining the endpoint of local programming;

[0037] Determine the search height of the last layer in the sampling space, and use the height difference dp between the current planning start point p and the local global endpoint local_global_goal to determine the sampling height position of the local planning endpoint local_goal;

[0038] dp = posz - local_global_goalz

[0039] The search range of the last layer of the sampling space is filtered based on the sign of the height difference dp;

[0040] S52: Determine the selection range for the local planning endpoint;

[0041] S53: Determine the local planning endpoint based on the principle of proximity.

[0042] Furthermore, the obstacle avoidance cost function defined in step S6 is as follows:

[0043]

[0044] In the formula, d obs Represented as the distance between discrete sampling points and obstacles

[0045]

[0046] obs X obs Y obs Z These represent the position coordinates of the detected obstacles relative to the global coordinate system; Represents the relative global coordinate system O XYZ The waypoint coordinates in the discrete sampling space, where i = 1, 2, ..., ln; j = 1, 2, ..., wn; k = 1, 2, ..., hn;

[0047] The defined cost function for the tendency towards the local global endpoint is as follows:

[0048]

[0049] In the formula, d goal This is expressed as the distance between discrete sampling points and the target endpoint;

[0050]

[0051] llg X ,llg Y ,llg Z The local_global_goal is the position coordinate of the robot's local global endpoint relative to the global coordinate system; Let i represent the waypoint coordinates in the discrete sampling space relative to the global coordinate system, where i = 1, 2, ..., ln; j = 1, 2, ..., wn; k = 1, 2, ..., hn;

[0052] d max2 This represents the farthest distance between the current planning starting point and the local / global endpoint.

[0053]

[0054] In the formula, pos X ,pos Y ,pos Z This represents the robot's spatial position coordinates relative to the global coordinate system;

[0055] The defined path length cost function is as follows:

[0056]

[0057] In the formula, d start Represented as the distance between discrete sampling points and the current planning starting point.

[0058]

[0059] Current planning starting point pos X ,pos Y ,pos ZThis represents the robot's spatial position coordinates relative to the global coordinate system; Let i represent the waypoint coordinates in the discrete sampling space relative to the global coordinate system, where i = 1, 2, ..., ln; j = 1, 2, ..., wn; k = 1, 2, ..., hn;

[0060] d max3 This represents the maximum distance of a discrete sampling point from the current planning starting point, obtained from the construction parameters of the sampling space, as shown below.

[0061]

[0062] In the formula, W represents the lateral sampling distance of the robot waypoint in the three-dimensional discrete sampling space; Dx represents the longitudinal sampling interval of the robot waypoint in the three-dimensional discrete sampling space; and H represents the vertical sampling distance of the robot waypoint in the three-dimensional discrete sampling space.

[0063] The total cost function, cost, can be expressed as follows using weighted averages:

[0064] cost = ω1g1 + ω2g2 + ω2g2

[0065] Where ω1 represents the weight of obstacle avoidance cost; ω2 represents the weight of the cost of approaching the local global endpoint; and ω3 represents the weight of the path length cost.

[0066] Compared with the prior art, the significant advantages of this invention are:

[0067] This invention designs a local path planning method for underwater autonomous robots based on discrete space sampling and dynamic programming algorithms. This method can still achieve effective and feasible local path planning for underwater autonomous robots even when forward-looking sonar perception is limited. It can effectively cope with obstacle avoidance problems in complex and ever-changing underwater environments, especially for obstacle avoidance of perceived static obstacles in the water. At the same time, the local path planning algorithm is simple and efficient, and has the advantages of high real-time performance and high computational efficiency.

[0068] This method can overcome the shortcomings of existing DWA and TEB algorithms, break through the limitations of underwater autonomous robot sensors in obstacle perception, and still achieve effective autonomous navigation and obstacle avoidance for underwater autonomous robots even when underwater sensor perception is limited. This method is free from the influence of underwater autonomous robot perception limitations on existing path planning algorithms, is simple and efficient, and is more suitable for local path planning of underwater autonomous robots. It can be used as a general local path planning method for underwater autonomous robots. Attached Figure Description

[0069] Figure 1A flowchart of a local path planning method for an autonomous underwater robot based on forward-looking sonar, provided for the implementation of this invention;

[0070] Figure 2 This is a schematic diagram of the discrete sampling space generated at the current position given the sampling space parameters in an embodiment of the present invention;

[0071] Figure 3 This is a schematic diagram illustrating the positional relationship and differences between the local and global endpoints and the local endpoints relative to the discrete sampling space in an embodiment of the present invention.

[0072] Figure 4 This is a schematic diagram illustrating the distribution of obstacles within the sonar sensing range in an embodiment of the present invention.

[0073] Figure 5 This is a schematic diagram illustrating the obstacle avoidance cost in an embodiment of the present invention;

[0074] Figure 6 This is a schematic diagram illustrating the tendency towards a local global endpoint cost in the implementation of this invention;

[0075] Figure 7 This is a schematic diagram illustrating the path length cost in an embodiment of the present invention;

[0076] Figure 8 This is a schematic diagram of the reference waypoint results for local path planning in the implementation of this invention. Detailed Implementation

[0077] To make the objectives, technical solutions, and advantages of this invention clearer, the embodiments of this invention are further described in detail below with reference to the accompanying drawings:

[0078] like Figure 1 As shown, this invention provides a local path planning method for an autonomous underwater robot based on forward-looking sonar, comprising the following steps:

[0079] Step 1: Use the global path planning A* algorithm to obtain the global reference path;

[0080] Specifically, a global reference path is given based on map information, using the A* algorithm and the robot's localization coordinates. The global reference path is a set of three-dimensional spatial points defined relative to global coordinates.

[0081] Step 2: Based on the positioning coordinates and attitude quaternion information of the underwater autonomous robot, set the spatial sampling parameters, and then establish a waypoint discrete sampling space based on the robot's global coordinate system;

[0082] Specifically, based on the positioning coordinates and attitude quaternion information of the underwater autonomous robot, sampling space parameters are set, including longitudinal sampling distance L, number of longitudinal sampling points ln, longitudinal sampling interval Dx, lateral sampling distance W, number of lateral sampling points wn, lateral sampling interval Dy, vertical sampling distance H, number of vertical sampling points hn, and vertical sampling interval Dz, to establish a discrete sampling space. Therefore, the coordinates of the discrete sampling space are relative to the robot's global coordinate system.

[0083] Based on the relationship between line segments and points—that is, for n points there are (n-1) line segments—we can see that the longitudinal sampling parameters satisfy the following relationship:

[0084]

[0085] Similarly, the lateral sampling parameters satisfy the following relationship

[0086]

[0087] Similarly, the vertical sampling parameters satisfy the following relationship

[0088]

[0089] S represents the longitudinal distance from the robot's current planned starting point to the sampling space, and can be set to S = Dx.

[0090] The position coordinates of the waypoint discrete sampling space relative to the robot coordinate system are denoted as follows:

[0091] It can be represented by the longitudinal sampling interval Dx and the longitudinal distance S between the robot's current position and the sampling space.

[0092]

[0093] It can be obtained from the transverse sampling interval Dy

[0094]

[0095] Similarly It can be obtained from the vertical sampling interval Dz.

[0096]

[0097] Meanwhile, the points in this discrete sampling space It is also necessary to transform from the robot coordinate system to the global coordinate system. To more intuitively understand the discrete sampling space, Figure 2 The discrete sampling space obtained from the current robot position is listed.

[0098] Step 3: Transform the obstacle position coordinates sensed by the sonar into coordinates relative to the robot's global coordinate system using the robot's current position coordinates and attitude quaternions;

[0099] Specifically, the obstacle position coordinates obtained by sonar are relative to the robot coordinate system and cannot be directly used in local path planning. Because the discrete sampling points obtained in step 2 are transformed from the robot coordinate system to the global coordinate system, the obstacle position coordinates obtained by perception must also be transformed accordingly to the global coordinate system. The transformation relationship between the position coordinates in the robot coordinate system and the position coordinates in the global coordinate system can be obtained using the robot attitude quaternions, as shown in the following formula.

[0100]

[0101] In the above formula, p′ obs p′ represents a pure imaginary quaternion composed of obstacle coordinates relative to the global coordinate system. obs =[0,obs X ,obs Y ,obs Z ]; obs X obs Y obs Z Let represent the position coordinates of the detected obstacle relative to the global coordinate system; q represents the unit quaternion of the robot's current pose, denoted as q = [ω, x, y, z]; p obs p represents a pure imaginary quaternion consisting of waypoint coordinates in a discrete sampling space relative to the robot's coordinate system. obs =[0,obs x ,obs y ,obs z ]; obs x obs y obs z These represent the position coordinates of the detected obstacle relative to the robot's coordinate system; q -1 The unit inverse quaternion representing the robot's current position and pose is denoted as q. -1 = [ω, -x, -y, -z]; △p represents the imaginary quaternion formed by the robot's current position, denoted as △p = [0, pos] X ,pos Y ,pos Z ], where pos X ,pos Y ,pos Z This represents the robot's spatial position coordinates relative to the global coordinate system.

[0102] Step 4: Based on the robot's current positioning coordinates, find the current global path matching point on the global reference path established in Step 1, and then find the local global endpoint from the location in the discrete sampling space established in Step 2.

[0103] Specifically, step 1 yields a set of points in three-dimensional space, including global coordinates gpx, gpy, and gpz. Assume the global path has ng discrete points. Before determining the local global endpoint, the matching point of the planning starting point p on the global path (global_path) must be determined beforehand. i .

[0104] Assuming the current planning starting point is p, the matching point on the global path can be obtained by traversing the global path from the current planning starting point p to the global path point with the minimum distance. i Global path matching point: global_path i Ideally, the planning starting point p should be directly in front of the current planning starting point p. However, this is the ideal situation in practice, where the planning starting point p is perpendicular to the global path direction. Because of control biases, the actual planning starting point may be p1 or p2, which needs to be distinguished. The current planning starting point p1 and the global path matching point global_path... i The vector is The current planning starting point p1 and the global path matching point global_path i The vector is global path matching point global_path i and the next global path point global_path i+1 The vector is

[0105]

[0106] Determine the matching point of the planning starting point p on the global path. i Next, the local global endpoint, local_global_goal, needs to be determined based on the length of the sampling space. The local global endpoint, local_global_goal, should be located outside the sampling space as much as possible and is determined by parameters such as the sampling space parameters, the vertical sampling distance L, the sampling space interval S, and the raster map resolution re.

[0107]

[0108] in, α is the distance coefficient considering the curvature of the global path, which can be taken as 1.2.

[0109] Step 5: Perform layered processing on the discrete sampling space points obtained in Step 2, filter the last layer of sampling space points, and find the point closest to the local global endpoint obtained in Step 4 according to the proximity principle as the local planning endpoint.

[0110] Specifically, step 5 is divided into 3 sub-steps to obtain the local planning endpoint.

[0111] 5-1 Determining the Search Layer for the Local Programming Endpoint

[0112] The local planning endpoint, local_goal, in the discrete space can be determined by filtering the last layer in the sampling space. First, the search height of the last layer in the sampling space must be determined. The sampling height position of the local planning endpoint, local_goal, is determined by the height difference dp between the current planning starting point p and the local-global endpoint, local_goal.

[0113] dp = posz - local_global_goalz

[0114] The search range of the last layer of the sampling space is filtered based on the sign of the height difference dp, taking an example where the number of horizontal sampling points wn = 3.

[0115]

[0116] Where df is the height tolerance distance. When the vertical sampling distance H = 2m, the number of vertical sampling points Hn = 3, and the vertical sampling interval Dz = 1m, df can be taken as 0.3m.

[0117] 5-2 Determining the selection range for the local planning endpoint

[0118] Assuming the number of lateral sampling points wn = 7, the local planning endpoint local_goal can be determined by discussing the distribution of the sensing data detected by sonar.

[0119] To more intuitively understand the discrete sampling space, Figure 3 Examples illustrate the positional relationship and differences between local and global endpoints and local endpoints relative to the discrete sampling space.

[0120] at the same time Figure 4 This is a schematic diagram illustrating the distribution of obstacles within the sonar sensing range in an embodiment of the present invention. Figure 4 In the obstacle distribution diagram of the sonar sensing range: point A is the installation location of the sonar; oo′ is the central axis; R is the maximum detection range of the sonar, which is 100m; arc F1AF2 represents the sonar opening angle, that is, the detection angle range of the sonar ∠F1AF2 is 130°; L is the longitudinal sampling distance. W is the lateral sampling distance. S is the sampling spatial interval. Arc G1AG2 represents the critical danger radius, indicating that obstacles within arc G1AG2 pose a threat to local path planning. The radius R of arc G1AG2 can be calculated. d for R d This is also considered the critical danger radius; the obstacles sensed by sonar are divided into the left obstacle zone, which is the arc F1AE1 region, i.e., ∠F1AE1 approximately 43°; the middle obstacle zone, which is the arc E1AE2 region, i.e., ∠E1AE2 approximately 44°; and the right obstacle zone, which is the arc E2AF2 region, i.e., ∠E2AF2 approximately 43°.

[0121] The distribution of the horizontal sampling points is determined as follows: Left obstacle zone [1,2], Middle obstacle zone [3,4,5], and Right obstacle zone [6,7].

[0122] When the number of detected obstacle points in a certain obstacle zone exceeds a certain limit num obs When the obstacle perception result is not cleared, the area is considered impassable, meaning the corresponding filter point number for that obstacle perception result cannot be selected as the local planning endpoint.

[0123] Determine the distance of the detected obstacle point within the obstacle zone to R d Within the critical danger radius, the obstacle points are accumulated.

[0124] (dobs<=R) d → (num = num + 1)

[0125] In the formula, the distance from the obstacle point within the obstacle perception result partition to the current planning starting point is dobs.

[0126] The formula for determining whether passage is possible within an obstacle zone is as follows:

[0127]

[0128] In the formula, num obs The limit on the number of obstacle detection points within the obstacle zone, num obs Set to 10; flag_left indicates whether the left obstacle zone is passable, flag_middle indicates whether the middle obstacle zone is passable, and flag_right indicates whether the right obstacle zone is passable.

[0129] Count the number of obstacle perception points in each obstacle perception area (Left, Middle, and Right) to determine the range of filter points for the local planning endpoint. The rule table for determining the range of filter points for the local planning endpoint is as follows: When the left obstacle area flag_left is 0, the middle obstacle area flag_middle is 0, and the right obstacle area flag_right is 0, the filter point range is [1,2,3,4,5,6,7]; when the left obstacle area flag_left is 0, the middle obstacle area flag_middle is 0, and the right obstacle area flag_right is 1, the filter point range is [3,4,5,6,7]; when the left obstacle area flag_left is 0, the middle obstacle area flag_middle is 1, and the right obstacle area flag_right is 0, the filter point range is [1,2 ... When the flag_middle of the obstacle zone is 1 and the flag_right of the right obstacle zone is 1, the range of the selection points is [6,7]. When the flag_left of the left obstacle zone is 1, the flag_middle of the middle obstacle zone is 0, and the flag_right of the right obstacle zone is 0, the range of the selection points is [1,2,3,4,5]. When the flag_left of the left obstacle zone is 1, the flag_middle of the middle obstacle zone is 0, and the flag_right of the right obstacle zone is 1, the range of the selection points is [3,4,5]. When the flag_left of the left obstacle zone is 1, the flag_middle of the middle obstacle zone is 1, and the flag_right of the right obstacle zone is 0, the range of the selection points is [1,2]. When the flag_left of the left obstacle zone is 1, the flag_middle of the middle obstacle zone is 1, and the flag_right of the right obstacle zone is 0, the range of the selection points is [1,2]. When the flag_left of the left obstacle zone is 1, the flag_middle of the middle obstacle zone is 1, and the flag_right of the right obstacle zone is 1, the range of the selection points is an empty set, which is considered a planning failure.

[0130] 5-3 Determining the local planning endpoint based on the principle of proximity

[0131] Iterate through the searchrange of the local planning endpoint and consider the point closest to the local global endpoint local_global_goal as the local planning endpoint local_goal.

[0132] Step 6: Define obstacle avoidance cost, approaching local global endpoint cost, and path length cost based on the discrete sampling space coordinates obtained in Step 2, the global coordinates of obstacles obtained in Step 3, the local global endpoint coordinates obtained in Step 4, and the local planning endpoint obtained in Step 5. Use the analytic hierarchy process (AHP) to perform a weighted summation of each cost function value.

[0133] Specifically, a cost function is defined based on the discrete sampling space and obstacle information, and the weighted summation of multiple cost function values ​​is performed using the analytic hierarchy process (AHP). These multiple cost functions include obstacle avoidance cost, convergence to local / global endpoint cost, and path length cost.

[0134] Obstacle avoidance cost refers to the cost of avoiding obstacles, and it measures the degree to which obstacles affect the selection of discrete sampling points. A larger value indicates a more dangerous discrete sampling point, while a smaller value indicates a safer discrete sampling point. The closer to the obstacle, the greater the cost; the farther away from the obstacle, the smaller the cost. The obstacle avoidance cost can be calculated using the following formula.

[0135]

[0136] In the formula, d obs Represented as the distance between discrete sampling points and obstacles

[0137]

[0138] obs X obs Y obs Z These represent the position coordinates of the detected obstacles relative to the global coordinate system; Represents the relative global coordinate system O XYZ The waypoint coordinates in the discrete sampling space, where i = 1, 2, ..., ln; j = 1, 2, ..., wn; k = 1, 2, ..., hn;

[0139] The cost of approaching the local global endpoint refers to the cost relative to the distance to the local global endpoint. It is expressed as a function of the distance between the discrete sampling point and the target endpoint. In practical terms, it represents the degree to which the discrete sampling point tends to reach the local global endpoint. The cost of approaching the local global endpoint can be calculated using the following formula.

[0140]

[0141] In the formula, d goal This is expressed as the distance between discrete sampling points and the target endpoint;

[0142]

[0143] llg X ,llg Y ,llg Z The local_global_goal is the position coordinate of the robot's local global endpoint relative to the global coordinate system; Let i represent the waypoint coordinates in the discrete sampling space relative to the global coordinate system, where i = 1, 2, ..., ln; j = 1, 2, ..., wn; k = 1, 2, ..., hn;

[0144] d max2 This represents the farthest distance between the current planning starting point and the local / global endpoint.

[0145]

[0146] In the formula, pos X ,pos Y ,pos Z This represents the robot's spatial position coordinates relative to the global coordinate system.

[0147] Path length cost refers to the cost relative to the distance from the current planning starting point. It is expressed as a function of the distance between the discrete sampling point and the current planning starting point. In practical terms, it represents the length of the locally planned path. The path length cost can be calculated using the following formula.

[0148]

[0149] In the formula, d start Represented as the distance between discrete sampling points and the current planning starting point.

[0150]

[0151] Current planning starting point pos X ,pos Y ,pos Z This represents the robot's spatial position coordinates relative to the global coordinate system; Let i represent the waypoint coordinates in the discrete sampling space relative to the global coordinate system, where i = 1, 2, ..., ln; j = 1, 2, ..., wn; k = 1, 2, ..., hn.

[0152] d max3 The maximum distance between a discrete sampling point and the current planning starting point can be obtained from the construction parameters of the sampling space, as shown below.

[0153]

[0154] In the formula, W represents the lateral sampling distance of the three-dimensional discrete sampling space of the robot waypoint; Dx represents the longitudinal sampling interval of the three-dimensional discrete sampling space of the robot waypoint; and H represents the vertical sampling distance of the three-dimensional discrete sampling space of the robot waypoint.

[0155] The total cost function, cost, can be expressed as a weighted sum of values.

[0156] cost = ω1g1 + ω2g2 + ω2g2

[0157] Where ω1 represents the weight of obstacle avoidance cost; ω2 represents the weight of the cost of approaching the local global endpoint; and ω3 represents the weight of the path length cost.

[0158] A hierarchical analysis structure model is established based on the analytic hierarchy process (AHP) to determine the corresponding weights of several cost values.

[0159] The Analytic Hierarchy Process (AHP) first establishes a hierarchical structure model, based on the comparison scale reference values, as shown in the table below:

[0160] Scale reference value of the analytic hierarchy process

[0161]

[0162] For the three types of influencing factors of the total cost function: 1-obstacle avoidance cost, 2-cost of approaching the local / global endpoint, and 3-path length cost, the judgment matrix is ​​constructed as follows:

[0163]

[0164] This means that, at the subjective level, it is believed that...

[0165] (1) The cost of obstacle avoidance is slightly more important than the cost of approaching the local global endpoint;

[0166] (2) The cost of obstacle avoidance is more important than the path length;

[0167] (3) The cost of approaching the local global endpoint is slightly more important than the cost of path length;

[0168] To ensure the logical consistency of the results from the analytic hierarchy process (AHP), a consistency check is required. If the judgment matrix A is a perfectly consistent pairwise comparison matrix, it should satisfy the following condition:

[0169] a ij a jk =a ik ,1≤i,j,k≤n

[0170] However, it is practically impossible to satisfy all the above equations when constructing pairwise comparison matrices. Therefore, a less desirable requirement is that the pairwise comparison matrices have consistency, meaning that some inconsistencies are permissible. Analysis shows that for a completely consistent pairwise comparison matrix, the eigenvalue with the largest absolute value is equal to the dimension of the matrix. Therefore, the consistency requirement for pairwise comparison matrices is transformed into requiring the eigenvalue with the largest absolute value.

[0171] The dimensions of these matrices are not significantly different.

[0172] The consistency test formula for the analytic hierarchy process is as follows:

[0173]

[0174] Wherein, CR is the consistency ratio, and it is generally considered that the consistency of the judgment matrix is ​​acceptable when CR < 0.1; CI is the consistency index. RI stands for Random Consistency Index. Generally, the smaller the CR value, the better the consistency of the judgment matrix. Typically, a CR value less than 0.1 indicates that the judgment matrix meets the consistency test; if the CR value is greater than 0.1, it indicates a lack of consistency, and the judgment matrix should be adjusted appropriately before re-analysis.

[0175] The CI value calculated for the 3rd order judgment matrix is ​​0.019, and the RI value is 0.520 according to the table. Therefore, the CR value is 0.037 < 0.1, which means that the judgment matrix satisfies the consistency test, the calculated weights are consistent, and the weight settings are reasonable.

[0176] The judgment matrix A is normalized by column and the arithmetic mean is taken by row to obtain the weights ω1 = 0.634, ω2 = 0.260, and ω3 = 0.106.

[0177] That is, the total cost function can ultimately be expressed as:

[0178] cost=0.634g1+0.260g2+0.106g2

[0179] To more intuitively illustrate the definitions of several cost functions, Figure 5 This is a schematic diagram illustrating the obstacle avoidance cost in an embodiment of the present invention; Figure 6 This is a schematic diagram illustrating the tendency towards a local global endpoint cost in the implementation of this invention; Figure 7 This is a schematic diagram illustrating the path length cost in an embodiment of the present invention.

[0180] Step 7: Calculate the cost between waypoints in each layer of the discrete sampling space obtained in Step 6, and use dynamic programming algorithm to find the reference waypoints in each layer of the discrete sampling space that minimize the cost from the current position to the local planning endpoint.

[0181] Specifically, in step 6, the cost between waypoints at each layer of the discrete sampling space is calculated. Based on the dynamic programming algorithm, the reference waypoints in each layer of the discrete sampling space with the minimum cost from the current position to the local planning endpoint are found. Then, all reference waypoints to the local endpoint in the sampling space are backtracked and used as the solution result of the local path planning.

[0182] Based on the discrete sampling space of waypoints established in the previous section, the minimum cost path is solved for each sampling layer in the vertical direction, and finally the set of waypoints with the minimum total agency value is found as the reference waypoints.

[0183] The dynamic programming state is defined by defining a cost matrix costM, where each value can be calculated using a cost function. Therefore, costM(j,i) represents the cost from node j to node i. The minimum path cost vector pathcost for each waypoint is defined, with the starting point assigned a value of 0 and the rest initialized with the maximum value.

[0184] The state transition equation for dynamic programming, for the path from node j to node i, is as follows: if the minimum path cost of node j (pathcost(j)) and the cost M(j,i) from node j to node i are both less than the minimum path cost of node i (pathcost(i), then the minimum path cost of node i is updated. The state transition equation for updating the minimum path cost of node i is as follows:

[0185] pathcost(i)=pathcost(j)+costM(j,i)

[0186] Define a predecessor node vector `prenodeidx`. For all nodes, initialize it with the starting index. When node `i` updates its state, also update the predecessor nodes of node `i`.

[0187] prenodeidx(i)=j

[0188] After the dynamic programming solution is completed, the robot backtracks from the local planning endpoint to the planning starting point using the predecessor node vector `prenodeidx`, based on the backtracking algorithm. Then, it sequentially connects the robot's current starting position to the reference waypoints at each layer. Path smoothing is performed using curved path construction to finally obtain the robot's reference path within the current planning cycle, thus completing the local path planning. To more intuitively illustrate the discrete sampling space, sonar obstacle perception, and reference waypoint solutions, Figure 8 This is a schematic diagram of the reference waypoint results for local path planning in the implementation of this invention.

[0189] Step 8: Connect the reference waypoints obtained in Step 7 using curve construction to construct the path and plan the trajectory.

[0190] Specifically, the path construction and trajectory planning are performed by connecting the reference waypoints obtained in step 7 through curve construction. A suitable curve construction method is selected, such as polynomial curves, Bézier curves, etc., and a smooth path is constructed for the reference waypoints in sequence and trajectory planning is performed, including position information, attitude information, three-axis velocity information, three-axis acceleration information, three-axis angular velocity information, and three-axis angular acceleration information in a certain time period. The planned trajectory results are sent to the trajectory tracking controller for trajectory tracking control. Figure 8The distance also shows the path result constructed based on the reference waypoints obtained in step 7.

[0191] The above are merely preferred embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A local path planning method for an underwater autonomous robot, characterized in that, Includes the following steps: S1: Obtain the three-dimensional global reference path using the global path planning A* algorithm; S2: Based on the positioning and attitude quaternion information of the underwater autonomous robot, set the spatial sampling parameters and establish a waypoint discrete sampling space based on the robot's global coordinate system; S3: Transform the obstacle position coordinates sensed by the sonar into coordinates relative to the robot's global coordinate system using the robot's current position coordinates and attitude quaternions; S4: Based on the current robot's positioning coordinates, find the current global path matching point on the global reference path obtained in step S1, and find the local global endpoint from the position of the discrete sampling space established in step S2. S5: Perform layered processing on the discrete sampling space established in step S2, filter the last layer of sampling space points, and find the local planning endpoint that is closest to the local global endpoint in step S4 according to the proximity principle. S6: Based on the discrete sampling space coordinates of step S2, the global coordinates of obstacles sensed by the sonar in step S3, the local and global endpoint coordinates obtained in step S4, and the local planning endpoint obtained in step S5, define the obstacle avoidance cost function, the cost function for reaching the local and global endpoint, and the path length cost function. Use the analytic hierarchy process (AHP) to perform a weighted summation of the cost function values. S7: Calculate the cost between waypoints in each layer of the discrete sampling space, that is, calculate the value of the weighted sum of the cost function values ​​in step S6, and solve the reference waypoints in each layer of the discrete sampling space with the minimum cost from the current position to the local planning endpoint based on the dynamic programming algorithm. S8: Local path planning is performed by constructing a path that connects reference waypoints using curves.

2. The method according to claim 1, characterized in that, The spatial sampling parameters set in step S2 include: longitudinal sampling distance L, number of longitudinal sampling points ln, longitudinal sampling interval Dx, lateral sampling distance W, number of lateral sampling points wn, lateral sampling interval Dy, vertical sampling distance H, number of vertical sampling points hn, and vertical sampling interval Dz.

3. The method according to claim 2, characterized in that, Step S2, which establishes the waypoint discrete sampling space based on the robot's global coordinate system, specifically involves: Based on the relationship between line segments and points, that is, for n points there are n-1 line segments, the longitudinal sampling parameters satisfy the following relationship: , Similarly, the following relationship applies to the lateral sampling parameters: , Similarly, the vertical sampling parameters satisfy the following relationship: , This indicates the longitudinal distance from the robot's current planned starting point to the sampling space, and is set as follows: ; The position coordinates of the waypoint discrete sampling space relative to the robot coordinate system are denoted as follows: , , ; The longitudinal sampling interval Dx and the longitudinal distance from the robot's current position to the sampling space are used to determine the distance. Represented as ， , Obtained from the transverse sampling interval Dy ， , Similarly Obtained from the vertical sampling interval Dz ， , Meanwhile, the points in this discrete sampling space , , Transform from the robot coordinate system to the global coordinate system.

4. The method according to claim 3, characterized in that, Step S3 is as follows: The transformation relationship between the robot's position coordinates in the robot coordinate system and its position coordinates in the global coordinate system is obtained from the robot's pose quaternions, as shown in the following formula. , In the above formula, A pure imaginary quaternion representing the coordinates of obstacles relative to the global coordinate system is denoted as . ; , , These represent the position coordinates of the detected obstacles relative to the global coordinate system; The unit quaternion representing the robot's current pose is denoted as . ; A pure imaginary quaternion, denoted as , is formed by the coordinates of waypoints in the discrete sampling space relative to the robot's coordinate system. ; , , These represent the position coordinates of the detected obstacle relative to the robot's coordinate system; The inverse quaternion representing the robot's current position and pose is denoted as . ; Let the imaginary quaternion formed by the robot's current position be denoted as . ,in , , This represents the robot's spatial position coordinates relative to the global coordinate system.

5. The method according to claim 4, characterized in that, Step S5 specifically includes the following steps: S51: Search layer for determining the endpoint of local programming; Determine the search height of the last layer in the sampling space, starting from the current planning starting point. and local and global endpoints height difference Determine the endpoint of local planning The sampling height location; , Based on height difference The symbols are used to filter the search range of the last layer of the sampling space; S52: Determine the selection range for the local planning endpoint; S53: Determine the local planning endpoint based on the principle of proximity.

6. The method according to claim 5, characterized in that, The obstacle avoidance cost function defined in step S6 is as follows: , In the formula, Represented as the distance between discrete sampling points and obstacles , , , These represent the position coordinates of the detected obstacles relative to the global coordinate system; , , Represents the relative global coordinate system O XYZ The waypoint coordinates in the discrete sampling space, where ; ; ; The defined cost function for the tendency towards the local global endpoint is as follows: , In the formula, This is expressed as the distance between discrete sampling points and the target endpoint; , , , For the robot's local and global endpoints Position coordinates relative to the global coordinate system; , , Represents the waypoint coordinates in the discrete sampling space relative to the global coordinate system, where ; ; ; This represents the farthest distance between the current planning starting point and the local / global endpoint. , In the formula, , , This represents the robot's spatial position coordinates relative to the global coordinate system; The defined path length cost function is as follows: , In the formula, Represented as the distance between discrete sampling points and the current planning starting point. , Current planning starting point , , This represents the robot's spatial position coordinates relative to the global coordinate system; , , Represents the waypoint coordinates in the discrete sampling space relative to the global coordinate system, where ; ; ; This represents the maximum distance of a discrete sampling point from the current planning starting point, obtained from the construction parameters of the sampling space, as shown below. , In the formula, W represents the lateral sampling distance of the robot waypoint in the three-dimensional discrete sampling space; Dx represents the longitudinal sampling interval of the robot waypoint in the three-dimensional discrete sampling space; and H represents the vertical sampling distance of the robot waypoint in the three-dimensional discrete sampling space. Total cost function Weighted summation can be expressed as: , in, The weight representing the cost of obstacle avoidance; The weight representing the cost of approaching the local global endpoint; The weights represent the path length cost.

Images