Space-time joint trajectory planning method and device based on sparse sampling
Through sparse sampling and the convex hull characteristics of the Bezier curve, high-quality trajectories are generated, solving the problems of low computing efficiency and suboptimal trajectory in the prior art, and achieving efficient trajectory planning in complex scenarios.
Patent Information
- Application Number
- CN202411108892.7
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-13
- Publication Date
- 2024-11-22
- Estimated Expiration
- 2044-08-13
AI Technical Summary
The existing trajectory planning methods have low computational efficiency in complex dynamic scenarios, are prone to falling into suboptimal trajectory solutions, and the intensive sampling method is computationally large, making it difficult to generate high-quality safe and feasible fine-grained trajectories.
Using a spatiotemporal joint trajectory planning method based on sparse sampling, a spatiotemporal joint map is constructed by obtaining the status and environment information of the bicycle, sparse sampling of longitudinal acceleration, and generating Bessel fine-grained trajectory, considering complex configuration spatial obstacles and dynamic constraints, improving Computational efficiency and trajectory quality.
The calculation efficiency and quality of trajectory planning are improved, the safety and feasibility of trajectory are ensured, and the optimal fine-grained trajectory with high quality is generated.
Smart Images

Figure CN118991820A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of assisted driving technology, and in particular to a spatiotemporal joint trajectory planning method and device based on sparse sampling. Background Art
[0002] Trajectory planning is one of the key technologies in autonomous driving, and is mainly divided into two types: space-time decoupling planning and space-time joint planning. Space-time decoupling planning decomposes the space-time trajectory into path and velocity curves, and reduces the difficulty of solving the two problems by solving the path and velocity separately, but it sacrifices the optimality of the final trajectory to a certain extent, and it is easy to fall into suboptimal trajectory in complex dynamic scenes. Space-time joint planning does not decompose the path and velocity, but directly solves the optimal space-time trajectory, so it can obtain a higher quality trajectory, but the computational efficiency will be reduced. At present, 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, the algorithm lacks supervision in the process of generating new nodes, converges slowly, and the dense sampling method often has a large amount of calculation when obtaining a fine trajectory.
[0003] In summary, the efficiency of obtaining fine trajectories based on dense sampling spatiotemporal decoupling planning is low, the amount of calculation is large, and it is easy to fall into suboptimal trajectory 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 configuration space obstacles and dynamic constraints, to achieve the generation of high-quality, safe and feasible optimal fine-grained trajectories. Summary of the invention
[0004] In view of this, the present invention provides a spatiotemporal joint trajectory planning method and device based on sparse sampling, which fully considers the optimality of the trajectory under complex configuration spatial obstacles and dynamic constraints, improves the calculation efficiency and trajectory quality, and ensures the safety and feasibility of the trajectory. The technical solution is as follows:
[0005] An embodiment of the present invention provides a spatiotemporal joint trajectory planning method based on sparse sampling, the method comprising:
[0006] Obtain the vehicle status information and the dynamic and static obstacle status information around the vehicle;
[0007] According to the state information of the vehicle and the environment around the vehicle, a spatiotemporal joint map is obtained;
[0008] Based on the longitudinal acceleration in the ego vehicle state information and the spatiotemporal joint map, the ego vehicle coarse-grained trajectory points are sparsely sampled to obtain the ego vehicle coarse-grained trajectory point set;
[0009] Determine the optimal coarse-grained trajectory according to the set of coarse-grained trajectory points of the ego vehicle;
[0010] Determine the three-dimensional convex space based on the optimal coarse-grained trajectory and the spatiotemporal joint map;
[0011] The convex hull property of Bezier curves is used to generate Bezier fine-grained trajectories in three-dimensional convex space.
[0012] Optionally, the longitudinal acceleration of the ego vehicle is sampled, and based on the sampled acceleration, a coarse-grained sampling trajectory point of the longitudinal direction of the ego vehicle is obtained;
[0013] According to the spatiotemporal joint map, trajectory points that meet the lane keeping requirements and trajectory points that meet the lane changing requirements are sampled from the longitudinal coarse-grained sampling trajectory points of the ego vehicle to form a coarse-grained trajectory point set of the ego vehicle.
[0014] Optionally, within a set acceleration interval, the acceleration of the ego vehicle is sampled at a set resolution.
[0015] Optionally, different sampling points between every two adjacent time layers in the coarse-grained trajectory point set of the ego vehicle are connected to obtain a coarse-grained trajectory set;
[0016] The cost of each coarse-grained trajectory in the coarse-grained trajectory set is calculated, and the coarse-grained trajectory with the smallest cost is selected as the optimal coarse-grained trajectory, where the cost of each coarse-grained trajectory includes obstacle cost, offset reference line cost and lateral speed cost.
[0017] Optionally, taking the optimal coarse-grained trajectory point as the starting point, the convex space is expanded in the horizontal and vertical directions until it stops when it hits the drivable boundary, thereby 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 according to the previous sampled acceleration and the next sampled acceleration of the optimal coarse-grained trajectory; the lateral drivable boundary is determined according to the maximum lateral offset allowed by the optimal coarse-grained trajectory.
[0018] Optionally, with the goal of minimizing the cost of the trajectory, terminal constraints, continuity constraints, collision constraints and dynamic constraints are used as constraints, and a Bezier fine-grained trajectory is determined according to an optimized trajectory model, wherein the optimized trajectory model is constructed using n segments of Bezier curves; the cost of the trajectory is determined by integrating the square of the trajectory's jerk value over time.
[0019] The embodiment of the present invention provides a spatiotemporal joint trajectory planning device based on sparse sampling, comprising:
[0020] An acquisition module is used to obtain the vehicle status information and the dynamic and static obstacle status information around the vehicle;
[0021] The trajectory planning module is used to obtain a spatiotemporal joint map based on the state information of the ego vehicle and the environment information around the ego vehicle; based on the longitudinal acceleration in the state information of the ego vehicle and the spatiotemporal joint map, sparsely sample the coarse-grained trajectory points of the ego vehicle to obtain a set of coarse-grained trajectory points of the ego vehicle; determine the optimal coarse-grained trajectory based on the set of coarse-grained trajectory points of the ego vehicle; determine the three-dimensional convex space based on the optimal coarse-grained trajectory and the spatiotemporal joint map; and generate a Bezier fine-grained trajectory in the three-dimensional convex space using the convex hull characteristics of the Bezier curve.
[0022] An embodiment of the present invention provides a control device, which includes one or more processors and one or more memories, wherein at least one program code is stored in the one or more memories, and the at least one 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 of the above implementation methods.
[0023] An embodiment of the present invention provides a computer-readable storage medium, in which at least one program code is stored. The at least one program code 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] An embodiment of the present invention provides a computer program product, which includes a computer program code, wherein the computer program code is 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 the processor executes the computer program code, so that the control device executes a spatiotemporal joint trajectory planning method based on sparse sampling as described in any of the above implementations.
[0025] The beneficial effects brought about by the technical solution provided by the embodiment of the present invention include at least:
[0026] An embodiment of the present invention provides a space-time joint trajectory planning method based on sparse sampling, which performs sparse sampling on the coarse-grained trajectory points of the vehicle based on the longitudinal acceleration, searches the sampling points using a dynamic programming method, obtains the optimal coarse-grained trajectory, and constructs an SLT three-dimensional convex space based on the space-time joint map, thereby taking into account the spatial obstacles of complex configurations. The convex hull characteristics of the Bezier curve are used to generate Bezier fine-grained trajectories in the three-dimensional convex space, fully considering the complex configuration spatial obstacles and dynamic constraints, improving the calculation efficiency, and ensuring the trajectory safety and feasibility. BRIEF DESCRIPTION OF THE DRAWINGS
[0027] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following briefly introduces the drawings required for use in the description of the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.
[0028] Figure 1 A flowchart of a spatiotemporal joint trajectory planning method based on sparse sampling provided by an embodiment of the present invention;
[0029] Figure 2 A schematic diagram of sparse sampling of longitudinal acceleration provided by an embodiment of the present invention;
[0030] Figure 3 A schematic diagram of constructing a space-time joint convex space provided in an embodiment of the present invention. DETAILED DESCRIPTION
[0031] In order to make the objectives, technical solutions and advantages of the present invention more clear, the embodiments of the present invention will be further described in detail below with reference to the accompanying drawings.
[0032] Before explaining the embodiments of the present invention in detail, the application scenarios and system architectures involved in the embodiments of the present invention are first introduced.
[0033] First, the system architecture involved in the implementation manner of the present invention is introduced.
[0034] A system involved in a spatiotemporal joint trajectory planning method based on sparse sampling provided by an embodiment of the present invention includes a vehicle-mounted sensor and an intelligent driving controller.
[0035] The on-board sensors are used to obtain the information required for trajectory planning, including the vehicle status information and the dynamic and static obstacle status information around the vehicle. Each status information includes position, speed, acceleration, etc.
[0036] The intelligent driving controller is used to perform trajectory planning based on the information required for trajectory planning obtained by the on-board sensors to obtain the planned trajectory.
[0037] Those skilled in the art should understand that the above system architecture is only an example, and other existing or future components or modules that may appear, if applicable to the present application, should also be included in the scope of protection of the present application and are included here by reference.
[0038] Finally, the application scenarios of the method provided by the embodiment of the present invention are introduced.
[0039] The method provided by the embodiment of the present invention is applied to the scene of autonomous driving. The current spatiotemporal joint planning for autonomous driving does not decompose the path and speed, but directly solves the optimal spatiotemporal trajectory, so a higher quality trajectory can be obtained, but the computational efficiency will be reduced. At present, 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, the algorithm lacks supervision in the process of generating new nodes, and the convergence speed is slow. In addition, the dense sampling method often has a large amount of calculation when obtaining a fine trajectory.
[0040] Based on such an application scenario, an embodiment of the present application provides a spatiotemporal joint trajectory planning method based on sparse sampling.
[0041] In order to make the objectives, technical solutions and advantages of the present invention more clear, the embodiments of the present invention will be further described in detail below with reference to the accompanying drawings.
[0042] Figure 1 A method flow chart of a spatiotemporal joint trajectory planning method based on sparse sampling provided by the present invention is shown. The method can be executed by an on-board device of a vehicle, and the on-board device includes an on-board sensor and an intelligent driving controller. Figure 1 , the method comprises the following steps:
[0043] Step (1) obtains the vehicle status information and the dynamic and static obstacle status information around the vehicle.
[0044] Step (2) obtaining a spatiotemporal joint map based on the vehicle's state information and the vehicle's 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 a set of coarse-grained trajectory points of the vehicle;
[0046] Step (4) determining the optimal coarse-grained trajectory according to the set of coarse-grained trajectory points of the ego vehicle;
[0047] Step (5) determining a three-dimensional convex space according to the optimal coarse-grained trajectory and the spatiotemporal joint map;
[0048] Step (6) utilizes the convex hull property of the Bezier curve to generate a Bezier fine-grained trajectory in a three-dimensional convex space.
[0049] An embodiment of the present invention provides a spatiotemporal joint trajectory planning method based on sparse sampling. The method performs sparse sampling on the coarse-grained trajectory points of the vehicle based on longitudinal acceleration, searches the sampling points using a dynamic programming method, obtains the optimal coarse-grained trajectory, constructs an SLT three-dimensional convex space based on the spatiotemporal joint map, fully considers complex configuration spatial obstacles and dynamic constraints, and utilizes the convex hull characteristics of the Bezier curve to achieve optimized Bezier fine-grained trajectory generation, thereby improving computational efficiency and ensuring trajectory safety and feasibility.
[0050] In step (1), the vehicle state information and the dynamic and static obstacle state information around the vehicle are obtained using the vehicle-mounted sensors, each of which includes position, velocity, acceleration, etc. Obstacles are objects that constrain the behavior of the vehicle, including road geometry information, guardrails or trees at the road boundary.
[0051] In step (2), the vehicle status information and the vehicle surrounding environment information obtained in step (1) are converted into Frenet coordinates and expanded on the time axis to construct a spatiotemporal joint map in real time.
[0052] The spatiotemporal joint map M in the time period T consists of three parts, including the positions of the vehicles around the ego vehicle Static obstacle position And road geometry information The spatiotemporal joint map M is represented as follows:
[0053]
[0054] Where N D 、N S 、N L are the number of surrounding vehicles, static obstacles, and lanes respectively; are the s and l coordinates in the Frenet coordinate system; They are the width and number of the lanes; is the Frenet coordinate system coordinate of the lane centerline.
[0055] In step (3), the sparse sampling of the coarse-grained trajectory points of the vehicle includes longitudinal sparse sampling and transverse 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, within a set acceleration interval, the acceleration of the vehicle is sampled at a set resolution.
[0058] The acceleration a of the vehicle in the s-axis direction s In [amax ,a min ] interval with a resolution of δ a Sampling is performed to obtain the acceleration set A, and the planning period [0,t max ] with a resolution of δ t The sampled time set T is the planning time set. Assuming that the vehicle is uniformly accelerated or decelerated in a short period of time, to each acceleration sampling point a s The corresponding coarse-grained sampling trajectory point set ST of the vehicle longitudinal direction is:
[0059]
[0060] In the formula, a min ,a max are the maximum deceleration and maximum acceleration, t max is the time period of single trajectory planning, s 0 is the s coordinate in the Frenet coordinate system at the initial moment, is the velocity in the s direction at the initial moment.
[0061] Step (32) samples trajectory points that meet lane keeping requirements and trajectory points that meet lane changing requirements from the longitudinal coarse-grained sampling trajectory points of the ego vehicle according to the spatiotemporal joint map, to form a coarse-grained trajectory point set of the ego vehicle.
[0062] The process of sampling trajectory points that meet lane keeping requirements is as follows: Taking the center line of the current lane 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 divided into sampling interval δ lk Perform equidistant dispersion to obtain the horizontal sampling point set L k , the set of trajectory points SLT that satisfy lane keeping corresponding to each acceleration sampling point k for:
[0063]
[0064] In the formula, Δl max is the maximum allowable displacement of the smart car in the l-axis direction, f Δ is a constant to be determined.
[0065] The process of sampling trajectory points that meet the lane change 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], that is, the lateral distance l of the vehicle's current position ego To the target lane reference line, when the vehicle is on the left side of the reference line, ego is negative when the vehicle is on the right side of the reference line. egois positive, in order to ensure that the vehicle completes the lane change within the planning period, the sampling point of the last time layer is set on the reference line. lc Perform equidistant dispersion to obtain the horizontal sampling point set L c , the set of trajectory points that meet the lane change requirements is SLT c :
[0066]
[0067] In step (4), different sampling points between every two adjacent time layers in the coarse-grained trajectory point set of the vehicle are connected to obtain a coarse-grained trajectory set; the cost of each coarse-grained trajectory in the coarse-grained trajectory set is calculated, and the coarse-grained trajectory with the smallest cost is selected as the optimal coarse-grained trajectory, where the cost of each coarse-grained trajectory includes obstacle cost, offset reference line cost and lateral speed cost.
[0068] In some embodiments, at each acceleration sampling point a s In the corresponding SLT sampling point set, linear segments are used to connect different sampling points between every two adjacent time layers to obtain a set of coarse-grained trajectories between adjacent time layers. The cost of each coarse-grained trajectory in the coarse-grained trajectory set is calculated using the coarse-grained trajectory cost function.
[0069] The steps of constructing a coarse-grained trajectory cost function and searching for the optimal coarse-grained trajectory include:
[0070] Step (41) constructs a coarse-grained trajectory cost function, time layer L t The i-th sampling point in With time layer The jth sampling point in The cost of the connection trajectory Including obstacle cost C obs , offset reference line cost C offset and the lateral velocity cost C velocity :
[0071]
[0072] Among them, for a given trajectory, the obstacle cost C corresponding to the trajectory obs Determined by the distance d from the obstacle boundary to the trajectory in the environment:
[0073]
[0074] Where, d n is the maximum impact boundary of the obstacle, which is determined by the shape, state and other characteristics of the obstacle; d c is the collision boundary of the obstacle; c nu deg is a monotonically decreasing function; w obs is the obstacle cost weight coefficient.
[0075] The vehicle needs to drive along the center line of the road as much as possible to avoid interfering with vehicles in other lanes. offset Reflects the degree to which the vehicle deviates from the center line of the current lane, the cost of the deviation reference line C offset for:
[0076]
[0077] In the formula, The time layer L t The i-th sampling point in The l-axis coordinate of offset is the lateral offset cost weight coefficient.
[0078] The greater the lateral speed cost, the greater the displacement of the vehicle in the l-axis direction of the connecting line between two adjacent time layers, resulting in a greater lateral speed of the vehicle when tracking the trajectory, and the vehicle's driving comfort decreases. The lateral speed cost C velocity The expression is as follows:
[0079]
[0080] In the formula, For the time layer The jth sampling point in The l-axis coordinate of the lane keeping function; for lane keeping, w velocity is the lateral speed cost weight; for lane changing, in order to make the lateral lane change of the vehicle smoother, w velocity is the lateral velocity coefficient:
[0081]
[0082] In the formula, l lane It is the lateral distance between the lane edge 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 coarse-grained trajectory set, including three steps: cost design, file creation iterative calculation, and file search iterative calculation, specifically including:
[0084] The optimal coarse-grained trajectory searched out in step (421) is composed of a linear connection sequence of each adjacent time layer in the node network, and the sequence satisfies the following conditions:
[0085]
[0086] In the formula, is the connection time layer Lt With time layer The total cost of the path segment.
[0087] Step (422) file creation 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 With the previous time layer The archive content sequence composed of all sampling points in , and the archive content with the minimum cost stored in it is selected as the archive content of the sampling point. For the first time layer, the sampling point is the initial state point of the vehicle, and its archive content is set to The formula is as follows:
[0088]
[0089] Where h is the previous time layer The index of the sampling point in, H is the time layer The index set of all sampling points in; For the time layer The cost value stored in the archive content of the sampling point with index h in , From the sampling point to the time layer L t The cost value of the trajectory segment of the sampling point with index i in .
[0090] Step (423) Iterative calculation of file query: Starting from the last time layer, use the cost corresponding to each sampling point obtained by file creation to select the sampling point with the minimum cost For lane-changing trajectory selection, it is directly selected. Using the index k of the sampling point of the previous time layer in the archive content, the sampling points of each layer are iteratively searched to the first layer in turn, and the entire trajectory point sequence, that is, the optimal coarse-grained trajectory, can be determined.
[0091] Step (43) 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 designed, and the cost function is introduced to introduce the ideal speed deviation cost to screen out the optimal coarse-grained trajectory between different sampling accelerations and lane changing operations. The cost function is as follows:
[0092]
[0093] In the formula, is the velocity at the end of the trajectory, v des i re is the ideal speed of the vehicle, w traj ,w desire is the weight coefficient.
[0094] In step (5), 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 hits the drivable boundary, thereby 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 according to the previous sampled acceleration and the next sampled acceleration of the optimal coarse-grained trajectory; the lateral drivable boundary is determined according to the maximum lateral offset allowed by the optimal coarse-grained trajectory.
[0095] When expanding the convex space, the state information of dynamic and static obstacles is taken into account. Static obstacles mainly include road geometry information, guardrails or trees at the road boundary, etc. They are areas where vehicles are prohibited from driving in the entire time axis span in the spatiotemporal joint map. Dynamic obstacles mainly refer to the predicted driving trajectories of surrounding vehicles, etc. The area they occupy in the spatiotemporal joint map will change over time. The specific process of determining the three-dimensional convex space is:
[0096] Taking the optimal coarse-grained trajectory point obtained in step (4) as the starting point, expand the convex space in both the horizontal and vertical directions until it reaches the drivable boundary;
[0097] For the longitudinal drivable boundary, when the previous sampled acceleration trajectory of the optimal coarse-grained trajectory and the next sampled acceleration trajectory exist, it means that in the acceleration sampling interval [a lowbound ,a upbound ]The longitudinal coarse-grained trajectories of vehicles within the acceleration sampling interval are all safe, and the longitudinal reachable area formed by the acceleration sampling interval is the longitudinal boundary of the convex space;
[0098] For the lateral drivable boundary, the main factor to be considered is the allowable lateral displacement. Similar to the longitudinal drivable boundary acquisition, the expansion of the lateral drivable boundary also starts from the coarse-grained trajectory of each time layer and expands in the positive and negative directions of the L axis. When the boundary risk value of the expanded area reaches the maximum lateral displacement Δl max When , the expansion ends and the transverse convex plane of the current time layer is obtained.
[0099] In step (6), the goal is to minimize the cost of the trajectory, and the terminal constraints, continuity constraints, collision constraints and dynamic constraints are used as constraints. According to the optimized trajectory model, the Bezier fine-grained trajectory is determined, wherein the optimized trajectory model is constructed using n segments of 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 Bezier curves under dimension σ∈{s,l} is:
[0101]
[0102] In the formula, represents the i-th control point of the j-th Bezier curve; t 0 ,t 1 ,…,t n is the time dimension coordinate of the starting point and the ending point of each Bezier curve, α j is the asynchronous time domain scaling factor.
[0103] In order to ensure that the acceleration change of the vehicle during driving should not be too large to ensure the riding comfort of the passengers, the integral of the square of the jerk value of the trajectory over time is used as the optimized cost function. j It can be expressed as:
[0104]
[0105] In the formula, w s and w l Divided into vertical and horizontal control weights.
[0106] In order to ensure the smoothness and feasibility of the trajectory, terminal constraints, continuity constraints, collision constraints and dynamic constraints are set for the piecewise Bezier curve optimization problem.
[0107] Among them, the terminal constraints are: the starting state and the end state of the trajectory and the starting state of the optimal coarse-grained trajectory With the end state Same, where σ∈{s,l}, is the k-order derivative of the position coordinate at time t. The terminal constraint acts on the starting point of the first convex space and the ending point of the last convex space. The formula is as follows:
[0108]
[0109] The continuity constraint is: the k-order derivative of the obtained trajectory should be continuous at each 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] The collision constraint and dynamic constraint are that the trajectory is between the upper and lower boundaries of the convex space, and the position, velocity and acceleration of the trajectory are constrained, that is, the k=0, 1, 2 derivatives of the Bezier curve. Therefore, the collision constraint and dynamic constraint can be expressed as:
[0112]
[0113] In the formula, and They represent the k-th order lower and upper boundaries of the j-th convex space, respectively. For the above linear equality and inequality constraints, all constraints together 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 optimization trajectory model.
[0114] The quadratic programming formula is as follows:
[0115]
[0116] Ax=b,d≤Cx≤f,l≤x≤u,
[0117] Where Q is a symmetric positive semidefinite n×n matrix; x∈R n is a variable vector; A and C are respectively a ×n and m c ×n matrix, b, d, f, l, u are vectors of appropriate dimensions.
[0118] When solving the trajectory, the control point parameters of the Bezier curve are used as optimization variables to convert the constraints and optimization variables into the corresponding matrices and vectors of the OOQP solver. The optimal control point parameters can be obtained by calling the solving command, and then the optimal piecewise Bezier trajectory can be obtained, which is a Bezier fine-grained trajectory.
[0119] An embodiment of the present invention provides a spatiotemporal joint trajectory planning method based on sparse sampling. The method performs sparse sampling on the coarse-grained trajectory points of the vehicle based on longitudinal acceleration, searches the sampling points using a dynamic programming method, obtains the optimal coarse-grained trajectory, constructs an SLT three-dimensional convex space based on the spatiotemporal joint map, fully considers complex configuration spatial obstacles and dynamic constraints, and utilizes the convex hull characteristics of the Bezier curve to achieve optimized Bezier fine-grained trajectory generation, thereby improving computational efficiency and ensuring trajectory safety and feasibility.
[0120] The following are device embodiments of the present application. For details not described in detail in the device embodiments, reference may be made 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] An acquisition module is used to obtain the vehicle status information and the dynamic and static obstacle status information around the vehicle;
[0123] The trajectory planning module is used to obtain a spatiotemporal joint map based on the state information of the ego vehicle and the environment information around the ego vehicle; based on the longitudinal acceleration in the state information of the ego vehicle and the spatiotemporal joint map, sparsely sample the coarse-grained trajectory points of the ego vehicle to obtain a set of coarse-grained trajectory points of the ego vehicle; determine the optimal coarse-grained trajectory based on the set of coarse-grained trajectory points of the ego vehicle; determine the three-dimensional convex space based on the optimal coarse-grained trajectory and the spatiotemporal joint map; and generate a Bezier fine-grained trajectory in the three-dimensional convex space using the convex hull characteristics of the Bezier curve.
[0124] To sum up, the control device of an assisted driving system provided in an embodiment of the present application can solve the problem in the related art that when the assisted driving system is turned on in double flash mode, there are safety risks and unstable operation, ensure the safety of the vehicle when the double flash mode is turned on, and the stability of the assisted driving system operation, take into account safety and convenience, and meet users' needs for double flash and intelligent driving.
[0125] An exemplary embodiment of the present invention provides a control device, which includes one or more processors and one or more memories, wherein at least one program code is stored in the one or more memories, and the at least one 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 of the above implementation methods.
[0126] An exemplary embodiment of the present invention provides a computer-readable storage medium, in which at least one program code is stored, and the at least one program code 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, which includes a computer program code, and the computer program code is 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 the processor executes the computer program code, so that the control device executes a spatiotemporal joint trajectory planning method based on sparse sampling as described in any of the above implementations.
[0128] Those skilled in the art will readily appreciate other embodiments of the present application after considering the specification and practicing the present application disclosed herein. The present application is intended to cover any variations, uses or adaptations of the present application, which follow the general principles of the present application and include common knowledge or customary techniques in the art that are not disclosed in the present application. The specification and examples are intended to be exemplary only.
[0129] It should be understood that the present application is not limited to the precise structures that have been described above and shown in the drawings, and that various modifications and changes may be made without departing from the scope thereof. The scope of the present 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 comprises: Obtain the vehicle status information and the dynamic and static obstacle status information around the vehicle; According to the state information of the vehicle and the environment around the vehicle, a spatiotemporal joint map is obtained; Based on the longitudinal acceleration in the ego vehicle state information and the spatiotemporal joint map, the ego vehicle coarse-grained trajectory points are sparsely sampled to obtain the ego vehicle coarse-grained trajectory point set; Determine the optimal coarse-grained trajectory according to the set of coarse-grained trajectory points of the ego vehicle; Determine the three-dimensional convex space based on the optimal coarse-grained trajectory and the spatiotemporal joint map; The convex hull property of Bezier curves is used to generate Bezier fine-grained trajectories in three-dimensional convex space.
2. A spatiotemporal joint trajectory planning method based on sparse sampling as claimed in claim 1, characterized in that: The longitudinal acceleration of the vehicle is sampled, and based on the sampled acceleration, the longitudinal coarse-grained sampling trajectory points of the vehicle are obtained; According to the spatiotemporal joint map, trajectory points that meet the lane keeping requirements and trajectory points that meet the lane changing requirements are sampled from the longitudinal coarse-grained sampling trajectory points of the ego vehicle to form a coarse-grained trajectory point set of the ego vehicle.
3. A spatiotemporal joint trajectory planning method based on sparse sampling as claimed in claim 2, characterized in that: Within the set acceleration range, the acceleration of the vehicle is sampled according to the set resolution.
4. The spatiotemporal joint trajectory planning method based on sparse sampling according to claim 1, characterized in that: Connect different sampling points between every two adjacent time layers in the coarse-grained trajectory point set of the ego vehicle to obtain a coarse-grained trajectory set; The cost of each coarse-grained trajectory in the coarse-grained trajectory set is calculated, and the coarse-grained trajectory with the smallest cost is selected as the optimal coarse-grained trajectory, where the cost of each coarse-grained trajectory includes obstacle cost, offset reference line cost and lateral speed cost.
5. The spatiotemporal joint trajectory planning method based on sparse sampling according to claim 1, characterized in that: Taking the optimal coarse-grained trajectory point as the starting point, the convex space is expanded in both the horizontal and vertical directions until it stops when it hits 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 according to the previous sampled acceleration and the next sampled acceleration of the optimal coarse-grained trajectory; the lateral drivable boundary is determined according to the maximum lateral offset allowed by the optimal coarse-grained trajectory.
6. A spatiotemporal joint trajectory planning method based on sparse sampling as claimed in claim 1, characterized in that: With the goal of minimizing the cost of the trajectory, terminal constraints, continuity constraints, collision constraints and dynamic constraints are used as constraints, and the Bezier fine-grained trajectory is determined according to the optimized trajectory model. The optimized trajectory model is constructed using n-segment Bezier curves; the cost of the trajectory is determined by integrating the square of the trajectory's jerk value over time.
7. A spatiotemporal joint trajectory planning device based on sparse sampling, characterized in that: include: An acquisition module is used to obtain the vehicle status information and the dynamic and static obstacle status information around the vehicle; The trajectory planning module is used to obtain a spatiotemporal joint map based on the state information of the ego vehicle and the environment information around the ego vehicle; Based on the longitudinal acceleration in the ego vehicle state information and according to the spatiotemporal joint map, the ego vehicle coarse-grained trajectory points are sparsely sampled to obtain the ego vehicle coarse-grained trajectory point set; the optimal coarse-grained trajectory is determined according to the ego vehicle coarse-grained trajectory point set; the three-dimensional convex space is determined according to the optimal coarse-grained trajectory and the spatiotemporal joint map; the Bezier fine-grained trajectory is generated in the three-dimensional convex space by utilizing the convex hull characteristics of the Bezier curve.
8. A control device, characterized in that: The control device includes one or more processors and one or more memories, wherein at least one program code is stored in the one or more memories, and the at least one 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 6.
9. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores at least one program code, and the at least one program code is loaded and executed by the processor to implement the spatiotemporal joint trajectory planning method based on sparse sampling as described in any one of claims 1 to 6.
10. A computer program product, characterized in that The computer program product includes a computer program code, which is 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 the processor executes the computer program code, so that the control device executes a spatiotemporal joint trajectory planning method based on sparse sampling as described in any one of claims 1 to 6.
Citation Information
Patent Citations
Staged automatic driving vehicle trajectory planning method
CN115730756A
Automatic driving real-time trajectory planning method for dynamic structured road
CN115903854A
Driving decision planning method and device considering traffic semantic information under three-dimensional time-space
CN118254826A
Automatic generation of dimensionally reduced maps and spatiotemporal localization for navigation of a vehicle
US20200133272A1
Cited By
Lane changing path planning method and device, vehicle and computer program product
CN119705453A
Medical robot autonomous path planning method based on convex optimization and related device
CN120643305A
Sparse sampling-based spatio-temporal joint trajectory planning method and device
EP4806753A1
Sparse sampling-based spatio-temporal joint trajectory planning method and device
WO2026037346A1
