Intelligent vehicle secondary trajectory planning method based on vehicle motion capability boundary prediction

Abstract

The application discloses an intelligent vehicle secondary trajectory planning method based on vehicle motion ability boundary prediction and belongs to the technical field of intelligent vehicle motion planning and trajectory tracking control. The method comprises the following steps: S1. collecting a reference trajectory, a vehicle state, chassis feedback, road surface constraints and completing time synchronization of multi-source data; S2. fusing an information domain target and a physical domain vehicle state, calculating a tracking deviation, correcting a vehicle effective motion ability boundary in combination with road surface adhesion and actuator availability, and evaluating trajectory feasibility; and S3. predicting a time domain reachable region based on vehicle dynamics, comparing and identifying continuous infeasible trajectory segments, triggering local correction in a low-dimensional trajectory space, generating a continuous executable trajectory, and directly outputting the original trajectory if feasible. The application fuses multi-dimensional real working condition to judge a trajectory, pre-judges an infeasible section through a reachable set, only makes local correction, guarantees trajectory continuity, avoids controller saturation oscillation, and is suitable for automatic driving scenes such as low adhesion road surfaces and actuator attenuation.

Description

Technical Field

[0001] This invention belongs to the field of intelligent vehicle motion planning and trajectory tracking control technology, specifically relating to an intelligent vehicle secondary trajectory planning method based on vehicle motion capability boundary prediction. Background Technology

[0002] With the development of intelligent vehicles, unmanned transport vehicles, and drive-by-wire chassis technology, the vehicle motion planning module and the trajectory tracking control module need to periodically exchange target reference trajectories, vehicle motion states, and chassis execution feedback. In application scenarios involving partial autonomous driving, relative trajectory tracking, and the lack of a fixed global coordinate reference, the target reference trajectory is usually established in a non-inertial coordinate system that continuously translates and rotates with the vehicle's movement. Even if the upstream module has completed trajectory synchronization, reconstruction, compression, or dimensionality reduction, the target reference trajectory input to the planning control system will still be continuously updated as the vehicle's position, attitude, and local reference frame change. Due to potential coordinate reference transformations, trajectory node resampling, local trajectory replacements, and trajectory progress deviations between adjacent planning cycles, the reference position, heading, curvature, and velocity states are prone to instantaneous changes, resulting in reference reference jumps that are not caused by actual vehicle motion deviations.

[0003] Existing trajectory tracking prediction methods typically focus on predicting active steering, adapting in advance to different adhesion conditions ahead, and predicting motion range. For example, Falcone et al., in "Predictive Active Steering Control for Autonomous Vehicle Systems," proposed an active front-wheel steering method based on model predictive control, using a vehicle model to predict future states and optimize steering inputs. However, its basic goal remains tracking a pre-given reference trajectory without determining whether the trajectory exceeds the vehicle's current and future achievable range before tracking. Svensson et al., in "Traction Adaptive Motion Planning at the Limits of Handling," proposed a traction adaptive motion planning method based on predicted adhesion coefficients and time-varying tire force constraints. This method adapts the planning results to different adhesion conditions ahead. Experiments show that only by predicting future changes in vehicle capabilities in advance can trajectory infeasibility and constraint saturation be avoided after the vehicle enters a low-adhesion zone. Vaskov et al., in "Guaranteed Safe Reachability-based Trajectory Design for a High-Fidelity Model of an Autonomous Passenger Vehicle," proposed a safe trajectory design method based on forward realizable state sets, eliminating potentially dangerous candidate trajectories before execution. Manzinger et al., in "Using ReachableSets for Trajectory Planning of Automated Vehicles," proposed a travel corridor planning method combining reachable sets and convex optimization, using state sets to form feasible travel corridors, demonstrating that predicting the future achievable range of motion is a crucial condition for identifying infeasible trajectory segments in advance. However, these methods primarily focus on safety constraints, attachment adaptation, or travel corridor generation, and have not yet continuously determined infeasible segments and subsequent reconnectable locations for existing reference trajectories. Therefore, it is difficult to simultaneously maintain the continuity of the original trajectory structure, vehicle execution capabilities, and the secondary planned trajectory.

[0004] Therefore, it is necessary to set up a secondary motion planning stage oriented towards the trajectory layer before the target reference trajectory enters the tracking controller. This stage synchronously correlates the dynamic target reference trajectory in the information domain with the actual vehicle state, chassis response, road conditions, and actuator availability in the physical domain, forming a capability boundary that reflects the vehicle's current actual operating conditions. Based on this, the reachable area of ​​the vehicle within a finite time domain is predicted, determining whether the target trajectory node is within the vehicle's achievable range, identifying continuous infeasible trajectory segments, and searching backward from subsequent feasible trajectories for re-entry positions that meet the continuous reachability conditions. Compared to traditional methods that only filter the reference trajectory or rely on internal compensation within the controller, this method can discover and correct infeasible targets before control execution, performing only local secondary planning for infeasible segments. While preserving the effective segments and motion trends of the original trajectory as much as possible, it ensures the continuity of the corrected trajectory at boundaries such as position, heading, and curvature, and continuously updates the capability boundary and planning trigger conditions based on the vehicle's actual response. This provides the trajectory tracking controller with a continuous, stable, and executable reference trajectory that conforms to the vehicle's current motion capabilities. Summary of the Invention

[0005] In view of this, the purpose of this invention is to provide an intelligent vehicle secondary trajectory planning method based on vehicle motion capability boundary prediction, which aims to solve the technical problems of easy reference trajectory jumps and mismatch between trajectory requirements and vehicle real-time motion limits in existing autonomous driving systems.

[0006] To achieve the above objectives, the present invention adopts the following technical solution:

[0007] A method for intelligent vehicle quadratic trajectory planning based on vehicle motion capability boundary prediction includes the following steps:

[0008] S1. Obtain the low-dimensional flat trajectory and reconstruction information output by the dynamic trajectory synchronization module, and collect vehicle motion status, chassis feedback, operating conditions and constraint information;

[0009] S2. Based on the cyber-physical fusion mechanism, the dynamic target reference trajectory is synchronously associated with the actual motion state of the vehicle, chassis response, and operating conditions to construct the trajectory tracking error, execution residual, and vehicle motion capability boundary, and to evaluate the continuity and dynamic feasibility of the current target reference trajectory.

[0010] S3. When the target reference trajectory has a reference jump or does not meet the current motion capability of the vehicle, a secondary motion planning problem is constructed in the low-dimensional flat trajectory space. Combining the trajectory boundary inheritance characteristics, vehicle reachability, actuator constraints and driving safety constraints, the original target reference trajectory is locally modified to generate a continuous and executable target trajectory.

[0011] Furthermore, step S1 includes the following sub-steps:

[0012] S1.1 Obtain the low-dimensional flat original target reference trajectory output by the dynamic trajectory synchronization module;

[0013] ;

[0014]

[0015] In the formula, For the timestamp of the trajectory nodes; For trajectory progress; For reference position; For reference heading; For reference speed; For reference curvature; This represents the number of trajectory nodes.

[0016] S1.2 Collects the real-time motion state of the vehicle, and the vehicle motion state vector is denoted as:

[0017]

[0018] In the formula, Let k be the real-time motion state vector of the vehicle at time k. This is the timestamp of the k-th sampling time; For vehicle location; For heading angle; Longitudinal velocity; This refers to the yaw rate; and These are longitudinal and lateral accelerations, respectively.

[0019] S1.3 Establish chassis response and actuator status input, wherein the chassis response and actuator availability are denoted as:

[0020]

[0021] In the formula, This is the actual execution response vector of the chassis at the k-th sampling time; and These represent the actual steering response, driving response, and braking response at time k, respectively. Let k be the diagonal matrix of executor availability. , , These respectively characterize the availability of the actuators corresponding to steering, driving, and braking; This indicates the construction of a diagonal matrix;

[0022] S1.4 Establish road surface conditions and vehicle constraint inputs;

[0023] Road surface conditions based on adhesion level Characterization;

[0024] The nominal constraint set of vehicles is denoted as:

[0025]

[0026] In the formula, L is the vehicle wheelbase; and These are the maximum steering angle and the maximum steering change, respectively. and These are the maximum driving acceleration and the maximum braking deceleration, respectively. This is the maximum permissible lateral acceleration; Maximum permissible curvature;

[0027] S1.5 performs time synchronization for multiple input sources;

[0028] Based on the current second planning time To unify the time reference, the target reference trajectory, vehicle status, chassis response, road conditions and actuator status are queried or interpolated so that all types of inputs enter the subsequent processing at the same time.

[0029] Among them, the heading angle is interpolated using the shortest angle difference; the low-frequency state is interpolated using adjacent time intervals, with adjacent sampling times... and The state between The time synchronization relationship is as follows:

[0030] ;

[0031]

[0032] In the formula, This is the current second planning phase; , These are the adjacent original sampling times; , They are respectively , The original state at any given moment; To align to The interpolation state at time step; These are the time interpolation weighting coefficients.

[0033] Furthermore, step S2 includes the following sub-steps:

[0034] S2.1 Construct the target state in the information domain, and characterize the change in the reference baseline by the difference in the target state of adjacent cycles at the same trajectory progress:

[0035]

[0036]

[0037] In the formula, The information domain target state vector of the i-th trajectory progress node in the k-th planning period; The target state of the previous cycle is mapped to the target state of the same trajectory progress i under the current planning coordinate system after coordinate transformation and trajectory progress correspondence. The difference in target state at the same trajectory progress node in adjacent planning periods represents the change in the reference trajectory baseline.

[0038] S2.2 Constructs the physical domain vehicle state, which consists of the vehicle's actual motion state and chassis response, denoted as:

[0039]

[0040] S2.3 Project the vehicle's actual position onto the current target reference trajectory, and calculate the state deviation between the information domain and the physical domain at the projected position:

[0041] ;

[0042] ;

[0043]

[0044] In the formula, The arc length progress variable is the reference trajectory; The progress of the arc length corresponding to the optimal projection of the actual position of the vehicle on the target reference trajectory in the kth planning period; This represents the vehicle's current actual position vector in the physical domain. Let be the reference position vector of the reference trajectory at arc length s in the kth planning period; This is the state deviation vector between the information domain and the physical domain; This represents the lateral positional deviation of the trajectory normal; For the target reference trajectory at the projection position The unit normal vector at that location; This refers to the deviation in heading angle; This is the vehicle's current actual heading angle; Projection position The corresponding reference heading angle; This is the normalization function for the heading angle; For speed deviation; Projection position The corresponding reference speed;

[0045] S2.4 Adjusts the boundaries of the vehicle's currently available steering, driving, braking, and lateral capabilities based on the level of road adhesion and actuator availability:

[0046]

[0047] ;

[0048] ;

[0049]

[0050] In the formula, This represents the set of current effective capability boundaries for the vehicle. The effective maximum front wheel steering angle after correction in the k-th planning period; This is the corrected effective maximum steering angular velocity; This refers to the maximum steering angular velocity of the vehicle's hardware. This is the corrected effective maximum driving acceleration; This is the corrected effective maximum braking deceleration; This represents the current road surface adhesion coefficient. It is the acceleration due to gravity; This is the corrected effective maximum lateral acceleration; This is the lateral safety reduction factor; Let k be the maximum feasible curvature of the trajectory at time k;

[0051] S2.5 unifies the target state in the information domain, the vehicle state in the physical domain, changes in the reference baseline, state deviations, and the corrected vehicle capability boundaries to form a cyber-physical fusion planning state:

[0052]

[0053] In the formula, Let be the cyber-physical fusion planning state vector for the k-th planning period; Projected arc length The target status of the information domain at the location.

[0054] Furthermore, step S3 includes the following sub-steps:

[0055] S3.1 Starting from the current physical domain state of the vehicle, predict the future state in the prediction time domain based on the discrete motion relationship of the vehicle's position, heading, and speed;

[0056]

[0057]

[0058] In the formula, To predict the steering angle; To predict longitudinal acceleration; To predict the time step;

[0059] S3.2 Construct the prediction input set based on the vehicle effective capability boundary obtained in step S2. :

[0060] ;

[0061] ;

[0062] ;

[0063]

[0064] The predicted state simultaneously satisfies To limit the intensity of lateral movement under changes in road surface adhesion and actuation capability;

[0065] in, To predict the vehicle speed at the i-th step in the time domain; Let be the trajectory curvature corresponding to the i-th step;

[0066] S3.3 Vehicle state measurement error, road surface adhesion fluctuation, and chassis response deviation are represented as a bounded set of disturbances. The reachable areas for vehicles can be recursively calculated as follows:

[0067]

[0068] In the formula, Let k be the initial reachable set; This represents the set of reachable states of the vehicle at time k+i, predicted at time k. For discrete vehicle dynamics mapping functions; Indicates the superposition of sets;

[0069] S3.4 Compare the target reference trajectory nodes with the vehicle reachable areas at the corresponding predicted time, and calculate the minimum weighted state distance:

[0070]

[0071] In the formula, This is the selection matrix from the vehicle state to the target trajectory state; The state distance weight matrix is ​​positive definite. To be taken from the reachable set The vehicle state vector within;

[0072] S3.5 Determine the feasibility of trajectory nodes based on minimum state distance and lateral capability conditions:

[0073]

[0074] In the formula, Feasibility identifier for trajectory nodes; This indicates that the target node is within the vehicle's current reachability and is therefore considered a feasible node. =0 indicates that the corresponding node is not feasible and is therefore determined to be an infeasible node; The minimum weighted state distance; The preset distance threshold;

[0075] S3.6 Continuously assess infeasible nodes;

[0076] when =0 continuously reaches the preset number of nodes or duration When an infeasible trajectory segment is confirmed, a single discrete outlier does not trigger secondary motion planning; and the first consecutive infeasible node is determined as the starting point of the segment. The last infeasible node before the state is restored to a continuously feasible state is determined as the segment endpoint. Then, the output includes the reachable region sequence, node feasibility markers, and infeasible sections. .

[0077] Furthermore, the triggering conditions for the secondary motion planning include at least one of the following:

[0078] The difference in coordinates, heading, or curvature of the target reference trajectory at the same trajectory progress position in adjacent cycles exceeds a preset threshold;

[0079] Predict the existence of consecutive infeasible trajectory nodes within the time domain;

[0080] The decrease in road surface adhesion level resulted in the maximum permissible lateral acceleration being lower than the original target trajectory requirement;

[0081] A decrease in the availability factor of steering, drive, or braking actuators causes the original target reference trajectory to exceed the vehicle's current capability boundary;

[0082] The nonlinear model predictive controller failed to keep the tracking error within the preset range for several consecutive cycles.

[0083] Beneficial effects:

[0084] 1. A cyber-physical fusion state for secondary motion planning is constructed. Compared with the method of only inputting the target reference trajectory into the controller, this invention integrates the target reference, the actual vehicle motion, the chassis response, the road surface adhesion, and the actuator availability for planning judgment, so that the trajectory feasibility evaluation is based on the actual operating conditions of the vehicle.

[0085] 2. A mechanism for predicting reachable areas and identifying infeasible sections was established. By predicting the reachable areas in the future based on the vehicle's current speed and available steering, driving, and braking capabilities, nodes and continuous sections that cannot be reached in the original target reference trajectory can be identified before the controller executes, thus avoiding the controller from tracking infeasible references for a long time.

[0086] 3. A reconnection and local quadratic planning method based on reverse search is proposed. By reversely judging the accessibility from subsequent feasible trajectory segments, the boundaries of infeasible segments and reconnection points are determined. Only necessary segments are corrected, and the position, heading and curvature of the corrected trajectory are kept continuous with the original trajectory. This reduces the reliance on high-intensity compensation within the controller to handle reference jumps and trajectory infeasibility issues.

[0087] Other advantages, objectives, and features of the invention will be set forth in part in the description which follows, and in part will be apparent to those skilled in the art from the following examination, or may be learned from practice of the invention. The objectives and other advantages of the invention can be realized and obtained through the following description. Attached Figure Description

[0088] Figure 1 This is a flowchart of an intelligent vehicle secondary trajectory planning method based on vehicle motion capability boundary prediction according to the present invention.

[0089] Figure 2 This is a schematic diagram illustrating the principles of vehicle reachability prediction, infeasible trajectory segment identification, and secondary planning in the embodiment.

[0090] Figure 3 This is a schematic diagram of secondary planning and continuous splicing in the embodiment. Detailed Implementation

[0091] To make the technical solutions, advantages, and objectives of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. All other embodiments obtained by those skilled in the art based on the described embodiments of the present invention without creative effort are within the protection scope of this application.

[0092] like Figures 1-3As shown, this embodiment provides an intelligent vehicle secondary trajectory planning method based on vehicle motion capability boundary prediction. This method is positioned between the upstream target reference trajectory processing module and the downstream trajectory tracking controller. It receives the generated dynamic target reference trajectory and, combined with the vehicle's actual motion, chassis response, road conditions, and actuator availability, determines the executability of the original target reference trajectory. When the prediction result indicates that a local segment of the trajectory exceeds the vehicle's current capability, an executable reference trajectory is generated through vehicle reachability area prediction, candidate access point reverse search, and local secondary motion planning, and then handed over to the tracking controller for execution.

[0093] This implementation does not limit the generation, synchronization, compression, or coordinate transformation methods of the upstream target reference trajectory, nor does it focus on the internal model modification, online weight adjustment, or residual compensation of the nonlinear model predictive controller. This invention begins implementation from the moment the upstream module outputs the target reference trajectory, and its key technical aspects lie in trajectory feasibility assessment before control execution, backtracking of infeasible sections, and regeneration of executable trajectories.

[0094] This implementation presents an intelligent vehicle quadratic trajectory planning method based on vehicle motion capability boundary prediction, comprising the following steps:

[0095] S1. To address the issue that the coordinate representation of the target reference trajectory in a non-inertial frame is dynamically updated with the vehicle's motion, causing a jump in the control reference of the target reference trajectory input to the controller, the system is based on the low-dimensional flat trajectory and reconstructed information output by the dynamic trajectory synchronization module, and obtains vehicle motion status, chassis feedback, operating conditions and constraint information.

[0096] This step is used to construct the unified input required for secondary motion planning and to ensure that the target reference, vehicle state, chassis response, and operating conditions have consistent temporal semantics.

[0097] S1.1 Establish dynamic target reference trajectory input. The target reference trajectory is provided by the upstream trajectory processing module and can be represented by discrete trajectory points, parametric curves, or other equivalent forms. For ease of subsequent time series correlation and feasibility assessment, the target reference trajectory for the k-th period is represented as:

[0098]

[0099]

[0100] In the formula, For the timestamp of the trajectory node, For trajectory progress, For reference position, For reference heading, For reference speed, For reference curvature, The number of trajectory nodes is specified. The trajectory can be represented by discrete nodes, parametric curves, or equivalent data packets; its upstream generation method is not a necessary limitation of this invention. The upstream module can be a trajectory reconstruction, trajectory synchronization, or trajectory compression module, and the method used internally to generate the target reference trajectory is not a necessary limitation of this invention.

[0101] S1.2 Establish vehicle motion state input. The actual vehicle motion state is recorded as:

[0102]

[0103] In the formula, For vehicle location, For heading angle, For longitudinal velocity, The yaw rate is angular velocity. and These are longitudinal and lateral accelerations, respectively. The state can be obtained by combining positioning, wheel speed, inertial measurement unit, and chassis state estimation.

[0104] S1.3 Establish chassis response and actuator status input. Record the availability of steering, drive, and braking actuators as follows:

[0105]

[0106] in, and These are the availability coefficients for steering, driving, and braking, respectively. When the availability coefficient is close to 1, the corresponding actuator is in normal condition; when the availability coefficient decreases, it indicates a reduction in execution speed, maximum output, or response accuracy. The availability coefficients can be determined based on chassis diagnostic information, persistent deviations between commands and feedback, or actuator fault codes.

[0107] The availability factor can be determined based on the chassis diagnostic status or the sliding window deviation between the command and feedback, for example, for any execution channel. Possible methods:

[0108]

[0109] In the formula, This is the response deviation index for the corresponding execution channel. This represents the upper limit of the allowable deviation. This expression is used to convert actuator performance variations into dimensionless capability coefficients that can be used for planning.

[0110] S1.4 Establish road surface conditions and vehicle constraint inputs. Road surface conditions are set at the adhesion level. The nominal constraint set of the vehicle is denoted as:

[0111]

[0112] In the formula, L is the vehicle wheelbase. and These are the maximum steering angle and the maximum steering change, respectively. and These are the maximum driving acceleration and the maximum braking deceleration, respectively. For the maximum permissible lateral acceleration, This represents the maximum permissible curvature.

[0113] S1.5 performs time synchronization for multiple input sources;

[0114] Based on the current second planning time To unify the time reference, the target reference trajectory, vehicle status, chassis response, road conditions and actuator status are queried or interpolated so that all types of inputs enter the subsequent processing at the same time.

[0115] The heading angle is calculated using the shortest angle difference interpolation.

[0116]

[0117] In the formula, , They are respectively , Original heading angle at any moment; To align to The heading angle obtained by interpolation at time; These are the interpolation weighting coefficients for the heading angle; This is an angle normalization function used to solve for the shortest angle difference and constrain the angle to a preset angle range;

[0118] Low-frequency states are interpolated using adjacent time intervals, with adjacent sampling times... and The state between The time synchronization relationship is as follows:

[0119] ;

[0120]

[0121] In the formula, This is the current second planning phase; , These are the adjacent original sampling times; , They are respectively , The original state at any given moment; To align to The interpolation state at time step; These are the time interpolation weighting coefficients.

[0122] S2. Based on the cyber-physical fusion mechanism, the dynamic target reference trajectory is synchronously associated with the actual motion state of the vehicle, chassis response and operating conditions, to construct the trajectory tracking error, execution residual and vehicle capability boundary, and to evaluate the continuity and dynamic feasibility of the current target reference trajectory.

[0123] This step is used to establish a correspondence between the dynamic target reference in the information domain and the actual vehicle response in the physical domain, and to convert the road surface and actuator states into the vehicle's current available motion capabilities.

[0124] S2.1 Construct the target state of the information domain.

[0125] The target state in the information domain is determined by the current target reference trajectory and the corresponding state of the target reference trajectory in the previous period at the same trajectory progress position. To evaluate the degree of reference benchmark update, after transforming the previous period trajectory to the current planning coordinate system, the reference change index is calculated:

[0126]

[0127] In the formula, The selected normalized trajectory progress, This indicates the state transition from the previous period's planning coordinate system to the current period's planning coordinate system. This is the reference state difference weight matrix. This index is used to distinguish between normal incremental updates and reference baseline mutations that may affect control stability. It is used to quantify the degree of update of the reference benchmark in adjacent periods, without limiting the processing method inside the upstream trajectory.

[0128] The difference in target state between adjacent cycles at the same trajectory progress is used to characterize the change in the reference baseline:

[0129]

[0130]

[0131] In the formula, The information domain target state vector for the i-th trajectory progress node in the k-th planning period; The target state of the previous cycle is mapped to the target state of the same trajectory progress i under the current planning coordinate system after coordinate transformation and trajectory progress correspondence. The difference in target state at the same trajectory progress node in adjacent planning periods represents the change in the reference trajectory baseline.

[0132] S2.2 Constructs the physical domain vehicle state, which consists of the vehicle's actual motion state and chassis response, denoted as:

[0133]

[0134] S2.3 Project the vehicle's actual position onto the current target reference trajectory, and calculate the state deviation between the information domain and the physical domain at the projected position:

[0135] ;

[0136] ;

[0137]

[0138] In the formula, The arc length progress variable is the reference trajectory; The progress of the arc length corresponding to the optimal projection of the actual position of the vehicle on the target reference trajectory in the kth planning period; This represents the vehicle's current actual position vector in the physical domain. Let be the reference position vector of the reference trajectory at arc length s in the kth planning period; This is the state deviation vector between the information domain and the physical domain; This represents the lateral positional deviation of the trajectory normal; For the target reference trajectory at the projection position The unit normal vector at that location; This refers to the deviation in heading angle; This is the vehicle's current actual heading angle; Projection position The corresponding reference heading angle; This is the normalization function for the heading angle; For speed deviation; Projection position The corresponding reference speed;

[0139] S2.4 Adjusts the boundaries of the vehicle's currently available steering, driving, braking, and lateral capabilities based on the level of road adhesion and actuator availability:

[0140]

[0141] ;

[0142] ;

[0143]

[0144] In the formula, This represents the set of current effective capability boundaries for the vehicle. The effective maximum front wheel steering angle after correction in the k-th planning period; This is the corrected effective maximum steering angular velocity; This refers to the maximum steering angular velocity of the vehicle's hardware. This is the corrected effective maximum driving acceleration; This is the corrected effective maximum braking deceleration; This represents the current road surface adhesion coefficient. It is the acceleration due to gravity; This is the corrected effective maximum lateral acceleration; This is the lateral safety reduction factor; Let k be the maximum feasible curvature of the trajectory at time k;

[0145] S2.5 unifies the target state in the information domain, the vehicle state in the physical domain, changes in the reference baseline, state deviations, and the corrected vehicle capability boundaries to form a cyber-physical fusion planning state:

[0146]

[0147] In the formula, Let be the cyber-physical fusion planning state vector for the k-th planning period; Projected arc length The target state in the information domain at the location. This planned state serves as the common input for vehicle reachability area prediction and secondary motion planning.

[0148] S3. When the target reference trajectory has a reference jump or does not meet the current motion capability of the vehicle, a secondary motion planning problem is constructed in the low-dimensional flat trajectory space. Combining trajectory boundary inheritance, vehicle reachability, actuator constraints and driving safety constraints, the original target reference trajectory is locally modified to generate a continuous and executable target trajectory.

[0149] This step is used to determine whether the original target reference trajectory is within the vehicle's current capability range before the actual control quantity is issued.

[0150] S3.1 Based on the current state of the vehicle Starting from a point, within a length of The future state is predicted using discrete vehicle motion relationships in the prediction time domain:

[0151]

[0152]

[0153] In the formula, To predict the steering angle; To predict longitudinal acceleration; To predict the time step;

[0154] The relationship is used to describe the reachability changes of a vehicle's position, heading, and speed over a finite time period.

[0155] S3.2 Construct the prediction input set based on the vehicle effective capability boundary obtained in step S2. :

[0156] ;

[0157] ;

[0158] ;

[0159]

[0160] The predicted state simultaneously satisfies To limit the intensity of lateral movement under changes in road surface adhesion and actuation capability;

[0161] in, To predict the vehicle speed at the i-th step in the time domain; Let be the trajectory curvature corresponding to the i-th step;

[0162] S3.3 Vehicle state measurement error, road surface adhesion fluctuation, and chassis response deviation are represented as a bounded set of disturbances. The reachable areas for vehicles can be recursively calculated as follows:

[0163]

[0164] In the formula, Let k be the initial reachable set; This represents the set of reachable states of the vehicle at time k+i, predicted at time k. For discrete vehicle dynamics mapping functions; Indicates the superposition of sets;

[0165] According to the recursive formula Iterative calculations yield the set of vehicle reachable states at each time point in the predicted time domain. This set reflects both the vehicle's control capability and the system's finite uncertainty. The boundary of this set is the boundary of the vehicle's motion capability.

[0166] S3.4 Compare the target reference trajectory nodes with the vehicle reachable areas at the corresponding predicted time, and calculate the minimum weighted state distance:

[0167]

[0168] In the formula, This is the selection matrix from the vehicle state to the target trajectory state; The state distance weight matrix is ​​positive definite. To be taken from the reachable set The vehicle state vector within;

[0169] S3.5 Determine the feasibility of trajectory nodes based on minimum state distance and lateral capability conditions:

[0170]

[0171] In the formula, Feasibility identifier for trajectory nodes; This indicates that the target node is within the vehicle's current reachability and is therefore considered a feasible node. =0 indicates that the corresponding node is not feasible and is therefore determined to be an infeasible node; The minimum weighted state distance; The preset distance threshold;

[0172] S3.6 Continuously assess infeasible nodes;

[0173] when =0 continuously reaches the preset number of nodes or duration When an infeasible trajectory segment is confirmed, a single discrete outlier does not trigger secondary motion planning; and the first consecutive infeasible node is determined as the starting point of the segment. The last infeasible node before the state is restored to a continuously feasible state is determined as the segment endpoint. Then, the output includes the reachable region sequence, node feasibility markers, and infeasible sections. .

[0174] This implementation method, through the above steps, keeps the dynamic target reference trajectory, the actual vehicle response, and the vehicle's current capability correlated in each planning cycle. When the coordinate representation of the reference trajectory changes but remains within the vehicle's reach, unnecessary trajectory replacement is not performed. When a local segment of the original trajectory exceeds the vehicle's capability, secondary planning is only performed on the infeasible segment, while the remaining valid trajectories are retained, thus balancing the original trajectory intent, planning continuity, and vehicle execution safety.

[0175] It is hereby declared that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the present invention, and all such modifications or substitutions should be covered within the scope of the claims of the present invention.

Claims

1. A method for intelligent vehicle quadratic trajectory planning based on vehicle motion capability boundary prediction, characterized in that, Includes the following steps: S1. Obtain the low-dimensional flat trajectory and reconstruction information output by the dynamic trajectory synchronization module, and collect vehicle motion status, chassis feedback, operating conditions and constraint information; S2. Based on the cyber-physical fusion mechanism, the dynamic target reference trajectory is synchronously associated with the actual motion state of the vehicle, chassis response, and operating conditions to construct the trajectory tracking error, execution residual, and vehicle motion capability boundary, and to evaluate the continuity and dynamic feasibility of the current target reference trajectory. S3. When the target reference trajectory has a reference jump or does not meet the current motion capability of the vehicle, a secondary motion planning problem is constructed in the low-dimensional flat trajectory space. Combining the trajectory boundary inheritance characteristics, vehicle reachability, actuator constraints and driving safety constraints, the original target reference trajectory is locally modified to generate a continuous and executable target trajectory.

2. The intelligent vehicle quadratic trajectory planning method based on vehicle motion capability boundary prediction according to claim 1, characterized in that, Step S1 includes the following sub-steps: S1.1 Obtain the low-dimensional flat original target reference trajectory output by the dynamic trajectory synchronization module; ； In the formula, For the timestamp of the trajectory nodes; For trajectory progress; For reference position; For reference heading; For reference speed; For reference curvature; This represents the number of trajectory nodes. S1.2 Collects the real-time motion state of the vehicle, and the vehicle motion state vector is denoted as: In the formula, Let k be the real-time motion state vector of the vehicle at time k. This is the timestamp of the k-th sampling time; For vehicle location; For heading angle; Longitudinal velocity; This refers to the yaw rate; and These are longitudinal and lateral accelerations, respectively. S1.3 Establish chassis response and actuator status input, wherein the chassis response and actuator availability are denoted as: In the formula, This is the actual execution response vector of the chassis at the k-th sampling time; and These represent the actual steering response, driving response, and braking response at time k, respectively. Let k be the diagonal matrix of executor availability. , , These respectively characterize the availability of the actuators corresponding to steering, driving, and braking; This indicates the construction of a diagonal matrix; S1.4 Establish road surface conditions and vehicle constraint inputs; Road surface conditions based on adhesion level Characterization; The nominal constraint set of vehicles is denoted as: In the formula, L is the vehicle wheelbase; and These are the maximum steering angle and the maximum steering change, respectively. and These are the maximum driving acceleration and the maximum braking deceleration, respectively. This is the maximum permissible lateral acceleration; Maximum permissible curvature; S1.5 performs time synchronization for multiple input sources; Based on the current second planning time To unify the time reference, the target reference trajectory, vehicle status, chassis response, road conditions and actuator status are queried or interpolated so that all types of inputs enter the subsequent processing at the same time. Among them, the heading angle is interpolated using the shortest angle difference; the low-frequency state is interpolated using adjacent time intervals, with adjacent sampling times... and The state between The time synchronization relationship is as follows: ； In the formula, This is the current second planning phase; , These are the adjacent original sampling times; , They are respectively , The original state at any given moment; To align to The interpolation state at time step; These are the time interpolation weighting coefficients.

3. The intelligent vehicle quadratic trajectory planning method based on vehicle motion capability boundary prediction according to claim 2, characterized in that, Step S2 includes the following sub-steps: S2.1 Construct the target state in the information domain, and characterize the change in the reference baseline by the difference in the target state of adjacent cycles at the same trajectory progress: In the formula, The information domain target state vector of the i-th trajectory progress node in the k-th planning period; The target state of the previous cycle is mapped to the target state of the same trajectory progress i under the current planning coordinate system after coordinate transformation and trajectory progress correspondence. The difference in target state at the same trajectory progress node in adjacent planning periods represents the change in the reference trajectory baseline. S2.2 Constructs the physical domain vehicle state, which consists of the vehicle's actual motion state and chassis response, denoted as: S2.3 Project the vehicle's actual position onto the current target reference trajectory, and calculate the state deviation between the information domain and the physical domain at the projected position: ； ； In the formula, The arc length progress variable is the reference trajectory; The progress of the arc length corresponding to the optimal projection of the actual position of the vehicle on the target reference trajectory in the kth planning period; This represents the vehicle's current actual position vector in the physical domain. Let be the reference position vector of the reference trajectory at arc length s in the kth planning period; This is the state deviation vector between the information domain and the physical domain; This represents the lateral positional deviation of the trajectory normal; For the target reference trajectory at the projection position The unit normal vector at that location; This refers to the deviation in heading angle; This is the vehicle's current actual heading angle; Projection position The corresponding reference heading angle; This is the normalization function for the heading angle; For speed deviation; Projection position The corresponding reference speed; S2.4 Adjusts the boundaries of the vehicle's currently available steering, driving, braking, and lateral capabilities based on the level of road adhesion and actuator availability: ； ； In the formula, This represents the set of current effective capability boundaries for the vehicle. The effective maximum front wheel steering angle after correction in the k-th planning period; This is the corrected effective maximum steering angular velocity; This refers to the maximum steering angular velocity of the vehicle's hardware. This is the corrected effective maximum driving acceleration; This is the corrected effective maximum braking deceleration; This represents the current road surface adhesion coefficient. It is the acceleration due to gravity; This is the corrected effective maximum lateral acceleration; This is the lateral safety reduction factor; Let k be the maximum feasible curvature of the trajectory at time k; S2.5 unifies the target state in the information domain, the vehicle state in the physical domain, changes in the reference baseline, state deviations, and the corrected vehicle capability boundaries to form a cyber-physical fusion planning state: In the formula, Let be the cyber-physical fusion planning state vector for the k-th planning period; Projected arc length The target status of the information domain at the location.

4. The intelligent vehicle quadratic trajectory planning method based on vehicle motion capability boundary prediction according to claim 3, characterized in that, Step S3 includes the following sub-steps: S3.1 Starting from the current physical domain state of the vehicle, predict the future state in the prediction time domain based on the discrete motion relationship of the vehicle's position, heading, and speed; In the formula, To predict the steering angle; To predict longitudinal acceleration; To predict the time step; S3.2 Construct the prediction input set based on the vehicle effective capability boundary obtained in step S2. : ； ； ； The predicted state simultaneously satisfies To limit the intensity of lateral movement under changes in road surface adhesion and actuation capability; in, To predict the vehicle speed at the i-th step in the time domain; Let be the trajectory curvature corresponding to the i-th step; S3.3 Vehicle state measurement error, road surface adhesion fluctuation, and chassis response deviation are represented as a bounded set of disturbances. The reachable areas for vehicles can be recursively calculated as follows: In the formula, Let k be the initial reachable set; This represents the set of reachable states of the vehicle at time k+i, predicted at time k. For discrete vehicle dynamics mapping functions; Indicates the superposition of sets; S3.4 Compare the target reference trajectory nodes with the vehicle reachable areas at the corresponding predicted time, and calculate the minimum weighted state distance: In the formula, This is the selection matrix from the vehicle state to the target trajectory state; The state distance weight matrix is ​​positive definite. To be taken from the reachable set The vehicle state vector within; S3.5 Determine the feasibility of trajectory nodes based on minimum state distance and lateral capability conditions: In the formula, Feasibility identifier for trajectory nodes; This indicates that the target node is within the vehicle's current reachability and is therefore considered a feasible node. =0 indicates that the corresponding node is not feasible and is therefore determined to be an infeasible node; The minimum weighted state distance; The preset distance threshold; S3.6 Continuously assess infeasible nodes; when =0 continuously reaches the preset number of nodes or duration When an infeasible trajectory segment is confirmed, a single discrete outlier does not trigger secondary motion planning; and the first consecutive infeasible node is determined as the starting point of the segment. The last infeasible node before the state is restored to a continuously feasible state is determined as the segment endpoint. Then, the output includes the reachable region sequence, node feasibility markers, and infeasible sections. .

Images