Amphibious unmanned platform path planning method based on sonar imaging

Abstract

The invention relates to an amphibious unmanned platform path planning method based on sonar imaging. Comprising the following steps: performing color space conversion on sonar imaging; filtering is carried out through morphological opening operation, obstacle geometry is obtained, and the shape and coordinate position of an obstacle are simulated by adopting a circumcircle method according to the obstacle geometry; performing sonar frequency band judgment, and performing global map processing on a high frequency band; performing global path search by adopting an RRT algorithm according to the positioning information and the target point information; the searched global path is optimized, a reference line is obtained, and a frenet coordinate system is obtained; longitudinal speed sampling is carried out based on the coordinate system, and a quartic polynomial is used for fitting; sampling the transverse coordinates, and fitting a sampling path by adopting a quintic polynomial; and selecting an optimal sampling path according to the local map and the cost function of the high-frequency band, and discretizing to obtain a reference airline array. The method gets rid of the high dependence of an existing amphibious robot on a prior map, optimizes the smoothness of a search path, and is simple and efficient.

Description

Technical Field

[0001] The invention belongs to the field of underwater robot path planning, and in particular relates to an amphibious unmanned platform path planning method based on sonar imaging. Background Art

[0002] An amphibious unmanned platform is a robot that can complete specific tasks in an underwater environment. It has high application value in fisheries, underwater exploration, and military industry. However, it is limited by the need for a priori high-precision underwater maps and the complex and changeable characteristics of the underwater environment. Autonomous navigation and autonomous obstacle avoidance have always been major challenges for amphibious platforms in their application. Real-time path planning through switching between high and low frequency bands of sonar plays a very important role in solving the high dependence on global maps, improving the movement and obstacle avoidance capabilities of amphibious platforms, and ensuring the safety of amphibious platforms during underwater operations.

[0003] Most traditional underwater robots detect obstacles based on vision, sonar and other sensors. Current underwater autonomous navigation algorithms for robots include the dynamic window method and the artificial potential field method. Although these methods can meet basic obstacle avoidance requirements, there are still many problems. In particular, when traveling at high speeds, they often rely only on local path planning for obstacle avoidance without a priori maps. Due to the lack of prior path references, they often cannot avoid obstacles in a timely and effective manner when facing large obstacles. At the same time, due to the lack of a global map, the above algorithms can only avoid obstacles in a small area, which may lead to a high probability of sudden deceleration. The robot's dynamics or kinematic characteristics are not well considered, and the planned path may not allow the robot to track effectively. Summary of the invention

[0004] The purpose of the present invention is to provide a path planning method for an amphibious unmanned platform based on sonar imaging.

[0005] The technical solution to achieve the purpose of the present invention is: a path planning method for an amphibious unmanned platform based on sonar imaging, comprising the following steps:

[0006] Step (1): perform sonar imaging of the environment according to the sonar low-frequency band and high-frequency band working modes, identify the highlighted areas as obstacles, and obtain the original global map and original local map of the environment where the amphibious unmanned platform is located according to the sonar imaging;

[0007] Step (2): Based on the original global map and the original local map in step (1), a morphological algorithm is used for filtering, and then the obstacles are simulated by a circumscribed circle method to obtain a filtered global map and a filtered local map;

[0008] Step (3): Based on the filtered global map obtained in step (2), the RRT algorithm is used to search the global path to obtain a rough solution of the global path;

[0009] Step (4): using a two-dimensional cubic spline interpolation method to optimize the rough solution of the global path searched by the RRT algorithm to obtain an optimized global path;

[0010] Step (5): Based on the optimized global path obtained in step (4), according to the local map filtered in step (2), the time is sampled m times and the lateral displacement L is sampled n times to obtain m*n longitudinal-time-lateral three-dimensional paths of STL, and then the time cost, lateral displacement cost, collision cost, and acceleration sudden change cost are weighted with corresponding weights respectively, and the optimal path is selected according to the weighted cost function.

[0011] Furthermore, step (2) specifically includes the following steps:

[0012] Step (21): converting the sonar image into an HSV color space image;

[0013] Step (22): according to the HSV color space image obtained in step (21), upper and lower thresholds are set, and the brightness area is filtered using the threshold method to obtain a rough map solution;

[0014] Step (23): The morphological opening method is used to eliminate the noise points that remain due to hardware defects and cannot be completely eliminated by the threshold method. The calculation formula is:

[0015]

[0016] f is the input image, B is the structural element, For corrosion operations, For expansion operation;

[0017] Step (24): Find obstacles by tracing pixel boundaries;

[0018] Step (25): After searching all boundaries, call the minEnclosingCircle function of OpenCV to calculate and return the center and radius of the minimum circumscribed circle of the obstacle geometry. Multiply the returned circumscribed circle radius by the coefficient k, and finally return the center coordinates and radius of all obtained circumscribed circles to obtain the filtered global map.

[0019] Furthermore, step (21) is specifically as follows:

[0020] 1. V = max (R, G, B)

[0021] 2.C min=min(R,G,B)

[0022]

[0023] 4. C = VC min

[0024]

[0025] Among them: H∈[0,360], S∈[0,1], V∈[0,1], R, G, B are the values ​​of the three primary colors of red, green and blue, respectively, ranging from [0-1], V is the brightness, S is the saturation, C is the difference of the color components, and H is the hue.

[0026] Furthermore, step (24) is specifically as follows:

[0027] Initialize the current node as the starting node and the current direction as 0: current_point = start_point, direction = 0, and then for each boundary point current_point and direction direction, calculate the next candidate point new_point:

[0028] new_point=(x+dx i ,y+dy i )where(dx i ,dy i )∈directions

[0029] If the current node is not the starting point, update the current node and direction:

[0030] current_point=new_point

[0031] direction=(new_direction+5)%8

[0032] Stop until you return to the starting point or do not find the next boundary point; get the obstacle geometry.

[0033] Furthermore, step (3) is specifically as follows:

[0034] Step (31): obtaining the current coordinate position according to the fused positioning information of the amphibious platform inertial navigation and the Doppler log DVL, and setting the end position;

[0035] Step (32): The filtered global map obtained in step (22) starts from the starting point, recorded as the parent node, takes the end point sampling rate f and randomly scatters points in the map space, recording the angle between the random point and its parent node as θ, takes the parent node as the reference node, takes the step size step, and starts to diffuse the child nodes with θ as the growth direction, traverses and calculates the coordinate center point of the obstacle and the distance l between the child nodes. If l>r, the generated child node connection is judged for collision, and the distance l1 from the center point to the straight line where the parent and child nodes are located is calculated. If l1>r, a child node is generated and updated as the parent node until the end point is searched. Finally, trace back from the end point to the parent node to find the searched path point, that is, the global coordinate point.

[0036] Furthermore, step (4) is specifically as follows:

[0037] Step (41): For the coordinate points searched in step (3), assuming that there are 7 (n+1) coordinate points, sample every seven coordinate points. If there are less than seven, sample once and finally obtain n+1 coordinate points. If every two coordinate points are regarded as one interval, there are n intervals in total. Each interval [x i , x i+1 ] are all composed of the cubic polynomial y = a i +b i x+c i x 2 +d i x 3 The polynomial is continuous at the endpoints and its first and second derivatives are continuous. There are n intervals in total, and 4n unknowns need to be solved. Except for the two endpoints, each of the n-1 internal points satisfies the first and last piecewise cubic equations, so there are 2(n-1) boundary conditions. In addition, the two endpoints satisfy the first and last cubic equations respectively, so there are 2n boundary conditions in total. Secondly, S′ i (x i+1 ) = S′ i+1 (x i+1 ) A total of n-1 boundary conditions S″ i (x i+1 )=S″ i+1 (x i+1 ) There are n-1 boundary conditions in total, plus the natural boundary S″(x0)=0=S″(x n ) There are 4n boundary conditions in total, and the final tridiagonal matrix is:

[0038]

[0039] Step (42): Solve the tridiagonal matrix to obtain a one-dimensional cubic spline curve.

[0040] Furthermore, step (5) is specifically as follows:

[0041] Step (51): Longitudinal sampling: define t as sampling time, initial position xs along the s direction, initial velocity vxs in the s direction, initial acceleration axs in the s direction, and the desired terminal velocity vxe and terminal acceleration axe along the s axis at time t, thereby obtaining the quartic polynomial f(t)=a0+a1*t+a2*t 2 +a3*t 3 +a4*t 4 , where f′(0) is a0 equal to xs, f″(0) is a1 equal to vxs, f′(t) is the expected speed vxe at time t, and f″(t) is the expected acceleration axe at time t. Finally, a quartic polynomial is solved, which satisfies the planned longitudinal speed, acceleration, and jerk continuity;

[0042] Step (52): Horizontal sampling: According to the known sampling points in the s direction, each sampling point takes a different longitudinal sampling step length, and t is defined as the sampling time, the initial position xl along the l direction, the initial velocity vxl in the l direction, the initial acceleration axl in the l direction, and the expected terminal velocity vxe and terminal acceleration axe along the l axis at time t, thereby obtaining the fifth-order polynomial f(t) = a0+a1*t+a2*t 2 +a3*t 3 +a4*t 4 +a5*t 5 , where f(0) is the starting l-axis coordinate, f′(0) is a0 in the l1 direction, equal to xs, f″(0) is a1 equal to vxs, f(t) is the l-axis coordinate at time t, f′(t) is the expected speed vxe at time t, f″(t) is the expected acceleration axe at time t, and finally a fifth-order polynomial is solved, which satisfies the continuity of the robot's horizontal and vertical speed, acceleration, and jerk during motion;

[0043] Step (53): Design a cost function to evaluate the paths sampled in steps (51) and (52), and select the optimal solution as the optimal path.

[0044] Furthermore, step (53) is specifically as follows:

[0045] Step (531): Calculate the time cost: time_cost = K_TIME*t1;

[0046] Step (532): Calculate the collision cost: collision_cost = K_COLLISION * is_collision, where is_collision is a bool value of 0 or 1, which specifically means that if a collision occurs, the cost of the path is extremely high and it is not selected;

[0047] Step (533): Calculate the lateral offset cost:

[0048] offset_cost=K_OFFSET*abs(path_pre.l) / 2;

[0049] Step (534): Calculate the acceleration sudden change cost:

[0050] jerk_cos t=KJERK*(l_jerk_sum+s_jerk_sum)

[0051] in:

[0052]

[0053] Step (535): Calculate the total cost of the current path:

[0054] path_pre.cost=K_COLLISION*is_collision+K_JERK*(l_jerk_sum+s_jerk_sum)+K_TIME*t1*K_OFFSET*abs(path_pre.l) / 2

[0055] Step (536): Select the path with the smallest cost among all sampling points, which is the optimal path.

[0056] Compared with the prior art, the present invention has the following significant advantages:

[0057] (1) The present invention designs a path planning method for an amphibious unmanned platform based on front-end multi-beam sonar imaging and discrete space sampling. In the absence of prior map information, the method uses the low-frequency band of sonar to obtain global map information, preliminarily solves the sampling space, and then performs effective autonomous navigation and obstacle avoidance based on the sampling space, thus getting rid of the high dependence of the global planning of underwater autonomous robots on prior maps.

[0058] (2) When optimizing the global path searched by the RRT algorithm, the cubic spline interpolation method for x and y is only effective when x is increasing. If x suddenly decreases during the path search, re-planning is required and then splicing is performed. This method not only consumes a lot of computing power, but also causes the path at the splicing point to not conform to the kinematic constraints of the robot. Therefore, this method proposes a two-dimensional cubic spline interpolation method. First, the distance u between the two coordinate points is calculated, and then ux and uy are interpolated respectively. Since u cannot be a negative number, u will not decrease when the curve is optimized, which effectively avoids the limitations of traditional cubic spline interpolation.

[0059] (3) Compared with the classic artificial potential field method and DWA algorithm, the algorithm of the present invention makes up for the defect of not being able to meet the kinematic constraints of the robot well. Aiming at the situation that the speed change of the robot is not smooth during the process of high-speed underwater movement and obstacle avoidance, the algorithm adds acceleration planning and uses a fifth-order polynomial for fitting to make the path smoother. It is more suitable for path planning of underwater autonomous robots during high-speed movement and can be used as a path planning method for amphibious platforms. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] Figure 1 A flow chart of the path planning method for an amphibious unmanned platform based on sonar imaging provided by the present invention;

[0061] Figure 2 It is the original data graph of the sonar imaging of the present invention;

[0062] Figure 3 This is a post-processing result diagram of the obstacle area imaged by sonar according to the present invention;

[0063] Figure 4 It is a comparison diagram of the cubic spline curve and the two-dimensional cubic spline curve fitting of the present invention;

[0064] Figure 5 This is a schematic diagram of the global path search and optimization results of the present invention;

[0065] Figure 6 This is the final effect diagram of the present invention. DETAILED DESCRIPTION

[0066] The present invention is further described in detail below in conjunction with the accompanying drawings.

[0067] The present invention is based on a path planning method of front multi-beam sonar imaging, uses the low frequency band of sonar to obtain rough global map information, and uses it for global path search to avoid large obstacles and provide reference lines for subsequent local obstacle avoidance. Based on the searched global path, a two-dimensional cubic spline curve is used for optimization and a frenet coordinate system is established. Local path sampling is performed based on this coordinate system, and the low frequency band is switched during the movement process to update the obstacle information. If the sampled path collides with the local obstacle, its cost is set to a maximum value to achieve the purpose of obstacle avoidance. At the same time, the cost function involves a speed sudden change cost, a time cost, and a lateral offset cost in addition to the collision cost. The four costs jointly judge the quality of the sampled path, select the optimal path curve for tracking, and finally solve the speed, acceleration, and trajectory change of the amphibious platform kinematic characteristics. The safe obstacle avoidance path with smoothness.

[0068] like Figure 1 The present invention provides a path planning method for an amphibious unmanned platform based on sonar imaging, which specifically includes the following steps:

[0069] Step 1: Sonar imaging of the environment is performed according to the sonar low-frequency band and high-frequency band working modes, and the highlighted areas are identified as obstacles. The original global map and original local map of the environment where the amphibious unmanned platform is located are obtained based on the sonar imaging;

[0070] Step 2: Based on the original global map and the original local map in step (1), the morphological algorithm is used to filter them, and then the obstacles are simulated by the circumscribed circle method to obtain the filtered global map and the filtered local map;

[0071] Specifically, the sonar imaging information is first converted into the HSV color space:

[0072] 1. V = max (R, G, B)

[0073] 2.C min =min(R,G,B)

[0074]

[0075] 4. C = VC min

[0076]

[0077] Among them: H∈[0,360], S∈[0,1], V∈[0,1], R, G, B are the values ​​of the three primary colors of red, green and blue, respectively, ranging from [0-1], V is the brightness, S is the saturation, C is the difference of the color components, and H is the hue.

[0078] Then, the upper and lower thresholds are set to (30, 255, 255) and (0, 100, 100) respectively, and the threshold method is used to filter the brightness area. The morphological opening method is used to eliminate the remaining noise points that cannot be completely eliminated by the threshold method due to hardware defects. The calculation formula is:

[0079]

[0080] f is the input image, B is the structural element, For corrosion operations, For expansion operation;

[0081] Find obstacles by tracing pixel boundaries: first initialize the current node as the starting node and the current direction as 0: current_point = start_point, direction = 0, then for each boundary point current_point and direction direction, calculate the next candidate point new_point:

[0082] new_point=(x+dx i ,y+dy i )where(dx i ,dy i )∈directions

[0083] If the current node is not the starting point, update the current node and direction:

[0084] current_point=new_point

[0085] direction=(new_direction+5)%8

[0086] It stops when it returns to the starting point or fails to find the next boundary point, and obtains the obstacle geometry;

[0087] After searching all boundaries, call the opencv minEnclosingCircle function to calculate and return the center and radius of the minimum circumscribed circle of the obstacle, such as Figure 2 Figure 3 As shown, for the returned circumscribed circle radius, in order to ensure the accuracy of obstacle avoidance, it is multiplied by a coefficient of 1.15 to ensure that enough collision space is reserved, and finally the center coordinates and radius of all obtained circumscribed circles are returned, so that the global map can be obtained.

[0088] Step 3: By subscribing to the positioning information and the location of the end point, we have the starting point and end point to be searched. According to the global map information obtained above, including the coordinate center of the obstacle, the radius r of the circumscribed circle and other information, we start from the starting point, recorded as the parent node, and take the end point sampling rate k (that is, there is a probability of k / 100) to directly sample the end point) and then randomly scatter points in the map space. The angle between the random point and its parent node is θ. The parent node is taken as the reference node, a certain step length step is taken, and the child nodes are diffused with θ as the growth direction. The coordinate center point of the obstacle and the distance between the child nodes are traversed and calculated. If 1>r, the generated child node connection is collided and judged. The distance l1 from the center point to the straight line where the parent and child nodes are located is calculated. If l1>r, a child node is generated and updated as the parent node until the end point is searched. Finally, trace back from the end point to the parent node to find the searched path point, that is, the global coordinate point.

[0089] Step 4: For the coordinate points searched in step 3 (assuming there are 7(n+1) points), sample every seven coordinate points. If there are less than seven, sample once and finally get n+1 coordinate points. If every two coordinate points are regarded as one interval, there are n intervals in total. Each interval [x i , x i+1 ] are all composed of the cubic polynomial y = a i +bi x+c i x 2 +d i x 3 The polynomial is continuous at the endpoints and its first and second derivatives are continuous. There are n intervals in total, and 4n unknowns need to be solved. Except for the two endpoints, each of the n-1 internal points satisfies the first and last piecewise cubic equations, so there are 2(n-1) boundary conditions. In addition, the two endpoints satisfy the first and last cubic equations respectively, so there are 2n boundary conditions in total. Secondly, S′ i (x i+1 ) = S′ i+1 (x i+1 ) A total of n-1 boundary conditions S″ i (x i+1 )=S″ i+1 (x i+1 ) There are n-1 boundary conditions in total, plus the natural boundary S″(x0)=0=S″(x n ) There are 4n boundary conditions in total, and finally the tridiagonal matrix is ​​obtained:

[0090]

[0091] Solving the tridiagonal matrix will yield a one-dimensional cubic spline curve. Assuming the distance between two coordinate points is u, and performing cubic spline interpolation on uX and uy, respectively, we can get a two-dimensional cubic spline curve UXY. This method is smoother at the junction of two segments than directly taking the two-dimensional cubic spline curve of XY. Figure 4 To compare the interpolation of x and y directly and the interpolation of ux and uy separately, Figure 5 The original path searched for RRT and the global path obtained after interpolation.

[0092] Step 5: Based on the global map obtained in the above steps, subscribe to the local map message. The specific processing method is the same as step 2. The difference is that the sonar is in high-frequency working mode at this time, so it will not be repeated. According to the global path obtained in step 4, the horizontal and vertical directions are sampled respectively. The specific method is as follows:

[0093] First, longitudinal sampling is performed: define t as the sampling time, the initial position xs, the initial velocity vxs in the s direction, and the initial acceleration axs in the s direction as the current position, velocity, and acceleration along the s direction, respectively. The terminal velocity vxe and terminal acceleration axe are the expected velocity and acceleration along the s axis at time t, respectively. From this, the fourth-order polynomial f(t) = a0+a1*t+a2*t can be obtained. 2 +a3*t 3 +a4*t 4, where f′(0) is a0 equal to xs, f″(0) is a1 equal to vxs, f′(t) is the expected velocity vxe at time t, and f″(t) is the expected acceleration axe at time t. Finally, a quartic polynomial is solved, which satisfies the planned longitudinal velocity, acceleration, and jerk continuity.

[0094] Then, transverse sampling is performed based on the longitudinal sampling: sampling points in the s direction, each sampling point takes a different longitudinal sampling step (odd number), t is defined as the sampling time, the initial position xl, the initial velocity vxl in the l direction, the initial acceleration axl in the l direction are respectively the current position, velocity, and acceleration along the l direction, the terminal velocity vxe and the terminal acceleration axe are respectively the expected velocity and acceleration along the l axis at time t, thus the quintic polynomial f(t) = a0+a1*t+a2*t 2 +a3*t 3 +a4*t 4 +a5*t 5 Among them, f(0) is the starting l-axis coordinate, f′(0) is the l-direction a0 equals xs, a1 equals vxs, is the l-axis coordinate at time t, is the expected speed vxe at time t, is the expected acceleration axe at time t, and finally a fifth-order polynomial is solved. This polynomial satisfies the continuity of the robot's horizontal and vertical speed, acceleration, and jerk during movement.

[0095] After obtaining the sampling path, each path needs to be evaluated, mainly including acceleration sudden change cost, collision cost, lateral offset cost, and time cost. The calculation formula of each cost is:

[0096] Time cost: time_cost = K_TIME*t1;

[0097] Collision cost: collision_cost = K_COLLISION * is_collision;

[0098] Where is_collision is a bool value of 0 or 1, which means that if a collision occurs, the path is extremely costly and will not be chosen;

[0099] Lateral offset cost: offset_cost = K_OFFSET*abs(path_pre.l) / 2;

[0100] Acceleration jerk cost: jerk_cost = KJERK*(l_jerk_sum + s_jerk_sum);

[0101] in:

[0102]

[0103] Calculate the total cost of each path:

[0104] path_pre.cost=K_COLLISION*is_collision+K_JERK*(l_jerk_svm+s_jerk_sum)+K_TIME*t1*K_OFFSET*abs(path_pre.l) / 2

[0105] Select the sampling path with the minimum cost, and the final planning effect is as follows Figure 6 .

Claims

1. A path planning method for an amphibious unmanned platform based on sonar imaging, characterized in that: The steps include: Step (1): perform sonar imaging of the environment according to the sonar low-frequency band and high-frequency band working modes, identify the highlighted areas as obstacles, and obtain the original global map and original local map of the environment where the amphibious unmanned platform is located according to the sonar imaging; Step (2): Based on the original global map and the original local map in step (1), a morphological algorithm is used for filtering, and then the obstacles are simulated by a circumscribed circle method to obtain a filtered global map and a filtered local map; Step (3): Based on the filtered global map obtained in step (2), the RRT algorithm is used to search the global path to obtain a rough solution of the global path; Step (4): using a two-dimensional cubic spline interpolation method to optimize the rough solution of the global path searched by the RRT algorithm to obtain an optimized global path; Step (5): Based on the optimized global path obtained in step (4), according to the local map filtered in step (2), the time is sampled m times and the lateral displacement L is sampled n times to obtain m*n longitudinal-time-lateral three-dimensional paths of STL, and then the time cost, lateral displacement cost, collision cost, and acceleration sudden change cost are weighted with corresponding weights respectively, and the optimal path is selected according to the weighted cost function.

2. The method according to claim 1, characterized in that Step (2) specifically includes the following steps: Step (21): converting the sonar image into an HSV color space image; Step (22): according to the HSV color space image obtained in step (21), upper and lower thresholds are set, and the brightness area is filtered using the threshold method to obtain a rough map solution; Step (23): The morphological opening method is used to eliminate the noise points that remain due to hardware defects and cannot be completely eliminated by the threshold method. The calculation formula is: f is the input image, B is the structural element, is the corrosion operation, ⊕ is the expansion operation; Step (24): Find obstacles by tracing pixel boundaries; Step (25): After searching all boundaries, call the minEnclosingCircle function of OpenCV to calculate and return the center and radius of the minimum circumscribed circle of the obstacle geometry. Multiply the returned circumscribed circle radius by the coefficient k, and finally return the center coordinates and radius of all obtained circumscribed circles to obtain the filtered global map.

3. The method according to claim 2, characterized in that Step (21) is specifically:

1. V = max (R, G, B) 2.C min =min(R,G,B) 4.C=V-C min Among them: H∈[0,360], S∈[0,1], V∈[0,1], R, G, B are the values ​​of the three primary colors of red, green and blue, respectively, ranging from [0-1], V is the brightness, S is the saturation, C is the difference of the color components, and H is the hue.

4. The method according to claim 3, characterized in that Step (24) is specifically: Initialize the current node as the starting node and the current direction as 0: current_point = start_point, direction = 0, and then for each boundary point current_point and direction direction, calculate the next candidate point new_point: new_point=(x+dx i ,y+dy i )where(dx i ,dy i )∈directions If the current node is not the starting point, update the current node and direction: current_point=new_point direction=(new_direction+5)%8 Stop until you return to the starting point or fail to find the next boundary point; Get the obstacle geometry.

5. The method according to claim 4, characterized in that Step (3) is specifically as follows: Step (31): obtaining the current coordinate position according to the fused positioning information of the amphibious platform inertial navigation and the Doppler log DVL, and setting the end position; Step (32): The filtered global map obtained in step (22) starts from the starting point, recorded as the parent node, takes the end point sampling rate f and randomly scatters points in the map space, recording the angle between the random point and its parent node as θ, takes the parent node as the reference node, takes the step size step, and starts to diffuse the child nodes with θ as the growth direction, traverses and calculates the coordinate center point of the obstacle and the distance l between the child nodes. If l>r, the generated child node connection is judged for collision, and the distance l1 from the center point to the straight line where the parent and child nodes are located is calculated. If l1>r, a child node is generated and updated as the parent node until the end point is searched. Finally, trace back from the end point to the parent node to find the searched path point, that is, the global coordinate point.

6. The method according to claim 5, characterized in that Step (4) is specifically as follows: Step (41): For the coordinate points searched in step (3), assuming that there are 7 (n+1) coordinate points, sample every seven coordinate points. If there are less than seven, sample once and finally obtain n+1 coordinate points. If every two coordinate points are regarded as one interval, there are n intervals in total. Each interval [x i ,x i+1 ] are all composed of the cubic polynomial y = a i +b i x+c i x 2 +d i x 3 The polynomial is continuous at the endpoints and its first and second derivatives are continuous. There are n intervals in total, and 4n unknowns need to be solved. Except for the two endpoints, each of the n-1 internal points satisfies the first and last piecewise cubic equations, so there are 2(n-1) boundary conditions. In addition, the two endpoints satisfy the first and last cubic equations respectively, so there are 2n boundary conditions in total. Secondly, S′ i (x i+1 ) = S′ i+1 (x i+1 ) A total of n-1 boundary conditions S″ i (x i+1 )=S″ i+1 (x i+1 ) There are n-1 boundary conditions in total, plus the natural boundary S″(x0)=0=S″(x n ) There are 4n boundary conditions in total, and the final tridiagonal matrix is: Step (42): Solve the tridiagonal matrix to obtain a one-dimensional cubic spline curve.

7. The method according to claim 6, characterized in that Step (5) is specifically as follows: Step (51): Longitudinal sampling: define t as sampling time, initial position xs along the s direction, initial velocity vxs in the s direction, initial acceleration axs in the s direction, and the desired terminal velocity vxe and terminal acceleration axe along the s axis at time t, thereby obtaining the quartic polynomial f(t)=a0+a1*t+a2*t 2 +a3*t 3 +a4*t 4 , where f′(0) is a0 equal to xs, f″(0) is a1 equal to vxs, f′(t) is the expected speed vxe at time t, and f″(t) is the expected acceleration axe at time t. Finally, a quartic polynomial is solved, which satisfies the planned longitudinal speed, acceleration, and jerk continuity; Step (52): Horizontal sampling: According to the known sampling points in the s direction, each sampling point takes a different longitudinal sampling step length, and t is defined as the sampling time, the initial position xl along the l direction, the initial velocity vxl in the l direction, the initial acceleration axl in the l direction, and the expected terminal velocity vxe and terminal acceleration axe along the l axis at time t, thereby obtaining the fifth-order polynomial f(t) = a0+a1*t+a2*t 2 +a3*t 3 +a4*t 4 +a5*t 5 , where f(0) is the starting l-axis coordinate, f′(0) is a0 in the l1 direction, equal to xs, f″(0) is a1 equal to vxs, f(t) is the l-axis coordinate at time t, f′(t) is the expected speed vxe at time t, f″(t) is the expected acceleration axe at time t, and finally a fifth-order polynomial is solved, which satisfies the continuity of the robot's horizontal and vertical speed, acceleration, and jerk during motion; Step (53): Design a cost function to evaluate the paths sampled in steps (51) and (52), and select the optimal solution as the optimal path.

8. The method according to claim 7, characterized in that Step (53) is specifically: Step (531): Calculate the time cost: time_cos t = K_TIME*t1; Step (532): Calculate the collision cost: collision_cos t = K_COLLISION*is_collision, where is_collision is a bool value of 0 or 1, which specifically means that if a collision occurs, the cost of the path is extremely high and is not selected; Step (533): Calculate the lateral offset cost: offset_cos t=K_OFFSET*abs(path_pre.l) / 2; Step (534): Calculate the acceleration sudden change cost: jerk_cost=K_JERK*(l_jerk_sum+s_jerk_sum) in: Step (535): Calculate the total cost of the current path: path_pre.cost=K_COLLISION*is_collision+K_JERK*(l_jerk_sum+s_jerk_sum)+K_TIME*t1*K_OFFSET*abs(path_pre.l) / 2 Step (536): Select the path with the smallest cost among all sampling points, which is the optimal path.

Images