Automatic driving vehicle trajectory planning method and system

Abstract

The invention relates to the technical field of automatic driving, in particular to an automatic driving vehicle trajectory planning method and system. According to the invention, on the basis of planning the vehicle driving track based on the preset track evaluation index and the current frame state of the vehicle in the traditional automatic driving algorithm, the initial driving track and the continuous constraint track are spliced to obtain the final driving track. The continuity constraint trajectory is calculated based on a vehicle dynamics model according to the current frame state and the historical frame state of the vehicle, and reflects the historical driving trajectory of the vehicle before the current moment and the dynamics trajectory of the vehicle at the next moment based on the current state. And the continuity constraint track and the initial driving track are spliced, so that the final driving track generated through splicing is improved in continuity with the historical frame track of the vehicle and continuity with the current movement trend of the vehicle, the situation that the vehicle planning track has violent course angle sudden change between the previous frame and the next frame is prevented, and the risk of sudden braking or sudden steering is prevented.

Description

Technical Field

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

[0002] Currently, the driving trajectory planning system of autonomous driving vehicles generally adopts a modular hierarchical architecture, that is, a hierarchical design of independent modules such as environment perception, behavior prediction, decision-making, and trajectory planning is executed in sequence. Under this architecture, the path planner generates a local trajectory frame by frame based on real-time perception data to guide vehicle motion control.

[0003] However, the above traditional path planning methods have significant limitations. On the one hand, mainstream autonomous driving vehicle driving trajectory planners such as the Lattice sampling method and the quadratic programming optimizer focus on the geometric smoothness of the vehicle's single-frame driving trajectory, but ignore the motion continuity constraint between adjacent frame trajectories. This results in the vehicle's planned trajectory being prone to severe heading angle mutations, causing high-frequency steering wheel jitter, triggering risks of sudden braking or sharp turning, and reducing the driving stability and safety of the vehicle. On the other hand, existing methods mainly rely on static hard boundary conditions such as collision boundary constraints for the trajectory planner 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 suddenly changes, the trajectories of adjacent frames generated by the planner may deviate severely, forcing the vehicle to perform unnecessary emergency braking or aggressive steering operations, triggering risks of sudden braking or sharp turning, and significantly increasing the uncertainty and safety risks of motion control.

[0004] Therefore, how to avoid severe heading angle mutations in the trajectory of an autonomous driving vehicle, triggering 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 trajectory planning of an autonomous driving vehicle to improve the continuity and stability of the trajectory planning of the autonomous driving vehicle, and solve the technical problems that the trajectory of the autonomous driving vehicle is prone to severe heading angle mutations, triggering unnecessary emergency braking or aggressive steering operations.

[0006] To achieve the above object, in the first aspect of the present invention, a method for trajectory planning of an autonomous driving vehicle is provided, including the following steps: Construct a vehicle dynamics model, and obtain the current frame state and historical frame state of the vehicle; Based on the vehicle dynamics model, obtain a continuous constraint trajectory according to the current frame state and the historical frame state; Plan an initial driving trajectory based on a preset trajectory evaluation index and the current frame state; Perform spatio-temporal stitching on the continuous constraint trajectory and the initial driving trajectory to obtain the final driving trajectory.

[0007] The above-mentioned autonomous vehicle trajectory planning method stitches the initial driving trajectory and the continuous constraint trajectory on the basis of the traditional autonomous driving algorithm planning the vehicle driving trajectory based on the preset trajectory evaluation index and the vehicle current frame state to obtain the final driving trajectory. The continuous constraint trajectory is calculated based on the vehicle dynamics model according to the vehicle current frame state and the historical frame state, reflecting the historical driving trajectory of the vehicle before the current moment and the dynamic trajectory of the vehicle at the next moment based on the current state. Stitching the continuous constraint trajectory with the initial driving trajectory improves the continuity of the stitched final driving trajectory with the vehicle historical frame trajectory and the continuity with the current motion trend of the vehicle, preventing a strong jump in the motion direction between the previous frame and the next frame of the vehicle planned trajectory, thereby preventing a sharp change in the heading angle of the vehicle trajectory, improving the inter-frame continuity and stability of the trajectory planning, and enhancing the safety of autonomous driving.

[0008] Since the obtained final driving trajectory is formed by spatio-temporal stitching of the continuous constraint trajectory and the initial driving trajectory, it can not only improve the continuity of the trajectory planning with the vehicle historical motion trend, but also maintain the timeliness of the traditional autonomous driving algorithm to plan the best path for obstacle avoidance and other functions.

[0009] Further, obtaining the continuous constraint trajectory according to the current frame state and the historical frame state based on the vehicle dynamics model includes: Predict the inertial trajectory based on the vehicle dynamics model and the current frame state; Obtain the historical trajectory based on the historical frame state; Perform spatio-temporal stitching on the inertial trajectory and the historical trajectory to obtain the continuous constraint trajectory.

[0010] In this implementation, the continuous constraint trajectory is formed by stitching the inertial trajectory and the historical trajectory. The inertial trajectory is generated by recursively predicting the position sequence of the vehicle in the future for a certain period of time based on the state equation in the vehicle dynamics model and the vehicle current state data, representing the current motion inertial trend of the vehicle; stitching the inertial trajectory in the continuous constraint trajectory can improve the continuity of the final driving trajectory with the current motion state of the vehicle and avoid a sharp change in the heading angle of the vehicle. The historical trajectory refers to the previous frame trajectory of the vehicle current frame trajectory in this implementation and is obtained from the historical data of the vehicle driving; stitching the historical trajectory in the continuous constraint trajectory can improve the similarity of the final driving trajectory with the vehicle historical driving trajectory, thereby ensuring the stability between multiple frame driving trajectories of the vehicle, avoiding a sharp deviation of the adjacent frame trajectory routes, and ultimately enhancing the safety of autonomous driving trajectory planning.

[0011] Further, the step of performing spatio-temporal splicing on the inertial trajectory and the historical trajectory to obtain the continuous constraint trajectory includes: Obtain a first trajectory point in the inertial trajectory; Obtain the starting point of the inertial trajectory in the inertial trajectory, and then obtain the inertial clipped trajectory between the first trajectory point and the starting point of the inertial trajectory in the inertial trajectory; Obtain a second trajectory point in the historical trajectory that satisfies a preset spatio-temporal threshold with the first trajectory point; Obtain the ending point of the historical trajectory in the historical trajectory, and then obtain the historical clipped trajectory between the ending point of the historical trajectory and the second trajectory point in the historical trajectory; Based on the first trajectory point and the second trajectory point, splice the inertial clipped trajectory and the historical clipped trajectory to obtain the continuous constraint trajectory.

[0012] 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 an overlapping part between the two trajectories. Performing spatio-temporal alignment here is to find and cut off the overlapping part of the two trajectories. Specifically, using the method of spatio-temporal splicing, cut out the trajectory between the starting point of the inertial trajectory and the first trajectory point; obtain the second trajectory point in the historical trajectory that is closest to the first trajectory point of the inertial trajectory, and cut out the trajectory between the second trajectory point of the historical trajectory and the ending point of the historical trajectory; taking 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, splice the inertial clipped trajectory and the historical clipped trajectory to obtain the continuous constraint trajectory. The continuous constraint trajectory takes the inertial trajectory as the starting trajectory, which can improve the continuity between the starting path of the continuous constraint trajectory and the current motion state of the vehicle, and avoid sudden changes in the heading angle of the vehicle; taking the historical trajectory as the subsequent trajectory can improve the similarity between the overall continuous constraint trajectory and the historical driving trajectory of the vehicle, thereby ensuring the stability between multiple frames of the vehicle's driving trajectory.

[0013] Further, the step of performing spatio-temporal splicing on the continuous constraint trajectory and the initial driving trajectory to obtain the final driving trajectory includes: Obtain a third trajectory point in the continuous constraint trajectory based on a preset continuous constraint rule, where the continuous constraint rule is used to determine the splicing ratio between the continuous constraint trajectory and the initial driving trajectory; Obtain the starting point of the constraint trajectory in the continuous constraint trajectory, and then obtain the constraint clipped trajectory between the third trajectory point and the starting point of the constraint trajectory in the continuous constraint trajectory; Obtain a fourth trajectory point in the initial driving trajectory that satisfies a preset spatio-temporal threshold with the third trajectory point; Obtain the end point of the driving trajectory in the initial driving trajectory, and then obtain the driving clipped trajectory between the end point of the driving trajectory and the fourth trajectory point in the initial driving trajectory; Based on the third trajectory point and the fourth trajectory point, splice the constrained clipped trajectory and the driving clipped trajectory to obtain the final driving trajectory.

[0014] In this implementation manner, in order to splice the continuous constraint trajectory and the initial driving trajectory, considering that both are trajectories extending forward with 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 an overlapping part between the two trajectories. Performing spatio-temporal alignment here is to find out and clip off the overlapping part of the two trajectories. Specifically, use the spatio-temporal splicing method to clip out the trajectory between the starting point of the continuous constraint trajectory and the third trajectory point; obtain the fourth trajectory point in the initial driving trajectory that is closest to the third trajectory point, and clip out the trajectory between the fourth trajectory point of the initial driving trajectory and the end point of the initial driving trajectory; use the continuous 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, realize splicing the constrained clipped trajectory and the driving clipped trajectory to obtain the final driving trajectory. This final driving trajectory uses the continuous constraint trajectory as the starting trajectory, which can improve the continuity between the starting path of the final driving trajectory and the current motion state of the vehicle, as well as the similarity with the vehicle's historical driving trajectory, thereby preventing the vehicle trajectory from having a drastic heading angle mutation, improving the inter-frame continuity and stability of the trajectory planning, and enhancing the safety of autonomous driving. At the same time, using the initial driving trajectory planned by the traditional autonomous driving algorithm based on the preset trajectory evaluation index and the current frame state of the vehicle as the subsequent trajectory can maintain the timeliness of the traditional autonomous driving algorithm to plan the best path for obstacle avoidance and other functions.

[0015] It should be noted that the continuous constraint rule is used to determine the splicing ratio of the continuous constraint trajectory and the initial driving trajectory. Specifically, intercepting more continuous 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, to a certain extent, it will weaken the prediction and planning intention of the initial driving trajectory for the vehicle's itinerary. Therefore, the splicing ratio of the continuous constraint trajectory and the initial driving trajectory can be adjusted according to different vehicle driving scenarios and requirements to make a trade-off between the inter-frame continuity and the prediction and planning intention of the final driving trajectory.

[0016] Further, the step of splicing the constrained clipped trajectory and the driving clipped trajectory based on the third trajectory point and the fourth trajectory point to obtain the final driving trajectory includes: Stitch the constrained trimmed trajectory and the driving trimmed trajectory based on the third trajectory point and the fourth trajectory point to obtain a first stitched driving trajectory; Smooth the first stitched driving trajectory to eliminate the curvature mutation at the third trajectory point and the fourth trajectory point, thereby obtaining a final driving trajectory.

[0017] In this implementation manner, after aligning the first trajectory point and the second trajectory point in the global coordinate system and completing the stitching of the constrained trimmed trajectory and the driving trimmed trajectory, smooth the alignment part of the third trajectory point and the fourth trajectory point, so as to eliminate the curvature mutation at the stitching position, which is beneficial to the faster convergence of the driving trajectory and obtain a smoother vehicle driving planning route.

[0018] Further, the step of smoothing the first stitched driving trajectory to eliminate the curvature mutation at the third trajectory point and the fourth trajectory point to obtain a final driving trajectory includes: Smooth the first stitched driving trajectory to eliminate the curvature mutation at the third trajectory point and the fourth trajectory point, thereby obtaining a second stitched driving trajectory; Adopt a quadratic programming algorithm to optimize the second stitched driving trajectory based on the preset hard constraints of the driving boundary and the preset soft constraints to obtain a final driving trajectory.

[0019] In this implementation manner, after completing the smoothing process at the trajectory stitching position, use the second stitched driving trajectory as a guiding line, and adopt a quadratic programming algorithm to refine and optimize the second stitched driving trajectory, so that it meets the hard constraints of the preset driving road boundary and the preset soft constraints such as jerk (acceleration change rate) constraint and smoothing constraint, and finally obtain a final driving trajectory that meets the requirements.

[0020] Further, the expression of the vehicle dynamics model is as follows: ; Wherein, represents the vehicle's abscissa, represents the vehicle's ordinate, represents the vehicle's heading angle, represents the distance between the vehicle's front wheels and the vehicle's rear wheels, represents the vehicle's speed, represents the vehicle's front wheel steering angle.

[0021] In this implementation manner, a vehicle dynamics model is constructed. Based on the state equation of the vehicle dynamics model, the vehicle's heading angle at the next moment can be deduced from the current vehicle speed and the current vehicle's front wheel steering angle, so as to calculate the vehicle's inertial position at the next moment. Finally, by deducing the inertial position sequence of the vehicle within a certain future time interval, the vehicle's inertial trajectory is obtained.

[0022] In a second aspect of the present invention, there is provided an automatic 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 stitching module, wherein: The dynamic model construction module is used to construct a vehicle dynamic 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 dynamic model, based on the current frame state and the historical frame state; The initial trajectory planning module is used to plan an initial driving trajectory according to a preset trajectory evaluation index and the current frame state; The final trajectory stitching module is used to perform spatio-temporal stitching on the continuity constraint trajectory and the initial driving trajectory to obtain a final driving trajectory.

[0023] Based on the traditional automatic driving algorithm that plans the vehicle driving trajectory according to a preset trajectory evaluation index and the current frame state of the vehicle, the above automatic driving vehicle trajectory planning system stitches the initial driving trajectory and the continuity constraint trajectory to obtain a final driving trajectory. The continuity constraint trajectory is calculated based on the vehicle dynamic model according to the current frame state and the historical frame state of the vehicle, and 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. Stitching the continuity constraint trajectory and the initial driving trajectory makes the generated final driving trajectory improve the continuity with the vehicle historical frame trajectory and the continuity with the current motion trend of the vehicle, preventing a strong jump in the motion direction between the previous frame and the next frame of the vehicle planned trajectory, thereby preventing a sharp change in the heading angle of the vehicle trajectory, improving the inter-frame continuity and stability of the trajectory planning, and enhancing the safety of automatic driving.

[0024] Since the obtained final driving trajectory is formed by spatio-temporal stitching of the continuity constraint trajectory and the initial driving trajectory, it can not only improve the continuity of the trajectory planning with the historical motion trend of the vehicle, but also maintain the timeliness of the function of planning the best path for obstacle avoidance and the like by the traditional automatic driving algorithm.

[0025] Further, the obtaining of the continuity constraint trajectory based on the vehicle dynamic model, based on the current frame state and the historical frame state, includes: Predicting an inertial trajectory based on the vehicle dynamic model and the current frame state; Obtaining a historical trajectory based on the historical frame state; Performing spatio-temporal stitching on the inertial trajectory and the historical trajectory to obtain the continuity constraint trajectory.

[0026] In this implementation manner, the continuous constraint trajectory is formed by splicing an inertial trajectory and a historical trajectory. The inertial trajectory is generated by recursively predicting a sequence of positions 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, representing the current motion inertia trend of the vehicle; splicing the inertial trajectory in the continuous constraint trajectory can improve the continuity between the final driving trajectory and the current motion state of the vehicle, and avoid sudden sharp changes in the heading angle of the vehicle. The historical trajectory refers to the previous frame trajectory of the current frame trajectory of the vehicle in this implementation manner, and is obtained from the historical data of the vehicle's driving; splicing the historical trajectory in the continuous constraint trajectory can improve the similarity between the final driving trajectory and the vehicle's historical driving trajectory, thereby ensuring the stability between multiple frame driving trajectories of the vehicle, avoiding sharp offsets between adjacent frame trajectory routes, and ultimately improving the safety of autonomous driving trajectory planning.

[0027] Further, the step of splicing the inertial trajectory and the historical trajectory in space-time to obtain the continuous constraint trajectory includes: Obtain a first trajectory point in the inertial trajectory; Obtain the starting point of the inertial trajectory in the inertial trajectory, and further obtain the inertial clipped trajectory between the first trajectory point and the starting point of the inertial trajectory in the inertial trajectory; Obtain a second trajectory point in the historical trajectory that satisfies a preset space-time threshold with the first trajectory point; Obtain the end point of the historical trajectory in the historical trajectory, and further obtain the historical clipped trajectory between the end point of the historical trajectory and the second trajectory point in the historical trajectory; Based on the first trajectory point and the second trajectory point, splice the inertial clipped trajectory and the historical clipped trajectory to obtain the continuous constraint trajectory.

[0028] 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, there will inevitably be an overlapping part between the two trajectories. Here, the spatio-temporal alignment is to find out and cut off the overlapping part of the two trajectories. Specifically, using the spatio-temporal splicing method, cut off the trajectory between the starting point of the inertial trajectory and the first trajectory point; obtain the second trajectory point in the historical trajectory that is closest to the first trajectory point of the inertial trajectory, and cut off the trajectory between the second trajectory point of the historical trajectory and the end point of the historical trajectory; take 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, splice the inertial cut-off trajectory and the historical cut-off trajectory to obtain a continuous constraint trajectory. This continuous constraint trajectory takes the inertial trajectory as the starting trajectory, which can improve the continuity between the starting path of the continuous constraint trajectory and the current motion state of the vehicle, and avoid sudden changes in the heading angle of the vehicle; taking the historical trajectory as the subsequent trajectory can improve the similarity between the overall continuous constraint trajectory and the historical driving trajectory of the vehicle, thereby ensuring the stability between multiple frames of the vehicle's driving trajectory. Brief Description of the Drawings

[0029] Figure 1 is a schematic flowchart of a method for trajectory planning of an autonomous driving vehicle provided by an embodiment of the present invention; Figure 2 is a schematic structural diagram of a system for trajectory planning of an autonomous driving vehicle provided by an embodiment of the present invention. Detailed Embodiments

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

[0031] It should be understood that although the steps in the flowchart of the accompanying drawings are shown sequentially in the direction of the arrows, these steps are not necessarily executed sequentially in the order indicated by the arrows. Unless otherwise clearly stated in this document, there is no strict order restriction for the execution of these steps, and they can be executed in other orders. Moreover, at least a part of the steps in the flowchart of the accompanying drawings may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily executed at the same time, but can be executed at different times, and their execution order is not necessarily sequential, but can be executed alternately or in turn with at least a part of other steps or sub-steps or stages of other steps.

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

[0033] 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 splicing continuous constraint trajectories and initial driving trajectories to perform inter-frame trajectory continuity constraint on the initial driving trajectory. The planning module of the current autonomous driving system usually adopts a modular architecture, that is, a hierarchical design of road condition perception, vehicle driving state prediction, vehicle driving plan decision-making, and vehicle driving trajectory planning. By real-time perceiving the road condition environment, a single-frame driving trajectory is generated based on the predicted future state of the vehicle. However, the above traditional methods have the following limitations: 1) Insufficient temporal continuity. Traditional path planners, such as Lattice Planner (trajectory planning algorithm) and QP optimization (Quadratic Programming), only focus on the trajectory smoothness within a single-frame driving trajectory generated based on the predicted future state of the vehicle, ignoring the inter-frame continuity between the current frame and the previous frame, which is likely to cause large changes in the routes of the previous frame and the next frame, resulting in sudden changes in the vehicle's heading angle and steering wheel jitter, increasing the danger of vehicle driving.

[0034] 2) Poor adaptability to dynamic scenarios. In the scenario of planning an obstacle avoidance path by traditional path planning methods, the trajectories of the front and rear frames may have drastic offsets due to obstacle prediction errors or environmental mutations, triggering risks of sudden braking or sudden steering. The trajectory mutation may exceed the vehicle dynamics limit, increasing the danger of vehicle driving.

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

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

[0037] To solve the above technical problems, referring to Figure 1 , the first aspect of the embodiment of the present invention provides an autonomous vehicle trajectory planning method, including the following steps: S101. Construct a vehicle dynamics model, and obtain the current frame state and historical frame state of the vehicle; S102. Based on the vehicle dynamics model, obtain a continuity constraint trajectory according to the current frame state and the historical frame state; S103. Plan an initial driving trajectory based on a preset trajectory evaluation index and the current frame state; S104. Perform spatio-temporal stitching on the continuity constraint trajectory and the initial driving trajectory to obtain a final driving trajectory.

[0038] In the above autonomous vehicle trajectory planning method, on the basis that the traditional autonomous driving algorithm plans the vehicle driving trajectory based on a preset trajectory evaluation index and the current frame state of the vehicle, the initial driving trajectory and the continuity constraint trajectory are stitched together to obtain a final driving trajectory. The continuity constraint trajectory is calculated based on the vehicle dynamics model according to the current frame state and the historical frame state of the vehicle, and reflects the historical driving trajectory of the vehicle before the current moment, as well as the dynamic trajectory of the vehicle based on the current state at the next moment. Stitching the continuity constraint trajectory with the initial driving trajectory makes the finally generated driving trajectory improve the continuity with the vehicle historical frame trajectory and the continuity with the current motion trend of the vehicle, preventing a strong jump in the motion direction of the vehicle planning trajectory between the previous frame and the next frame, thereby preventing a sharp change in the heading angle of the vehicle trajectory, improving the frame-to-frame continuity and stability of the trajectory planning, and enhancing the safety of autonomous driving.

[0039] Since the obtained final driving trajectory is formed by spatio-temporal stitching of the continuity constraint trajectory and the initial driving trajectory, it can not only improve the continuity of the trajectory planning with the historical motion trend of the vehicle, but also maintain the timeliness of the traditional autonomous driving algorithm to plan the best path for obstacle avoidance and other functions.

[0040] Further, the obtaining of the continuity constraint trajectory based on the vehicle dynamics model according to the current frame state and the historical frame state includes: Predict an inertial trajectory based on the vehicle dynamics model and the current frame state; Obtain a historical trajectory based on the historical frame state; Perform spatio-temporal stitching on the inertial trajectory and the historical trajectory to obtain the continuity constraint trajectory.

[0041] Specifically, in a possible embodiment, based on the state equation of the vehicle dynamics model, the position sequence of the vehicle within the next 0.3 seconds is recursively derived, and then the inertial trajectory of the vehicle within the next 0.3 seconds is obtained. The previous frame trajectory of the vehicle at the current moment is obtained based on the historical information of the vehicle as the historical trajectory of the vehicle.

[0042] It should be noted that since the inertial trajectory is mainly used to reflect the inertial motion trend of the vehicle at the current moment, only the inertial trajectory for a short period of time needs to be recursively derived forward. The historical trajectory is a record of the trajectory planning within a relatively long distance in the previous frame of the vehicle, usually with a length of 150 meters to 200 meters. Therefore, the length of the inertial trajectory is usually shorter than that of the historical trajectory.

[0043] In this implementation, the continuous constraint trajectory is formed by splicing the inertial trajectory and the 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, representing the current motion inertia trend of the vehicle; splicing the inertial trajectory in the continuous constraint trajectory can improve the continuity between the final driving trajectory and the current motion state of the vehicle, and avoid sudden changes in the heading angle of the vehicle. The historical trajectory in this implementation refers to the previous frame trajectory of the current frame trajectory of the vehicle, which is obtained from the historical data of the vehicle's driving; splicing the historical trajectory in the continuous constraint trajectory can improve the similarity between the final driving trajectory and the historical driving trajectory of the vehicle, thereby ensuring the stability between multiple frame driving trajectories of the vehicle, avoiding drastic deviations in the adjacent frame trajectory routes, and ultimately improving the safety of the autonomous driving trajectory planning.

[0044] Further, the step of splicing the inertial trajectory and the historical trajectory in space-time to obtain the continuous constraint trajectory includes: Obtain the first trajectory point in the inertial trajectory; Obtain the starting point of the inertial trajectory in the inertial trajectory, and then obtain the inertial clipped trajectory between the first trajectory point and the starting point of the inertial trajectory in the inertial trajectory; Obtain the second trajectory point in the historical trajectory that satisfies the preset space-time threshold with the first trajectory point; Obtain the end point of the historical trajectory in the historical trajectory, and then obtain the historical clipped trajectory between the end point of the historical trajectory and the second trajectory point in the historical trajectory; Based on the first trajectory point and the second trajectory point, splice the inertial clipped trajectory and the historical clipped trajectory to obtain the continuous constraint trajectory.

[0045] It should be noted that after aligning the starting points of the inertial trajectory and the historical trajectory, the preset spatio-temporal threshold can be the second trajectory point closest to the first trajectory point, or: 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.

[0046] Specifically, in a possible embodiment, after obtaining the inertial trajectory within the next 0.3 seconds of the vehicle and the complete historical frame trajectory of the previous frame of the vehicle, there are two ways of clipping and splicing: 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; delete all the points before the second trajectory point in the historical trajectory, and the remaining part is used as the historical clipped trajectory; use the first trajectory point of the inertial trajectory and the second trajectory point of the historical clipped trajectory as the splicing anchor points to splice the two trajectories into one trajectory.

[0047] 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; match the second trajectory point in the historical trajectory that is 0.3 seconds after the starting point in terms of time; delete all the points before the second trajectory point in the historical trajectory, and the remaining part is used as the historical clipped trajectory; use the first trajectory point of the inertial trajectory and the second trajectory point of the historical clipped trajectory as the splicing anchor points to splice the two trajectories into one trajectory.

[0048] In this implementation method, 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 an overlapping part between the two trajectories. Here, spatio-temporal alignment is to find and clip out the overlapping part of the two trajectories. Specifically, using the method of spatio-temporal splicing, clip out the trajectory between the starting point of the inertial trajectory and the first trajectory point; obtain the second trajectory point in the historical trajectory that is closest to the first trajectory point of the inertial trajectory, and clip out the trajectory between the second trajectory point of the historical trajectory and the end point of the historical trajectory; use 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, splice the inertial clipped trajectory and the historical clipped trajectory to obtain a continuous constraint trajectory. This continuous constraint trajectory uses the inertial trajectory as the starting trajectory, which can improve the continuity between the starting path of the continuous constraint trajectory and the current motion state of the vehicle, and avoid sudden changes in the heading angle of the vehicle; using the historical trajectory as the subsequent trajectory can improve the similarity between the overall continuous constraint trajectory and the historical driving trajectory of the vehicle, thus ensuring the stability between multiple frames of the vehicle's driving trajectory.

[0049] Further, in a possible embodiment, the initial driving trajectory is planned based on the preset trajectory evaluation index and the current frame state. The method samples multiple trajectories in the drivable area of the road according to the traditional lattice trajectory planning algorithm or the dynamic programming algorithm, and then selects an optimal path as the initial driving trajectory according to a series of trajectory evaluation indexes such as the minimum lateral displacement, no collision between the trajectory and obstacles, the minimum trajectory jerk (acceleration change rate), and the highest trajectory forward efficiency. In the traditional trajectory planning method, the initial driving trajectory directly undergoes subsequent secondary planning. However, in the embodiment of the present invention, the initial driving trajectory is spliced with the continuous constraint trajectory and then undergoes secondary planning. Specifically: In a typical autonomous driving path planning architecture, a sampling-based strategy is usually adopted in the initial stage. Specifically, the algorithm systematically generates multiple candidate trajectories within the drivable area constrained by the dynamic environment in front of the vehicle. The generation of these trajectories follows industry-recognized planning paradigms, such as the Lattice sampling algorithm or the Dynamic Programming algorithm.

[0050] Subsequently, the system quantitatively evaluates these candidate trajectories based on a series of preset and comprehensive evaluation indexes. The core evaluation dimensions usually include but are not limited to: the lateral displacement of the trajectory, which is required to be minimized to ensure centering driving; the trajectory collision risk, which must strictly meet the collision-free constraint; the trajectory motion smoothness, with the goal of minimizing the acceleration change rate; and the highest trajectory driving efficiency, such as the path length or time optimization.

[0051] By comprehensively scoring and ranking all candidate trajectories in the above multi-dimensions, the algorithm finally selects an optimal path, which is the reference line (Reference Line) output in this stage, that is, the initial driving trajectory.

[0052] Further, the space-time splicing of the continuous constraint trajectory and the initial driving trajectory to obtain the final driving trajectory includes: Obtaining the third trajectory point in the continuous constraint trajectory based on the preset continuous constraint rule, where the continuous constraint rule is used to determine the splicing ratio of the continuous constraint trajectory and the initial driving trajectory; Obtaining the starting point of the constraint trajectory in the continuous constraint trajectory, and further obtaining the constrained trimmed trajectory between the third trajectory point and the starting point of the constraint trajectory in the continuous constraint trajectory; Obtaining the fourth trajectory point in the initial driving trajectory that satisfies the preset space-time threshold with the third trajectory point; Obtaining the end point of the driving trajectory in the initial driving trajectory, and further obtaining the driving trimmed trajectory between the end point of the driving trajectory and the fourth trajectory point in the initial driving trajectory; Splice the constrained trimmed trajectory and the driving trimmed trajectory based on the third trajectory point and the fourth trajectory point to obtain the final driving trajectory.

[0053] In this implementation, in order to splice the continuous constraint trajectory and 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, there must be an overlapping part between the two trajectories. Here, the spatio-temporal alignment is to find out and trim the overlapping part of the two trajectories. Specifically, use the spatio-temporal splicing method to trim the trajectory between the starting point of the continuous constraint trajectory and the third trajectory point; obtain the fourth trajectory point in the initial driving trajectory that is closest to the third trajectory point, and trim the trajectory between the fourth trajectory point of the initial driving trajectory and the end point of the initial driving trajectory; take the continuous 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, splice the constrained trimmed trajectory and the driving trimmed trajectory to obtain the final driving trajectory. The final driving trajectory takes the continuous constraint trajectory as the starting trajectory, which can improve the continuity between the starting path of the final driving trajectory and the current motion state of the vehicle, as well as the similarity with the vehicle's historical driving trajectory, thereby preventing sudden changes in the heading angle of the vehicle trajectory, improving the inter-frame continuity and stability of the trajectory planning, and enhancing the safety of autonomous driving. At the same time, taking the initial driving trajectory planned by the traditional autonomous driving algorithm based on the preset trajectory evaluation index and the current frame state of the vehicle as the subsequent trajectory can maintain the timeliness of the traditional autonomous driving algorithm to plan the best path for obstacle avoidance and other functions.

[0054] It should be noted that the continuous constraint rule is used to determine the splicing ratio of the continuous constraint trajectory and the initial driving trajectory. Specifically, intercepting more continuous 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, to a certain extent, it will weaken the prediction and planning intention of the initial driving trajectory for the vehicle's itinerary. Therefore, the splicing ratio of the continuous constraint trajectory and the initial driving trajectory can be adjusted according to different vehicle driving scenarios and requirements to make a trade-off between the inter-frame continuity and the prediction and planning intention of the final driving trajectory. And by selecting the position of the third trajectory point in the continuous constraint trajectory, the splicing ratio of the continuous constraint trajectory can be adjusted.

[0055] In a possible embodiment, in the scenario where the vehicle is pulling over, since the pulling-over scenario does not involve complex itinerary planning such as lane change and obstacle avoidance, the splicing ratio of the continuous constraint trajectory in the final driving trajectory can be increased to further improve the trajectory stability in the vehicle pulling-over scenario.

[0056] In another possible embodiment, in the scenario where the vehicle changes lanes or bypasses obstacles on the road, due to the involvement of long-distance complex route planning, the splicing ratio of the continuous constraint trajectory in the final driving trajectory can be reduced. On the basis of ensuring the minimum inter-frame continuity, the current frame trajectory planning intention in the initial driving trajectory is improved, so that the final driving trajectory can complete lane change or obstacle bypass faster.

[0057] Further, the step of splicing the constrained trimmed trajectory and the driving trimmed trajectory based on the third trajectory point and the fourth trajectory point to obtain the final driving trajectory includes: Splicing the constrained trimmed trajectory and the driving trimmed trajectory based on the third trajectory point and the fourth trajectory point to obtain a first spliced driving trajectory; Performing smoothing processing on the first spliced driving trajectory to eliminate the curvature mutation at the third trajectory point and the fourth trajectory point, thereby obtaining the final driving trajectory.

[0058] 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 and 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 for linear equations of symmetric positive definite matrices, the conjugate gradient method achieves fast convergence by constructing a sequence of conjugate directions and is particularly suitable for the optimization of large-scale sparse systems. In the trajectory processing scenario, this algorithm gradually reduces the residual deviation between the third trajectory point and the fourth trajectory point through multiple iterations and finally converges into a smoothest path. Its convergence has been theoretically proven and experimentally verified to ensure the stability of the optimization result. Compared with the traditional gradient descent method, the conjugate gradient method adopted in this embodiment significantly improves the convergence speed through direction conjugacy and at the same time ensures the physical rationality of the final driving trajectory.

[0059] In this implementation manner, after aligning the first trajectory point and the second trajectory point in the global coordinate system and completing the splicing of the constrained trimmed trajectory and the driving trimmed trajectory, smoothing processing is performed on the alignment part of the third trajectory point and the fourth trajectory point, thereby eliminating the curvature mutation at the splicing point, which is beneficial to the faster convergence of the driving trajectory and obtaining a more stable vehicle driving planning route.

[0060] Further, the step of performing smoothing processing on the first spliced driving trajectory to eliminate the curvature mutation at the third trajectory point and the fourth trajectory point, thereby obtaining the final driving trajectory, includes: Performing smoothing processing on the first spliced driving trajectory to eliminate the curvature mutation at the third trajectory point and the fourth trajectory point, thereby obtaining a second spliced driving trajectory; The quadratic programming algorithm is adopted to optimize the second spliced driving trajectory based on the preset hard constraints of the driving boundary and the preset soft constraints, so as to obtain the final driving trajectory.

[0061] Specifically, the process of adopting the quadratic programming algorithm to optimize the second spliced driving trajectory based on the preset hard constraints of the driving boundary and the preset soft constraints to obtain the final driving trajectory is as follows: Step 1: Input the initial solution. The second spliced driving trajectory generated in the previous step is used as the high-quality initial solution of the quadratic programming algorithm. This guiding line indicates the general direction and expected trend of the driving path.

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

[0063] Step 3: Solve using the quadratic programming algorithm. Within the feasible space strictly satisfying all the defined hard constraints, find the trajectory parameters that can minimize the value of the objective function, including but not limited to the positions, speeds, and accelerations of the trajectory points, etc. The solution process will finely adjust the initial guiding line: ensure that the trajectory is strictly within the safety boundary; check and make the trajectory satisfy the acceleration change rate constraint to ensure vehicle driving comfort; improve the trajectory smoothness; and as much as possible, keep close to the initial expected path of the second spliced driving trajectory on the premise of satisfying the hard constraints.

[0064] Step 4: After the solution of the quadratic programming algorithm in Step 3 converges successfully, output the optimized trajectory as the final driving trajectory.

[0065] In this implementation manner, after the smoothing process at the trajectory splicing is completed, the second spliced driving trajectory is used as the guiding line, and the quadratic programming algorithm is adopted to refine and optimize the second spliced driving trajectory, making it satisfy the hard constraints of the preset driving road boundary, as well as the preset soft constraints such as the jerk constraint and the smoothing constraint, and finally obtaining the final driving trajectory that meets the requirements.

[0066] It should be noted that in the present invention, by splicing the inertial trajectory, the historical trajectory, and the best trajectory given by the current lattice trajectory planning algorithm based on the preset trajectory evaluation index, a guiding line is formed, which is optimized by the quadratic programming algorithm. The finally generated trajectory naturally takes into account the connection between the vehicle motion inertia and the planned trajectory, as well as the similarity between the current trajectory and the previous frame trajectory. At the same time, it also takes into account the best planned path of the lattice trajectory planning algorithm, ensuring the vehicle stability while also considering the timeliness of functions such as obstacle avoidance.

[0067] It should be understood that in principle, the present invention is equivalent to splicing a continuous constraint 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 desired optimization result processed by the quadratic programming algorithm is as close as possible to the inertial trajectory and the historical trajectory within the feasible solution range. Thus, on the basis of improving the continuity between the trajectory planning and the historical motion trend of the vehicle, the timeliness of functions such as obstacle avoidance by maintaining the best path planned by the traditional autonomous driving algorithm can also be maintained.

[0068] In a possible embodiment, a higher weight is set for the guiding line of the part of the second spliced driving trajectory close to the vehicle head in the quadratic programming algorithm. This part of the guiding line includes all of the inertial trajectory and a part of the historical trajectory. In this case, when the quadratic programming algorithm inx is optimized, it will consider more the similarity between the trajectory of the vehicle head part and the second spliced driving trajectory, thereby improving the continuity between the initial part of the final driving trajectory and the inertial trajectory and the historical trajectory of the vehicle, preventing the sudden change of the heading angle of the vehicle head during driving, and improving the safety of trajectory planning.

[0069] Furthermore, the expression of the vehicle dynamics model is as follows: ; Wherein, represents the abscissa of the vehicle, represents the ordinate of the vehicle, represents the heading angle of the vehicle, represents the distance between the front wheels and the rear wheels of the vehicle, represents the vehicle speed, represents the steering angle of the front wheels of the vehicle.

[0070] It should be noted that the above vehicle dynamics model is a bicycle model, which simplifies the vehicle into a structure with front and rear two wheels. Based on the state equation of the vehicle dynamics model, the heading angle of the vehicle at the next moment can be deduced from the current vehicle speed and the current steering angle of the front wheels of the vehicle, so as to obtain the change rates of the abscissa position and the ordinate position of the vehicle at the next moment. Multiplying the abscissa change rate and the ordinate change rate by the unit time dt respectively can obtain the inertial position of the vehicle at the next moment. Finally, by recursively calculating the inertial position sequence of the vehicle within a certain time interval in the future, the inertial trajectory of the vehicle can be obtained.

[0071] In another possible embodiment, the instantaneous lateral speed and longitudinal speed of the vehicle at the current moment can be directly multiplied by the unit time dt, and the position coordinates of the vehicle at the current moment are accumulated, and finally the inertial position of the vehicle after one unit time is deduced. Among them, the lateral speed and longitudinal speed of the vehicle can be directly obtained through the speed sensors preset in the vehicle. Since in one embodiment of the present invention, only the inertial trajectory of the vehicle within the next 0.3 seconds is deduced through the state equation, only the inertial displacement for a very short period of time is deduced, and the deviation between the result directly deduced through the instantaneous lateral acceleration and instantaneous longitudinal acceleration of the vehicle and the result deduced through the complex vehicle dynamics model can be ignored. Moreover, compared with the complex vehicle dynamics model, the above-mentioned embodiment uses a simpler recursive method and a bicycle model, with lower computational complexity and higher operation efficiency.

[0072] As Figure 2 shown, the second aspect of the present invention provides an autonomous vehicle trajectory planning system, which includes a dynamics 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, where: The dynamics model construction module 100 is used to construct a vehicle dynamics model; The vehicle state detection module 200 is used to obtain the current frame state and historical frame state of the vehicle; The continuity constraint module 300 is used to obtain a continuity constraint trajectory based on the vehicle dynamics model, based on the current frame state and the historical frame state; The initial trajectory planning module 400 is used to plan an initial driving trajectory according to a preset trajectory evaluation index and the current frame state; The final trajectory splicing module 500 is used to perform spatio-temporal splicing on the continuity constraint trajectory and the initial driving trajectory to obtain a final driving trajectory.

[0073] Based on the traditional autonomous driving algorithm that plans the vehicle driving trajectory based on a preset trajectory evaluation index and the current frame state of the vehicle, the above autonomous vehicle trajectory planning system splices the initial driving trajectory and the continuity constraint trajectory to obtain a final driving trajectory. The continuity constraint trajectory is calculated based on the vehicle dynamics model according to the current frame state and historical frame state of the vehicle, and 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. Splicing the continuity constraint trajectory and the initial driving trajectory improves the continuity of the spliced final driving trajectory with the vehicle's historical frame trajectory and the continuity with the current motion trend of the vehicle, preventing a strong jump in the motion direction between the previous frame and the next frame of the vehicle's planned trajectory, thereby preventing a sharp change in the heading angle of the vehicle's trajectory, improving the inter-frame continuity and stability of the trajectory planning, and enhancing the safety of autonomous driving.

[0074] Since the obtained final driving trajectory is formed by spatiotemporally splicing the continuous constraint trajectory and the initial driving trajectory, it can not only improve the continuity between the trajectory planning and the historical movement trend of the vehicle, but also maintain the timeliness of functions such as obstacle avoidance by planning the optimal path with traditional autonomous driving algorithms.

[0075] Further, the obtaining of the continuous constraint trajectory based on the current frame state and the historical frame state according to the vehicle dynamics model includes: Predicting an inertial trajectory based on the vehicle dynamics model and the current frame state; Obtaining a historical trajectory based on the historical frame state; Spatiotemporally splicing the inertial trajectory and the historical trajectory to obtain the continuous constraint trajectory.

[0076] In this implementation manner, the continuous constraint trajectory is formed by splicing the inertial trajectory and the 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, representing the current motion inertia trend of the vehicle; splicing the inertial trajectory in the continuous constraint trajectory can improve the continuity between the final driving trajectory and the current motion state of the vehicle, and avoid sudden changes in the heading angle of the vehicle. The historical trajectory refers to the previous frame trajectory of the current frame trajectory of the vehicle in this implementation manner, which is obtained from the historical data of the vehicle's driving; splicing the historical trajectory in the continuous constraint trajectory can improve the similarity between the final driving trajectory and the historical driving trajectory of the vehicle, thereby ensuring the stability between multiple frame driving trajectories of the vehicle, avoiding drastic deviations in the adjacent frame trajectory routes, and ultimately improving the safety of autonomous driving trajectory planning.

[0077] Further, the spatiotemporally splicing the inertial trajectory and the historical trajectory to obtain the continuous constraint trajectory includes: Obtaining a first trajectory point in the inertial trajectory; Obtaining the starting point of the inertial trajectory in the inertial trajectory, and further obtaining the inertial trimmed trajectory between the first trajectory point and the starting point of the inertial trajectory in the inertial trajectory; Obtaining a second trajectory point in the historical trajectory that satisfies a preset spatiotemporal threshold with the first trajectory point; Obtaining the ending point of the historical trajectory in the historical trajectory, and further obtaining the historical trimmed trajectory between the ending point of the historical trajectory and the second trajectory point in the historical trajectory; Splicing the inertial trimmed trajectory and the historical trimmed trajectory based on the first trajectory point and the second trajectory point to obtain the continuous constraint trajectory.

[0078] 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, there must be an overlapping part between the two trajectories. Here, the spatio-temporal alignment is to find out and cut off the overlapping part of the two trajectories. Specifically, using the spatio-temporal splicing method, cut off the trajectory between the starting point of the inertial trajectory and the first trajectory point; obtain the second trajectory point in the historical trajectory that is closest to the first trajectory point of the inertial trajectory, and cut off the trajectory between the second trajectory point of the historical trajectory and the end point of the historical trajectory; taking 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, realize splicing the inertial cut-off trajectory and the historical cut-off trajectory to obtain a continuous constraint trajectory. This continuous constraint trajectory takes the inertial trajectory as the starting trajectory, which can improve the continuity between the starting path of the continuous constraint trajectory and the current motion state of the vehicle, and avoid sudden changes in the heading angle of the vehicle; taking the historical trajectory as the subsequent trajectory, it can improve the similarity between the overall continuous constraint trajectory and the historical driving trajectory of the vehicle, thereby ensuring the stability between multiple frames of the vehicle's driving trajectory.

[0079] According to an autonomous vehicle trajectory planning method and system provided by the present invention, compared with the prior art, it has at least the following advantages: The present invention predicts the inertial motion trajectory of the vehicle in the next 0.2 - 0.5 seconds based on the current frame state of the vehicle, including but not limited to vehicle speed, vehicle acceleration, and vehicle yaw angle; the present invention cuts and splices the front section of the inertial trajectory and the rear section of the previous frame's historical trajectory, and realizes seamless splicing through the conjugate smoothing algorithm; the present invention assigns a higher weight to the front section of the spliced trajectory in the quadratic programming to ensure the continuity between the starting path of the part of the trajectory near the vehicle body and the previous frame's trajectory and the vehicle's motion trend, thereby improving the stability of the vehicle's driving trajectory.

[0080] Based on the present invention, the continuous constraint trajectory and the initial driving trajectory are spliced spatio-temporally to obtain the final driving trajectory. In scenarios where the trajectory changes during the start-up phase such as lane change and obstacle avoidance, due to the continuity between the starting section of the final driving trajectory and the vehicle's historical frame trajectory and the current motion trend, the autonomous vehicle is more stable when turning the steering wheel; during the obstacle avoidance process, when the perception module jitters in detecting obstacles slightly farther away, even if the distal end of the final driving trajectory makes an obstacle avoidance path change, due to the continuity between the starting section of the final driving trajectory and the vehicle's historical frame trajectory and the current motion trend, the vehicle trajectory still tends to be stable; during the obstacle avoidance process, even when encountering interference from dynamic obstacles, the trajectory of the front part of the autonomous vehicle still tends to be stable, ensuring that the heading angle of the autonomous vehicle does not mutate and improving the safety of autonomous driving.

[0081] Those of ordinary skill in the art can understand that all or part of the processes in the methods of the above embodiments can be completed by instructing 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 embodiments of the above methods. Among them, any reference to a memory, database, or other medium used in the embodiments provided in the present application can include at least one of non-volatile and volatile memories. 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), magnetoresistive 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 be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM), etc. The databases involved in the embodiments provided in the present application can include at least one of relational databases and non-relational databases. Non-relational databases can include distributed databases based on blockchain, etc., without limitation. The processors involved in the embodiments provided in the present application can be general-purpose processors, central processing units, graphics processing units, digital signal processors, programmable logic devices, data processing logics based on quantum computing, etc., without limitation.

[0082] The term "embodiment" as used herein means that a particular feature, structure, or characteristic described in connection with the embodiment can be included in at least one embodiment of the present application. The phrase appears in various places in the specification and does not necessarily refer to the same embodiment, nor is it an independent or alternative embodiment mutually exclusive with other embodiments. Those skilled in the art will explicitly and implicitly understand that the embodiments described herein can be combined with other embodiments. For the sake of brevity of description, not all possible combinations of all the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, it should be considered to be within the scope recorded in this specification.

[0083] The above-described embodiments merely represent several implementation manners of the present application. The description thereof is relatively specific and detailed, but it should not be construed as a limitation on the patent scope of the present application. It should be noted that for those of ordinary skill in the art, without departing from the concept of the present application, several improvements and substitutions can be made, and these improvements and substitutions should also be regarded as the protection scope of the present invention. Therefore, the protection scope of the present application shall be subject to the appended claims.

Claims

1. A trajectory planning method for an autonomous vehicle, characterized in that, Including: Construct a vehicle dynamics model and obtain the current frame state and historical frame state of the vehicle; Based on the vehicle dynamics model, obtain a continuous constraint trajectory according to the current frame state and the historical frame state; Plan an initial driving trajectory based on a preset trajectory evaluation index and the current frame state; Perform spatio-temporal splicing on the continuous constraint trajectory and the initial driving trajectory to obtain a final driving trajectory.

2. The method for trajectory planning of an autonomous vehicle according to claim 1, wherein, The step of obtaining a continuous constraint trajectory according to the current frame state and the historical frame state based on the vehicle dynamics model includes: Predict an inertial trajectory based on the vehicle dynamics model and the current frame state; Obtain a historical trajectory based on the historical frame state; Perform spatio-temporal splicing on the inertial trajectory and the historical trajectory to obtain the continuous constraint trajectory.

3. The method for trajectory planning of an autonomous vehicle according to claim 2, wherein The step of performing spatio-temporal splicing on the inertial trajectory and the historical trajectory to obtain the continuous constraint trajectory includes: Obtain a first trajectory point in the inertial trajectory; Obtain the inertial trajectory starting point in the inertial trajectory, and further obtain an inertial trimmed trajectory between the first trajectory point and the inertial trajectory starting point in the inertial trajectory; Obtain a second trajectory point in the historical trajectory that satisfies a preset spatio-temporal threshold with the first trajectory point; Obtain the historical trajectory end point in the historical trajectory, and further obtain a historical trimmed trajectory between the historical trajectory end point and the second trajectory point in the historical trajectory; Splice the inertial trimmed trajectory and the historical trimmed trajectory based on the first trajectory point and the second trajectory point to obtain the continuous constraint trajectory.

4. A trajectory planning method for an autonomous vehicle according to claim 1, wherein The step of performing spatio-temporal splicing on the continuous constraint trajectory and the initial driving trajectory to obtain a final driving trajectory includes: Obtain a third trajectory point in the continuous constraint trajectory based on a preset continuous constraint rule, and the continuous constraint rule is used to determine the splicing ratio of the continuous constraint trajectory and the initial driving trajectory; Obtain the constraint trajectory starting point in the continuous constraint trajectory, and further obtain a constraint trimmed trajectory between the third trajectory point and the constraint trajectory starting point in the continuous constraint trajectory; Obtain a fourth trajectory point in the initial driving trajectory that satisfies a preset spatio-temporal threshold with the third trajectory point; Obtain the driving trajectory end point in the initial driving trajectory, and further obtain a driving trimmed trajectory between the driving trajectory end point and the fourth trajectory point in the initial driving trajectory; Splice the constraint trimmed trajectory and the driving trimmed trajectory based on the third trajectory point and the fourth trajectory point to obtain the final driving trajectory.

5. A method for trajectory planning of an autonomous vehicle according to claim 4, characterized in that, The step of splicing the constraint trimmed trajectory and the driving trimmed trajectory based on the third trajectory point and the fourth trajectory point to obtain the final driving trajectory includes: Splice the constraint trimmed trajectory and the driving trimmed trajectory based on the third trajectory point and the fourth trajectory point to obtain a first spliced driving trajectory; Perform smoothing processing on the first spliced driving trajectory, and further eliminate the curvature mutation at the third trajectory point and the fourth trajectory point to obtain the final driving trajectory.

6. The trajectory planning method for an autonomous vehicle according to claim 5, wherein, Smoothing the first spliced driving trajectory to eliminate the curvature mutations at the third trajectory point and the fourth trajectory point, and obtaining the final driving trajectory, including: Smoothing the first spliced driving trajectory to eliminate the curvature mutations at the third trajectory point and the fourth trajectory point, and obtaining the second spliced driving trajectory; Using a quadratic programming algorithm to optimize the second spliced driving trajectory based on preset hard constraints of the driving boundary and preset soft constraints, and obtaining the final driving trajectory.

7. A method for trajectory planning of an autonomous vehicle according to claim 1, characterized in that, The expression of the vehicle dynamics model is as follows: ； Among them, represents the vehicle's abscissa, represents the vehicle's ordinate, represents the vehicle's heading angle, represents the distance between the vehicle's front wheels and rear wheels, represents the vehicle's speed, represents the steering angle of the vehicle's front wheels.

8. An automatic driving vehicle trajectory planning system, characterized in that, Including a dynamics model construction module, a vehicle state detection module, a continuity constraint module, an initial trajectory planning module, and a final trajectory splicing module, where: The dynamics model construction module is used to construct 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, the current frame state, and the historical frame state; The initial trajectory planning module is used to plan an initial driving trajectory according to preset trajectory evaluation indicators and the current frame state; The final trajectory splicing module is used to perform spatio-temporal splicing on the continuity constraint trajectory and the initial driving trajectory to obtain the final driving trajectory.

9. An automatic driving vehicle trajectory planning system according to claim 8, characterized in that, The obtaining of the continuity constraint trajectory based on the vehicle dynamics model, the current frame state, and the historical frame state includes: Predicting an inertial trajectory based on the vehicle dynamics model and the current frame state; Obtaining a historical trajectory based on the historical frame state; Performing spatio-temporal splicing on the inertial trajectory and the historical trajectory to obtain the continuity constraint trajectory.

10. An automatic driving vehicle trajectory planning system according to claim 9, characterized in that, The performing of spatio-temporal splicing on the inertial trajectory and the historical trajectory to obtain the continuity constraint trajectory includes: Obtaining a first trajectory point in the inertial trajectory; Obtaining the inertial trajectory starting point in the inertial trajectory, and further obtaining the inertial trimmed trajectory between the first trajectory point and the inertial trajectory starting point in the inertial trajectory; Obtaining a second trajectory point in the historical trajectory that satisfies a preset spatio-temporal threshold with the first trajectory point; Obtaining the historical trajectory end point in the historical trajectory, and further obtaining the historical trimmed trajectory between the historical trajectory end point and the second trajectory point in the historical trajectory; Splicing the inertial trimmed trajectory and the historical trimmed trajectory based on the first trajectory point and the second trajectory point to obtain the continuity constraint trajectory.

Images