Systems and methods for accelerated curve projection

1. A computer-implemented method of determining whether a trajectory of an obstacle to an autonomous driving vehicle (ADV) will require a navigation adjustment for the ADV, the method comprising:

determining a trajectory of an obstacle to an autonomous driving vehicle (ADV), the trajectory represented by a first ordered plurality of points {P 0 . . . P M };

determining a reference line of navigation for the ADV, the reference line represented by a second ordered plurality of points {R 0 . . . R N };

in response to determining that the obstacle and the ADV are traveling in substantially the same direction:

finding a first point S 0 ∈{R 0 . . . R N } of the reference line of the ADV such that the distance from a first point P 0 of the trajectory of the obstacle to S 0 is the minimum distance from P 0 to any of the points in {R 0 . . . R N } of the reference line of the ADV;

for each subsequent point P i ∈{P 1 . . . P M }, i=1 to M, of the trajectory of the obstacle, finding the point S i ∈{S i−1 . . . R N } on the reference line of the ADV such that the distance from P i to S i is the minimum distance from P i to any of the points in {S i−1 . . . R N } of the reference line of the ADV;

adjusting the navigation of the ADV in response to determining that the distance between P i and S i , for any i∈{0 . . . M} is less than a threshold distance value.

2. The method of claim 1 , further comprising:

in response to determining that the obstacle and the ADV are traveling in substantially opposing directions:

finding the point S M ∈{R 0 . . . R N } such that the distance from P M to S M is the minimum distance from P M to any of the points in {R 0 . . . R N };

for each point P i ∈{P 0 . . . P M−1 }, i=M−1 to 0, finding the point S i in {R0 to S i+1 } such that the distance from P i to S i is the minimum distance from P i to any point in {R 0 to S i+1 }.

3. The method of claim 1 , the direction of travel of the obstacle with reference to the ADV is determined by at least one of:

determining, using a high definition map, a direction of a roadway lane of the obstacle and a direction of a roadway lane of the ADV, and determining, using a perception module of the ADV, whether the obstacle is moving according to the direction of the roadway lane of the obstacle and whether the ADV is moving according to the direction of the roadway lane of the ADV; or

determining, using a perception module of the ADV, a heading of the obstacle, θ OBS , a heading of the ADV, θ ADV , and further determining that:

the obstacle and the ADV are traveling in the same direction, if the absolute value of θ ADV −θ OBS is less than a threshold heading value; and

the obstacle and the ADV are traveling in opposing directions, if the absolute value of θ ADV −θ OBS −180° is less than the threshold heading value.

4. The method of claim 3 , wherein the threshold heading value is between 30° and 40°.

5. The method of claim 1 , wherein M=100 and N=1000.

6. The method of claim 1 , further comprising:

interpolating

(

N

M

-

1

)

points between each P i P i+1 in {P 0 . . . P M }, for i=1 to M−1;

setting M=N as an iteration counter for the obstacle trajectory line, such that finding S i for each P i comprises: for each P i ∈{P 1 . . . P N }, i=1 to N, finding the point S i ∈{S i−1 . . . R N } such that the distance from P i to S i is the minimum distance from P i to any of the points in {S i−1 . . . R N }.

7. A non-transitory machine-readable medium having instructions stored therein, which when executed by a processor, cause the processor to perform operations, the operations comprising:

determining a trajectory of an obstacle to an autonomous driving vehicle (ADV), the trajectory represented by a first ordered plurality of points {P 0 . . . P M };

determining a reference line of navigation for the ADV, the reference line represented by a second ordered plurality of points {R 0 . . . R N };

in response to determining that the obstacle and the ADV are traveling in substantially the same direction:

finding a first point S 0 ∈{R 0 . . . R N } of the reference line of the ADV such that the distance from a first point P 0 of the trajectory of the obstacle to S 0 is the minimum distance from P 0 to any of the points in {R 0 . . . R N } of the reference line of the ADV;

for each subsequent point P i ∈{P 1 . . . P M }, i=1 to M, of the trajectory of the obstacle, finding the point S i ∈{S i−1 . . . R N } of the reference line of the ADV such that the distance from P i to S i is the minimum distance from P i to any of the points in {S i−1 . . . R N } of the reference line of the ADV;

adjusting the navigation of the ADV in response to determining that the distance between P i and S i , for any i∈{0 . . . M} is less than a threshold distance value.

8. The medium of claim 7 , the operations further comprising:

in response to determining that the obstacle and the ADV are traveling in substantially opposing directions:

finding the point S M ∈{R 0 . . . R N } such that the distance from P M to S M is the minimum distance from P M to any of the points in {R 0 . . . R N };

for each point P i ∈{P 0 . . . P M−1 }, i=M−1 to 0, finding the point S i in {R0 to S i+1 } such that the distance from P i to S i is the minimum distance from P i to any point in {R 0 to S i+1 }.

9. The medium of claim 7 , the direction of travel of the obstacle with reference to the ADV is determined by at least one of:

determining, using a high definition map, a direction of a roadway lane of the obstacle and a direction of a roadway lane of the ADV, and determining, using a perception module of the ADV, whether the obstacle is moving according to the direction of the roadway lane of the obstacle and whether the ADV is moving according to the direction of the roadway lane of the ADV; or

determining, using a perception module of the ADV, a heading of the obstacle, θ OBS , a heading of the ADV, θ ADV , and further determining that:

the obstacle and the ADV are traveling in the same direction, if the absolute value of θ ADV −θ OBS is less than a threshold heading value; and

the obstacle and the ADV are traveling in opposing directions, if the absolute value of θ ADV −θ OBS −180° is less than the threshold heading value.

10. The medium of claim 9 , wherein the threshold heading value is between 30° and 40°.

11. The medium of claim 7 , wherein M=100 and N=1000.

12. The medium of claim 7 , the operations further comprising:

interpolating

(

N

M

-

1

)

points between each P i P i+1 in {P 0 . . . P M }, for i=1 to M−1;

setting M=N as an iteration counter for the obstacle trajectory line, such that finding S i for each P i comprises: for each P i ∈{P 1 . . . P N }, i=1 to N, finding the point S i ∈{S i−1 . . . R N } such that the distance from P i to S i is the minimum distance from P i to any of the points in {S i−1 . . . R N }.

13. A data processing system, comprising:

a processor; and

a memory coupled to the processor to store instructions, which when executed by the processor, cause the processor to perform operations, the operations including:

determining a trajectory of an obstacle to an autonomous driving vehicle (ADV), the trajectory represented by a first ordered plurality of points {P 0 . . . P M };

determining a reference line of navigation for the ADV, the reference line represented by a second ordered plurality of points {R 0 . . . R N };

in response to determining that the obstacle and the ADV are traveling in substantially the same direction:

finding a first point S 0 ∈{R 0 . . . R N } of the reference line of the ADV such that the distance from a first point P 0 of the trajectory of the obstacle to S 0 is the minimum distance from P 0 to any of the points in {R 0 . . . R N } of the reference line of the ADV;

for each subsequent point P i ∈{P 1 . . . P M }, i=1 to M, of the trajectory of the obstacle, finding the point S i ∈{S i−1 . . . R N } of the reference line of the ADV such that the distance from P i to S i is the minimum distance from P i to any of the points in {S i−1 . . . R N } of the reference line of the ADV;

adjusting the navigation of the ADV in response to determining that the distance between P i and S i , for any i∈{0 . . . M} is less than a threshold distance value.

14. The system of claim 13 , the operations further comprising:

in response to determining that the obstacle and the ADV are traveling in substantially opposing directions:

finding the point S M ∈{R 0 . . . R N } such that the distance from P M to S M is the minimum distance from P M to any of the points in {R 0 . . . R N };

for each point P i ∈{P 0 . . . P M−1 }, i=M−1 to 0, finding the point S i in {R0 to S i+1 } such that the distance from P i to S i is the minimum distance from P i to any point in {R 0 to S i+1 }.

15. The system of claim 13 , the direction of travel of the obstacle with reference to the ADV is determined by at least one of:

determining, using a high definition map, a direction of a roadway lane of the obstacle and a direction of a roadway lane of the ADV, and determining, using a perception module of the ADV, whether the obstacle is moving according to the direction of the roadway lane of the obstacle and whether the ADV is moving according to the direction of the roadway lane of the ADV; or

determining, using a perception module of the ADV, a heading of the obstacle, θ OBS , a heading of the ADV, θ ADV , and further determining that:

the obstacle and the ADV are traveling in the same direction, if the absolute value of θ ADV −θ OBS is less than a threshold heading value; and

the obstacle and the ADV are traveling in opposing directions, if the absolute value of θ ADV −θ OBS −180° is less than the threshold heading value.

16. The system of claim 15 , wherein the threshold heading value is between 30° and 40°.

17. The system of claim 13 , wherein M=100 and N=1000.

18. The system of claim 13 , the operations further comprising:

interpolating

(

N

M

-

1

)

points between each P i P i+1 in {P 0 . . . P M }, for i=1 to M−1;

setting M=N as an iteration counter for the obstacle trajectory line, such that finding S i for each P i comprises: for each P i ∈{P 1 . . . P N }, i=1 to N, finding the point S i ∈{S i−1 . . . R N } such that the distance from P i to S i is the minimum distance from P i to any of the points in {S i−1 . . . R N }.