A method for replanning the trajectory of autonomous vehicles

Abstract

This invention relates to the field of autonomous driving technology, specifically providing a trajectory replanning method for autonomous vehicles. First, stability risks are assessed using real-time vehicle dynamic parameters, defining three states of vehicle stability, and based on these, intervention and exit criteria for trajectory replanning are designed. When vehicle movement is assessed as unstable, virtual obstacles are generated using predictive information from the path tracking module, transforming the dynamic stability boundary into spatial constraints. Nonlinear model predictive control is employed for trajectory replanning, combined with a dynamic weight adjustment mechanism, adjusting the weights between path tracking accuracy and obstacle avoidance requirements based on the relative positions of the virtual obstacles. Finally, an optimized trajectory containing stability constraints is generated, and high-precision tracking is achieved through linear MPC. This invention can effectively balance path tracking accuracy and vehicle stability under complex operating conditions, preventing the vehicle from entering dynamically unstable regions.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving technology, and more specifically, to a method for replanning the trajectory of autonomous vehicles to ensure the stability and safety of vehicles under extremely complex conditions. Background Technology

[0002] Autonomous driving technology, as a crucial development direction for future transportation, has received widespread attention from both academia and industry in recent years. Its core lies in achieving autonomous navigation and driving of vehicles through advanced sensor technology, control algorithms, and decision-making systems. The functionality of modern autonomous driving systems heavily relies on cross-domain collaboration between the autonomous driving domain controller and the vehicle domain controller. The autonomous driving domain controller, as the system's "decision-making center," generates collision-free planned paths based on environmental perception and global decision-making; while the vehicle domain controller, as the "execution center," needs to achieve high-precision path tracking through chassis control. However, in existing technical architectures, the functional separation between the planning and execution layers makes it difficult to integrate vehicle dynamics into the trajectory generation process. Traditional path planning methods are mainly based on kinematic models or simplified dynamic assumptions, focusing on calculating safe trajectories for static obstacle avoidance, but neglecting the impact of nonlinear vehicle dynamics characteristics such as tire lateral force saturation and dynamic load transfer on stability. This limitation is particularly pronounced in extreme conditions such as emergency obstacle avoidance on low-adhesion surfaces and high-speed lane changes—the reference trajectory output by the planning layer may exceed the actual dynamic capabilities of the vehicle, causing the vehicle domain controller to fail to track effectively, and even triggering dangerous states such as yaw instability or sideslip.

[0003] As autonomous driving applications extend to complex conditions such as highways and slippery roads, vehicle dynamics stability has become a core constraint that cannot be ignored in path planning. Current mainstream planning architectures adopt a layered, modular design: global path planning relies on search algorithms such as A* and RRT to generate coarse-grained routes, behavior planning provides collision-free decisions, and motion planning generates time-space trajectories within a collision-free space based on a cost function. However, the cost function of such methods typically only considers vehicle physical dimensions, kinematic constraints, or road adhesion limits, failing to incorporate the dynamic stability boundary into the optimization objective. This leads to the planned trajectory potentially approaching or exceeding the vehicle's dynamic stability region under extreme conditions, forcing the vehicle domain controller to frequently trigger the electronic stability control system to intervene and correct during path tracking. This not only reduces ride comfort but also exhibits lag in correction, failing to mitigate instability risks at the source and potentially exacerbating instability risks due to control conflicts.

[0004] The industry has attempted to overcome limitations through various architectures: while integrated frameworks try to combine stability control and path tracking, their customized designs struggle to adapt to the modular requirements of mass-production systems; furthermore, end-to-end planning methods simulate human driving behavior through direct mapping of sensor signals and control commands, but their black-box nature leads to a lack of interpretability and insufficient reliability in scenarios with stringent safety requirements. None of the above solutions establish a real-time dynamic interaction mechanism between the planning and execution layers; autonomous driving can only passively respond to unstable states and cannot proactively avoid dynamic risks during the trajectory generation phase.

[0005] A deeper contradiction stems from the information barriers between cross-domain controllers. The planning module of the autonomous driving domain controller lacks the ability to perceive the real-time dynamic state of the vehicle (such as yaw rate and tire slip angle), while the execution layer of the vehicle domain controller cannot feed back stability boundaries to the planning layer to form a closed-loop optimization. This unidirectional data flow leads to a dilemma for the system in scenarios such as emergency obstacle avoidance: excessive pursuit of path tracking accuracy may trigger dynamic instability, while conservative stability control will sacrifice obstacle avoidance capabilities. The existing technical architecture struggles to achieve a dynamic trade-off between the two, becoming a key bottleneck restricting the full-scenario deployment of high-level autonomous driving.

[0006] Against this backdrop, a novel control architecture capable of deeply integrating vehicle dynamics and path planning is urgently needed. An ideal solution must overcome the information barriers of traditional hierarchical architectures, achieving closed-loop optimization of planning and execution through cross-domain collaboration. Specifically, the system should possess the ability to determine vehicle dynamic instability, consider vehicle dynamic stability during trajectory generation, and adaptively adjust the priority of control objectives based on real-time operating conditions. Such a design not only needs to address the complexity of nonlinear dynamic modeling but also requires efficient computation on embedded hardware platforms to meet the stringent real-time and robustness requirements of mass-production systems. Current technologies have not yet effectively overcome these challenges, and this invention aims to fill this critical gap. Summary of the Invention

[0007] To address the challenge of autonomous driving domain controllers failing to plan trajectories that ensure vehicle dynamic stability under extreme conditions, and to achieve an ideal balance between tracking accuracy and vehicle stability, this invention proposes a path replanning method in a cross-domain controller collaboration context. First, this invention proposes an instability determination method for autonomous vehicles, using whether the vehicle's current state exceeds a stability threshold to determine whether trajectory replanning should be initiated, and designs a reasonable intervention and exit mechanism to ensure safe and timely intervention while avoiding frequent interventions. Second, this invention reconstructs the path replanning framework within the context of cross-domain integration between the autonomous driving domain controller and the vehicle domain controller. It introduces the concept of virtual obstacles, modifies the obstacle avoidance cost function to guide the vehicle away from dynamically unstable regions, and utilizes the path tracking model to predict the location information of virtual obstacles provided by the controller. Finally, this invention achieves a good balance between tracking accuracy, vehicle stability, and collision avoidance by designing a variable weight mechanism.

[0008] To achieve the above objectives, this invention proposes a trajectory replanning method for autonomous vehicles, comprising:

[0009] The real-time stability status of a vehicle is determined based on its dynamic parameters.

[0010] The execution or exit of trajectory replanning is determined based on the real-time stability status.

[0011] When performing trajectory replanning, the future position prediction information of the path tracking model predictive controller is used to generate a sequence of virtual obstacles, and the vehicle dynamics stability boundary is transformed into a spatial obstacle avoidance constraint. Based on the optimization problem of the path tracking model predictive controller framework, combined with an adaptive weight adjustment strategy, the path tracking error weight and obstacle avoidance weight are adjusted according to the relative position of the vehicle and the virtual obstacles, thereby switching the vehicle's path tracking mode or collision avoidance priority mode, and obtaining the vehicle's control quantity sequence.

[0012] The target trajectory point sequence is obtained based on the control quantity sequence, and the trajectory point sequence is converted into a reference trajectory sequence based on a fourth-order polynomial.

[0013] The vehicle is driven according to the reference trajectory sequence, and the system input control quantity during the driving process is obtained based on the path tracking objective function. The driving process is controlled based on the system input control quantity.

[0014] Furthermore, the real-time stability state includes an unstable state, a stable state, and an overstable state;

[0015] The conditions for determining the unstable state are: the sideslip angle and sideslip velocity of the center of mass exceed 80% of the stability boundary, or the yaw rate deviation exceeds 80% of the limit.

[0016] The conditions for determining the stable state are: the sideslip angle and sideslip velocity of the center of mass are within 80% of the stability boundary and the deviation of the yaw velocity from the reference value is less than 80% of the limit.

[0017] The criteria for determining the over-stable state are: the centroid sideslip angle is less than 30% of the stable boundary, and the yaw rate deviation is less than 30% of the limit.

[0018] Furthermore, the process of determining whether to execute or exit trajectory replanning based on the real-time stability state includes:

[0019] When the vehicle transitions from a "stable state" to an "unstable state," trajectory replanning is initiated.

[0020] When the vehicle returns from an "unstable state" to an "overstable state" and remains so for more than 3 seconds, it exits the trajectory replanning process.

[0021] If the vehicle enters an "overstable state" but the duration is less than 3 seconds, trajectory replanning will continue, and the duration of the state will be recalculated.

[0022] Furthermore, the process of generating a virtual obstacle sequence using path tracking model prediction information of the controller's future position includes:

[0023] Using the potential instability point in the future position prediction information of the path tracking model predictive controller as the center coordinate, several virtual obstacles are deployed at equal intervals along the vehicle's direction of travel; the positions of the virtual obstacles satisfy the following spatial mapping relationship:

[0024] ,

[0025] in The distance between obstacles. For the vehicle's heading angle, The center coordinates of the potential instability point, ( , () represents the coordinates of the virtual obstacle. This represents the number of virtual obstacles.

[0026] Furthermore, the optimization problem of the path tracking model predictive controller framework includes: minimizing the path tracking error in the prediction time domain, minimizing the control input sequence in the control time domain, and minimizing the obstacle cost function, as detailed below:

[0027] ,

[0028] in, Indicates the optimization objective. Represents the obstacle cost function. This represents the prediction time domain of the trajectory replanning module. This represents the control time domain of the trajectory replanning module. This indicates the vehicle's current position in the global coordinate system. These are the coordinates of the virtual obstacles. These are the weights of the obstacle cost function. and These are the path tracking error weights and the control input weights, respectively. and Indicates the lower and upper bounds of the control input. Represents the reference trajectory sequence. k Indicates the current moment. i This indicates the step size for the prediction.

[0029] Furthermore, the adaptive weight adjustment strategy includes:

[0030] When the vehicle approaches the instability zone, the obstacle avoidance weight is increased to 100, while the path tracking weight is reduced to 0.5; when the vehicle passes the instability point, the obstacle avoidance weight is reset to zero and the path tracking weight is increased to 200; the control input weight is kept at 10.

[0031] Furthermore, the conversion method for the reference trajectory sequence is as follows:

[0032] ,

[0033] in, For reference lateral displacement, For reference heading angle, and These are the fitting coefficients. For longitudinal displacement, This is the heading angle.

[0034] Furthermore, the method for calculating the path tracking objective function is as follows:

[0035] ,

[0036] in, This represents the path tracing objective function. Indicates the prediction time domain, Indicates control of the time domain. and These are the upper and lower limits for controlling the increment, respectively. and This indicates the saturation constraint of the steering system. and This indicates a hard constraint applied to the system output. This represents the optimal control sequence. This represents the sequence of control increments obtained by the path tracking controller. k Indicates the current moment. i Indicates the step size of the prediction. The weight matrix represents the tracking error. This represents the weight matrix that controls the input. Represents the state vector. This represents the control increment for each step.

[0037] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0038] This invention defines three states of vehicle dynamics stability and designs intervention and exit criteria for trajectory replanning. It dynamically generates virtual obstacles by obtaining predicted vehicle position information from the trajectory tracking controller, transforming the dynamic boundary into obstacle avoidance constraints for path planning, thus guiding the vehicle to avoid areas that may lead to dynamic instability. Based on a nonlinear model predictive control framework, this invention constructs a trajectory replanning optimization problem, using path tracking error, control input, and virtual obstacle distance cost as multiple objective functions. Under the physical limits of the vehicle actuators and road boundary constraints, it generates a smooth, executable trajectory, and finally achieves high-precision tracking through linear model predictive control. This invention effectively mitigates the risk of vehicle dynamics instability caused by the disconnect between the planning and execution layers, improving the safety of autonomous driving systems in extreme scenarios such as high-speed emergency obstacle avoidance and low road surface adhesion conditions. Attached Figure Description

[0039] Various other advantages and benefits will become apparent to those skilled in the art upon reading the following detailed description of preferred embodiments. The accompanying drawings are for illustrative purposes only and are not intended to limit the invention. Furthermore, the same reference numerals denote the same parts throughout the drawings. In the drawings:

[0040] Figure 1 Block diagram of the autonomous vehicle trajectory replanning method provided by this invention;

[0041] Figure 2 This is a schematic diagram of a four-wheeled, two-degree-of-freedom vehicle dynamics model according to an embodiment of the present invention;

[0042] Figure 3 This is a diagram showing the relationship between tire lateral force, vertical load, and road adhesion coefficient in a tire model according to an embodiment of the present invention.

[0043] Figure 4 This is a diagram showing the phase plane trajectory and stability boundary of the vehicle's center of gravity sideslip angle according to an embodiment of the present invention.

[0044] Figure 5 A flowchart illustrating the variable weighting mechanism provided by the present invention;

[0045] Figure 6 The simulation test results are compared in typical scenarios of double lane change and low-adhesion road surface according to the embodiments of the present invention; wherein (a) is a comparison of path tracking results, (b) is a comparison of steering wheel angle control input, (c) is a comparison of oversteering instability index, and (d) is a comparison of vehicle position and heading angle.

[0046] Figure 7 The comparison of system computational load rate in hardware-in-the-loop testing according to the embodiments of the present invention; wherein (a) is the periodic task load rate of 10ms and (b) is the periodic task load rate of 200s. Detailed Implementation

[0047] Exemplary embodiments of the present disclosure will now be described in more detail with reference to the accompanying drawings. While exemplary embodiments of the present disclosure are shown in the drawings, it should be understood that the present disclosure may be implemented in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided to enable a more thorough understanding of the present disclosure and to fully convey the scope of the disclosure to those skilled in the art. It should be noted that, unless otherwise specified, the embodiments and features described herein can be combined with each other. The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0048] like Figure 1 As shown, the autonomous vehicle trajectory replanning method proposed in this invention consists of three parts: a vehicle dynamics instability prediction method, trajectory replanning based on virtual obstacle mapping and variable weight mechanism, and trajectory tracking based on model predictive control.

[0049] First, stability risk is assessed using real-time vehicle dynamic parameters (center of gravity sideslip angle, yaw rate, vehicle speed) and road condition information (coefficient of adhesion). An instability warning signal is generated when the vehicle's condition exceeds a calibrated threshold. The vehicle's "unstable state" is defined as the center of gravity sideslip angle... and the angular velocity of the center of mass deflection Exceeding 80% of the stability boundary or yaw rate Deviation exceeding the limit by 80%:

[0050] ,

[0051] The "steady state" of a vehicle is defined as the sideslip angle at the center of gravity. and the angular velocity of the center of mass deflection Within the 80% stability boundary and the yaw rate Compared with reference value The deviation is less than 80% of the limit:

[0052] ,

[0053] To avoid discontinuous trajectory planning due to frequent switching of the replanning system, an "over-stable state" (i.e., the vehicle is in a relatively stable state) is defined as follows: the sideslip angle of the center of gravity is lower than 30% of the stability boundary, and the yaw rate deviation is lower than 30% of the limit.

[0054] ,

[0055] in, This represents the slope value of the stable boundary at the centroid sideslip angle. This is the stability boundary intercept value for the centroid sideslip angle. This refers to the yaw rate instability deviation threshold. The above three thresholds can be calibrated relative to the adhesion coefficient. and longitudinal speed The relevant two-dimensional table.

[0056] This invention designs the access and exit logic of the trajectory replanning system based on three defined states:

[0057] (1) Intervention trigger: When the vehicle enters the "unstable state" from the "stable state", it indicates that the vehicle is at risk of instability and the trajectory replanning is initiated;

[0058] (2) Exit condition: When the vehicle returns from the "unstable state" to the "overstable state" and continues for more than 3 seconds, it means that the vehicle has completely returned to the "stable state" and exits the trajectory replanning;

[0059] (3) State maintenance: If the vehicle enters an "overly stable state" but the duration is less than 3 seconds, the replanning intervention will continue, and the state duration will need to be re-accumulated. This hysteresis control strategy effectively avoids frequent system switching caused by critical fluctuations in operating conditions through time window filtering.

[0060] The above vehicle instability prediction methods can effectively determine the instability of vehicle dynamics in advance, initiate vehicle trajectory replanning for intervention, and maintain continuous system intervention until the vehicle is completely stabilized.

[0061] Secondly, after determining that there is a risk of instability, the system uses the look-ahead information from the path tracking module to generate a sequence of virtual obstacles. Specifically, it uses a linear model to predict the future output of the controller. Step position prediction sequence (usually) To predict potential instability points in the trajectory. Deployed at equal intervals along the direction of vehicle movement, with the center coordinates as the base. One virtual obstacle (typical value) =10). The positions of the obstacles satisfy the spatial mapping relationship:

[0062] ,

[0063] in The distance between obstacles. This refers to the vehicle's heading angle.

[0064] A nonlinear model predictive controller transforms the vehicle's dynamic stability boundary into spatial obstacle avoidance constraints and designs an adaptive weight adjustment strategy: when the vehicle approaches the instability zone, it enters a "collision-priority mode," increasing the obstacle avoidance weight to 100 and reducing the path tracking weight to 0.5; when the vehicle passes the instability point, it switches to a "high-precision path tracking mode," increasing the tracking weight to 200 to improve trajectory tracking accuracy. This mechanism ensures that the vehicle reduces tracking requirements in the initial stage of emergency obstacle avoidance to extend the cornering time, while strengthening tracking accuracy after leaving the critical zone. The overall logic is as follows: Figure 3 The flowchart is shown.

[0065] Specifically, an optimization problem under a nonlinear model predictive control framework was designed in the trajectory replanning stage, namely minimizing the prediction time domain. Minimize path tracking error in the control time domain The control input sequence within, and the function for minimizing obstacle cost. The combination of the three By solving this optimization problem, the ideal control input can be obtained. The target trajectory point sequence is obtained by substituting these values ​​into the state-space equations. Similar to path tracking control, solving this optimization problem also requires imposing reasonable constraints on the front wheel steering angle, the vehicle's peak yaw rate, and the road boundaries to prevent the vehicle steering system from exceeding physical limits, the tire lateral force from exceeding the road adhesion limit, and the vehicle's motion from exceeding the actual road boundary limits. In summary, the design optimization problem is as follows:

[0066] ,

[0067] Among them, the obstacle cost function In the prediction time domain of the replanning module It is an inverse proportional function of the square of the Euclidean distance from the vehicle's center of gravity to the obstacle. This indicates the vehicle's current position in the global coordinate system. These are the coordinates of the obstacle. Clearly, the cost function will increase as the vehicle approaches the obstacle. These are the weights of the obstacle cost function. . It is to prevent The correction term whose denominator is 0. and These are the path tracking error weights and the control input weights, respectively. and Indicates the lower and upper bounds of the control input. Represents the reference trajectory sequence. k Indicates the current moment. i This indicates the step size for the prediction.

[0068] Finally, the generated optimized path points are fitted into a smooth trajectory using a fifth-order polynomial, and a linear model predicts and controls the tracker to achieve high-precision tracking.

[0069] This invention uses a four-wheel, two-degree-of-freedom vehicle dynamics model that considers vertical load transfer to analyze and predict the nonlinear dynamic characteristics of the vehicle as accurately as possible, such as... Figure 2 As shown. Based on d'Alembert's principle, in the vehicle coordinate system, the dynamic equations for the vehicle's lateral motion along the y-axis and its yaw motion around the z-axis can be modeled as follows:

[0070] ,

[0071] in, and These are the vehicle's longitudinal speed and lateral speed, respectively. Let yaw rate be the vehicle's angular velocity. For vehicle weight, It is the yaw moment of inertia of the vehicle about the z-axis. and These are the distances from the vehicle's center of gravity to the front and rear axles, respectively. It is half the wheel track of the vehicle. These represent the lateral tire forces of the vehicle's left front wheel, right front wheel, left rear wheel, and right rear wheel, respectively.

[0072] Under extreme conditions, the lateral force on the tire is close to saturation. Therefore, the small-angle assumption of the slip angle is not used. The tire slip angle is calculated as follows:

[0073] ,

[0074] ,

[0075] ,

[0076] ,

[0077] The calculation of the tire's vertical force, considering static load distribution and lateral dynamic load transfer, is as follows:

[0078] ,

[0079] ,

[0080] ,

[0081] ,

[0082] in, For the height of the vehicle's center of gravity, and These are the vehicle's longitudinal acceleration and lateral acceleration, respectively. These are the vertical forces on the left front wheel, right front wheel, left rear wheel, and right rear wheel of the vehicle, respectively.

[0083] To accurately determine and predict vehicle stability, a suitable tire model needs to be selected to determine the lateral forces on the wheels. The Fiala tire model is a nonlinear, physics-based dynamic tire model that provides a solution for accurately fitting tire forces, and its calibration method is relatively simple. The Fiala tire model addresses lateral forces... The calculation method is as follows:

[0084] ,

[0085] in, Indicates tire lateral stiffness. This is the slip angle corresponding to the peak tire force. This indicates the coefficient of friction between the tire and the road surface. Figure 3 These represent the lateral forces of the tire in the Fiala tire model. With vertical load and road surface adhesion coefficient The relationship.

[0086] The vehicle model was built using Simulink, and the sideslip angle under different operating conditions was generated through simulation. angular velocity of the center of mass deflection Phase trajectory clusters are used to calibrate the stable boundary of the linear centroid side deflection angle:

[0087] ,

[0088] in, This represents the slope value of the stable boundary at the centroid sideslip angle. The centroid sideslip angle stable boundary intercept value can be calibrated as the adhesion coefficient. and longitudinal speed The relevant two-dimensional map. Figure 4 This represents the phase plane diagram consisting of the centroid sideslip angle and the centroid sideslip angular velocity trajectory cluster at 80 km / h and a road surface adhesion coefficient of 1. The trajectories within the two parallel lines will eventually converge to 0, which is the stable state of the centroid sideslip angle phase plane.

[0089] To assess the stability of a vehicle's yaw rate, a reference value for the yaw rate needs to be calculated. However, solving complex differential equations using a nonlinear tire model is insufficient for real-time requirements. The core of stability monitoring is detecting the vehicle's tendency to deviate from the linear stability region; therefore, the tire model is simplified to a linear model.

[0090] ,

[0091] Meanwhile, assuming that the tire slip angle is small within the linear stability region, the simplified tire slip angle is obtained as follows:

[0092] ,

[0093] ,

[0094] Therefore, the reference yaw rate value under stable conditions without yaw can be calculated using the above four-wheel two-degree-of-freedom vehicle dynamics model. for:

[0095] ,

[0096] in, ( A yaw rate less than 0 indicates an understeer coefficient. The above reference values ​​represent ideal values ​​for a vehicle in a stable state. If the actual yaw rate exceeds the reference value significantly, the vehicle is likely in an unstable state of "oversteer." Conversely, if the actual yaw rate is significantly lower than the reference value, the vehicle is in an extreme state of "understeer," which will prevent it from following the reference path. Therefore, the designed yaw rate stability region is:

[0097] ,

[0098] in, The yaw rate deviation threshold, which is related to vehicle speed and adhesion coefficient, was obtained through tests at different vehicle speeds and road surfaces.

[0099] The "unstable state" of a vehicle is defined as the sideslip angle at the center of gravity. and the angular velocity of the center of mass deflection Exceeding 80% of the stability boundary or yaw rate Deviation exceeding the limit by 80%:

[0100] ,

[0101] The "steady state" of a vehicle is defined as the sideslip angle at the center of gravity. and the angular velocity of the center of mass deflection Within the 80% stability boundary and the yaw rate Compared with reference value The deviation is less than 80% of the limit:

[0102] ,

[0103] To avoid discontinuous trajectory planning due to frequent switching of the replanning system, an "over-stable state" (i.e., the vehicle is in a relatively stable state) is defined as follows: the sideslip angle of the center of gravity is lower than 30% of the stability boundary, and the yaw rate deviation is lower than 30% of the limit.

[0104] .

[0105] This invention designs the access and exit logic of the trajectory replanning system based on three defined states:

[0106] (1) Intervention trigger: When the vehicle enters the "unstable state" from the "stable state", it indicates that the vehicle is at risk of instability and the trajectory replanning is initiated;

[0107] (2) Exit condition: When the vehicle returns from the "unstable state" to the "overstable state" and continues for more than 3 seconds, it means that the vehicle has completely returned to the "stable state" and exits the trajectory replanning;

[0108] (3) State maintenance: If the vehicle enters an "overly stable state" but the duration is less than 3 seconds, the replanning intervention will continue, and the state duration will need to be re-accumulated. This hysteresis control strategy effectively avoids frequent system switching caused by critical fluctuations in operating conditions through time window filtering.

[0109] The above vehicle instability prediction and judgment methods can effectively determine the instability of vehicle dynamics in advance, initiate vehicle trajectory replanning for intervention, and maintain continuous system intervention until the vehicle is completely stabilized.

[0110] The method proposed in this invention improves both vehicle motion stability and path tracking accuracy—two objectives often considered difficult to achieve simultaneously—by actively adjusting the target path. This embodiment employs a model predictive control framework based on linear bicycle dynamics for path replanning. The state equation for path replanning can be expressed as follows, where the subscript "pr" represents path replanning.

[0111] ,

[0112] in, , , , , , They are respectively:

[0113] , , ,

[0114] ,

[0115] ,

[0116] in, and These are the equivalent lateral stiffnesses of the front and rear axles in the linear interval, respectively, and are constants.

[0117] Rewrite the state equations in discrete state-space form:

[0118] ,

[0119] in, , ,in , which is the sampling time of the path replanning module.

[0120] An optimization problem under a nonlinear model predictive control framework was designed during the trajectory replanning stage, namely minimizing the prediction time domain. Minimize path tracking error in the control time domain The control input sequence within, and the function for minimizing obstacle cost. The combination of the three By solving this optimization problem, the ideal control input can be obtained. The target trajectory point sequence is obtained by substituting these values ​​into the state-space equations. Similar to path tracking control, solving this optimization problem also requires imposing reasonable constraints on the front wheel steering angle, the vehicle's peak yaw rate, and the road boundaries to prevent the vehicle steering system from exceeding physical limits, the tire lateral force from exceeding the road adhesion limit, and the vehicle's motion from exceeding the actual road boundary limits. In summary, the design optimization problem is as follows:

[0121] ,

[0122] in, and These are the path tracking error weights and control input weights, respectively. Obstacle cost function. As shown in the following equation, in the prediction time domain of the replanning module It is an inverse proportional function of the square of the Euclidean distance from the vehicle's center of gravity to the obstacle. This indicates the vehicle's current position in the global coordinate system. These are the coordinates of the obstacle. Clearly, the cost function will increase as the vehicle approaches the obstacle. These are the weights of the obstacle cost function. . It is to prevent The correction term whose denominator is 0.

[0123] ,

[0124] The effectiveness of path replanning depends on obstacle avoidance weights. Path tracking error weights and control input weights The ideal allocation strategy is to dynamically adjust the allocation between them. , and The size of the target path tracking accuracy and the vehicle's safe obstacle avoidance target are balanced under different working conditions.

[0125] This invention proposes a path replanning method that dynamically adjusts the weights of the cost function based on virtual obstacles. When the stability prediction system anticipates vehicle instability, the path replanning module intervenes first to determine whether the vehicle will deviate from or approach the reference path in the future. The path replanning system generates a sequence of virtual obstacles based on the prediction information from the path tracking module. In specific implementation, a linear model is used to predict the future output of the controller. Step position prediction sequence (usually) To predict potential instability points in the trajectory. Deployed at equal intervals along the direction of vehicle movement, with the center coordinates as the base. One virtual obstacle (typical value) =10). The positions of the obstacles satisfy the spatial mapping relationship:

[0126] ,

[0127] in The distance between obstacles. This refers to the vehicle's heading angle.

[0128] During trajectory replanning, an adaptive weighting strategy dynamically coordinates conflicts among multiple objectives. The system calculates the nearest index between the vehicle's position and the virtual obstacle cluster in real time. The system then switches control modes accordingly: when the vehicle approaches the instability zone, it adjusts the obstacle avoidance weight. The tracking accuracy is increased to 100, while the path tracking weight is reduced to 0.5. In this mode, the optimization objective is primarily to increase the distance between the vehicle and the instability zone, allowing for a temporary reduction in tracking accuracy to extend cornering time and improve stability margin.

[0129] When the vehicle passes the instability point, The system is reset to zero and the path tracking weight is increased to 200. This stage enhances tracking accuracy by forcing the vehicle to quickly return to the reference path through high weighting. The input weights are then controlled. RThe weights are always kept at 10 to suppress shifting mutations. For details on the specific weight adjustment strategy, please refer to [link to relevant documentation]. Figure 5 .

[0130] After solving the optimization problem, this invention substitutes the obtained control quantity sequence back into the state space equation of the system to obtain the final target trajectory point sequence.

[0131] This invention uses a fourth-order polynomial to further transform the obtained trajectory points into a stable, collision-free reference trajectory sequence. The reference yaw angle is... and reference lateral displacement Both are about longitudinal displacement The function. Among them and These are the fitting coefficients, which can be used to modify the reference trajectory sequence. Substituting into the state-space equations, we obtain:

[0132]

[0133] For the path tracing problem of this invention, the system's state vector Control input vector and system output vector It can be written in state-space form:

[0134] ,

[0135] in, , , .

[0136] Similarly, the system's reference state Reference control input and reference output It can be defined as:

[0137]

[0138] exist Performing a Taylor expansion at the point to linearize, ignoring higher-order terms, yields:

[0139] ,

[0140] in, , , respectively right and The Jacobian matrix. The difference between the values ​​before and after linearization yields the prediction error model:

[0141] ,

[0142] in, , , .

[0143] Discretizing the continuous prediction error model using the forward Euler method yields the following results:

[0144] ,

[0145] in, , , This indicates the sampling cycle of the path tracking controller; It is an identity matrix.

[0146] Furthermore, in order to control and constrain the control increment, the state-space equations are rewritten as follows:

[0147] ,

[0148] in, , , , .

[0149] In order to predict the system in the future The output within each time domain (the subscript "pt" represents path tracing, the same below) can be combined and rewritten as follows:

[0150] ,

[0151] in,

[0152] , ,

[0153] , .

[0154] Indicates the future System prediction output within each time domain Represents the future obtained from the solution. The control input of the system in the time domain.

[0155] Path tracking objective function This is represented as minimizing the predicted output. In the prediction time domain Internal tracking reference output The error, and minimizing the control time domain The objective function is a weighted sum of the vehicle control input increments and the objective function is ultimately transformed into a standard quadratic form to facilitate quadratic programming. The optimization problem of the quadratic form is as follows:

[0156] ,

[0157] in, .

[0158] In solving quadratic programming problems, the incremental control input of the system must also be considered. Constraints, system control input Constraints and system output Constraints. and These are the upper and lower limits of the control increment, i.e., the change in front wheel steering angle per unit sampling time. Restrictions. and This represents the saturation constraint of the steering system. and The aim is to apply hard constraints to the system output. k Indicates the current moment. i Indicates the step size of the prediction. The weight matrix represents the tracking error. This represents the weight matrix that controls the input. Represents the state vector. This represents the control increment for each step.

[0159] At every moment For the objective function After performing the quadratic programming solution, the path tracking controller will obtain the future... Control increment within a control time domain This allows us to determine the optimal control sequence. and the first control quantity It acts on the controlled system. At any given time, the path tracking controller will update the system state input, thereby achieving rolling optimization of the control variables.

[0160] The proposed autonomous vehicle trajectory replanning method will be simulated and verified to enhance vehicle stability and tracking accuracy under extreme conditions such as emergency obstacle avoidance. Finally, hardware-in-the-loop testing results will further verify the real-time performance and effectiveness of the proposed method in practical automotive-grade controllers.

[0161] Figure 6 These are test results from a typical scenario involving double lane changes and low-adhesion road surfaces. After identifying a vehicle instability trend, the system quickly plans a more stable vehicle trajectory, improving vehicle path tracking accuracy and effectively preventing the vehicle from entering the dynamic instability zone throughout the entire process. Hardware-in-the-loop testing further confirms this. Figure 7As shown, the framework exhibits extremely low computational load on embedded platforms, with a resource utilization rate of only 0.3%, meeting the real-time constraints of automotive-grade controllers. This invention provides autonomous driving systems with the ability to ensure vehicle stability and safety under extreme conditions, and is of great significance for improving safety redundancy in complex traffic environments.

[0162] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit it. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that modifications or equivalent substitutions can still be made to the specific implementation of the present invention. Any modifications or equivalent substitutions that do not depart from the spirit and scope of the present invention should be covered within the scope of protection of the claims of the present invention.

Claims

1. A method for replanning the trajectory of an autonomous vehicle, characterized in that, include: The real-time stability status of a vehicle is determined based on its dynamic parameters. The execution or exit of trajectory replanning is determined based on the real-time stability status. When performing trajectory replanning, the path tracking model predicts the future position of the controller to generate a virtual obstacle sequence, transforming the vehicle dynamics stability boundary into a spatial obstacle avoidance constraint. The optimization problem based on the path tracking model predictive controller framework is combined with an adaptive weight adjustment strategy. The path tracking error weight and obstacle avoidance weight are adjusted according to the relative position of the vehicle and the virtual obstacle, thereby switching the vehicle's path tracking mode or collision avoidance priority mode and obtaining the vehicle's control quantity sequence. The target trajectory point sequence is obtained based on the control quantity sequence, and the trajectory point sequence is converted into a reference trajectory sequence based on a fourth-order polynomial. The vehicle is driven according to the reference trajectory sequence, and the system input control quantity during the driving process is obtained based on the path tracking objective function. The driving process is controlled based on the system input control quantity. The process of generating a virtual obstacle sequence using the path tracking model to predict the future position of the controller, as described above, includes: Using the potential instability point in the future position prediction information of the path tracking model predictive controller as the center coordinate, several virtual obstacles are deployed at equal intervals along the vehicle's direction of travel; the positions of the virtual obstacles satisfy the following spatial mapping relationship: ， in, The distance between obstacles. For the vehicle's heading angle, The center coordinates of the potential instability point, ( , () represents the coordinates of the virtual obstacle. This represents the number of virtual obstacles.

2. The autonomous vehicle trajectory replanning method according to claim 1, characterized in that, The real-time stability states include unstable states, stable states, and overstable states. The conditions for determining the unstable state are: the sideslip angle and sideslip velocity of the center of mass exceed 80% of the stability boundary, or the yaw rate deviation exceeds 80% of the limit. The conditions for determining the stable state are: the sideslip angle and sideslip velocity of the center of mass are within 80% of the stability boundary and the deviation of the yaw velocity from the reference value is less than 80% of the limit. The criteria for determining the over-stable state are: the centroid sideslip angle is less than 30% of the stable boundary, and the yaw rate deviation is less than 30% of the limit.

3. The autonomous vehicle trajectory replanning method according to claim 1, characterized in that, The process of determining whether to execute or exit trajectory replanning based on the real-time stability status includes: When the vehicle transitions from a "stable state" to an "unstable state", trajectory replanning is initiated. When the vehicle returns from an "unstable state" to an "overstable state" and remains so for more than 3 seconds, it exits the trajectory replanning process. If the vehicle enters an "overstable state" but the duration is less than 3 seconds, trajectory replanning will continue, and the duration of the state will be recalculated.

4. The autonomous vehicle trajectory replanning method according to claim 1, characterized in that, The optimization problems of the path tracking model predictive controller framework include: minimizing the path tracking error in the prediction time domain, minimizing the control input sequence in the control time domain, and minimizing the obstacle cost function, as detailed below: ， in, Optimize objectives Represents the obstacle cost function. The prediction time domain of the trajectory replanning module The control time domain of the trajectory replanning module This indicates the vehicle's current position in the global coordinate system. These are the coordinates of the virtual obstacles. These are the weights of the obstacle cost function. and These are the path tracking error weights and the control input weights, respectively. and Indicates the lower and upper bounds of the control input. Represents the reference trajectory sequence. k Indicates the current moment. i This indicates the step size for the prediction.

5. The autonomous vehicle trajectory replanning method according to claim 1, characterized in that, The adaptive weight adjustment strategy includes: When the vehicle approaches the instability zone, the obstacle avoidance weight is increased to 100, while the path tracking weight is reduced to 0.5; when the vehicle passes the instability point, the obstacle avoidance weight is reset to zero and the path tracking weight is increased to 200; the control input weight is kept at 10.

6. The autonomous vehicle trajectory replanning method according to claim 1, characterized in that, The conversion method for the reference trajectory sequence is as follows: ， in, , For reference heading angle, and These are the fitting coefficients. For longitudinal displacement, This is the heading angle.

7. The autonomous vehicle trajectory replanning method according to claim 1, characterized in that, The method for calculating the path tracking objective function is as follows: ， in, This represents the path tracing objective function. Indicates the prediction time domain, Indicates control of the time domain, and These are the upper and lower limits for controlling the increment, respectively. and This indicates the saturation constraint of the steering system. and This indicates a hard constraint applied to the system output. This represents the optimal control sequence. This represents the sequence of control increments obtained by the path tracking controller. k Indicates the current moment. i Indicates the step size of the prediction. The weight matrix represents the tracking error. This represents the weight matrix that controls the input. Represents the state vector. This represents the control increment for each step.

Images