Sparse sampling-based spatio-temporal joint trajectory planning method and device

Abstract

A sparse sampling-based spatio-temporal joint trajectory planning method and device. The method comprises: acquiring state information of an ego vehicle and state information of a dynamic / static obstacle surrounding the ego vehicle (S1); acquiring a spatio-temporal joint map on the basis of the state information of the ego vehicle and surrounding environment information of the ego vehicle (S2); on the basis of the longitudinal acceleration in the state information of the ego vehicle and the spatio-temporal joint map, performing sparse sampling on coarse-grained trajectory points of the ego vehicle to obtain a coarse-grained trajectory point set of the ego vehicle (S3); determining an optimal coarse-grained trajectory on the basis of the coarse-grained trajectory point set of the ego vehicle (S4); determining a three-dimensional convex space on the basis of the optimal coarse-grained trajectory and the spatio-temporal joint map (S5); and generating a Bezier fine-grained trajectory in the three-dimensional convex space according to the convex hull property of a Bezier curve (S6).

Description

A sparsely-sampled space-time joint trajectory planning method and device

[0001] The present application claims priority to the Chinese Patent Application No. 202411108892.7, filed on August 13, 2024, and entitled "A Sparsely-Sampled Space-Time Joint Trajectory Planning Method and Device", the content of which is incorporated herein by reference in its entirety. TECHNICAL FIELD

[0002] The present application relates to the field of assisted driving, in particular to a sparsely-sampled space-time joint trajectory planning method and device. BACKGROUND

[0003] Trajectory planning is one of the key technologies in autonomous driving, which is mainly divided into space-time decoupling planning and space-time joint planning. Space-time decoupling planning decomposes the space-time trajectory into path and speed curve, and solves the path and speed respectively to reduce the difficulty of solving the two problems, but at the expense of the optimality of the final trajectory to some extent, which is easy to fall into suboptimal trajectory in complex dynamic scenarios. Space-time joint planning does not decompose the path and speed, but directly solves the optimal space-time trajectory, so it can obtain a higher quality trajectory, but the calculation efficiency will be reduced. Most trajectory generation schemes currently use dense sampling to obtain trajectory points, such as the Rapidly-exploring Random Tree (RRT) algorithm, which can quickly and effectively search for a path in space, but the algorithm lacks supervision in the process of generating new nodes, and the convergence speed is slow, and the dense sampling method often has a large amount of calculation when obtaining a fine trajectory.

[0004] In summary, the space-time decoupling planning based on dense sampling has low efficiency and large amount of calculation when obtaining a fine trajectory, and is easy to fall into a suboptimal trajectory. Therefore, it is necessary to consider the complexity and real-time performance of trajectory planning, as well as the optimality of trajectory under complex configuration space obstacles and dynamic constraints, to realize the generation of high-quality safe and feasible optimal fine-grained trajectory. SUMMARY

[0005] Therefore, the present application provides a sparsely-sampled space-time joint trajectory planning method and device, which fully considers the optimality of trajectory under complex configuration space obstacles and dynamic constraints, improves the calculation efficiency and trajectory quality, and guarantees the safety and feasibility of the trajectory. The technical solution is as follows:

[0006] The present application provides a sparsely-sampled space-time joint trajectory planning method, which is executed by an electronic device, and the method comprises:

[0007] Obtain the ego vehicle state information and the dynamic / static obstacle state information around the ego vehicle;

[0008] obtain a spatio-temporal map according to the state information of the ego vehicle and the surrounding environment information of the ego vehicle;

[0009] obtain a set of coarse-grained trajectory points of the ego vehicle according to the spatio-temporal map based on the longitudinal acceleration in the state information of the ego vehicle;

[0010] determine an optimal coarse-grained trajectory according to the set of coarse-grained trajectory points of the ego vehicle;

[0011] determine a three-dimensional convex space according to the optimal coarse-grained trajectory and the spatio-temporal map;

[0012] generate a Bezier fine-grained trajectory in the three-dimensional convex space by using the convex hull property of the Bezier curve.

[0013] Optionally, the longitudinal acceleration of the ego vehicle is sampled, and a coarse-grained sampled trajectory point of the ego vehicle in the longitudinal direction is obtained according to the sampled acceleration.

[0014] obtain the set of coarse-grained trajectory points of the ego vehicle by sampling trajectory points meeting the lane keeping and trajectory points meeting the lane changing requirement from the coarse-grained sampled trajectory point of the ego vehicle in the longitudinal direction according to the spatio-temporal map.

[0015] Optionally, the acceleration of the ego vehicle is sampled in a set acceleration interval according to a set resolution.

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

[0017] 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, wherein the cost of each coarse-grained trajectory includes an obstacle cost, a deviation reference line cost, and a lateral velocity cost.

[0018] Optionally, the three-dimensional convex space is obtained by expanding the convex space in the lateral and longitudinal directions from the optimal coarse-grained trajectory point as a starting point until the drivable boundary is encountered, wherein 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; and the lateral drivable boundary is determined according to the maximum lateral deviation allowed by the optimal coarse-grained trajectory.

[0019] Optionally, the Bezier fine-grained trajectory is determined according to an optimal trajectory model with the minimum cost of the trajectory as a target and with terminal constraints, continuity constraints, collision constraints, and dynamic constraints as constraint conditions, wherein the optimal trajectory model is obtained by constructing an n-segment Bezier curve; and the cost of the trajectory is determined by integrating the square of the jerk value of the trajectory with respect to time.

[0020] The embodiment of the application provides a kind of based on sparse sampling space-time joint trajectory planning device, comprising:

[0021] The acquisition module is used to acquire the state information of the ego vehicle and the state information of the dynamic and static obstacles around the ego vehicle.

[0022] The trajectory planning module is used to obtain a space-time joint map according to the state information of the ego vehicle and the environmental information around the ego vehicle, perform sparse sampling on coarse-grained trajectory points of the ego vehicle based on the longitudinal acceleration in the state information of the ego vehicle according to the space-time joint map, and obtain a set of coarse-grained trajectory points of the ego vehicle, determine an optimal coarse-grained trajectory according to the set of coarse-grained trajectory points of the ego vehicle, determine a three-dimensional convex space according to the optimal coarse-grained trajectory and the space-time joint map, and generate a Bezier fine-grained trajectory in the three-dimensional convex space by using the convex hull property of the Bezier curve.

[0023] The embodiment of the application provides a control device, which comprises one or more processors and one or more memories, and at least one piece of program code is stored in the one or more memories.

[0024] The embodiment of the application provides a computer-readable storage medium, and at least one piece of program code is stored in the computer-readable storage medium.

[0025] The embodiment of the application provides a computer program product, which comprises computer program code stored in a computer-readable storage medium.

[0026] The technical scheme provided by the embodiment of the application has at least the following beneficial effects:

[0027] The embodiment of the application provides a kind of based on sparse sampling space-time joint trajectory planning method, based on longitudinal acceleration to the car coarse-grained trajectory point is sparsely sampled, search is carried out to sampling point using dynamic programming method, obtain optimal coarse-grained trajectory, according to space-time joint map constructs S-L-T three-dimensional convex space, so as to consider the space barrier of complex configuration, will utilize the convex hull characteristics of Bezier curve, generate Bezier fine-grained trajectory in three-dimensional convex space, fully consider complex configuration space barrier and dynamic constraint, improve computational efficiency, guarantee trajectory safety and feasibility. BRIEF DESCRIPTION OF DRAWINGS

[0028] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings needed in the embodiment description will be briefly introduced. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can also be obtained by those skilled in the art without creative labor.

[0029] Fig. 1 is a flowchart of a space-time joint trajectory planning method based on sparse sampling provided by the embodiment of the present application;

[0030] Fig. 2 is a schematic diagram of longitudinal acceleration sparse sampling provided by the embodiment of the present application;

[0031] Fig. 3 is a schematic diagram of space-time joint convex space construction provided by the embodiment of the present application. DETAILED DESCRIPTION

[0032] In order to make the purpose, technical solutions and advantages of the present application more clear, the embodiments of the present application will be further described in detail with reference to the drawings.

[0033] Before explaining the embodiments of the present application in detail, the application scenarios and system architecture involved in the embodiments of the present application will be introduced.

[0034] Firstly, the system architecture involved in the embodiments of the present application will be introduced.

[0035] The system involved in the space-time joint trajectory planning method based on sparse sampling provided by the embodiment of the present application includes vehicle-mounted sensor and intelligent driving controller.

[0036] The vehicle-mounted sensor is used to obtain the information required for trajectory planning, including the state information of the ego vehicle and the state information of dynamic and static obstacles around the ego vehicle, each state information includes position, velocity and acceleration, etc.

[0037] The intelligent driving controller is used to perform trajectory planning according to the information required for trajectory planning obtained by the vehicle-mounted sensor, and obtain the planned trajectory.

[0038] Those skilled in the art shall understand that the system architecture described above is only an example, and other existing or future components or modules that can be applicable to the present application shall also be included in the protection scope of the present application and are hereby incorporated by reference.

[0039] Finally, the application scenarios of the method provided by the embodiments of the present application are introduced.

[0040] The method provided by the embodiments of the present application is applied to an automatic driving scenario. The current spatiotemporal joint planning for automatic driving does not decompose the path and the speed, directly solves the optimal spatiotemporal trajectory, and thus a higher quality trajectory can be obtained, but the calculation efficiency is reduced. Most of the current trajectory generation schemes adopt a dense sampling method to obtain trajectory points. For example, the RRT algorithm can quickly and effectively search for a path in space, but the algorithm lacks supervision in the process of generating a new node, the convergence speed is slow, and the dense sampling method often has a large amount of calculation when obtaining a fine trajectory.

[0041] Based on such an application scenario, the embodiments of the present application provide a spatiotemporal joint trajectory planning method based on sparse sampling.

[0042] To make the purpose, technical solutions and advantages of the present application clearer, the embodiments of the present application will be further described in detail below with reference to the drawings.

[0043] FIG. 1 shows a method flowchart of a spatiotemporal joint trajectory planning method based on sparse sampling provided by the present application. The method can be executed by a vehicle-mounted device of a vehicle, and the vehicle-mounted device includes a vehicle-mounted sensor and an intelligent driving controller. Referring to FIG. 1, the method includes the following steps:

[0044] Step (S1) obtains self-vehicle state information and dynamic and static obstacle state information around the self-vehicle.

[0045] When obtaining the self-vehicle state information, the real-time position (latitude and longitude, lateral offset in the lane), instantaneous speed (longitudinal speed, lateral speed), acceleration (longitudinal acceleration, lateral acceleration) and heading angle of the self-vehicle can be collected in real time by the vehicle-mounted sensor. When obtaining the dynamic and static obstacle state information, the static obstacles include road boundaries (guardrails, kerbs), traffic signs (stop lines, no-passing zones), fixed buildings, etc., and the dynamic obstacles include other vehicles (position, speed, acceleration, driving direction), pedestrians, non-motor vehicles, etc. The multi-source data is denoised and calibrated by a sensor fusion technology to ensure the data accuracy. At the same time, the future motion trajectory of the dynamic obstacle is short-term predicted (for example, based on a constant speed model or an interactive perception model) to provide time dimension obstacle information for subsequent spatiotemporal joint map construction.

[0046] Step (S2) obtains a space-time joint map according to the state information of the ego vehicle and the surrounding environment information of the ego vehicle.

[0047] Converting the ego vehicle and obstacle information into a Frenet coordinate system (taking the road center line as a reference line, s as a longitudinal distance along the reference line, and l as a lateral offset) can simplify the geometric description of path planning. The construction of the space-time joint map needs to combine the time axis to form a three-dimensional (s, l, t) space, in which the position information of the dynamic obstacle is dynamically updated over time t (such as predicting the s-l coordinates of the front vehicle at t=1s and t=2s), and the static obstacle maintains fixed s-l coordinates throughout the planning period. The boundaries of the drivable area (such as lane lines and no-entry areas) also need to be marked in the map to provide a constraint range for subsequent trajectory point sampling.

[0048] Step (S3) performs sparse sampling of the coarse-grained trajectory points of the ego vehicle based on the longitudinal acceleration in the state information of the ego vehicle and the space-time joint map to obtain a set of coarse-grained trajectory points of the ego vehicle.

[0049] When sampling the longitudinal acceleration, the initial sampling interval is set according to vehicle performance parameters, and the resolution is dynamically adjusted according to the driving scene (such as reducing the resolution in high-speed scenes to reduce the amount of calculation, and increasing the resolution in low-speed congestion scenes to ensure flexibility). When screening the lateral sampling points, the lane line positions in the space-time joint map need to be combined: for lane-keeping scenes, the l coordinates of the sampling points need to fall within the current lane width range; for lane-changing scenes, the sampling points need to cover the target lane and ensure that the lateral displacement is completed within the planning period. After sampling, trajectory points that conflict with obstacles need to be removed.

[0050] Step (S4) determines an optimal coarse-grained trajectory based on the set of coarse-grained trajectory points of the ego vehicle.

[0051] When connecting adjacent time layers of sampling points, linear interpolation is used to generate coarse-grained trajectory segments to ensure that the trajectory is continuous in the time dimension. When calculating the trajectory cost, the obstacle cost needs to be weighted according to the obstacle type (such as giving a higher weight to pedestrians than to static guardrails), the offset reference line cost needs to be combined with the road curvature (the tolerance for offset is lower in curved road scenes), and the lateral velocity cost needs to be limited within a comfortable range (such as a lateral acceleration ≤ 2 m / s2). When searching for the optimal coarse trajectory through dynamic programming, the minimum cost path of each sampling point is stored by "filing", and the global optimal solution is found by "checking the file" from the end point in reverse, which avoids unreasonable trajectories caused by local optimization (such as excessive offset or sharp turns).

[0052] Step (S5) determines a three-dimensional convex space based on the optimal coarse-grained trajectory and the space-time joint map.

[0053] The inflation process of the three-dimensional convex space needs to be performed in time layers, and the longitudinal forward and backward inflation needs to refer to the sampling acceleration range of the front and rear time layers to ensure that the longitudinal motion conforms to the vehicle dynamic constraints; when the lateral inflation is performed to the left and right, the road drivable width (such as the current lane remaining width, whether the adjacent lane can be borrowed) and the obstacle lateral boundary (such as the left guardrail, the l-axis left boundary is the guardrail position) need to be referred to, and the inflation stop condition is to touch the obstacle boundary or reach the maximum lateral offset. The finally formed three-dimensional convex space needs to cover all the areas that the safe and feasible fine-grained trajectory can pass through.

[0054] In step (S6), the Bezier fine-grained trajectory is generated in the three-dimensional convex space by using the convex hull property of the Bezier curve.

[0055] The order of the Bezier curve is selected according to the size of the convex space, and the initial value of the control point is set based on the optimal coarse trajectory point. In the optimization process, the terminal constraint ensures that the fine trajectory is completely coincident with the coarse trajectory at the beginning and the end (position, speed, and acceleration are consistent), the continuity constraint requires that the first derivative (speed) and the second derivative (acceleration) of adjacent Bezier curve segments at the connection point are equal to avoid trajectory mutation. The collision constraint is realized by checking whether the convex hull of the Bezier curve is completely located within the three-dimensional convex space (if the convex hull vertex does not exceed the space boundary, the trajectory has no collision risk). The dynamic constraint needs to limit the longitudinal acceleration, the lateral acceleration, and the steering angle, and finally the optimal control point is quickly obtained through the quadratic programming solver to generate the fine-grained trajectory that satisfies all the constraints.

[0056] The embodiment of the application provides a kind of based on sparse sampling space-time joint trajectory planning method, based on longitudinal acceleration, the coarse-grained trajectory point of ego vehicle is sparsely sampled, the sampling point is searched using dynamic programming method, and optimal coarse-grained trajectory is obtained, according to space-time joint map, S-L-T three-dimensional convex space is constructed, the complex configuration space obstacle and dynamic constraint are fully considered, the convex hull property of Bezier curve is used, and the Bezier fine-grained trajectory based on optimization is generated, the calculation efficiency is improved, and the safety and feasibility of trajectory are guaranteed.

[0057] In step (S1), the state information of the ego vehicle and the state information of the dynamic and static obstacles around the ego vehicle are obtained using the vehicle-mounted sensor, and each state information includes position, speed and acceleration, etc. The obstacle is an object that constrains the behavior of the vehicle, including road geometry information, road boundary guardrails or trees.

[0058] In step (S2), the state information of the ego vehicle and the environment information around the ego vehicle obtained in step (S1) are converted into Frenet coordinates and expanded on the time axis to obtain a space-time joint map in real time.

[0059] The space-time joint map M in the time period T is composed 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 shown in the following formula 1:

[0060] Wherein, N D , N S , N L are the number of surrounding vehicles, static obstacles, and lanes respectively;[s t,n , l t,n ] are the s, l coordinates in the Frenet coordinate system; , and l are the width and number of the lane respectively.

[0061] In step (S3), the sparse sampling of the ego vehicle coarse-grained trajectory points includes longitudinal sparse sampling and transverse sparse sampling, and the specific steps include:

[0062] Step (S31) samples the longitudinal acceleration of the ego vehicle, and obtains the ego vehicle longitudinal coarse-grained sampling trajectory points according to the sampled acceleration.

[0063] In some embodiments, the acceleration of the ego vehicle is sampled at a set resolution within a set acceleration interval.

[0064] The acceleration a s of the vehicle in the s-axis direction is sampled within the interval [amin max ] at a resolution δ a to obtain an acceleration set A, and the planning period [0, t max ] is sampled at a resolution δ t to obtain a planning time set T, assuming that the vehicle is uniformly accelerated or uniformly decelerated in a short time, and each acceleration sampling point a s corresponds to an ego vehicle longitudinal coarse-grained sampling trajectory point set ST, as shown in the following formula 2:

[0065] Wherein, a min , a max are the maximum deceleration and the maximum acceleration respectively, t max is the time period of single trajectory planning, s0 is the s coordinate in the Frenet coordinate system at the initial time, is the s-direction velocity at the initial time.

[0066] As shown in FIG. 2, which shows the sampling interval and sampling point distribution rule of longitudinal acceleration in the present application, the vertical axis represents the longitudinal acceleration, and the horizontal axis represents the planning time. The upper longitudinal boundary 201 and the lower longitudinal boundary 202 correspond to the maximum acceleration and the maximum deceleration of the vehicle, respectively, forming the physically feasible interval of acceleration. The discrete points in the sampling interval are the acceleration values obtained by sampling according to the set resolution. Each sampling point corresponds to the longitudinal motion state of a time layer. By this sparse sampling method, under the premise of covering all possible longitudinal motions of the vehicle, a large number of invalid sampling points are reduced compared with dense sampling (such as random sampling of the RRT algorithm), significantly reducing the calculation amount of subsequent trajectory search. At the same time, the distribution density of the sampling points can be dynamically adjusted according to the actual scene: for example, in the high-speed cruising scene, the sampling resolution is reduced to reduce the number of points; in the low-speed following scene, the sampling resolution is increased to ensure the trajectory accuracy, achieving the balance between efficiency and accuracy.

[0067] The method provided by the embodiments of the present application can avoid the sampling points exceeding the physical performance limit of the vehicle (such as the acceleration exceeding the power system capacity), and ensure that the generated coarse trajectory is basically feasible. The flexible setting of the resolution (such as dynamic adjustment according to the scene) can balance the calculation efficiency and the trajectory accuracy: low resolution in high-speed scenes reduces the number of points, and high resolution in low-speed scenes ensures the response speed, solving the problem of high-speed redundancy and low-speed deficiency under fixed sampling rate.

[0068] Step (S32) samples trajectory points satisfying lane keeping and trajectory points satisfying lane changing requirement from the self-vehicle longitudinal coarse-grained sampling trajectory points according to the space-time joint map, to form a self-vehicle coarse-grained trajectory point set.

[0069] The process of sampling trajectory points satisfying lane keeping is as follows: taking the current lane center line as the reference line, the sampling interval of each sampling point (s, t) in the l-axis direction is [-Δl max ,Δl max ], and the sampling interval is evenly scattered with a sampling interval δ lk to obtain a set of lateral sampling points L k , and each acceleration sampling point corresponds to a set of trajectory points SLT k satisfying lane keeping. Please refer to the following formula 3:

[0070] Where Δl max is the maximum allowed displacement of the intelligent vehicle in the l-axis direction, and f Δ is a to-be-determined constant.

[0071] The process of sampling trajectory points satisfying lane changing requirement is as follows: taking the target lane center line as the reference line, the sampling interval of each sampling point (s, t) in the l-axis direction 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. ego To 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 Please refer to Formula 4 below:

[0072] The method provided in this application first samples longitudinal acceleration to obtain longitudinal trajectory points, and then combines a spatiotemporal joint map to filter lane keeping and lane changing trajectory points, realizing a sparse sampling logic of "longitudinal priority and lateral adaptation". Compared with indiscriminate sampling, this method can reduce a large number of invalid sampling points (such as points that are outside the lane range or conflict with obstacles), and significantly reduce the computational load of subsequent trajectory search while ensuring coverage of all feasible driving behaviors (lane keeping / lane changing); at the same time, the lane changing trajectory points are forced to align their endpoints with the center line of the target lane, ensuring the integrity and safety of lane changing behavior.

[0073] In step (S4), 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 minimum 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.

[0074] 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.

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

[0076] Step (S41) constructs 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 Coffset And lateral velocity cost C velocity Please refer to Formula 5 below:

[0077] 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 the following formula 6:

[0078] Where, 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. nudeg It is a monotonically decreasing function; w obs This represents the obstacle cost weighting coefficient.

[0079] 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 Please refer to Formula 7 below:

[0080] in, 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.

[0081] 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:

[0082] Formula 8:

[0083] in, 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 This represents the lateral velocity coefficient. velocity Please refer to Formula 9 below:

[0084] Among them, l lane This refers to the lateral distance between the lane edge line and the reference line that the vehicle needs to cross.

[0085] The method provided by the embodiments of the present application ensures that the optimal coarse trajectory reaches a balance in three dimensions of obstacle avoidance safety, road compliance (close to the reference line), and ride comfort (smooth lateral speed) by connecting adjacent sampling points to form a trajectory set and evaluating and selecting optimization based on multi-dimensional cost (obstacle / offset / lateral speed). The application of the dynamic programming method can efficiently search for a global optimal solution, and compared with the greedy algorithm, can reduce local suboptimal trajectories (such as excessively close to obstacles or excessively large offset), thereby providing a high-quality basic framework for subsequent fine-grained trajectory generation.

[0086] In some embodiments, the dynamic programming method is used to select an optimal coarse-grained trajectory from a coarse-grained trajectory set, including three steps of cost design, filing iterative calculation, and searching iterative calculation, specifically including:

[0087] The optimal coarse-grained trajectory searched in step S421 is composed of a linear connection sequence of each adjacent time layer in the node network, and the sequence satisfies the following formula 10:

[0088] Wherein, is the total cost of the path segment connecting the time layer L t and the time layer .

[0089] In step S422, filing iterative calculation is performed: starting from the second time layer, for each sampling point t in each time layer L , the Bellman optimality principle is used to calculate a sequence of filing contents composed of the sampling point and all sampling points in the previous time layer , and the filing content with the minimum stored cost in the sequence is selected as the filing content of the sampling point, and for the first time layer, the sampling point therein is the initial state point of the vehicle, and the filing content thereof is set as

[0090] Wherein, h is the index of the sampling point in the previous time layer , H is the index set of all sampling points in the time layer ; is the cost value stored in the filing content of the sampling point with the index h in the time layer , is the cost value of the trajectory segment from the sampling point to the sampling point with the index i in the time layer L t .

[0091] In step S423, searching iterative calculation is performed: starting from the last time layer, the cost corresponding to each sampling point obtained by filing is used to select the sampling point with the minimum cost For lane change trajectory selection, the index k of the sample point in the previous time layer in the recorded content is directly selected, and the sample points in each layer are iteratively searched in sequence to the first layer, so as to determine the entire trajectory point sequence, that is, the optimal coarse-grained trajectory.

[0092] In step (S43), each acceleration sample point a s The optimal coarse-grained trajectory in the corresponding SLT sample space plane is designed as a cost function, and an ideal speed deviation cost is introduced to screen the optimal coarse-grained trajectory between different sampling accelerations and lane change operations. The cost function is shown in formula 12:

[0093] Wherein, v is the speed at the end of the trajectory, v desire is the ideal speed of the vehicle, w traj is the weight coefficient. desire

[0094] In step (S5), the optimal coarse-grained trajectory point is taken as the starting point, and the convex space is inflated in the horizontal and vertical directions until it stops when it hits the drivable boundary, 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 sampling acceleration and the next sampling 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 inflating the convex space, the dynamic and static obstacle state information is considered. The static obstacle mainly includes road geometry information, road boundary guardrails or trees, etc., which are all forbidden areas for vehicle driving in the entire time axis span in the space-time joint map. The dynamic obstacle mainly refers to the driving trajectory of the surrounding vehicles predicted, and the area occupied by the dynamic obstacle in the space-time joint map will change over time. The specific process of determining the three-dimensional convex space is as follows:

[0096] The optimal coarse-grained trajectory point obtained in step (S4) is taken as the starting point, and the convex space is inflated in the horizontal and vertical directions until it stops when it hits the drivable boundary;

[0097] For the longitudinal drivable boundary, when the previous sampling acceleration trajectory and the next sampling acceleration trajectory of the optimal coarse-grained trajectory exist, it means that the longitudinal coarse-grained trajectory of the vehicle in the acceleration sampling interval [a lowbound ,a upbound ] is safe, and the longitudinal reachable region formed by the acceleration sampling interval is the longitudinal boundary of the convex space;

[0098] For the lateral drivable boundary, the main factor to consider is the allowed lateral displacement. As with the longitudinal drivable boundary acquisition, the expansion of the lateral drivable boundary also starts with 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 region reaches the lateral maximum displacement Δl max , the expansion ends, and the lateral convex plane of the current time layer is obtained.

[0099] As shown in FIG. 3, it intuitively shows the process of generating a three-dimensional convex space based on the optimal coarse-grained trajectory.

[0100] Subgraph (a) presents the spatial relationship of the vehicle lane-changing scene, which is used to intuitively show the initial environmental constraints of trajectory planning: the road is represented by parallel horizontal lines to indicate the lanes, the ego vehicle 301 is the current planning subject, and the "current lane front vehicle" and the "target lane vehicle" are dynamic obstacles, which constitute the constraint conditions for lane-changing decision; the curve represents the intended path of the planned trajectory, which embodies the macro trend of changing lanes from the current lane to the target lane, and clearly shows that the trajectory planning needs to solve the core task of "obstacle avoidance + cross-lane connection", which provides the scene basis for subsequent time-space sampling and convex space construction.

[0101] Subgraph (b) shows the construction logic of coarse-grained trajectory and convex space in a three-dimensional space-time coordinate system (S: longitudinal distance, T: time, L: lateral displacement). The S-axis extends along the longitudinal direction of the road, the T-axis represents the planning time sequence, and the L-axis represents the lateral position. The grid assists in quantifying the distribution of trajectory points; the convex space 311 is the safe and feasible region defined in each time-space slice (fixed T) by combining vehicle dynamics (acceleration, speed constraints) and obstacle distribution, which ensures that the trajectory points meet the physical limits and avoid obstacles; the coarse trajectory selected points 312 are black broken lines, which are selected from the sampling points in the convex space through cost function evaluation (obstacle avoidance, comfort, etc.), and embody the process of "searching for the globally optimal path under time-space constraints", which provides a basis for fine trajectory optimization.

[0102] Subgraph (c) focuses on the adaptability verification of fine trajectory generation and convex space, and supplements the technical details of the coarse trajectory optimization link: the fine-grained trajectory 321 is a black smooth curve generated based on the coarse trajectory framework through optimization methods such as Bezier curve, which ensures the continuity and smoothness of the trajectory (consistent with vehicle acceleration and steering constraints); the fine trajectory completely falls within the convex space 322, as long as the trajectory generation meets the convex space boundary (dynamic obstacles, road boundaries, dynamic limits), collision can be avoided, which embodies the advantages of "coarse space constraint + fine trajectory optimization" in hierarchical planning, and finally outputs an executable high-precision driving trajectory.

[0103] The coarse trajectory points in FIG. 3 are discrete points of the optimal coarse trajectory determined in step (S4). Starting from these points, spatial expansion is performed in both longitudinal (S-axis) and lateral (L-axis) directions: longitudinal expansion is constrained by the sampling acceleration of the adjacent time layer to form the longitudinal drivable boundary (such as the "longitudinal upper bound" and "longitudinal lower bound" in FIG. 3); lateral expansion is constrained by the maximum lateral offset to form the lateral drivable boundary. During the expansion process, if the boundary of a static obstacle (such as a road guardrail) or a dynamic obstacle (such as the current preceding vehicle, target vehicle) is encountered, the expansion is stopped, and the final closed space is the S-L-T (longitudinal-lateral-time) three-dimensional convex space. The fine trajectory in the figure is located inside the convex space, verifying the convex hull property of the Bezier curve: by constraining the fine trajectory inside the convex space, it can be ensured that the trajectory avoids obstacles throughout the entire journey and complies with vehicle dynamics constraints, thereby guaranteeing the safety and feasibility of the trajectory from a spatial perspective.

[0104] The method provided by the embodiments of the present application forms a three-dimensional convex space by expanding in the lateral and longitudinal directions starting from the optimal coarse trajectory points, strictly limits the spatial range by drivable boundaries (longitudinal based on acceleration constraints, lateral based on maximum offset), and ensures that all trajectories inside the convex space comply with vehicle dynamics characteristics and road environment constraints. This space not only provides a safe "container" (collision avoidance) for fine trajectory generation, but also adapts to different scenarios through dynamic adjustment of the expansion boundary (such as reducing the lateral range on a curved road), which can reduce invalid optimization regions and improve fine trajectory generation efficiency compared to fixed spatial range.

[0105] In step (S6), a Bezier fine trajectory is determined based on an optimal trajectory model, with the minimum cost of the trajectory as the target and terminal constraints, continuity constraints, collision constraints, and dynamic constraints as the constraint conditions, where the optimal trajectory model is obtained by constructing an n-segment Bezier curve; the cost of the trajectory is determined by integrating the square of the jerk value of the trajectory with respect to time.

[0106] In some embodiments, the optimal trajectory model constructed by the n-segment Bezier curve in dimension σ∈{s,l} can be referred to as the following formula 13:

[0107] wherein, represents the i-th control point of the j-th segment of the Bezier curve; t0, t1, …, t n are the time dimension coordinates of the starting point and the ending point of each segment of the Bezier curve, and α j is an asynchronous time domain scaling factor.

[0108] For the purpose of ensuring passenger comfort by not allowing the acceleration of the vehicle to change too much during the journey, the integral of the square of the jerk value of the trajectory with respect to time is used as the cost function of optimization. The cost function J jwhich can be expressed as Equation 14 as follows:

[0109] where w s and w l are the control weights in the longitudinal and lateral directions, respectively.

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

[0111] where the terminal constraints are that the initial state and the final state of the trajectory are the same as the initial state and the final state of the optimal coarse trajectory, respectively, where σ∈{s, l}, and the k-th derivative of the position coordinate at time t. The terminal constraints act on the starting point of the first convex space and the ending point of the last convex space, as shown in Equation 15 as follows:

[0112] The continuity constraints are that the k-th derivative of the trajectory should be continuous at the connection points of each adjacent convex space. The continuity constraints between the j-th convex space and the j+1-th convex space can be expressed as Equation 16 as follows:

[0113] The collision constraints and the dynamic constraints are that the trajectory is between the upper boundary and the lower boundary of the convex space, which are constraints on the position, velocity and acceleration of the trajectory, i.e., the k=0, 1, 2-th derivative of the Bezier curve. Therefore, the collision constraints and the dynamic constraints can be expressed as Equation 17 as follows:

[0114] where and represent the k-th lower boundary and the k-th upper boundary of the j-th convex space, respectively.

[0115] For the above linear equality and inequality constraints, all the constraints can be converted into a quadratic programming (QP) form together with the cost function, and a quadratic programming solver (OOQP) is used to quickly solve the optimization trajectory model.

[0116] The quadratic programming formula is shown in Equation 18 as follows:

[0117] where Q is a symmetric positive semi-definite n x n matrix, x∈R n is a variable vector, A and C are matrices with dimensions m a x n and m c x n, respectively, and b, d, f, l, u are vectors with appropriate dimensions.

[0118] ​​In the trajectory solving, the control point parameters of the Bezier curve are taken as optimization variables, the constraint conditions and the optimization variables are converted into corresponding matrices and vectors of the OOQP solver, and an optimal control point parameter is obtained by calling a solving command, so that an optimal segmented Bezier trajectory is obtained, and the Bezier fine-grained trajectory is obtained.

[0119] The method provided by the embodiment of the application adopts a segment Bezier curve to construct an optimization model, the convex hull characteristic of which can guarantee that the fine-grained trajectory completely falls within a three-dimensional convex space, thereby eliminating the collision risk from the geometric level; the square integral of the trajectory jerk is taken as a cost function, so that the acceleration change rate is minimized, and the ride comfort is significantly improved; the combination of multiple constraint conditions (terminal / continuity / collision / dynamics) ensures that the trajectory meets the smoothness while strictly following the vehicle physical limit and the road rules, and finally the fine trajectory has safety, feasibility and comfort.

[0120] The method provided by the embodiment of the application is a sparsely sampled space-time joint trajectory planning method, which sparsely samples coarse-grained trajectory points of a host vehicle based on longitudinal acceleration, searches the sampling points by using a dynamic programming method, obtains an optimal coarse-grained trajectory, constructs an S-L-T three-dimensional convex space based on a space-time joint map, fully considers complex configuration space obstacles and dynamic constraints, and realizes the generation of an optimized Bezier fine-grained trajectory based on the convex hull characteristic of the Bezier curve, thereby improving the calculation efficiency and guaranteeing the safety and feasibility of the trajectory.

[0121] The following is an apparatus embodiment of the application, and details not described in the apparatus embodiment can be referred to the above method embodiments.

[0122] The apparatus provided by an example embodiment of the application is a sparsely sampled space-time joint trajectory planning apparatus, which comprises:

[0123] The acquisition module is configured to acquire the state information of the host vehicle and the state information of dynamic / static obstacles around the host vehicle.

[0124] The trajectory planning module is configured to obtain a space-time joint map according to the state information of the host vehicle and the environment information around the host vehicle, sparsely sample coarse-grained trajectory points of the host vehicle based on the longitudinal acceleration in the state information of the host vehicle according to the space-time joint map, obtain a set of coarse-grained trajectory points of the host vehicle, determine an optimal coarse-grained trajectory according to the set of coarse-grained trajectory points of the host vehicle, determine a three-dimensional convex space according to the optimal coarse-grained trajectory and the space-time joint map, and generate a Bezier fine-grained trajectory in the three-dimensional convex space by using the convex hull characteristic of the Bezier curve.

[0125] In conclusion, the control device of the auxiliary driving system provided by the embodiments of the present application can solve the problem of safety risk and unstable operation of the auxiliary driving system in the double flash mode, ensure the safety of the vehicle in the double flash mode and the stability of the auxiliary driving system, balance safety and convenience, and meet the needs of users for double flash and intelligent driving.

[0126] An example embodiment of the present application provides a control device, the control device comprising one or more processors and one or more memories, the one or more memories storing at least one program code, the at least one program code being loaded and executed by the one or more processors to implement the sparsely sampled space-time joint trajectory planning method according to any of the above implementation manners.

[0127] An example embodiment of the present application provides a computer readable storage medium, the computer readable storage medium storing at least one program code, the at least one program code being loaded and executed by a processor to implement the sparsely sampled space-time joint trajectory planning method according to any of the above implementation manners.

[0128] An example embodiment of the present application provides a computer program product, the computer program product comprising computer program code, the computer program code being stored in a computer readable storage medium, a processor of a control device reading the computer program code from the computer readable storage medium, the processor executing the computer program code to make the control device execute the sparsely sampled space-time joint trajectory planning method according to any of the above implementation manners.

[0129] Other embodiments of the present application will be apparent to those skilled in the art from consideration of the specification and practice of the present application disclosed herein. It is intended that the present application cover any and all variations of the present application that come within the scope of the claims and their equivalents. It is intended that the specification and examples be considered exemplary only, with the true scope of the application being indicated by the following claims.

[0130] It should be understood that the present application is not limited to the precise construction that has been described above and illustrated in the accompanying drawings, and that various modifications and changes can be made by those skilled in the art without departing from the scope of the present application. The scope of the present application is limited only by the appended claims.

Claims

1. A sparse sampling-based spatiotemporal joint trajectory planning method, executed by an electronic device, the method comprising: obtaining ego vehicle state information and dynamic and static obstacle state information around the ego vehicle; obtaining a spatiotemporal joint map according to the ego vehicle state information and the environment information around the ego vehicle; based on longitudinal acceleration in the ego vehicle state information, sparsely sampling coarse-grained trajectory points of the ego vehicle according to the spatiotemporal joint map to obtain a set of coarse-grained trajectory points of the ego vehicle; determining an optimal coarse-grained trajectory according to the set of coarse-grained trajectory points of the ego vehicle; determining a three-dimensional convex space according to the optimal coarse-grained trajectory and the spatiotemporal joint map; and generating a Bezier fine-grained trajectory in the three-dimensional convex space by using the convex hull property of a Bezier curve.

2. The sparsely-sampled based space-time joint trajectory planning method of claim 1, wherein, sampling longitudinal acceleration of the ego vehicle, and obtaining coarse-grained sampling trajectory points of the ego vehicle according to the sampled acceleration; according to the spatiotemporal joint map, sampling trajectory points that meet lane keeping and trajectory points that meet lane changing requirements from the coarse-grained sampling trajectory points of the ego vehicle to form a set of coarse-grained trajectory points of the ego vehicle.

3. The sparsely-sampled based space-time joint trajectory planning method of claim 1 or 2, wherein, sampling acceleration of the ego vehicle in a set acceleration range according to a set resolution.

4. The sparsely-sampled based space-time joint trajectory planning method of any one of claims 1 to 3, wherein, connecting different sampling points between every two adjacent time layers in the set of coarse-grained trajectory points of the ego vehicle to obtain a set of coarse-grained trajectories; calculating a cost of each coarse-grained trajectory in the set of coarse-grained trajectories, and selecting a coarse-grained trajectory with the minimum cost as an optimal coarse-grained trajectory, wherein the cost of each coarse-grained trajectory includes an obstacle cost, an offset reference line cost, and a lateral velocity cost.

5. The sparse-sampling-based spatio-temporal joint trajectory planning method of any one of claims 1 to 4, wherein, expanding the convex space in the longitudinal and lateral directions from the optimal coarse-grained trajectory point as a starting point until a drivable boundary is reached to obtain 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 a previous sampling acceleration and a next sampling acceleration of the optimal coarse-grained trajectory; and the lateral drivable boundary is determined according to a maximum lateral offset allowed by the optimal coarse-grained trajectory.

6. The sparse-sampling-based spatio-temporal joint trajectory planning method of any one of claims 1 to 5, wherein, determining the Bezier fine-grained trajectory according to an optimal trajectory model, with the minimum cost of the trajectory as a target and terminal constraints, continuity constraints, collision constraints, and dynamic constraints as constraint conditions; the optimal trajectory model is constructed by using an n-segment Bezier curve; and the cost of the trajectory is determined by integrating the square of a jerk value of the trajectory with respect to time.

7. A sparse sampling-based spatiotemporal joint trajectory planning device, comprising: an obtaining module configured to obtain ego vehicle state information and dynamic and static obstacle state information around the ego vehicle; a trajectory planning module configured to obtain a spatiotemporal joint map according to the ego vehicle state information and the environment information around the ego vehicle; based on longitudinal acceleration in the ego vehicle state information, sparsely sample coarse-grained trajectory points of the ego vehicle according to the spatiotemporal joint map to obtain a set of coarse-grained trajectory points of the ego vehicle; determine an optimal coarse-grained trajectory according to the set of coarse-grained trajectory points of the ego vehicle; determine a three-dimensional convex space according to the optimal coarse-grained trajectory and the spatiotemporal joint map; and generate a Bezier fine-grained trajectory in the three-dimensional convex space by using the convex hull property of a Bezier curve. ​ 8. A control device comprising one or more processors and one or more memories having stored therein at least one program code, the at least one program code being loaded and executed by the one or more processors to implement the sparse-sampling-based spatiotemporal joint trajectory planning method according to any one of claims 1-6.

9. A computer-readable storage medium having stored therein at least one program code, the at least one program code being loaded and executed by a processor to implement the sparse-sampling-based spatiotemporal joint trajectory planning method according to any one of claims 1-6.

10. A computer program product comprising a computer program code stored in a computer-readable storage medium, a processor of a control device reading the computer program code from the computer-readable storage medium, the processor executing the computer program code to cause the control device to implement the sparse-sampling-based spatiotemporal joint trajectory planning method according to any one of claims 1-6.

Images