A path planning method for amphibious unmanned platforms based on sonar imaging

Abstract

This invention discloses a path planning method for amphibious unmanned platforms based on sonar imaging. The method includes: color space conversion of the sonar image; filtering using morphological opening operations to obtain obstacle geometry; simulating the shape and coordinate position of obstacles using the circumcircle method based on the obstacle geometry; determining the sonar frequency band and performing global map processing on the high-frequency band; performing global path search using the RRT algorithm based on positioning and target point information; optimizing the searched global path, obtaining reference lines, and acquiring the Frenet coordinate system; sampling the longitudinal velocity using this coordinate system and fitting it with a fourth-order polynomial; sampling the lateral coordinates and fitting the sampled path using a fifth-order polynomial; selecting the optimal sampled path based on the local map and cost function in the high-frequency band, and obtaining a reference waypoint array after discretization. This invention eliminates the high dependence of existing amphibious robots on prior maps and optimizes the smoothness of the search path, offering simplicity and efficiency.

Description

Technical Field

[0001] This invention belongs to the field of underwater robot path planning, specifically relating to a path planning method for amphibious unmanned platforms based on sonar imaging. Background Technology

[0002] Amphibious unmanned platforms are robots capable of performing specific tasks in underwater environments. They have high application value in fisheries, underwater exploration, and military industries. However, due to the need for prior high-precision underwater maps and the complex and ever-changing nature of the underwater environment, autonomous navigation and obstacle avoidance have always been major challenges in the application of amphibious platforms. Real-time path planning through the switching of high and low frequency bands of sonar is of great importance in solving the high dependence on global maps, improving the mobility and obstacle avoidance capabilities of amphibious platforms, and ensuring the safety of amphibious platforms during underwater operations.

[0003] Traditional underwater robots mostly rely on sensors such as vision and sonar for obstacle detection. Current underwater autonomous navigation algorithms for robots include dynamic window methods and artificial potential field methods. Although these methods can meet basic obstacle avoidance requirements, they still have many problems. In particular, when moving at high speeds, they often rely on local path planning for obstacle avoidance without a prior map. Due to the lack of a pre-defined path reference, they often cannot avoid large obstacles in a timely and effective manner. At the same time, without a global map, the above algorithms can only avoid obstacles in a small area, which may lead to sudden deceleration. They also cannot well take into account the robot's dynamics or kinematic characteristics, and the planned path may not be able to allow the robot to track effectively. Summary of the Invention

[0004] The purpose of this invention is to provide a path planning method for amphibious unmanned platforms based on sonar imaging.

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

[0006] Step (1): Based on the two working modes of sonar low frequency band and high frequency band, perform sonar imaging on the environment, identify the bright areas as obstacles, and obtain the original global map and original local map of the environment where the amphibious unmanned platform is located based on the sonar imaging.

[0007] Step (2): Based on the original global map and the original local map from step (1), a morphological algorithm is used for filtering, and then the circumcircle method is used to simulate obstacles to obtain the filtered global map and the filtered local map.

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

[0009] Step (4): The coarse solution of the global path searched by the RRT algorithm is optimized by using two-dimensional cubic spline interpolation to obtain the 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 vertical-time-lateral three-dimensional paths of STL. Then, the corresponding weights are taken for time cost, lateral offset cost, collision cost and acceleration change cost 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): Convert the sonar image into an HSV color space image;

[0013] Step (22): Based on the HSV color space image obtained in step (21), set upper and lower thresholds, and use the threshold method to filter the brightness area to obtain a coarse map solution;

[0014] Step (23): Morphological opening operation is used to eliminate noise remaining due to hardware limitations that prevent the thresholding method from completely eliminating the coarse map solution. The calculation formula is as follows:

[0015]

[0016] f is the input image, and B is the structuring element. For corrosion operation, For expansion operation;

[0017] Step (24): Locate 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 circumcircle of the obstacle geometry. Multiply the returned circumcircle radius by the coefficient k, and finally return the center coordinates and radius of all obtained circumcircles, thus obtaining the filtered global map.

[0019] Furthermore, step (21) specifically involves:

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

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

[0022]

[0023] 4. C = VC min

[0024]

[0025] Where: 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, within the range of [0-1], V is the brightness, S is the saturation, C is the difference of color components, and H is the hue.

[0026] Furthermore, step (24) specifically involves:

[0027] Initialize the current node as the starting node, with the current orientation set to 0: current_point = start_point, direction = 0. Then, for each boundary point current_point and 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, then update the current node and direction:

[0030] current_point = new_point

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

[0032] Stop when you return to the starting point or when no next boundary point is found; obtain the obstacle geometry.

[0033] Furthermore, step (3) specifically involves:

[0034] Step (31): Obtain the current coordinate position based on the positioning information fused from the amphibious platform's inertial navigation and Doppler log (DVL), and set the endpoint position;

[0035] Step (32): The filtered global map obtained in step (22) starts from the starting point and is recorded as the parent node. After taking the sampling rate f of the endpoint, points are randomly scattered in the map space. The angle between the random point and its parent node is recorded as θ. The parent node is taken as the reference node, the step size is taken, and the child nodes are spread with θ as the growth direction. The coordinate center point of the obstacle and the distance l between the child node are calculated. If l>r, the collision judgment is performed on the connection of the generated child nodes. The distance l1 from the center point to the line where the parent and child nodes are located is calculated. If l1>r, a child node is generated and the child node is updated to the parent node. This continues until the endpoint position is found. Finally, the path point is traced from the endpoint to the parent node to find the searched path point, which is the global coordinate point.

[0036] Furthermore, step (4) specifically involves:

[0037] Step (41): For the coordinate points searched in step (3), assuming there are 7(n+1) points, sample every seven coordinate points. If there are fewer than seven, calculate as one sample. Finally, obtain n+1 coordinate points. Consider every two coordinate points as an interval, so there are a total of n intervals. Each interval [x i x i+1 Both are derived from the cubic polynomial y = a i +b i x+c i x 2 +d i x 3 The polynomial is constructed such that it is continuous at the endpoints and its first and second derivatives are also continuous. It has n intervals and requires solving for 4n unknowns. Excluding the two endpoints, each of the n-1 internal points satisfies both pieceswise cubic equations, resulting in 2(n-1) boundary conditions. Adding the fact that the two endpoints satisfy the first and last cubic equations respectively, the total number of boundary conditions is 2n. Secondly, S′ i (x i+1 )=S′ i+1 (x i+1 There are a total of n-1 boundary conditions S″ i (x i+1 )=S″ i+1 (x i+1 There are a total of n-1 boundary conditions, plus the natural boundary S″(x0)=0=S″(x n There are a total of 4n boundary conditions, resulting in a tridiagonal matrix:

[0038]

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

[0040] Furthermore, step (5) specifically involves:

[0041] Step (51): Longitudinal sampling: Define t as the sampling time, the initial position xs along the s direction, the initial velocity vxs along the s direction, the initial acceleration axs along the s direction, and the expected final velocity vxe and final acceleration axe along the s axis at time t. From this, we obtain the fourth-order 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 velocity vxe at time t, and f″(t) is the expected acceleration axe at time t. Finally, a fourth-order polynomial is obtained, which satisfies the planned longitudinal velocity, acceleration, and jerk continuity.

[0042] Step (52): Lateral sampling: Based on the known sampling points in the s direction, take different longitudinal sampling step sizes for each sampling point, define t as the sampling time, the initial position xl along the l direction, the initial velocity vxl along the l direction, the initial acceleration axl along the l direction, and the expected final velocity vxe and final acceleration axe along the l axis at time t. From this, we obtain 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 initial 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 velocity vxe at time t, and f″(t) is the expected acceleration axe at time t. Finally, a fifth-degree polynomial is obtained, which satisfies the continuity of the robot's lateral and longitudinal velocities, accelerations, and jerk during its movement.

[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) specifically involves:

[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. Specifically, if a collision occurs, the cost of the path is extremely high, so 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 cost of a sudden change in acceleration:

[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 lowest cost among all sampling points, which is the optimal path.

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

[0057] (1) This invention designs a path planning method for amphibious unmanned platforms based on front-mounted multibeam sonar imaging and discrete spatial sampling. This method is designed to obtain global map information by using the low-frequency band of sonar when there is no prior map information, and initially solves the sampling space. Then, based on the sampling space, it performs effective autonomous navigation and obstacle avoidance, 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 increases continuously. If x suddenly decreases during the path search process, replanning is required and then splicing is performed. This method not only consumes a lot of computing power, but also has the problem that the path at the splicing point does 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 interpolation is performed on ux and uy respectively. Since u cannot be negative, 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 this invention makes up for the shortcomings of not being able to satisfy the kinematic constraints of the robot well. In view of the unsmooth speed change of the robot during 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 underwater autonomous robot path planning during high-speed movement and can be used as a path planning method for amphibious platforms. Attached Figure Description

[0060] Figure 1 Flowchart of the amphibious unmanned platform path planning method based on sonar imaging provided by the present invention;

[0061] Figure 2 This is a raw data image of the sonar imaging of the present invention;

[0062] Figure 3 This is a post-processing result of sonar imaging of the obstacle region according to the present invention;

[0063] Figure 4 This is a comparison diagram of the fitting of cubic spline curves and bi-dimensional cubic spline curves 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 a final effect diagram of the present invention. Detailed Implementation

[0066] The present invention will now be described in further detail with reference to the accompanying drawings.

[0067] This invention relates to a path planning method based on forward-facing multibeam sonar imaging. It utilizes the low-frequency band of the sonar to acquire coarse global map information for global path search, facilitating the avoidance of large obstacles and providing reference lines for subsequent local obstacle avoidance. Based on the searched global path, it optimizes using two-dimensional cubic spline curves and establishes a Frenet coordinate system. Local path sampling is performed based on this coordinate system. During movement, the low-frequency band is switched to update obstacle information. If the sampled path collides with a local obstacle, its cost is set to a maximum value to achieve obstacle avoidance. The cost function includes, in addition to collision cost, velocity change cost, time cost, and lateral offset cost. These four costs jointly determine the quality of the sampled path, selecting the optimal path curve for tracking. Finally, a safe obstacle avoidance path with smooth velocity, acceleration, and trajectory changes in the amphibious platform's kinematic characteristics is obtained.

[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: Based on the two working modes of sonar, low frequency band and high frequency band, perform sonar imaging of the environment, identify the bright areas as obstacles, and obtain the original global map and original local map of the environment where the amphibious unmanned platform is located based on the sonar imaging.

[0070] Step 2: Based on the original global map and the original local map from step (1), a morphological algorithm is used to filter them, and then the circumcircle method is used to simulate the obstacles 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] Where: 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, within the range of [0-1], V is the brightness, S is the saturation, C is the difference of 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. Thresholding is used to filter the brightness areas. Morphological opening is then used on the coarsely processed map to eliminate noise remaining due to hardware limitations that prevent the thresholding method from completely eliminating it. The calculation formula is as follows:

[0079]

[0080] f is the input image, and B is the structuring element. For corrosion operation, For expansion operation;

[0081] Obstacles are found by tracing pixel boundaries: First, the current node is initialized as the starting node, and the current orientation is 0: current_point = start_point, direction = 0. Then, for each boundary point current_point and direction, the next candidate point new_point is calculated:

[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, then update the current node and direction:

[0084] current_point = new_point

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

[0086] The process continues until the starting point is reached or no next boundary point is found, at which point the obstacle geometry is obtained.

[0087] After searching all boundaries, the `minEnclosingCircle` function in OpenCV is called to calculate and return the center and radius of the smallest circumcircle of the obstacle. Figure 2 Figure 3 As shown, for the returned circumcircle radius, in order to ensure the accuracy of obstacle avoidance, it is multiplied by a coefficient of 1.15 to ensure sufficient collision space is reserved. Finally, the center coordinates and radii of all the obtained circumcircles are returned, and the global map can be obtained.

[0088] Step 3: By subscribing to the location information and the destination, we have the starting point and destination to search for. Based on the global map information obtained above, including the coordinate center of the obstacle and the radius of the circumscribed circle r, we start from the starting point and denote it as the parent node. We take the sampling rate k (i.e., the probability of directly sampling to the destination is k / 100) and then randomly scatter points in the map space. Let the angle between the random point and its parent node be θ. With the parent node as the reference node, we take a certain step size step and start to spread child nodes with θ as the growth direction. We traverse and calculate the distance 1 between the coordinate center point of the obstacle and the child node. If 1>r, we perform collision judgment on the connection of the generated child nodes and calculate the distance l1 from the center point to the line where the parent and child nodes are located. If l1>r, we generate a child node and update the child node to the parent node. We continue until the destination position is found. Finally, we trace back from the destination to the parent node to find the searched path point, which 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 fewer than seven, count as one sample. This results in n+1 coordinate points. Each pair of coordinate points is considered an interval, resulting in n intervals. Each interval [x...] i x i+1 Both are derived from the cubic polynomial y = a i +bi x+c i x 2 +d i x 3 The polynomial is constructed such that it is continuous at the endpoints and its first and second derivatives are also continuous. It has n intervals and requires solving for 4n unknowns. Excluding the two endpoints, each of the n-1 internal points satisfies both pieceswise cubic equations, resulting in 2(n-1) boundary conditions. Adding the fact that the two endpoints satisfy the first and last cubic equations respectively, the total number of boundary conditions is 2n. Secondly, S′ i (x i+1 )=S′ i+1 (x i+1 There are a total of n-1 boundary conditions S″ i (x i+1 )=S″ i+1 (x i+1 There are a total of n-1 boundary conditions, plus the natural boundary S″(x0)=0=S″(x n There are a total of 4n boundary conditions, which ultimately yields a tridiagonal matrix:

[0090]

[0091] One-dimensional cubic spline curves can be obtained by solving the tridiagonal matrix. Assuming the distance u between two coordinate points, performing cubic spline interpolation on uX and uy respectively yields a two-dimensional cubic spline curve UXY. This method produces a smoother curve at the junction of two curve segments compared to directly obtaining the two-dimensional cubic spline curve from the XY coordinates. Figure 4 To compare direct interpolation of x and y with interpolation of ux and uy respectively, Figure 5 The original path for the RRT search and the global path obtained after interpolation.

[0092] Step 5: Based on the global map obtained in the above steps, subscribe to local map messages. The specific processing method is the same as in Step 2, except that the sonar is in high-frequency operating mode at this time, so it will not be described again. Sample the horizontal and vertical directions according to the global path obtained in Step 4. The specific method is as follows:

[0093] First, longitudinal sampling is performed: Let t be the sampling time, the initial position xs, the initial velocity vxs in the s direction, and the initial acceleration axs in the s direction be the current position, velocity, and acceleration along the s direction, respectively. The final velocity vxe and the final acceleration axe are the expected velocity and acceleration along the s-axis at time t, respectively. From this, we can obtain the fourth-order 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 velocity vxe at time t, and f″(t) is the expected acceleration axe at time t. Finally, a fourth-order polynomial is obtained, which satisfies the planned longitudinal velocity, acceleration, and jerk continuity.

[0094] Then, based on the longitudinal sampling, lateral sampling is performed: sampling points in the s direction, with each sampling point having a different longitudinal sampling step size (an odd number). Let t be the sampling time, the initial position xl, the initial velocity vxl in the l direction, and the initial acceleration axl in the l direction be the current position, velocity, and acceleration along the l direction, respectively. The final velocity vxe and final acceleration axe are the expected velocity and acceleration along the l axis at time t, respectively. From this, the fifth-order polynomial f(t) = a0 + a1*t + a2*t can be obtained. 2 +a3*t 3 +a4*t 4 +a5*t 5 Where f(0) is the initial l-axis coordinate, f′(0) is the l-direction a0 equal to xs, a1 equal to vxs, is the l-axis coordinate at time t, is the expected velocity vxe at time t, and is the expected acceleration axe at time t. Finally, a fifth-degree polynomial is obtained, which satisfies the continuity of the robot's lateral and longitudinal velocities, accelerations, and jerk during its movement.

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

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

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

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

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

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

[0101] in:

[0102]

[0103] Calculate the total cost for 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] Selecting the sampling path with the lowest cost, the final planning result is as follows: Figure 6 .

Claims

1. A path planning method for amphibious unmanned platforms based on sonar imaging, characterized in that, Includes the following steps: Step (1): Based on the two working modes of sonar low frequency band and high frequency band, perform sonar imaging on the environment, identify the bright areas as obstacles, and obtain the original global map and original local map of the environment where the amphibious unmanned platform is located based on the sonar imaging. Step (2): Based on the original global map and the original local map from step (1), a morphological algorithm is used for filtering, and then the circumcircle method is used to simulate obstacles to obtain the filtered global map and the filtered local map. Step (3): Based on the filtered global map obtained in step (2), the RRT algorithm is used to search for global paths and obtain a coarse solution of the global path. Step (4): The coarse solution of the global path searched by the RRT algorithm is optimized by using two-dimensional cubic spline interpolation to obtain the 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 vertical-time-lateral three-dimensional paths of STL. Then, the corresponding weights of time cost, lateral offset cost, collision cost and acceleration change cost are respectively taken and weighted. The optimal path is selected according to the weighted cost function. Step (2) specifically includes the following steps: Step (21): Convert the sonar image into an HSV color space image; Step (22): Based on the HSV color space image obtained in step (21), set upper and lower thresholds, and use the threshold method to filter the brightness area to obtain a coarse map solution; Step (23): Morphological opening operation is used to eliminate residual noise in the coarse map solution, which cannot be completely eliminated by the thresholding method due to hardware limitations. The calculation formula is as follows: f is the input image, B is the structuring element, and θ is the erosion operation. For expansion operation; Step (24): Locate 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 circumcircle of the obstacle geometry. Multiply the returned circumcircle radius by the coefficient k, and finally return the center coordinates and radius of all obtained circumcircles. Thus, the filtered global map is obtained. Step (3) specifically involves: Step (31): Obtain the current coordinate position based on the positioning information fused from the amphibious platform's inertial navigation and Doppler log (DVL), and set the endpoint position; Step (32): The filtered global map obtained in step (22) starts from the starting point and is recorded as the parent node. After taking the sampling rate f of the endpoint, points are randomly scattered in the map space. The angle between the random point and its parent node is θ. The parent node is taken as the reference node, the step size is taken, and the child nodes are spread with θ as the growth direction. The coordinate center point of the obstacle and the distance l between the child node are calculated. If l>r, the collision judgment is performed on the generated child node connection. The distance l1 from the center point to the line where the parent and child nodes are located is calculated. If l1>r, the child node is generated and the child node is updated to the parent node. This continues until the endpoint position is found. Finally, the path point is found from the endpoint to the parent node, which is the global coordinate point. Step (4) is as follows: Step (41): For the coordinate points searched in step (3), assuming there are 7(n+1) points, sample every seven coordinate points. If there are fewer than seven, calculate as one sample. Finally, obtain n+1 coordinate points. Consider every two coordinate points as an interval, so there are a total of n intervals. Each interval [x i ,x i+1 Both are derived from the cubic polynomial y = a i +b i x+c i x 2 +d i x 3 The polynomial is constructed such that it is continuous at the endpoints and its first and second derivatives are also continuous. It has n intervals and requires solving for 4n unknowns. Excluding the two endpoints, each of the n-1 internal points satisfies both pieceswise cubic equations, resulting in 2(n-1) boundary conditions. Adding the fact that the two endpoints satisfy the first and last cubic equations respectively, the total number of boundary conditions is 2n. Secondly, S′ i (x i+1 )=S′ i+1 (x i+1 There are a total of n-1 boundary conditions S″ i (x i+1 )=S″ i+1 (x i+1 There are a total of n-1 boundary conditions, plus the natural boundary S″(x0)=0=S″(x n There are a total of 4n boundary conditions, resulting in a tridiagonal matrix: Step (42): Solve the tridiagonal matrix to obtain a one-dimensional cubic spline curve.

2. The method according to claim 1, characterized in that, Step (21) is as follows:

1. V = max(R, G, B) 2.C min =min(R,G,B) 4.C=V-C min Where: 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, within the range of [0-1], V is the brightness, S is the saturation, C is the difference of color components, and H is the hue.

3. The method according to claim 2, characterized in that, Step (24) is as follows: Initialize the current node as the starting node, with the current orientation set to 0: current_point = start_point, direction = 0. Then, for each boundary point current_point and 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, then update the current node and direction: current_point = new_point direction=(new_direction+5)%8 Stop when returning to the starting point or when no next boundary point is found; Obtain the geometry of the obstacles.

4. The method according to claim 3, characterized in that, Step (5) specifically involves: Step (51): Longitudinal sampling: Define t as the sampling time, the initial position xs along the s direction, the initial velocity vxs along the s direction, the initial acceleration axs along the s direction, and the expected final velocity vxe and final acceleration axe along the s axis at time t. From this, we obtain the fourth-order 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 velocity vxe at time t, and f″(t) is the expected acceleration axe at time t. Finally, a fourth-order polynomial is obtained, which satisfies the planned longitudinal velocity, acceleration, and jerk continuity. Step (52): Lateral sampling: Based on the known sampling points in the s direction, take different longitudinal sampling step sizes for each sampling point, define t as the sampling time, the initial position xl along the l direction, the initial velocity vxl along the l direction, the initial acceleration axl along the l direction, and the expected final velocity vxe and final acceleration axe along the l axis at time t. From this, we obtain 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 initial 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 velocity vxe at time t, and f″(t) is the expected acceleration axe at time t. Finally, a fifth-degree polynomial is obtained, which satisfies the continuity of the robot's lateral and longitudinal velocities, accelerations, and jerk during its movement. 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.

5. The method according to claim 4, characterized in that, Step (53) is as follows: Step (531): Calculate the time cost: time_cost = K_TIME * t1; Step (532): Calculate the collision cost: collision_cost = K_COLLISION * is_collision, where is_collision is a bool value of 0 or 1. Specifically, if a collision occurs, the cost of the path is extremely high, so it is not selected. Step (533): Calculate the lateral offset cost: offset_cost=K_OFFSET*abs(path_pre.l) / 2; Step (534): Calculate the cost of a sudden change in acceleration: 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 lowest cost among all sampling points, which is the optimal path.

Images