Local path planning method and system with obstacle as anchor point

Abstract

The invention relates to a local path planning method and system with an obstacle as an anchor point, and belongs to the technical field of unmanned vehicles. The method comprises the following steps: acquiring a road obstacle, a reference line, a boundary position and a current position of a vehicle, and converting the positions into a frenet coordinate system; acquiring the starting point, the ending point and the width of each obstacle; traversing the reference line at a fixed step length, generating a new channel, namely a new node, based on the starting point and the ending point of the obstacle, and generating a tree node graph based on all the nodes; traversing all paths in the node graph, and solving the solution space of each path by adopting a quadratic programming method to obtain the trajectory of the path and obtain the score of the path; and selecting the path with the highest score as the optimal path. According to the method, the obstacle is taken as an anchor point, the road boundary is taken as a strong constraint, and a solution space of the path is constructed from a global perspective, so that a global optimal local path is planned, and the situation of unreasonable path planning such as obstacle surrounding and turning is avoided.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned vehicles, and in particular to a local path planning method and system using obstacles as anchor points. Background Art

[0002] As a hot topic in vehicle research today, unmanned driving technology has received widespread attention. Path planning for unmanned vehicles is one of the difficulties in unmanned driving technology. Path planning for unmanned vehicles is based on the planning of local paths to obtain the target path, so the quality of the target path depends on the quality of the local path.

[0003] The existing local path planning methods mainly include the scattering method and the optimization method. The scattering method is to sample the road horizontally within a certain longitudinal range of the road, take the current position of the vehicle as the starting point, and take the position of each sampling point as the end point to perform a fifth-order polynomial fitting; however, when there are multiple obstacles within the longitudinal range, the fifth-order polynomial cannot fit a curve that bypasses multiple obstacles, and often each fitted curve collides with the obstacle, resulting in no solution for path planning. The optimization method is to construct an optimization function and use the quadratic programming method to solve the optimal solution from the solution space of the path. Therefore, for the optimization method, the key factor is how to open up the solution space. The existing optimization method framework, such as apollo, does not find a solution space for local path planning from a global perspective when opening up the solution space, which results in the local path being not the global optimal solution or the planned path being easily surrounded by obstacles, detours, and other unreasonable path planning situations.

[0004] The existing technology mainly has the following defects: first, the existing quintic polynomial fitting algorithm is often unable to fit a curve that bypasses multiple obstacles, resulting in no solution for path planning; second, the existing optimization method framework does not find the solution space for local path planning from a global perspective, which results in the local path not being the global optimal solution or the planned path is easily surrounded by obstacles, detours, and other unreasonable path planning situations. Summary of the invention

[0005] In view of the above analysis, the present invention aims to provide a local path planning method and system with obstacles as anchor points, so as to solve the problem that the existing local path planning has no solution or the solved local path is not the global optimal solution or the solved local path planning is unreasonable.

[0006] The present invention provides a local path planning method using obstacles as anchor points, the method comprising the following steps:

[0007] Obtain the positions of obstacles and road boundaries within a certain length of the road and the current position of the vehicle, and use the center line of the road where the vehicle is currently located as a reference line; transform the above positions from the Cartesian coordinate system to the Frenet coordinate system;

[0008] Obtain the starting point and end point of each obstacle; traverse the reference line with a fixed step size, divide the current channel into two channels when there is an obstacle starting point, and merge the two channels or equivalent channels divided by the corresponding obstacle starting point into one channel when there is an obstacle end point; each channel is regarded as a different node, and a tree node diagram is generated based on all nodes;

[0009] All paths in the node graph are obtained, and the solution space of each path is solved by the quadratic programming method to obtain the trajectory of the path and the score of the path is obtained based on the curve length, reference line length, curvature, minimum turning radius of the vehicle and the number of sampling points of the path trajectory; the path with the highest score is selected as the optimal path.

[0010] Furthermore, obtaining the starting point and the end point of each obstacle includes:

[0011] For each obstacle: obtain the Frenet coordinates of each vertex of the minimum enveloping polygon of the obstacle; select the maximum and minimum values ​​of s and l in the vertex coordinates, take the point with the minimum s value as the starting point of the obstacle, and take the point with the maximum s value as the end point of the obstacle;

[0012] Arrange the starting points and end points of all obstacles in ascending order of s value.

[0013] Further, the traversing the reference line with a fixed step size, dividing the current channel into two channels when there is an obstacle starting point, and merging the two channels divided by the corresponding obstacle starting point or equivalent channels into one channel when there is an obstacle end point includes:

[0014] Taking the current s coordinate of the vehicle as the starting point, traverse along the reference line with a fixed step size to perform obstacle detection; when there is an obstacle starting point, the current channel is divided into two new channels based on the l coordinate of the current channel boundary, the minimum and maximum l coordinates of the obstacle; when there is an obstacle end point, the two channels divided by the obstacle starting point corresponding to the obstacle end point or their equivalent channels are merged into a new channel, the boundary of which is the same as the boundary of the channel before the obstacle starting point is divided; equivalent channels are channels with the same boundaries.

[0015] Furthermore, each channel is a different node, and generating a tree node diagram based on all nodes includes:

[0016] The channel where the vehicle's current position is located is taken as the starting node, and the channel where the path planning end position is located is taken as the ending node; each node in the tree node diagram except the ending node contains the length and width of the respective channel, whether the vehicle can pass through, and the position list of each child node in the next layer of nodes that can be connected; the ending node contains the length and width of the node channel;

[0017] When there is an obstacle starting point, the obstacle starting point divides the current channel into two new channels, that is, generates two new child nodes in the next layer node; when there is an obstacle end point, the obstacle end point merges the two channels divided by the corresponding obstacle starting point or equivalent channels into a new channel. If the length of the new channel is less than the turning radius of the vehicle and there is an obstacle starting point at the end s coordinate of the channel, two child nodes with the same length and width are generated in the next layer node, otherwise a new child node is generated in the next layer node;

[0018] The structure formed by all nodes based on the connection relationship between the nodes is the tree-shaped node graph.

[0019] Furthermore, the solution space of each path is solved by a quadratic programming method to obtain the trajectory of the path, including:

[0020] Construct a cost function and set the cost function as the objective function of the quadratic programming; set the constraints of the quadratic programming based on the vehicle width, the s coordinates of each node in the path, and the maximum and minimum values ​​of the l coordinates; traverse along the reference line between the minimum s coordinate and the maximum s coordinate with a fixed step size to solve the optimal l coordinate corresponding to the current s coordinate; connect all the corresponding optimal l coordinates to obtain the solution space trajectory of the path, which is the trajectory of the path.

[0021] Furthermore, the following method is used to obtain the score of the path:

[0022]

[0023]

[0024] Where L is the curve length of the path trajectory, L r is the length of the reference line corresponding to the path, k max is the inverse of the vehicle’s minimum turning radius, k i is the curvature of the path trajectory point i corresponding to each sampling point, K s For all k i The sum of the absolute values ​​of , w1 and w2 are weight values, and n is the number of sampling points.

[0025] Furthermore, all paths in the node graph are obtained by the following method:

[0026] Starting from the starting node of the node graph, connect the next layer of nodes along the tree structure of the node graph; determine whether each child node in the next layer of nodes can allow the vehicle to pass through. If not, stop connecting the child node to the next layer of nodes. If so, continue connecting the child node to the next layer of nodes; stop connecting when the next layer of nodes is the end node; all paths connecting the starting node and the end node are all paths in the node graph.

[0027] Furthermore, the frenet coordinate system includes an s coordinate and an l coordinate, the s coordinate is the length along the reference line from the reference line zero point, and the absolute value of the l coordinate is the distance from the reference line.

[0028] Furthermore, the cost function is obtained by the following method:

[0029]

[0030] Among them, l is the l coordinate of the path trajectory to be planned, l', l", l'' are the first, second and third order derivatives of the l coordinate respectively, and w l 、w dl 、w ddl 、w dddl are the weights of l, l', l", and l'' respectively, and n is the number of sampling points.

[0031] Furthermore, the system comprises:

[0032] LiDAR, used to obtain obstacles, reference lines, road boundary positions and the current position of the vehicle within a certain length of the road;

[0033] A coordinate transformation module is used to transform obstacles, reference lines, road boundary positions and the current position of the vehicle into the Frenet coordinate system, and input the coordinates into the node graph generation module and the path planning module;

[0034] The node graph generation module obtains the starting point and the end point of each obstacle; traverses the reference line with a fixed step size, divides the current channel into two channels when there is an obstacle starting point, and merges the two channels or equivalent channels divided by the corresponding obstacle starting point into one channel when there is an obstacle end point; each channel is regarded as a different node, a tree node graph is generated based on all nodes, and the node graph is input into the path planning module;

[0035] The path planning module is used to obtain all paths in the node graph, use the quadratic programming method to solve the solution space of each path to obtain the trajectory of the path, and obtain the score of the path based on the curve length, reference line length, curvature, vehicle minimum turning radius and number of sampling points of the path trajectory; select the path with the highest score as the optimal path and output it.

[0036] Compared with the prior art, the present invention can achieve at least one of the following beneficial effects:

[0037] 1. The present invention uses obstacles as anchor points and road boundaries as strong constraints to construct a solution space for path planning from a global perspective, thereby planning a globally optimal local path and avoiding unreasonable path planning situations such as the local path being surrounded by obstacles or taking detours.

[0038] 2. The present invention adopts a quadratic programming method to solve the solution space of the local path, thereby avoiding the problem that the local path planning often has no solution through polynomial fitting when there are multiple obstacles.

[0039] 3. The present invention obtains the score of the path through the curve length, curvature and reference line length of the path trajectory, which not only provides a quantitative evaluation method but also has a small amount of calculation, thereby enabling the optimal path to be selected quickly.

[0040] 4. When all the paths in the node graph are obtained, the present invention stops path searching for nodes that the vehicle cannot pass through, thereby improving the efficiency of selecting the optimal path.

[0041] 5. The cost function of the quadratic programming of the present invention includes the first-order, second-order, and third-order derivatives of the l coordinate, which can obtain a smooth solution space trajectory, thereby reducing vehicle vibration and making the vehicle run safer and more stable.

[0042] In the present invention, the above-mentioned technical solutions can also be combined with each other to achieve more preferred combination solutions. Other features and advantages of the present invention will be described in the subsequent description, and some advantages can become obvious from the description, or can be understood by practicing the present invention. The purpose and other advantages of the present invention can be realized and obtained through the contents particularly pointed out in the description and the drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] The drawings are only for the purpose of illustrating particular embodiments and are not to be considered limiting of the present invention. Like reference symbols denote like components throughout the drawings.

[0044] Figure 1 A flowchart of a local path planning method using obstacles as anchor points according to an embodiment of the present invention;

[0045] Figure 2 is a schematic diagram of frenet coordinates according to an embodiment of the present invention;

[0046] Figure 3 A schematic diagram of dividing the current channel into two channels at the starting point of an obstacle in an embodiment of the present invention;

[0047] Figure 4A schematic diagram of the distribution of road channels and its corresponding node diagram according to an embodiment of the present invention;

[0048] Figure 5 A schematic diagram of another road channel distribution and its corresponding node diagram according to an embodiment of the present invention;

[0049] Figure 6 A block diagram of a local path planning system with obstacles as anchor points according to an embodiment of the present invention. DETAILED DESCRIPTION

[0050] The preferred embodiments of the present invention are described in detail below in conjunction with the accompanying drawings, wherein the accompanying drawings constitute a part of this application and are used together with the embodiments of the present invention to illustrate the principles of the present invention, but are not used to limit the scope of the present invention.

[0051] A specific embodiment of the present invention discloses a local path planning method using obstacles as anchor points. Figure 1 As shown, the method comprises the following steps:

[0052] Step S1, obtaining the positions of obstacles and road boundaries within a certain length of the road and the current position of the vehicle, taking the center line of the road where the current position of the vehicle is located as a reference line; transforming the above positions from a Cartesian coordinate system to a Frenet coordinate system;

[0053] Step S2, obtaining the starting point and the end point of each obstacle; traversing the reference line with a fixed step length, dividing the current channel into two channels when there is an obstacle starting point, and merging the two channels or equivalent channels divided by the corresponding obstacle starting point into one channel when there is an obstacle end point; each channel is regarded as a different node, and a tree node diagram is generated based on all nodes;

[0054] Step S3, obtaining all paths in the node graph, using the quadratic programming method to solve the solution space of each path to obtain the trajectory of the path and obtain the score of the path based on the curve length, reference line length, curvature, minimum turning radius of the vehicle and the number of sampling points of the path trajectory; selecting the path with the highest score as the optimal path.

[0055] Specifically, in step S1, the positions of obstacles and road boundaries within a certain length of the road and the current position of the vehicle are obtained by the laser radar on the vehicle. The position of the obstacle includes the x and y coordinates of each vertex of the minimum enveloping polygon of the obstacle in 2D space, the position of the road boundary includes the x and y coordinates of the left and right boundaries of the road, and the current position of the vehicle includes the x and y coordinates and heading of the four wheels of the vehicle. The center line of the road where the vehicle is currently located is used as a reference line, and the vehicle travels along the reference line when there are no obstacles.

[0056] Furthermore, when solving the local path planning, the problem is transformed from the Cartesian coordinate system to the Frenet coordinate system for solution. The Frenet coordinate system includes an s coordinate and an l coordinate, wherein the s coordinate is the length along the reference line from the reference line zero point, and the absolute value of the l coordinate is the distance from the reference line, such as Figure 2 shown.

[0057] Exemplarily, the laser radar acquires information 10 times per second, and acquires the position of obstacles and road boundaries within 100 meters of the road and the current position of the vehicle each time; the reference line is transformed from the Cartesian coordinate system to the Frenet coordinate system, and the reference line position corresponding to the rear wheel of the vehicle's current position is used as the zero point of the reference line, and the vehicle's heading is used as the direction of the reference line. The obstacle position is transformed from the Cartesian coordinate system to the Frenet coordinate system, and a perpendicular line is drawn from any vertex of the obstacle's minimum envelope polygon to the reference line, and the length along the reference line between the foot of the perpendicular and the zero point of the reference line is used as the s coordinate of the vertex; the distance between the vertex and the foot of the perpendicular is used as the absolute value of the l coordinate of the vertex, and if the vertex is on the left side of the reference line, the l coordinate is positive, otherwise the l coordinate is negative; the s and l coordinates of each vertex of the obstacle's minimum envelope polygon can be obtained by the above method.

[0058] It can be understood that the above method can obtain the s and l coordinates of all obstacles within 100 meters of the road, the positions of the road boundaries, and the current position of the vehicle. At any s position, the maximum and minimum values ​​of l are the left and right boundaries of the road, respectively. Therefore, the difference between the maximum and minimum values ​​of l is the width of the road. The boundary constraint formed by the road width is the natural solution space for path planning. When there are no obstacles, path planning is solved in the natural solution space.

[0059] Specifically, in step S2, obtaining the starting point and the end point of each obstacle includes:

[0060] For each obstacle: obtain the Frenet coordinates of each vertex of the minimum enveloping polygon of the obstacle; select the maximum and minimum values ​​of s and l in the vertex coordinates, take the point with the minimum s value as the starting point of the obstacle, and take the point with the maximum s value as the end point of the obstacle;

[0061] Arrange the starting points and end points of all obstacles in ascending order of s value.

[0062] Specifically, the information of the starting point or end point of the obstacle is [flag, s, l_obs_min, l_obs_max]; flag is 1 for the starting point of the obstacle, and 0 for the end point of the obstacle; when flag is 1, s is the minimum s coordinate of the obstacle, and when flag is 0, s is the maximum s coordinate of the obstacle; l_obs_min is the minimum l coordinate of the obstacle, and l_obs_max is the maximum l coordinate of the obstacle.

[0063] Further, the traversing the reference line with a fixed step size, dividing the current channel into two channels when there is an obstacle starting point, and merging the two channels divided by the corresponding obstacle starting point or equivalent channels into one channel when there is an obstacle end point includes:

[0064] Taking the current s coordinate of the vehicle as the starting point, traverse along the reference line with a fixed step size to perform obstacle detection; when there is an obstacle starting point, the current channel is divided into two new channels based on the l coordinate of the current channel boundary, the minimum and maximum l coordinates of the obstacle; when there is an obstacle end point, the two channels divided by the obstacle starting point corresponding to the obstacle end point or their equivalent channels are merged into a new channel, the boundary of which is the same as the boundary of the channel before the obstacle starting point is divided; equivalent channels are channels with the same boundaries.

[0065] For example, Figure 3 As shown, the current s coordinate of the vehicle, i.e., the zero point of the reference line, is taken as the starting point. Preferably, the current s coordinate of the vehicle + 2 meters is taken as the starting point. Obstacle detection is performed by traversing along the reference line with a fixed step length of 0.5 meters. When there is an obstacle starting point, the current channel is divided into two new left and right channels, the boundary of the left channel is [l_max, l_obs_max], and the boundary of the right channel is [l_obs_min, l_min]. When there is an obstacle end point, the left and right channels divided by the obstacle starting point are merged into a new channel, the boundary of which is [l_max, l_min].

[0066] Furthermore, each channel is a different node, and generating a tree node diagram based on all nodes includes:

[0067] The channel where the vehicle's current position is located is taken as the starting node, and the channel where the path planning end position is located is taken as the ending node; each node in the tree node diagram except the ending node contains the length and width of the respective channel, whether the vehicle can pass through, and the position list of each child node in the next layer of nodes that can be connected; the ending node contains the length and width of the channel corresponding to the node;

[0068] When there is an obstacle starting point, the obstacle starting point divides the current channel into two new channels, that is, generates two new child nodes in the next layer node; when there is an obstacle end point, the obstacle end point merges the two channels divided by the corresponding obstacle starting point or equivalent channels into a new channel. If the length of the new channel is less than the turning radius of the vehicle and there is an obstacle starting point at the end s coordinate of the channel, two child nodes with the same length and width are generated in the next layer node, otherwise a new child node is generated in the next layer node;

[0069] The structure formed by all nodes based on the connection relationship between the nodes is the tree-shaped node graph.

[0070] For example, Figure 4 As shown in the figure, each node represents a channel, the starting node is node A, and the ending node is node G. The information contained in node G is [flag, start_s, end_s, lmin, lmax]; flag is 1, which means that the vehicle can pass through the node, and 0, which means that the vehicle cannot pass through the node; start_s is the starting s coordinate of the node, and end_s is the ending s coordinate of the node; lmin is the minimum l coordinate of the node, and lmax is the maximum l coordinate of the node. The information contained in the remaining nodes in the tree node diagram is [flag, start_s, end_s, lmin, lmax, next_node_addrs]; next_node_addrs is the location list of each child node in the next layer of nodes that can be connected.

[0071] Specifically, the width of the vehicle itself is expanded to a certain extent as the width of the vehicle. For example, the width of the vehicle itself is increased by 1 meter as the width of the vehicle. The width of the node is obtained by the difference between the node lmax and lmin. If the width of the vehicle is less than the width of the node, the vehicle can pass through the node and the flag of the node is set to 1. Otherwise, the flag of the node is set to 0.

[0072] like Figure 4As shown, when there is a starting point of obstacle 1, the current channel where the starting point of obstacle 1 is located is the channel corresponding to node A, and the starting point of obstacle 1 divides the channel corresponding to A into channels corresponding to B and C, and the next layer of nodes generates two child nodes B and C; when there is a starting point of obstacle 2, the current channel where the starting point of obstacle 2 is located is the channel corresponding to node C, and the starting point of obstacle 2 divides the channel corresponding to C into channels corresponding to D and E, and the next layer of nodes generates two child nodes D and E; when there is an end point of obstacle 2, the end point of obstacle 2 merges the channels corresponding to D and E divided by the starting point of obstacle 2 into the channel corresponding to F, and since there is no obstacle starting point at the end s coordinate of the channel corresponding to F, the next layer of nodes generates a child node F; when there is an end point of obstacle 1, since the boundaries of the channels corresponding to F and C are the same, the channels corresponding to F and C are equivalent channels, and the end point of obstacle 1 merges the channels corresponding to B and F divided by the starting point of obstacle 1 into the channel corresponding to G, G is the termination node, and the next layer of nodes generates a child node G.

[0073] For example, Figure 5 As shown, when there is an end point of obstacle 1, since the channels corresponding to F and C are equivalent channels, the end point of obstacle 1 merges the channel corresponding to B and the channel corresponding to F divided by the starting point of obstacle 1 into the channel corresponding to G; since the length of G is less than the turning radius of the vehicle and the starting point of obstacle 3 exists at the terminal s coordinate of G, the next layer of nodes generates two child nodes G1 and G2 with the same length and width.

[0074] It is understandable that the vehicle has a minimum turning radius when turning. Since the wheelbase of the vehicle and the maximum turning angle of the front wheels of the vehicle are known, the minimum turning radius of the vehicle can be obtained according to the vehicle kinematics principle. When the length of the node is less than the minimum turning radius of the vehicle and there is an obstacle starting point at the terminal s coordinate of the node, the vehicle cannot bypass the obstacle by turning. Figure 5 In the example, since the length of G is less than the turning radius of the vehicle, the vehicle cannot reach I from node B through G, or reach H from node F through G. Therefore, two independent nodes G1 and G2 are generated, and the vehicle can reach H from node B through G1, or reach I from node F through G2.

[0075] All nodes form a unidirectional structure based on the connection relationship between the nodes, and the structure is the tree-shaped node graph.

[0076] Specifically, in step S3, all paths in the node graph are obtained by the following method:

[0077] Starting from the starting node of the node graph, connect the next layer of nodes along the tree structure of the node graph; determine whether each child node in the next layer of nodes can allow the vehicle to pass through. If not, stop connecting the child node to the next layer of nodes. If so, continue connecting the child node to the next layer of nodes; stop connecting when the next layer of nodes is the end node; all paths connecting the starting node and the end node are all paths in the node graph.

[0078] Specifically, the flag value of the node is read to determine whether the node allows vehicles to pass through. A flag value of 1 indicates that the node allows vehicles to pass through, and a flag value of 0 indicates that the node does not allow vehicles to pass through.

[0079] Furthermore, the solution space of each path is solved by a quadratic programming method to obtain the trajectory of the path, including:

[0080] Construct a cost function and set the cost function as the objective function of the quadratic programming; set the constraints of the quadratic programming based on the vehicle width, the s coordinates of each node in the path, and the maximum and minimum values ​​of the l coordinates; traverse along the reference line between the minimum s coordinate and the maximum s coordinate with a fixed step size to solve the optimal l coordinate corresponding to the current s coordinate; connect all the corresponding optimal l coordinates to obtain the solution space trajectory of the path, which is the trajectory of the path.

[0081] Specifically, each path can generate a solution space, each path includes multiple nodes, each node contains four elements of the node's starting s coordinate, ending s coordinate, left boundary l coordinate, and right boundary l coordinate; by traversing each node on any path, the maximum and minimum constraints of the l coordinate at every fixed step sampling point along the reference line can be generated. Assuming that the width of the vehicle is car_width, the solution space constraint at each sampling point is [lmin+car_width, lmax-car_width]; since the boundaries of each node are different, the constraints of the corresponding channels of each node are different; the constraints constitute the solution space for local path planning, and local path planning can be performed in this solution space to obtain an optimal local path.

[0082] Specifically, the cost function is obtained by the following method:

[0083]

[0084] Among them, l is the l coordinate of the path trajectory to be planned, l', l", l'' are the first, second and third order derivatives of the l coordinate respectively, and w l 、w dl 、w ddl 、w dddlare the weights of l, l', l", and l'' respectively, and n is the number of sampling points.

[0085] The l coordinate corresponding to each sampling point when the cost function is minimized is solved, and the l coordinate is the optimal solution for the sampling point.

[0086] Furthermore, the following method is used to obtain the score of the path:

[0087]

[0088]

[0089] Where L is the curve length of the path trajectory, L r is the length of the reference line corresponding to the path, k max k is the maximum allowable curvature, i.e. the inverse of the minimum turning radius of the vehicle. i is the curvature of the path trajectory point i corresponding to each sampling point, K s For all k i The sum of the absolute values ​​of , w1 and w2 are weight values, and n is the number of sampling points.

[0090] Another specific embodiment of the present invention discloses a local path planning system using obstacles as anchor points. Figure 6 As shown, the system includes:

[0091] LiDAR, used to obtain obstacles, reference lines, road boundary positions and the current position of the vehicle within a certain length of the road;

[0092] A coordinate transformation module transforms obstacles, reference lines, road boundary positions and the current position of the vehicle into the Frenet coordinate system, and inputs the coordinates into the node graph generation module and the path planning module;

[0093] The node graph generation module obtains the starting point and the end point of each obstacle; traverses the reference line with a fixed step size, divides the current channel into two channels when there is an obstacle starting point, and merges the two channels or equivalent channels divided by the corresponding obstacle starting point into one channel when there is an obstacle end point; each channel is regarded as a different node, a tree node graph is generated based on all nodes, and the node graph is input into the path planning module;

[0094] The path planning module is used to obtain all paths in the node graph, use the quadratic programming method to solve the solution space of each path to obtain the trajectory of the path, and obtain the score of the path based on the curve length, reference line length, curvature, vehicle minimum turning radius and number of sampling points of the path trajectory; select the path with the highest score as the optimal path and output it.

[0095] Compared with the prior art, the local path planning method and system with obstacles as anchor points provided by the present invention have the following beneficial effects:

[0096] 1. The present invention uses obstacles as anchor points and road boundaries as strong constraints to construct a solution space for path planning from a global perspective, thereby planning a globally optimal local path and avoiding unreasonable path planning situations such as the local path being surrounded by obstacles or taking detours.

[0097] 2. The present invention adopts a quadratic programming method to solve the solution space of the local path, thereby avoiding the problem that the local path planning often has no solution through polynomial fitting when there are multiple obstacles.

[0098] 3. The present invention obtains the score of the path through the curve length, curvature and reference line length of the path trajectory, which not only provides a quantitative evaluation method but also has a small amount of calculation, thereby enabling the optimal path to be selected quickly.

[0099] 4. When all the paths in the node graph are obtained, the present invention stops path searching for nodes that the vehicle cannot pass through, thereby improving the efficiency of selecting the optimal path.

[0100] 5. The cost function of the quadratic programming of the present invention includes the first-order, second-order, and third-order derivatives of the l coordinate, which can obtain a smooth solution space trajectory, thereby reducing vehicle vibration and making the vehicle run safer and more stable.

[0101] Those skilled in the art will appreciate that all or part of the processes of the above-mentioned embodiments can be implemented by instructing related hardware through a computer program, and the program can be stored in a computer-readable storage medium, wherein the computer-readable storage medium is a disk, an optical disk, a read-only storage memory, or a random access memory, etc.

[0102] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by any technician familiar with the technical field within the technical scope disclosed by the present invention should be covered within the protection scope of the present invention.

Claims

1. A local path planning method using obstacles as anchor points, characterized in that: The method comprises the following steps: Obtain the positions of obstacles and road boundaries within a certain length of the road and the current position of the vehicle, and use the center line of the road where the vehicle is currently located as a reference line; transform the above positions from the Cartesian coordinate system to the Frenet coordinate system; Obtain the starting point and end point of each obstacle; traverse the reference line with a fixed step size, divide the current channel into two channels when there is an obstacle starting point, and merge the two channels or equivalent channels divided by the corresponding obstacle starting point into one channel when there is an obstacle end point; each channel is regarded as a different node, and a tree node diagram is generated based on all nodes; All paths in the node graph are obtained, and the solution space of each path is solved by the quadratic programming method to obtain the trajectory of the path and the score of the path is obtained based on the curve length, reference line length, curvature, minimum turning radius of the vehicle and the number of sampling points of the path trajectory; the path with the highest score is selected as the optimal path.

2. The local path planning method with obstacles as anchor points according to claim 1, characterized in that: The obtaining of the starting point and the end point of each obstacle comprises: For each obstacle: obtain the Frenet coordinates of each vertex of the minimum enveloping polygon of the obstacle; select the maximum and minimum values ​​of s and l in the vertex coordinates, take the point with the minimum s value as the starting point of the obstacle, and take the point with the maximum s value as the end point of the obstacle; Arrange the starting points and end points of all obstacles in ascending order of s value.

3. The method and system for local path planning with obstacles as anchor points according to claim 1, characterized in that: The traversing the reference line with a fixed step length, dividing the current channel into two channels when there is an obstacle starting point, and merging the two channels divided by the corresponding obstacle starting point or equivalent channels into one channel when there is an obstacle end point includes: Taking the current s coordinate of the vehicle as the starting point, traverse along the reference line with a fixed step size to perform obstacle detection; when there is an obstacle starting point, the current channel is divided into two new channels based on the l coordinate of the current channel boundary, the minimum and maximum l coordinates of the obstacle; when there is an obstacle end point, the two channels divided by the obstacle starting point corresponding to the obstacle end point or their equivalent channels are merged into a new channel, the boundary of which is the same as the boundary of the channel before the obstacle starting point is divided; equivalent channels are channels with the same boundaries.

4. The method and system for local path planning with obstacles as anchor points according to claim 3, characterized in that: Each channel is a different node, and generating a tree node diagram based on all nodes includes: The channel where the vehicle's current position is located is taken as the starting node, and the channel where the path planning end position is located is taken as the ending node; each node in the tree node diagram except the ending node contains the length and width of the respective channel, whether the vehicle can pass through, and the position list of each child node in the next layer of nodes that can be connected; the ending node contains the length and width of the node channel; When there is an obstacle starting point, the obstacle starting point divides the current channel into two new channels, that is, generates two new child nodes in the next layer node; when there is an obstacle end point, the obstacle end point merges the two channels divided by the corresponding obstacle starting point or equivalent channels into a new channel. If the length of the new channel is less than the turning radius of the vehicle and there is an obstacle starting point at the end s coordinate of the channel, two child nodes with the same length and width are generated in the next layer node, otherwise a new child node is generated in the next layer node; The structure formed by all nodes based on the connection relationship between the nodes is the tree-shaped node graph.

5. The method and system for local path planning with obstacles as anchor points according to claim 1, characterized in that: The method of using a quadratic programming method to solve the solution space of each path to obtain the trajectory of the path includes: Construct a cost function and set the cost function as the objective function of the quadratic programming; set the constraints of the quadratic programming based on the vehicle width, the s coordinates of each node in the path, and the maximum and minimum values ​​of the l coordinates; traverse along the reference line between the minimum s coordinate and the maximum s coordinate with a fixed step size to solve the optimal l coordinate corresponding to the current s coordinate; connect all the corresponding optimal l coordinates to obtain the solution space trajectory of the path, which is the trajectory of the path.

6. The method and system for local path planning with obstacles as anchor points according to claim 1, characterized in that: The path score is obtained using the following method: Where L is the curve length of the path trajectory, L r is the length of the reference line corresponding to the path, k max is the inverse of the vehicle’s minimum turning radius, k i is the curvature of the path trajectory point i corresponding to each sampling point, K s For all k i The sum of the absolute values ​​of , w1 and w2 are weight values, and n is the number of sampling points.

7. The method and system for local path planning with obstacles as anchor points according to claim 4, characterized in that: Get all paths in the node graph using the following method: Starting from the starting node of the node graph, connect the next layer of nodes along the tree structure of the node graph; determine whether each child node in the next layer of nodes can allow the vehicle to pass through. If not, stop connecting the child node to the next layer of nodes. If so, continue connecting the child node to the next layer of nodes; stop connecting when the next layer of nodes is the end node; all paths connecting the starting node and the end node are all paths in the node graph.

8. The method and system for local path planning with obstacles as anchor points according to claim 1, characterized in that: The frenet coordinate system includes an s coordinate and an l coordinate, wherein the s coordinate is the length along the reference line from the reference line zero point, and the absolute value of the l coordinate is the distance from the reference line.

9. The method and system for local path planning with obstacles as anchor points according to claim 5, characterized in that: The cost function is obtained by the following method: Among them, l is the l coordinate of the path trajectory to be planned, l', l", l'' are the first, second and third order derivatives of the l coordinate respectively, and w l 、w dl 、w ddl 、w dddl are the weights of l, l', l", and l'' respectively, and n is the number of sampling points.

10. A local path planning system with obstacles as anchor points, characterized in that: include: LiDAR, used to obtain obstacles, reference lines, road boundary positions and the current position of the vehicle within a certain length of the road; A coordinate transformation module is used to transform obstacles, reference lines, road boundary positions and the current position of the vehicle into the Frenet coordinate system, and input the coordinates into the node graph generation module and the path planning module; The node graph generation module obtains the starting point and end point of each obstacle; Traverse the reference line with a fixed step size, divide the current channel into two channels when there is an obstacle starting point, and merge the two channels or equivalent channels divided by the corresponding obstacle starting point into one channel when there is an obstacle end point; each channel is regarded as a different node, a tree node graph is generated based on all nodes, and the node graph is input into the path planning module; The path planning module is used to obtain all paths in the node graph, use the quadratic programming method to solve the solution space of each path to obtain the trajectory of the path, and obtain the score of the path based on the curve length, reference line length, curvature, vehicle minimum turning radius and number of sampling points of the path trajectory; select the path with the highest score as the optimal path and output it.

Images