An automatic driving three-dimensional trajectory optimization method based on dynamic safety corridor and MPC

Abstract

The application relates to an automatic driving three-dimensional trajectory optimization method based on a dynamic safety corridor and MPC, and the method comprises the following steps: S1. vehicle motion modeling in a Frenet coordinate system; S2. dynamic safety corridor generation based on obstacle prediction; S3. multi-objective optimization cost function construction; S4. constraint condition construction based on a safety corridor hard constraint; S5. numerical solution based on an NLP solver and rolling horizon optimization; and S6. trajectory recovery and post-processing from the Frenet coordinate system to the global coordinate system. The application not only compensates for actuator deviation and cumulative error of perception noise in real time, significantly reduces the dependence on external prediction accuracy, and avoids constraint conflict problems caused by one-by-one processing or linear approximation.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving technology, and in particular to a method for optimizing three-dimensional trajectory of autonomous driving based on dynamic safety corridors and MPC. Background Technology

[0002] The 3D trajectory optimization module for autonomous driving is a crucial link connecting the decision-making and planning layer with the underlying controller. Its core task is to transform the coarse-grained reference trajectory output by the decision layer into a smooth and feasible trajectory that simultaneously satisfies vehicle dynamics constraints, traffic safety constraints, and occupant comfort requirements. Currently, the widely used ILQR (Iterative Linear Quadratic Regulator) method has revealed significant shortcomings in robustness and safety in real-world vehicle deployments. Specifically, the ILQR method performs a second-order approximation of the nonlinear system along the current trajectory (i.e., linearizing the state variables and quadratizing the cost function), and solves for the optimal control sequence through alternating iterations of forward simulation and backward gain updates. In terms of constraint handling, the ILQR method mainly adopts a soft constraint strategy, that is, applying a high-weight penalty term to the region near obstacles or constraint boundaries in the cost function. However, the above technical approach has the following inherent defects: First, a high dependence on the prediction of control input. The ILQR method requires pre-input of the planned speed and front wheel steering angle to construct the initial trajectory. However, in a real-world vehicle environment, these parameters are predicted and generated by a low-order controller, inevitably leading to biases such as actuator response lag, actuator saturation, and sensing measurement noise. These biases accumulate and amplify during the ILQR iteration process, causing the optimized trajectory to deviate from the actual feasible region. Second, safety lacks verifiable guarantees. The ILQR method handles obstacle avoidance requirements with soft constraints, and the selection of penalty weights is highly sensitive to the optimization results. It cannot mathematically guarantee that all safety constraints will be strictly satisfied under constraint conflicts or large external disturbances. Third, consistency is insufficient in multi-obstacle scenarios. When multiple obstacles exist, the ILQR method processes them one by one or fuses constraints through linear approximation, easily leading to contradictory situations where "local constraints are satisfied, but global constraints are violated." These defects manifest in real-world vehicle verification as: significantly increased trajectory tracking error, occasional system instability oscillations, and inability to complete obstacle avoidance tasks in complex obstacle scenarios.

[0003] In the prior art, Chinese patent (publication number: CN112258547A) discloses a method for optimizing vehicle 3D trajectory based on inverse perspective projection transformation and a vehicle-following model. This method comprises three parts: solving the vehicle's 3D trajectory coordinates based on inverse perspective projection transformation, predicting the vehicle's motion coordinates based on the vehicle-following model, and optimizing and correcting the vehicle's 3D trajectory coordinates using a combination of inverse perspective projection transformation and the vehicle-following model. However, this approach has inherent limitations due to its reliance on a single visual perception model for trajectory reconstruction. Specifically, the inverse perspective projection transformation method is susceptible to interference from factors such as changes in camera shooting angle and target occlusion, resulting in limited accuracy in solving the initial trajectory coordinates. Furthermore, this method lacks a mechanism to ensure trajectory continuity in the time dimension, making it difficult to adaptively correct dynamically accumulated errors, thus affecting the smoothness, realism, and robustness of the reconstructed trajectory in complex traffic scenarios.

[0004] In the prior art, Chinese patent (publication number: CN117906621A) discloses a lane matching method, device, electronic device, and storage medium. The method includes: acquiring the current position and current direction of the target to be tracked; and determining the current distance matching degree and current direction matching degree between the target to be tracked and multiple lanes based on the current position and current direction of the target to be tracked, as well as nearby lane information; wherein, the lane information includes the lane position and lane direction of multiple lanes on the road. However, when dealing with complex traffic scenarios such as intersections, this solution mainly relies on preset rules or simple kinematic models for trajectory generation, which is difficult to effectively cope with complex interactive behaviors in dynamic traffic flow. The method has limited adaptability to the time-varying uncertainty of the movement of surrounding vehicles, resulting in insufficient planning flexibility of the generated trajectory. The accuracy and environmental adaptability of the trajectory optimization results are both constrained, making it difficult to meet the high-order requirements of autonomous driving systems for safety and trajectory smoothness in actual road applications. Summary of the Invention

[0005] In view of the shortcomings of the prior art, the present invention provides an autonomous driving three-dimensional trajectory optimization method based on dynamic safety corridor and MPC, which not only compensates for actuator deviation and perception noise accumulation error in real time, significantly reducing the dependence on external prediction accuracy, but also effectively avoids the constraint conflict problem caused by processing one by one or linear approximation.

[0006] To achieve the above and other related objectives, the present invention provides the following technical solution: A method for optimizing the three-dimensional trajectory of autonomous driving based on dynamic safety corridors and MPC, the method comprising: Vehicle motion modeling in the S1 Frenet coordinate system; S2. Dynamic safe corridor generation based on obstacle prediction; S3. Construction of cost function for multi-objective optimization; S4. Construction of constraint conditions based on hard constraints of the safety corridor; S5. Numerical solution and rolling temporal optimization based on NLP solver; S6. Trajectory recovery and post-processing from Frenet coordinate system to global coordinate system.

[0007] Further, in step S1, the vehicle motion modeling in the Frenet coordinate system includes: S11. Use the Frenet coordinate system, with the center line of the reference lane as the s-axis and the vertical direction as the d-axis; S12. State variables x(τ), x(τ)∈R 4 Defined as, , Where s(τ) is the vertical position and d(τ) is the horizontal position. For longitudinal velocity, For lateral velocity; S13. Control variable u(τ), u(τ)∈R 2 Positioned as, , in, For longitudinal acceleration, The longitudinal acceleration is given by a discrete time interval of τ = 0, 1, ..., N-1, with a sampling period of Δt.

[0008] Further, in step S2, the dynamic safety corridor generation based on obstacle prediction includes: S21. Determine road boundary constraints, extract the left and right road boundaries from the lateral drivable range of the lanes provided by the high-precision map, and reserve a safety margin for the vehicle width to form an initial lateral feasible domain. S22. For each prediction time step k, predict the vehicle position (s,d) based on the reference velocity sequence. S23. With (s, d) as the center, gradually expand the safe area in the four directions of front, back, left and right. After each expansion step, call the rectangle-ellipse collision detection to determine whether it intersects with the obstacle. S24. Collision detection and extension termination: If a collision is detected between the extension boundary and the predicted position of the obstacle during the extension process in a certain direction, or if the preset maximum extension distance limit has been reached, the extension in that direction will stop and the current boundary value will be recorded. S25. Corridor boundary fusion: The road boundary constraints determined in step S21 are fused with the expansion results in each direction obtained in step S24. For each prediction time step, the most stringent boundary value among the constraint sources is taken to generate the dynamic safe corridor boundary at that moment, forming a time-varying lateral safe feasible region along the longitudinal coordinate s direction.

[0009] Further, in step S3, the multi-objective optimization cost function is constructed such that the optimization objective aims to minimize the lateral deviation, the control input, and their rate of change, while incorporating an obstacle avoidance potential field penalty. The total cost function J is defined as follows: , Among them, J track To track and stabilize at the cost, J comfort For the price of comfort, J safety To avoid the cost of soft constraints, J corridor The cost of relaxing the corridor boundaries.

[0010] Furthermore, the tracking and stabilization cost J track for, , Among them, w d and w s Penalize errors in horizontal and vertical deviations from the reference line. and To penalize lateral and longitudinal speeds in order to enhance driving stability.

[0011] Furthermore, the comfort cost J comfort for, , Among them, w a To limit the magnitude of acceleration, w j To limit jerk.

[0012] Furthermore, the obstacle avoidance soft constraint cost J safety for, , Where p(τ) is the position of the vehicle, p obs,m (τ) represents the position of the m-th obstacle, ε is a small quantity to prevent division by zero, and M is a positive integer.

[0013] Furthermore, the corridor boundary relaxation cost J corridor To introduce a slack variable penalty at the safety corridor boundary, a high penalty w is applied when a vehicle exceeds the safety corridor boundary. big .

[0014] Further, in step S5, the numerical solution and rolling time-domain optimization based on the NLP solver are as follows: The optimization variables must satisfy the following physical and geometric constraints: Dynamic constraints: , Physical constraints of state and control variables: , Lateral acceleration, lateral jerk, and lateral velocity were restricted respectively. , The longitudinal acceleration, longitudinal jerk, and longitudinal velocity were respectively limited; Hard constraints on safe corridors: Based on the predicted location of obstacles, area expansion calculations are performed, and lane restrictions are determined, thereby dynamically shrinking or adjusting safety boundaries. The resulting dynamic safety corridor limits the lateral drivable area of ​​vehicles. , Among them, D min D max H min and H max It is a convex feasible region boundary generated based on the road boundary and the location of static / dynamic obstacles.

[0015] Further, in step S5, the numerical solution and rolling time-domain optimization based on the NLP solver are performed by using the CasADi symbolic computation framework for mathematical modeling and automatic differentiation of the optimization problem, and the IPOPT interior point method solver is used to numerically solve the above NLP problem; in step S6, the trajectory recovery and post-processing from the Frenet coordinate system to the global coordinate system is performed by converting the optimal trajectory (s(t), d(t)) in the Frenet coordinate system obtained by optimization back to the global Cartesian coordinate system (x,y,θ) through the geometric transformation relationship of the reference lane centerline, and performing spline interpolation smoothing on the converted trajectory point sequence.

[0016] The present invention has the following positive effects: 1. This invention ensures safety through hard constraints: the dynamic safety corridor boundary is directly embedded as a hard constraint condition into the constraint set of the optimization problem. The solver mathematically guarantees that the optimal solution strictly satisfies the obstacle avoidance constraint and the road boundary constraint, thereby providing a verifiable constraint satisfaction guarantee for vehicle driving safety.

[0017] 2. The present invention reduces dependence on external predictions: Based on the rolling time-domain replanning mechanism, the optimization problem is resolved using the latest sensor observation data in each control cycle, thereby compensating for actuator deviations and accumulated errors of sensing noise in real time, and significantly reducing dependence on the accuracy of external predictions.

[0018] 3. Consistency guarantee of multi-obstacle constraints in this invention: The avoidance constraints of multiple obstacles can be simultaneously incorporated into the constraint set of the optimization problem for unified solution, effectively avoiding constraint conflict problems caused by processing them one by one or linear approximation.

[0019] 4. The trajectory quality of the present invention is significantly improved: Simulation results show that in typical scenarios such as lane changing, lane detour, and static and dynamic obstacle detour, the trajectory generated by the MPC method of the present invention is superior to the existing ILQR method in terms of smoothness, control command continuity and jerk suppression.

[0020] 5. The architecture of this invention has good scalability: the method framework can be easily combined with technologies such as Robust Model Predictive Control (Robust MPC), Adaptive Model Predictive Control (Adaptive MPC), or Terminal Invariant Set to further improve the provable security and environmental robustness of the system. Attached Figure Description

[0021] Figure 1 This is a schematic diagram of the method flow of the present invention. Detailed Implementation

[0022] The exemplary embodiments of this disclosure are described below with reference to the accompanying drawings, including various details of the embodiments to aid understanding, and should be considered merely exemplary. Therefore, those skilled in the art will recognize that various changes and modifications can be made to the embodiments described herein without departing from the scope and spirit of this disclosure. Similarly, for clarity and brevity, descriptions of well-known functions and structures are omitted in the following description.

[0023] Example: Figure 1 As shown, an autonomous driving three-dimensional trajectory optimization method based on dynamic safety corridors and MPC is presented, the method comprising: Vehicle motion modeling in the S1 Frenet coordinate system; S2. Dynamic safe corridor generation based on obstacle prediction; S3. Construction of cost function for multi-objective optimization; S4. Construction of constraint conditions based on hard constraints of the safety corridor; S5. Numerical solution and rolling temporal optimization based on NLP solver; S6. Trajectory recovery and post-processing from Frenet coordinate system to global coordinate system.

[0024] In this embodiment, in step S1, the vehicle motion modeling in the Frenet coordinate system includes: S11. Use the Frenet coordinate system, with the center line of the reference lane as the s-axis and the vertical direction as the d-axis; S12. State variables x(τ), x(τ)∈R 4 Defined as, , Where s(τ) is the vertical position and d(τ) is the horizontal position. For longitudinal velocity, For lateral velocity; S13. Control variable u(τ), u(τ)∈R 2 Positioned as, , in, For longitudinal acceleration, The longitudinal acceleration is given by a discrete time interval of τ = 0, 1, ..., N-1, with a sampling period of Δt.

[0025] In this embodiment, in step S2, the generation of the dynamic safety corridor based on obstacle prediction includes: S21. Determine road boundary constraints, extract the left and right road boundaries from the lateral drivable range of the lanes provided by the high-precision map, and reserve a safety margin for the vehicle width to form an initial lateral feasible domain. S22. For each prediction time step k, predict the vehicle position (s,d) based on the reference velocity sequence. S23. With (s, d) as the center, gradually expand the safe area in the four directions of front, back, left and right. After each expansion step, call the rectangle-ellipse collision detection to determine whether it intersects with the obstacle. S24. Collision detection and extension termination: If a collision is detected between the extension boundary and the predicted position of the obstacle during the extension process in a certain direction, or if the preset maximum extension distance limit has been reached, the extension in that direction will stop and the current boundary value will be recorded. S25. Corridor boundary fusion: The road boundary constraints determined in step S21 are fused with the expansion results in each direction obtained in step S24. For each prediction time step, the most stringent boundary value among the constraint sources is taken to generate the dynamic safe corridor boundary at that moment, forming a time-varying lateral safe feasible region along the longitudinal coordinate s direction.

[0026] In this embodiment, in step S3, the multi-objective optimization cost function is constructed such that the optimization objective aims to minimize the lateral deviation, control input, and their rate of change, while incorporating an obstacle avoidance potential field penalty. The total cost function J is defined as follows: , Among them, J track To track and stabilize at the cost, J comfort For the price of comfort, J safety To avoid the cost of soft constraints, J corridor The cost of relaxing the corridor boundaries.

[0027] In this embodiment, the tracking and stabilization cost J track for, , Among them, w d and w s Penalize errors in horizontal and vertical deviations from the reference line. and To penalize lateral and longitudinal speeds in order to enhance driving stability.

[0028] In this embodiment, the comfort cost J comfort for, , Among them, w a To limit the magnitude of acceleration, w j To limit jerk.

[0029] In this embodiment, the obstacle avoidance soft constraint cost J safety for, , Where p(τ) is the position of the vehicle, p obs,m (τ) represents the position of the m-th obstacle, ε is a small quantity to prevent division by zero, and M is a positive integer.

[0030] In this embodiment, the corridor boundary relaxation cost J corridor To introduce a slack variable penalty at the safety corridor boundary, a high penalty w is applied when a vehicle exceeds the safety corridor boundary. big .

[0031] In this embodiment, in step S5, the numerical solution and rolling time-domain optimization based on the NLP solver are as follows: The optimization variables must satisfy the following physical and geometric constraints: Dynamic constraints: , Physical constraints of state and control variables: , Lateral acceleration, lateral jerk, and lateral velocity were restricted respectively. , The longitudinal acceleration, longitudinal jerk, and longitudinal velocity were respectively limited; Hard constraints on safe corridors: Based on the predicted location of obstacles, area expansion calculations are performed, and lane restrictions are determined, thereby dynamically shrinking or adjusting safety boundaries. The resulting dynamic safety corridor limits the lateral drivable area of ​​vehicles. , Among them, D min D max Hmin and H max It is a convex feasible region boundary generated based on the road boundary and the location of static / dynamic obstacles.

[0032] In this embodiment, in step S5, the numerical solution and rolling time-domain optimization based on the NLP solver are performed by using the CasADi symbolic computation framework for mathematical modeling and automatic differentiation of the optimization problem, and the IPOPT interior point method solver is used to numerically solve the above NLP problem. In step S6, the trajectory recovery and post-processing from the Frenet coordinate system to the global coordinate system is performed by converting the optimal trajectory (s(t), d(t)) in the Frenet coordinate system obtained by optimization back to the global Cartesian coordinate system (x,y,θ) through the geometric transformation relationship of the reference lane centerline, and then performing spline interpolation smoothing on the converted trajectory point sequence.

[0033] In this embodiment, the present invention provides an autonomous driving three-dimensional trajectory optimization system based on dynamic safety corridors and MPC, including a computer device that is programmed or configured to perform the steps of the autonomous driving three-dimensional trajectory optimization method based on dynamic safety corridors and MPC.

[0034] The system specifically includes the following modules: (i) a Frenet coordinate system vehicle motion modeling module, which is responsible for transforming the three-dimensional kinematic model of the vehicle in the global Cartesian coordinate system to the Frenet coordinate system. The module contains coordinate transformation units and dynamic order reduction units, decoupling the vehicle state into a longitudinal state along the reference line and a lateral state perpendicular to the reference line. Simultaneously, it introduces road slope and roll angle parameters to construct a discrete-time state-space equation containing three-dimensional spatial characteristics, providing a basic prediction model for subsequent model predictive control.

[0035] (ii) The dynamic safety corridor generation module is connected to the Frenet coordinate system vehicle motion modeling module. This module utilizes environmental perception data and combines it with a hidden Markov model to predict the probability reachability set of obstacles in the future prediction time domain. A convex envelope processing unit generates a time-varying three-dimensional dynamic obstacle envelope surface. Centered on the vehicle itself and incorporating road boundary information, a three-dimensional safety corridor that dynamically shrinks or expands over time is constructed in the Frenet coordinate system, ensuring that the vehicle is always within a collision-free safe space.

[0036] (III) A multi-objective optimization cost function construction module, connected to the dynamic safety corridor generation module, constructs a cost function that includes multiple optimization objectives such as trajectory tracking error, dynamic constraints, energy consumption, and safety boundaries. By introducing an adaptive weight coefficient matrix, the above multi-objective costs are weighted and summed to form the final optimization objective function, so as to balance safety, comfort, and energy consumption.

[0037] (iv) Constraint construction module, which is connected to the multi-objective optimization cost function construction module. This module transforms the dynamic safety corridor into hard constraints in model predictive control, including lateral position constraints, longitudinal position constraints, three-dimensional spatial constraints and the vehicle's own physical limit constraints, to ensure that the optimization solution process strictly meets safety and physical boundaries.

[0038] (v) Numerical solution and rolling time-domain optimization module, which is connected to the constraint construction module. This module uses a sequential quadratic programming solver to construct a nonlinear programming problem from the discrete state space equation, multi-objective optimization cost function and hard constraints. It performs rolling optimization in the set prediction time domain and control time domain and outputs the optimal control sequence at the current time.

[0039] (vi) The trajectory recovery and post-processing module is connected to the numerical solution and rolling time domain optimization module. This module maps the optimal three-dimensional trajectory sequence obtained in the Frenet coordinate system back to the global Cartesian coordinate system through the coordinate transformation matrix, and uses B-spline curves for smoothing post-processing to eliminate the small jitter caused by the numerical solution, and outputs it to the underlying vehicle actuator.

[0040] The present invention also provides a computer-readable storage medium storing a computer program programmed or configured to perform the described method for optimizing the three-dimensional trajectory of autonomous driving based on dynamic safety corridors and MPC.

[0041] Any references to memory, storage, database, or other media used in the embodiments provided in this application may 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 RAMbus dynamic RAM (RDRAM), etc.

[0042] In summary, this invention not only compensates for actuator deviation and accumulated error of perceived noise in real time, significantly reducing dependence on external prediction accuracy, but also effectively avoids constraint conflict problems caused by processing one by one or linear approximation.

[0043] The specific embodiments described above do not constitute a limitation on the scope of protection of this disclosure. Those skilled in the art should understand that various modifications, combinations, sub-combinations, and substitutions can be made according to design requirements and other factors. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of this disclosure should be included within the scope of protection of this disclosure.

Claims

1. A method for optimizing the three-dimensional trajectory of autonomous driving based on dynamic safety corridors and MPC, characterized in that, The method includes: Vehicle motion modeling in the S1 Frenet coordinate system; S2. Dynamic safe corridor generation based on obstacle prediction; S3. Construction of cost function for multi-objective optimization; S4. Construction of constraint conditions based on hard constraints of the safety corridor; S5. Numerical solution and rolling temporal optimization based on NLP solver; S6. Trajectory recovery and post-processing from Frenet coordinate system to global coordinate system.

2. The method for optimizing the three-dimensional trajectory of autonomous driving based on dynamic safety corridors and MPC according to claim 1, characterized in that, In step S1, the vehicle motion modeling in the Frenet coordinate system includes: S11. Use the Frenet coordinate system, with the center line of the reference lane as the s-axis and the vertical direction as the d-axis; S12. State variables x(τ), x(τ)∈R 4 Defined as, ， Where s(τ) is the vertical position and d(τ) is the horizontal position. For longitudinal velocity, For lateral velocity; S13. Control variable u(τ), u(τ)∈R 2 Positioned as, ， in, For longitudinal acceleration, The longitudinal acceleration is given by a discrete time interval of τ = 0, 1, ..., N-1, with a sampling period of Δt.

3. The method for optimizing the three-dimensional trajectory of autonomous driving based on dynamic safety corridors and MPC according to claim 1, characterized in that, In step S2, the generation of the dynamic safety corridor based on obstacle prediction includes: S21. Determine road boundary constraints, extract the left and right road boundaries from the lateral drivable range of the lanes provided by the high-precision map, and reserve a safety margin for the vehicle width to form an initial lateral feasible domain. S22. For each prediction time step k, predict the vehicle position (s,d) based on the reference velocity sequence. S23. With (s, d) as the center, gradually expand the safe area in the four directions of front, back, left and right. After each expansion step, call the rectangle-ellipse collision detection to determine whether it intersects with the obstacle. S24. Collision detection and extension termination: If a collision is detected between the extension boundary and the predicted position of the obstacle during the extension process in a certain direction, or if the preset maximum extension distance limit has been reached, the extension in that direction will stop and the current boundary value will be recorded. S25. Corridor boundary fusion: The road boundary constraints determined in step S21 are fused with the expansion results in each direction obtained in step S24. For each prediction time step, the most stringent boundary value among the constraint sources is taken to generate the dynamic safe corridor boundary at that moment, forming a time-varying lateral safe feasible region along the longitudinal coordinate s direction.

4. The method for optimizing the three-dimensional trajectory of autonomous driving based on dynamic safety corridors and MPC according to claim 1, characterized in that, In step S3, the multi-objective optimization cost function is constructed such that the optimization objective aims to minimize the lateral deviation, control input, and their rate of change, while incorporating an obstacle avoidance potential field penalty. The total cost function J is defined as follows: ， Among them, J track To track and stabilize at the cost, J comfort For the price of comfort, J safety To avoid the cost of soft constraints, J corridor The cost of relaxing the corridor boundaries.

5. The method for optimizing the three-dimensional trajectory of autonomous driving based on dynamic safety corridors and MPC according to claim 4, characterized in that: The tracking and stabilization cost J track for, ， Among them, w d and w s Penalize errors in horizontal and vertical deviations from the reference line. and To penalize lateral and longitudinal speeds in order to enhance driving stability.

6. The method for optimizing the three-dimensional trajectory of autonomous driving based on dynamic safety corridors and MPC according to claim 4, characterized in that: The comfort cost J comfort for, ， Among them, w a To limit the magnitude of acceleration, w j To limit jerk.

7. The autonomous driving three-dimensional trajectory optimization method based on dynamic safety corridor and MPC according to claim 4, characterized in that: The obstacle avoidance soft constraint cost J safety for, ， Where p(τ) is the position of the vehicle, p obs,m (τ) represents the position of the m-th obstacle, ε is a small quantity to prevent division by zero, and M is a positive integer.

8. The method for optimizing the three-dimensional trajectory of autonomous driving based on dynamic safety corridors and MPC according to claim 4, characterized in that: The corridor boundary relaxation cost J corridor To introduce a slack variable penalty at the safety corridor boundary, a high penalty w is applied when a vehicle exceeds the safety corridor boundary. big .

9. The method for optimizing the three-dimensional trajectory of autonomous driving based on dynamic safety corridors and MPC according to claim 1, characterized in that, In step S5, the numerical solution and rolling time-domain optimization based on the NLP solver are as follows: The optimization variables must satisfy the following physical and geometric constraints: Dynamic constraints: ， Physical constraints of state and control variables: ， Lateral acceleration, lateral jerk, and lateral velocity were restricted respectively. ， The longitudinal acceleration, longitudinal jerk, and longitudinal velocity were respectively limited; Hard constraints on safe corridors: Based on the predicted location of obstacles, area expansion calculations are performed, and lane restrictions are determined, thereby dynamically shrinking or adjusting safety boundaries. The resulting dynamic safety corridor limits the lateral drivable area of ​​vehicles. ， Among them, D min D max H min and H max It is a convex feasible region boundary generated based on the road boundary and the location of static / dynamic obstacles.

10. The method for optimizing the three-dimensional trajectory of autonomous driving based on dynamic safety corridors and MPC according to claim 1, characterized in that, In step S5, the numerical solution and rolling time-domain optimization based on the NLP solver are performed by using the CasADi symbolic computation framework for mathematical modeling and automatic differentiation of the optimization problem, and the IPOPT interior point method solver is used to numerically solve the above NLP problem. In step S6, the trajectory recovery and post-processing from the Frenet coordinate system to the global coordinate system is performed by converting the optimal trajectory (s(t), d(t)) in the Frenet coordinate system obtained by optimization back to the global Cartesian coordinate system (x,y,θ) through the geometric transformation relationship of the reference lane centerline, and then performing spline interpolation smoothing on the converted trajectory point sequence.

Images