Automatic driving vehicle track re-planning method

Abstract

The invention relates to the technical field of automatic driving, and particularly provides an automatic driving vehicle track re-planning method, which comprises the following steps of: firstly, evaluating stability risk and defining three states of vehicle stability through real-time vehicle dynamic parameters, and designing intervention and exit criteria of track re-planning according to the three states; when the vehicle driving is evaluated to be in an unstable state, generating a virtual obstacle by using prediction information of a path tracking module, and converting a dynamic stability boundary into a spatial constraint; performing trajectory re-planning by adopting nonlinear model predictive control, and adjusting the weight between the path tracking precision and the obstacle avoidance demand according to the relative position of the virtual obstacle in combination with a dynamic weight adjustment mechanism; and finally, generating an optimized trajectory containing stability constraint, and realizing high-precision tracking through linear MPC. The method can effectively balance the path tracking precision and the vehicle stability under a complex working condition, and prevents the vehicle from entering a dynamic unstable region.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to a method for replanning the trajectory of an autonomous driving vehicle, for ensuring the stability and safety of the vehicle under extremely complex working conditions. Background Art

[0002] Autonomous driving technology, a key development direction in future transportation, has garnered widespread attention in both academia and industry in recent years. Its core focus lies in enabling autonomous vehicle navigation and driving through advanced sensor technology, control algorithms, and decision-making systems. The functional implementation of modern autonomous driving systems relies heavily on cross-domain collaboration between the autonomous driving domain controller (DCU) and the vehicle domain controller (VDC). The DCU, as the system's "decision-making hub," generates collision-free planned paths based on environmental perception and global decision-making. The VDC, as the "execution hub," must achieve high-precision tracking of these paths through chassis control. However, the functional separation between the planning and execution layers in existing technology architectures makes it difficult to integrate vehicle dynamics into the trajectory generation process. Traditional path planning methods, primarily based on kinematic models or simplified dynamic assumptions, focus on calculating safe trajectories for static obstacle avoidance, while ignoring the impact of nonlinear vehicle dynamics, 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 roads and high-speed lane changes. The reference trajectory output by the planning layer may exceed the vehicle's actual dynamic capabilities, preventing the VDC from effectively tracking the path and even causing the vehicle to enter dangerous states such as yaw instability or sideslip.

[0003] As autonomous driving applications extend to complex conditions such as highways and slippery roads, vehicle dynamic stability has become a core constraint that cannot be ignored in path planning. The current mainstream planning architecture adopts a hierarchical 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 functions of such methods typically only consider the vehicle's physical dimensions, kinematic constraints, or road adhesion limits, and fail to incorporate dynamic stability boundaries into the optimization objective. This can cause the planned trajectory to approach or exceed the vehicle's dynamic stability region under extreme conditions, forcing the vehicle's domain controller to frequently trigger the electronic stability control system to intervene and correct during path tracking. This not only reduces ride smoothness, but also has a lag in these corrections, failing to avoid the risk of instability at the source and potentially exacerbating the risk of instability due to control conflicts.

[0004] The industry has attempted to overcome these limitations through various architectures. While integrated frameworks attempt to integrate stability control and path tracking, their customized designs struggle to adapt to the modular requirements of mass-produced systems. Furthermore, end-to-end planning approaches simulate human driving behavior by directly mapping sensor signals to control commands, but their black-box nature results in a lack of interpretability and reliability in safety-critical scenarios. None of these approaches establish a real-time, dynamic interaction between the planning and execution layers, rendering autonomous driving only reactive to instability and unable to proactively mitigate dynamic risks during trajectory generation.

[0005] A deeper conflict stems from information barriers between cross-domain controllers. The planning module of the autonomous driving domain controller lacks the ability to perceive the vehicle's real-time dynamic state (such as yaw rate and tire slip angle), while the execution layer of the vehicle domain controller is unable to feed stability boundaries back to the planning layer for closed-loop optimization. This one-way data flow creates 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. Existing technical architectures make it difficult to achieve a dynamic trade-off between the two, becoming a key bottleneck hindering the full implementation of high-level autonomous driving.

[0006] In this context, there is an urgent need for a new control architecture that can deeply integrate vehicle dynamic characteristics and path planning. The ideal solution needs to break through the information barriers of the traditional hierarchical architecture and achieve closed-loop optimization of planning and execution through cross-domain collaboration. Specifically, the system should have the ability to determine vehicle dynamic instability, consider vehicle dynamic stability in the trajectory generation stage, and adaptively adjust the control target priority based on real-time working conditions. This type of design not only needs to solve the complexity of nonlinear dynamic modeling, but also needs to achieve efficient computing on the embedded hardware platform to meet the stringent real-time and robustness requirements of mass production systems. Current technology has not yet effectively overcome the above difficulties, and the present invention is proposed to fill this key gap. Summary of the Invention

[0007] In order to solve the problem that the autonomous driving domain controller cannot plan a trajectory that satisfies the vehicle's dynamic stability under extreme working conditions, and to achieve an ideal balance between tracking accuracy and vehicle stability. The present invention proposes a path replanning method in the context of cross-domain controller collaboration. First, the present invention proposes a method for determining the instability of an autonomous driving vehicle, which determines whether to enable trajectory replanning by whether the current state of the vehicle exceeds the stability threshold, and designs a reasonable intervention and exit mechanism to ensure safe and timely intervention while avoiding frequent intervention. Secondly, the present invention reconstructs the path replanning framework in the context of cross-domain integration of the autonomous driving domain controller and the vehicle domain controller. The concept of virtual obstacles is proposed, the obstacle avoidance cost function is modified to guide the vehicle to avoid the unstable area of ​​dynamics, and the path tracking model prediction controller is used to provide the position information of the virtual obstacles. Finally, the present 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, the present invention proposes a method for replanning the trajectory of an autonomous driving vehicle, comprising: Determine the real-time stability state of the vehicle based on vehicle dynamic parameters; determining execution or exit of trajectory replanning according to the real-time stability status; When performing trajectory replanning, the future position prediction information of the path tracking model predictive controller is used to generate a virtual obstacle sequence, converting the vehicle's dynamic stability boundary 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 obstacle, thereby switching the vehicle's path tracking mode or collision avoidance priority mode, and obtaining the vehicle's control variable sequence. Acquire a target trajectory point sequence based on the control amount sequence, and convert the trajectory point sequence into a reference trajectory sequence based on a quartic polynomial; The vehicle is caused to travel according to the reference trajectory sequence, a system input control amount during the travel is obtained based on a path tracking objective function, and the travel process is controlled based on the system input control amount.

[0009] Furthermore, the real-time stability state includes an unstable state, a stable state and an over-stable state; The unstable state is determined as follows: the center of mass slip angle and the center of mass slip angular velocity exceed 80% of the stability boundary or the yaw rate deviation exceeds 80% of the limit; The determination conditions for the stable state are: the sideslip angle and the sideslip velocity of the center of mass are within the 80% stability boundary and the deviation of the yaw rate from the reference value is less than 80% of the limit; The judgment conditions for the over-stable state are: the sideslip angle of the center of mass is less than 30% of the stability boundary, and the yaw rate deviation is less than 30% of the limit.

[0010] Furthermore, the process of determining whether to execute or exit trajectory replanning according to the real-time stability state includes: When the vehicle enters an "unstable state" from a "stable state", trajectory replanning is initiated; When the vehicle returns from an unstable state to an overstable state and the state lasts for more than 3 seconds, the trajectory replanning is exited; If the vehicle enters the "overstable state" but lasts for less than 3 seconds, trajectory replanning will continue and the state duration will be counted again.

[0011] Furthermore, the process of generating a virtual obstacle sequence using the future position prediction information of the controller predicted by the path tracking model includes: The potential instability point in the future position prediction information of the path tracking model prediction controller is used as the central coordinate, and several virtual obstacles are deployed at equal intervals along the vehicle's forward direction. The positions of the virtual obstacles satisfy the following spatial mapping relationship: , in is the obstacle distance, is the vehicle heading angle, is the center coordinate of the potential instability point, ( , ) is the coordinate of the virtual obstacle, is the number of virtual obstacles.

[0012] Furthermore, the optimization problem of the path tracking model predictive controller framework includes minimizing the path tracking error in the prediction domain, minimizing the control input sequence in the control domain, and minimizing the obstacle cost function, as shown below: , in, represents the optimization goal, represents the obstacle cost function, represents the prediction time domain of the trajectory replanning module, represents the control time domain of the trajectory replanning module, Indicates the current position of the vehicle in the global coordinate system, are the coordinates of the virtual obstacle, is the weight of the obstacle cost function, and are the path tracking error weight and control input weight respectively, and represents the lower and upper bounds of the control input, represents the reference trajectory sequence, k Indicates the current moment, i Indicates the prediction step size.

[0013] Furthermore, the adaptive weight adjustment strategy includes: When the vehicle approaches the instability zone, the obstacle avoidance weight is increased to 100, and the path tracking weight is reduced to 0.5; when the vehicle passes the instability point, the obstacle avoidance weight is returned to zero and the path tracking weight is increased to 200; the control input weight is maintained at 10.

[0014] Furthermore, the conversion method of the reference trajectory sequence is as follows: , in, is the reference lateral displacement, is the reference heading angle, and is the fitting coefficient, is the longitudinal displacement, is the heading angle.

[0015] Furthermore, the calculation method of the path tracking objective function is as follows: , in, represents the path tracking objective function, represents the prediction time domain, represents the control time domain, and are the upper and lower limits of the control increment, and represents the saturation constraint of the steering system, and represents a hard constraint enforced on the system output, represents the optimal control sequence, represents the control increment sequence obtained by the path tracking controller, k Indicates the current moment, i represents the prediction step size, represents the weight matrix of tracking error, represents the weight matrix of the control input, represents the state vector, Represents the control increment for each step.

[0016] Compared with the prior art, the present invention has the following beneficial effects: The present invention defines three states of vehicle dynamic stability and designs intervention and exit criteria for trajectory replanning. The present invention dynamically generates virtual obstacles by obtaining predicted information of the vehicle position from the trajectory tracking controller, converts the dynamic boundary into an obstacle avoidance constraint for path planning, and guides the vehicle to avoid areas that may cause dynamic instability. The present invention constructs a trajectory replanning optimization problem based on a nonlinear model predictive control framework, takes path tracking error, control input, and virtual obstacle distance cost as multi-objective functions, generates a smooth executable trajectory under the physical limits of the vehicle actuator and the road boundary constraints, and finally achieves high-precision tracking through linear model predictive control. The present invention effectively alleviates the risk of vehicle dynamic instability caused by the separation of the planning layer and the execution layer, and improves the safety of the autonomous driving system in extreme scenarios such as high-speed emergency obstacle avoidance and low road adhesion conditions. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Various other advantages and benefits will become apparent to those skilled in the art upon reading the detailed description of the preferred embodiment below. The accompanying drawings are for illustration purposes only and are not to be considered as limiting the present invention. The same reference symbols are used throughout the drawings to represent the same components. In the drawings: Figure 1 Block diagram of the autonomous vehicle trajectory replanning method provided by the present invention; Figure 2 Schematic diagram of a four-wheel two-degree-of-freedom vehicle dynamics model according to an embodiment of the present invention; Figure 3 is a relationship diagram of tire lateral force, vertical load, and road adhesion coefficient in a tire model according to an embodiment of the present invention; Figure 4 A phase plane trajectory of the vehicle's center of mass sideslip angle and a stability boundary diagram according to an embodiment of the present invention; Figure 5 A logic flow chart of a variable weight mechanism provided according to the present invention; Figure 6 Comparison of simulation test results in a typical scenario of dual lane changes and low-adhesion roads according to an embodiment of the present invention; (a) comparison of path tracking results, (b) comparison of steering wheel angle control input, (c) comparison of oversteer instability indicators, and (d) comparison of vehicle position and heading angle. Figure 7 Comparison of system calculation load rates in a hardware-in-the-loop test according to an embodiment of the present invention; (a) is the load rate of a periodic task of 10 ms, and (b) is the load rate of a periodic task of 200 s. DETAILED DESCRIPTION

[0018] The exemplary embodiments of the present disclosure will be described in more detail below with reference to the accompanying drawings. Although exemplary embodiments of the present disclosure are shown in the accompanying drawings, it should be understood that the present disclosure can be implemented in various forms and should not be limited by the embodiments set forth herein. On the contrary, these embodiments are provided to enable a more thorough understanding of the present disclosure and to fully convey the scope of the present disclosure to those skilled in the art. It should be noted that, unless there is a conflict, the embodiments of the present disclosure and the features described in the embodiments can be combined with each other. The present invention will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

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

[0020] First, the stability risk is assessed by real-time vehicle dynamic parameters (center of mass slip angle, yaw rate, vehicle speed) and road condition information (adhesion coefficient), and an instability warning signal is generated when the vehicle state exceeds the calibrated threshold. The "unstable state" of the vehicle is defined as the center of mass slip angle and the sideslip angular velocity of the center of mass 80% of the stability limit or yaw rate Deviation exceeds 80% of the limit: , The vehicle's "stable state" is defined as the sideslip angle of the center of mass and the sideslip angular velocity of the center of mass Within the 80% stability boundary and the yaw rate With reference value The deviation is less than 80% of the limit: , To avoid discontinuous trajectory planning caused by frequent switching of the replanning system, an "overstable state" (i.e., the vehicle is in a relatively stable state) is defined as when the sideslip angle of the center of mass is less than 30% of the stability boundary and the yaw rate deviation is less than 30% of the limit: , in, is the slope value of the center of mass sideslip angle stability boundary, is the center of mass sideslip angle stability boundary intercept value, is the yaw rate instability deviation threshold. The above three thresholds can be calibrated to be related to the adhesion coefficient and longitudinal speed Related two-dimensional tables.

[0021] The present invention designs the trajectory replanning system entry and exit logic based on the three defined states: (1) Intervention trigger: When the vehicle enters an unstable state from a stable state, it indicates that the vehicle is at risk of instability and trajectory replanning is initiated; (2) Exit condition: When the vehicle returns from an "unstable state" to an "overstable state" and this state lasts for more than 3 seconds, it indicates that the vehicle has completely returned to a "stable state" and exits trajectory replanning; (3) State retention: If the vehicle enters an "overstable state" but the duration is less than 3 seconds, replanning intervention will continue, and the state duration will be re-accumulated. This hysteresis control strategy effectively avoids frequent system switching caused by critical fluctuations in operating conditions through time window filtering.

[0022] The above vehicle instability prediction method 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.

[0023] Secondly, after determining that there is a risk of instability, the system uses the forward-looking information of the path tracking module to generate a sequence of virtual obstacles. Specifically: the linear model is used to predict the future output of the controller. Step position prediction sequence (usually ), to predict potential unstable points in the trajectory As the center coordinate, it is deployed at equal intervals along the vehicle's forward direction. Virtual obstacles (typical =10). The obstacle position satisfies the spatial mapping relationship: , in is the obstacle distance, is the vehicle heading angle.

[0024] The vehicle dynamic stability boundary is converted into a spatial obstacle avoidance constraint through a nonlinear model predictive controller, and an adaptive weight adjustment strategy is designed: when the vehicle approaches the unstable area, it enters the "collision avoidance priority mode", increases the obstacle avoidance weight to 100, and reduces the path tracking weight to 0.5; when the vehicle passes the unstable point, it switches to the "high-precision path tracking mode", increases the tracking weight to 200 to improve the trajectory tracking accuracy. This mechanism ensures that the vehicle reduces the tracking requirements in the early stage of emergency obstacle avoidance to extend the cornering time, and strengthens the tracking accuracy after leaving the critical area. The overall logic is as follows: Figure 3 The flowchart is shown in FIG.

[0025] Specifically, in the trajectory replanning stage, an optimization problem is designed under the framework of nonlinear model predictive control, that is, minimizing the prediction time domain Minimize the path tracking error in the control time domain The control input sequence within, and the obstacle cost function minimized A combination of the three , by solving the optimization problem, we can get the ideal control input , and substitute it into the state-space equation to obtain the target trajectory point sequence. Similar to path-following control, solving this optimization problem also requires applying reasonable constraints on the front wheel angle, the vehicle's peak yaw rate, and the road boundaries to prevent the vehicle's steering system from exceeding physical limitations, the tire's lateral force from exceeding the road's adhesion limits, and the vehicle's motion from exceeding the actual road boundary limits. In summary, the design optimization problem is as follows: , Among them, the obstacle cost function In the prediction domain of the re-planning module It is an inverse proportional function of the square of the Euclidean distance from the vehicle's center of mass to the obstacle. Indicates the current position of the vehicle in the global coordinate system, are the obstacle coordinates. Obviously, when the vehicle gets closer to the obstacle, the cost function will increase. is the weight of the obstacle cost function, . , is to prevent The correction term with a denominator of 0. and are the path tracking error weight and control input weight respectively, and represents the lower and upper bounds of the control input, represents the reference trajectory sequence, k Indicates the current moment, i Indicates the prediction step size.

[0026] Finally, the generated optimized path points are fitted into a smooth trajectory through a quintic polynomial and tracked with high precision by a linear model predictive control tracker.

[0027] The present invention uses a four-wheel two-degree-of-freedom vehicle dynamics model that takes into account vertical load transfer to analyze and predict the nonlinear dynamic characteristics of the vehicle as accurately as possible, such as Figure 2 According to the D'Alembert principle, in the vehicle coordinate system, the dynamic equations of the vehicle's lateral motion along the y-direction and the yaw motion around the z-axis can be modeled as follows: , in, and are the longitudinal and lateral velocities of the vehicle, is the vehicle yaw angular velocity. For vehicle weight, is the vehicle's yaw moment of inertia around the z-axis, and are the distances from the vehicle's center of mass to the front and rear axles, respectively. Half the vehicle's wheelbase. Represent the lateral tire forces of the vehicle's left front wheel, right front wheel, left rear wheel, and right rear wheel respectively.

[0028] Under extreme conditions, the tire lateral force is close to saturation, so the small slip angle assumption is not used. The tire slip angle is calculated as follows: , , , , The tire vertical force considering the static load distribution and lateral dynamic load transfer is calculated as follows: , , , , in, is the height of the vehicle's center of mass, and are the vehicle's longitudinal acceleration and lateral acceleration, are the vertical forces on the left front wheel, right front wheel, left rear wheel and right rear wheel of the vehicle respectively.

[0029] In order to accurately judge and predict the vehicle stability state, it is necessary to select a suitable tire model to determine the wheel lateral force. The Fiala tire model is a nonlinear, physics-based dynamic tire model that provides a solution for accurate fitting of tire forces, and the calibration method is relatively simple. The calculation method is: , in, Indicates the tire cornering stiffness, is the sideslip angle corresponding to the peak tire force, Indicates the coefficient of friction between the tire and the road. Figure 3 Represents the tire lateral force in the Fiala tire model and vertical load and road adhesion coefficient relationship.

[0030] The above vehicle model was built using Simulink, and the center of mass side slip angle under different working conditions was generated through simulation. Side slip angular velocity with center of mass Phase trajectory cluster, based on which the linear centroid sideslip angle stability boundary is calibrated: , in, is the slope value of the center of mass sideslip angle stability boundary, is the center of mass sideslip angle stability boundary intercept value, which can be calibrated to the adhesion coefficient and longitudinal speed Related two-dimensional Map. Figure 4 The phase plane diagram represents the cluster of center of mass slip angle and center of mass slip velocity trajectories at 80 km / h and a road adhesion coefficient of 1. The trajectories within the two parallel lines will eventually converge to 0, which is the stable state of the center of mass slip angle phase plane.

[0031] To assess the stability of the 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 makes it difficult to meet real-time requirements. The core of stability monitoring is to detect the tendency of the vehicle to deviate from the linear stability region, so the tire model is simplified to a linear model: , At the same time, it is considered that the tire slip angle is small in the linear stable region, and the simplified tire slip angle is: , , Therefore, the reference yaw rate value in the stable state without yaw can be calculated by the above four-wheel two-degree-of-freedom vehicle dynamics model. for: , in, ( ) < 0 is the understeer coefficient. The above reference values ​​represent ideal values ​​for a stable vehicle. If the actual yaw rate exceeds the reference value by a large margin, the vehicle is likely in an unstable state of "oversteering." If the actual yaw rate falls far below the reference value, the vehicle is in an extreme "understeering" state, which can cause the vehicle to be unable to track the reference path. Therefore, the design yaw rate stability range is: , in, is the yaw rate deviation threshold related to vehicle speed and adhesion coefficient, and is obtained through calibration tests at different vehicle speeds and road surfaces.

[0032] The definition of "unstable state" of the vehicle is, the sideslip angle of the center of mass and the sideslip angular velocity of the center of mass 80% of the stability limit or yaw rate Deviation exceeds 80% of the limit: , The vehicle's "stable state" is defined as the sideslip angle of the center of mass and the sideslip angular velocity of the center of mass Within the 80% stability boundary and the yaw rate With reference value The deviation is less than 80% of the limit: , To avoid discontinuous trajectory planning caused by frequent switching of the replanning system, an "overstable state" (i.e., the vehicle is in a relatively stable state) is defined as when the sideslip angle of the center of mass is less than 30% of the stability boundary and the yaw rate deviation is less than 30% of the limit: .

[0033] The present invention designs the trajectory replanning system entry and exit logic based on the three defined states: (1) Intervention trigger: When the vehicle enters an unstable state from a stable state, it indicates that the vehicle is at risk of instability and trajectory replanning is initiated; (2) Exit condition: When the vehicle returns from an "unstable state" to an "overstable state" and this state lasts for more than 3 seconds, it indicates that the vehicle has completely returned to a "stable state" and exits trajectory replanning; (3) State retention: If the vehicle enters an "overstable state" but the duration is less than 3 seconds, replanning intervention will continue, and the state duration will be re-accumulated. This hysteresis control strategy effectively avoids frequent system switching caused by critical fluctuations in operating conditions through time window filtering.

[0034] The above vehicle instability prediction and judgment method 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.

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

[0036] , in, , , , , , They are: , , , ,

[0037] , in, and are the equivalent cornering stiffness of the front and rear axles in the linear range, respectively, and are constants.

[0038] Rewrite the state equation into discrete state space form: , in, , ,in , is the sampling time of the path replanning module.

[0039] In the trajectory replanning stage, an optimization problem is designed under the framework of nonlinear model predictive control, that is, minimizing the prediction time domain Minimize the path tracking error in the control time domain The control input sequence within, and the obstacle cost function minimized A combination of the three , by solving the optimization problem, we can get the ideal control input , and substitute it into the state-space equation to obtain the target trajectory point sequence. Similar to path-following control, solving this optimization problem also requires applying reasonable constraints on the front wheel angle, the vehicle's peak yaw rate, and the road boundaries to prevent the vehicle's steering system from exceeding physical limitations, the tire's lateral force from exceeding the road's adhesion limits, and the vehicle's motion from exceeding the actual road boundary limits. In summary, the design optimization problem is as follows: , in, and are the path tracking error weight and the control input weight respectively. Obstacle cost function As shown in the following formula, in the prediction domain of the re-planning module It is an inverse proportional function of the square of the Euclidean distance from the vehicle's center of mass to the obstacle. Indicates the current position of the vehicle in the global coordinate system, are the obstacle coordinates. Obviously, when the vehicle gets closer to the obstacle, the cost function will increase. is the weight of the obstacle cost function, . , is to prevent The correction term with a denominator of 0.

[0040] , The effect of path replanning depends on the obstacle avoidance weight , path tracking error weight and control input weights The ideal allocation strategy is to dynamically adjust , and The size of is used to achieve a trade-off between the path tracking accuracy target and the vehicle safety obstacle avoidance target under different working conditions.

[0041] The present invention proposes a path replanning method that can dynamically adjust the weight of the cost function according to virtual obstacles. When the stability prediction system predicts that the vehicle is unstable, the path replanning module will first intervene 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 of the path tracking module. In specific implementation, the linear model is used to predict the future output of the controller. Step position prediction sequence (usually ), to predict potential unstable points in the trajectory As the center coordinate, it is deployed at equal intervals along the vehicle's forward direction. Virtual obstacles (typical =10). The obstacle position satisfies the spatial mapping relationship: , in is the obstacle distance, is the vehicle heading angle.

[0042] During the trajectory replanning process, the system dynamically coordinates the conflicts among multiple objectives through adaptive weighting strategy. The system calculates the nearest index between the vehicle position and the virtual obstacle cluster in real time. , and switch the control mode accordingly: When the vehicle approaches the unstable area, the obstacle avoidance weight Increase it to 100, and reduce the path tracking weight to 0.5. In this mode, the optimization goal is to increase the distance between the vehicle and the unstable zone, allowing for a temporary reduction in tracking accuracy to extend the turn entry time and improve the stability margin.

[0043] When the vehicle passes the instability point, Reset to zero and increase the path tracking weight to 200. This stage strengthens the tracking accuracy and forces the vehicle to return to the reference path quickly through high weights. Control input weights R Always keep 10 to suppress sudden changes. For detailed weight adjustment strategy, please refer to Figure 5 .

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

[0045] The present invention uses a quartic polynomial to further convert the obtained trajectory points into a stable, collision-free reference trajectory sequence. The reference yaw angle is and reference lateral displacement It's all about longitudinal displacement The function of . and is the fitting coefficient, which can be used to Substituting into the state space equation, we can get:

[0046] For the path tracking problem of the present invention, the state vector of the system , control input vector and the system output vector It can be written in state space form: , in, , , .

[0047] Likewise, the reference state of the system , reference control input and reference output It can be defined as:

[0048] exist Performing Taylor expansion at the point to linearize and ignoring higher-order terms, we can obtain: , in, , , respectively right and The Jacobian matrix of . The prediction error model is obtained by subtracting the linearization before and after: , in, , , .

[0049] The continuous prediction error model is discretized using the forward Euler method, and we can get , in, , , represents the sampling period of the path tracking controller; is the identity matrix.

[0050] Furthermore, in order to control and constrain the control increment, the state space equation is rewritten as: , in, , , , .

[0051] To predict the future The output in the time domain (subscript "pt" stands for path tracking, the same below) can be combined and rewritten as , in, , , , .

[0052] Indicates the future The system prediction output in the time domain is Represents the future obtained by solution The control input of the system in the time domain.

[0053] Path tracking objective function Expressed as minimizing the predicted output In the prediction domain Internal tracking reference output The error, and minimize the control time domain The weighted sum of the vehicle control input increments within is calculated, and the objective function is finally converted into a standard quadratic form to facilitate quadratic programming. The quadratic optimization problem is: , in, .

[0054] The system control input increment must also be considered in the quadratic programming solution Constraints, system control input Constraints and system outputs constraints. and They are the upper and lower limits of the control increment, that is, the change in the front wheel angle per unit sampling time. restrictions. and represents the saturation constraint of the steering system, and Aims to enforce hard constraints on system outputs, k Indicates the current moment, i represents the prediction step size, represents the weight matrix of tracking error, represents the weight matrix of the control input, represents the state vector, Represents the control increment for each step.

[0055] At every moment For the objective function After executing the quadratic programming solution, the path tracking controller will obtain the future Control increments within the control time domain , and then the optimal control sequence can be obtained , and the first control quantity Act on the controlled system. At this moment, the path tracking controller will update the system state input, thereby achieving a rolling optimization solution for the control quantity.

[0056] The following simulations will verify the proposed trajectory replanning method for autonomous vehicles, demonstrating its improved vehicle stability and tracking accuracy under extreme conditions such as emergency obstacle avoidance. Finally, hardware-in-the-loop testing will further validate the real-time and effectiveness of the proposed trajectory replanning method in an actual automotive-grade controller.

[0057] Figure 6 The test results are for a typical scenario of double lane changes and low-adhesion roads. After the system identifies the trend of vehicle instability, it quickly plans a more stable vehicle trajectory, improves the vehicle path tracking accuracy, and effectively avoids the vehicle from entering the dynamic instability area throughout the process. Hardware-in-the-loop testing further confirms that, Figure 7 As shown, the framework's actual computational load on the embedded platform is extremely low, 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 maintain vehicle stability and safety under extreme operating conditions, which is of great significance for improving safety redundancy in complex traffic environments.

[0058] 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, ordinary technicians in the field should understand that the specific implementation methods of the present invention can still be modified or replaced by equivalents. Any modification or equivalent replacement that does not depart from the spirit and scope of the present invention should be covered by the scope of protection of the claims of the present invention.

Claims

1. A method for replanning a trajectory of an autonomous driving vehicle, characterized in that: include: Determine the real-time stability state of the vehicle based on vehicle dynamic parameters; determining execution or exit of trajectory replanning according to the real-time stability status; When performing trajectory replanning, the future position prediction information of the path tracking model predictive controller is used to generate a virtual obstacle sequence, converting the vehicle dynamic stability boundary into a spatial obstacle avoidance constraint; Based on the optimization problem of the path tracking model predictive controller framework, combined with the 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 variable sequence; Acquire a target trajectory point sequence based on the control amount sequence, and convert the trajectory point sequence into a reference trajectory sequence based on a quartic polynomial; The vehicle is caused to travel according to the reference trajectory sequence, a system input control amount during the travel is obtained based on a path tracking objective function, and the travel process is controlled based on the system input control amount.

2. The autonomous driving vehicle trajectory replanning method according to claim 1, characterized in that: The real-time stability state includes an unstable state, a stable state and an over-stable state; The unstable state is determined as follows: the center of mass slip angle and the center of mass slip angular velocity exceed 80% of the stability boundary or the yaw rate deviation exceeds 80% of the limit; The determination conditions for the stable state are: the sideslip angle and the sideslip velocity of the center of mass are within the 80% stability boundary and the deviation of the yaw rate from the reference value is less than 80% of the limit; The judgment conditions for the over-stable state are: the sideslip angle of the center of mass is less than 30% of the stability boundary, and the yaw rate deviation is less than 30% of the limit.

3. The autonomous driving vehicle trajectory replanning method according to claim 1, characterized in that: The process of determining execution or exit of trajectory replanning according to the real-time stability state includes: When the vehicle enters an "unstable state" from a "stable state", trajectory replanning is initiated; When the vehicle returns from an unstable state to an overstable state and remains in this state for more than 3 seconds, the trajectory replanning is exited. If the vehicle enters an "overstable state" but lasts for less than 3 seconds, trajectory replanning will continue and the state duration will be counted again.

4. The autonomous driving vehicle trajectory replanning method according to claim 1, characterized in that: The process of generating a virtual obstacle sequence using the future position prediction information of the path tracking model predictive controller includes: The potential instability point in the future position prediction information of the path tracking model prediction controller is used as the central coordinate, and several virtual obstacles are deployed at equal intervals along the vehicle's forward direction. The positions of the virtual obstacles satisfy the following spatial mapping relationship: ， in, is the obstacle distance, is the vehicle heading angle, is the center coordinate of the potential instability point, ( , ) is the coordinate of the virtual obstacle, is the number of virtual obstacles.

5. The autonomous driving vehicle trajectory replanning method according to claim 1, characterized in that: The optimization problem of the path tracking model predictive controller framework includes minimizing the path tracking error in the prediction domain, minimizing the control input sequence in the control domain, and minimizing the obstacle cost function, as shown below: ， in, represents the optimization goal, represents the obstacle cost function, represents the prediction time domain of the trajectory replanning module, represents the control time domain of the trajectory replanning module, Indicates the current position of the vehicle in the global coordinate system, are the coordinates of the virtual obstacle, is the weight of the obstacle cost function, and are the path tracking error weight and control input weight respectively, and represents the lower and upper bounds of the control input, represents the reference trajectory sequence, k Indicates the current moment, i Indicates the prediction step size.

6. The autonomous driving 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, and the path tracking weight is reduced to 0.5; when the vehicle passes the instability point, the obstacle avoidance weight is returned to zero and the path tracking weight is increased to 200; the control input weight is maintained at 10.

7. The autonomous driving vehicle trajectory replanning method according to claim 1, characterized in that: The conversion method of the reference trajectory sequence is as follows: ， in, is the reference lateral displacement, is the reference heading angle, and is the fitting coefficient, is the longitudinal displacement, is the heading angle.

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

Images