A spatiotemporal joint trajectory planning method and apparatus based on sparse sampling

Abstract

This invention provides a spatiotemporal joint trajectory planning method and apparatus based on sparse sampling, belonging to the field of autonomous driving technology. The method involves acquiring the vehicle's state information and the state information of dynamic and static obstacles surrounding the vehicle; obtaining a spatiotemporal joint map based on the vehicle's state information and surrounding environment information; sparsely sampling the vehicle's coarse-grained trajectory points based on the longitudinal acceleration in the vehicle's state information and the spatiotemporal joint map to obtain a set of coarse-grained trajectory points; determining the optimal coarse-grained trajectory based on the set of coarse-grained trajectory points; determining a three-dimensional convex space based on the optimal coarse-grained trajectory and the spatiotemporal joint map; and generating a fine-grained Bézier trajectory in the three-dimensional convex space using the convex hull property of Bézier curves. This approach fully considers the optimality of the trajectory under complex spatial obstacles and dynamic constraints, improving computational efficiency and trajectory quality, and ensuring trajectory safety and feasibility.

Description

Technical Field

[0001] This invention relates to the field of driver assistance technology, and in particular to a spatiotemporal joint trajectory planning method and apparatus based on sparse sampling. Background Technology

[0002] Trajectory planning, a key technology in autonomous driving, is mainly divided into two types: spatiotemporal decoupled planning and spatiotemporal joint planning. Spatiotemporal decoupled planning decomposes the spatiotemporal trajectory into path and velocity curves, reducing the difficulty of solving the two problems by solving the path and velocity separately. However, this sacrifices the optimality of the final trajectory to some extent, and it is prone to getting suboptimal in complex dynamic scenarios. Spatiotemporal joint planning does not decompose the path and velocity, but directly solves for the optimal spatiotemporal trajectory, thus obtaining a higher quality trajectory, but the computational efficiency is reduced. Currently, most trajectory generation schemes use dense sampling to obtain trajectory points, such as the RRT algorithm. Although it can quickly and effectively search for paths in space, this algorithm lacks supervision in the process of generating new nodes, has a slow convergence speed, and the dense sampling method often has a large computational cost when obtaining a fine trajectory.

[0003] In summary, spatiotemporal decoupling planning based on dense sampling is inefficient for obtaining fine-grained trajectories, involves high computational costs, and is prone to getting trapped in suboptimal solutions. Therefore, it is necessary to comprehensively consider the complexity and real-time performance of trajectory planning, as well as the optimality of trajectories under complex spatial constraints and dynamic limitations, to achieve high-quality, safe, and feasible generation of optimal fine-grained trajectories. Summary of the Invention

[0004] In view of this, the present invention provides a spatiotemporal joint trajectory planning method and apparatus based on sparse sampling, which fully considers the optimality of trajectories under complex spatial obstacles and dynamic constraints, improves computational efficiency and trajectory quality, and ensures trajectory safety and feasibility. The technical solution is as follows:

[0005] This invention provides a spatiotemporal joint trajectory planning method based on sparse sampling, the method comprising:

[0006] Acquire vehicle status information and the status information of dynamic and static obstacles around the vehicle;

[0007] Based on the vehicle's status information and the surrounding environment information, a spatiotemporal joint map is obtained;

[0008] Based on the longitudinal acceleration in the vehicle's state information, and according to the spatiotemporal joint map, the vehicle's coarse-grained trajectory points are sparsely sampled to obtain the set of vehicle coarse-grained trajectory points.

[0009] The optimal coarse-grained trajectory is determined based on the set of coarse-grained trajectory points of the vehicle.

[0010] The three-dimensional convex space is determined based on the optimal coarse-grained trajectory and the spatiotemporal joint map;

[0011] By utilizing the convex hull property of Bézier curves, fine-grained Bézier trajectories are generated in a three-dimensional convex space.

[0012] Optionally, the longitudinal acceleration of the vehicle is sampled, and the longitudinal coarse-grained sampling trajectory points of the vehicle are obtained based on the sampled acceleration.

[0013] Based on the spatiotemporal joint map, trajectory points that satisfy lane keeping and lane changing requirements are sampled from the longitudinal coarse-grained trajectory points of the vehicle to form a set of coarse-grained trajectory points of the vehicle.

[0014] Optionally, within a set acceleration range, the vehicle's acceleration is sampled according to a set resolution.

[0015] Optionally, different sampling points between every two adjacent time layers in the set of coarse-grained trajectory points of the vehicle are connected to obtain a set of coarse-grained trajectories;

[0016] Calculate the cost of each coarse-grained trajectory in the set of coarse-grained trajectories, and select the coarse-grained trajectory with the minimum cost as the optimal coarse-grained trajectory. The cost of each coarse-grained trajectory includes obstacle cost, offset reference line cost, and lateral velocity cost.

[0017] Optionally, starting from the optimal coarse-grained trajectory point, the convex space is expanded in both the horizontal and vertical directions until it stops when it encounters the drivable boundary, thus obtaining a three-dimensional convex space. The drivable boundary includes a longitudinal drivable boundary and a lateral drivable boundary. The longitudinal drivable boundary is determined based on the previous sampled acceleration and the next sampled acceleration of the optimal coarse-grained trajectory. The lateral drivable boundary is determined based on the maximum allowable lateral offset of the optimal coarse-grained trajectory.

[0018] Optionally, with the goal of minimizing the cost of the trajectory, and with terminal constraints, continuity constraints, collision constraints, and dynamic constraints as constraints, a fine-grained Bézier trajectory is determined based on the optimized trajectory model. The optimized trajectory model is constructed using n Bézier curves. The cost of the trajectory is determined by integrating the square of the trajectory's jerk value over time.

[0019] This invention provides a spatiotemporal joint trajectory planning device based on sparse sampling, comprising:

[0020] The acquisition module is used to acquire the vehicle's status information and the status information of dynamic and static obstacles around the vehicle;

[0021] The trajectory planning module is used to obtain a spatiotemporal joint map based on the vehicle's state information and surrounding environment information; based on the longitudinal acceleration in the vehicle's state information, it performs sparse sampling on the vehicle's coarse-grained trajectory points according to the spatiotemporal joint map to obtain a set of coarse-grained trajectory points; based on the set of coarse-grained trajectory points, it determines the optimal coarse-grained trajectory; based on the optimal coarse-grained trajectory and the spatiotemporal joint map, it determines a three-dimensional convex space; and using the convex hull property of Bézier curves, it generates a fine-grained Bézier trajectory in the three-dimensional convex space.

[0022] This invention provides a control device, which includes one or more processors and one or more memories. The one or more memories store at least one piece of program code, which is loaded and executed by the one or more processors to implement a spatiotemporal joint trajectory planning method based on sparse sampling as described in any of the above implementations.

[0023] This invention provides a computer-readable storage medium storing at least one piece of program code, which is loaded and executed by a processor to implement a spatiotemporal joint trajectory planning method based on sparse sampling as described in any of the above implementations.

[0024] This invention provides a computer program product, which includes computer program code stored in a computer-readable storage medium. A processor of a control device reads the computer program code from the computer-readable storage medium and executes the computer program code, causing the control device to execute a spatiotemporal joint trajectory planning method based on sparse sampling as described in any of the above implementations.

[0025] The beneficial effects of the technical solutions provided in the embodiments of the present invention include at least the following:

[0026] This invention provides a spatiotemporal joint trajectory planning method based on sparse sampling. It sparsely samples coarse-grained trajectory points of a vehicle based on longitudinal acceleration, searches the sampled points using dynamic programming to obtain the optimal coarse-grained trajectory, and constructs a three-dimensional convex space (SLT) based on a spatiotemporal joint map. This takes into account spatial obstacles of complex configurations. Furthermore, it utilizes the convex hull properties of Bézier curves to generate fine-grained Bézier trajectories in the three-dimensional convex space. This method fully considers spatial obstacles and dynamic constraints of complex configurations, improves computational efficiency, and ensures trajectory safety and feasibility. Attached Figure Description

[0027] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0028] Figure 1 A flowchart of a spatiotemporal joint trajectory planning method based on sparse sampling provided in an embodiment of the present invention;

[0029] Figure 2 This is a schematic diagram of sparse sampling of longitudinal acceleration provided in an embodiment of the present invention;

[0030] Figure 3 This is a schematic diagram of the spatiotemporal joint convex space construction provided in an embodiment of the present invention. Detailed Implementation

[0031] To make the objectives, technical solutions, and advantages of the present invention clearer, the embodiments of the present invention will be described in further detail below with reference to the accompanying drawings.

[0032] Before providing a detailed explanation of the embodiments of the present invention, the application scenarios and system architecture involved in the embodiments of the present invention will be introduced first.

[0033] First, the system architecture involved in the embodiments of the present invention will be introduced.

[0034] The spatiotemporal joint trajectory planning method based on sparse sampling provided in this embodiment of the invention involves a system including vehicle-mounted sensors and an intelligent driving controller.

[0035] Onboard sensors are used to acquire the information needed for trajectory planning, including the vehicle's status information and the status information of dynamic and static obstacles around the vehicle. Each status information includes position, speed, and acceleration.

[0036] The intelligent driving controller is used to perform trajectory planning based on the information obtained from the vehicle's onboard sensors, and obtain the planned trajectory.

[0037] Those skilled in the art should understand that the above system architecture is merely an example, and other existing or future components or modules that are applicable to this application should also be included within the scope of protection of this application, and are hereby incorporated by reference.

[0038] Finally, the application scenarios of the methods provided in the embodiments of the present invention will be introduced.

[0039] The method provided in this invention is applied to autonomous driving scenarios. Current spatiotemporal joint planning methods for autonomous driving do not decompose paths and speeds, but directly solve for the optimal spatiotemporal trajectory, thus obtaining higher quality trajectories, but with reduced computational efficiency. Most current trajectory generation schemes use dense sampling to obtain trajectory points, such as the RRT algorithm. While it can quickly and effectively search for paths in space, this algorithm lacks supervision in the process of generating new nodes, resulting in slow convergence. Furthermore, dense sampling often involves a large computational load when obtaining a fine-grained trajectory.

[0040] Based on this application scenario, this application provides a spatiotemporal joint trajectory planning method based on sparse sampling.

[0041] To make the objectives, technical solutions, and advantages of the present invention clearer, the embodiments of the present invention will be described in further detail below with reference to the accompanying drawings.

[0042] Figure 1 This diagram illustrates a spatiotemporal joint trajectory planning method based on sparse sampling provided by the present invention. This method can be executed by onboard equipment in a vehicle, which includes onboard sensors and an intelligent driving controller. (Refer to...) Figure 1 The method includes the following steps:

[0043] Step (1) Obtain the vehicle status information and the status information of dynamic and static obstacles around the vehicle.

[0044] Step (2) Obtain a spatiotemporal joint map based on the vehicle's status information and the surrounding environment information;

[0045] Step (3) Based on the longitudinal acceleration in the vehicle state information, and according to the spatiotemporal joint map, sparsely sample the coarse-grained trajectory points of the vehicle to obtain the set of coarse-grained trajectory points of the vehicle.

[0046] Step (4) Determine the optimal coarse-grained trajectory based on the set of coarse-grained trajectory points of the vehicle;

[0047] Step (5) Determine the three-dimensional convex space based on the optimal coarse-grained trajectory and the spatiotemporal joint map;

[0048] Step (6) utilizes the convex hull property of the Bézier curve to generate a fine-grained Bézier trajectory in a three-dimensional convex space.

[0049] This invention provides a spatiotemporal joint trajectory planning method based on sparse sampling. It sparsely samples coarse-grained trajectory points of a vehicle based on longitudinal acceleration, searches the sampled points using dynamic programming to obtain the optimal coarse-grained trajectory, constructs a three-dimensional convex space (SLT) based on a spatiotemporal joint map, fully considers complex spatial obstacles and dynamic constraints, and utilizes the convex hull properties of Bézier curves to achieve optimized fine-grained trajectory generation based on Bézier curves, thereby improving computational efficiency and ensuring trajectory safety and feasibility.

[0050] In step (1), the vehicle's state information and the state information of dynamic and static obstacles around the vehicle are acquired using onboard sensors. Each state information includes position, speed, and acceleration. Obstacles are objects that constrain the vehicle's behavior, including road geometry information, guardrails at road boundaries, or trees.

[0051] In step (2), the vehicle status information and surrounding environment information obtained in step (1) are converted into Frenet coordinates and extended on the time axis to build a spatiotemporal joint map in real time.

[0052] The spatiotemporal joint map M within time period T consists of three parts, including the positions of vehicles around the vehicle. Static obstacle position and road geometry information The spatiotemporal joint map M is represented as follows:

[0053]

[0054] In the formula, N D N S N L These represent the number of surrounding vehicles, static obstacles, and lanes, respectively. These are the s and l coordinates in the Frenet coordinate system. These are the lane width and lane number, respectively; The Frenet coordinates are for the center line of the lane.

[0055] In step (3), the sparse sampling of the vehicle's coarse-grained trajectory points includes longitudinal sparse sampling and lateral sparse sampling, and the specific steps include:

[0056] Step (31) samples the longitudinal acceleration of the vehicle and obtains the longitudinal coarse-grained sampling trajectory points of the vehicle based on the sampled acceleration.

[0057] In some embodiments, the vehicle's acceleration is sampled at a set resolution within a set acceleration range.

[0058] The acceleration a in the s-axis direction of the vehicle s In [amax ,a min Within the interval, with resolution δ a The acceleration set A is obtained by sampling, and the planning period [0, t] is also included. max [At resolution δ] t The samples are set as the planning time set T, assuming the vehicle undergoes uniform acceleration or deceleration within a short period of time, up to each acceleration sampling point a. s The corresponding longitudinal coarse-grained sampling trajectory point set ST of the vehicle:

[0059]

[0060] In the formula, a min ,a max These are the maximum deceleration and maximum acceleration, respectively, t max The time period for a single trajectory planning iteration is given by s, where s0 is the initial s-coordinate in the Frenet coordinate system. Let be the initial velocity in the s direction.

[0061] Step (32) Based on the spatiotemporal joint map, sample the trajectory points that meet the lane keeping requirements and the trajectory points that meet the lane changing requirements from the longitudinal coarse-grained sampling trajectory points of the vehicle to form a set of coarse-grained trajectory points of the vehicle.

[0062] The process of sampling trajectory points that satisfy lane keeping is as follows: taking the current lane centerline as the reference line, the sampling interval in the l-axis direction corresponding to each sampling point (s,t) is [-Δl]. max ,Δl max The sampling interval is set at a sampling interval δ. lk Perform equidistant divergence to obtain the set L of lateral sampling points. k The set of trajectory points that satisfy lane keeping for each acceleration sampling point (SLT) k for:

[0063]

[0064] In the formula, Δl max f represents the maximum permissible displacement along the l-axis of the intelligent vehicle. Δ It is an undetermined constant.

[0065] The process of sampling trajectory points that meet the lane-changing requirements is as follows: taking the center line of the target lane as the reference line, the sampling interval in the l-axis direction corresponding to each sampling point (s,t) is [l ego [,0], which is the lateral distance l of the vehicle's current position. ego In the section of the target lane reference line, when the vehicle is to the left of the reference line... ego The value is negative when the vehicle is to the right of the reference line. egoTo ensure vehicles complete lane changes within the planned timeframe, the sampling point for the last time layer is set on the reference line. This sampling interval is then set at sampling interval δ. lc Perform equidistant divergence to obtain the set L of lateral sampling points. c The set of trajectory points that meet the lane-changing requirements is SLT. c :

[0066]

[0067] In step (4), different sampling points between every two adjacent time layers in the set of coarse-grained trajectory points of the vehicle are connected to obtain a set of coarse-grained trajectories; the cost of each coarse-grained trajectory in the set of coarse-grained trajectories is calculated, and the coarse-grained trajectory with the lowest cost is selected as the optimal coarse-grained trajectory. The cost of each coarse-grained trajectory includes obstacle cost, offset reference line cost and lateral velocity cost.

[0068] In some embodiments, at each acceleration sampling point a s The corresponding SLT sampling point set uses linear line segments to connect different sampling points between every two adjacent time layers, resulting in a coarse-grained trajectory set between adjacent time layers. The cost of each coarse-grained trajectory in the coarse-grained trajectory set is calculated using a coarse-grained trajectory cost function.

[0069] The steps for constructing the coarse-grained trajectory cost function and searching for the optimal coarse-grained trajectory include:

[0070] Step (41) Construct a coarse-grained trajectory cost function, time layer L t The i-th sampling point With time layer The j-th sampling point The cost on the connected trajectory Including obstacle cost C obs Offset reference line cost C offset And lateral velocity cost C velocity :

[0071]

[0072] Wherein, for a given trajectory, the obstacle cost C corresponding to that trajectory is... obs The distance d from the boundary of the obstacle in the environment to the trajectory is determined by:

[0073]

[0074] In the formula, d n The maximum influence boundary of an obstacle is determined by its shape, state, and other characteristics; d c c represents the collision boundary of the obstacle. nu deg It is a monotonically decreasing function; w obs This represents the obstacle cost weighting coefficient.

[0075] During driving, vehicles should stay as close to the center line of the road as possible to avoid interfering with vehicles in other lanes. This is achieved by offsetting the reference line at a cost C. offset The cost C for deviating from the reference line reflects the degree to which the vehicle deviates from the current lane centerline. offset for:

[0076]

[0077] In the formula, For time layer L t The i-th sampling point The l-axis coordinate; w offset This is the weighting coefficient for the lateral offset cost.

[0078] The greater the lateral velocity cost, the greater the vehicle's displacement along the l-axis of the connecting line between two adjacent time layers, resulting in a greater lateral velocity when the vehicle is tracking the trajectory. This leads to a decrease in vehicle ride comfort. The lateral velocity cost C velocity The expression is as follows:

[0079]

[0080] In the formula, For time layer The j-th sampling point The l-axis coordinate; for lane keeping, w velocity For lateral speed cost weighting; for lane changes, w is used to make the lateral lane change smoother. velocity For the lateral velocity coefficient:

[0081]

[0082] In the formula, l lane This refers to the lateral distance between the lane edge line and the reference line that the vehicle needs to cross.

[0083] In some embodiments, a dynamic programming method is used to select the optimal coarse-grained trajectory from the set of coarse-grained trajectories. This includes three steps: cost design, iterative calculation of data establishment, and iterative calculation of data retrieval. Specifically, it includes:

[0084] The optimal coarse-grained trajectory searched in step (421) consists of a linear connection sequence of each adjacent time layer in the node network, which satisfies the following condition:

[0085]

[0086] In the formula, For connecting time layer Lt With time layer The total cost of the path segment.

[0087] Step (422) Documentation and Iterative Calculation: Starting from the second time layer, for each time layer L t Each sampling point Using the Bellman optimality principle, calculate the sampling point. Compared with the previous time layer The document is constructed from a sequence of all sampling points, and the document with the lowest storage cost is selected as the document for that sampling point. For the first time layer, the sampling point is the initial state point of the vehicle, and its document is set to... The formula is as follows:

[0088]

[0089] In the formula, h represents the previous time layer. The index of the sampling point in the time layer, H is the time layer. The set of indices of all sampling points in the dataset; For time layer The value stored in the archive content of the sampling point with index h. For this sampling point to time layer L t The cost value of the trajectory segment of the sampling point with index i.

[0090] Step (423) Documentation Iterative Calculation: Starting from the last time layer, using the cost corresponding to each sampling point obtained from the documentation, select the sampling point with the minimum cost. For lane change trajectory selection, it is directly selected. Using the index k of the sampling points of the previous time layer in the archived content, the sampling points of each layer are iteratively searched up to the first layer to determine the entire trajectory point sequence, i.e., the optimal coarse-grained trajectory.

[0091] In step (42), each acceleration sampling point a can be obtained. s The optimal coarse-grained trajectory in the corresponding SLT sampling space plane is determined by designing a cost function that incorporates an ideal velocity deviation cost. This function is used to filter out the optimal coarse-grained trajectory for different sampling accelerations and lane-changing operations. The cost function is as follows:

[0092]

[0093] In the formula, v is the velocity at the moment the trajectory terminates. des i re For the ideal speed of the vehicle, w traj ,w desire These are the weighting coefficients.

[0094] In step (5), the convex space is expanded in both the horizontal and vertical directions starting from the optimal coarse-grained trajectory point until it stops when it encounters the drivable boundary, thus obtaining a three-dimensional convex space. The drivable boundary includes the longitudinal drivable boundary and the lateral drivable boundary. The longitudinal drivable boundary is determined based on the previous sampled acceleration and the next sampled acceleration of the optimal coarse-grained trajectory. The lateral drivable boundary is determined based on the maximum allowable lateral offset of the optimal coarse-grained trajectory.

[0095] When expanding the convex space, both static and dynamic obstacle state information are considered. Static obstacles mainly include road geometry, road boundary guardrails, or trees, which constitute prohibited vehicle areas across the entire time axis in the spatiotemporal joint map. Dynamic obstacles mainly refer to the predicted trajectories of surrounding vehicles, and their occupied areas in the spatiotemporal joint map change over time. The specific process for determining the three-dimensional convex space is as follows:

[0096] Starting from the optimal coarse-grained trajectory point obtained in step (4), the convex space is expanded in both the horizontal and vertical directions until it stops when it encounters the drivable boundary.

[0097] For the longitudinal drivable boundary, when the preceding and following sampled acceleration trajectories of the optimal coarse-grained trajectory exist, this indicates that within the acceleration sampling interval [a lowbound ,a upbound The longitudinal coarse-grained trajectories of vehicles within the space are all safe, and the longitudinally reachable region formed by this acceleration sampling interval is the longitudinal boundary of the convex space.

[0098] For the lateral drivable boundary, the primary consideration is the allowable lateral displacement. Similar to the longitudinal drivable boundary, the expansion of the lateral drivable boundary also starts with the coarse-grained trajectory of each time layer and expands in both positive and negative directions along the L-axis. The expansion continues until the boundary risk value of the expanded region reaches the maximum lateral offset Δl. max At that time, the expansion ends, and the lateral convex plane of the current time layer is obtained.

[0099] In step (6), with the goal of minimizing the cost of the trajectory, and with terminal constraints, continuity constraints, collision constraints and dynamic constraints as constraints, a fine-grained Bezier trajectory is determined based on the optimized trajectory model. The optimized trajectory model is constructed using n Bezier curves. The cost of the trajectory is determined by integrating the square of the trajectory's jerk value over time.

[0100] In some embodiments, the optimized trajectory model constructed by n Bézier curves under dimension σ∈{s,l} is as follows:

[0101]

[0102] In the formula, The i-th control point of the j-th segment of the Bézier curve is represented by t0, t1, ..., t. n Let α be the time dimension coordinates of the start and end points of each Bézier curve segment. j This is the asynchronous time-domain scaling factor.

[0103] To ensure passenger comfort and minimize vehicle acceleration variations during travel, the integral of the square of the jerk value over time is used as the cost function for optimization. The cost function J for the j-th segment of the Bézier curve is... j It can be represented as:

[0104]

[0105] In the formula, w s and w l The control weights are divided into vertical and horizontal weights.

[0106] To ensure the smoothness and feasibility of the trajectory, terminal constraints, continuity constraints, collision constraints, and dynamic constraints are set for the piecewise Bézier curve optimization problem.

[0107] The terminal constraint is: the starting state and ending state of the trajectory are related to the starting state of the optimal coarse-grained trajectory. With the final state The same, where σ∈{s,l}, Let be the k-th derivative of the position coordinates at time t. The terminal constraint acts at the starting point of the first convex space and the ending point of the last convex space, as shown in the following formula:

[0108]

[0109] The continuity constraint is that the k-th derivative of the obtained trajectory should be continuous at every connection point of adjacent convex spaces. The continuity constraint between the j-th convex space and the (j+1)-th convex space can be expressed as:

[0110]

[0111] Collision constraints and dynamic constraints are applied when the trajectory lies between the upper and lower boundaries of a convex space. They constrain the position, velocity, and acceleration of the trajectory, i.e., the k-th derivative of the Bézier curve (k = 0, 1, 2). Therefore, collision constraints and dynamic constraints can be expressed as:

[0112]

[0113] In the formula, and Let $\mathbf{k}$ and $\mathbf{j}$ represent the $k$-th lower and upper boundaries of the $j$-th convex space, respectively. For the above linear equality and inequality constraints, all constraints along with the cost function can be transformed into a quadratic programming (QP) form, and the existing quadratic programming solver (OOQP) can be used to quickly solve the optimal trajectory model.

[0114] The formula for quadratic programming is as follows:

[0115]

[0116] Ax=b, d≤Cx≤f, l≤x≤u

[0117] In the formula, Q is a symmetric positive semi-definite n×n matrix; x∈R n It is a variable vector; A and C are respectively of dimension m a ×n and m c A matrix of size ×n, where b, d, f, l, u are vectors of appropriate dimensions.

[0118] During trajectory solving, the control point parameters of the Bézier curve are used as optimization variables. The constraints and optimization variables are converted into corresponding matrices and vectors in the OOQP solver. The optimal control point parameters can be obtained by calling the solver command, thus obtaining the optimal piecewise Bézier trajectory, which is the fine-grained Bézier trajectory.

[0119] This invention provides a spatiotemporal joint trajectory planning method based on sparse sampling. It sparsely samples coarse-grained trajectory points of a vehicle based on longitudinal acceleration, searches the sampled points using dynamic programming to obtain the optimal coarse-grained trajectory, constructs a three-dimensional convex space (SLT) based on a spatiotemporal joint map, fully considers complex spatial obstacles and dynamic constraints, and utilizes the convex hull properties of Bézier curves to achieve optimized fine-grained trajectory generation based on Bézier curves, thereby improving computational efficiency and ensuring trajectory safety and feasibility.

[0120] The following are device embodiments of this application. For details not described in detail in the device embodiments, please refer to the above method embodiments.

[0121] An exemplary embodiment of the present invention provides a spatiotemporal joint trajectory planning device based on sparse sampling, comprising:

[0122] The acquisition module is used to acquire the vehicle's status information and the status information of dynamic and static obstacles around the vehicle;

[0123] The trajectory planning module is used to obtain a spatiotemporal joint map based on the vehicle's state information and surrounding environment information; based on the longitudinal acceleration in the vehicle's state information, it performs sparse sampling on the vehicle's coarse-grained trajectory points according to the spatiotemporal joint map to obtain a set of coarse-grained trajectory points; based on the set of coarse-grained trajectory points, it determines the optimal coarse-grained trajectory; based on the optimal coarse-grained trajectory and the spatiotemporal joint map, it determines a three-dimensional convex space; and using the convex hull property of Bézier curves, it generates a fine-grained Bézier trajectory in the three-dimensional convex space.

[0124] In summary, the control device for an assisted driving system provided in this application embodiment can solve the problems of safety risks and unstable operation of the assisted driving system when it is turned on in hazard light mode in related technologies, ensure the safety of the vehicle in hazard light mode and the stability of the assisted driving system, balance safety and convenience, and meet the user's needs for hazard lights and intelligent driving.

[0125] An exemplary embodiment of the present invention provides a control device, the control device comprising one or more processors and one or more memories, wherein the one or more memories store at least one piece of program code, the at least one piece of program code being loaded and executed by the one or more processors to implement a spatiotemporal joint trajectory planning method based on sparse sampling as described in any of the above implementations.

[0126] An exemplary embodiment of the present invention provides a computer-readable storage medium storing at least one piece of program code, which is loaded and executed by a processor to implement a spatiotemporal joint trajectory planning method based on sparse sampling as described in any of the above implementations.

[0127] An exemplary embodiment of the present invention provides a computer program product comprising computer program code stored in a computer-readable storage medium. A processor of a control device reads the computer program code from the computer-readable storage medium and executes the computer program code, causing the control device to execute a spatiotemporal joint trajectory planning method based on sparse sampling as described in any of the above implementations.

[0128] Other embodiments of this application will readily occur to those skilled in the art upon consideration of the specification and practice of the application disclosed herein. This application is intended to cover any variations, uses, or adaptations of this application that follow the general principles of this application and include common knowledge or customary techniques in the art not disclosed herein. The specification and examples are to be considered exemplary only.

[0129] It should be understood that this application is not limited to the precise structure described above and shown in the accompanying drawings, and various modifications and changes can be made without departing from its scope. The scope of this application is limited only by the appended claims.

Claims

1. A spatiotemporal joint trajectory planning method based on sparse sampling, characterized in that, The method includes: Acquire vehicle status information and the status information of dynamic and static obstacles around the vehicle; Based on the vehicle's status information and the surrounding environment information, a spatiotemporal joint map is obtained; The longitudinal acceleration of the vehicle is sampled, and the longitudinal coarse-grained sampling trajectory points of the vehicle are obtained based on the sampled acceleration. Based on the spatiotemporal joint map, track points that satisfy lane keeping and track points that satisfy lane changing requirements are sampled from the longitudinal coarse-grained track points of the vehicle to form a set of coarse-grained track points of the vehicle. By connecting the different sampling points between every two adjacent time layers in the set of coarse-grained trajectory points of the vehicle, a set of coarse-grained trajectories is obtained; Calculate the cost of each coarse-grained trajectory in the coarse-grained trajectory set, and select the coarse-grained trajectory with the minimum cost as the optimal coarse-grained trajectory. The cost of each coarse-grained trajectory includes obstacle cost, offset reference line cost, and lateral velocity cost. The three-dimensional convex space is determined based on the optimal coarse-grained trajectory and the spatiotemporal joint map; By utilizing the convex hull property of Bézier curves, fine-grained Bézier trajectories are generated in a three-dimensional convex space.

2. The spatiotemporal joint trajectory planning method based on sparse sampling as described in claim 1, characterized in that, Within the set acceleration range, the vehicle's acceleration is sampled according to the set resolution.

3. The spatiotemporal joint trajectory planning method based on sparse sampling as described in claim 1, characterized in that, Starting from the optimal coarse-grained trajectory point, the convex space is expanded in both the horizontal and vertical directions until it stops when it encounters the drivable boundary, thus obtaining a three-dimensional convex space. The drivable boundary includes the longitudinal drivable boundary and the lateral drivable boundary. The longitudinal drivable boundary is determined based on the previous sampled acceleration and the next sampled acceleration of the optimal coarse-grained trajectory. The lateral drivable boundary is determined based on the maximum allowable lateral offset of the optimal coarse-grained trajectory.

4. The spatiotemporal joint trajectory planning method based on sparse sampling as described in claim 1, characterized in that, With the objective of minimizing trajectory cost, and constrained by terminal constraints, continuity constraints, collision constraints, and dynamic constraints, a fine-grained Bessel trajectory is determined based on an optimized trajectory model. The optimized trajectory model employs... Obtained by constructing a segment of Bézier curve; the cost of the trajectory is obtained through the trajectory's... The integral of the square of the value over time is used to determine it.

5. A spatiotemporal joint trajectory planning device based on sparse sampling, characterized in that, include: The acquisition module is used to acquire the vehicle's status information and the status information of dynamic and static obstacles around the vehicle; The trajectory planning module is used to obtain a spatiotemporal joint map based on the vehicle's state information and surrounding environment information; sample the vehicle's longitudinal acceleration and obtain longitudinal coarse-grained sampled trajectory points based on the sampled acceleration; sample trajectory points that meet lane keeping and lane changing requirements from the longitudinal coarse-grained sampled trajectory points according to the spatiotemporal joint map, forming a set of coarse-grained trajectory points for the vehicle; connect the different sampled points between every two adjacent time layers in the set of coarse-grained trajectory points to obtain a set of coarse-grained trajectories; calculate the cost of each coarse-grained trajectory in the set of coarse-grained trajectories, and select the coarse-grained trajectory with the minimum cost as the optimal coarse-grained trajectory, where the cost of each coarse-grained trajectory includes obstacle cost, reference line offset cost, and lateral velocity cost; and determine the three-dimensional convex space based on the optimal coarse-grained trajectory and the spatiotemporal joint map. By utilizing the convex hull property of Bézier curves, fine-grained Bézier trajectories are generated in a three-dimensional convex space.

6. A control device, characterized in that, The control device includes one or more processors and one or more memories, wherein at least one piece of program code is stored in the one or more memories, and the at least one piece of program code is loaded and executed by the one or more processors to implement a spatiotemporal joint trajectory planning method based on sparse sampling as described in any one of claims 1 to 4.

7. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores at least one piece of program code, which is loaded and executed by a processor to implement a spatiotemporal joint trajectory planning method based on sparse sampling as described in any one of claims 1 to 4.

8. A computer program product, characterized in that, The computer program product includes computer program code stored in a computer-readable storage medium. The processor of the control device reads the computer program code from the computer-readable storage medium and executes the computer program code, causing the control device to perform a spatiotemporal joint trajectory planning method based on sparse sampling as described in any one of claims 1 to 4.

Images