Trajectory planning method and device for autonomous vehicle under complex road working condition

Abstract

This invention relates to the field of autonomous driving technology, specifically disclosing a method and apparatus for trajectory planning of autonomous vehicles under complex road conditions. The method involves acquiring complex road condition data, including at least longitudinal slope, lateral slope, and road boundary information; constructing a vehicle dynamics model in a three-dimensional Cartesian coordinate system based on the complex road condition data; determining an objective function and constraints based on the vehicle dynamics model, where the objective function is a time-based spatial domain state variable, and the constraints include at least three-dimensional dynamic constraints, vehicle kinematic constraints, and road constraints; and optimizing the vehicle dynamics model to obtain the fastest trajectory planning result for the autonomous vehicle. The trajectory planning method for autonomous vehicles under complex road conditions provided by this invention can perform safe and efficient fastest trajectory planning under complex road conditions including sharp curves and three-dimensional slopes.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving technology, and in particular to an autonomous vehicle trajectory planning method, an autonomous vehicle trajectory planning device, and a storage medium for complex road conditions. Background Technology

[0002] In recent years, with the continuous maturation of autonomous driving technology, its application scenarios are expanding from structured roads to transportation tasks in complex and enclosed environments such as mines, mountains, and ports. In these scenarios, autonomous vehicles need to operate under complex three-dimensional road conditions, including continuous sharp curves (large curvature), significant gradient changes, and road surface inclination. To improve the overall efficiency of transportation operations, vehicle trajectory planning algorithms not only need to strictly ensure driving stability and safety but also pursue time optimization, which places extremely high demands on the accuracy, real-time performance, and dynamic modeling of the planning system.

[0003] Existing trajectory planning methods, especially those based on the Frenet coordinate system, have inherent limitations in this scenario. The Frenet coordinate system relies on a pre-defined reference path. In high-curvature curves, when vehicles deviate significantly from the reference line in pursuit of the optimal route, problems such as large coordinate transformation errors, non-unique projection points, or even abrupt changes can arise, leading to planning failure. Furthermore, most mainstream methods are based on two-dimensional plane assumptions, making it difficult to naturally and uniformly handle three-dimensional road geometry information (such as longitudinal and transverse slopes). They cannot accurately reflect the direct impact of this geometry on vehicle dynamics (such as load transfer, sideslip risk, and power demand), making it difficult for the planned trajectory to simultaneously achieve optimality and safety in a real three-dimensional environment.

[0004] Therefore, how to perform safe and efficient fastest trajectory planning under complex road conditions, including sharp curves and three-dimensional ramps, has become a technical problem that urgently needs to be solved by those skilled in the art. Summary of the Invention

[0005] This invention provides a method, device, and storage medium for autonomous vehicle trajectory planning under complex road conditions, thereby solving the problem in related technologies that cannot perform fastest trajectory planning under complex road conditions.

[0006] As a first aspect of the present invention, a method for trajectory planning of autonomous vehicles under complex road conditions is provided, comprising:

[0007] Acquire complex road condition data, which includes at least the longitudinal slope, transverse slope, and road boundary information.

[0008] A vehicle dynamics model in a three-dimensional Cartesian coordinate system is constructed based on the complex road condition data.

[0009] The objective function and constraints are determined based on the vehicle dynamics model, wherein the objective function is a time-based spatial domain state variable, and the constraints include at least three-dimensional dynamic constraints, vehicle kinematic constraints, and road constraints.

[0010] The vehicle dynamics model is optimized and solved to obtain the fastest trajectory planning result for the autonomous vehicle.

[0011] Furthermore, based on the complex road condition data, a vehicle dynamics model is constructed in a three-dimensional Cartesian coordinate system, including:

[0012] by As a state variable, As a control variable, a vehicle mass model is constructed in a three-dimensional Cartesian coordinate system. The expression of the vehicle mass model is as follows:

[0013] ,

[0014] in, and These represent the horizontal and vertical coordinates of the rear axle center of the vehicle, respectively. Indicates the vehicle's heading angle. Indicates the vehicle's current speed. Indicates the current time of the vehicle; Indicates vehicle acceleration; Indicates the total path length. Indicates arc length; For the vehicle at its current location longitudinal slope angle, Indicates path curvature;

[0015] The vehicle mass model in above The interval is uniformly discretized to obtain the state vector of the discrete points. Control vector ,in , This represents the number of discrete points.

[0016] Further, the objective function is determined based on the vehicle dynamics model, including:

[0017] Taking the vehicle's current time t as the state variable, the expression for the objective function is obtained as follows:

[0018] ,

[0019] in, Represents the control weight matrix; and All represent weighting coefficients. This represents the control vector.

[0020] Furthermore, the three-dimensional dynamic constraints include: lateral dynamic constraints, longitudinal dynamic constraints, and tire friction ellipse constraints; the vehicle kinematic constraints include: vehicle model constraints and upper and lower bound constraints of variables; and the road constraints include collision constraints.

[0021] Furthermore, the lateral dynamic constraints include lateral acceleration constraints and rollover constraints, wherein the expression for the lateral acceleration constraint is:

[0022] ,

[0023] The expression for the rollover constraint is:

[0024] ,

[0025] in, This indicates the preset lateral acceleration threshold. Indicates the width of the vehicle. Indicates the height of the vehicle's center of gravity; Indicates position The lateral slope angle of the road surface;

[0026] The longitudinal dynamic constraints include the constraint that the longitudinal traction force is subject to its maximum engine power, expressed as:

[0027] ,

[0028] in, Indicates the maximum engine power. Indicates longitudinal traction force. Indicates the vehicle's current speed;

[0029] The expression for the tire friction ellipse constraint is:

[0030] ,

[0031] in, The expression is:

[0032] ,

[0033] in, This indicates the preset longitudinal acceleration threshold. This indicates the preset maximum longitudinal acceleration. This indicates the preset minimum longitudinal acceleration. Indicates longitudinal acceleration. This indicates lateral acceleration.

[0034] Furthermore, the expression for the vehicle model constraint is:

[0035] ;

[0036] The variable upper and lower bound constraints include velocity constraints, acceleration constraints, and curvature upper and lower bound constraints. The expression for the velocity constraint is:

[0037] ,

[0038] The expressions for the acceleration constraint and the curvature upper and lower bound constraints are as follows:

[0039] .

[0040] Furthermore, the expression for the collision constraint is:

[0041] ,

[0042] ,

[0043] ,

[0044] ,

[0045] ,

[0046] ,

[0047] Where L represents the wheelbase; line segment AB represents the center position of the rear axle. The nearest boundary polyline segment on the left boundary, segment DE represents the center point of the rear axle. The nearest boundary polyline segment on the right boundary, segment BC represents the nearest boundary polyline segment on the left boundary of the front axle center position point, and segment EF represents the nearest boundary polyline segment on the right boundary of the front axle center position point.

[0048] Furthermore, the vehicle dynamics model is optimized and solved to obtain the fastest trajectory planning result for the autonomous vehicle, including:

[0049] The vehicle dynamics model is solved using a lightweight method to obtain an initial trajectory that satisfies the basic constraints;

[0050] The initial trajectory is optimized using nonlinear programming to obtain the fastest trajectory planning result for the autonomous vehicle.

[0051] As another aspect of the present invention, an autonomous vehicle trajectory planning device for complex road conditions is provided, for implementing the autonomous vehicle trajectory planning method for complex road conditions described above, wherein the device includes:

[0052] The acquisition module is used to acquire complex road condition data information, which includes at least the longitudinal slope of the road, the transverse slope of the road, and the road boundary information.

[0053] The vehicle dynamics model construction module is used to construct a vehicle dynamics model in a three-dimensional Cartesian coordinate system based on the complex road condition data information.

[0054] The objective function and constraint determination module is used to determine the objective function and constraint conditions based on the vehicle dynamics model, wherein the objective function is a time-based spatial domain state quantity, and the constraint conditions include at least three-dimensional dynamic constraints, vehicle kinematic constraints, and road constraints.

[0055] The optimization solution module is used to optimize and solve the vehicle dynamics model to obtain the fastest trajectory planning result of the autonomous vehicle.

[0056] As another aspect of the present invention, a storage medium is provided for storing computer instructions that can be loaded and executed by a processor to implement the autonomous vehicle trajectory planning method under complex road conditions described above.

[0057] The present invention provides a trajectory planning method for autonomous vehicles under complex road conditions. By acquiring complex road condition data, and establishing a vehicle dynamics model including time state variables in a three-dimensional Cartesian coordinate system based on the complex road condition data, the method aims to minimize the travel time and considers three-dimensional dynamic constraints, vehicle kinematic constraints, and road constraints to optimize and solve the vehicle dynamics model, thereby obtaining the fastest trajectory planning result for the autonomous vehicle. This trajectory planning method for autonomous vehicles under complex road conditions, by directly modeling in the Cartesian coordinate system, completely avoids the problems of non-unique projection points, abrupt changes, and numerical singularities caused by significant deviations of the vehicle trajectory from the reference line. This makes it possible to plan the classic "outer-inner-outer" optimal racing line in continuous sharp curves with high curvature, resulting in more accurate and stable planning results. By explicitly introducing longitudinal and lateral slope angles, this invention fully considers the complex influence of real 3D road geometry on vehicle dynamics (such as the impact of slope on sideslip risk, rollover threshold, and power demand). The organic combination of "spatial domain modeling" and "temporal stateification" allows for the most natural handling of minimum time objectives and highly dispersed spatial constraints, greatly simplifying the problem's complexity. Therefore, this invention's trajectory planning method for autonomous vehicles under complex road conditions can perform safe and efficient fastest trajectory planning in complex road conditions including sharp curves and 3D slopes. Attached Figure Description

[0058] The accompanying drawings are provided to further illustrate the invention and form part of the specification. They are used together with the following detailed description to explain the invention, but do not constitute a limitation thereof.

[0059] Figure 1 This is a schematic diagram of the projection intersection problem of the Frenet coordinate system in the existing technology.

[0060] Figure 2 The flowchart of the autonomous vehicle trajectory planning method under complex road conditions provided by the present invention.

[0061] Figure 3 This is a schematic diagram of a vehicle mass model in a three-dimensional Cartesian coordinate system provided by the present invention.

[0062] Figure 4 This is a schematic diagram of the friction circle constraint provided by the present invention.

[0063] Figure 5 This is a schematic diagram of vehicle collision constraints provided by the present invention.

[0064] Figure 6 This invention provides a schematic diagram of a complex road with large curvature and three-dimensional ramps.

[0065] Figure 7This is a schematic diagram of the trajectory planning results provided by the present invention.

[0066] Figure 8 A schematic diagram of the speed curve planning results provided by this invention.

[0067] Figure 9 The structural block diagram of the autonomous vehicle trajectory planning device under complex road conditions provided by the present invention.

[0068] Figure 10 This is a structural block diagram of the electronic device provided by the present invention. Detailed Implementation

[0069] It should be noted that, unless otherwise specified, the embodiments and features described in the present invention can be combined with each other. The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

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

[0071] It should be noted that the terms "first," "second," etc., in the specification, claims, and accompanying drawings of this invention are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate for the embodiments of the invention described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover a non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0072] Fastest trajectory planning for autonomous vehicles, especially in complex road conditions including sharp curves and three-dimensional slopes, places extremely high demands on the accuracy, safety, and extreme performance of trajectory planning algorithms. Existing mainstream technologies face the following key technical bottlenecks in this scenario:

[0073] (1) Coordinate system limitations: The widely used Frenet coordinate system is based on the core assumption that the vehicle trajectory closely follows a smooth reference centerline. However, as... Figure 1As shown, in sharp bends with high curvature, autonomous vehicles will deviate significantly from the centerline in pursuit of the optimal route (such as "outer-inner-outer" route), resulting in problems such as non-unique projection points, abrupt changes, and numerical singularities in the Frenet coordinate system. This leads to increased coordinate transformation errors and distorted or even invalid planning results.

[0074] (2) Dimensional loss problem: Most existing methods are based on the assumption of two-dimensional plane, which makes it difficult to handle three-dimensional road geometry information, such as longitudinal slope and lateral slope (road inclination), naturally and uniformly. This makes the planning algorithm unable to accurately reflect the constraints of slope on the longitudinal and lateral dynamics of vehicles (such as the influence of gravity components on acceleration, braking and sideslip risk), causing the generated trajectory to deviate from the optimal or even be unsafe in the real three-dimensional environment.

[0075] (3) Modeling domain mismatch: When modeling trajectory planning problems in the time domain, it is difficult to handle constraints that are strongly dependent on spatial location (such as road boundaries, curvature, and slope of each point), resulting in high problem dimensionality, complex modeling, and difficulty in directly integrating the natural spatial domain goal of "minimum time".

[0076] Based on this, this embodiment provides a method for trajectory planning of autonomous vehicles under complex road conditions. Figure 2 This is a flowchart of an autonomous vehicle trajectory planning method under complex road conditions according to an embodiment of the present invention, such as... Figure 2 As shown, it includes:

[0077] S100. Obtain complex road condition data information, wherein the complex road condition data information includes at least the longitudinal slope of the road, the transverse slope of the road, and the road boundary information.

[0078] In this embodiment of the invention, a vehicle equipped with inertial navigation can be used to pre-collect complex road condition data information on complex roads, thereby enabling the subsequent construction of a vehicle dynamics model based on the publicly available data information on the complex roads.

[0079] S200. Construct a vehicle dynamics model in a three-dimensional Cartesian coordinate system based on the complex road condition data information.

[0080] In this embodiment of the invention, the inherently limited Frenet coordinate system is abandoned, and the vehicle dynamics model is directly established in a three-dimensional Cartesian coordinate system. It should be understood that using a "Cartesian coordinate system + spatial domain (arc length domain)" instead of the traditional "Frenet coordinate system + time domain" can fundamentally solve the projection singularity problem under large curvature.

[0081] In addition, by using longitudinal and lateral slopes as direct inputs to the model, trajectory planning was upgraded from a two-dimensional plane to a three-dimensional surface.

[0082] S300. Determine the objective function and constraints based on the vehicle dynamics model, wherein the objective function is a time-based spatial domain state variable, and the constraints include at least three-dimensional dynamic constraints, vehicle kinematic constraints, and road constraints.

[0083] In this embodiment of the invention, the trajectory planning problem is established in the spatial domain (arc length domain), and the time... As a state quantity that changes with spatial location, this fundamental shift allows the total travel time to be directly expressed as the time difference between the terminal and the initial state. This allows for the natural integration and direct minimization of the core objective of "minimum time" within the objective function. The constraints constitute a complete system including three-dimensional dynamics and road constraints, with all constraints explicitly dependent on three-dimensional road information, enabling the natural handling of spatially dependent constraints.

[0084] It should be understood that by treating time t as a state variable in the spatial domain, the minimum time objective can be expressed as a simple state difference, which greatly simplifies the mathematical formulation of the problem.

[0085] S400. Optimize and solve the vehicle dynamics model to obtain the fastest trajectory planning result for the autonomous vehicle.

[0086] In this embodiment of the invention, the vehicle dynamics model is specifically optimized and solved to obtain the fastest trajectory planning result of the autonomous vehicle. It should be noted that the fastest trajectory can be understood as the optimal speed trajectory.

[0087] Therefore, the trajectory planning method for autonomous vehicles under complex road conditions provided by this invention acquires complex road condition data and establishes a vehicle dynamics model including time state variables in a three-dimensional Cartesian coordinate system based on the complex road condition data. With the goal of minimizing travel time, it considers three-dimensional dynamic constraints, vehicle kinematic constraints, and road constraints, thereby optimizing and solving the vehicle dynamics model to obtain the fastest trajectory planning result for autonomous vehicles. This trajectory planning method for autonomous vehicles under complex road conditions, by directly modeling in the Cartesian coordinate system, completely avoids the problems of non-unique projection points, abrupt changes, and numerical singularities caused by significant deviations of the vehicle trajectory from the reference line. This makes it possible to plan the classic "outer-inner-outer" optimal racing line in continuous sharp curves with high curvature, resulting in more accurate and stable planning results. By explicitly introducing longitudinal and lateral slope angles, this invention fully considers the complex influence of real 3D road geometry on vehicle dynamics (such as the impact of slope on sideslip risk, rollover threshold, and power demand). The organic combination of "spatial domain modeling" and "temporal stateification" allows for the most natural handling of minimum time objectives and highly dispersed spatial constraints, greatly simplifying the problem's complexity. Therefore, this invention's trajectory planning method for autonomous vehicles under complex road conditions can perform safe and efficient fastest trajectory planning in complex road conditions including sharp curves and 3D slopes.

[0088] In this embodiment of the invention, a vehicle dynamics model is constructed in a three-dimensional Cartesian coordinate system based on the complex road condition data, including:

[0089] (1) with As a state variable, As a control variable, a vehicle mass model is constructed in a three-dimensional Cartesian coordinate system. The expression of the vehicle mass model is as follows:

[0090] ,

[0091] in, and These represent the horizontal and vertical coordinates of the rear axle center of the vehicle, respectively. Indicates the vehicle's heading angle. Indicates the vehicle's current speed. Indicates the current time of the vehicle; Indicates vehicle acceleration; Indicates the total path length. Indicates arc length; For the vehicle at its current location longitudinal slope angle, Indicates path curvature;

[0092] (2) The vehicle mass model is placed in above The interval is uniformly discretized to obtain the state vector of the discrete points. Control vector ,in , This represents the number of discrete points.

[0093] It should be understood that the embodiments of the present invention abandon the inherently limited Frenet coordinate system and directly establish the vehicle kinematic model in a three-dimensional Cartesian coordinate system. The road longitudinal slope angle is used as an example. and lateral slope angle The model is embedded as a known function that varies with arc length s to accurately describe the influence of three-dimensional road geometry on vehicle motion.

[0094] like Figure 3 As shown, with As a state variable, As a control variable, the vehicle mass model described above is constructed in a three-dimensional Cartesian coordinate system.

[0095] The vehicle model in above Uniformly discretized, the state vector of all discrete points Control vector ,in , The number of discrete points.

[0096] In this embodiment of the invention, determining the objective function based on the vehicle dynamics model includes:

[0097] Taking the vehicle's current time t as the state variable, the expression for the objective function is obtained as follows:

[0098] ,

[0099] in, Represents the control weight matrix; and All represent weighting coefficients. This represents the control vector.

[0100] It should be understood that, thanks to modeling in the spatial domain and using time t as a state variable, the minimum time objective function can be simplified to:

[0101] ,

[0102] in, ; , These are the weighting coefficients.

[0103] This invention establishes the trajectory planning problem in the spatial domain (arc length domain) and incorporates time... As a state quantity that changes with spatial location, this fundamental shift allows the total travel time to be directly expressed as the time difference between the terminal and the initial state. This approach achieves a natural integration and direct minimization of the core objective of "minimum time" within the objective function. It avoids the complex transformations and integration operations required to handle spatially dependent constraints within the traditional time-domain framework, greatly simplifying the mathematical formulation of the optimal control problem and laying a solid foundation for efficiently and stably solving extreme driving trajectories in complex three-dimensional scenarios.

[0104] In this embodiment of the invention, the three-dimensional dynamic constraints include: lateral dynamic constraints, longitudinal dynamic constraints, and tire friction ellipse constraints; the vehicle kinematic constraints include: vehicle model constraints and upper and lower bound constraints of variables; and the road constraints include collision constraints.

[0105] Specifically, the lateral dynamic constraints include lateral acceleration constraints and rollover constraints, wherein the expression for the lateral acceleration constraint is:

[0106] ,

[0107] The expression for the rollover constraint is:

[0108] ,

[0109] in, This indicates the preset lateral acceleration threshold. Indicates the width of the vehicle. Indicates the height of the vehicle's center of gravity; Indicates position The lateral slope angle of the road surface;

[0110] It should be noted that when turning in a steady state, we can consider that the lateral acceleration is less than a set threshold:

[0111] ,

[0112] To ensure the vehicle has no risk of rollover, rollover constraints are considered:

[0113] ,

[0114] in, For vehicle width; The height of the vehicle's center of gravity; Location The lateral slope angle (or side slope angle) of the road surface.

[0115] To ensure the vehicle has no risk of rollover, sideslip constraints are considered:

[0116]

[0117] in, This refers to the road surface adhesion coefficient.

[0118] In summary, we have:

[0119] .

[0120] Specifically, the longitudinal dynamic constraints include the constraint that the longitudinal traction force is subject to its maximum engine power, expressed as:

[0121] ,

[0122] in, Indicates the maximum engine power. Indicates longitudinal traction force. Indicates the vehicle's current speed;

[0123] It should be noted that the longitudinal dynamics constraints take into account the vehicle's maximum power limit, meaning that the longitudinal traction force is constrained by its maximum engine power:

[0124] ,

[0125] in, Let represent the maximum engine power. This constraint holds at every spatial location, therefore:

[0126] ,

[0127] in, It is the gravitational constant; air density; For windward area; This is the drag coefficient.

[0128] Specifically, the expression for the tire friction ellipse constraint is:

[0129] ,

[0130] in, The expression is:

[0131] ,

[0132] in, This indicates the preset longitudinal acceleration threshold. This indicates the preset maximum longitudinal acceleration. This indicates the preset minimum longitudinal acceleration. Indicates longitudinal acceleration. This indicates lateral acceleration.

[0133] It should be noted that the vehicle friction circle constraint takes the following form:

[0134] ,

[0135] in, The expression is:

[0136] ,

[0137] Therefore, the friction circle constraint at each discrete point is:

[0138] .

[0139] like Figure 4 As shown, the friction circle constraint is represented graphically by two semi-ellipses, which describe the constraint relationship between the lateral acceleration and the longitudinal acceleration.

[0140] In this embodiment of the invention, after discretizing the vehicle mass model using the forward Euler method, the expression for the vehicle model constraints is obtained as follows:

[0141] ;

[0142] in, .

[0143] Due to the periodicity of angles, the third heading angle constraint in the vehicle model constraints is transformed into an equation with sine functions on both sides to maintain consistency, i.e., it is written as follows: .

[0144] In this embodiment of the invention, the variable upper and lower bound constraints include velocity constraints, acceleration constraints, and curvature upper and lower bound constraints. The expression for the velocity constraint is:

[0145] ,

[0146] The expressions for the acceleration constraint and the curvature upper and lower bound constraints are as follows:

[0147] .

[0148] It should be noted that, To prevent the matrix singularity from being too small, we take... .

[0149] In this embodiment of the invention, the collision constraint is highly dependent on the spatial position of the vehicle, such as Figure 5As shown, the vehicle is surrounded by two circles, the centers of which are located at the center of the vehicle's rear axle and the center of its front axle, respectively. To ensure the vehicle does not collide with the boundaries, the distance from the centers of the two circles to the lines connecting their respective left and right boundaries is limited to the circle radius R. This results in the following four inequalities, which constitute the expression for the collision constraint:

[0150] ,

[0151] ,

[0152] ,

[0153] ,

[0154] Where L represents the wheelbase; line segment AB represents the center position of the rear axle. The nearest boundary polyline segment on the left boundary, segment DE represents the center point of the rear axle. The nearest boundary polyline segment on the right boundary, segment BC represents the nearest boundary polyline segment on the left boundary of the front axle center position point, and segment EF represents the nearest boundary polyline segment on the right boundary of the front axle center position point.

[0155] It should be noted that, in order to prevent significant longitudinal movement of the vehicle, the center point of the rear axle is restricted to the quadrilateral enclosed by the nearest left and right boundary line segments. Figure 5 (gray shaded area), therefore, additional constraints are added:

[0156] ,

[0157] ,

[0158] Similarly, for any discrete point location, its left and right boundary points and boundary segments can be found, and the above constraints can be applied.

[0159] In this embodiment of the invention, optimizing the vehicle dynamics model to obtain the fastest trajectory planning result for the autonomous vehicle includes:

[0160] 1) Perform lightweight solution on the vehicle dynamics model to obtain the initial trajectory that satisfies the basic constraints;

[0161] Specifically, the first stage of the two-stage solution strategy is lightweight initial solution generation. A physically feasible initial path is generated based on the track centerline, and a preliminary velocity curve is generated using a forward-backward integration algorithm combined with simplified longitudinal dynamic constraints. The two are then combined to form an initial trajectory that satisfies the basic constraints.

[0162] 2) Perform nonlinear programming optimization on the initial trajectory to obtain the fastest trajectory planning result for the autonomous vehicle.

[0163] Specifically, the second stage involves precise optimization of the nonlinear programming. The initial trajectory described above is used as a high-performance preliminary solution and input into the nonlinear programming solver. In a preferred embodiment of the invention, the CasaADi framework is used for problem modeling, and the IPOPT solver is invoked for numerical solution. This preliminary solution effectively guides the solver, avoiding getting trapped in local optima, thereby quickly converging to a time-optimal trajectory that satisfies all complex constraints.

[0164] It should be understood that a two-stage optimization strategy can efficiently and reliably solve nonlinear, nonconvex optimal control problems.

[0165] Pick , , , , , , , , ,exist Figure 6 The example includes trajectory planning under road conditions involving large curvature turns and three-dimensional ramps. The trajectory planning results are as follows: Figure 7 and Figure 8 As shown, the theoretical fastest trajectory length is 813.2m and the optimal travel time is 83.66s.

[0166] In summary, the trajectory planning method for autonomous vehicles under complex road conditions provided by this invention has the following significant advantages compared with the prior art:

[0167] (1) It fundamentally overcomes the limitations of the Frenet coordinate system in high curvature scenarios, significantly improving planning accuracy and reliability: Since modeling is done directly in the Cartesian coordinate system, the problems of non-unique projection points, abrupt changes, and numerical singularities caused by the vehicle trajectory deviating significantly from the reference line are completely avoided. This makes it possible to plan the classic "outer-inner-outer" optimal racing line in continuous sharp curves with high curvature, and the planning results are more accurate and stable.

[0168] (2) Achieving a leap from two-dimensional plane to three-dimensional space, with simultaneous optimization of trajectory safety and ultimate performance: By explicitly introducing longitudinal and lateral slope angles, this invention enables trajectory planning to fully consider the complex influence of real three-dimensional road geometry on vehicle dynamics (such as the influence of slope on sideslip risk, rollover threshold, and power demand). Experimental data show that, compared with methods that ignore three-dimensional road information, the trajectory planned by this invention can increase the ultimate speed on the same curve by about 5%-10%, while increasing the lateral acceleration margin by more than 15%, achieving synergistic optimization of safety and speed.

[0169] (3) Problem modeling and solution efficiency are fundamentally simplified and improved: The organic combination of "spatial domain modeling" and "temporal stateification" allows the minimum time objective and highly dispersed spatial constraints to be handled in the most natural way, greatly simplifying the problem complexity. Combined with the proposed two-stage solution strategy, the solution efficiency and success rate of nonlinear programming problems are effectively improved.

[0170] As another embodiment of the present invention, an autonomous vehicle trajectory planning device 100 for complex road conditions is provided, used to implement the autonomous vehicle trajectory planning method for complex road conditions described above, wherein, as Figure 9 As shown, it includes:

[0171] The acquisition module 110 is used to acquire complex road condition data information, which includes at least the longitudinal slope of the road, the transverse slope of the road, and the road boundary information.

[0172] The vehicle dynamics model construction module 120 is used to construct a vehicle dynamics model in a three-dimensional Cartesian coordinate system based on the complex road condition data information.

[0173] The objective function and constraint determination module 130 is used to determine the objective function and constraint conditions based on the vehicle dynamics model, wherein the objective function is a time-based spatial domain state quantity, and the constraint conditions include at least three-dimensional dynamic constraints, vehicle kinematic constraints, and road constraints.

[0174] The optimization and solution module 140 is used to optimize and solve the vehicle dynamics model to obtain the fastest trajectory planning result of the autonomous vehicle.

[0175] The trajectory planning device for autonomous vehicles under complex road conditions provided by this invention constructs the trajectory planning problem in the spatial domain (arc length domain) in a three-dimensional Cartesian coordinate system, and treats time t as one of the system state variables. This fundamental shift allows the total travel time to be directly expressed as the time difference between the terminal and the initial state, thereby achieving an essential simplification of the objective function and allowing spatially dependent constraints to be handled naturally. This trajectory planning device for autonomous vehicles under complex road conditions, by modeling directly in the Cartesian coordinate system, completely avoids the problems of non-unique projection points, abrupt changes, and numerical singularities caused by significant deviations of the vehicle trajectory from the reference line. This makes it possible to plan the classic "outer-inner-outer" optimal racing line in continuous sharp curves with high curvature, resulting in more accurate and stable planning results. By explicitly introducing longitudinal and lateral slope angles, this invention fully considers the complex influence of real three-dimensional road geometry on vehicle dynamics (such as the impact of slope on sideslip risk, rollover threshold, and power demand). The organic combination of "spatial domain modeling" and "temporal stateification" allows for the most natural handling of minimum time objectives and highly dispersed spatial constraints, greatly simplifying the problem's complexity. Therefore, the trajectory planning device for autonomous vehicles under complex road conditions of this invention can perform safe and efficient fastest trajectory planning in complex road conditions including sharp curves and three-dimensional slopes.

[0176] The specific working principle of the autonomous vehicle trajectory planning device under complex road conditions provided by this invention can be referred to the description of the autonomous vehicle trajectory planning method under complex road conditions above, and will not be repeated here.

[0177] As another embodiment of the present invention, a storage medium is provided, wherein computer instructions are stored, which can be loaded and executed by a processor to implement the autonomous vehicle trajectory planning method under complex road conditions described above.

[0178] In this embodiment of the invention, a non-transitory computer-readable storage medium is provided. The computer-readable storage medium stores computer-executable instructions that can execute the trajectory planning method for autonomous vehicles under complex road conditions in any of the above method embodiments. The storage medium may be a magnetic disk, optical disk, read-only memory (ROM), random access memory (RAM), flash memory, hard disk drive (HDD), or solid-state drive (SSD), etc.; the storage medium may also include combinations of the above types of memory.

[0179] As another embodiment of the present invention, an electronic device is provided, comprising a memory and a processor, wherein the processor is communicatively connected to the memory, the memory is used to store a computer program, and the processor is used to load and execute the computer program to implement the autonomous vehicle trajectory planning method under complex road conditions described above.

[0180] like Figure 10 As shown, the electronic device 10 may include: at least one processor 11, such as a CPU (Central Processing Unit), at least one communication interface 13, a memory 14, and at least one communication bus 12. The communication bus 12 is used to enable communication between these components. The communication interface 13 may include a display screen or a keyboard; optionally, the communication interface 13 may also include a standard wired interface or a wireless interface. The memory 14 may be high-speed RAM (Random Access Memory) or non-volatile memory, such as at least one disk drive. Optionally, the memory 14 may also be at least one storage device located remotely from the aforementioned processor 11. The memory 14 stores application programs, and the processor 11 calls the program code stored in the memory 14 to execute any of the aforementioned method steps.

[0181] The communication bus 12 can be a peripheral component interconnect (PCI) bus or an extended industry standard architecture (EISA) bus, etc. The communication bus 12 can be divided into an address bus, a data bus, a control bus, etc. For ease of representation, Figure 7 The bus is represented by a single thick line, but this does not mean that there is only one bus or one type of bus.

[0182] The memory 14 may include volatile memory, such as random-access memory (RAM); the memory may also include non-volatile memory, such as flash memory, hard disk drive (HDD) or solid-state drive (SSD); the memory 14 may also include a combination of the above types of memory.

[0183] The processor 11 can be a central processing unit (CPU), a network processor (NP), or a combination of CPU and NP.

[0184] The processor 11 may further include a hardware chip. This hardware chip may be an application-specific integrated circuit (ASIC), a programmable logic device (PLD), or a combination thereof. The PLD may be a complex programmable logic device (CPLD), a field-programmable gate array (FPGA), a generic array logic (GAL), or any combination thereof.

[0185] Optionally, memory 14 is also used to store program instructions. Processor 11 can invoke program instructions to implement the present invention. Figure 2 The embodiment illustrates a trajectory planning method for autonomous vehicles under complex road conditions.

[0186] It is understood that the above embodiments are merely exemplary implementations used to illustrate the principles of the present invention, and the present invention is not limited thereto. For those skilled in the art, various modifications and improvements can be made without departing from the spirit and essence of the present invention, and these modifications and improvements are also considered to be within the scope of protection of the present invention.

Claims

1. A trajectory planning method for autonomous vehicles under complex road conditions, characterized in that, include: Acquire complex road condition data, which includes at least the longitudinal slope, transverse slope, and road boundary information. A vehicle dynamics model in a three-dimensional Cartesian coordinate system is constructed based on the complex road condition data. The objective function and constraints are determined based on the vehicle dynamics model, wherein the objective function is a time-based spatial domain state variable, and the constraints include at least three-dimensional dynamic constraints, vehicle kinematic constraints, and road constraints. The vehicle dynamics model is optimized and solved to obtain the fastest trajectory planning result for the autonomous vehicle, where the fastest trajectory is the optimal speed trajectory.

2. The trajectory planning method for autonomous vehicles under complex road conditions according to claim 1, characterized in that, Based on the complex road condition data, a vehicle dynamics model is constructed in a three-dimensional Cartesian coordinate system, including: by As a state variable, As a control variable, a vehicle mass model is constructed in a three-dimensional Cartesian coordinate system. The expression of the vehicle mass model is as follows: ， in, and These represent the horizontal and vertical coordinates of the rear axle center of the vehicle, respectively. Indicates the vehicle's heading angle. Indicates the vehicle's current speed. Indicates the current time of the vehicle; Indicates vehicle acceleration; Indicates the total path length. Indicates arc length; For the vehicle at its current location longitudinal slope angle, Indicates path curvature; The vehicle mass model in above The interval is uniformly discretized to obtain the state vector of the discrete points. Control vector ,in , This represents the number of discrete points.

3. The trajectory planning method for autonomous vehicles under complex road conditions according to claim 1, characterized in that, The objective function is determined based on the vehicle dynamics model, including: Taking the vehicle's current time t as the state variable, the expression for the objective function is obtained as follows: ， in, This represents the control weight matrix; and All represent weighting coefficients. This represents the control vector.

4. The trajectory planning method for autonomous vehicles under complex road conditions according to claim 1, characterized in that, The three-dimensional dynamic constraints include: lateral dynamic constraints, longitudinal dynamic constraints, and tire friction ellipse constraints; the vehicle kinematic constraints include: vehicle model constraints and upper and lower bound constraints of variables; the road constraints include collision constraints.

5. The trajectory planning method for autonomous vehicles under complex road conditions according to claim 4, characterized in that, The lateral dynamic constraints include lateral acceleration constraints and rollover constraints, wherein the expression for the lateral acceleration constraint is: ， The expression for the rollover constraint is: ， in, This indicates the preset lateral acceleration threshold. Indicates the width of the vehicle. Indicates the height of the vehicle's center of gravity; Indicates position The lateral slope angle of the road surface; The longitudinal dynamic constraints include the constraint that the longitudinal traction force is subject to its maximum engine power, expressed as: ， in, Indicates the maximum engine power. Indicates longitudinal traction force. Indicates the vehicle's current speed; The expression for the tire friction ellipse constraint is: ， in, The expression is: ， in, This indicates the preset longitudinal acceleration threshold. This indicates the preset maximum longitudinal acceleration. This indicates the preset minimum longitudinal acceleration. Indicates longitudinal acceleration. This indicates lateral acceleration.

6. The trajectory planning method for autonomous vehicles under complex road conditions according to claim 4, characterized in that, The expression for the vehicle model constraint is: ； The variable upper and lower bound constraints include velocity constraints, acceleration constraints, and curvature upper and lower bound constraints. The expression for the velocity constraint is: ， The expressions for the acceleration constraint and the curvature upper and lower bound constraints are as follows: 。 7. The trajectory planning method for autonomous vehicles under complex road conditions according to claim 4, characterized in that, The expression for the collision constraint is: ， ， ， ， ， ， Where L represents the wheelbase; line segment AB represents the center point of the rear axle. The nearest boundary polyline segment on the left boundary, segment DE represents the center point of the rear axle. The nearest boundary polyline segment on the right boundary, segment BC represents the nearest boundary polyline segment on the left boundary of the front axle center position point, and segment EF represents the nearest boundary polyline segment on the right boundary of the front axle center position point.

8. The trajectory planning method for autonomous vehicles under complex road conditions according to claim 1, characterized in that, The vehicle dynamics model is optimized and solved to obtain the fastest trajectory planning result for the autonomous vehicle, including: The vehicle dynamics model is solved using a lightweight method to obtain an initial trajectory that satisfies the basic constraints; The initial trajectory is optimized using nonlinear programming to obtain the fastest trajectory planning result for the autonomous vehicle.

9. An autonomous vehicle trajectory planning device for complex road conditions, used to implement the autonomous vehicle trajectory planning method for complex road conditions as described in any one of claims 1 to 8, characterized in that, include: The acquisition module is used to acquire complex road condition data information, which includes at least the longitudinal slope of the road, the transverse slope of the road, and the road boundary information. The vehicle dynamics model construction module is used to construct a vehicle dynamics model in a three-dimensional Cartesian coordinate system based on the complex road condition data information. The objective function and constraint determination module is used to determine the objective function and constraint conditions based on the vehicle dynamics model, wherein the objective function is a time-based spatial domain state quantity, and the constraint conditions include at least three-dimensional dynamic constraints, vehicle kinematic constraints, and road constraints. The optimization solution module is used to optimize and solve the vehicle dynamics model to obtain the fastest trajectory planning result of the autonomous vehicle, where the fastest trajectory is the optimal speed trajectory.

10. A storage medium, characterized in that, Used to store computer instructions that can be loaded and executed by a processor to implement the autonomous vehicle trajectory planning method under complex road conditions as described in any one of claims 1 to 8.

Images