Two-stage path planning method and device based on piecewise jerk path optimization
By using a two-stage planning method with segmented acceleration path optimization, a smooth path is generated, which solves the problem of bumps and swaying in complex environments for autonomous vehicles, and improves both comfort and computational efficiency.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- CHANGSHA XINGSHEN INTELLIGENT TECH CO LTD
- Filing Date
- 2025-07-15
- Publication Date
- 2026-07-07
AI Technical Summary
Existing path planning algorithms cannot generate smooth, comfortable paths, especially in complex urban environments, resulting in severe bumps and swaying for autonomous vehicles.
A two-stage path planning method with piecewise acceleration path optimization is adopted. First, an initial path is generated. Then, a quadratic objective function is constructed through Taylor expansion and curvature estimation. A quadratic programming optimizer is used to optimize the path curvature and reduce bumps and swaying.
It generates high-quality, smooth paths, improving passenger comfort and significantly enhancing computational efficiency, while also demonstrating high flexibility in adapting to complex environments.
Smart Images
Figure CN120848498B_ABST
Abstract
Description
Technical Field
[0001] This application relates to the field of autonomous driving technology, and in particular to a two-stage path planning method and apparatus based on segmented acceleration path optimization. Background Technology
[0002] With the rapid development of cutting-edge technologies such as artificial intelligence, the Internet of Things, and big data, various industries are undergoing a profound intelligent transformation. In the field of intelligent transportation, autonomous driving technology has emerged, paving new paths for solving traditional traffic problems and improving the travel experience. Autonomous driving technology aims to enable vehicles to autonomously complete driving tasks without human intervention. One of its core functions is path planning, which, based on environmental perception, generates safe, feasible, and comfortable driving paths for the vehicle. It not only considers how to avoid obstacles and follow traffic rules but also optimizes the driving route to ensure passenger comfort. Path planning is a crucial link in the motion planning of autonomous vehicles. The path determines the vehicle's trajectory, thus it is essential for ensuring safe and comfortable driving. In urban driving environments, autonomous vehicles need to have the ability to navigate in complex environments, such as roads partially blocked by multiple vehicles or obstacles. Finding a smooth path that meets kinematic requirements while avoiding collisions in complex environments makes path planning an extremely challenging task.
[0003] In existing path planning technologies, algorithms can generally be categorized into several types: sampling-based methods, graph-based methods, and optimization-based methods. Sampling-based methods excel at exploring high-dimensional configuration spaces, such as stochastic planners like RRT. However, these planners are typically general-purpose and cannot effectively utilize domain knowledge within the structured road environment, thus the quality of the generated paths fails to meet the demands of comfortable high-speed driving. Graph-based methods, such as A* and state lattice methods, plan routes by connecting a series of pre-computed path segments or operations. This connection is achieved by checking whether the ending configuration of a path segment is sufficiently close to the starting configuration of the target path segment. These methods generally perform well in simple environments. However, these methods can be viewed as special graph search methods, where the transformation from one grid to another implicitly indicates a path segment. To solve complex autonomous driving problems in urban areas, the required path segments need to grow exponentially, requiring high graph resolution. Moreover, the shape of the path largely depends on the discretization of the lattice, which limits the flexibility of the path. Optimization-based methods typically generate a safe, drivable path within a given drivable area. Their advantage lies in their ability to directly achieve optimal modeling. Furthermore, because the path or trajectory is densely discretized as optimization variables, these methods offer maximum control over the path or trajectory in complex scenarios, making them more flexible and suitable for complex traffic environments than the previous two methods. However, the paths generated by these methods are not smooth enough, resulting in significant bumps and swaying for autonomous vehicles during operation, leading to low comfort. Summary of the Invention
[0004] Therefore, it is necessary to provide a two-stage path planning method and apparatus based on segmented acceleration path optimization that can generate smooth paths, addressing the aforementioned technical problems.
[0005] A two-stage path planning method based on piecewise acceleration path optimization, the method comprising:
[0006] Generate the initial path for the first stage within the given drivable area using the path optimization method;
[0007] Taylor expansion and curvature estimation are performed at each path point of the initial path to obtain the curvature estimation results of the second-stage path optimization.
[0008] Construct the objective function for the first-stage path optimization problem. Based on the objective function of the first-stage path optimization problem, expand the curvature estimation result of the second-stage path optimization with a quadratic form to obtain a quadratic cost matrix and a first-order cost vector considering curvature cost.
[0009] The quadratic cost matrix and the linear cost vector, which take curvature cost into account, are added as curvature cost to the objective function of the first-stage path optimization problem to obtain the new optimization objective function of the second stage. The new optimization objective function of the second stage is then optimized and solved using a quadratic programming optimizer to obtain the path optimization solution.
[0010] A two-stage path planning device based on segmented acceleration path optimization, the device comprising:
[0011] The initial path generation module is used to generate the initial path for the first stage within a given drivable area based on the path optimization method.
[0012] The curvature estimation module for the second-stage path optimization is used to perform Taylor expansion and curvature estimation at each path point of the initial path to obtain the curvature estimation results for the second-stage path optimization.
[0013] The curvature cost calculation module is used to construct the objective function of the first-stage path optimization problem. Based on the objective function of the first-stage path optimization problem, the curvature estimation result of the second-stage path optimization is expanded by a quadratic form to obtain a quadratic cost matrix and a first-order cost vector considering curvature cost.
[0014] The second-stage optimization module is used to add the quadratic cost matrix and the linear cost vector, which take curvature cost into the objective function of the first-stage path optimization problem as curvature cost, to obtain the new optimization objective function of the second stage. The quadratic programming optimizer is used to optimize and solve the new optimization objective function of the second stage to obtain the path optimization solution.
[0015] The aforementioned two-stage path planning method and apparatus based on piecewise jerk path optimization employs a staged optimization strategy. First, it uses the original piecewise-jerk method to generate an initial feasible path, laying the foundation for subsequent optimization. Due to the strong nonlinearity of the curvature term in the Frenet coordinate system, direct optimization would be limited by solving a complex nonlinear optimization problem, resulting in low computational efficiency. This method divides the optimization into two stages: first, an initial path is obtained, then curvature estimation and targeted optimization are performed, transforming the problem into a more easily solvable quadratic programming problem. While ensuring optimization effectiveness, it significantly reduces computational load, making the rapid generation of smooth paths possible. By introducing a second-order Taylor expansion to estimate the curvature at path points, and incorporating the quadratic cost function of curvature estimation into the quadratic optimization process, minimizing this cost function during optimization directly affects the curvature and rate of change of curvature of the path, making the path curve smoother and effectively reducing bumps and swaying during vehicle operation, thereby improving passenger comfort. The two-stage optimization method of this application possesses high flexibility. After the first stage of optimization is completed, the path curvature can be checked in advance to see if it meets the requirements. If it does, the result can be output directly without entering the second stage of optimization. The second stage of optimization is only performed when the curvature does not meet the requirements. This flexible mechanism ensures both optimization efficiency and that the final output path meets the smoothness standard in terms of curvature, thereby stably generating high-quality smooth paths. Attached Figure Description
[0016] Figure 1 This is a flowchart illustrating a two-stage path planning method based on segmented jerk path optimization in one embodiment;
[0017] Figure 2 This is a structural block diagram of a two-stage path planning device based on segmented jerk path optimization in one embodiment;
[0018] Figure 3 This is an internal structural diagram of a computer device in one embodiment. Detailed Implementation
[0019] To make the objectives, technical solutions, and advantages of this application clearer, the following detailed description is provided in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the scope of this application.
[0020] In one embodiment, such as Figure 1 As shown, a two-stage path planning method based on segmented jerk path optimization is provided, including the following steps:
[0021] Step 102: Generate the initial path for the first stage within the given drivable area according to the path optimization method.
[0022] The first stage of optimization begins by generating an initial solution with unoptimized curvature, based on the original piecewise-jerk path optimization method. This initial solution represents the first stage of path optimization. An initial feasible path is generated based on the vehicle's initial state, dynamic constraints, and environmental information. This path satisfies basic dynamic constraints and obstacle avoidance requirements, providing a foundation for the second stage of optimization. The generated initial path is then represented as a lateral state sequence in the Frenet coordinate system, consisting of a series of lateral distances and their first and second derivatives ([x(s), x′(s), x″(s)]), where s is the longitudinal position of the path. The original piecewise-jerk method is used to generate the initial feasible path, laying the foundation for subsequent optimization. Since the curvature term in the Frenet coordinate system is highly nonlinear, direct optimization would be limited by solving complex nonlinear optimization problems, resulting in low computational efficiency. This method divides the optimization into two stages: first obtaining the initial path, then performing curvature estimation and targeted optimization. This transforms the problem into a more easily solvable quadratic programming problem, significantly reducing computation while maintaining optimization effectiveness, making the rapid generation of smooth paths possible.
[0023] Step 104: Perform Taylor expansion and curvature estimation at each path point of the initial path to obtain the curvature estimation results of the second-stage path optimization.
[0024] Using the lateral state of the initial path and its derivative as variables, and the path points from the first path optimization result as expansion points, the curvature estimates and their squared terms at the path points are calculated. Typically, the curvature of the path in the Frenet coordinate system can be calculated using the following formula:
[0025]
[0026] Furthermore, path planning in the Frenet coordinate system typically makes the following two assumptions:
[0027] 1. When the vehicle is driving, it is almost parallel to the driving reference line, that is, it is assumed that the heading angle of the vehicle is in the same direction as the corresponding point of the reference line, so Δθ=0.
[0028] 2. The lateral acceleration x″ of the vehicle is relatively small (generally less than 10). -2 Therefore, we assume it to be 0.
[0029] Therefore, based on these two assumptions, formula (1) can be calculated as follows: Based on the preceding conditions, we can find the second-order Taylor expansion of the solution to the two-stage optimization problem. Assuming we take the solution x from the first stage as the expansion point of the second-order Taylor expansion, the second-order expansion formula can be written in the following form:
[0030]
[0031] Substituting equation (2) into equation (3), we can obtain the curvature estimation result for the second-stage path optimization:
[0032]
[0033] Step 106: Construct the objective function for the first-stage path optimization problem. Based on the objective function for the first-stage path optimization problem, perform a quadratic expansion of the curvature estimation results for the second-stage path optimization to obtain a quadratic cost matrix and a linear cost vector that consider curvature costs.
[0034] The objective function for the first-stage path optimization problem is:
[0035]
[0036] In addition to the characteristics typically found in quadratic programming problems In addition, the first term of the P matrix is:
[0037]
[0038] The second term, vector q, is: This indicates an encouragement to optimize results by moving away from left and right obstacles.
[0039] The object, whether to add this term depends on actual needs; if not added, it is a zero vector.
[0040] Formula (4) can be expanded into the following quadratic form when modeling the curvature of multiple path points:
[0041]
[0042] Therefore, substituting the results of formulas (2), (3), and (4) into the P matrix yields a new quadratic cost matrix p that considers curvature cost. * : Substituting into the q vector yields a new cost vector q. * :
[0043]
[0044] By introducing a second-order Taylor expansion to estimate the curvature at path points, and incorporating the quadratic cost function of the curvature estimation into the quadratic optimization process, minimizing this cost function during optimization directly affects the curvature and rate of change of curvature of the path, making the path curve smoother and effectively reducing bumps and swaying during vehicle operation, thereby improving passenger comfort.
[0045] Step 108: The quadratic cost matrix and the linear cost vector, which take curvature cost into account, are added as curvature cost to the objective function of the first-stage path optimization problem to obtain the new optimization objective function of the second stage. The quadratic programming optimizer is used to optimize and solve the new optimization objective function of the second stage to obtain the path optimization solution.
[0046] p * and q * Without changing the quadratic programming form of the original problem, the curvature cost can be directly added to the first and second cost matrices p and q in the first stage, resulting in a new optimization objective function for the second stage. Then, the second stage optimization is solved by a quadratic programming optimizer such as OSQP, resulting in a path optimization solution with a smaller maximum curvature κ and its rate of change κ′ compared to the optimization solution in the first stage.
[0047] The two-stage optimization method of this application is highly flexible. After the first stage of optimization is completed, the path curvature can be checked in advance to see if it meets the requirements. If it does, the result can be output directly without entering the second stage of optimization. The second stage of optimization is only performed when the curvature does not meet the requirements. This flexible mechanism ensures both optimization efficiency and that the final output path meets the smoothness standard in terms of curvature, thereby stably generating high-quality smooth paths.
[0048] The two-stage path planning method based on piecewise jerk path optimization described above adopts a phased optimization strategy. First, it uses the original piecewise-jerk method to generate an initial feasible path, laying the foundation for subsequent optimization. Due to the strong nonlinearity of the curvature term in the Frenet coordinate system, direct optimization would be limited by solving a complex nonlinear optimization problem, resulting in low computational efficiency. This method divides the optimization into two stages: first, obtaining the initial path, and then performing curvature estimation and targeted optimization. This transforms the problem into a more easily solvable quadratic programming problem, significantly reducing computational load while ensuring optimization effectiveness, making the rapid generation of smooth paths possible. By introducing a second-order Taylor expansion to estimate the curvature at path points, and incorporating the quadratic cost function of curvature estimation into the quadratic optimization process, minimizing this cost function during optimization directly affects the curvature and rate of change of curvature of the path, making the path curve smoother and effectively reducing bumps and swaying during vehicle operation, thereby improving passenger comfort. The two-stage optimization method of this application possesses high flexibility. After the first stage of optimization is completed, the path curvature can be checked in advance to see if it meets the requirements. If it does, the result can be output directly without entering the second stage of optimization. The second stage of optimization is only performed when the curvature does not meet the requirements. This flexible mechanism ensures both optimization efficiency and that the final output path meets the smoothness standard in terms of curvature, thereby stably generating high-quality smooth paths.
[0049] In one embodiment, Taylor expansion and curvature estimation are performed at each path point of the initial path to obtain the curvature estimation results of the second-stage path optimization, including:
[0050] The initial path is represented as a sequence of lateral states in the Frenet coordinate system. Using the lateral states of the initial path and their derivatives as variables, a Taylor expansion is performed at each path point of the initial path to calculate the curvature estimate at the path point, thus obtaining the curvature estimate result of the second-stage path optimization.
[0051] In one embodiment, the initial path is represented as a lateral state sequence in the Frenet coordinate system, including:
[0052] The initial path is represented as a sequence of lateral states in the Frenet coordinate system, and the curvature of the path in the Frenet coordinate system is calculated.
[0053]
[0054] Among them, κ r The curvature of the driving reference line itself is represented by x, which represents the lateral coordinate of the vehicle's position relative to the driving reference line in the Frenet coordinate system.
[0055] In one embodiment, using the lateral state of the initial path and its derivative as variables, a Taylor expansion is performed at each path point of the initial path to calculate the curvature estimate at the path point, resulting in the curvature estimate for the second-stage path optimization, including:
[0056] The optimization solution x of the first stage is used as the expansion point of the second-order Taylor expansion of the optimization solution of the second stage, and the second-order expansion is obtained.
[0057] Substituting the curvature of the path in the Frenet coordinate system into the second-order expansion yields the curvature estimation results for the second-stage path optimization.
[0058] In one embodiment, the curvature of the path in the Frenet coordinate system is substituted into the second-order expansion to obtain the curvature estimation result of the second-stage path optimization, including:
[0059] Substituting the curvature of the path in the Frenet coordinate system into the second-order expansion, we obtain the curvature estimation result for the second-stage path optimization.
[0060]
[0061] Where Δθ represents the deviation between the vehicle's actual heading angle and the driving reference line, κ represents the curvature of the path in the Frenet coordinate system, and x *″ represents the second derivative of the horizontal coordinate in the Frenet coordinate system during the second-stage path optimization, κ′ represents the first derivative of the curvature of the path in the Frenet coordinate system, and x * ′ represents the first derivative of the horizontal coordinate in the Frenet coordinate system during the second-stage path optimization, x * This represents the horizontal coordinate in the Frenet coordinate system for the second-stage path optimization.
[0062] In one embodiment, the objective function for constructing the first-stage path optimization problem is:
[0063]
[0064] Where x represents the horizontal coordinate in the Frenet coordinate system of the first-stage path optimization problem, P represents the quadratic coefficient matrix, q represents the linear coefficient vector, and T represents the transpose operation.
[0065] In one embodiment, based on the objective function of the first-stage path optimization problem, the curvature estimation result of the second-stage path optimization is expanded quadratically to obtain a quadratic cost matrix and a linear cost vector considering curvature costs, and the method further includes:
[0066] Based on the objective function of the first-stage path optimization problem, the curvature estimation results of the second-stage path optimization are expanded using a quadratic form as follows:
[0067]
[0068] Where, x * p represents the horizontal coordinate in the Frenet coordinate system for the second-stage path optimization. * Let q represent the quadratic cost matrix considering curvature costs. * This represents a cost vector.
[0069] In one embodiment, the quadratic cost matrix considering curvature costs can be written in the following form:
[0070]
[0071] Where Δθ represents the deviation between the vehicle's actual heading angle and the driving reference line, κ represents the curvature of the path in the Frenet coordinate system, κ′ represents the first derivative of the curvature of the path in the Frenet coordinate system, and x represents the lateral coordinate in the Frenet coordinate system of the first-stage path optimization problem.
[0072] In one embodiment, the cost vector is...
[0073]
[0074] Where Δθ represents the deviation between the vehicle's actual heading angle and the driving reference line, κ represents the curvature of the path in the Frenet coordinate system, κ′ represents the first derivative of the curvature of the path in the Frenet coordinate system, and x represents the optimized solution of the first-stage path optimization problem.
[0075] It should be understood that, although Figure 1 The steps in the flowchart are shown sequentially as indicated by the arrows, but these steps are not necessarily executed in the order indicated by the arrows. Unless otherwise specified herein, there is no strict order in which these steps are executed, and they can be performed in other orders. Figure 1 At least some of the steps in the process may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily completed at the same time, but can be executed at different times. The execution order of these sub-steps or stages is not necessarily sequential, but can be executed in turn or alternately with other steps or at least some of the sub-steps or stages of other steps.
[0076] In one embodiment, such as Figure 2 As shown, a two-stage path planning device based on piecewise acceleration path optimization is provided, comprising: an initial path generation module 202, a curvature estimation module 204 for the second-stage path optimization, a curvature cost calculation module 206, and a second-stage optimization module 208, wherein:
[0077] The initial path generation module is used to generate the initial path for the first stage within a given drivable area based on the path optimization method.
[0078] The curvature estimation module for the second-stage path optimization is used to perform Taylor expansion and curvature estimation at each path point of the initial path to obtain the curvature estimation results for the second-stage path optimization.
[0079] The curvature cost calculation module is used to construct the objective function of the first-stage path optimization problem. Based on the objective function of the first-stage path optimization problem, the curvature estimation result of the second-stage path optimization is expanded by a quadratic form to obtain a quadratic cost matrix and a first-order cost vector considering curvature cost.
[0080] The second-stage optimization module is used to add the quadratic cost matrix and the linear cost vector, which take curvature cost into the objective function of the first-stage path optimization problem as curvature cost, to obtain the new optimization objective function of the second stage. The quadratic programming optimizer is used to optimize and solve the new optimization objective function of the second stage to obtain the path optimization solution.
[0081] Specific limitations regarding the two-stage path planning device based on piecewise acceleration path optimization can be found in the limitations of the two-stage path planning method based on piecewise acceleration path optimization described above, and will not be repeated here. Each module in the aforementioned two-stage path planning device based on piecewise acceleration path optimization can be implemented entirely or partially through software, hardware, or a combination thereof. These modules can be embedded in or independent of the processor in a computer device, or stored in the memory of a computer device as software, so that the processor can call and execute the operations corresponding to each module.
[0082] In one embodiment, a computer device is provided, which may be a terminal, and its internal structure diagram may be as follows: Figure 3 As shown, the computer device includes a processor, memory, network interface, display screen, and input devices connected via a system bus. The processor provides computing and control capabilities. The memory includes non-volatile storage media and internal memory. The non-volatile storage media stores the operating system and computer programs. The internal memory provides an environment for the operation of the operating system and computer programs stored in the non-volatile storage media. The network interface is used to communicate with external terminals via a network connection. When executed by the processor, the computer program implements a two-stage path planning method based on piecewise acceleration path optimization. The display screen can be an LCD screen or an e-ink display screen. The input devices can be a touch layer covering the display screen, buttons, a trackball, or a touchpad mounted on the computer device casing, or an external keyboard, touchpad, or mouse.
[0083] Those skilled in the art will understand that Figure 3 The structure shown is merely a block diagram of a portion of the structure related to the present application and does not constitute a limitation on the computer device to which the present application is applied. Specific computer devices may include more or fewer components than those shown in the figure, or combine certain components, or have different component arrangements.
[0084] 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. The computer program can be stored in a non-volatile computer-readable storage medium, and when executed, it can include the processes of the embodiments of the above methods. 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 can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in various 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), etc.
[0085] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.
[0086] The embodiments described above are merely illustrative of several implementation methods of this application, and while the descriptions are specific and detailed, they should not be construed as limiting the scope of the invention. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of this application, and these all fall within the protection scope of this application. Therefore, the protection scope of this application should be determined by the appended claims.
Claims
1. A two-stage path planning method based on piecewise acceleration path optimization, characterized in that, The method includes: Generate the initial path for the first stage within the given drivable area using the path optimization method; Taylor expansion and curvature estimation are performed at each path point of the initial path to obtain the curvature estimation results of the second-stage path optimization. Construct the objective function for the first-stage path optimization problem. Based on the objective function of the first-stage path optimization problem, expand the curvature estimation result of the second-stage path optimization with a quadratic form to obtain a quadratic cost matrix and a first-order cost vector considering curvature cost. The quadratic cost matrix and the linear cost vector, which take curvature cost into account, are added as curvature cost to the objective function of the first-stage path optimization problem to obtain the new optimization objective function of the second stage. The new optimization objective function of the second stage is then optimized and solved using a quadratic programming optimizer to obtain the path optimization solution.
2. The method according to claim 1, characterized in that, Taylor expansion and curvature estimation are performed at each path point of the initial path to obtain the curvature estimation results for the second-stage path optimization, including: The initial path is represented as a lateral state sequence in the Frenet coordinate system. Using the lateral state of the initial path and its derivative as variables, a Taylor expansion is performed at each path point of the initial path to calculate the curvature estimate at the path point, thus obtaining the curvature estimate result of the second-stage path optimization.
3. The method according to claim 2, characterized in that, The initial path is represented as a lateral state sequence in the Frenet coordinate system, including: The initial path is represented as a lateral state sequence in the Frenet coordinate system, and the curvature of the path in the Frenet coordinate system is calculated. Among them, κ r The curvature of the driving reference line itself is represented by x, which represents the lateral coordinate of the vehicle's position relative to the driving reference line in the Frenet coordinate system.
4. The method according to claim 3, characterized in that, Using the lateral state of the initial path and its derivative as variables, a Taylor expansion is performed at each path point of the initial path to calculate the curvature estimate at the path point. The curvature estimate results for the second-stage path optimization include: The optimization solution x of the first stage is used as the expansion point of the second-order Taylor expansion of the optimization solution of the second stage, and the second-order expansion is obtained. Substituting the curvature of the path in the Frenet coordinate system into the second-order expansion yields the curvature estimation result for the second-stage path optimization.
5. The method according to claim 4, characterized in that, Substituting the curvature of the path in the Frenet coordinate system into the second-order expansion yields the curvature estimation results for the second-stage path optimization, including: Substituting the curvature of the path in the Frenet coordinate system into the second-order expansion, we obtain the curvature estimation result for the second-stage path optimization: Where Δθ represents the deviation between the vehicle's actual heading angle and the driving reference line in the Frenet coordinate system, κ represents the curvature of the path in the Frenet coordinate system, and x * ″ represents the second derivative of the horizontal coordinate in the Frenet coordinate system during the second-stage path optimization, κ′ represents the first derivative of the curvature of the path in the Frenet coordinate system, and x * ′ represents the first derivative of the horizontal coordinate in the Frenet coordinate system during the second-stage path optimization, x * This represents the horizontal coordinate in the Frenet coordinate system for the second-stage path optimization.
6. The method according to claim 1, characterized in that, The objective function for the first-stage path optimization problem is: Where x represents the horizontal coordinate in the Frenet coordinate system of the first-stage path optimization problem, P represents the quadratic coefficient matrix, q represents the linear coefficient vector, and T represents the transpose operation.
7. The method according to claim 6, characterized in that, Based on the objective function of the first-stage path optimization problem, the curvature estimation results of the second-stage path optimization are expanded using a quadratic form to obtain a quadratic cost matrix and a first-order cost vector considering curvature costs, and also include: Based on the objective function of the first-stage path optimization problem, the curvature estimation result of the second-stage path optimization is expanded using a quadratic form as follows: Where, x * p represents the horizontal coordinate in the Frenet coordinate system for the second-stage path optimization. * Let q represent the quadratic cost matrix considering curvature costs. * This represents a cost vector.
8. The method according to claim 7, characterized in that, The quadratic cost matrix considering curvature cost is of the following form: Where Δθ represents the deviation between the vehicle's actual heading angle and the driving reference line, κ represents the curvature of the path in the Frenet coordinate system, κ′ represents the first derivative of the curvature of the path in the Frenet coordinate system, and x represents the lateral coordinate in the Frenet coordinate system for the first stage of path optimization.
9. The method according to claim 7, characterized in that, The first-order cost vector is: Where Δθ represents the deviation between the vehicle's actual heading angle and the driving reference line, κ represents the curvature of the path in the Frenet coordinate system, κ′ represents the first derivative of the curvature of the path in the Frenet coordinate system, and x represents the optimized solution of the first-stage path optimization problem.
10. A two-stage path planning device based on segmented acceleration path optimization, characterized in that, The device includes: The initial path generation module is used to generate the initial path for the first stage within a given drivable area based on the path optimization method. The curvature estimation module for the second-stage path optimization is used to perform Taylor expansion and curvature estimation at each path point of the initial path to obtain the curvature estimation results for the second-stage path optimization. The curvature cost calculation module is used to construct the objective function of the first-stage path optimization problem. Based on the objective function of the first-stage path optimization problem, the curvature estimation result of the second-stage path optimization is expanded quadratically to obtain a quadratic cost matrix and a first-order cost vector considering curvature cost. The second-stage optimization module is used to add the quadratic cost matrix and the linear cost vector, which consider curvature costs, as curvature costs to the objective function of the first-stage path optimization problem to obtain a new optimization objective function for the second stage. The quadratic programming optimizer is then used to optimize and solve the new optimization objective function for the second stage to obtain the path optimization solution.