CN109814576B - Method, apparatus and storage medium for speed planning of autonomous vehicles - Google Patents
Method, apparatus and storage medium for speed planning of autonomous vehicles Download PDFInfo
- Publication number
- CN109814576B CN109814576B CN201910135324.9A CN201910135324A CN109814576B CN 109814576 B CN109814576 B CN 109814576B CN 201910135324 A CN201910135324 A CN 201910135324A CN 109814576 B CN109814576 B CN 109814576B
- Authority
- CN
- China
- Prior art keywords
- speed
- obstacle
- track
- time point
- cost function
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Abstract
The embodiment of the invention provides a speed planning method and device for an automatic driving vehicle and a storage medium. The method comprises the following steps: under the condition that an automatic driving vehicle needs to pass through an obstacle, generating a plurality of speed tracks to be selected for the automatic driving vehicle, wherein the speed tracks to be selected comprise speeds to be selected corresponding to a plurality of time points in a planning time range; setting the expected speed of the automatic driving vehicle passing through the obstacle according to the minimum transverse distance between the automatic driving vehicle and the obstacle for each speed track to be selected; calculating a cost function value of the to-be-selected speed track according to the to-be-selected speed and the expected speed corresponding to each time point; and selecting the speed track to be selected with the minimum cost function value as the planned speed track of the automatic driving vehicle passing through the obstacle. The embodiment of the invention can plan a reasonable and accurate speed track for the automatic driving vehicle and ensure that a sufficient interval is kept when the automatic driving vehicle passes through an obstacle.
Description
Technical Field
The present invention relates to the field of automatic driving technologies, and in particular, to a speed planning method and apparatus for an automatic driving vehicle, and a storage medium.
Background
Autonomous vehicles include vehicles that operate in an autonomous mode (e.g., unmanned). Autonomous vehicles may free the driver from some driving-related responsibilities, allowing for driving with minimal human-machine interaction. Autonomous vehicles encounter obstacles that appear ahead during travel and may need to pass by the obstacle. In this case, trajectory planning, including velocity planning, is required for it. If the speed planning is not reasonable, the psychological safety and comfort of passengers can be reduced, and even traffic accidents can happen.
Disclosure of Invention
Embodiments of the present invention provide a speed planning method, apparatus, and storage medium for an autonomous vehicle, so as to solve one or more technical problems in the prior art.
In a first aspect, an embodiment of the present invention provides a speed planning method for an autonomous vehicle, including:
under the condition that an automatic driving vehicle needs to pass through an obstacle, generating a plurality of speed tracks to be selected for the automatic driving vehicle, wherein the speed tracks to be selected comprise speeds to be selected corresponding to a plurality of time points in a planning time range;
setting the expected speed of the automatic driving vehicle passing through the obstacle according to the minimum transverse distance between the automatic driving vehicle and the obstacle for each speed track to be selected; calculating a cost function value of the to-be-selected speed track according to the to-be-selected speed and the expected speed corresponding to each time point;
and selecting the speed track to be selected with the minimum cost function value as the planned speed track of the automatic driving vehicle passing through the obstacle.
In one embodiment, calculating a cost function value of the candidate speed trajectory according to the desired speed and the candidate speed corresponding to the plurality of time points includes:
calculating a cost function value corresponding to each time point;
and accumulating the cost function values corresponding to the time points to obtain the cost function value of the speed track to be selected.
In one embodiment, calculating the cost function value corresponding to each of the time points includes:
generating a corresponding travel path track in a path time coordinate system according to the speed track to be selected, wherein the horizontal axis and the vertical axis of the path time coordinate system are respectively time and position, and the travel path track comprises the positions to be selected corresponding to the multiple time points;
mapping the contour track of the obstacle to the path time coordinate system to obtain a mapping track band of the obstacle;
determining whether the position to be selected corresponding to the time point falls into the mapping track zone;
and if the position to be selected falls into the mapping track band, the cost function value corresponding to the time point is 0.
In one embodiment, calculating the cost function value corresponding to each of the time points includes:
if the candidate position does not fall into the mapping track band, according to the formula cost, x1 × max (v-v _ desired, 0)2Calculating a cost function value corresponding to the time point; the cost is a cost function value corresponding to the time point, v is a to-be-selected speed corresponding to the time point, v _ desired is the expected speed, and x1 is a first preset parameter.
In one embodiment, mapping the contour trajectory of the obstacle into the path time coordinate system to obtain a mapped trajectory band of the obstacle includes:
and adjusting the width of the mapping track band to enable the width to be larger than a preset value.
In one embodiment, setting a desired speed of the autonomous vehicle past the obstacle in accordance with a minimum lateral separation of the autonomous vehicle from the obstacle comprises:
determining a distance threshold according to the type of the obstacle;
if the obstacle is a static obstacle, calculating the expected speed corresponding to the time point according to a formula v _ desired ═ x2 × (L-L0); wherein v _ desired is the expected speed corresponding to the time point, L is the minimum transverse distance, L0 is the distance threshold, and x2 is a second preset parameter.
In one embodiment, setting a desired speed of the autonomous vehicle past the obstacle in accordance with a minimum lateral separation of the autonomous vehicle from the obstacle comprises:
determining a distance threshold according to the type of the obstacle;
if the obstacle is a moving obstacle, judging the moving direction of the obstacle;
calculating the desired speed corresponding to the time point according to a formula v _ desired ═ x2 × (L-L0) × cos θ if the moving direction of the obstacle is approaching the autonomous vehicle; wherein v _ desired is the expected speed corresponding to the time point, L is the minimum transverse distance, L0 is the distance threshold, x2 is a second preset parameter, and θ is an included angle between the moving direction and the head direction of the autonomous vehicle.
In one embodiment, setting a desired speed of the autonomous vehicle past the obstacle in accordance with a minimum lateral separation of the autonomous vehicle from the obstacle comprises:
if the direction of movement of the obstacle is away from the autonomous vehicle, the desired speed corresponding to the point in time is calculated according to the formula v _ desired x2 x (L-L0).
In a second aspect, an embodiment of the present invention provides a speed planning apparatus for an autonomous vehicle, including:
the system comprises a generating module, a judging module and a judging module, wherein the generating module is used for generating a plurality of candidate speed tracks for an automatic driving vehicle under the condition that the automatic driving vehicle needs to pass through an obstacle, and the candidate speed tracks comprise candidate speeds corresponding to a plurality of time points in a planning time range;
the calculation module is used for setting the expected speed of the automatic driving vehicle passing through the obstacle according to the minimum transverse distance between the automatic driving vehicle and the obstacle for each speed track to be selected; calculating a cost function value of the to-be-selected speed track according to the to-be-selected speed and the expected speed corresponding to each time point;
and the selection module is used for selecting the speed track to be selected with the minimum cost function value as a planned speed track of the automatic driving vehicle passing through the obstacle.
In one embodiment, the calculation module comprises:
the first calculation submodule is used for calculating a cost function value corresponding to each time point;
and the accumulation submodule is used for accumulating the cost function value corresponding to each time point to obtain the cost function value of the speed track to be selected.
In one embodiment, the first computation submodule includes:
the generating unit is used for generating a corresponding travel path track according to the speed track to be selected in a path time coordinate system, wherein the horizontal axis and the vertical axis of the path time coordinate system are respectively time and position, and the travel path track comprises the positions to be selected corresponding to the multiple time points;
the mapping unit is used for mapping the contour track of the obstacle to the path time coordinate system to obtain a mapping track band of the obstacle;
the determining unit is used for determining whether the position to be selected corresponding to the time point falls into the mapping track band;
and the cost function value obtaining unit is used for obtaining a cost function value of 0 corresponding to the time point if the position to be selected falls into the mapping track zone.
In one embodiment, the first computation submodule includes:
a cost function value calculating unit, configured to, if the candidate position does not fall into the mapping track zone, obtain a cost as x1 × max (v-v _ desired, 0)2Calculating a cost function value corresponding to the time point; the cost is a cost function value corresponding to the time point, v is a to-be-selected speed corresponding to the time point, v _ desired is the expected speed, and x1 is a first preset parameter.
In one embodiment, the mapping unit comprises:
and the adjusting subunit is used for adjusting the width of the mapping track band to enable the width to be larger than a preset value.
In one embodiment, the calculation module comprises:
the determining submodule is used for determining a distance threshold according to the type of the obstacle;
a second calculating submodule, configured to calculate the expected speed corresponding to the time point according to a formula v _ desired ═ x2 × (L-L0) if the obstacle is a static obstacle; wherein v _ desired is the expected speed corresponding to the time point, L is the minimum transverse distance, L0 is the distance threshold, and x2 is a second preset parameter.
In one embodiment, the calculation module comprises:
the determining submodule is used for determining a distance threshold according to the type of the obstacle;
the judgment submodule is used for judging the movement direction of the barrier if the barrier is a movement barrier;
a third calculation submodule for calculating the desired speed corresponding to the time point according to a formula v _ desired ═ x2 × (L-L0) × cos θ if the direction of movement of the obstacle is approaching the autonomous vehicle; wherein v _ desired is the expected speed corresponding to the time point, L is the minimum transverse distance, L0 is the distance threshold, x2 is a second preset parameter, and θ is an included angle between the moving direction and the head direction of the autonomous vehicle.
In one embodiment, the calculation module comprises:
a fourth calculation submodule for calculating the desired speed corresponding to the time point according to a formula v _ desired ═ x2 × (L-L0) if the direction of movement of the obstacle is away from the autonomous vehicle.
In a third aspect, an embodiment of the present invention provides a speed planning apparatus for an autonomous vehicle, where functions of the apparatus may be implemented by hardware, or may be implemented by hardware executing corresponding software. The hardware or software includes one or more modules corresponding to the above-described functions.
In one possible design, the apparatus includes a processor and a memory, the memory is used for storing a program supporting the apparatus to execute the method, and the processor is configured to execute the program stored in the memory. The apparatus may also include a communication interface for communicating with other devices or a communication network.
In a fourth aspect, embodiments of the present invention provide a computer readable storage medium for storing computer software instructions for a speed planning apparatus for an autonomous vehicle, comprising a program for performing the method described above.
According to the technical scheme, the cost function value of each speed track to be selected is calculated according to the psychological safety speed of passengers when the automatic driving vehicle passes through the obstacle and the relative speed of the passengers and the obstacle, and the speed track to be selected with the minimum cost function value is determined as the planning speed track of the lane change of the automatic driving vehicle, so that the comfort level of the passengers can be improved, the speed planning is more reasonable, and the automatic driving vehicle and the obstacle can be kept in a sufficient interval.
The foregoing summary is provided for the purpose of description only and is not intended to be limiting in any way. In addition to the illustrative aspects, embodiments, and features described above, further aspects, embodiments, and features of the present invention will be readily apparent by reference to the drawings and following detailed description.
Drawings
In the drawings, like reference numerals refer to the same or similar parts or elements throughout the several views unless otherwise specified. The figures are not necessarily to scale. It is appreciated that these drawings depict only some embodiments in accordance with the disclosure and are therefore not to be considered limiting of its scope.
Fig. 1 shows a flow chart of a method for speed planning of an autonomous vehicle according to an embodiment of the invention.
Fig. 2 is a diagram illustrating an application example of a speed planning method of an autonomous vehicle according to an embodiment of the present invention.
FIG. 3 shows a flow chart of a method of speed planning for an autonomous vehicle according to an embodiment of the invention.
FIG. 4 shows a flow chart of a method of speed planning for an autonomous vehicle according to another implementation of an embodiment of the invention.
FIG. 5 shows a mapping track strip schematic of a travel path track and a static obstacle according to an embodiment of the invention.
Fig. 6 shows a schematic diagram of a driving path trajectory and a mapping trajectory band of a dynamic obstacle according to an embodiment of the present invention.
Fig. 7 shows a block diagram of a speed planning apparatus of an autonomous vehicle according to an embodiment of the present invention.
Fig. 8 shows a block diagram of a speed planning apparatus of an autonomous vehicle according to an embodiment of the present invention.
Fig. 9 shows a block diagram of a speed planning apparatus of an autonomous vehicle according to another embodiment of the present invention.
Fig. 10 shows a block diagram of the structure of a speed planning apparatus of an autonomous vehicle according to an embodiment of the present invention.
Detailed Description
In the following, only certain exemplary embodiments are briefly described. As those skilled in the art will recognize, the described embodiments may be modified in various different ways, all without departing from the spirit or scope of the present invention. Accordingly, the drawings and description are to be regarded as illustrative in nature, and not as restrictive.
Fig. 1 shows a flow chart of a method for speed planning of an autonomous vehicle according to an embodiment of the invention. As shown in fig. 1, the method may include the steps of:
step S100, under the condition that an automatic driving vehicle needs to pass through an obstacle, generating a plurality of speed tracks to be selected for the automatic driving vehicle, wherein the speed tracks to be selected comprise speeds to be selected corresponding to a plurality of time points in a planning time range;
s200, setting an expected speed of the automatic driving vehicle passing through the obstacle according to the minimum transverse distance between the automatic driving vehicle and the obstacle for each speed track to be selected; calculating a cost function value of the to-be-selected speed track according to the to-be-selected speed and the expected speed corresponding to each time point;
and S300, selecting a speed track to be selected with the minimum cost function value as a planned speed track of the automatic driving vehicle passing through the obstacle.
The speed of the autonomous vehicle at various points in time may be planned during travel of the autonomous vehicle along the selected route, thereby planning a speed trajectory for the autonomous vehicle.
Fig. 2 shows an application scenario of the speed planning method of an autonomous vehicle according to an embodiment of the invention. In fig. 2, a road with obstacles is shown, the driving direction of which is upward in the picture. The obstacles may include static obstacles such as road shoulders, walls, green belts, etc.; the obstacles may also include dynamic obstacles, such as pedestrians, vehicles, etc. In fig. 2, the driving route of the autonomous vehicle includes portions a to B, and it can be seen that the autonomous vehicle needs to pass by an obstacle.
In the embodiment, under the condition that the automatic driving vehicle needs to pass through the obstacle, a plurality of speed tracks to be selected can be generated for the automatic driving vehicle; and then calculating a cost function value of each to-be-selected speed track, and selecting the to-be-selected speed track with the minimum cost function value as a planned speed track. The autonomous vehicle may pass by the obstacle at the planned speed trajectory.
Wherein the candidate speed trajectory may be represented by a two-dimensional curve of speed and time. The planning time range is divided into a plurality of time points at preset time intervals, and then the speed trajectory to be selected is discretized, so that the speed to be selected corresponding to each time point can be obtained.
In one example, the planning time range may be 8 seconds. Because errors may be increased when planning the speed trajectory after 8 seconds, data redundancy can be reduced and planning accuracy can be improved when speed planning within 8 seconds is performed.
In step S200, a minimum lateral spacing between the autonomous vehicle and the obstacle may be determined based on the course of the autonomous vehicle and the location of the obstacle. For example: l shown in fig. 2 represents the minimum lateral spacing. The transverse direction refers to a direction perpendicular to the direction of the head of the automatic driving vehicle. The desired speed of the autonomous vehicle when passing over the obstacle may be set according to the minimum lateral separation. For example: the minimum lateral spacing is smaller and a smaller desired speed can be set to improve the ride comfort and the psychological safety of the passengers. Further, a cost function value of the candidate speed trajectory can be calculated according to a difference value between the candidate speed and the expected speed.
In one embodiment, as shown in fig. 3, in step S200, the method may include:
step S210, calculating a cost function value corresponding to each time point;
and step S220, accumulating the cost function values corresponding to the time points to obtain the cost function values of the speed tracks to be selected.
The candidate speed trajectory may include a plurality of discrete points, each discrete point including a time point and a corresponding candidate speed. And accumulating the cost function value of each discrete point to obtain the cost function value of the speed track to be selected.
In one embodiment, as shown in fig. 4, in step S210, the method may include:
step S211, generating a corresponding travel path track in a path time coordinate system according to the speed track to be selected, wherein the horizontal axis and the vertical axis of the path time coordinate system are respectively time and position, and the travel path track comprises positions to be selected corresponding to the multiple time points;
step S212, the contour track of the obstacle is mapped to the path time coordinate system to obtain a mapping track band of the obstacle;
step S213, determining whether the position to be selected corresponding to the time point falls into the mapping track band;
step S214, if the position to be selected falls into the mapping track band, the cost function value corresponding to the time point is 0;
step S215, if the position to be selected does not fall into the mapping track band, x1 × max (v-v _ desired, 0) according to the formula cost2Calculating a cost function value corresponding to the time point; wherein cost is a cost function value corresponding to the time point, and v is the time pointAnd the corresponding candidate speed, v _ desired is the expected speed, and x1 is a first preset parameter.
In one example, as shown in fig. 5 and 6, in the path time coordinate system, the horizontal axis is time (t) in seconds; the vertical axis is position(s) in meters. According to the formula s-v × t, the candidate speed trajectory v (t) may be converted into a travel path trajectory s (t), and the travel path trajectory s (t) may be expressed in a path time coordinate system. Wherein the planning time range Δ t includes a plurality of time points t0, t1, t2, t3, t4 and t 5; the multiple time points t0, t1, t2, t3, t4 and t5 respectively correspond to a candidate position s0, a candidate position s1, a candidate position s2, a candidate position s3, a candidate position s4 and a candidate position s 5.
In fig. 5, the longitudinal (s-direction) profile of the obstacle C1 at each time point is mapped into a path time coordinate system, resulting in a plurality of discrete bands. By fitting these plurality of discrete bands, a mapping trajectory band N1 of the obstacle C1 can be obtained. Among them, the obstacle C1 is a static obstacle. In a similar manner, a mapped trajectory band N2 of obstacle C2 may be obtained in FIG. 6. Among them, the obstacle C2 is a dynamic obstacle. Therefore, the mapping track strip N2 is an efficient strip area.
In one embodiment, in step S212, the method may include: and adjusting the width of the mapping track band to enable the width to be larger than a preset value.
For example: the preset value was set at 25 meters. If the length of the contour of the obstacle C1 in the s-direction is 6 meters, i.e., less than the preset value, the width of the corresponding mapping track strip will be less than the preset value. The obstacle C1 may be added with 10 meters of buffer zone in each direction s to form the mapped track zone N1 in fig. 5. The width of the mapping track belt N1 is 26 meters and is larger than a preset value.
In this embodiment, whether the position to be selected corresponding to each time point on the travel path track falls into the mapping track zone or not can be determined according to the travel path track and the mapping track zone; and then, according to the falling condition, calculating the cost function value corresponding to each time point in different modes respectively.
For example: in fig. 5, the candidate positions s0, s1, and s2 fall into the map of obstacle C1A trajectory belt N1; the cost function values corresponding to the time points t0, t1, and t2 are 0. The candidate positions s3, s4 and s5 do not fall into the mapping track zone N1 of the obstacle C1, then x1 × max (v-v _ desired, 0) according to the formula cost2The cost function values are calculated for time points t3, t4, and t5, respectively.
In this embodiment, the cost function value of the candidate speed trajectory is related to the desired speed. In one embodiment, as shown in fig. 7, in step S200, the method may include:
step S230, determining a distance threshold according to the type of the obstacle;
step S240, if the obstacle is a static obstacle, calculating the expected speed corresponding to the time point according to a formula v _ desired ═ x2 × (L-L0); wherein v _ desired is the expected speed corresponding to the time point, L is the minimum transverse distance, L0 is the distance threshold, and x2 is a second preset parameter;
step S250, if the obstacle is a moving obstacle, judging the moving direction of the obstacle;
step S260, if the moving direction of the obstacle is approaching the autonomous vehicle, calculating the desired speed corresponding to the time point according to a formula v _ desired ═ x2 × (L-L0) × cos θ; wherein θ is an included angle between the motion direction and the direction of the head of the autonomous vehicle;
and step S270, if the moving direction of the obstacle is away from the autonomous vehicle, calculating the desired speed corresponding to the time point according to a formula v _ desired ═ x2 × (L-L0).
For example: in fig. 5, the obstacle C1 is a static obstacle, and the expected speed at each time point on the trajectory v (t) to be selected is calculated according to the formula v _ desired ═ x2 × (L-L0).
Another example is: in fig. 6, the obstacle C2 is a dynamic obstacle, and the expected speed at each time point on the candidate speed trajectory v (t) is calculated according to the formula v _ desired ═ x2 × (L-L0) × cos θ.
That is, the desired velocity may be related to the direction of movement of the obstacle. The desired speed may decrease when the direction of movement of the obstacle is close to the driving path of the autonomous vehicle. When the moving direction of the obstacle is away from the running route of the autonomous vehicle, θ may be set to 0 °.
Wherein the spacing threshold L0 is associated with the category of the obstacle. For example: if the obstacle is a pedestrian, a smaller distance threshold may be set, such as L0 ═ 0.5 m.
After obtaining the desired velocity at each time point, the cost function value of the candidate velocity trajectory v (t) may be obtained according to the methods described in step S210 and step S220.
To sum up, the speed planning method for the autonomous vehicle according to the embodiment of the present invention generates a plurality of candidate speed trajectories, calculates the cost function value of each candidate speed trajectory according to the expected speed of the autonomous vehicle when passing through the obstacle, and determines the candidate speed trajectory with the minimum cost function value as the planned speed trajectory for the lane change of the autonomous vehicle, so that the speed planning is more reasonable, and it is ensured that an adequate interval is maintained between the autonomous vehicle and the obstacle. Further, in the process of speed planning, when the expected speed is calculated, the type, motion and motion direction of the obstacle are considered, so that the speed planning can be more accurate.
Fig. 8 is a block diagram showing a configuration of a velocity trajectory generation apparatus according to an embodiment of the present invention. As shown in fig. 8, the apparatus may include:
the automatic selection system comprises a generation module 100, a selection module and a selection module, wherein the generation module 100 is used for generating a plurality of candidate speed tracks for an automatic driving vehicle under the condition that the automatic driving vehicle needs to pass through an obstacle, and the candidate speed tracks comprise candidate speeds corresponding to a plurality of time points in a planning time range;
a calculation module 200, configured to set, for each candidate speed trajectory, an expected speed of the autonomous vehicle passing through the obstacle according to a minimum lateral distance between the autonomous vehicle and the obstacle; calculating a cost function value of the to-be-selected speed track according to the to-be-selected speed and the expected speed corresponding to each time point;
and the selecting module 300 is configured to select a to-be-selected speed trajectory with the smallest cost function value as a planned speed trajectory of the autonomous vehicle passing through the obstacle.
In one embodiment, as shown in fig. 9, the computing module 200 may include:
the first calculating submodule 210 is configured to calculate a cost function value corresponding to each time point;
and the accumulation submodule 220 is configured to accumulate the cost function value corresponding to each time point to obtain a cost function value of the speed trajectory to be selected.
In one embodiment, the first computation submodule 210 may include:
the generating unit is used for generating a corresponding travel path track according to the speed track to be selected in a path time coordinate system, wherein the horizontal axis and the vertical axis of the path time coordinate system are respectively time and position, and the travel path track comprises the positions to be selected corresponding to the multiple time points;
the mapping unit is used for mapping the contour track of the obstacle to the path time coordinate system to obtain a mapping track band of the obstacle;
the determining unit is used for determining whether the position to be selected corresponding to the time point falls into the mapping track band;
and the cost function value obtaining unit is used for obtaining a cost function value of 0 corresponding to the time point if the position to be selected falls into the mapping track zone.
In one embodiment, the first computation submodule 212 may include:
a cost function value calculating unit, configured to, if the candidate position does not fall into the mapping track zone, obtain a cost as x1 × max (v-v _ desired, 0)2Calculating a cost function value corresponding to the time point; the cost is a cost function value corresponding to the time point, v is a to-be-selected speed corresponding to the time point, v _ desired is the expected speed, and x1 is a first preset parameter.
In one embodiment, the mapping unit may include:
and the adjusting subunit is used for adjusting the width of the mapping track band to enable the width to be larger than a preset value.
In one embodiment, the computing module 200 may include:
a determining submodule 230, configured to determine a distance threshold according to the category of the obstacle;
a second calculating submodule 240, configured to calculate the expected speed corresponding to the time point according to a formula v _ desired ═ x2 × (L-L0) if the obstacle is a static obstacle; wherein v _ desired is the expected speed corresponding to the time point, L is the minimum transverse distance, L0 is the distance threshold, and x2 is a second preset parameter.
In one embodiment, the computing module 200 may include:
a determining submodule 230, configured to determine a distance threshold according to the category of the obstacle;
a judging submodule 250, configured to judge a movement direction of the obstacle if the obstacle is a movement obstacle;
a third calculation sub-module 260 for calculating the desired speed corresponding to the time point according to a formula v _ desired ═ x2 × (L-L0) × cos θ if the moving direction of the obstacle is approaching the autonomous vehicle; wherein v _ desired is the expected speed corresponding to the time point, L is the minimum transverse distance, L0 is the distance threshold, x2 is a second preset parameter, and θ is an included angle between the moving direction and the head direction of the autonomous vehicle.
In one embodiment, the computing module 200 may include:
a fourth calculation submodule 270, configured to calculate the desired speed corresponding to the time point according to the formula v _ desired ═ x2 × (L-L0) if the direction of movement of the obstacle is away from the autonomous vehicle.
The functions of each module in each apparatus in the embodiments of the present invention may refer to the corresponding description in the above method, and are not described herein again.
Fig. 10 shows a block diagram of the structure of a speed planning apparatus of an autonomous vehicle according to an embodiment of the present invention. As shown in fig. 10, the apparatus includes: a memory 1010 and a processor 1020, the memory 1010 having stored therein computer programs executable on the processor 1020. The processor 1020, when executing the computer program, implements the method for speed planning for an autonomous vehicle in the above-described embodiments. The number of the memory 1010 and the processor 1020 may be one or more.
The device also includes:
and a communication interface 1030, configured to communicate with an external device, and perform data interactive transmission.
If the memory 1010, the processor 1020, and the communication interface 1030 are implemented independently, the memory 1010, the processor 1020, and the communication interface 1030 may be connected to each other through a bus and perform communication with each other. The bus may be an Industry Standard Architecture (ISA) bus, a Peripheral Component Interconnect (PCI) bus, an Extended Industry Standard Architecture (EISA) bus, or the like. The bus may be divided into an address bus, a data bus, a control bus, etc. For ease of illustration, only one thick line is shown in FIG. 10, but this is not intended to represent only one bus or type of bus.
Optionally, in an implementation, if the memory 1010, the processor 1020, and the communication interface 1030 are integrated on a chip, the memory 1010, the processor 1020, and the communication interface 1030 may communicate with each other through an internal interface.
An embodiment of the present invention provides a computer-readable storage medium, which stores a computer program, and the computer program is used for implementing the method of any one of the above embodiments when being executed by a processor.
In the description herein, references to the description of the term "one embodiment," "some embodiments," "an example," "a specific example," or "some examples," etc., mean that a particular feature, structure, material, or characteristic described in connection with the embodiment or example is included in at least one embodiment or example of the invention. Furthermore, the particular features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples. Furthermore, various embodiments or examples and features of different embodiments or examples described in this specification can be combined and combined by one skilled in the art without contradiction.
Furthermore, the terms "first", "second" and "first" are used for descriptive purposes only and are not to be construed as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one such feature. In the description of the present invention, "a plurality" means two or more unless specifically defined otherwise.
Any process or method descriptions in flow charts or otherwise described herein may be understood as representing modules, segments, or portions of code which include one or more executable instructions for implementing specific logical functions or steps of the process, and alternate implementations are included within the scope of the preferred embodiment of the present invention in which functions may be executed out of order from that shown or discussed, including substantially concurrently or in reverse order, depending on the functionality involved, as would be understood by those reasonably skilled in the art of the present invention.
The logic and/or steps represented in the flowcharts or otherwise described herein, e.g., an ordered listing of executable instructions that can be considered to implement logical functions, can be embodied in any computer-readable medium for use by or in connection with an instruction execution system, apparatus, or device, such as a computer-based system, processor-containing system, or other system that can fetch the instructions from the instruction execution system, apparatus, or device and execute the instructions. For the purposes of this description, a "computer-readable medium" can be any means that can contain, store, communicate, propagate, or transport the program for use by or in connection with the instruction execution system, apparatus, or device. More specific examples (a non-exhaustive list) of the computer-readable medium would include the following: an electrical connection (electronic device) having one or more wires, a portable computer diskette (magnetic device), a Random Access Memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber device, and a portable read-only memory (CDROM). Additionally, the computer-readable medium could even be paper or another suitable medium upon which the program is printed, as the program can be electronically captured, via for instance optical scanning of the paper or other medium, then compiled, interpreted or otherwise processed in a suitable manner if necessary, and then stored in a computer memory.
It should be understood that portions of the present invention may be implemented in hardware, software, firmware, or a combination thereof. In the above embodiments, the various steps or methods may be implemented in software or firmware stored in memory and executed by a suitable instruction execution system. For example, if implemented in hardware, as in another embodiment, any one or combination of the following techniques, which are known in the art, may be used: a discrete logic circuit having a logic gate circuit for implementing a logic function on a data signal, an application specific integrated circuit having an appropriate combinational logic gate circuit, a Programmable Gate Array (PGA), a Field Programmable Gate Array (FPGA), or the like.
It will be understood by those skilled in the art that all or part of the steps carried by the method for implementing the above embodiments may be implemented by hardware related to instructions of a program, which may be stored in a computer readable storage medium, and when the program is executed, the program includes one or a combination of the steps of the method embodiments.
In addition, functional units in the embodiments of the present invention may be integrated into one processing module, or each unit may exist alone physically, or two or more units are integrated into one module. The integrated module can be realized in a hardware mode, and can also be realized in a software functional module mode. The integrated module, if implemented in the form of a software functional module and sold or used as a separate product, may also be stored in a computer readable storage medium. The storage medium may be a read-only memory, a magnetic or optical disk, or the like.
The above description is only for the specific embodiment of the present invention, but the scope of the present invention is not limited thereto, and any person skilled in the art can easily conceive various changes or substitutions within the technical scope of the present invention, and these should be covered by the scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the protection scope of the appended claims.
Claims (18)
1. A method of speed planning for an autonomous vehicle, comprising:
under the condition that an automatic driving vehicle needs to pass through an obstacle, generating a plurality of speed tracks to be selected for the automatic driving vehicle, wherein the speed tracks to be selected comprise speeds to be selected corresponding to a plurality of time points in a planning time range;
setting the expected speed of the automatic driving vehicle passing through the obstacle according to the minimum transverse distance between the automatic driving vehicle and the obstacle for each speed track to be selected; calculating a cost function value of the to-be-selected speed track according to the to-be-selected speed and the expected speed corresponding to each time point;
and selecting the speed track to be selected with the minimum cost function value as the planned speed track of the automatic driving vehicle passing through the obstacle.
2. The method of claim 1, wherein calculating a cost function value for the candidate velocity trajectory based on the candidate velocity and the desired velocity corresponding to each of the time points comprises:
calculating a cost function value corresponding to each time point;
and accumulating the cost function values corresponding to the time points to obtain the cost function value of the speed track to be selected.
3. The method of claim 2, wherein calculating the cost function value corresponding to each of the time points comprises:
generating a corresponding travel path track in a path time coordinate system according to the speed track to be selected, wherein the horizontal axis and the vertical axis of the path time coordinate system are respectively time and position, and the travel path track comprises the positions to be selected corresponding to the multiple time points;
mapping the contour track of the obstacle to the path time coordinate system to obtain a mapping track band of the obstacle;
determining whether the position to be selected corresponding to the time point falls into the mapping track zone;
and if the position to be selected falls into the mapping track band, the cost function value corresponding to the time point is 0.
4. The method of claim 3, wherein calculating the cost function value corresponding to each of the time points comprises:
if the candidate position does not fall into the mapping track band, according to the formula cost, x1 × max (v-v _ desired, 0)2Calculating a cost function value corresponding to the time point; the cost is a cost function value corresponding to the time point, v is a to-be-selected speed corresponding to the time point, v _ desired is the expected speed, and x1 is a first preset parameter.
5. The method of claim 3, wherein mapping the contour trajectory of the obstacle into the path time coordinate system to obtain a mapped trajectory band of the obstacle comprises:
and adjusting the width of the mapping track band to enable the width to be larger than a preset value.
6. The method of claim 1, wherein setting a desired speed of the autonomous vehicle past the obstacle based on a minimum lateral spacing of the autonomous vehicle from the obstacle comprises:
determining a distance threshold according to the type of the obstacle;
if the obstacle is a static obstacle, calculating the expected speed corresponding to the time point according to a formula v _ desired ═ x2 × (L-L0); wherein v _ desired is the expected speed corresponding to the time point, L is the minimum transverse distance, L0 is the distance threshold, and x2 is a second preset parameter.
7. The method of claim 1, wherein setting a desired speed of the autonomous vehicle past the obstacle based on a minimum lateral spacing of the autonomous vehicle from the obstacle comprises:
determining a distance threshold according to the type of the obstacle;
if the obstacle is a moving obstacle, judging the moving direction of the obstacle;
calculating the desired speed corresponding to the time point according to a formula v _ desired ═ x2 × (L-L0) × cos θ if the moving direction of the obstacle is approaching the autonomous vehicle; wherein v _ desired is the expected speed corresponding to the time point, L is the minimum transverse distance, L0 is the distance threshold, x2 is a second preset parameter, and θ is an included angle between the moving direction and the head direction of the autonomous vehicle.
8. The method of claim 7, wherein setting a desired speed of the autonomous vehicle past the obstacle based on a minimum lateral spacing of the autonomous vehicle from the obstacle comprises:
if the direction of movement of the obstacle is away from the autonomous vehicle, the desired speed corresponding to the point in time is calculated according to the formula v _ desired x2 x (L-L0).
9. A speed planning apparatus for an autonomous vehicle, comprising:
the system comprises a generating module, a judging module and a judging module, wherein the generating module is used for generating a plurality of candidate speed tracks for an automatic driving vehicle under the condition that the automatic driving vehicle needs to pass through an obstacle, and the candidate speed tracks comprise candidate speeds corresponding to a plurality of time points in a planning time range;
the calculation module is used for setting the expected speed of the automatic driving vehicle passing through the obstacle according to the minimum transverse distance between the automatic driving vehicle and the obstacle for each speed track to be selected; calculating a cost function value of the to-be-selected speed track according to the to-be-selected speed and the expected speed corresponding to each time point;
and the selection module is used for selecting the speed track to be selected with the minimum cost function value as a planned speed track of the automatic driving vehicle passing through the obstacle.
10. The apparatus of claim 9, wherein the computing module comprises:
the first calculation submodule is used for calculating a cost function value corresponding to each time point;
and the accumulation submodule is used for accumulating the cost function value corresponding to each time point to obtain the cost function value of the speed track to be selected.
11. The apparatus of claim 10, wherein the first computation submodule comprises:
the generating unit is used for generating a corresponding travel path track according to the speed track to be selected in a path time coordinate system, wherein the horizontal axis and the vertical axis of the path time coordinate system are respectively time and position, and the travel path track comprises the positions to be selected corresponding to the multiple time points;
the mapping unit is used for mapping the contour track of the obstacle to the path time coordinate system to obtain a mapping track band of the obstacle;
the determining unit is used for determining whether the position to be selected corresponding to the time point falls into the mapping track band;
and the cost function value obtaining unit is used for obtaining a cost function value of 0 corresponding to the time point if the position to be selected falls into the mapping track zone.
12. The apparatus of claim 11, wherein the first computation submodule comprises:
a cost function value calculating unit, configured to, if the candidate position does not fall into the mapping track zone, obtain a cost as x1 × max (v-v _ desired, 0)2Calculating a cost function value corresponding to the time point; the cost is a cost function value corresponding to the time point, v is a to-be-selected speed corresponding to the time point, v _ desired is the expected speed, and x1 is a first preset parameter.
13. The apparatus of claim 11, wherein the mapping unit comprises:
and the adjusting subunit is used for adjusting the width of the mapping track band to enable the width to be larger than a preset value.
14. The apparatus of claim 9, wherein the computing module comprises:
the determining submodule is used for determining a distance threshold according to the type of the obstacle;
a second calculating submodule, configured to calculate the expected speed corresponding to the time point according to a formula v _ desired ═ x2 × (L-L0) if the obstacle is a static obstacle; wherein v _ desired is the expected speed corresponding to the time point, L is the minimum transverse distance, L0 is the distance threshold, and x2 is a second preset parameter.
15. The apparatus of claim 9, wherein the computing module comprises:
the determining submodule is used for determining a distance threshold according to the type of the obstacle;
the judgment submodule is used for judging the movement direction of the barrier if the barrier is a movement barrier;
a third calculation submodule for calculating the desired speed corresponding to the time point according to a formula v _ desired ═ x2 × (L-L0) × cos θ if the direction of movement of the obstacle is approaching the autonomous vehicle; wherein v _ desired is the expected speed corresponding to the time point, L is the minimum transverse distance, L0 is the distance threshold, x2 is a second preset parameter, and θ is an included angle between the moving direction and the head direction of the autonomous vehicle.
16. The apparatus of claim 15, wherein the computing module comprises:
a fourth calculation submodule for calculating the desired speed corresponding to the time point according to a formula v _ desired ═ x2 × (L-L0) if the direction of movement of the obstacle is away from the autonomous vehicle.
17. A speed planning apparatus for an autonomous vehicle, comprising:
one or more processors;
storage means for storing one or more programs;
the one or more programs, when executed by the one or more processors, cause the one or more processors to implement the method of any of claims 1-8.
18. A computer-readable storage medium, in which a computer program is stored which, when being executed by a processor, carries out the method according to any one of claims 1 to 8.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910135324.9A CN109814576B (en) | 2019-02-22 | 2019-02-22 | Method, apparatus and storage medium for speed planning of autonomous vehicles |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910135324.9A CN109814576B (en) | 2019-02-22 | 2019-02-22 | Method, apparatus and storage medium for speed planning of autonomous vehicles |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109814576A CN109814576A (en) | 2019-05-28 |
| CN109814576B true CN109814576B (en) | 2022-01-28 |
Family
ID=66607293
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910135324.9A Active CN109814576B (en) | 2019-02-22 | 2019-02-22 | Method, apparatus and storage medium for speed planning of autonomous vehicles |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109814576B (en) |
Families Citing this family (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111123952B (en) * | 2019-12-31 | 2021-12-31 | 华为技术有限公司 | Trajectory planning method and device |
| CN112526999B (en) * | 2020-12-22 | 2023-04-28 | 北京百度网讯科技有限公司 | Speed planning method, device, electronic equipment and storage medium |
| CN113071520B (en) * | 2021-04-16 | 2024-01-16 | 阿波罗智联(北京)科技有限公司 | Vehicle running control method and device |
| CN113654556A (en) * | 2021-07-05 | 2021-11-16 | 的卢技术有限公司 | Local path planning method, medium and equipment based on improved EM algorithm |
| CN113703453A (en) * | 2021-08-24 | 2021-11-26 | 京东鲲鹏(江苏)科技有限公司 | Method and device for determining driving track, electronic equipment and storage medium |
| CN113741453A (en) * | 2021-08-31 | 2021-12-03 | 广州文远知行科技有限公司 | Path planning method, device, equipment and medium for unstructured environment |
| CN114355921B (en) * | 2021-12-28 | 2022-10-18 | 北京易航远智科技有限公司 | Vehicle tracking track generation method and device, electronic equipment and storage medium |
| CN117601867A (en) * | 2024-01-18 | 2024-02-27 | 杭州鉴智机器人科技有限公司 | Vehicle lane changing method, vehicle lane changing device, storage medium and vehicle control system |
Citations (15)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS5990111A (en) * | 1982-11-16 | 1984-05-24 | Shinko Electric Co Ltd | Obstacle detector of unmanned guiding truck |
| WO2009123650A1 (en) * | 2008-04-02 | 2009-10-08 | Irobot Corporation | Robotics systems |
| JP2013045265A (en) * | 2011-08-24 | 2013-03-04 | Toyota Central R&D Labs Inc | Autonomous mobile body |
| CN104483967A (en) * | 2014-11-11 | 2015-04-01 | 浙江师范大学 | Wheeled mobile robot trace tracking control method based on energy saving consideration |
| EP3064967A1 (en) * | 2015-03-04 | 2016-09-07 | Sercel | Method for determining a collision free sail path of at least one vessel of a fleet of vessels, corresponding device, computer program product and non-transitory computer-readable carrier medium |
| CN106527452A (en) * | 2016-12-30 | 2017-03-22 | 广州汽车集团股份有限公司 | Motion path planning method of pilotless automobile when encountering obstacle and system thereof |
| WO2017054346A1 (en) * | 2015-09-28 | 2017-04-06 | 小米科技有限责任公司 | Method and apparatus for controlling balanced vehicle |
| CN107992050A (en) * | 2017-12-20 | 2018-05-04 | 广州汽车集团股份有限公司 | Pilotless automobile local path motion planning method and device |
| CN108334077A (en) * | 2017-01-18 | 2018-07-27 | 百度（美国）有限责任公司 | Determine the method and system of the unit gain of the speed control of automatic driving vehicle |
| CN108428243A (en) * | 2018-03-07 | 2018-08-21 | 北京航空航天大学 | A kind of pedestrian movement's speed predicting method based on artificial neural network |
| CN108780610A (en) * | 2016-03-24 | 2018-11-09 | 本田技研工业株式会社 | For the system and method for unexpected pedestrian's planned trajectory |
| CN108806021A (en) * | 2018-06-12 | 2018-11-13 | 重庆大学 | Electric vehicle target road section energy consumption prediction technique based on physical model and roadway characteristic parameter |
| CN109059944A (en) * | 2018-06-06 | 2018-12-21 | 上海国际汽车城（集团）有限公司 | Motion planning method based on driving habit study |
| CN109062227A (en) * | 2018-09-25 | 2018-12-21 | 电子科技大学 | A kind of more curve local paths planning methods based on dynamic window |
| CN109188419A (en) * | 2018-09-07 | 2019-01-11 | 百度在线网络技术（北京）有限公司 | Detection method, device, computer equipment and the storage medium of barrier speed |
Family Cites Families (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| DK178276B1 (en) * | 2014-12-19 | 2015-10-26 | Conpleks Innovation Aps | Method for recording and predicting position data for a selfpropelled wheeled vehicle, and delivery or pick up system comprising a self-propelled, self-guided wheeled vehicle |
| CN106114507B (en) * | 2016-06-21 | 2018-04-03 | 百度在线网络技术（北京）有限公司 | Local path planning method and device for intelligent vehicle |
| US11584372B2 (en) * | 2016-12-28 | 2023-02-21 | Baidu Usa Llc | Method to dynamically adjusting speed control rates of autonomous vehicles |
| CN108032858B (en) * | 2017-11-30 | 2020-06-12 | 广州小鹏汽车科技有限公司 | Adaptive cruise control method and system based on bypass driving path prediction |
| CN108519773B (en) * | 2018-03-07 | 2020-01-14 | 西安交通大学 | Path planning method for unmanned vehicle in structured environment |
-
2019
- 2019-02-22 CN CN201910135324.9A patent/CN109814576B/en active Active
Patent Citations (15)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS5990111A (en) * | 1982-11-16 | 1984-05-24 | Shinko Electric Co Ltd | Obstacle detector of unmanned guiding truck |
| WO2009123650A1 (en) * | 2008-04-02 | 2009-10-08 | Irobot Corporation | Robotics systems |
| JP2013045265A (en) * | 2011-08-24 | 2013-03-04 | Toyota Central R&D Labs Inc | Autonomous mobile body |
| CN104483967A (en) * | 2014-11-11 | 2015-04-01 | 浙江师范大学 | Wheeled mobile robot trace tracking control method based on energy saving consideration |
| EP3064967A1 (en) * | 2015-03-04 | 2016-09-07 | Sercel | Method for determining a collision free sail path of at least one vessel of a fleet of vessels, corresponding device, computer program product and non-transitory computer-readable carrier medium |
| WO2017054346A1 (en) * | 2015-09-28 | 2017-04-06 | 小米科技有限责任公司 | Method and apparatus for controlling balanced vehicle |
| CN108780610A (en) * | 2016-03-24 | 2018-11-09 | 本田技研工业株式会社 | For the system and method for unexpected pedestrian's planned trajectory |
| CN106527452A (en) * | 2016-12-30 | 2017-03-22 | 广州汽车集团股份有限公司 | Motion path planning method of pilotless automobile when encountering obstacle and system thereof |
| CN108334077A (en) * | 2017-01-18 | 2018-07-27 | 百度（美国）有限责任公司 | Determine the method and system of the unit gain of the speed control of automatic driving vehicle |
| CN107992050A (en) * | 2017-12-20 | 2018-05-04 | 广州汽车集团股份有限公司 | Pilotless automobile local path motion planning method and device |
| CN108428243A (en) * | 2018-03-07 | 2018-08-21 | 北京航空航天大学 | A kind of pedestrian movement's speed predicting method based on artificial neural network |
| CN109059944A (en) * | 2018-06-06 | 2018-12-21 | 上海国际汽车城（集团）有限公司 | Motion planning method based on driving habit study |
| CN108806021A (en) * | 2018-06-12 | 2018-11-13 | 重庆大学 | Electric vehicle target road section energy consumption prediction technique based on physical model and roadway characteristic parameter |
| CN109188419A (en) * | 2018-09-07 | 2019-01-11 | 百度在线网络技术（北京）有限公司 | Detection method, device, computer equipment and the storage medium of barrier speed |
| CN109062227A (en) * | 2018-09-25 | 2018-12-21 | 电子科技大学 | A kind of more curve local paths planning methods based on dynamic window |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109814576A (en) | 2019-05-28 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109814576B (en) | Method, apparatus and storage medium for speed planning of autonomous vehicles | |
| CN109712421B (en) | Method, apparatus and storage medium for speed planning of autonomous vehicles | |
| CN109583151B (en) | Method and device for predicting running track of vehicle | |
| US10982961B2 (en) | Vehicle control system and vehicle control device | |
| CN109760675B (en) | Method, device, storage medium and terminal equipment for predicting vehicle track | |
| JP6428928B2 (en) | Occlusion controller | |
| JP6468261B2 (en) | Automated driving system | |
| CN109814574B (en) | Method and device for planning speed of junction of automatic driving lanes and storage medium | |
| CN112639849A (en) | Route selection method and route selection device | |
| US20220234615A1 (en) | In-vehicle device and driving assist method | |
| CN109598816B (en) | Method, device, server and system for collecting behavior characteristics of automatic driving vehicle | |
| CN109655076B (en) | Vehicle turning speed planning method and device and storage medium | |
| US20220392276A1 (en) | Vehicle behavior evaluation device, vehicle behavior evaluation method, and vehicle behavior evaluation program product | |
| JP2012137999A (en) | Driving support device and driving support method | |
| CN102201174B (en) | Traveling road estimation system | |
| CN113895456A (en) | Intersection driving method and device for automatic driving vehicle, vehicle and medium | |
| US20220371620A1 (en) | Path planning device, path planning method, computer program product | |
| CN109724614B (en) | Method, apparatus and storage medium for speed planning of autonomous vehicles | |
| CN109855641B (en) | Method, device, storage medium and terminal equipment for predicting motion trail | |
| US20210197811A1 (en) | Course prediction device, computer readable medium, and course prediction method | |
| CN109887320B (en) | Speed planning method, device and storage medium | |
| US20220379918A1 (en) | Vehicle behavior generation device, vehicle behavior generation method, and vehicle behavior generation program product | |
| JP7006203B2 (en) | Trajectory setting device | |
| KR101980509B1 (en) | Inter-vehicle distance estimation method and inter-vehicle distance estimation device | |
| JP6971315B2 (en) | Information management device |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |