Unstructured road path planning method based on JPS algorithm and gradient descent

Abstract

This invention discloses an unstructured road path planning method based on the JPS algorithm and gradient descent, comprising the following steps: Step 1: Initialize map information and path node information; Step 2: Obtain a series of collision-free control nodes from the starting point to the target point, and obtain the angle between the path formed by the connecting lines of the nodes and the map x-axis; Step 3: Add auxiliary control points to the control nodes and perform path curve fitting; Step 4: Optimize the path curve to obtain the optimal path. This invention solves the problems of low path search efficiency and non-optimal generated trajectories in existing path planning methods.

Description

Technical Field

[0001] This invention belongs to the field of path planning technology and relates to an unstructured road path planning method based on the JPS algorithm and gradient descent. Background Technology

[0002] Path planning, a crucial component of autonomous driving systems, is a prerequisite for ensuring driving safety and passenger comfort. In unstructured roads, obstacles are generally stationary and irregular, and ensuring the safe and smooth journey of the vehicle from the starting point to the destination has been a key research focus in the field of path planning.

[0003] Traditional search algorithms for autonomous driving path planning commonly include A-Star, D-Star, and RRT algorithms. These algorithms typically find a path from the starting point to the target point in a gridded environment, but the generated path is usually a polyline, not smooth, and cannot be directly used as a path for the autonomous vehicle. The Hybrid-A-Star algorithm can generate a path from the starting point to the target point that satisfies curvature constraints, but it requires a large amount of computing resources during the calculation, resulting in low search efficiency, and the final trajectory is not locally optimal. Summary of the Invention

[0004] To achieve the above objectives, this invention provides an unstructured road path planning method based on the JPS algorithm and gradient descent, which solves the problems of low path search efficiency and non-optimal generated trajectories in existing path planning methods.

[0005] The technical solution adopted in this invention is an unstructured road path planning method based on the JPS algorithm and gradient descent, comprising the following steps:

[0006] Step 1: Initialize map information and path node information;

[0007] Step 2: Obtain a series of collision-free control nodes from the starting point to the target point, and obtain the angle between the path formed by the connecting lines of the nodes and the x-axis of the map;

[0008] Step 3: Add auxiliary control points to the control nodes and perform path curve fitting;

[0009] Step 4: Optimize the path curve to obtain the optimal path.

[0010] Further, step 1 specifically includes:

[0011] Step 1-1: Obtain the map of the current environment, perform rasterization on the map, and divide it into segments based on the vehicle's length L. car Inflate obstacles in a raster map;

[0012] Step 1-2: Based on the rasterized map, establish a coordinate system and initialize the vehicle's current position P(x,y,θ), where x and y represent the vehicle's horizontal and vertical coordinates in the coordinate system, and θ represents the vehicle's facing angle.

[0013] Steps 1-3: Determine the starting point as P based on the vehicle's current location. start (x start ,y start ,θ start ), set the target point as P goal (x goal ,y goal ,θ goal ); where x start y start Let x be the vehicle's x and y coordinates at the starting point. goal y goal Let θ be the vehicle's horizontal and vertical coordinates at the target point. start Let θ be the vehicle's orientation at the starting point. goal This represents the vehicle's orientation at the target point.

[0014] Furthermore, step 2 specifically includes:

[0015] Step 2-1: Initialize two lists, openlist and closedlist, and additionally initialize the list pathlist;

[0016] Step 2-2: Using the JPS algorithm, add the nodes to be searched to the open list and the nodes that have already been searched to the closed list, generating a list starting from the starting point P. start (x start ,y start ,θ start ) to target point P goal (x goal ,y goal ,θ goal The collision-free path path; where x start y start Let x be the vehicle's x and y coordinates at the starting point. goal y goal Let θ be the vehicle's horizontal and vertical coordinates at the target point. start Let θ be the vehicle's orientation at the starting point. goal The vehicle's orientation at the target point;

[0017] Steps 2-3: After obtaining the path from the starting point to the target point, extract each node P1, P2, P3, ..., P from the path. n Starting from point P1 = P start To target point Pn =P goal Add them sequentially to pathlist, pathlist = {P1, P2, P3, ..., P...} n}, where P1 = P start P n =P goal P2, P3, ..., P n-1 Starting from point P1 = P start and target point P n =P goal Path points between;

[0018] Steps 2-4: Calculate all nodes {P2, P3, P4, ..., P} except for the starting point and the target point. n-1 The average slope between each node and its parent and child nodes is converted into the angle with the x-axis of the coordinate system.

[0019] Furthermore, step 3 specifically includes:

[0020] Based on the control node pathlist obtained in step 2 = {P1, P2, P3, ..., P...} n For all nodes {P2, P3, P4, ..., P} except the starting point and the target point, n-1 The algorithm sequentially determines the Manhattan distance between each node and its parent and child nodes. If other nodes exist within one grid cell, it indicates that there are obstacles in the area around the node and that the node is an inflection point of the path obtained by the JPS algorithm. In this case, auxiliary control points are added to both ends of the control point. Auxiliary control points are added to the starting point and the target point based on the initial pose. The basis function of the cubic B-spline curve is established and fitted to the control nodes to obtain a path with continuous curvature from the starting point to the target point.

[0021] Furthermore, all nodes {P2, P3, P4, ..., P...} except for the starting point and the target point... n-1 The specific method for adding auxiliary control points is as follows:

[0022] Step 3-1: Calculate the Manhattan distance between P2 and P1, and between P2 and P3. When d 21 ≤3 or d 23 When the value is ≤3, it indicates that there are obstacles around the node, and auxiliary control points need to be added; among them, These are the x and y coordinates of node P2, respectively. These are the x and y coordinates of node P3, respectively;

[0023] Step 3-2: When P2 needs to add auxiliary control points, add auxiliary control points on both sides. and These are the auxiliary control points added on both sides of point P2, where L is half the length of the vehicle body and θ2 is the vehicle's orientation at point P2.

[0024] Step 3-3: Calculate P3, P4, ..., P in sequence. n-1 The Manhattan distance between each node and its parent and child nodes is used to determine whether an auxiliary control point needs to be added.

[0025] Steps 3-4: Update the node list with the added auxiliary control points. After adding auxiliary control points to the node list, define the node list containing auxiliary control points as pathlist. append ,Right now:

[0026]

[0027] Furthermore, regarding the starting point P1 and the target point P... n The specific method for adding auxiliary control points is as follows:

[0028] Steps 3-6: Define L as half the length of the vehicle body, and take point P1 = P start =P start (x start ,y start ,θ start ), P n =P goal =P goal (x goal ,y goal ,θ goal Define the starting vehicle's velocity direction V. start =θ start The direction of the velocity when traveling to the target point is V. goal =θ goal ; where x start y start Let x be the vehicle's x and y coordinates at the starting point. goal y goal Let θ be the vehicle's horizontal and vertical coordinates at the target point. start Let θ be the vehicle's orientation at the starting point. goal The vehicle's orientation at the target point;

[0029] Steps 3-7: Add control points on both sides of the starting point P1 Add target point P n Control points on both sides Where L is half the length of the vehicle body;

[0030] Steps 3-8: Add the newly added control points to the pathlist append middle, Update pathlistappend Internal node name, pathlist append ={P1,P2,P3,…,P n}; At this time, pathlist∈pathlist append .

[0031] Furthermore, the path curve fitting method in step 3 is specifically as follows:

[0032] Step 3-9: Establish the basis functions for the cubic B-spline curve:

[0033] P s =F 0,3 C i +F 1,3 C i+1 +F 2,3 C i+2 +F 3,3 C i+3

[0034] The coefficient F of the cubic B-spline curve 0,3 ,F 1,3 ,F 2,3 ,F 3,3 for:

[0035]

[0036] C i C i+1 C i+2 C i+3 s is the reference point for the cubic B-spline curve, and s is the vector formed by discrete points when the B-spline curve is drawn and calculated by the computer.

[0037] Step 3-10: Obtain the velocity basis function V by taking the first derivative from the cubic B-spline curve basis function. s And by taking the second derivative, the acceleration basis function A is obtained. s :

[0038] V s =V 0,3 C i +V 1,3 C i+1 +V 2,3 C i+2 +V 3,3 C i+3

[0039] Among them, V 0,3 V 1,3 V 2,3 V 3,3 C respectively i C i+1 Ci+2 C i+3 The value of the derivative with respect to s is used as the velocity basis function V. s The coefficient;

[0040] A s =A 0,3 C i +A 1,3 C i+1 +A 2,3 C i+2 +A 3,3 C i+3

[0041] Among them, A 0,3 A 1,3 A 2,3 A 3,3 V 0,3 V 1,3 V 2,3 V 3,3 The value of the derivative with respect to s is used as the acceleration basis function A. s The coefficient;

[0042] in,

[0043]

[0044]

[0045] Step 3-11: For pathlist append Fit the points to C, and let C be the point. i C i+1 C i+2 C i+3 Equal to P1, P2, P3, and P4 respectively, we obtain the first path curve P. s1 The first velocity curve V s1 The first acceleration curve A s1 Let C i C i+1 C i+2 C i+3 Equal to P2, P3, P4, and P5 respectively, resulting in curve P. s2 V s2 A s2 Substitute them one by one until C is satisfied. i C i+1 C i+2 C i+3 They are respectively equal to P n-3 P n-2 P n-1 P n The curve P is obtained. s(n-3) V s(n-3)A s(n-3) ;

[0046] Step 3-12: Place P si i = 1, 2, 3, ..., n-3; V si i = 1, 2, 3, ..., n-3; A si ,i=1,2,3,…,n-3; by concatenating the first and second parts of each curve segment, the path curve P can be obtained. (s) velocity curve V (s) Acceleration curve A (s) .

[0047] Furthermore, step 4 specifically includes:

[0048] A repulsive force field is established for the obstacle, and a curvature formula is established for the path curve. The repulsive force field and the curvature formula are added with different weight coefficients. At the same time, gradient descent is applied to the points on the path in turn to change the coordinates of the points on the path. The optimal value is obtained between moving away from the obstacle and minimizing the curvature of the path, so as to obtain a path with no collision with the obstacle and a small curvature.

[0049] Furthermore, the specific operation process of step 4 is as follows:

[0050] Step 4-1: Establish the repulsive field U rep (P):

[0051]

[0052] Where η is the repulsive force scale factor, ρ(P,P) obs ) represents the distance between the center of the vehicle and the center of the obstacle, and ρ0 represents the radius of influence of the obstacle;

[0053] The gradient of the repulsive field is:

[0054]

[0055] Step 4-2: For curve P (s) The first curve P s1 Establish the curvature equation, and derive the gradient descent loss function from the curvature equation:

[0056] The P s1 The equation of curvature is:

[0057]

[0058] Among them, K numerator and K denominator Representing the numerator and denominator of curvature K respectively, θ0 = x p4 θ1=y p4 x p4 and yp4 These are the x and y coordinates of node P4, respectively. Among them, V′1, V′2, A′1, and A′2 are intermediate values ​​proposed for ease of expression, V 0,3 V 1,3 V 2,3 V 3,3 V is the velocity basis function s The coefficient, A 0,3 A 1,3 A 2,3 A 3,3 Let A be the acceleration basis function. s coefficient, These are the x and y coordinates of P1, respectively. These are the x and y coordinates of P2, respectively. These are the x and y coordinates of P3, respectively;

[0059]

[0060]

[0061] Where s is a vector composed of discrete points when the B-spline curve is drawn and calculated by the computer;

[0062] The loss function of gradient descent includes:

[0063]

[0064]

[0065]

[0066] in Let be the loss function for gradient descent with respect to a repulsive field. Let F be the loss function for curvature gradient descent. cost Let be the loss function for gradient descent;

[0067] Step 4-3: Calculate the loss function with respect to the vector θ = [θ0, θ1] T gradient descent direction Gradient descent is initiated to optimize the path curve, where μ1 and μ2 represent the proportions of the curve that satisfy the minimum curvature or are farthest from obstacles. in, This represents the partial derivative of curvature K with respect to θ0. This represents the partial derivative of curvature K with respect to θ1; Let be the partial derivative of the repulsive field with respect to θ0. Let be the partial derivative of the repulsive field with respect to θ1;

[0068] Step 4-4: Perform gradient descent using the following formula:

[0069]

[0070] Where, θ new This represents the optimized vector form, where m is the number of points after the curve is discretized, and α is the learning rate.

[0071] Steps 4-5: For P s1 After performing gradient descent N times, for curve P s2 ,P s3 ,P s4 …P s(n-3) Optimize sequentially;

[0072] Steps 4-6: After all curves have been optimized, the optimal path is obtained.

[0073] Furthermore, in step 4: for route P s1 ,P s2 ,P s3 …P s(n-3) In addition to the forward order, the optimization order of line segments also includes the reverse order and random order; the larger the number of gradient descent operations N, the closer the curve is to the local optimum.

[0074] The beneficial effects of this invention are:

[0075] (1) A path planning method is proposed. This method uses the JPS algorithm to obtain path control points and determines whether auxiliary control points need to be added for each control point. A drivable path is obtained by fitting the control points with a B-spline curve. The path curvature is continuous and the path curve does not conflict with obstacles. (2) An optimization method for the B-spline curve fitted path is proposed. Gradient descent is applied to both the curvature of the path curve and the repulsive force field of obstacles. The coordinates of the control points are changed to reduce the curvature of the path curve without conflicting with obstacles, thereby improving ride comfort. (3) Based on the optimization method of B-spline curve fitted path using gradient descent, this invention proposes an optimization sequence that starts by optimizing the path curve from the starting point and gradually optimizes towards the end point. This ensures sufficient optimization time and allows for optimization of subsequent paths while driving. Attached Figure Description

[0076] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0077] Figure 1 This is a flowchart illustrating the overall process of the unstructured road path planning method based on the JPS algorithm and gradient descent, according to an embodiment of the present invention.

[0078] Figure 2 This is a polyline path diagram from the starting point to the target point in an embodiment of the present invention.

[0079] Figure 3 This is a flowchart of the algorithm for adding auxiliary control points to the control node according to an embodiment of the present invention.

[0080] Figure 4 This is a schematic diagram of the path curve that the vehicle needs to optimize according to an embodiment of the present invention.

[0081] Figure 5 These are comparison diagrams of path optimization before and after in an embodiment of the present invention; wherein, (a) is a path-B-spline curve diagram before and after path optimization, (b) is a path curvature-B-spline curve diagram before and after path optimization, and (c) is a repulsion-B-spline curve diagram before and after path optimization.

[0082] Figure 6 These are simulation results before and after path optimization in different scenarios according to embodiments of the present invention; where (a) to (f) are simulation results before and after path optimization under different paths and different obstacles, respectively. Detailed Implementation

[0083] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0084] This invention proposes an unstructured road path planning method based on the JPS algorithm and gradient descent, such as... Figure 1 As shown, it includes the following steps:

[0085] Step 1: Initialize map information and path node information. Based on the high-precision map of the current environment, perform rasterization processing and obstacle inflation processing, specifically including:

[0086] Step 1-1: Obtain the map of the current environment, perform rasterization on the map, and divide it into segments based on the vehicle's length L. car Inflate obstacles in a raster map.

[0087] Step 1-2: Based on the rasterized map, establish a coordinate system and initialize the vehicle's current position P(x,y,θ), where x and y represent the vehicle's horizontal and vertical coordinates in the coordinate system, and θ represents the vehicle's facing angle.

[0088] Steps 1-3: Determine the starting point as P based on the vehicle's current location. start (x start ,y start ,θ start ), set the target point as P goal (x goal ,y goal ,θ goal ). Where x start y start Let x be the vehicle's x and y coordinates at the starting point. goal y goal Let θ be the vehicle's horizontal and vertical coordinates at the target point. start Let θ be the vehicle's orientation at the starting point. goal This refers to the vehicle's orientation at the finish line.

[0089] Step 2: Obtain a series of collision-free control nodes from the starting point to the target point. Please refer to [link / reference needed]. Figure 2 The JPS algorithm is used to obtain a collision-free polyline path from the starting point to the target point, and the nodes on the path are extracted as control nodes. The angle between the path formed by the nodes and the map's x-axis is obtained. The slope of the straight path between each node and its parent and child nodes (excluding the starting and target points) in the map coordinate system is calculated. The average of the two slopes is taken, and the average slope is converted into an angle with the map's x-axis and stored in the node attributes. Specifically, the following steps are included:

[0090] Step 2-1: Initialize the two essential lists for the JPS algorithm, openlist and closedlist, and additionally initialize the pathlist to store the nodes traversed by the collision-free path generated by the JPS algorithm.

[0091] Step 2-2: Using the JPS algorithm, add the nodes to be searched to the open list and the nodes that have already been searched to the closed list, generating a list starting from the starting point P. start (x start ,y start ,θ start ) to target point P goal (x goal ,y goal ,θ goal The collision-free path.

[0092] Steps 2-3: After obtaining the path from the starting point to the ending point, process each node P1, P2, P3, ..., P in the path. n Starting from point P1 = P start To target point P n =P goal Add them sequentially to pathlist, i.e., pathlist = {P1, P2, P3, ..., P...} n}, where P1 = P start P n =P goal Where P2, P3, ..., P n-1 Starting from point P1 = P start and endpoint P n =P goal Path points between them.

[0093] Steps 2-4: Calculate all nodes {P2, P3, P4, ..., P} except for the starting point and the target point (end point). n-1 The average slope of each node in the coordinate system, along with its parent and child nodes, is converted into the angle between the slope and the x-axis. For example, the slope of the path between P2 and P1 is... The slope of the path between P2 and P3 is The average slope can be obtained by averaging the two slopes. in These are the x and y coordinates of node P2, respectively. These are the x and y coordinates of node P3, respectively. k2 is the average slope between P2 and its parent and child nodes. The average slope is converted into the angle with the x-axis of the map coordinate system. This angle represents the vehicle's orientation at that moment, i.e., the vehicle orientation at node P2 is θ2 = arctan(k2), and is stored in the node attributes. The same calculation is performed sequentially for P3, P4, ..., P... n-1 The vehicle body is oriented towards θ3, θ4, ..., θ n-1 .

[0094] Step 3: Add auxiliary control points to the control nodes and perform path curve fitting. Based on the control node pathlist obtained in the previous step = {P1, P2, P3, ..., P...} n For all nodes {P2, P3, P4, ..., P} except the starting point and the target point, n-1The algorithm sequentially determines the Manhattan distance between each node and its parent and child nodes, setting a limit of 3. If other nodes exist within one grid cell around the node, it indicates an obstacle in the surrounding area and an inflection point on the path obtained by the JPS algorithm. In this case, auxiliary control points are added to both ends of this control point to ensure that the path curve fitted by the cubic B-spline curve passes through this control point, thus avoiding conflicts with obstacles. Auxiliary control points are added to the start and target points based on their initial poses, ensuring that the fitted path curve passes through both points. A basis function for the cubic B-spline curve is established and fitted to the control nodes to obtain a path with continuous curvature from the start point to the target point; this path is the drivable path. (Reference) Figure 3 Specifically, it includes the following steps:

[0095] Step 3-1: For nodes P2, P3, P4, ..., P n-1 Then, sequentially determine whether there are other nodes within one surrounding grid cell. Taking P2 as an example, calculate the Manhattan distance between P2 and P1, and between P2 and P3. When d 21 ≤3 or d 23 When the value is ≤3, it means that there are obstacles around this node, and auxiliary control points need to be added.

[0096] Step 3-2: When P2 = P2(x2,y2,θ2) requires additional auxiliary control points, add auxiliary control points on both sides. and The auxiliary control points added on both sides of point P2 ensure that the curve passes through point P2 when fitting with a cubic B-spline curve, where L is half the length of the vehicle body and θ2 is the angle between the average slope of the path obtained in steps 2-4 and the x-axis.

[0097] Step 3-3: Calculate P3, P4, ..., P in sequence. n-1 The Manhattan distance between each node and its parent and child nodes is used to determine whether an auxiliary control point needs to be added.

[0098] Steps 3-4: Update the node list with the added auxiliary control points. For ease of description, after adding auxiliary control points to the node list, define the node list containing auxiliary control points as pathlist. append ,Right now:

[0099]

[0100] Steps 3-5: To ensure the cubic B-spline curve fit passes through the start and target points, for pathlist = {P1, P2, P3, ..., P...} n The starting point P1 and the target point P of} n Add auxiliary control points.

[0101] Steps 3-6: Define L as half the length of the vehicle body, and take point P1 = P start =P start (x start ,y start ,θ start ), P n =P goal =P goal (x goal ,y goal ,θ goal Define the direction of the vehicle's velocity V at the starting point. start =θ start The direction of the velocity when traveling to the target point is V. goal =θ goal .

[0102] Steps 3-7: Add control points on both sides of the starting point P1 Add target point P n Control points on both sides

[0103] Steps 3-8: Add the newly added control points to the pathlist append middle, For ease of explanation, update the pathlist. append Internal node name, pathlist append ={P1,P2,P3,…,P n}. At this time, pathlist∈pathlist append .

[0104] Step 3-9: Establish the basis functions for the cubic B-spline curve:

[0105] P s =F 0,3 C i +F 1,3 C i+1 +F 2,3 C i+2 +F 3,3 C i+3

[0106] The coefficient F of the cubic B-spline curve 0,3 ,F 1,3 ,F 2,3 ,F 3,3 for

[0107]

[0108] C i C i+1 C i+2 Ci+3 s is the reference point for the cubic B-spline curve, and s is the discrete point when the B-spline curve is drawn and calculated by the computer. Optionally, the value of s can be [0:0.01:1], that is, the vector formed by all points between 0 and 1 with an interval of 0.01.

[0109] Step 3-10: Based on the basis functions of the cubic B-spline curve, its first derivative, i.e., the velocity basis function V, can be obtained. s The second derivative is the acceleration basis function A. s :

[0110] V s =V 0,3 C i +V 1,3 C i+1 +V 2,3 C i+2 +V 3,3 C i+3

[0111] Among them, V 0,3 V 1,3 V 2,3 V 3,3 C i C i+1 C i+2 C i+3 The value of the derivative with respect to s is used as the velocity basis function V. s The coefficient.

[0112] A s =A 0,3 C i +A 1,3 C i+1 +A 2,3 C i+2 +A 3,3 C i+3

[0113] Among them, A 0,3 A 1,3 A 2,3 A 3,3 For V 0,3 V 1,3 V 2,3 V 3,3 The value of the derivative with respect to s is used as the acceleration basis function A. s The coefficient.

[0114] in,

[0115]

[0116]

[0117] Step 3-11: For pathlist append Fit the points to C, and let C be the point. i C i+1 C i+2 C i+3 Equal to P1, P2, P3, and P4 respectively, we obtain the first path curve P. s1 The first velocity curve V s1 The first acceleration curve A s1 Using the same method, let C i C i+1 C i+2 C i+3 Equal to P2, P3, P4, and P5 respectively, resulting in curve P. s2 V s2 A s2 Similarly, substitute them in one by one until C is satisfied. i C i+1 C i+2 C i+3 They are respectively equal to P n-3 P n-2 P n-1 P n The curve P is obtained. s(n-3) V s(n-3) A s(n-3) .

[0118] Step 3-12: Place P si i = 1, 2, 3, ..., n-3; V si i = 1, 2, 3, ..., n-3; A si ,i=1,2,3,…,n-3; by concatenating the first and second parts of each curve segment, the path curve P can be obtained. (s) velocity curve V (s) Acceleration curve A (s) That is, the path curve, velocity curve, and acceleration curve of the car are fitted by cubic B-spline curves.

[0119] Step 3-13: When calculating the above curves using a computer, substitute the horizontal and vertical coordinate values ​​of each coordinate point, that is, substitute the vector [x,y] composed of x and y. This process is a vector calculation.

[0120] Step 3-14: Obtain the path curve P (s) This refers to the drivable path.

[0121] Step 4: Optimize the path curve to obtain the optimal path. The path curve obtained in Step 3 is optimized. This invention establishes a repulsive force field for obstacles and a curvature formula for the path curve. The repulsive force field and curvature formula are added with different weighting coefficients. Simultaneously, gradient descent is applied to the points on the path, changing their coordinates to find the optimal balance between minimizing the path curvature and avoiding collisions with obstacles. This results in a path with minimal curvature and no collisions with obstacles. Since B-spline curves have local support, the optimization proceeds from the beginning of the path curve to the end. Furthermore, the control point set used for optimization is a pathlist, not a path-specific control point set. append The specific steps are as follows:

[0122] Step 4-1: The repulsive field is established as follows:

[0123] Repulsive field U rep (P) is modeled as follows:

[0124]

[0125] Where η is the repulsive force scale factor, ρ(P,P) obs ) represents the distance between the vehicle's center and the obstacle, and ρ0 represents the radius of influence of the obstacle.

[0126] The gradient of the repulsive field is:

[0127]

[0128] Step 4-2: For curve P (s) Establish the curvature equation and derive the gradient descent loss function from it, with P... s1 For example, the details are as follows:

[0129] First, consider the first segment of curve P. s1 Establish the curvature equation, P s1 The path is formed by reference points P1, P2, P3, and P4. As shown in steps 3-3 and 3-4, P1 and P3 are the control points added later. Starting from P2, P4 is used as the parameter for gradient descent. Coordinate gradient descent is performed on P4 to change its coordinates, thereby achieving the minimum curvature without conflicting with obstacles. The x-coordinate of P4 is taken as... p4 y-axis p4 Let θ0 = x p4 θ1=y p4 Then the vector form is θ = [θ0, θ1] T .

[0130] Step 4-3: From step 3-10, we know that:

[0131]

[0132]

[0133] Step 4-4: To ensure the simplicity of subsequent calculations and facilitate the expression of formulas, the original expression is simplified as follows:

[0134] Let the median value

[0135] Similarly, the median value

[0136] Then V s1 With A s1 It can be simplified to

[0137]

[0138] in, Let P1 be the x and y coordinates;

[0139] Steps 4-5: Obtain P based on the curvature formula s1 Curvature relationship:

[0140]

[0141] For the sake of convenience in subsequent formula representation, K numerator and K denominator These represent the numerator and denominator of the curvature K, respectively.

[0142] Steps 4-6: According to the curvature formula, calculate the partial derivatives with respect to θ0 and θ1 respectively:

[0143]

[0144]

[0145] For ease of explanation later, and None of these will be elaborated upon.

[0146] Step 4-7: Same as step 4-5, calculate the partial derivatives of the repulsive field with respect to θ0 and θ1. The result is:

[0147]

[0148] Steps 4-8: Discretize the curve. The spacing between points in the curve discretization process determines the computational efficiency; the more precise the discretization, the lower the computational efficiency. Curve discretization is fundamental knowledge and will not be elaborated upon here.

[0149] Steps 4-9: Establish the loss function for gradient descent:

[0150] For the repulsive force field, take the center of the obstacle as the reference point P. obs :

[0151]

[0152]

[0153]

[0154] in Let be the loss function for gradient descent with respect to a repulsive field. Let F be the loss function for curvature gradient descent. cost The loss function is the sum of the loss functions of repulsive field gradient descent and curvature gradient descent.

[0155] Step 4-10: Calculate the loss function with respect to θ = [θ0, θ1] T gradient descent direction Gradient descent is initiated to optimize the path curve, where μ1 and μ2 are the proportions of the curve that satisfy the minimum curvature or are farthest from the obstacle, and the specific values ​​are not limited.

[0156] Step 4-11: Each gradient descent iteration is as follows:

[0157]

[0158] Where m is the number of points after discretization of the curve, and α is the learning rate, which determines the step size of gradient descent. Gradient descent is the process of moving the path curve away from obstacles and minimizing its curvature.

[0159] Step 4-12: For P s1 After performing gradient descent optimization N times, select the option for P. s2 ,P s3 ,P s4 …P s(n-3) Optimize sequentially, that is, optimize P5, P6, P7...P n-3 The optimal value is found by using the x and y coordinates. Similarly, the gradient descent is performed N times.

[0160] Steps 4-13: Optional, for P s1 ,P s2 ,P s3 …P s(n-3) The optimization order of line segments can be in ascending, descending, or random order. The optimized curve P... (s) In other words, to satisfy the requirement of a path with small curvature and far from obstacles, the larger the number of gradient descents N, the closer the curve is to the local optimum.

[0161] For further details, please refer to... Figure 4 Using P s1 ,P s2 ,P s3 ,P s4 …P s(n-3) The optimized sequence allows the vehicle to optimize its path curve while driving. During the journey, the vehicle will first follow path P... s1 During driving, P can be gradually adjusted. s2 ,P s3 ,P s4 …P s(n-3) Optimization can increase the optimization time of subsequent path curves, increase the number of gradient descent optimization iterations, and bring the path curve closer to the optimum.

[0162] Figure 5 This section compares the optimization results before and after optimization for a certain path curve. Based on simulation results, such as... Figure 5 As shown in (a) above, the optimized curve (solid line) is significantly smoother than the unoptimized curve (dashed line). Figure 5 As shown in (b), the maximum curvature of the path curve decreased from 3.0 before optimization (dashed line) to 0.7 after optimization (solid line), indicating a significant decrease in curvature. Figure 5 As shown in (c), the repulsive force of the obstacle on the curve decreases significantly from a maximum repulsive force of 26.2 before optimization (dashed line) to a maximum repulsive force of 13.12 after optimization (solid line). The simulation results show that the optimized path, compared to the unoptimized path, increases curvature while moving further away from the obstacle, thus improving vehicle ride comfort and safety.

[0163] like Figure 6 As shown, simulation results were presented for different paths and obstacle locations (i.e., different scenarios). The optimized path (solid line) is significantly better than the unoptimized path (dashed line) in terms of curvature and distance from obstacles. Table 1 shows the specific values ​​of path curvature, obstacle repulsion force, and optimization time before and after optimization in different scenarios.

[0164] Table 1: Figure 6 Specific numerical values ​​of optimization methods in different scenarios

[0165]

[0166] The above description is merely a preferred embodiment of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention are included within the scope of protection of the present invention.

Claims

1. A method for unstructured road path planning based on the JPS algorithm and gradient descent, characterized in that, Includes the following steps: Step 1: Initialize map information and path node information; Step 2: Obtain a series of collision-free control nodes from the starting point to the target point, and obtain the angle between the path formed by the connecting lines of the nodes and the x-axis of the map; Step 3: Add auxiliary control points to the control nodes and perform path curve fitting; Step 4: Optimize the path curve to obtain the optimal path; Step 2 specifically involves: Step 2-1: Initialize two lists , and additionally initialize the list. ; Step 2-2: Use the JPS algorithm to add the nodes to be searched. Add nodes that have already been searched Generate from the starting point To the target point Collision-free path ;in, , The x and y coordinates of the vehicle at the starting point. , The x and y coordinates of the vehicle at the target point. The vehicle's orientation at the starting point. The vehicle's orientation at the target point; Steps 2-3: Obtain the path from the starting point to the target point. Then, the path Each node in From the starting point To the target point Add in sequence middle, ,in, , , Starting point and target point Path points between; Steps 2-4: Calculate all nodes except the starting point and the target point. The average slope between each node and its parent and child nodes is converted into the angle with the x-axis of the coordinate system. Step 4 specifically involves: A repulsive force field is established for the obstacle, and a curvature formula is established for the path curve. The repulsive force field and the curvature formula are added with different weight coefficients. At the same time, gradient descent is applied to the points on the path in turn to change the coordinates of the points on the path. The optimal value is obtained between moving away from the obstacle and minimizing the curvature of the path, so as to obtain a path with no collision with the obstacle and a small curvature.

2. The unstructured road path planning method based on the JPS algorithm and gradient descent according to claim 1, characterized in that, Step 1 specifically involves: Step 1-1: Obtain the map of the current environment, rasterize the map, and divide it into segments based on the vehicle's length. Inflate obstacles in a raster map; Steps 1-2: Based on the rasterized map, establish a coordinate system and initialize the vehicle's current location. , where x and y represent the horizontal and vertical coordinates of the vehicle in the coordinate system, and θ represents the vehicle's heading angle; Steps 1-3: Determine the starting point based on the vehicle's current location. Set the target point as ;in , The x and y coordinates of the vehicle at the starting point. , The x and y coordinates of the vehicle at the target point. The vehicle's orientation at the starting point. This represents the vehicle's orientation at the target point.

3. The unstructured road path planning method based on the JPS algorithm and gradient descent according to claim 1, characterized in that, Step 3 specifically involves: Based on the control node obtained in step 2 For all nodes except the starting point and the target point The Manhattan distance between each node and its parent and child nodes is determined sequentially. If there are other nodes within one grid cell, it means that there are obstacles in the area around the node and that it is an inflection point of the path obtained by the JPS algorithm. Then, auxiliary control points are added to both ends of this control point. Auxiliary control points are added to the starting point and the target point according to the initial pose. The basis function of the cubic B-spline curve is established and fitted to the control nodes to obtain a path with continuous curvature from the starting point to the target point.

4. The unstructured road path planning method based on the JPS algorithm and gradient descent according to claim 3, characterized in that, All nodes except the starting point and the target point The specific method for adding auxiliary control points is as follows: Step 3-1: Calculation and , and Manhattan distance , ,when or When this occurs, it indicates that there are obstacles around the node, requiring the addition of auxiliary control points; among them, , These are nodes The horizontal and vertical coordinates; , These are nodes respectively The horizontal and vertical coordinates; Step 3-2: When When additional auxiliary control points are needed, add auxiliary control points on both sides. , , and for The newly added auxiliary control points on both sides of the point, among which It is half the length of the car body. for The vehicle's orientation at that time; Step 3-3: Calculate sequentially The Manhattan distance between each node and its parent and child nodes is used to determine whether an auxiliary control point needs to be added. Steps 3-4: Update the node list with the added auxiliary control points. After adding auxiliary control points to the node list, define the node list containing auxiliary control points as follows: ,Right now: 。 5. The unstructured road path planning method based on the JPS algorithm and gradient descent according to claim 4, characterized in that, At the starting point and target point The specific method for adding auxiliary control points is as follows: Steps 3-6: Define L as half the length of the vehicle body, and select points. , Define the starting point vehicle speed and direction. The speed and direction of travel to the target point are ;in, , The x and y coordinates of the vehicle at the starting point. , The x and y coordinates of the vehicle at the target point. The vehicle's orientation at the starting point. The vehicle's orientation at the target point; Steps 3-7: Add a starting point Control points on both sides , Add target points Control points on both sides , Where L is half the length of the vehicle body; Steps 3-8: Add the new control points to middle, ;renew Internal node name, ;at this time .

6. A method for unstructured road path planning based on the JPS algorithm and gradient descent according to claim 1, 3, or 5, characterized in that, The path curve fitting method in step 3 is as follows: Step 3-9: Establish the basis functions for the cubic B-spline curve: ； The coefficients of the cubic B-spline curve for: ； s is the reference point for the cubic B-spline curve, and s is the vector formed by discrete points when the B-spline curve is drawn and calculated by the computer. Step 3-10: Obtain the velocity basis function by taking the first derivative from the basis function of the cubic B-spline curve. And by taking the second derivative to obtain the acceleration basis function. : ； in They are respectively The value of the derivative with respect to s is used as the velocity basis function. The coefficient; ； in, They are respectively The value of the derivative with respect to s is used as the basis function of acceleration. The coefficient; in, ； ； Step 3-11: [Regarding...] Fit the points to let They are respectively equal to The first path curve is obtained. The first velocity curve The first acceleration curve ;make They are respectively equal to To obtain the curve Substitute them in sequence until you get They are respectively equal to To obtain the curve ; Step 3-12: [The text appears to be incomplete and contains several grammatical errors. A more accurate translation would require the full context.] ; ; By splicing the first and second segments of each curve, the path curve can be obtained. , acceleration curve .

7. The unstructured road path planning method based on JPS algorithm and gradient descent according to claim 1, characterized in that, The specific operation process of step 4 is as follows: Step 4-1: Establish a repulsive field : ； in, It is the repulsive force scale factor. Represents the distance between the center of the vehicle and the center of the obstacle. Represents the radius of influence of the obstacle; The gradient of the repulsive field is: ； Step 4-2: For the curve The first curve Establish the curvature equation, and derive the gradient descent loss function from the curvature equation: The The equation of curvature is: ； in, and Representing curvature respectively The numerator and denominator, = , = , and These are nodes The x and y coordinates, , , , ; in, , , , These are intermediate values ​​proposed for ease of expression. velocity basis functions coefficient, Acceleration basis functions coefficient, , They are respectively x and y coordinates , They are respectively x and y coordinates , They are respectively The horizontal and vertical coordinates; ； ； Where s is a vector composed of discrete points when the B-spline curve is drawn and calculated by the computer; The loss function of gradient descent includes: ； ； ； in Let be the loss function for gradient descent with respect to a repulsive field. Let be the loss function for curvature gradient descent. Let be the loss function for gradient descent; Step 4-3: Calculate the loss function with respect to the vector gradient descent direction Gradient descent is then initiated to optimize the path curve, where... and For a curve to satisfy the condition of minimum curvature or the proportion of the distance from the obstacle, , ,in, Indicates curvature K pairs The partial derivatives, Indicates curvature K pairs The partial derivatives; For repulsive field The partial derivatives, For repulsive field pairs The partial derivatives; Step 4-4: Perform gradient descent using the following formula: ； in, This represents the optimized vector form, where m is the number of points after the curve is discretized. The learning rate; Steps 4-5: [Regarding...] After performing gradient descent N times, the curve Optimize sequentially; Steps 4-6: After all curves have been optimized, the optimal path is obtained.

8. The unstructured road path planning method based on JPS algorithm and gradient descent according to claim 7, characterized in that, In step 4: for the route In addition to the forward order, the optimization order of line segments also includes the reverse order and random order; the larger the number of gradient descent operations N, the closer the curve is to the local optimum.

Images