A path planning method, system, computer device, readable storage medium and motor vehicle

Abstract

The application discloses a path planning method and system, computer equipment, a readable storage medium and a motor vehicle, relates to the field of automatic driving technology, and the vehicle running in an automatic driving mode plans an automatic driving path through the path planning method, and the method comprises the following steps: initializing various parameters, and determining a target point through a reverse tree; sampling with the target point as a starting point, and generating a new node; connecting the generated nodes, and performing obstacle testing after the connection; if there is no collision with the obstacles, a path is generated and the generated path is optimized, thereby forming a path for automatic driving of the vehicle. The path planning method provided by the application meets the requirements of any initial position and attitude of the vehicle, speeds up the path planning speed, improves the planning quality, and is more in line with the actual driving conditions of the vehicle.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving technology, and more specifically to a path planning method, system, computer device, readable storage medium, and motor vehicle. Background Technology

[0002] Intelligent motion control of vehicles is a crucial research direction in autonomous driving technology. In recent years, autonomous driving technology has developed rapidly. However, current trajectory planning for autonomous driving is designed for structured roads—planned roads marked by traffic authorities. Traditional geometric methods divide the road along its lines, generating trajectories through multiple arcs. Existing methods for unstructured roads, however, generate multiple geometric curves based on the vehicle's current position and target location, or through node expansion. This requires clearly defining the planning area and expanding nodes in a continuous space to generate the planned trajectory. However, this approach places strict requirements on the vehicle's initial position and attitude. Furthermore, existing search-based trajectory planning algorithms for unstructured roads suffer from increased search time and significantly impact real-time planning performance due to the limited number of valid and invalid nodes during the search, making it difficult to construct path points. Summary of the Invention

[0003] To address the aforementioned problems, this invention provides a path planning method that satisfies the requirements of any initial position and attitude of the vehicle, accelerates path planning speed, improves planning quality, and better reflects the actual driving conditions of the vehicle.

[0004] To achieve the above objectives, the present invention adopts the following technical solution:

[0005] A path planning method for planning driving routes during autonomous driving includes the following steps:

[0006] Initialize all parameters and determine the target point through a reverse tree. The target point is the position after a preset time at the current vehicle speed.

[0007] Sampling is performed using the target point as the starting point to generate new nodes;

[0008] Connect the generated nodes, perform obstacle testing after connection, and if there is no collision with the obstacle, generate a path and optimize the generated path to form the path for autonomous driving of the vehicle.

[0009] Optionally, sampling with the target point as the starting point includes the following steps:

[0010] At the starting point, a new node is generated with the vehicle's direction of travel as the center line of the sampling area. At the new node, the direction of the line connecting the new node and its parent node is taken as the center line of the sampling area. Random numbers are generated for the new node according to the following formula:

[0011]

[0012] Where, x i Let y be the x-coordinate of the i-th node. i x is the ordinate of the i-th node; old Let y be the x-coordinate of the parent node of the new node. old Let r be the ordinate of the parent node of the new node, r be the radius of the sampling area, and θ be the heading angle of the vehicle. rand ~N(μ) r ,σ r ) and θ rand ~N(μ) θ ,σ θ ) is a random number, u r σ represents the relative distance between the new node and its parent node. r σ represents the radial range of the sampling area. θ μ is the angle between the center line of the sampling area and the boundary of the sampling area. θ The angle between the center line of the current sampling area and the x-axis.

[0013]

[0014] θ init The heading angle at the starting point;

[0015] Determine whether the sampling point falls into the dead zone. If it does, discard the sampling point. The dead zone is the area where obstacles in the sampling area overlap.

[0016] Optionally, when sampling from the target point, the generated nodes are adjusted using a linearly growing inverse expansion tree, including the following steps:

[0017] Starting from the initial point, the generated nodes are adjusted by expanding the tree in reverse with a fixed step size. It is determined whether a collision occurs when the previous node reaches the next node. If there is no collision, the previous node is the adjusted node. If there is a collision, the node is moved forward by Δt, and the node after the movement is the adjusted node.

[0018] Optionally, the path from the previous node to the next node is the minimum turning radius calculated based on the vehicle dynamics model, which is:

[0019]

[0020] Where, x r Let y be the x-coordinate of the midpoint of the rear axle of the vehicle. r v is the ordinate of the midpoint of the rear axle of the vehicle, L is the wheelbase of the vehicle, and v r Rear axle speed; δ fThe equivalent front wheel steering angle is δ. f Maximum equivalent front wheel steering angle δ max Constraints satisfying |δ f |≤δ max .

[0021] Optionally, obstacle testing may include the following steps:

[0022] The vehicle is represented as rectangle A, and the obstacle as rectangle B. The distance between rectangle A and rectangle B is calculated, as well as the angle between the line connecting the centers of rectangle A and rectangle B and the x-axis.

[0023] Determine whether a collision occurs when the previous node reaches the next node based on the following conditions:

[0024]

[0025] Where L is the distance between rectangle A and rectangle B, β is the angle between the line connecting the centers of rectangles A and B and the x-axis, and L A Let W be the length of rectangle A. A Let L be the width of rectangle A. B Let W be the length of rectangle B. B The width of rectangle B;

[0026] If both conditions are met, the vehicle collides with the obstacle.

[0027] Optionally, each new node has an index number, and optimizing the generated path includes the following steps:

[0028] Starting from the end point of the generated path, connect the nodes to be optimized and the starting point sequentially through RS curves according to the index number from smallest to largest.

[0029] Starting from the starting point, search for a collision-free RS path between the starting point and the node to be optimized. If a collision-free RS path exists, remove other nodes between the starting point and the node to be optimized, and replace the original path with the new path.

[0030] Starting from the current node, the next round of optimization is carried out until all the original paths are transformed into RS paths. All optimized paths are then merged to form the path for autonomous driving of the vehicle.

[0031] Optionally, the generated nodes can be connected using RS curves.

[0032] The technical solution provided by this invention, targeting unstructured roads, constructs a vehicle dynamics model and, under the constraints of this model, uses Gaussian distribution sampling and biased sampling for fusion sampling. It introduces a reverse expansion tree to adjust the nodes formed by the fast search tree, thus accelerating path planning. Simultaneously, through obstacle collision testing and path smoothing optimization based on RS curves, it improves the quality of path planning, overcoming the problems of excessive redundant searches, inefficient algorithms, and lack of vehicle motion constraints inherent in existing technologies that use geometric methods or search space node expansion methods for unstructured road planning.

[0033] Meanwhile, the present invention also provides a computer device, including a memory and a processor, wherein the memory stores a computer program, and the processor executes the computer program to implement the path planning method described in any of the preceding claims.

[0034] Furthermore, the present invention also provides a computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the path planning method described in any of the preceding claims.

[0035] In addition, the present invention also provides a motor vehicle having an automatic driving function. When the motor vehicle operates the automatic driving function, it plans an automatic driving path for the vehicle using the path planning method described in any one of the preceding claims.

[0036] Or the motor vehicle has the aforementioned computer equipment;

[0037] Alternatively, the motor vehicle may have the aforementioned computer-readable storage medium, and the computer program, when executed by a processor, implements the path planning method described in any of the preceding claims.

[0038] The motor vehicle provided by this invention has similar beneficial effects to the reasoning process of the beneficial effects of the aforementioned path planning method, and will not be repeated here.

[0039] These features and advantages of the present invention will be disclosed in detail in the following specific embodiments and accompanying drawings. The preferred embodiments or means of the present invention will be shown in detail in conjunction with the accompanying drawings, but are not intended to limit the technical solutions of the present invention. In addition, each of these features, elements and components appearing in the following text and drawings is a plurality of, and different symbols or numbers are used for convenience of representation, but all represent parts with the same or similar construction or function. Attached Figure Description

[0040] The present invention will be further described below with reference to the accompanying drawings:

[0041] Figure 1 This is a flowchart illustrating an embodiment of the present invention;

[0042] Figure 2 This is a schematic diagram of sampling in an embodiment of the present invention;

[0043] Figure 3 This is a schematic diagram of the vehicle dynamics model in an embodiment of the present invention;

[0044] Figure 4 This is a schematic diagram of obstacle testing in an embodiment of the present invention;

[0045] Figure 5 This is a schematic diagram of path optimization in an embodiment of the present invention. Detailed Implementation

[0046] The technical solutions of the embodiments of the present invention will be explained and described below with reference to the accompanying drawings. However, the following embodiments are only preferred embodiments of the present invention and not all of them. Other embodiments obtained by those skilled in the art based on the embodiments in the implementation methods without creative effort are all within the protection scope of the present invention.

[0047] The terms "an embodiment," "example," or "trademark" used in this specification refer to a particular feature, structure, or characteristic described in connection with the embodiment itself that may be included in at least one embodiment disclosed in this patent. The phrase "in an embodiment" appearing in various places throughout the specification does not necessarily refer to the same embodiment.

[0048] Example:

[0049] This embodiment provides a path planning method for vehicles with autonomous driving capabilities. The vehicle operating in autonomous driving mode plans its autonomous driving path using the method provided in this embodiment. The path planning method provided in this embodiment is particularly suitable for autonomous driving path planning in non-structured road scenarios. Non-structured roads refer to roads that have not been planned by the transportation department, or unpaved roads, or roads that have not been marked by the transportation department.

[0050] like Figure 1 As shown, the path planning method provided in this implementation includes the following steps:

[0051] Initialize the parameters; specifically, initialize the parameters of each model used in the path planning method provided in this embodiment.

[0052] The target point is determined by reverse tree mapping. The target point is the position after a preset time at the current vehicle speed. Specifically, in this embodiment, the target point is the position 3 seconds after the current vehicle speed. In other embodiments, the preset time can be set by those skilled in the art according to the vehicle model requirements, and is not limited here.

[0053] Starting from the target point, new nodes are generated by sampling using a fast search tree according to the following steps, with each new node having an index number:

[0054] Existing algorithms, with sampling points uniformly distributed across the search space, suffer from excessive redundancy, leading to low efficiency. The method presented in this embodiment utilizes Gaussian distribution sampling to reduce randomness and blindness. Simultaneously, it constrains the sampling direction and range through vehicle dynamics, making it more aligned with vehicle constraints and ensuring a more rational and vehicle-motor-compliant planned autonomous driving path. At the starting point, a new node is generated with the vehicle's direction of travel as the centerline of the sampling area. At each new node, the direction of the line connecting the new node to its parent node is used as the centerline of the sampling area. Random numbers are generated for each new node according to the following formula:

[0055]

[0056] Where, x i Let y be the x-coordinate of the i-th node. i x is the ordinate of the i-th node; old Let y be the x-coordinate of the parent node of the new node. old Let r be the ordinate of the parent node of the new node, r be the radius of the sampling area, and θ be the heading angle of the vehicle. rand ~N(μ) r ,σ r ) and θ rand ~N(μ) θ ,σ θ ) is a random number, u r σ represents the relative distance between the new node and its parent node. r σ represents the radial range of the sampling area. θ μ is the angle between the center line of the sampling area and the boundary of the sampling area. θ The angle between the center line of the current sampling area and the x-axis.

[0057]

[0058] In the initial stage, the fast search tree has only x vertices. init θinit is the heading angle of the starting point, and the number of vertices in the fast search tree during the sampling phase is greater than 1.

[0059] Determine if the sampling point falls into a dead zone. A dead zone is the area where the sampling area overlaps with an obstacle. Because Gaussian distribution sampling has a certain bias, there is a possibility that sampling points may fall into dead zones during the sampling process. For example... Figure 2 As shown, to ensure the stability of the sampling process, when a sampling point falls into the dead zone, the sampling point is discarded.

[0060] Due to the randomness of the fast search tree, the planned path often requires a lot of pose adjustments, which can easily lead to dangerous situations such as vehicle collisions. Therefore, pose adjustments for vehicles should be minimized. Thus, when sampling from the target point, the generated nodes are adjusted using a linearly growing inverse expansion tree, including the following steps:

[0061] The starting point of the reverse expansion tree is the vehicle's final pose. Therefore, starting from the starting point, the nodes generated are adjusted along the reverse expansion tree with a fixed step size. It is determined whether a collision occurs between the previous node and the next node. If there is no collision, the previous node becomes the adjusted node. If there is a collision, the node is moved forward by Δt, and the moved node becomes the adjusted node. The path from the previous node to the next node is the minimum turning radius calculated based on the vehicle dynamics model, such as... Figure 3 As shown, the vehicle dynamics model is:

[0062]

[0063] Where, x r Let y be the x-coordinate of the midpoint of the rear axle of the vehicle. r Let v be the ordinate of the midpoint of the rear axle of the vehicle, L be the wheelbase of the vehicle, with forward being positive. r Rear axle speed; δ f The equivalent front wheel steering angle is positively correlated with a counter-clockwise direction. Equivalent front wheel steering angle δ f Maximum equivalent front wheel steering angle δ max Constraints satisfying |δ f |≤δ max The calculation of the minimum turning radius based on vehicle dynamics models is a well-known existing technique, and will not be elaborated upon here.

[0064] The generated nodes are then connected using RS curves. RS curves are based on Dubins curves, which assume that the vehicle always turns with the minimum turning radius and uses straight lines as much as possible; the path from the start point to the end point consists only of arcs and straight lines. In this embodiment, RS curves are used to directly connect nodes during path planning, so the final planned path will consist only of arcs and straight lines, avoiding unnecessary pose adjustments and improving path safety.

[0065] After connecting, perform obstacle testing as follows:

[0066] like Figure 4 As shown, the vehicle is represented by rectangle A, and the obstacle by rectangle B. x A y Let B be a unit vector in a local coordinate system with Ao as the origin. x B yGiven a unit vector in a local coordinate system with Bo as the origin, find the distance between rectangle A and rectangle B, and the angle between the line connecting the centers of rectangle A and rectangle B and the x-axis.

[0067] Determine whether rectangles A and B lie on the four separating axes mentioned above. x A y B x B y Does the projection on satisfy the following conditions:

[0068]

[0069] Then, it is determined whether a collision occurs when the previous node reaches the next node. Here, L is the distance between rectangle A and rectangle B, β is the angle between the line connecting the centers of rectangles A and B and the x-axis, and L... A Let W be the length of rectangle A. A Let L be the width of rectangle A. B Let W be the length of rectangle B. B The width of rectangle B;

[0070] If all four conditions are met, the vehicle collides with the obstacle. If there is no collision, a path is generated and optimized to form the path for autonomous driving.

[0071] To address the issues of discontinuous curvature and uneven path connections in the fast search tree, this embodiment replaces the original fast search tree path with an RS curve. This optimizes the path to meet the requirements of the vehicle dynamics model; when there are no obstacles between two points, the RS path is the shortest path. Optimizing the generated path includes the following steps:

[0072] like Figure 5 As shown, starting from the end point of the generated path, the nodes to be optimized and the starting point are connected sequentially by RS curves according to the index number from small to large.

[0073] Starting from the starting point, search for a collision-free RS path between the starting point and the node to be optimized. If a collision-free RS path exists, remove other nodes between the starting point and the node to be optimized, and replace the original path with the new path.

[0074] Starting from the current node, the next round of optimization is carried out until all the original paths are transformed into RS paths. All optimized paths are then merged to form the path for autonomous driving of the vehicle.

[0075] The technical solution provided in this embodiment constructs a vehicle dynamics model and, under the constraints of this model, uses Gaussian distribution sampling and biased sampling for fusion sampling. It introduces a reverse expansion tree to adjust the nodes formed by the fast search tree, thus accelerating path planning. Simultaneously, through obstacle collision testing and path smoothing optimization based on RS curves, it improves the quality of path planning and overcomes the problems of excessive redundant searching, inefficient algorithms, and lack of vehicle motion constraints inherent in existing technologies that use geometric methods or search spaces for node expansion.

[0076] It should be noted that the path planning method provided in this embodiment is not only applicable to the planning of autonomous driving paths while in motion, but can also be applied to autonomous parking scenarios. When applied to autonomous parking scenarios, the target point is the vehicle's final position when it parks in the target parking space, and the remaining steps are no different from those for autonomous driving path planning while in motion.

[0077] Meanwhile, this embodiment also provides a computer device, including a memory and a processor. The memory stores a computer program, and when the computer program is executed by the processor, the processor performs the steps of the above-described path planning method.

[0078] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. Accordingly, the computer program can be stored in a non-volatile computer-readable storage medium, and when executed, the computer program can implement the methods of any of the above embodiments. Any references to memory, storage, databases, or other media used in the embodiments provided in this application can include non-volatile and / or volatile memory. Non-volatile memory may include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory may include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in a variety of forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), dual data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), RAMbus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM).

[0079] Furthermore, this embodiment also provides a motor vehicle with an autonomous driving function. When the motor vehicle provided in this embodiment operates the autonomous driving function, it plans the autonomous driving path of the vehicle through the aforementioned path planning method.

[0080] Or the motor vehicle has the aforementioned computer equipment;

[0081] The vehicle or motor vehicle has the aforementioned computer-readable storage medium, and the computer program, when executed by a processor, implements the path planning method described in any of the preceding claims.

[0082] The above are merely specific embodiments of the present invention, but the scope of protection of the present invention is not limited thereto. Those skilled in the art should understand that the present invention includes, but is not limited to, the contents described in the accompanying drawings and the specific embodiments above. Any modifications that do not depart from the functional and structural principles of the present invention will be included within the scope of the claims.

Claims

1. A path planning method for planning a driving path when a vehicle is automatically driven, characterized by, The path planning method comprises the following steps: initializing parameters, determining a target point by a reverse tree, the target point being a position after a preset time at a current vehicle speed; sampling with the target point as a starting point to generate a new node; connecting the generated nodes, testing obstacles after connection, generating a path if there is no collision with obstacles, and optimizing the generated path to form an automatic driving path of the vehicle; sampling with the target point as a starting point comprises the following steps: at the starting point, generating a new node in the direction of the vehicle driving direction as the center line direction of the sampling area, at the new node, the direction of the connection between the new node and its parent node is taken as the center line direction of the sampling area, and a random number is generated according to the following formula to generate a new node: where x i is the horizontal coordinate of the i-th node, y i is the vertical coordinate of the i-th node; x old is the horizontal coordinate of the parent node of the new node, y old is the vertical coordinate of the parent node of the new node, r is the radius of the sampling area, θ is the heading angle of the vehicle, r rand ~ N(μ r , σ r ) and θ rand ~ N(μ θ , μ θ ) are random numbers, u r is the relative distance between the new node and its parent node, σ r is the radial range of the sampling area, σ θ is the angle between the center line of the sampling area and the boundary of the sampling area, μ θ is the angle between the current center line of the sampling area and the x-axis, θ init heading angle from the origin point; determining whether the sampling point falls into a dead zone, if it falls into the dead zone, the sampling point is discarded, and the dead zone is a region where the sampling area and the obstacle overlap.

2. The path planning method of claim 1, wherein, When sampling with the target point as a starting point, the generated nodes are adjusted by a reverse expansion tree of straight line growth, comprising the following steps: starting from the starting point, adjusting the generated nodes along the reverse expansion tree at a fixed step, determining whether a collision occurs when the previous node reaches the next node, if there is no collision, the previous node is the adjusted node, if there is a collision, moving Δt, and the moved node is taken as the adjusted node.

3. The path planning method of claim 2, wherein, The path from the previous node to the next node is the minimum turning radius calculated based on a vehicle dynamics model, and the vehicle dynamics model is: Where, x r Let y be the x-coordinate of the midpoint of the rear axle of the vehicle. r v is the ordinate of the midpoint of the rear axle of the vehicle, L is the wheelbase of the vehicle, and v r Rear axle speed; δ f The equivalent front wheel steering angle is δ. f Maximum equivalent front wheel steering angle δ max Constraints satisfying |δ f |≤δ max .

4. The path planning method of claim 1, wherein, the obstacle test comprises the following steps: taking the vehicle as a rectangle A and the obstacle as a rectangle B, obtaining the distance between the rectangle A and the rectangle B, and the angle between the center line of the rectangle A and the rectangle B and the x-axis; determining whether a collision occurs when the previous node reaches the next node according to the following conditions: wherein L is the distance between rectangle A and rectangle B, β is the angle between the line connecting the centers of rectangle A and rectangle B and the x-axis, L A is the length of rectangle A, W A is the width of rectangle A, L B is the length of rectangle B, W B is the width of rectangle B; if the conditions are met at the same time, the vehicle collides with the obstacle.

5. The path planning method of claim 1, wherein, Each new node has an index number, and the optimization of the generated path comprises the following steps: taking the end point of the generated path as a starting point, connecting the nodes to be optimized and the starting point in order according to the index number from small to large through the RS curve; starting from the starting point, finding a collision-free RS path between the starting point and the node to be optimized, if there is a collision-free RS path, removing other nodes between the starting point and the node to be optimized, and replacing the original path with a new path; taking the current node as a new starting point, performing the next round of optimization until all original paths are converted into RS paths, and combining all optimized paths to form an automatic driving path of the vehicle.

6. The path planning method according to one of claims 1 to 5, characterized in that, The generated nodes are connected by the RS curve. 7.A computer device, comprising a memory and a processor, wherein the memory stores a computer program, and the computer device is configured to perform the method according to any one of claims 1-6 when the computer program is executed by the processor. The processor implements the path planning method of any one of claims 1-6 when executing the computer program.

8. A computer readable storage medium having stored thereon a computer program, characterized in that, The computer program implements the path planning method of any one of claims 1-6 when executed by the processor.

9. Motor vehicle, characterized in that The motor vehicle has an automatic driving function, and when the motor vehicle operates the automatic driving function, the automatic driving path of the vehicle is planned by the path planning method of any one of claims 1-6; or the motor vehicle has the computer device of claim 7. or the motor vehicle has a computer-readable storage medium according to claim 8, which computer program, when executed by a processor, implements the path planning method according to any one of claims 1 to 6.

Images