A method and system for autonomous driving vehicle trajectory planning

Abstract

The present invention relates to the field of autonomous driving technology, and in particular to a method and system for planning the trajectory of an autonomous driving vehicle. Based on the traditional autonomous driving algorithm that plans the vehicle's driving trajectory based on preset trajectory evaluation indicators and the current frame state of the vehicle, the present invention splices the initial driving trajectory with the continuity constraint trajectory to obtain a final driving trajectory. The continuity constraint trajectory is based on a vehicle dynamics model and is calculated according to the current frame state and the historical frame state of the vehicle. It reflects the historical driving trajectory of the vehicle before the current moment, as well as the dynamic trajectory of the vehicle at the next moment based on the current state. The continuity constraint trajectory is spliced ​​with the initial driving trajectory so that the final driving trajectory generated by the splicing improves the continuity with the historical frame trajectory of the vehicle and the continuity with the current motion trend of the vehicle, thereby preventing the vehicle's planned trajectory from experiencing a drastic change in heading angle between the previous frame and the next frame, and preventing the risk of sudden braking or sudden steering.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to a method and system for autonomous driving vehicle trajectory planning. Background Art

[0002] Current autonomous vehicle trajectory planning systems generally employ a modular, layered architecture, consisting of independent modules that sequentially execute environmental perception, behavior prediction, decision-making, and trajectory planning. Within this architecture, a path planner generates local trajectories frame by frame based on real-time perception data, guiding vehicle motion control.

[0003] However, the above-mentioned traditional path planning methods have significant limitations. On the one hand, mainstream autonomous driving vehicle trajectory planners, such as Lattice sampling methods and quadratic programming optimizers, focus on the geometric smoothness of the vehicle's single-frame trajectory, but ignore the motion continuity constraints between adjacent frame trajectories. This makes the vehicle's planned trajectory prone to drastic heading angle changes, causing high-frequency steering wheel vibration, leading to the risk of sudden braking or sudden steering, and reducing vehicle driving stability and safety. On the other hand, existing methods mainly rely on static hard boundary conditions such as collision boundary constraint trajectory planners to ensure the safety of autonomous driving, but do not consider the vehicle's historical motion state. When there are errors in obstacle prediction or the environment mutates, the adjacent frame trajectories generated by the planner may produce drastic deviations, forcing the vehicle to perform unnecessary emergency braking or aggressive steering operations, causing the risk of sudden braking or sudden steering, and significantly increasing the uncertainty and safety risks of motion control.

[0004] Therefore, how to avoid drastic changes in the heading angle of the autonomous vehicle trajectory, which may cause unnecessary emergency braking or aggressive steering operations, has become an urgent problem to be solved in this field. Summary of the Invention

[0005] The present invention aims to provide a method and system for autonomous vehicle trajectory planning to improve the continuity and stability of autonomous vehicle trajectory planning and solve the technical problem that the trajectory of autonomous vehicles is prone to drastic changes in heading angle, which may cause unnecessary emergency braking or aggressive steering operations.

[0006] To achieve the above objectives, the present invention provides a first aspect of a method for autonomous driving vehicle trajectory planning, comprising the following steps:

[0007] Build a vehicle dynamics model and obtain the vehicle's current frame state and historical frame state;

[0008] Based on the vehicle dynamics model, obtaining a continuity-constrained trajectory according to the current frame state and the historical frame state;

[0009] Planning an initial driving trajectory based on a preset trajectory evaluation index and the current frame state;

[0010] The continuity constraint trajectory and the initial driving trajectory are spatiotemporally spliced ​​to obtain a final driving trajectory.

[0011] The above-mentioned autonomous driving vehicle trajectory planning method, based on the traditional autonomous driving algorithm that plans the vehicle's driving trajectory based on preset trajectory evaluation indicators and the vehicle's current frame state, splices the initial driving trajectory with the continuity constraint trajectory to obtain the final driving trajectory. The continuity constraint trajectory is based on the vehicle dynamics model and is calculated according to the vehicle's current frame state and historical frame state. It reflects the vehicle's historical driving trajectory before the current moment, as well as the vehicle's dynamic trajectory at the next moment based on the current state. The continuity constraint trajectory is spliced ​​with the initial driving trajectory so that the final driving trajectory generated by the splicing improves the continuity with the vehicle's historical frame trajectory and the continuity with the vehicle's current motion trend, preventing the vehicle's planned trajectory from having a strong jump in motion direction between the previous frame and the next frame, thereby preventing the vehicle's trajectory from having a drastic change in heading angle, improving the inter-frame continuity and stability of the trajectory planning, and improving the safety of autonomous driving.

[0012] Since the final driving trajectory is formed by spatiotemporally splicing the continuity-constrained trajectory and the initial driving trajectory, it not only improves the continuity of trajectory planning and the vehicle's historical motion trends, but also maintains the timeliness of traditional autonomous driving algorithms in planning the optimal path for obstacle avoidance and other functions.

[0013] Furthermore, the obtaining of a continuity-constrained trajectory based on the vehicle dynamics model and the current frame state and the historical frame state includes:

[0014] predicting an inertial trajectory based on the vehicle dynamics model and the current frame state;

[0015] Acquire a historical trajectory based on the historical frame state;

[0016] The inertial trajectory and the historical trajectory are spliced ​​in time and space to obtain the continuity constraint trajectory.

[0017] In this implementation, the continuity-constrained trajectory is composed of an inertial trajectory and a historical trajectory. The inertial trajectory is generated by recursively predicting the position sequence of the vehicle within a certain period of time in the future based on the state equation in the vehicle dynamics model and the current state data of the vehicle, and represents the current inertial trend of the vehicle's motion; splicing the inertial trajectory in the continuity-constrained trajectory can improve the continuity between the final driving trajectory and the vehicle's current motion state, and avoid drastic sudden changes in the vehicle's heading angle. In this implementation, the historical trajectory refers to the previous frame trajectory of the vehicle's current frame trajectory, which is obtained from the historical data of the vehicle's driving; splicing the historical trajectory in the continuity-constrained trajectory can improve the similarity between the final driving trajectory and the vehicle's historical driving trajectory, thereby ensuring the stability between the vehicle's multi-frame driving trajectories, avoiding drastic deviations in adjacent frame trajectory routes, and ultimately improving the safety of autonomous driving trajectory planning.

[0018] Furthermore, the spatiotemporal splicing of the inertial trajectory and the historical trajectory to obtain the continuity-constrained trajectory includes:

[0019] Obtaining a first trajectory point in the inertial trajectory;

[0020] Acquire an inertial trajectory starting point in the inertial trajectory, and then acquire an inertial clipping trajectory between the first trajectory point and the inertial trajectory starting point in the inertial trajectory;

[0021] Acquire a second trajectory point in the historical trajectory that satisfies a preset spatiotemporal threshold with the first trajectory point;

[0022] Obtaining an end point of the historical trajectory in the historical trajectory, and then obtaining a historical clipping trajectory between the end point of the historical trajectory and the second trajectory point in the historical trajectory;

[0023] The inertial clipping trajectory and the historical clipping trajectory are spliced ​​based on the first trajectory point and the second trajectory point to obtain the continuity constraint trajectory.

[0024] In this implementation, in order to splice the inertial trajectory and the historical trajectory, considering that both are trajectories extending forward from the vehicle as the starting point, if the starting points of the two trajectories are aligned with the vehicle as the starting point, then there must be overlapping parts in the two trajectories. The purpose of the spatiotemporal alignment here is to find and cut out the overlapping parts of the two trajectories. Specifically, the spatiotemporal splicing method is used to cut out the trajectory between the starting point of the inertial trajectory and the first trajectory point; the second trajectory point closest to the first trajectory point of the inertial trajectory in the historical trajectory is obtained, and the trajectory between the second trajectory point of the historical trajectory and the end point of the historical trajectory is cut out; with the inertial trajectory as the starting trajectory and the historical trajectory as the subsequent trajectory, after aligning the first trajectory point and the second trajectory point in the global coordinate system, the inertial clipped trajectory is spliced ​​with the historical clipped trajectory to obtain a continuity-constrained trajectory. This continuity-constrained trajectory uses the inertial trajectory as the starting trajectory, which can improve the continuity between the starting path of the continuity-constrained trajectory and the vehicle's current motion state, avoiding drastic changes in the vehicle's heading angle; using the historical trajectory as the subsequent trajectory can improve the similarity between the overall continuity-constrained trajectory and the vehicle's historical driving trajectory, thereby ensuring the stability between the vehicle's multi-frame driving trajectories.

[0025] Furthermore, the spatiotemporal splicing of the continuity-constrained trajectory and the initial driving trajectory to obtain a final driving trajectory includes:

[0026] Acquiring a third trajectory point in the continuity-constrained trajectory based on a preset continuity constraint rule, wherein the continuity constraint rule is used to determine a splicing ratio between the continuity-constrained trajectory and the initial driving trajectory;

[0027] Obtaining a constraint trajectory starting point in the continuity constraint trajectory, and then obtaining a constraint clipping trajectory between the third trajectory point in the continuity constraint trajectory and the constraint trajectory starting point;

[0028] Acquire a fourth trajectory point in the initial driving trajectory that satisfies a preset spatiotemporal threshold with the third trajectory point;

[0029] Obtaining an end point of the driving trajectory in the initial driving trajectory, and then obtaining a driving clipping trajectory between the end point of the driving trajectory and the fourth trajectory point in the initial driving trajectory;

[0030] The constraint clipping trajectory and the driving clipping trajectory are spliced ​​based on the third trajectory point and the fourth trajectory point to obtain the final driving trajectory.

[0031] In this implementation, in order to splice the continuity constraint trajectory with the initial driving trajectory, considering that both are trajectories extending forward from the vehicle as the starting point, if the starting points of the two trajectories are aligned with the vehicle as the starting point, then there must be overlapping parts in the two trajectories. The purpose of doing spatiotemporal alignment here is to find and cut out the overlapping parts of the two trajectories. Specifically, using the spatiotemporal splicing method, the trajectory between the starting point of the continuity constraint trajectory and the third trajectory point is cut out; the fourth trajectory point closest to the third trajectory point in the initial driving trajectory is obtained, and the trajectory between the fourth trajectory point of the initial driving trajectory and the end point of the initial driving trajectory is cut out; using the continuity constraint trajectory as the starting trajectory and the initial driving trajectory as the subsequent trajectory, after aligning the third trajectory point and the fourth trajectory point in the global coordinate system, the constraint clipping trajectory and the driving clipping trajectory are spliced ​​to obtain the final driving trajectory. This final driving trajectory uses the continuity-constrained trajectory as its starting trajectory, enhancing the continuity between the starting path of the final driving trajectory and the vehicle's current motion state, as well as its similarity to the vehicle's historical driving trajectories. This prevents drastic changes in the vehicle's heading angle, improves the inter-frame continuity and stability of trajectory planning, and enhances autonomous driving safety. Furthermore, by using the initial driving trajectory planned by a traditional autonomous driving algorithm based on preset trajectory evaluation indicators and the vehicle's current frame state as the subsequent trajectory, the traditional autonomous driving algorithm maintains its ability to plan optimal paths for obstacle avoidance and other timely functions.

[0032] It should be noted that the continuity constraint rule determines the splicing ratio between the continuity-constrained trajectory and the initial driving trajectory. Specifically, intercepting more continuity-constrained trajectories means that the final driving trajectory contains more inertial trajectory information and historical trajectory information. While this improves the inter-frame continuity of the final driving trajectory, it also weakens the initial driving trajectory's predictive planning intent for the vehicle's journey. Therefore, the splicing ratio between the continuity-constrained trajectory and the initial driving trajectory can be adjusted according to different vehicle driving scenarios and requirements, making a trade-off between the inter-frame continuity and predictive planning intent of the final driving trajectory.

[0033] Furthermore, the step of splicing the constraint clipping trajectory and the driving clipping trajectory based on the third trajectory point and the fourth trajectory point to obtain the final driving trajectory includes:

[0034] splicing the constraint clipping trajectory and the driving clipping trajectory based on the third trajectory point and the fourth trajectory point to obtain a first spliced ​​driving trajectory;

[0035] The first spliced ​​driving trajectory is smoothed to eliminate sudden changes in curvature at the third trajectory point and the fourth trajectory point, thereby obtaining a final driving trajectory.

[0036] In this implementation, after the first and second trajectory points are aligned in the global coordinate system and the constraint clipping trajectory and the driving clipping trajectory are spliced, the aligned parts of the third and fourth trajectory points are smoothed to eliminate the sudden change in curvature at the splicing point, which is conducive to faster convergence of the driving trajectory and a smoother vehicle driving planning route.

[0037] Furthermore, the smoothing process is performed on the first spliced ​​driving trajectory to eliminate the sudden changes in curvature at the third and fourth trajectory points to obtain a final driving trajectory, including:

[0038] Smoothing the first spliced ​​driving trajectory to eliminate sudden changes in curvature at the third and fourth trajectory points, thereby obtaining a second spliced ​​driving trajectory;

[0039] A quadratic programming algorithm is used to optimize the second spliced ​​driving trajectory based on preset driving boundary hard constraints and preset soft constraints to obtain a final driving trajectory.

[0040] In this implementation, after smoothing the trajectory joints, the second joint is used as a guideline and refined and optimized using a quadratic programming algorithm to ensure that it meets the hard constraints of the preset road boundaries, as well as preset soft constraints such as jerk (acceleration rate) constraints and smoothness constraints, ultimately obtaining a final trajectory that meets the requirements.

[0041] Furthermore, the vehicle dynamics model is expressed as follows:

[0042] ;

[0043] in, represents the vehicle's horizontal coordinate, represents the vehicle's longitudinal coordinate, represents the vehicle heading angle, Indicates the distance between the front wheels and the rear wheels of the vehicle. Indicates the vehicle speed, Indicates the vehicle's front wheel turning angle.

[0044] In this implementation, a vehicle dynamics model is constructed. Based on the state equation of the vehicle dynamics model, the vehicle heading angle at the next moment can be recursively inferred from the current vehicle speed and the current vehicle front wheel angle, thereby calculating the vehicle's inertial position at the next moment. Finally, the vehicle's inertial trajectory is obtained by recursively inferring the vehicle's inertial position sequence within a certain time interval in the future.

[0045] A second aspect of the present invention provides an autonomous driving vehicle trajectory planning system, which includes a dynamic model construction module, a vehicle state detection module, a continuity constraint module, an initial trajectory planning module, and a final trajectory splicing module, wherein:

[0046] The dynamics model building module is used to build a vehicle dynamics model;

[0047] The vehicle state detection module is used to obtain the current frame state and historical frame state of the vehicle;

[0048] The continuity constraint module is used to obtain a continuity constraint trajectory based on the current frame state and the historical frame state according to the vehicle dynamics model;

[0049] The initial trajectory planning module is used to plan the initial driving trajectory according to the preset trajectory evaluation index and the current frame state;

[0050] The final trajectory splicing module is used to splice the continuity constraint trajectory with the initial driving trajectory in time and space to obtain a final driving trajectory.

[0051] The above-mentioned autonomous driving vehicle trajectory planning system, based on the traditional autonomous driving algorithm that plans the vehicle's driving trajectory based on preset trajectory evaluation indicators and the vehicle's current frame state, splices the initial driving trajectory with the continuity constraint trajectory to obtain the final driving trajectory. The continuity constraint trajectory is based on the vehicle dynamics model and is calculated according to the vehicle's current frame state and historical frame state. It reflects the vehicle's historical driving trajectory before the current moment, as well as the vehicle's dynamic trajectory at the next moment based on the current state. The continuity constraint trajectory is spliced ​​with the initial driving trajectory so that the final driving trajectory generated by the splicing improves the continuity with the vehicle's historical frame trajectory and the continuity with the vehicle's current motion trend, preventing the vehicle's planned trajectory from having a strong jump in motion direction between the previous frame and the next frame, thereby preventing the vehicle's trajectory from having a drastic change in heading angle, improving the inter-frame continuity and stability of the trajectory planning, and improving the safety of autonomous driving.

[0052] Since the final driving trajectory is formed by spatiotemporally splicing the continuity-constrained trajectory and the initial driving trajectory, it not only improves the continuity of trajectory planning and the vehicle's historical motion trends, but also maintains the timeliness of traditional autonomous driving algorithms in planning the optimal path for obstacle avoidance and other functions.

[0053] Furthermore, the obtaining of a continuity-constrained trajectory based on the current frame state and the historical frame state according to the vehicle dynamics model includes:

[0054] predicting an inertial trajectory based on the vehicle dynamics model and the current frame state;

[0055] Acquire a historical trajectory based on the historical frame state;

[0056] The inertial trajectory and the historical trajectory are spliced ​​in time and space to obtain the continuity constraint trajectory.

[0057] In this implementation, the continuity-constrained trajectory is composed of an inertial trajectory and a historical trajectory. The inertial trajectory is generated by recursively predicting the position sequence of the vehicle within a certain period of time in the future based on the state equation in the vehicle dynamics model and the current state data of the vehicle, and represents the current inertial trend of the vehicle's motion; splicing the inertial trajectory in the continuity-constrained trajectory can improve the continuity between the final driving trajectory and the vehicle's current motion state, and avoid drastic sudden changes in the vehicle's heading angle. In this implementation, the historical trajectory refers to the previous frame trajectory of the vehicle's current frame trajectory, which is obtained from the historical data of the vehicle's driving; splicing the historical trajectory in the continuity-constrained trajectory can improve the similarity between the final driving trajectory and the vehicle's historical driving trajectory, thereby ensuring the stability between the vehicle's multi-frame driving trajectories, avoiding drastic deviations in adjacent frame trajectory routes, and ultimately improving the safety of autonomous driving trajectory planning.

[0058] Furthermore, the spatiotemporal splicing of the inertial trajectory and the historical trajectory to obtain the continuity-constrained trajectory includes:

[0059] Obtaining a first trajectory point in the inertial trajectory;

[0060] Acquire an inertial trajectory starting point in the inertial trajectory, and then acquire an inertial clipping trajectory between the first trajectory point and the inertial trajectory starting point in the inertial trajectory;

[0061] Acquire a second trajectory point in the historical trajectory that satisfies a preset spatiotemporal threshold with the first trajectory point;

[0062] Obtaining an end point of the historical trajectory in the historical trajectory, and then obtaining a historical clipping trajectory between the end point of the historical trajectory and the second trajectory point in the historical trajectory;

[0063] The inertial clipping trajectory and the historical clipping trajectory are spliced ​​based on the first trajectory point and the second trajectory point to obtain the continuity constraint trajectory.

[0064] In this implementation, in order to splice the inertial trajectory and the historical trajectory, considering that both are trajectories extending forward from the vehicle as the starting point, if the starting points of the two trajectories are aligned with the vehicle as the starting point, then there must be overlapping parts in the two trajectories. The purpose of the spatiotemporal alignment here is to find and cut out the overlapping parts of the two trajectories. Specifically, the spatiotemporal splicing method is used to cut out the trajectory between the starting point of the inertial trajectory and the first trajectory point; the second trajectory point closest to the first trajectory point of the inertial trajectory in the historical trajectory is obtained, and the trajectory between the second trajectory point of the historical trajectory and the end point of the historical trajectory is cut out; with the inertial trajectory as the starting trajectory and the historical trajectory as the subsequent trajectory, after aligning the first trajectory point and the second trajectory point in the global coordinate system, the inertial clipped trajectory is spliced ​​with the historical clipped trajectory to obtain a continuity-constrained trajectory. This continuity-constrained trajectory uses the inertial trajectory as the starting trajectory, which can improve the continuity between the starting path of the continuity-constrained trajectory and the vehicle's current motion state, avoiding drastic changes in the vehicle's heading angle; using the historical trajectory as the subsequent trajectory can improve the similarity between the overall continuity-constrained trajectory and the vehicle's historical driving trajectory, thereby ensuring the stability between the vehicle's multi-frame driving trajectories. BRIEF DESCRIPTION OF THE DRAWINGS

[0065] Figure 1 This is a flow chart of a method for trajectory planning of an autonomous driving vehicle provided by an embodiment of the present invention;

[0066] Figure 2 Schematic diagram of the structure of an autonomous driving vehicle trajectory planning system provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0067] The present invention will be described in detail below with reference to the accompanying drawings and in conjunction with embodiments. It should be noted that the following detailed descriptions are all exemplary descriptions and are intended to provide further detailed descriptions of the present invention. Unless otherwise defined, all technical and scientific terms used herein have the same meanings as those generally understood by those skilled in the art to which this application belongs; the terms used herein in the specification of the application are only for the purpose of describing specific embodiments and are not intended to limit this application; the terms "including" and "having" in the specification and claims of this application and the description of the above-mentioned drawings, as well as any variations thereof, are intended to cover non-exclusive inclusions. The terms "first", "second", etc. in the specification and claims of this application or the above-mentioned drawings are used to distinguish different objects, rather than to describe a specific order.

[0068] It should be understood that although the steps in the flowcharts of the accompanying drawings are shown sequentially as indicated by the arrows, these steps are not necessarily executed in the order indicated by the arrows. Unless otherwise specified herein, there is no strict order restriction on the execution of these steps and they may be executed in other orders. Furthermore, at least some of the steps in the flowcharts of the accompanying drawings may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily executed and completed at the same time, but may be executed at different times. Their execution order is not necessarily sequential, but may be executed in rotation or alternation with other steps or at least a portion of their sub-steps or stages.

[0069] Before describing the present application in detail with reference to the accompanying drawings and in combination with the embodiments, the technical background involved in the present application will first be described.

[0070] The present invention belongs to the technical field of autonomous driving decision-making and planning, and specifically relates to an optimization method for improving path smoothness and safety by constraining the inter-frame trajectory continuity of the initial driving trajectory by splicing the continuity-constrained trajectory with the initial driving trajectory. The planning module of the current autonomous driving system generally adopts a modular architecture, namely, a hierarchical design of road condition perception, vehicle driving state prediction, vehicle driving plan decision, and vehicle driving trajectory planning. By perceiving the road condition environment in real time, a single-frame driving trajectory is generated based on the predicted future state of the vehicle. However, the above-mentioned traditional methods have the following limitations:

[0071] 1) Insufficient temporal continuity. Traditional path planners, such as the Lattice Planner (trajectory planning algorithm) and QP optimization (quadratic programming), focus only on the smoothness of a single-frame trajectory generated based on the predicted future vehicle state. They ignore the inter-frame continuity between the current frame and the previous frame. This can easily lead to significant changes in the route between the previous frame and the next frame, causing sudden changes in the vehicle's heading angle and steering wheel vibration, increasing vehicle driving risks.

[0072] 2) Poor adaptability to dynamic scenarios. When planning obstacle avoidance paths, traditional path planning methods can cause drastic deviations in the previous and next frame trajectories due to obstacle prediction errors or sudden changes in the environment, leading to the risk of sudden braking or swerving. These sudden changes in trajectory can exceed the vehicle's dynamic limits, increasing driving hazards.

[0073] 3) Lack of historical frame similarity optimization. Existing methods often rely on hard constraints such as collision boundaries to ensure driving trajectory safety, but do not consider the inertial trend of vehicle motion and the correlation between historical and current driving trajectories.

[0074] Among them, the quadratic programming trajectory optimization method only generates the optimal trajectory of a single frame through dynamic obstacle avoidance constraints and curvature continuity constraints, but ignores the inter-frame continuity between a single frame and the previous frame.

[0075] In order to solve the above technical problems, refer to Figure 1 According to a first aspect of an embodiment of the present invention, a method for trajectory planning of an autonomous driving vehicle is provided, comprising the following steps:

[0076] S101, constructing a vehicle dynamics model and obtaining the vehicle's current frame state and historical frame state;

[0077] S102: Based on the vehicle dynamics model, obtaining a continuity-constrained trajectory according to the current frame state and the historical frame state;

[0078] S103, planning an initial driving trajectory based on a preset trajectory evaluation index and the current frame state;

[0079] S104: splicing the continuity-constrained trajectory with the initial driving trajectory in time and space to obtain a final driving trajectory.

[0080] The above-mentioned autonomous driving vehicle trajectory planning method, based on the traditional autonomous driving algorithm that plans the vehicle's driving trajectory based on preset trajectory evaluation indicators and the vehicle's current frame state, splices the initial driving trajectory with the continuity constraint trajectory to obtain the final driving trajectory. The continuity constraint trajectory is based on the vehicle dynamics model and is calculated according to the vehicle's current frame state and historical frame state. It reflects the vehicle's historical driving trajectory before the current moment, as well as the vehicle's dynamic trajectory at the next moment based on the current state. The continuity constraint trajectory is spliced ​​with the initial driving trajectory so that the final driving trajectory generated by the splicing improves the continuity with the vehicle's historical frame trajectory and the continuity with the vehicle's current motion trend, preventing the vehicle's planned trajectory from having a strong jump in motion direction between the previous frame and the next frame, thereby preventing the vehicle's trajectory from having a drastic change in heading angle, improving the inter-frame continuity and stability of the trajectory planning, and improving the safety of autonomous driving.

[0081] Since the final driving trajectory is formed by spatiotemporally splicing the continuity-constrained trajectory and the initial driving trajectory, it not only improves the continuity of trajectory planning and the vehicle's historical motion trends, but also maintains the timeliness of traditional autonomous driving algorithms in planning the optimal path for obstacle avoidance and other functions.

[0082] Furthermore, the obtaining of a continuity-constrained trajectory based on the vehicle dynamics model and the current frame state and the historical frame state includes:

[0083] predicting an inertial trajectory based on the vehicle dynamics model and the current frame state;

[0084] Acquire a historical trajectory based on the historical frame state;

[0085] The inertial trajectory and the historical trajectory are spliced ​​in time and space to obtain the continuity constraint trajectory.

[0086] Specifically, in one possible embodiment, the vehicle's position sequence within the next 0.3 seconds is recursively calculated based on the state equation of the vehicle dynamics model, thereby obtaining the vehicle's inertial trajectory within the next 0.3 seconds. Based on the vehicle's historical information, the vehicle's previous frame trajectory at the current moment is obtained as the vehicle's historical trajectory.

[0087] It should be noted that since the inertial trajectory primarily reflects the vehicle's current inertial motion trend, only the short-term inertial trajectory needs to be recursively calculated. The historical trajectory, on the other hand, is a record of the vehicle's trajectory planning over a longer distance in the previous frame, typically 150 to 200 meters in length. Therefore, the length of the inertial trajectory is usually shorter than that of the historical trajectory.

[0088] In this implementation, the continuity-constrained trajectory is composed of an inertial trajectory and a historical trajectory. The inertial trajectory is generated by recursively predicting the position sequence of the vehicle within a certain period of time in the future based on the state equation in the vehicle dynamics model and the current state data of the vehicle, and represents the current inertial trend of the vehicle's motion; splicing the inertial trajectory in the continuity-constrained trajectory can improve the continuity between the final driving trajectory and the vehicle's current motion state, and avoid drastic sudden changes in the vehicle's heading angle. In this implementation, the historical trajectory refers to the previous frame trajectory of the vehicle's current frame trajectory, which is obtained from the historical data of the vehicle's driving; splicing the historical trajectory in the continuity-constrained trajectory can improve the similarity between the final driving trajectory and the vehicle's historical driving trajectory, thereby ensuring the stability between the vehicle's multi-frame driving trajectories, avoiding drastic deviations in adjacent frame trajectory routes, and ultimately improving the safety of autonomous driving trajectory planning.

[0089] Furthermore, the spatiotemporal splicing of the inertial trajectory and the historical trajectory to obtain the continuity-constrained trajectory includes:

[0090] Obtaining a first trajectory point in the inertial trajectory;

[0091] Acquire an inertial trajectory starting point in the inertial trajectory, and then acquire an inertial clipping trajectory between the first trajectory point and the inertial trajectory starting point in the inertial trajectory;

[0092] Acquire a second trajectory point in the historical trajectory that satisfies a preset spatiotemporal threshold with the first trajectory point;

[0093] Obtaining an end point of the historical trajectory in the historical trajectory, and then obtaining a historical clipping trajectory between the end point of the historical trajectory and the second trajectory point in the historical trajectory;

[0094] The inertial clipping trajectory and the historical clipping trajectory are spliced ​​based on the first trajectory point and the second trajectory point to obtain the continuity constraint trajectory.

[0095] It should be noted that after aligning the starting points of the inertial trajectory and the historical trajectory, the preset spatiotemporal threshold can be the second trajectory point closest to the first trajectory point, or it can be: the time frame of the first trajectory point in the inertial trajectory is the same as the time frame of the second trajectory point in the historical trajectory.

[0096] Specifically, in one possible embodiment, after obtaining the vehicle's inertial trajectory within the next 0.3 seconds and the vehicle's complete historical frame trajectory of the previous frame, there are two clipping and splicing methods:

[0097] Method 1: Align the starting points of the inertial trajectory and the historical trajectory; obtain the last trajectory point of the inertial trajectory as the first trajectory point; match the second trajectory point in the historical trajectory that is closest to the first trajectory point in terms of spatial coordinates; trim and delete all points before the second trajectory point in the historical trajectory, and use the remaining part as the historical clipping trajectory; use the first trajectory point of the inertial trajectory and the second trajectory point of the historical clipping trajectory as the splicing anchor points to re-join the two trajectories into a single trajectory.

[0098] Method 2: Align the starting points of the inertial trajectory and the historical trajectory; obtain the last trajectory point of the inertial trajectory as the first trajectory point; temporally match the second trajectory point of the historical trajectory 0.3 seconds after the starting point; trim and delete all points before the second trajectory point in the historical trajectory, and use the remaining part as the historical trimmed trajectory; use the first trajectory point of the inertial trajectory and the second trajectory point of the historical trimmed trajectory as the splicing anchor points to re-join the two trajectories into a single trajectory.

[0099] In this implementation, in order to splice the inertial trajectory and the historical trajectory, considering that both are trajectories extending forward from the vehicle as the starting point, if the starting points of the two trajectories are aligned with the vehicle as the starting point, then there must be overlapping parts in the two trajectories. The purpose of the spatiotemporal alignment here is to find and cut out the overlapping parts of the two trajectories. Specifically, the spatiotemporal splicing method is used to cut out the trajectory between the starting point of the inertial trajectory and the first trajectory point; the second trajectory point closest to the first trajectory point of the inertial trajectory in the historical trajectory is obtained, and the trajectory between the second trajectory point of the historical trajectory and the end point of the historical trajectory is cut out; with the inertial trajectory as the starting trajectory and the historical trajectory as the subsequent trajectory, after aligning the first trajectory point and the second trajectory point in the global coordinate system, the inertial clipped trajectory is spliced ​​with the historical clipped trajectory to obtain a continuity-constrained trajectory. This continuity-constrained trajectory uses the inertial trajectory as the starting trajectory, which can improve the continuity between the starting path of the continuity-constrained trajectory and the vehicle's current motion state, avoiding drastic changes in the vehicle's heading angle; using the historical trajectory as the subsequent trajectory can improve the similarity between the overall continuity-constrained trajectory and the vehicle's historical driving trajectory, thereby ensuring the stability between the vehicle's multi-frame driving trajectories.

[0100] Furthermore, in one possible embodiment, the initial driving trajectory is planned based on preset trajectory evaluation indicators and the current frame state, using a traditional lattice trajectory planning algorithm or a dynamic programming algorithm, in which multiple trajectories are sampled in the drivable area of ​​the road. Then, based on a series of trajectory evaluation indicators such as minimum lateral displacement, no collision between the trajectory and obstacles, minimum trajectory jerk (acceleration rate of change), and maximum trajectory forward efficiency, an optimized path is selected as the initial driving trajectory. In traditional trajectory planning methods, the initial driving trajectory is directly subjected to subsequent secondary planning. However, in an embodiment of the present invention, the initial driving trajectory is spliced ​​with the continuity constraint trajectory before secondary planning is performed. Specifically:

[0101] In a typical autonomous driving path planning architecture, a sampling-based strategy is typically employed in the initial phase. Specifically, the algorithm systematically generates multiple candidate trajectories within the drivable area constrained by the dynamic environment ahead of the vehicle. These trajectories are generated using established planning paradigms, such as lattice sampling or dynamic programming.

[0102] The system then quantitatively evaluates these candidate trajectories based on a series of pre-defined, comprehensive evaluation metrics. Core evaluation criteria typically include, but are not limited to: lateral displacement, which must be minimized to ensure centered driving; collision risk, which must strictly adhere to the no-collision constraint; smoothness, which aims to minimize the rate of change of acceleration; and optimal driving efficiency, such as optimizing path length or time.

[0103] By comprehensively scoring and ranking all candidate trajectories based on these multiple dimensions, the algorithm ultimately selects an optimal path, which is the reference line output in this stage, i.e., the initial driving trajectory.

[0104] Furthermore, the spatiotemporal splicing of the continuity-constrained trajectory and the initial driving trajectory to obtain a final driving trajectory includes:

[0105] Acquiring a third trajectory point in the continuity-constrained trajectory based on a preset continuity constraint rule, wherein the continuity constraint rule is used to determine a splicing ratio between the continuity-constrained trajectory and the initial driving trajectory;

[0106] Obtaining a constraint trajectory starting point in the continuity constraint trajectory, and then obtaining a constraint clipping trajectory between the third trajectory point in the continuity constraint trajectory and the constraint trajectory starting point;

[0107] Acquire a fourth trajectory point in the initial driving trajectory that satisfies a preset spatiotemporal threshold with the third trajectory point;

[0108] Obtaining an end point of the driving trajectory in the initial driving trajectory, and then obtaining a driving clipping trajectory between the end point of the driving trajectory and the fourth trajectory point in the initial driving trajectory;

[0109] The constraint clipping trajectory and the driving clipping trajectory are spliced ​​based on the third trajectory point and the fourth trajectory point to obtain the final driving trajectory.

[0110] In this implementation, in order to splice the continuity constraint trajectory with the initial driving trajectory, considering that both are trajectories extending forward from the vehicle as the starting point, if the starting points of the two trajectories are aligned with the vehicle as the starting point, then there must be overlapping parts in the two trajectories. The purpose of doing spatiotemporal alignment here is to find and cut out the overlapping parts of the two trajectories. Specifically, using the spatiotemporal splicing method, the trajectory between the starting point of the continuity constraint trajectory and the third trajectory point is cut out; the fourth trajectory point closest to the third trajectory point in the initial driving trajectory is obtained, and the trajectory between the fourth trajectory point of the initial driving trajectory and the end point of the initial driving trajectory is cut out; using the continuity constraint trajectory as the starting trajectory and the initial driving trajectory as the subsequent trajectory, after aligning the third trajectory point and the fourth trajectory point in the global coordinate system, the constraint clipping trajectory and the driving clipping trajectory are spliced ​​to obtain the final driving trajectory. This final driving trajectory uses the continuity-constrained trajectory as its starting trajectory, enhancing the continuity between the starting path of the final driving trajectory and the vehicle's current motion state, as well as its similarity to the vehicle's historical driving trajectories. This prevents drastic changes in the vehicle's heading angle, improves the inter-frame continuity and stability of trajectory planning, and enhances autonomous driving safety. Furthermore, by using the initial driving trajectory planned by a traditional autonomous driving algorithm based on preset trajectory evaluation indicators and the vehicle's current frame state as the subsequent trajectory, the traditional autonomous driving algorithm maintains its ability to plan optimal paths for obstacle avoidance and other timely functions.

[0111] It should be noted that the continuity constraint rule is used to determine the splicing ratio of the continuity constraint trajectory and the initial driving trajectory. Specifically, intercepting more continuity constraint trajectories means that the final driving trajectory contains more inertial trajectory information and historical trajectory information. While improving the inter-frame continuity of the final driving trajectory, it will weaken the initial driving trajectory's predictive planning intention for the vehicle's journey to a certain extent. Therefore, the splicing ratio of the continuity constraint trajectory and the initial driving trajectory can be adjusted according to different vehicle driving scenarios and needs, and the inter-frame continuity and predictive planning intention of the final driving trajectory can be traded off. The splicing ratio of the continuity constraint trajectory can be adjusted by selecting the position of the third trajectory point in the continuity constraint trajectory.

[0112] In one possible embodiment, in a scenario where the vehicle pulls over, since the pull-over scenario does not involve complex journey planning such as lane changes and obstacle avoidance, the splicing ratio of continuity-constrained trajectories in the final driving trajectory can be increased to further improve the trajectory stability of the vehicle in the pull-over scenario.

[0113] In another possible embodiment, when a vehicle is changing lanes or circumventing obstacles on a highway, which involves complex, long-distance travel planning, the proportion of continuity-constrained trajectories in the final driving trajectory can be reduced. While ensuring minimum inter-frame continuity, the planning intent of the current frame trajectory in the initial driving trajectory can be improved, allowing the final driving trajectory to complete lane changes or obstacle avoidance more quickly.

[0114] Furthermore, the step of splicing the constraint clipping trajectory and the driving clipping trajectory based on the third trajectory point and the fourth trajectory point to obtain the final driving trajectory includes:

[0115] splicing the constraint clipping trajectory and the driving clipping trajectory based on the third trajectory point and the fourth trajectory point to obtain a first spliced ​​driving trajectory;

[0116] The first spliced ​​driving trajectory is smoothed to eliminate sudden changes in curvature at the third trajectory point and the fourth trajectory point, thereby obtaining a final driving trajectory.

[0117] In a possible embodiment, the first spliced ​​driving trajectory is smoothed by the conjugate gradient method. Specifically, the conjugate gradient method converts the non-smooth path of discrete trajectory points into a continuous smooth trajectory through an iterative algorithm, thereby eliminating the curvature mutation at the third trajectory point and the fourth trajectory point. As an efficient numerical solution method for symmetric positive definite matrix linear equations, the conjugate gradient method achieves rapid convergence by constructing a conjugate direction sequence, and is particularly suitable for the optimization of large sparse systems. In the trajectory processing scenario, the algorithm gradually reduces the residual deviation between the third trajectory point and the fourth trajectory point through multiple iterations, and finally converges to a smoothest path. Its convergence has been theoretically proven and experimentally verified to ensure the stability of the optimization results. Compared with the traditional gradient descent method, the conjugate gradient method used in this embodiment significantly improves the convergence speed through directional conjugation, while ensuring the physical rationality of the final driving trajectory.

[0118] In this implementation, after the first and second trajectory points are aligned in the global coordinate system and the constraint clipping trajectory and the driving clipping trajectory are spliced, the aligned parts of the third and fourth trajectory points are smoothed to eliminate the sudden change in curvature at the splicing point, which is conducive to faster convergence of the driving trajectory and a smoother vehicle driving planning route.

[0119] Furthermore, the smoothing process is performed on the first spliced ​​driving trajectory to eliminate the sudden changes in curvature at the third and fourth trajectory points to obtain a final driving trajectory, including:

[0120] Smoothing the first spliced ​​driving trajectory to eliminate sudden changes in curvature at the third and fourth trajectory points, thereby obtaining a second spliced ​​driving trajectory;

[0121] A quadratic programming algorithm is used to optimize the second spliced ​​driving trajectory based on preset driving boundary hard constraints and preset soft constraints to obtain a final driving trajectory.

[0122] Specifically, the process of using the quadratic programming algorithm to optimize the second spliced ​​driving trajectory based on preset driving boundary hard constraints and preset soft constraints to obtain the final driving trajectory is as follows:

[0123] Step 1: Input the initial solution. The second concatenated trajectory generated in the previous step is used as a high-quality initial solution for the quadratic programming algorithm. This guideline indicates the general direction and desired trajectory of the driving path.

[0124] Step 2: Construct the optimization problem. Based on the second spliced ​​trajectory and its surrounding environment, accurately construct the drivable boundary, typically represented as a convex space or corridor. Model the optimization objective as a quadratic objective function, which serves as a soft constraint on the second spliced ​​trajectory. Model constraints such as the drivable area boundary and the vehicle kinematic model as linear or quadratic equality or inequality constraints, which serve as hard constraints on the second spliced ​​trajectory.

[0125] Step 3: Solve using a quadratic programming algorithm. Within the feasible space defined by all hard constraints, find the trajectory parameters that minimize the objective function, including but not limited to the position, velocity, and acceleration of the trajectory points. The solution process fine-tunes the initial guidance lines: ensuring the trajectory is strictly within the safety boundary; checking and ensuring that the trajectory meets the acceleration rate constraint to ensure vehicle driving comfort; improving trajectory smoothness; and, while meeting the hard constraints, aligning as closely as possible with the initial desired path of the second spliced ​​driving trajectory.

[0126] After the quadratic programming algorithm solution of step 4 and step 3 successfully converges, the optimized trajectory is output as the final driving trajectory.

[0127] In this implementation, after smoothing the track joints, the second joint is used as a guideline and refined and optimized using a quadratic programming algorithm to ensure that it meets the hard constraints of the road boundary and the soft constraints such as jerk and smooth constraints. This results in a final trajectory that meets the requirements.

[0128] It should be noted that the present invention forms a guide line by splicing the inertial trajectory, historical trajectory, and the optimal trajectory given by the current lattice trajectory planning algorithm based on preset trajectory evaluation indicators to optimize the quadratic planning algorithm. The final generated trajectory naturally takes into account the connection between the vehicle's motion inertia and the planned trajectory, as well as the similarity between the current trajectory and the previous frame trajectory. It also takes into account the optimal planning path of the lattice trajectory planning algorithm, ensuring the stability of the vehicle while also considering the timeliness of functions such as obstacle avoidance.

[0129] It should be understood that the present invention is, in principle, equivalent to splicing a continuity-constrained trajectory for the initial driving trajectory generated by the lattice trajectory planning algorithm, and adding soft constraints of the inertial trajectory and the historical trajectory to the initial driving trajectory before the quadratic programming algorithm, so that the final expected optimization result after processing by the quadratic programming algorithm is as close as possible to the inertial trajectory and the historical trajectory within the feasible solution range. In this way, while improving the continuity of trajectory planning and the historical motion trend of the vehicle, it can also maintain the timeliness of the traditional autonomous driving algorithm in planning the optimal path for obstacle avoidance and other functions.

[0130] In one possible embodiment, a higher weight is assigned to the guide lines near the front of the vehicle in the second spliced ​​driving trajectory in the quadratic programming algorithm. This guide line includes the entire inertial trajectory and a portion of the historical trajectory. In this case, the quadratic programming algorithm inx will give greater consideration to the similarity between the front trajectory and the second spliced ​​driving trajectory during optimization, thereby improving the continuity between the initial portion of the final driving trajectory and the vehicle's inertial trajectory and historical trajectory, preventing sudden changes in the vehicle's heading angle during driving, and improving trajectory planning safety.

[0131] Furthermore, the vehicle dynamics model is expressed as follows:

[0132] ;

[0133] in, represents the vehicle's horizontal coordinate, represents the vehicle's longitudinal coordinate, represents the vehicle heading angle, Indicates the distance between the front wheels and the rear wheels of the vehicle. Indicates the vehicle speed, Indicates the vehicle's front wheel turning angle.

[0134] It should be noted that the aforementioned vehicle dynamics model is a bicycle model, which simplifies the vehicle into a two-wheeled structure. Based on the state equation of the vehicle dynamics model, the vehicle's heading angle at the next moment can be recursively inferred from the current vehicle speed and the current front wheel angle, thereby calculating the vehicle's horizontal and vertical position change rates at the next moment. Multiplying the horizontal and vertical change rates by the unit time dt, respectively, calculates the vehicle's inertial position at the next moment. Ultimately, by recursively inferring the vehicle's inertial position sequence over a certain future time interval, the vehicle's inertial trajectory is obtained.

[0135] In another possible embodiment, the vehicle's current lateral speed and longitudinal speed can be directly multiplied by the unit time dt, and the position coordinates of the vehicle at the current moment can be accumulated to finally deduce the vehicle's inertial position after a unit time. The lateral speed and longitudinal speed of the vehicle can be directly obtained by a preset speed sensor of the vehicle. Since one embodiment of the present invention only recursively calculates the vehicle's inertial trajectory within the next 0.3 seconds through the state equation, it only recursively calculates the inertial displacement for a very short period of time. The deviation between the result obtained by directly recursively calculating the vehicle's inertial trajectory through the vehicle's instantaneous lateral acceleration and instantaneous longitudinal acceleration and the complex vehicle dynamics model can be ignored. The recursive method and bicycle model used in the above embodiment have lower computational complexity and higher computational efficiency than the complex vehicle dynamics model.

[0136] like Figure 2 As shown, the second aspect of the present invention provides an autonomous driving vehicle trajectory planning system, which includes a dynamic model construction module 100, a vehicle state detection module 200, a continuity constraint module 300, an initial trajectory planning module 400 and a final trajectory splicing module 500, wherein:

[0137] The dynamic model building module 100 is used to build a vehicle dynamic model;

[0138] The vehicle state detection module 200 is used to obtain the current frame state and historical frame state of the vehicle;

[0139] The continuity constraint module 300 is used to obtain a continuity constraint trajectory based on the current frame state and the historical frame state according to the vehicle dynamics model;

[0140] The initial trajectory planning module 400 is used to plan the initial driving trajectory according to the preset trajectory evaluation index and the current frame state;

[0141] The final trajectory splicing module 500 is used to splice the continuity constraint trajectory with the initial driving trajectory in time and space to obtain a final driving trajectory.

[0142] The above-mentioned autonomous driving vehicle trajectory planning system, based on the traditional autonomous driving algorithm that plans the vehicle's driving trajectory based on preset trajectory evaluation indicators and the vehicle's current frame state, splices the initial driving trajectory with the continuity constraint trajectory to obtain the final driving trajectory. The continuity constraint trajectory is based on the vehicle dynamics model and is calculated according to the vehicle's current frame state and historical frame state. It reflects the vehicle's historical driving trajectory before the current moment, as well as the vehicle's dynamic trajectory at the next moment based on the current state. The continuity constraint trajectory is spliced ​​with the initial driving trajectory so that the final driving trajectory generated by the splicing improves the continuity with the vehicle's historical frame trajectory and the continuity with the vehicle's current motion trend, preventing the vehicle's planned trajectory from having a strong jump in motion direction between the previous frame and the next frame, thereby preventing the vehicle's trajectory from having a drastic change in heading angle, improving the inter-frame continuity and stability of the trajectory planning, and improving the safety of autonomous driving.

[0143] Since the final driving trajectory is formed by spatiotemporally splicing the continuity-constrained trajectory and the initial driving trajectory, it not only improves the continuity of trajectory planning and the vehicle's historical motion trends, but also maintains the timeliness of traditional autonomous driving algorithms in planning the optimal path for obstacle avoidance and other functions.

[0144] Furthermore, the obtaining of a continuity-constrained trajectory based on the current frame state and the historical frame state according to the vehicle dynamics model includes:

[0145] predicting an inertial trajectory based on the vehicle dynamics model and the current frame state;

[0146] Acquire a historical trajectory based on the historical frame state;

[0147] The inertial trajectory and the historical trajectory are spliced ​​in time and space to obtain the continuity constraint trajectory.

[0148] In this implementation, the continuity-constrained trajectory is composed of an inertial trajectory and a historical trajectory. The inertial trajectory is generated by recursively predicting the position sequence of the vehicle within a certain period of time in the future based on the state equation in the vehicle dynamics model and the current state data of the vehicle, and represents the current inertial trend of the vehicle's motion; splicing the inertial trajectory in the continuity-constrained trajectory can improve the continuity between the final driving trajectory and the vehicle's current motion state, and avoid drastic sudden changes in the vehicle's heading angle. In this implementation, the historical trajectory refers to the previous frame trajectory of the vehicle's current frame trajectory, which is obtained from the historical data of the vehicle's driving; splicing the historical trajectory in the continuity-constrained trajectory can improve the similarity between the final driving trajectory and the vehicle's historical driving trajectory, thereby ensuring the stability between the vehicle's multi-frame driving trajectories, avoiding drastic deviations in adjacent frame trajectory routes, and ultimately improving the safety of autonomous driving trajectory planning.

[0149] Furthermore, the spatiotemporal splicing of the inertial trajectory and the historical trajectory to obtain the continuity-constrained trajectory includes:

[0150] Obtaining a first trajectory point in the inertial trajectory;

[0151] Acquire an inertial trajectory starting point in the inertial trajectory, and then acquire an inertial clipping trajectory between the first trajectory point and the inertial trajectory starting point in the inertial trajectory;

[0152] Acquire a second trajectory point in the historical trajectory that satisfies a preset spatiotemporal threshold with the first trajectory point;

[0153] Obtaining an end point of the historical trajectory in the historical trajectory, and then obtaining a historical clipping trajectory between the end point of the historical trajectory and the second trajectory point in the historical trajectory;

[0154] The inertial clipping trajectory and the historical clipping trajectory are spliced ​​based on the first trajectory point and the second trajectory point to obtain the continuity constraint trajectory.

[0155] In this implementation, in order to splice the inertial trajectory and the historical trajectory, considering that both are trajectories extending forward from the vehicle as the starting point, if the starting points of the two trajectories are aligned with the vehicle as the starting point, then there must be overlapping parts in the two trajectories. The purpose of the spatiotemporal alignment here is to find and cut out the overlapping parts of the two trajectories. Specifically, the spatiotemporal splicing method is used to cut out the trajectory between the starting point of the inertial trajectory and the first trajectory point; the second trajectory point closest to the first trajectory point of the inertial trajectory in the historical trajectory is obtained, and the trajectory between the second trajectory point of the historical trajectory and the end point of the historical trajectory is cut out; with the inertial trajectory as the starting trajectory and the historical trajectory as the subsequent trajectory, after aligning the first trajectory point and the second trajectory point in the global coordinate system, the inertial clipped trajectory is spliced ​​with the historical clipped trajectory to obtain a continuity-constrained trajectory. This continuity-constrained trajectory uses the inertial trajectory as the starting trajectory, which can improve the continuity between the starting path of the continuity-constrained trajectory and the vehicle's current motion state, avoiding drastic changes in the vehicle's heading angle; using the historical trajectory as the subsequent trajectory can improve the similarity between the overall continuity-constrained trajectory and the vehicle's historical driving trajectory, thereby ensuring the stability between the vehicle's multi-frame driving trajectories.

[0156] The autonomous driving vehicle trajectory planning method and system provided by the present invention have at least the following advantages over the prior art:

[0157] The present invention predicts the vehicle's inertial motion trajectory for the next 0.2-0.5 seconds based on the vehicle's current frame state, including but not limited to vehicle speed, vehicle acceleration, and vehicle yaw angle. The present invention trims and splices the front segment of the inertial trajectory with the back segment of the previous frame's historical trajectory, achieving seamless splicing through a conjugate smoothing algorithm. The present invention assigns a higher weight to the front segment of the spliced ​​trajectory in quadratic programming to ensure the continuity of the starting path of the trajectory near the vehicle body with the previous frame's trajectory and the vehicle's motion trend, thereby improving the stability of the vehicle's driving trajectory.

[0158] Based on the present invention, the continuity-constrained trajectory and the initial driving trajectory are spliced ​​together in time and space to obtain a final driving trajectory. In scenarios where the trajectory changes during the startup phases such as lane changing and obstacle avoidance, the autonomous driving vehicle is more stable when steering because the starting segment of the final driving trajectory is continuous with the vehicle's historical frame trajectory and the vehicle's current motion trend. During the obstacle avoidance process, when the perception module's detection of a slightly distant obstacle fluctuates, even if the obstacle avoidance path is changed at the far end of the final driving trajectory, the vehicle trajectory remains stable because the starting segment of the final driving trajectory is continuous with the vehicle's historical frame trajectory and the vehicle's current motion trend. Even if the autonomous driving vehicle encounters interference from dynamic obstacles during the obstacle avoidance process, the trajectory of the front part of the vehicle remains stable, ensuring that the heading angle of the autonomous driving vehicle does not change suddenly, thereby improving the safety of autonomous driving.

[0159] Those skilled in the art will appreciate that all or part of the processes in the above-mentioned embodiments can be implemented by instructing the relevant hardware through a computer program. The computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the above-mentioned embodiments. In particular, any reference to memory, database, or other media used in the embodiments provided in this application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetic random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. By way of illustration and not limitation, RAM can take various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM). The databases involved in the various embodiments provided herein may include at least one of a relational database and a non-relational database. Non-relational databases may include, but are not limited to, distributed databases based on blockchains. The processors involved in the various embodiments provided herein may be, but are not limited to, general-purpose processors, central processing units (CPUs), graphics processing units (GPUs), digital signal processors (DSPs), programmable logic devices (PLDs), data processing logic devices based on quantum computing, and the like.

[0160] The "embodiment" mentioned in this document means that the specific features, structures or characteristics described in conjunction with the embodiment may be included in at least one embodiment of the present application. The appearance of this phrase in various places in the specification does not necessarily refer to the same embodiment, nor is it an independent or alternative embodiment that is mutually exclusive with other embodiments. It is explicitly and implicitly understood by those skilled in the art that the embodiments described herein can be combined with other embodiments. In order to make the description concise, not all possible combinations of the various technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0161] The above-described embodiments merely represent several implementation methods of the present application. While the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the present application. It should be noted that a person of ordinary skill in the art may make several improvements and substitutions without departing from the scope of the present application, and such improvements and substitutions should also be considered within the scope of protection of the present invention. Therefore, the scope of protection of the present application shall be determined by the appended claims.

Claims

1. A method for trajectory planning of an autonomous driving vehicle, characterized in that: include: Build a vehicle dynamics model and obtain the vehicle's current frame state and historical frame state; Predicting an inertial trajectory based on the vehicle dynamics model and the current frame state, wherein the inertial trajectory is generated by recursively predicting a position sequence of the vehicle within a certain period of time in the future based on the state equation in the vehicle dynamics model and the current frame state, and represents the current inertial trend of the vehicle; Acquire a historical trajectory based on the historical frame state; Obtaining a first trajectory point in the inertial trajectory; Acquire an inertial trajectory starting point in the inertial trajectory, and then acquire an inertial clipping trajectory between the first trajectory point and the inertial trajectory starting point in the inertial trajectory; Acquire a second trajectory point in the historical trajectory that satisfies a preset spatiotemporal threshold with the first trajectory point; Obtaining an end point of the historical trajectory in the historical trajectory, and then obtaining a historical clipping trajectory between the end point of the historical trajectory and the second trajectory point in the historical trajectory; splicing the inertial clipping trajectory and the historical clipping trajectory based on the first trajectory point and the second trajectory point to obtain a continuity constrained trajectory; Planning an initial driving trajectory based on a preset trajectory evaluation index and the current frame state; The continuity constraint trajectory and the initial driving trajectory are spatiotemporally spliced ​​to obtain a final driving trajectory.

2. The method for trajectory planning of an autonomous driving vehicle according to claim 1, wherein: The step of spatiotemporally splicing the continuity constraint trajectory with the initial driving trajectory to obtain a final driving trajectory includes: Acquiring a third trajectory point in the continuity-constrained trajectory based on a preset continuity constraint rule, wherein the continuity constraint rule is used to determine a splicing ratio between the continuity-constrained trajectory and the initial driving trajectory; Obtaining a constraint trajectory starting point in the continuity constraint trajectory, and then obtaining a constraint clipping trajectory between the third trajectory point in the continuity constraint trajectory and the constraint trajectory starting point; Acquire a fourth trajectory point in the initial driving trajectory that satisfies a preset spatiotemporal threshold with the third trajectory point; Obtaining an end point of the driving trajectory in the initial driving trajectory, and then obtaining a driving clipping trajectory between the end point of the driving trajectory and the fourth trajectory point in the initial driving trajectory; The constraint clipping trajectory and the driving clipping trajectory are spliced ​​based on the third trajectory point and the fourth trajectory point to obtain the final driving trajectory.

3. The method for trajectory planning of an autonomous driving vehicle according to claim 2, wherein: The step of splicing the constraint clipping trajectory and the driving clipping trajectory based on the third trajectory point and the fourth trajectory point to obtain the final driving trajectory includes: splicing the constraint clipping trajectory and the driving clipping trajectory based on the third trajectory point and the fourth trajectory point to obtain a first spliced ​​driving trajectory; The first spliced ​​driving trajectory is smoothed to eliminate sudden changes in curvature at the third trajectory point and the fourth trajectory point, thereby obtaining a final driving trajectory.

4. The method for trajectory planning of an autonomous driving vehicle according to claim 3, wherein: The smoothing process of the first spliced ​​driving trajectory to eliminate the sudden changes in curvature at the third and fourth trajectory points to obtain the final driving trajectory includes: Smoothing the first spliced ​​driving trajectory to eliminate sudden changes in curvature at the third and fourth trajectory points, thereby obtaining a second spliced ​​driving trajectory; A quadratic programming algorithm is used to optimize the second spliced ​​driving trajectory based on preset driving boundary hard constraints and preset soft constraints to obtain a final driving trajectory.

5. The method for trajectory planning of an autonomous driving vehicle according to claim 1, wherein: The expression of the vehicle dynamics model is as follows: Among them, X represents the horizontal coordinate of the vehicle, Y represents the vertical coordinate of the vehicle, represents the vehicle heading angle, L represents the distance between the front wheel and the rear wheel, v represents the vehicle speed, δ f Indicates the vehicle's front wheel turning angle.

6. An autonomous driving vehicle trajectory planning system, characterized in that: It includes a dynamic model construction module, a vehicle state detection module, a continuity constraint module, an initial trajectory planning module, and a final trajectory splicing module, among which: The dynamics model building module is used to build a vehicle dynamics model; The vehicle state detection module is used to obtain the current frame state and historical frame state of the vehicle; The continuity constraint module is used to obtain a continuity constraint trajectory based on the vehicle dynamics model and the current frame state and the historical frame state, specifically including: Predicting an inertial trajectory based on the vehicle dynamics model and the current frame state, wherein the inertial trajectory is generated by recursively predicting a position sequence of the vehicle within a certain period of time in the future based on the state equation in the vehicle dynamics model and the current frame state, and represents the current inertial trend of the vehicle; Acquire a historical trajectory based on the historical frame state; Obtaining a first trajectory point in the inertial trajectory; Acquire an inertial trajectory starting point in the inertial trajectory, and then acquire an inertial clipping trajectory between the first trajectory point and the inertial trajectory starting point in the inertial trajectory; Acquire a second trajectory point in the historical trajectory that satisfies a preset spatiotemporal threshold with the first trajectory point; Obtaining an end point of the historical trajectory in the historical trajectory, and then obtaining a historical clipping trajectory between the end point of the historical trajectory and the second trajectory point in the historical trajectory; splicing the inertial clipping trajectory and the historical clipping trajectory based on the first trajectory point and the second trajectory point to obtain the continuity constraint trajectory; The initial trajectory planning module is used to plan the initial driving trajectory according to the preset trajectory evaluation index and the current frame state; The final trajectory splicing module is used to splice the continuity constraint trajectory with the initial driving trajectory in time and space to obtain a final driving trajectory.

Images