CN118579058A - Speed planning method and device - Google Patents

Abstract

The present application provides a speed planning method and device, including: when the leading vehicle is within a preset range, the longitudinal trajectory is divided into N segments of trajectories with the goal of balancing driving comfort and driving safety, and multi-term expressions are established for each segment to obtain a speed planning expression, and multiple feasible solutions of the speed planning expression are obtained according to the position sampling point at the end of the trajectory; N is a positive integer; the feasible solution with the lowest cost is selected from the multiple feasible solutions as the optimal solution, and speed planning is performed with the optimal solution; the cost is obtained based on the weighted sum of a first evaluation standard for driving comfort and a second evaluation standard for driving safety; the use of the present application can improve the accuracy of speed planning.

Description

Speed planning method and device

Technical Field

The present application relates to the field of intelligent driving technology, and in particular to a speed planning method and device.

Background Art

At present, more and more intelligent driving technologies and products are beginning to serve people's lives, especially ADAS (Advanced Driving Assistant System) L2, L2++ level (partial automation of autonomous driving) related functions are being mass-produced. As a means of transportation for people, the comfort of a car during driving is crucial. During the autonomous driving process, abnormal acceleration and deceleration of the vehicle can cause passengers to vomit or feel dizzy, resulting in low driving comfort. However, in some emergency and dangerous scenarios, it is necessary to brake in time to avoid collisions to ensure driving safety.

The speed solution and planning module in intelligent driving mainly balances driving comfort and driving safety, and strives to be as comfortable as possible while ensuring safety. Common speed planning methods use methods such as quintic or quartic polynomials for speed planning, but the intermediate state of the polynomial is uncontrollable, it is difficult to approach the optimal solution, and the speed planning accuracy is low.

Summary of the invention

The purpose of this application is to address the deficiencies of the above-mentioned existing related technologies and provide a speed planning method and device that can improve the accuracy of speed planning.

In a first aspect, the present application provides a speed planning method, comprising:

If the preceding vehicle is within the preset range, the longitudinal trajectory is divided into N segments and multi-term expressions are established for each segment to obtain a speed planning expression, and multiple feasible solutions of the speed planning expression are obtained according to the position sampling points at the end of the trajectory; N is a positive integer;

A feasible solution with the lowest cost is selected from the multiple feasible solutions as the optimal solution, and speed planning is performed using the optimal solution; the cost is obtained by weighting the first evaluation standard of driving comfort and the second evaluation standard of driving safety.

Compared with a single multi-term expression, the present application adopts a method of balancing driving comfort and driving safety, divides the longitudinal trajectory into N segments and establishes multi-term expressions for each segment, which is equivalent to adding (N-1) intermediate control points, so that the intermediate process of speed planning of a single multi-term expression can be controlled, thereby improving the accuracy of speed planning, and selecting the optimal solution from multiple feasible solutions through the first evaluation standard of driving comfort and the second evaluation standard of driving safety, so that the speed planning expression can better fit the optimization goal of balancing driving comfort and driving safety, thereby being closer to the optimal solution and further improving the accuracy of speed planning.

Furthermore, the longitudinal trajectory is divided into N segments with the goal of balancing driving comfort and driving safety, and multi-term expressions are established for each segment to obtain a speed planning expression, including:

The current position is taken as the initial planning position, the end position of the trajectory is taken as the ending planning position, the planning distance is obtained, and a local coordinate system in which the planning distance changes with time is established. Based on the local coordinate system, with the goal of balancing driving comfort and driving safety, the longitudinal trajectory is divided into two trajectories, and multi-term expressions and equality constraints are established for the two trajectories respectively to obtain a speed planning expression.

Further, the speed planning expression includes: a first-segment speed planning expression, a second-segment speed planning expression and an equality constraint; the multi-term expression and the equality constraint are established for the two trajectories respectively to obtain the speed planning expression, including:

A quartic expression with the same format but different parameters is established for the two trajectories respectively to obtain the first speed planning expression and the second speed planning expression, and an equality constraint is established according to the initial planning position, the end planning position and the path continuity of the two trajectories.

Furthermore, the equation constraint includes: an acceleration equation constraint; the acceleration equation constraint establishes an equation constraint according to the path continuity of the two trajectories, including:

According to the path continuity of the two trajectories, a velocity equation constraint is established in which the end position of the velocity curve of the first trajectory and the initial position of the velocity curve of the second trajectory are the same, and the velocity equation constraint is rewritten according to the acceleration change of the two trajectories to obtain the acceleration equation constraint.

Further, the acceleration equation constraint includes: a first acceleration equation constraint; the velocity equation constraint is rewritten according to the acceleration change of the two trajectories to obtain the acceleration equation constraint, including:

According to the equal absolute values of the peaks or troughs of the acceleration curves of the first and second trajectory segments, the velocity equation constraint is rewritten to obtain the first acceleration equation constraint; wherein the first acceleration equation constraint includes a first velocity change of the velocity equation constraint, and the first velocity change includes: a situation where the first trajectory segment accelerates and the second trajectory segment decelerates, and a situation where the first trajectory segment decelerates and the second trajectory segment accelerates.

This application aims at different acceleration scenarios to obtain accurate acceleration equation constraints, so as to accurately correspond to the speed change curve of accelerating first and then decelerating or decelerating first and then accelerating, thereby improving the accuracy of speed planning.

Furthermore, the acceleration equation constraint further includes: a second acceleration equation constraint; the velocity equation constraint is rewritten according to the acceleration change of the two trajectories to obtain the acceleration equation constraint, and further includes:

The velocity equation constraint is rewritten according to the peak or trough of the acceleration curve of the second trajectory as a constant value to obtain the second acceleration equation constraint; wherein the second acceleration equation constraint includes the second velocity change of the velocity equation constraint, and the second velocity change includes: the first trajectory is accelerated and the second trajectory is uniform, and the first trajectory is decelerated and the second trajectory is uniform.

The present application also takes into account the acceleration scenario in which the peak or trough of the acceleration curve of the second segment of the trajectory is a constant value to obtain an accurate second acceleration equation constraint, thereby accurately corresponding to the speed change curve of first accelerating and then maintaining a constant speed or first decelerating and then maintaining a constant speed. The acceleration scenarios can be increased, thereby avoiding the situation where there is no optimal solution due to insufficient number of sampling points in a single acceleration scenario, and thus it is easier to approach the optimal solution, further improving the accuracy of speed planning.

Furthermore, the equality constraint also includes:

The first speed planning expression is the fixed value at the initial planning position; and

The starting speed of the initial planning position is the current actual speed of the vehicle; and,

The initial acceleration of the acceleration curve of the first trajectory is the constant value; and

The final speed of the speed curve of the first track segment is equal to the initial speed of the speed curve of the second track segment; and

The speed at the end of the speed curve of the second trajectory is equal to the current actual speed of the preceding vehicle; and

The final velocity acceleration of the second trajectory returns to the constant value; and

The total duration of the speed planning is a preset value; and,

The end position of the trajectory is a position sampling point.

Furthermore, the first evaluation criterion includes: comfort cost; the second evaluation criterion includes: collision cost, speed cost, centripetal acceleration cost and stable following cost; the stable following cost is obtained according to the difference between the longitudinal displacement at each moment and the safety distance at each moment; the cost is obtained according to the weighted sum of the first evaluation criterion of driving comfort and the second evaluation criterion of driving safety; including:

The cost is obtained according to a weighted sum of the comfort cost, the collision cost, the speed cost, the centripetal acceleration cost and the stable following cost.

Furthermore, if the leading vehicle is not within the preset range, a linear expression is established for the longitudinal trajectory, and the acceleration is sampled within the preset acceleration range to obtain a number of acceleration sampling points. Based on the number of acceleration sampling points, multiple feasible solutions of the speed planning expression are obtained, so that the feasible solution with the lowest cost is selected from the multiple feasible solutions as the optimal solution.

The present application also considers the case where the leading vehicle is not within the preset range and adopts a linear expression to perform speed planning. Compared with the existing related technology that adopts multi-term expressions regardless of whether the leading vehicle is within the preset range or not, the present application can adopt a linear expression with smaller computing power to perform speed planning for the scenario where the leading vehicle is not within the preset range, thereby reducing computing power requirements and computing power costs.

In a second aspect, the present application provides a speed planning device, including: a speed planning expression acquisition module and a solution and planning module; wherein,

The module for obtaining the speed planning expression is used to divide the longitudinal trajectory into N segments and establish multi-term expressions respectively to obtain the speed planning expression if the leading vehicle is within the preset range, with the goal of balancing driving comfort and driving safety, and obtain multiple feasible solutions of the speed planning expression according to the position sampling points at the end of the trajectory; N is a positive integer;

The solution and planning module is used to select the feasible solution with the lowest cost from the multiple feasible solutions as the optimal solution, and perform speed planning based on the optimal solution; the cost is obtained by the weighted sum of the first evaluation standard of driving comfort and the second evaluation standard of driving safety.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG1 is a flow chart of a speed planning method provided by the present application;

FIG2 is a schematic diagram of a front vehicle provided by the present application when the front vehicle is within a preset range;

FIG3 is a schematic diagram of a single-lane following cruise scenario provided by the present application;

FIG4 is a schematic diagram of an acceleration curve of first accelerating and then decelerating provided by the present application;

FIG5 is a schematic diagram of an acceleration curve of first deceleration and then acceleration provided by the present application;

FIG6 is a schematic diagram of an acceleration curve of first accelerating and then maintaining a constant speed provided by the present application;

FIG. 7 is a schematic diagram of an acceleration curve of first deceleration and then uniform speed provided by the present application;

FIG8 is a schematic diagram of a single lane single vehicle cruising scenario provided by the present application;

FIG. 9 is a schematic diagram of the structure of a speed planning device provided in the present application.

DETAILED DESCRIPTION

The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

It is worth mentioning that the commonly used speed planning methods often use quadratic optimization methods when performing speed planning, but this method has high requirements on the computing power of the on-board chip and the corresponding computing power cost is high; alternatively, methods such as quintic or quartic polynomials are used for speed planning, but the intermediate state of the polynomial is uncontrollable, making it difficult to approach the optimal solution and the speed planning accuracy is low.

Based on this, the present application provides a speed planning method and device, which decides whether to divide the longitudinal trajectory into N segments by judging the leading vehicle. If the leading vehicle is not within the preset range, a linear expression can be constructed for the longitudinal trajectory to model it, thereby reducing the calculation requirements; if the leading vehicle is within the preset range, N multi-term expressions can be established respectively by balancing the requirements of driving comfort and driving safety, and corresponding equality constraints are established for the N multi-term expressions, and multiple feasible solutions are obtained based on sampling points, thereby improving the accuracy of speed planning; finally, a cost function including comfort cost, collision cost, speed cost, centripetal acceleration cost and stable following cost is used to evaluate the driving comfort and driving safety of multiple feasible solutions to select the global optimal solution, thereby further balancing driving comfort and driving safety while improving the accuracy of speed planning.

In order to more clearly illustrate the speed planning method and device provided by the present application, a detailed description will be given through the following embodiments.

Example 1

Referring to FIG. 1 , it is a flow chart of a speed planning method provided by the present application, including: Steps S11 to S12, specifically:

Step S11: If the leading vehicle is within the preset range, the longitudinal trajectory is divided into N segments and multi-term expressions are established for each segment to obtain a speed planning expression, with the goal of balancing driving comfort and driving safety. Multiple feasible solutions of the speed planning expression are obtained based on the position sampling points at the end of the trajectory; N is a positive integer.

In some embodiments, determining whether the preceding vehicle is within a preset range includes: if there is a preceding vehicle and the preceding vehicle is within a preset distance threshold, then the preceding vehicle is within the preset range.

In some embodiments, the preset distance threshold may be 100 meters, or 50 meters, or may be within a certain distance range. For example, if the preceding vehicle is within the distance range of (50, 150) meters, it is considered to be within the preset range.

In some embodiments, see Figure 2, which is a schematic diagram of the front vehicle provided by the present application when it is within a preset range. In Figure 2, the front vehicle B is within a preset range of 100 meters in front of the self-vehicle A, and the self-vehicle A is driving in a one-way street and following the front vehicle A. At this time, the self-vehicle A needs to maintain a certain safety distance from the front vehicle B.

In some embodiments, the safety distance may be expressed as:

X _safe = t _safe * V ₀ + X _{min_safe} ;

Among them, t _safe represents the time parameter, X _{min_safe} represents the minimum distance between vehicles, and V ₀ represents that the starting speed of the vehicle during speed planning is the current actual speed of the vehicle V ₀ .

In some embodiments, the time parameter t _safe (ranging from 1 to 3 seconds) and the minimum following distance X _{min_safe} (ranging from 1 to 5 meters) can be set by the user. In this scenario, the distance between the vehicle and the front vehicle needs to be as close to the safe distance as possible. Since the speed and position of the front vehicle are dynamically changing and may even stop, the vehicle needs to plan its speed in real time and ensure safety and comfort.

In some embodiments, with the goal of balancing driving comfort and driving safety, the longitudinal trajectory is divided into N segments and multi-term expressions are established for each segment to obtain a speed planning expression, including: taking the current position as the initial planning position, taking the end position of the trajectory as the ending planning position, obtaining the planning distance, and establishing a local coordinate system in which the planning distance changes over time. Based on the local coordinate system, with the goal of balancing driving comfort and driving safety, the longitudinal trajectory is divided into two segments, and multi-term expressions and equality constraints are established for the two segments to obtain a speed planning expression.

In some embodiments, N is preferably 2.

In some embodiments, N may also be other positive integers to adapt to corresponding driving scenarios.

In some embodiments, the speed planning expression includes: a first-segment speed planning expression, a second-segment speed planning expression and an equality constraint; establishing multi-term expressions and equality constraints for the two trajectories respectively to obtain a speed planning expression, including: establishing quartic expressions with the same format but different parameters for the two trajectories respectively to obtain a first-segment speed planning expression and a second-segment speed planning expression, and establishing an equality constraint based on the initial planning position, the end planning position and the path continuity of the two trajectories.

It is worth noting that the first-segment speed planning expression and the second-segment speed planning expression are speed planning expressions of the front and rear trajectories (corresponding to the first and second trajectories, respectively) obtained when the longitudinal trajectory is divided into two trajectories.

In some embodiments, see FIG3, which is a schematic diagram of a single-lane following cruise scenario provided by the present application. FIG3 shows, from top to bottom, the curves of the planned distance (Speed, s), speed (Velocity, v) and acceleration (Acceleration, acc) as the speed planning time (time, t), that is, the first-order derivative and the second-order derivative of the first figure of FIG3 are respectively made for time, and the second figure of FIG3 and the third figure of FIG3 can be obtained. In FIG3, the total time of speed planning is 6 seconds, and speed planning is performed in this way. In the schematic diagram of the planned distance versus planning time in the first figure of FIG3, curve 301 is the predicted position curve of the vehicle in front, curve 302 is the predicted safety distance curve of the vehicle in front, and curve 303 is the planned position curve of the vehicle in front. In the schematic diagram of the speed versus planning time in the second figure of FIG3, curve 304 is the planned speed curve of the vehicle in front, curve 305 is the current speed of the vehicle in front, curve 306 is the current speed of the vehicle in front, and curve 307 is the user-set speed. In the third diagram of FIG. 3 , which is a schematic diagram showing acceleration changes with planning time, curve 308 is the acceleration curve of the first segment of the planned trajectory of the vehicle, and curve 309 is the acceleration curve of the second segment of the planned trajectory of the vehicle.

In some embodiments, the longitudinal trajectory of the vehicle in the local coordinate system is composed of two quartic polynomials, and the first-stage velocity planning expression and the first-stage velocity planning expression can be respectively expressed as:

f1(t)= a1*t ⁴ + b1*t ³ + c1*t ² + d1*t + e1;

f2(t)= a2* t ⁴ + b2* t ³ + c2* t ² + d2*t + e2;

Among them, a1, b1, c1, d1, e1 and a2, b2, c2, d2, e2 are the unknowns in the speed planning expression of the first segment and the second segment respectively, t is the time; f1(t) and f2(t) represent the planning distances of the first and second segments of the trajectory over time respectively.

In some embodiments, the equality constraint comprises: an acceleration equality constraint.

In some embodiments, the acceleration equation constraint establishes an equation constraint according to the path continuity of the two trajectories, including: according to the path continuity of the two trajectories, establishing a velocity equation constraint in which the end position of the velocity curve of the first trajectory and the initial position of the velocity curve of the second trajectory are the same, and rewriting the velocity equation constraint according to the acceleration change of the two trajectories to obtain the acceleration equation constraint.

In some embodiments, the velocity equality constraint may be expressed as:

f2(ta)=f1(ta);

Wherein, ta and tb represent the time length of the acceleration curve of the first segment and the time length of the acceleration curve of the second segment, respectively. In some embodiments, referring to the third figure of FIG3 , it is a schematic diagram of the acceleration changing with the planning time.

In some embodiments, the acceleration equation constraint includes: a first acceleration equation constraint; rewriting the velocity equation constraint according to the acceleration change of the two trajectory segments to obtain the acceleration equation constraint, including: rewriting the velocity equation constraint according to the absolute values of the peaks or troughs of the acceleration curves of the first trajectory segment and the second trajectory segment being equal to obtain the first acceleration equation constraint; wherein the first acceleration equation constraint includes a first velocity change of the velocity equation constraint, and the first velocity change includes: a situation where the first trajectory segment accelerates and the second trajectory segment decelerates and a situation where the first trajectory segment decelerates and the second trajectory segment accelerates.

This application aims at different acceleration scenarios to obtain accurate acceleration equation constraints, so as to accurately correspond to the speed change curve of accelerating first and then decelerating or decelerating first and then accelerating, thereby improving the accuracy of speed planning.

In some embodiments, the acceleration equation constraint also includes: a second acceleration equation constraint; rewriting the velocity equation constraint according to the acceleration change of the two trajectory segments to obtain the acceleration equation constraint, and also includes: rewriting the velocity equation constraint according to the peak or trough of the acceleration curve of the second trajectory segment as a constant value to obtain the second acceleration equation constraint; wherein the second acceleration equation constraint includes a second velocity change of the velocity equation constraint, and the second velocity change includes: the first trajectory segment is accelerated and the second trajectory segment is uniformly accelerated and the first trajectory segment is decelerated and the second trajectory segment is uniformly accelerated.

In some embodiments, the constant value may be 0.

It is worth mentioning that the first acceleration equation constraint and the second acceleration equation constraint are two specific expressions of the velocity equation constraint. If the absolute values of the peaks or troughs of the acceleration curves of the first and second trajectory segments are equal, the first acceleration equation constraint is needed to constrain the first and second velocity planning expressions; if the acceleration of the two trajectory segments changes, the second acceleration equation constraint is needed to constrain the first and second velocity planning expressions.

The present application also takes into account the acceleration scenario in which the peak or trough of the acceleration curve of the second segment of the trajectory is a constant value to obtain an accurate second acceleration equation constraint, thereby accurately corresponding to the speed change curve of first accelerating and then maintaining a constant speed or first decelerating and then maintaining a constant speed. The acceleration scenarios can be increased, thereby avoiding the situation where there is no optimal solution due to insufficient number of sampling points in a single acceleration scenario, and thus it is easier to approach the optimal solution, further improving the accuracy of speed planning.

In some embodiments, the velocity equation constraint includes: two cases, the first case is that the absolute values of the peaks or troughs of the acceleration curves of the first trajectory segment and the second trajectory segment are equal (i.e., the first acceleration equation constraint), and the second case is that the peaks or troughs of the acceleration curves of the second trajectory segment are constant (i.e., the second acceleration equation constraint).

In some embodiments, when the absolute values of the peaks or troughs of the acceleration curves of the first and second segments of the trajectory are equal, the velocity equation constraint can be expressed using the first acceleration equation, which can be written as:

|f1(ta/2)''| = |f1(ta+tb/2)''|;

Among them, f1( )'' is the second-order derivative operation of the first speed planning expression on the planning time. It is worth noting that in this case, four speed curves can be generated, including: acceleration first and then deceleration, deceleration first and then acceleration, and two sections of simultaneous acceleration or deceleration. However, only the cases of acceleration first and then deceleration and deceleration first and then acceleration are considered, and the speed curve of two sections of simultaneous acceleration or deceleration is discarded. The case of two sections of simultaneous acceleration or deceleration does not have the problem of balancing driving comfort and driving safety, so it is discarded.

In some embodiments, a speed curve that accelerates first and then decelerates is used to deal with scenarios such as the vehicle in front is outside a safe distance but has a lower speed than the vehicle itself. See Figure 4 for a schematic diagram of an acceleration curve that accelerates first and then decelerates provided by the present application; a speed curve that decelerates first and then accelerates is used to deal with scenarios such as the vehicle in front is within a safe distance but has a higher speed than the vehicle itself. See Figure 5 for a schematic diagram of an acceleration curve that decelerates first and then accelerates provided by the present application.

In some embodiments, when the peak or trough of the acceleration curve of the second trajectory is a constant value, the velocity equation constraint can be expressed using the second acceleration equation, which can be written as:

f1(ta+tb/2)''= 0.

It is worth noting that f1(ta+tb/2)''= 0 corresponds to two speed curves, including: acceleration first and then constant speed and deceleration first and then constant speed. The speed curve of acceleration followed by constant speed is used to cope with scenarios such as starting with a vehicle, and the speed curve of deceleration first and then constant speed is used to cope with scenarios such as braking and stopping. See Figures 6 and 7, which are schematic diagrams of the acceleration curve of acceleration first and then constant speed and the acceleration curve of deceleration first and then constant speed provided in this application.

In some embodiments, the equality constraint also includes: the first segment speed planning expression is a constant at the initial planning position; and the starting speed of the initial planning position is the current actual speed of the vehicle; and the starting acceleration of the acceleration curve of the first segment trajectory is a constant; and the end speed of the speed curve of the first segment trajectory is equal to the initial speed of the speed curve of the second segment trajectory; and the end speed of the speed curve of the second segment trajectory is equal to the current actual speed of the leading vehicle; and the end speed acceleration of the second segment trajectory regresses to a constant; and the total duration of the speed planning is a preset value; and the end position of the trajectory is a position sampling point.

In some embodiments, the remaining constraints of the equality constraint may be expressed as:

f1(0)=0;

f1(0)'=V ₀ ;

f1(0)''=0;

f1(ta)''= 0;

f2(ta)''=0;

f2(ta)=f1(ta);

f2(ta)'=f1(ta)';

f2(T)'= V _p ;

f2(T)''= 0;

ta + tb = T;

f2(T) =s;

Among them, f1(0)=0 means that the starting position of the vehicle is 0; f1(0)'=V ₀ means that the starting speed of the vehicle is the current actual speed of the vehicle V ₀ , f1( )' is the first-order derivative operation of the first speed planning expression on the planning time; f1(0)''=0 means that the initial acceleration of the acceleration curve of the first trajectory is 0, that is, the current acceleration of the vehicle is considered to be 0; f1(ta)''= 0 and f2(ta)''=0 respectively mean that the end of the acceleration curve of the first trajectory and the initial acceleration of the acceleration curve of the second trajectory are equal, both are 0, f1( )'' and f2( )'' are the second-order derivative operations of the first speed planning expression on the planning time and the second-order derivative operations of the second speed planning expression on the planning time; f2(ta)=f1(ta) means that the end position of the speed curve of the first trajectory is equal to the initial position of the speed curve of the second trajectory; f2(ta)'=f1(ta)' means that the end speed of the speed curve of the first trajectory is equal to the initial speed of the speed curve of the second trajectory, f2( )' is the first-order derivative operation of the speed planning expression for the planning time of the second segment; f2(T)'= V _p means that the speed at the end of the speed curve of the second segment of the trajectory is equal to the current actual speed V _p of the front vehicle. The current actual speed of the front vehicle is known and can be obtained in real time from the upstream perception; f2(T)''= 0 means that the acceleration of the vehicle at the end returns to 0; ta + tb= T, T represents the total time of speed planning; f2(T) =s means that the end position of the trajectory after speed planning is s (that is, the planned distance after speed planning under the total time of speed planning), and s is obtained by sampling.

In some embodiments, the total speed planning duration T may be preset to 6 seconds.

It is worth noting that the sampling of the end position s of the trajectory indicates how far the vehicle is ahead in T seconds. The total time of speed planning T is fixed. The larger the end position s of the trajectory is, the faster the speed of the entire longitudinal trajectory will be. The smaller the end position s of the trajectory is, the slower the trajectory speed will be.

In some embodiments, the position sampling is performed on the end position s of the trajectory after speed planning, and the sampling interval is [0, Xp+ _Vp *T]; wherein Xp represents the position of the current leading vehicle from the vehicle, and _Vp represents the speed of the current leading vehicle.

In some embodiments, the number of sampling points for position sampling can be 30; among them, according to the two cases of the speed equation constraint, each case is sampled separately, so the total number of sampling points is 60, so each control cycle will generate 60 feasible solutions, that is, 60 feasible solutions will be generated at each time t.

In some embodiments, if the leading vehicle is not within a preset range, a linear expression is established for the longitudinal trajectory, and the acceleration is sampled within a preset acceleration range to obtain a number of acceleration sampling points. Based on the number of acceleration sampling points, multiple feasible solutions of the speed planning expression are obtained, so that the feasible solution with the lowest cost is selected from the multiple feasible solutions as the optimal solution.

In some embodiments, see FIG8 , which is a schematic diagram of a single-lane single-vehicle cruising scenario provided by the present application. Vehicle A is the vehicle itself, and there are no other vehicles in front. The speed planning model at this time can be modeled by a linear expression, which can be expressed as:

V _d = a * t + V ₀ ;

V _o = max(0, min(V _d , V _s ));

Where a represents the expected acceleration, which is a hyperparameter that can be adjusted according to the user's demand parameters, _V0 represents the actual speed of the vehicle, t represents each moment under the total speed planning time T, that is, the time of the acceleration sampling point, _Vs represents the user-set speed, _V0 represents a linear speed curve, min( ) is the minimum value operation, To obtain the maximum value operation.

In some embodiments, the user can be the driver to adjust parameters in real time, or the manufacturer can set default parameters. For example, the manufacturer sets the expected acceleration a or the user-set speed V _s by default, and the driver resets the expected acceleration a or the user-set speed V _s in real time while driving.

In some embodiments, the acceleration a is expected to be sampled within a range between -1 and 1, and the number of acceleration sampling points is 30. Then, 30 feasible solutions will be generated in each control cycle, that is, 30 feasible solutions will be generated at each time t.

The present application also considers the case where the leading vehicle is not within the preset range and adopts a linear expression to perform speed planning. Compared with the existing related technology that adopts multi-term expressions regardless of whether the leading vehicle is within the preset range or not, the present application can adopt a linear expression with smaller computing power to perform speed planning for the scenario where the leading vehicle is not within the preset range, thereby reducing computing power requirements and computing power costs.

Step S12: Select the feasible solution with the lowest cost from multiple feasible solutions as the optimal solution, and perform speed planning based on the optimal solution; the cost is obtained by the weighted sum of the first evaluation standard of driving comfort and the second evaluation standard of driving safety.

In some embodiments, the first evaluation criterion includes: comfort cost; the second evaluation criterion includes: collision cost, speed cost, centripetal acceleration cost and stable following cost.

It is worth noting that the first evaluation standard is an evaluation standard for evaluating driving comfort at the cost of comfort, and the second evaluation standard is an evaluation standard for evaluating driving safety at the cost of driving safety.

In some embodiments, the stable following cost is obtained based on the difference between the longitudinal displacement at each moment and the safety distance at each moment.

In some embodiments, the cost is obtained by the weighted sum of a first evaluation standard for driving comfort and a second evaluation standard for driving safety; including: obtaining the cost by the weighted sum of comfort cost and collision cost, speed cost, centripetal acceleration cost and stable following cost.

In some embodiments, the cost may be expressed as:

f _cost =w ₁ *cost _LonObitive + w ₂ *cost _jerk + w ₃ *cost _collision + w ₄ *cost _centerAcc + w ₅ *cost _follow ;

Among them, cost _LonObiective , cost _jerk , cost _collision , cost _centerAcc and cost _follow are comfort cost, collision cost, speed cost, centripetal acceleration cost and stable following cost respectively; _w1 to _w5 are the weights of comfort cost, collision cost, speed cost, centripetal acceleration cost and stable following cost respectively.

In some embodiments, the stable following cost is expressed as:

;

in, , and are the longitudinal displacement at time t and the safety distance at time t respectively; It is the maximum value operation; is the total duration of speed planning.

In some embodiments, if multiple feasible solutions are obtained from a linear expression or multiple feasible solutions are obtained from two 4-order expressions, the feasible solutions with the lowest cost are selected as the optimal solutions of the linear expression or the two 4-order expressions, that is, the optimal solution is selected from 30 feasible solutions of the linear expression or 60 feasible solutions of the two 4-order expressions.

In some embodiments, when the front vehicle is within 100 meters, a double quartic polynomial expression is selected to calculate the optimal solution, and a linear model is not calculated; when there is no vehicle ahead or the front vehicle is outside 100 meters, a linear expression is selected to obtain the optimal solution, and a double quartic polynomial solution is not calculated, thereby increasing the amount of calculation and saving more than 50% of computing power relative to the optimization method.

Example 2

Referring to FIG. 9 , it is a schematic diagram of the structure of a speed planning device provided in the present application, including: a speed planning expression acquisition module 21 and a solution and planning module 22 .

In some embodiments, the speed planning expression acquisition module 21 outputs multiple feasible solutions through N-segment speed planning expressions when the leading vehicle is within a preset range, and outputs the multiple feasible solutions to the solution and planning module 22; after receiving the multiple feasible solutions, the solution and planning module 22 obtains the optimal solution and performs speed planning based on the optimal solution.

The module 21 for obtaining the speed planning expression is used to divide the longitudinal trajectory into N segments and establish multi-term expressions for each segment to obtain the speed planning expression if the leading vehicle is within a preset range, with the goal of balancing driving comfort and driving safety. According to the position sampling points at the end of the trajectory, multiple feasible solutions of the speed planning expression are obtained; N is a positive integer.

In some embodiments, with the goal of balancing driving comfort and driving safety, the longitudinal trajectory is divided into N segments and multi-term expressions are established for each segment to obtain a speed planning expression, including: taking the current position as the initial planning position, taking the end position of the trajectory as the ending planning position, obtaining the planning distance, and establishing a local coordinate system in which the planning distance changes over time. Based on the local coordinate system, with the goal of balancing driving comfort and driving safety, the longitudinal trajectory is divided into two segments, and multi-term expressions and equality constraints are established for the two segments to obtain a speed planning expression.

In some embodiments, the speed planning expression includes: a first-segment speed planning expression, a second-segment speed planning expression and an equality constraint; establishing multi-term expressions and equality constraints for the two trajectories respectively to obtain a speed planning expression, including: establishing quartic expressions with the same format but different parameters for the two trajectories respectively to obtain a first-segment speed planning expression and a second-segment speed planning expression, and establishing an equality constraint based on the initial planning position, the end planning position and the path continuity of the two trajectories.

In some embodiments, the equation constraint includes: an acceleration equation constraint; the acceleration equation constraint establishes an equation constraint based on the path continuity of the two trajectories, including: based on the path continuity of the two trajectories, establishing a velocity equation constraint in which the end position of the velocity curve of the first trajectory and the initial position of the velocity curve of the second trajectory are the same, and rewriting the velocity equation constraint according to the acceleration change of the two trajectories to obtain the acceleration equation constraint.

In some embodiments, the acceleration equation constraint includes: a first acceleration equation constraint; rewriting the velocity equation constraint according to the acceleration change of the two trajectory segments to obtain the acceleration equation constraint, including: rewriting the velocity equation constraint according to the absolute values of the peaks or troughs of the acceleration curves of the first trajectory segment and the second trajectory segment being equal to obtain the first acceleration equation constraint; wherein the first acceleration equation constraint includes a first velocity change of the velocity equation constraint, and the first velocity change includes: a situation where the first trajectory segment accelerates and the second trajectory segment decelerates and a situation where the first trajectory segment decelerates and the second trajectory segment accelerates.

In some embodiments, the acceleration equation constraint also includes: a second acceleration equation constraint; rewriting the velocity equation constraint according to the acceleration change of the two trajectory segments to obtain the acceleration equation constraint, and also includes: rewriting the velocity equation constraint according to the peak or trough of the acceleration curve of the second trajectory segment as a constant value to obtain the second acceleration equation constraint; wherein the second acceleration equation constraint includes a second velocity change of the velocity equation constraint, and the second velocity change includes: the first trajectory segment is accelerated and the second trajectory segment is uniformly accelerated and the first trajectory segment is decelerated and the second trajectory segment is uniformly accelerated.

In some embodiments, the equality constraint also includes: the first segment speed planning expression is a constant at the initial planning position; and the starting speed of the initial planning position is the current actual speed of the vehicle; and the starting acceleration of the acceleration curve of the first segment trajectory is a constant; and the end speed of the speed curve of the first segment trajectory is equal to the initial speed of the speed curve of the second segment trajectory; and the end speed of the speed curve of the second segment trajectory is equal to the current actual speed of the leading vehicle; and the end speed acceleration of the second segment trajectory regresses to a constant; and the total duration of the speed planning is a preset value; and the end position of the trajectory is a position sampling point.

In some embodiments, if the leading vehicle is not within a preset range, a linear expression is established for the longitudinal trajectory, and the acceleration is sampled within a preset acceleration range to obtain a number of acceleration sampling points. Based on the number of acceleration sampling points, multiple feasible solutions of the speed planning expression are obtained, so that the feasible solution with the lowest cost is selected from the multiple feasible solutions as the optimal solution.

The solution and planning module 22 is used to select the feasible solution with the lowest cost from multiple feasible solutions as the optimal solution, and perform speed planning based on the optimal solution; the cost is obtained by the weighted sum of the first evaluation standard of driving comfort and the second evaluation standard of driving safety.

In some embodiments, the first evaluation criterion includes: comfort cost; the second evaluation criterion includes: collision cost, speed cost, centripetal acceleration cost and stable following cost; the stable following cost is obtained based on the difference between the longitudinal displacement at each moment and the safety distance at each moment; the cost is obtained by the weighted sum of the first evaluation criterion for driving comfort and the second evaluation criterion for driving safety; including: obtaining the cost based on the weighted sum of the comfort cost and the collision cost, speed cost, centripetal acceleration cost and stable following cost.

Compared with a single multi-term expression, the present application adopts a balanced driving comfort and driving safety as the goal, divides the longitudinal trajectory into N segments and establishes multi-term expressions for each segment, which is equivalent to adding (N-1) intermediate control points, so that the intermediate process of speed planning of the single multi-term expression can be controlled, thereby improving the accuracy of speed planning, and using the comfort cost as the first evaluation criterion for driving comfort, and using the collision cost, speed cost, centripetal acceleration cost and stable following cost as the second evaluation criterion for driving safety. The optimal solution is selected from multiple feasible solutions, which can make the speed planning expression more fit the optimization goal of balancing driving comfort and driving safety, thereby being closer to the optimal solution and further improving the accuracy of speed planning.

Those skilled in the art will appreciate that the embodiments of the present application may also provide computer program products. Therefore, the present application may adopt the form of complete hardware embodiments, complete software embodiments, or embodiments in combination with software and hardware. Moreover, the present application may adopt the form of computer program products implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that contain computer-usable program codes.

The present application is described with reference to the flowchart and/or block diagram of the method, device (apparatus), and computer program product according to the embodiment of the present application. It should be understood that each process and/or box in the flowchart and/or block diagram, and the combination of the process and/or box in the flowchart and/or block diagram can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device produce a device for implementing the function specified in one process or multiple processes in the flowchart and/or one box or multiple boxes in the block diagram.

These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer-readable memory produce a manufactured product including an instruction device that implements the functions specified in one or more processes in the flowchart and/or one or more boxes in the block diagram.

These computer program instructions may also be loaded onto a computer or other programmable data processing device so that a series of operational steps are executed on the computer or other programmable device to produce a computer-implemented process, whereby the instructions executed on the computer or other programmable device provide steps for implementing the functions specified in one or more processes in the flowchart and/or one or more boxes in the block diagram.

The above is only a preferred implementation of the present application. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the technical principles of the present application. These improvements and modifications should also be regarded as the scope of protection of the present application.

Claims (10)

1. A speed planning method, characterized by comprising: If the preceding vehicle is within the preset range, the longitudinal trajectory is divided into N segments and multi-term expressions are established for each segment to obtain a speed planning expression, and multiple feasible solutions of the speed planning expression are obtained according to the position sampling points at the end of the trajectory; N is a positive integer; A feasible solution with the lowest cost is selected from the multiple feasible solutions as the optimal solution, and speed planning is performed using the optimal solution; the cost is obtained by weighting the first evaluation standard of driving comfort and the second evaluation standard of driving safety. 2. The speed planning method according to claim 1, characterized in that the goal of balancing driving comfort and driving safety is to divide the longitudinal trajectory into N segments and establish multi-term expressions for each segment to obtain a speed planning expression, including: The current position is taken as the initial planning position, the end position of the trajectory is taken as the ending planning position, the planning distance is obtained, and a local coordinate system in which the planning distance changes with time is established. Based on the local coordinate system, with the goal of balancing driving comfort and driving safety, the longitudinal trajectory is divided into two trajectories, and multi-term expressions and equality constraints are established for the two trajectories respectively to obtain a speed planning expression. 3. The speed planning method according to claim 2, characterized in that the speed planning expression comprises: a first-segment speed planning expression, a second-segment speed planning expression and an equality constraint; the multi-term expression and the equality constraint are established for the two segments of the trajectory respectively to obtain the speed planning expression, comprising: A quartic expression with the same format but different parameters is established for the two trajectories respectively, to obtain the first speed planning expression and the second speed planning expression, and an equality constraint is established according to the initial planning position, the end planning position and the path continuity of the two trajectories. 4. The speed planning method according to claim 3, wherein the equation constraint comprises: an acceleration equation constraint; the acceleration equation constraint is established according to the path continuity of the two trajectories, comprising: According to the path continuity of the two trajectories, a velocity equation constraint is established in which the end position of the velocity curve of the first trajectory and the initial position of the velocity curve of the second trajectory are the same, and the velocity equation constraint is rewritten according to the acceleration change of the two trajectories to obtain the acceleration equation constraint. 5. The velocity planning method according to claim 4, wherein the acceleration equation constraint comprises: a first acceleration equation constraint; and rewriting the velocity equation constraint according to the acceleration change of the two trajectories to obtain the acceleration equation constraint comprises: According to the equal absolute values of the peaks or troughs of the acceleration curves of the first and second trajectory segments, the velocity equation constraint is rewritten to obtain the first acceleration equation constraint; wherein the first acceleration equation constraint includes a first velocity change of the velocity equation constraint, and the first velocity change includes: a situation where the first trajectory segment accelerates and the second trajectory segment decelerates, and a situation where the first trajectory segment decelerates and the second trajectory segment accelerates. 6. The velocity planning method according to claim 5, wherein the acceleration equation constraint further comprises: a second acceleration equation constraint; and the velocity equation constraint is rewritten according to the acceleration change of the two trajectories to obtain the acceleration equation constraint, further comprising: The velocity equation constraint is rewritten according to the peak or trough of the acceleration curve of the second trajectory as a constant value to obtain the second acceleration equation constraint; wherein the second acceleration equation constraint includes the second velocity change of the velocity equation constraint, and the second velocity change includes: the first trajectory is accelerated and the second trajectory is uniform, and the first trajectory is decelerated and the second trajectory is uniform. 7. The speed planning method according to claim 6, wherein the equality constraint further comprises: The first speed planning expression is the fixed value at the initial planning position; and The starting speed of the initial planning position is the current actual speed of the vehicle; and, The initial acceleration of the acceleration curve of the first trajectory is the constant value; and The final speed of the speed curve of the first track segment is equal to the initial speed of the speed curve of the second track segment; and The speed at the end of the speed curve of the second trajectory is equal to the current actual speed of the preceding vehicle; and The final velocity acceleration of the second trajectory returns to the constant value; and The total duration of the speed planning is a preset value; and, The end position of the trajectory is a position sampling point. 8. The speed planning method according to claim 7, characterized in that the first evaluation criterion includes: comfort cost; the second evaluation criterion includes: collision cost, speed cost, centripetal acceleration cost and stable following cost; the stable following cost is obtained according to the difference between the longitudinal displacement at each moment and the safety distance at each moment; the cost is obtained according to the weighted sum of the first evaluation criterion of driving comfort and the second evaluation criterion of driving safety; including: The cost is obtained according to a weighted sum of the comfort cost, the collision cost, the speed cost, the centripetal acceleration cost and the stable following cost. 9. The speed planning method as described in claim 1 is characterized in that if the leading vehicle is not within a preset range, a linear expression is established for the longitudinal trajectory, and the acceleration is sampled within a preset acceleration range to obtain a plurality of acceleration sampling points, and based on the plurality of acceleration sampling points, a plurality of feasible solutions of the speed planning expression are obtained, so that the feasible solution with the lowest cost is selected from the plurality of feasible solutions as the optimal solution. 10. A speed planning device, characterized by comprising: a speed planning expression acquisition module and a solution and planning module; wherein, The module for obtaining the speed planning expression is used to divide the longitudinal trajectory into N segments and establish multi-term expressions respectively to obtain the speed planning expression if the leading vehicle is within the preset range, with the goal of balancing driving comfort and driving safety, and obtain multiple feasible solutions of the speed planning expression according to the position sampling points at the end of the trajectory; N is a positive integer; The solution and planning module is used to select the feasible solution with the lowest cost from the multiple feasible solutions as the optimal solution, and perform speed planning based on the optimal solution; the cost is obtained by the weighted sum of the first evaluation standard of driving comfort and the second evaluation standard of driving safety.