SYSTEMS AND METHODS FOR PLANNING AND UPDATING A VEHICLE'S TRAJECTORY
Techniques are provided for generating a driving trajectory for a vehicle while the vehicle is blocked (e.g., the sensor of the vehicle have detected an object results in the vehicle being unable to move) by an object (e.g., another vehicle, bicycle, or a pedestrian) and executing the driving trajectory when the vehicle becomes unblocked. In addition, techniques are provided for updating a portion of a driving trajectory of a vehicle based on a determination that an object will cross a segment of the current driving trajectory at a later point, without recalculating the whole trajectory.
1 . A method comprising:
generating, with at least one processor, one or more operational commands for a vehicle, wherein the one or more operational commands are associated with segments of a trajectory of the vehicle;
receiving, with the at least one processor, sensor data, the sensor data comprising locations and velocities of detected objects;
determining, with the at least one processor and based on the sensor data, that an object of the detected objects and the vehicle are projected to be proximate to a particular segment of the trajectory of the vehicle at a particular time;
based on the determining, identifying a portion of the one or more operational commands associated with the particular segment of the trajectory of the vehicle and updating the identified portion of the one or more operational commands; and
executing, with a control circuit of the vehicle, the one or more operational commands.
2 . The method of claim 1 , wherein determining that the object of the detected objects and the vehicle are projected to be proximate to the segment of the trajectory of the vehicle at the particular time, further comprises:
generating, based on the velocity and location of the object of the detected objects, a trajectory of the object;
determining, based on the trajectory of the vehicle and the trajectory of the object, whether the object and the vehicle will be proximate to each other at the particular time; and
responsive to determining that the object and the vehicle will be proximate to each other at the particular time, identifying a segment of the trajectory of the vehicle where the trajectory will be proximate to the object.
3 . The method of claim 1 , wherein determining, based on the trajectory of the vehicle and the trajectory of the object, whether the object and the vehicle will be proximate to each other at the particular time further comprises:
identifying, at the particular time, a segment of the trajectory of the object closest to the segment of the trajectory of the vehicle;
determining whether a distance between the segment of the trajectory of the object and the segment of the trajectory of the vehicle is below a threshold; and
responsive to determining that the distance is below a threshold, determining that the vehicle will be proximate to the object at the particular time.
4 . The method of claim 3 , further comprising:
identifying a portion of the one or more operational commands associated with the segment of the trajectory of the vehicle; and
updating the identified portion of the one or more operational commands.
5 . The method of claim 4 , wherein the one or more operational commands are stored in a data structure corresponding to the segment of the trajectory of the vehicle.
6 . The method of claim 4 , wherein the one or more operational commands are updated to not traverse the segment but rather to traverse one or more other segments of the trajectory of the vehicle.
7 . The method of claim 6 , wherein the one or more other segments surround the updated segment.
8 . The method of claim 1 , wherein prior to updating the one or more operational commands, the method further comprises:
modifying the speed of the vehicle to a specified magnitude;
receiving updated sensor data, where the updated sensor data includes updated locations and updated velocities of detected objects; and
based on the updated sensor data, generating a corresponding probability for each object, wherein each probability indicates how likely a corresponding object is to be proximate to the trajectory of the vehicle.
9 . The method of claim 8 , wherein the probability decreases as a time interval between the vehicle and the object being at a proximate location increases.
10 . The method of claim 8 , wherein the probability decreases as a distance between the location of the vehicle and the location of the object increases.
11 . A vehicle comprising:
one or more processors; and
a non-transitory computer-readable storage medium storing one or more programs for execution by the one or more processors, the one or more programs including instructions which, when executed by the one or more processors, cause the one or more processors to:
generate one or more operational commands for a vehicle, wherein the one or more operational commands are associated with segments of a trajectory of the vehicle;
receive sensor data, the sensor data comprising locations and velocities of detected objects;
determine, based on the sensor data, that an object of the detected objects and the vehicle are projected to be proximate to a particular segment of the trajectory of the vehicle at a particular time;
based on the determining, identify a portion of the one or more operational commands associated with the particular segment of the trajectory of the vehicle and updating the identified portion of the one or more operational commands; and
execute the one or more operational commands.
12 . The vehicle of claim 11 , wherein determining that the object of the detected objects and the vehicle are projected to be proximate to the segment of the trajectory of the vehicle at the particular time, wherein the operations further comprise:
generating, based on the velocity and location of the object of the detected objects, a trajectory of the object;
determining, based on the trajectory of the vehicle and the trajectory of the object, whether the object and the vehicle will be proximate to each other at the particular time; and
responsive to determining that the object and the vehicle will be proximate to each other at the particular time, identifying a segment of the trajectory of the vehicle where the trajectory will be proximate to the object.
13 . The vehicle of claim 11 , wherein determining, based on the trajectory of the vehicle and the trajectory of the object, whether the object and the vehicle will be proximate to each other at the particular time further comprises:
identifying, at the particular time, a segment of the trajectory of the object closest to the segment of the trajectory of the vehicle;
determining whether a distance between the segment of the trajectory of the object and the segment of the trajectory of the vehicle is below a threshold; and
responsive to determining that the distance is below a threshold, determining that the vehicle will be proximate to the object at the particular time.
14 . The vehicle of claim 13 , further comprising:
identifying a portion of the one or more operational commands associated with the segment of the trajectory of the vehicle; and
updating the identified portion of the one or more operational commands.
15 . The vehicle of claim 14 , wherein the one or more operational commands are stored in a data structure corresponding to the segment of the trajectory of the vehicle.
16 . The vehicle of claim 14 , wherein the one or more operational commands are updated to not traverse the segment but rather to traverse one or more other segments of the trajectory of the vehicle.
17 . The vehicle of claim 16 , wherein the one or more other segments surround the updated segment.
18 . The vehicle of claim 11 , wherein prior to updating the one or more operational commands, the operations further comprise:
modifying the speed of the vehicle to a specified magnitude;
receiving updated sensor data, where the updated sensor data includes updated locations and updated velocities of detected objects; and
based on the updated sensor data, generating a corresponding probability for each object, wherein each probability indicates how likely a corresponding object is to be proximate to the trajectory of the vehicle.
19 . The vehicle of claim 18 , wherein the probability decreases as a time interval between the vehicle and the object being at a proximate location increases.
20 . The vehicle of claim 18 , wherein the probability decreases as a distance between the location of the vehicle and the location of the object increases.