System and method for autonomous motion planning

1. A motion planning system comprising:

a processor; and

memory configured to store instructions that when executed by the processor, cause the processor to:

identify a reference path between a departure point and a destination point in an environment comprising one or more obstacles;

generate decomposition segments of a space surrounding the reference path by casting a ray orthogonal to the reference path from a vertex of a perimeter of each of the one or more obstacles to a boundary, the decomposition segments comprising a first free-space segment and a second free-space segment that are devoid of the obstacles;

generate a first path segment relative to the reference path for traversing the first free-space segment, and a second path segment relative to the reference path for traversing the second free-space segment; and

connect the first and second path segments to each other to generate a navigational path to traverse the environment.

2. The system of claim 1 , wherein the first and second free-space segments are orthogonal to the reference path.

3. The system of claim 1 , wherein the instructions further cause the processor to:

convert a Euclidean coordinate system corresponding to the space of the environment to a curvilinear coordinate system corresponding to the first and second free-space segments.

4. The system of claim 1 , wherein the instructions further cause the processor to:

order the first and second free-space segments to generate a directed acyclic graph (DAG),

wherein the first and second free-space segments correspond to nodes of the DAG, and the ray corresponds to an edge of the DAG connecting the nodes.

5. The system of claim 1 , wherein:

the ray comprises a first ray corresponding to a first vertex of the one or more obstacles, a second ray corresponding to a second vertex of the one or more obstacles, and a third ray corresponding to a third vertex of the one or more obstacles;

the first free-space segment is bounded between the first ray and the second ray; and

the second free-space segment is bounded between the second ray and the third ray.

6. The system of claim 5 , wherein to generate the first and second path segments, the instructions further cause the processor to:

generate a first discrete sample point-heading pair along the first ray, a second discrete sample point-heading pair along the second ray, and a third discrete sample point heading pair along the third ray;

connect the first discrete sample point-heading pair to the second discrete sample point-heading pair with a first curve; and

connect the second discrete sample point-heading pair to the third discrete sample point heading pair with a second curve.

7. The system of claim 6 , wherein to generate the first and second path segments, the instructions further cause the processor to:

calculate a cost of the first curve relative to the reference path;

calculate a cost of the second curve relative to the reference path;

determine that the first curve and the second curve have minimum costs relative to the reference path; and

set the first curve as the first path segment and the second curve as the second path segment.

8. The system of claim 5 , wherein to generate the first and second path segments, the instructions further cause the processor to:

generate a continuous cost value function for traversing between the first ray and the second ray;

connect a point-heading pair along the first ray to a point-heading pair along the second ray with a first curve;

generate a continuous cost value function for traversing between the second ray and the third ray; and

connect a point-heading pair along the second ray to a point-heading pair along the third ray with a second curve.

9. The system of claim 8 , wherein to generate the first and second path segments, the instructions further cause the processor to:

evaluate a cost of the first curve by minimizing the continuous value function for traversing between the first ray and the second ray relative to the reference path;

evaluate a cost of the second curve by minimizing the continuous value function for traversing between the second ray and the third ray relative to the reference path; and

determine the first path segment and the second path segment based on the minimizations.

10. An autonomous motion planning method comprising:

identifying, by a processor, a reference path between a departure point and a destination point in an environment comprising one or more obstacles;

generating, by the processor, decomposition segments of a space surrounding the reference path by casting a ray orthogonal to the reference path from a vertex of a perimeter of each of the one or more obstacles to a boundary, the decomposition segments comprising a first free-space segment and a second free-space segment that are devoid of the obstacles;

generating, by the processor, a first path segment relative to the reference path for traversing the first free-space segment, and a second path segment relative to the reference path for traversing the second free-space segment; and

connecting, by the processor, the first and second path segments to each other to generate a navigational path to traverse the environment.

11. The method of claim 10 , wherein the first and second free-space segments are orthogonal to the reference path.

12. The method of claim 10 , wherein the method further comprises:

converting, by the processor, a Euclidean coordinate system corresponding to the space of the environment to a curvilinear coordinate system corresponding to the first and second free-space segments.

13. The method of claim 10 , wherein the method further comprises:

ordering, by the processor, the first and second free-space segments to generate a directed acyclic graph (DAG),

wherein the first and second free-space segments correspond to nodes of the DAG, and the ray corresponds to an edge of the DAG connecting the nodes.

14. The method of claim 10 , wherein:

the ray comprises a first ray corresponding to a first vertex of the one or more obstacles, a second ray corresponding to a second vertex of the one or more obstacles, and a third ray corresponding to a third vertex of the one or more obstacles;

the first free-space segment is bounded between the first ray and the second ray; and

the second free-space segment is bounded between the second ray and the third ray.

15. The method of claim 14 , wherein to generate the first and second path segments, the method further comprises:

generating, by the processor, a first discrete sample point-heading pair along the first ray, a second discrete sample point-heading pair along the second ray, and a third discrete sample point heading pair along the third ray;

connecting, by the processor, the first discrete sample point-heading pair to the second discrete sample point-heading pair with a first curve; and

connecting, by the processor, the second discrete sample point-heading pair to the third discrete sample point heading pair with a second curve.

16. The method of claim 15 , wherein to generate the first and second path segments, the method further comprises:

calculating, by the processor, a cost of the first curve relative to the reference path;

calculating, by the processor, a cost of the second curve relative to the reference path;

determining, by the processor, that the first curve and the second curve have minimum costs relative to the reference path; and

setting, by the processor, the first curve as the first path segment and the second curve as the second path segment.

17. The method of claim 14 , wherein to generate the first and second path segments, the method further comprises:

generating, by the processor, a continuous cost value function for traversing between the first ray and the second ray;

connecting, by the processor, a point-heading pair along the first ray to a point-heading pair along the second ray with a first curve;

generating, by the processor, a continuous cost value function for traversing between the second ray and the third ray; and

connecting, by the processor, a point-heading pair along the second ray to a point-heading pair along the third ray with a second curve.

18. The method of claim 17 , wherein to generate the first and second path segments, the method further comprises:

evaluating, by the processor, a cost of the first curve by minimizing the continuous value function for traversing between the first ray and the second ray relative to the reference path;

evaluating, by the processor, a cost of the second curve by minimizing the continuous value function for traversing between the second ray and the third ray relative to the reference path; and

determining, by the processor, the first path segment and the second path segment based on the minimizations.