Motion controller for real-time continuous curvature path planning

1. A system for controlling a motion of a vehicle from an initial state to a target state, comprising:

a path planner to determine a discontinuous curvature path connecting the initial state with the target state by a sequential composition of driving patterns, wherein the discontinuous curvature path is collision-free within a tolerance envelope centered on the discontinuous curvature path;

a path transformer to locate and replace at least one treatable primitive in the discontinuous curvature path with a corresponding continuous curvature segment to form a modified path remaining within the tolerance envelope, wherein each treatable primitive is a predetermined pattern of elementary paths, wherein the treatable primitive includes at least one of a sequence of a straight-line segment connected to a first arc segment connected to a second arc segment, and a sequence of a first arc segment connected to a second arc segment connected to a straight-line segment, and wherein the path transformer replace the treatable primitive with a pair of clothoid turns; and

a controller to control the motion of the vehicle according to the modified path.

2. The system of claim 1 , wherein the path planner is a Reeds-Shepp (RS) path planner forming the discontinuous curvature path using a combination of a straight-line segment and a circular arc tangentially connected at discontinuous curvature junctions, further comprising:

a converter to convert the sequential composition of driving patterns into a sequence of treatable primitives.

3. The system of claim 2 , wherein the converter converts each RS driving pattern independently from each other.

4. The system of claim 2 , wherein the converter inserts a new elementary path in the sequential composition of RS driving patterns or splits at least one existing elementary path in the sequential composition of RS driving patterns to modify the sequential composition and partition the modified sequential composition of RS driving patterns into a sequence of treatable primitives.

5. The system of claim 2 , wherein the converter iteratively tests each tuple of elementary paths forming the discontinuous curvature path for being a treatable primitive and inserts a new straight-line segment staying inside the envelope of the discontinuous curvature path into a tuple not representing the treatable primitive.

6. The system of claim 5 , wherein the converter inserts the new straight-line segment in the front of the tuple or after the second elementary path of the tuple.

7. The system of claim 1 , wherein the path planner imposes a constraint on the discontinuous curvature path requiring at least one of an initial and a final segment of the discontinuous curvature path being a straight-line segment, such that the discontinuous curvature path is a sequence of the treatable primitives.

8. The system of claim 1 , wherein the path transformer replaces a sequence of an even number of circular arc segments connected to the straight-line segment with a sequence of the even number of clothoid turns.

9. The system of claim 8 , further comprising:

a converter to split a circular arc of the discontinuous curvature path to convert a sequence of an odd number of circular arc segments into the sequence of the even number of circular arc segments.

10. The system of claim 1 , wherein the path transformer determines parameters of the continuous curvature segment to satisfy a μ-tangency constraint and remain in the envelope of the discontinuous curvature path.

11. A method for controlling a motion of a vehicle from an initial state to a target state, wherein the method uses a processor coupled with stored instructions implementing the method, wherein the instructions, when executed by the processor carry out at least some steps of the method, comprising:

determining a discontinuous curvature path connecting the initial state with the target state by a sequential composition of driving patterns, wherein the discontinuous curvature path is collision-free within a tolerance envelope centered on the discontinuous curvature path, wherein the determining imposes a constraint on the discontinuous curvature path requiring at least one of an initial and a final segment of the discontinuous curvature path being a straight-line segment, such that the discontinuous curvature path is a sequence of the treatable primitives;

locating and replacing at least one treatable primitive in the discontinuous curvature path with a corresponding continuous curvature segment to form a modified path remaining within the tolerance envelope, wherein each treatable primitive is a predetermined pattern of elementary paths; and

controlling the motion of the vehicle according to the modified path.

12. The method of claim 11 , wherein the path planner is a Reeds-Shepp (RS) path planner forming the discontinuous curvature path using a combination of a straight-line segment and a circular arc tangentially connected at discontinuous curvature junctions, further comprising:

converting the sequential composition of driving patterns into a sequence of treatable primitives.

13. A non-transitory computer readable storage medium embodied thereon a program executable by a processor for performing a method, the method comprising:

determining a discontinuous curvature path connecting the initial state with the target state by a sequential composition of driving patterns, wherein the discontinuous curvature path is collision-free within a tolerance envelope centered on the discontinuous curvature path, wherein the determining imposes a constraint on the discontinuous curvature path requiring at least one of an initial and a final segment of the discontinuous curvature path being a straight-line segment, such that the discontinuous curvature path is a sequence of the treatable primitives;

locating and replacing at least one treatable primitive in the discontinuous curvature path with a corresponding continuous curvature segment to form a modified path remaining within the tolerance envelope, wherein each treatable primitive is a predetermined pattern of elementary paths; and

controlling the motion of the vehicle according to the modified path.

14. A system for controlling a motion of a vehicle from an initial state to a target state, comprising:

a path planner to determine a discontinuous curvature path connecting the initial state with the target state by a sequential composition of driving patterns, wherein the discontinuous curvature path is collision-free within a tolerance envelope centered on the discontinuous curvature path, wherein the path planner is a Reeds-Shepp (RS) path planner forming the discontinuous curvature path using a combination of a straight-line segment and a circular arc tangentially connected at discontinuous curvature junctions;

a converter to convert the sequential composition of driving patterns into a sequence of treatable primitives, wherein the converter converts each RS driving pattern independently from each other;

a path transformer to locate and replace at least one treatable primitive in the discontinuous curvature path with a corresponding continuous curvature segment to form a modified path remaining within the tolerance envelope, wherein each treatable primitive is a predetermined pattern of elementary paths; and

a controller to control the motion of the vehicle according to the modified path.

15. A system for controlling a motion of a vehicle from an initial state to a target state, comprising:

a path planner to determine a discontinuous curvature path connecting the initial state with the target state by a sequential composition of driving patterns, wherein the discontinuous curvature path is collision-free within a tolerance envelope centered on the discontinuous curvature path, wherein the path planner is a Reeds-Shepp (RS) path planner forming the discontinuous curvature path using a combination of a straight-line segment and a circular arc tangentially connected at discontinuous curvature junctions;

a converter to convert the sequential composition of driving patterns into a sequence of treatable primitives, wherein the converter inserts a new elementary path in the sequential composition of RS driving patterns or splits at least one existing elementary path in the sequential composition of RS driving patterns to modify the sequential composition and partition the modified sequential composition of RS driving patterns into a sequence of treatable primitives;

a path transformer to locate and replace at least one treatable primitive in the discontinuous curvature path with a corresponding continuous curvature segment to form a modified path remaining within the tolerance envelope, wherein each treatable primitive is a predetermined pattern of elementary paths; and

a controller to control the motion of the vehicle according to the modified path.

16. A system for controlling a motion of a vehicle from an initial state to a target state, comprising:

a path planner to determine a discontinuous curvature path connecting the initial state with the target state by a sequential composition of driving patterns, wherein the discontinuous curvature path is collision-free within a tolerance envelope centered on the discontinuous curvature path, wherein the path planner is a Reeds-Shepp (RS) path planner forming the discontinuous curvature path using a combination of a straight-line segment and a circular arc tangentially connected at discontinuous curvature junctions;

a converter to convert the sequential composition of driving patterns into a sequence of treatable primitives, wherein the converter iteratively tests each tuple of elementary paths forming the discontinuous curvature path for being a treatable primitive and inserts a new straight-line segment staying inside the envelope of the discontinuous curvature path into a tuple not representing the treatable primitive;

a path transformer to locate and replace at least one treatable primitive in the discontinuous curvature path with a corresponding continuous curvature segment to form a modified path remaining within the tolerance envelope, wherein each treatable primitive is a predetermined pattern of elementary paths; and

a controller to control the motion of the vehicle according to the modified path.