Systems and methods for autonomous vehicle path follower correction

1. A method for generating a vehicle path to operate an autonomous vehicle, comprising:

receiving, by one or more processors, data indicative of a vehicle path and data indicative of an initial position from a localization system for determining a lateral reentry error and a longitudinal reentry error;

using, by the one or more processors, a lateral re-entry planner system for determining a trajectory subject to curvature and heading restraints for following the vehicle path based upon the lateral reentry error, to thereby generate a spatial plan using a first convex quadratic cost function;

using, by the one or more processors, a longitudinal re-entry planner system for determining a trajectory subject to acceleration, velocity, and travel restraints for following the vehicle path based upon the longitudinal reentry error, to thereby generate a temporal plan using a second convex quadratic cost function;

generating path correction commands based upon the determined trajectory of the lateral re-entry planner system and the determined trajectory of the longitudinal re-entry planner system, by fusing results from the spatial plan with results from the temporal plan, generating a series of points along a path of travel for the autonomous vehicle; and

transmitting the generated path correction commands for controlling the autonomous vehicle.

2. The method of claim 1 , wherein the determined trajectory of the lateral re-entry planner system and the determined trajectory of the longitudinal re-entry planner system are combined to generate a local plan for serving as a reference for control of the autonomous vehicle.

3. The method of claim 1 , wherein the trajectory determined by the lateral re-entry planner system corrects for the lateral reentry error by solving for a path reentry plan for placing the autonomous vehicle on a correct path plan.

4. The method of claim 1 , wherein the trajectory determined by the longitudinal re-entry planner system corrects for the longitudinal reentry error by solving for a path reentry plan for placing the autonomous vehicle on a correct path plan.

5. The method of claim 1 , wherein the generated path correction commands are transmitted to control steering, braking, and engine components of the autonomous vehicle.

6. The method of claim 1 , wherein the generating of the path correction commands comprises generating the path correction commands based upon the determined trajectory of the lateral re-entry planner system and the determined trajectory of the longitudinal re-entry planner system, by interpolating values of the spatial plan from the lateral re-entry planner system and the temporal plan from the longitudinal re-entry planner system at predetermined intervals of time and space.

7. The method of claim 1 , wherein each point along the path is being associated with a time, x position, y position, velocity, acceleration, curvature, and heading, and creates a trajectory that is used as a reference for travel for the autonomous vehicle.

8. The method of claim 1 , wherein the first convex quadratic cost function optimizes cost based on smoothness of travel.

9. The method of claim 1 , wherein the first convex quadratic cost function utilizes cross track error (CTE) constraints, including avoidance of lane boundaries, turn radius, steering wheel velocity, and steering wheel acceleration using previous values from the longitudinal re-entry planner system.

10. The method of claim 1 , wherein the first convex quadratic cost function utilizes a kinematic model with the following states: deviation from lane center; heading; curvature; and spatial jerk.

11. The method of claim 1 , wherein the second convex quadratic cost function optimizes cost based on speed limits, acceleration limits, and jerk limits.

12. A system for generating a vehicle path to operate an autonomous vehicle, comprising:

a storage device for storing instructions for generating the vehicle path; and

one or more data processors configured to execute the instructions to:

receive data indicative of a vehicle path and data indicative of an initial position from a localization system for determining a lateral reentry error and a longitudinal reentry error;

use a lateral re-entry planner system for determining a trajectory subject to curvature and heading restraints for following the vehicle path based upon the lateral reentry error, to thereby generate a spatial plan using a first convex quadratic cost function;

use a longitudinal re-entry planner system for determining trajectory subject to acceleration, velocity, and travel restraints for following the vehicle path based upon the longitudinal reentry error, to thereby generate a temporal plan using a second convex quadratic cost function;

generate path correction commands based upon the determined trajectory of the lateral re-entry planner system and the determined trajectory of the longitudinal re-entry planner system, by fusing results from the spatial plan with results from the temporal plan, generating a series of points along a path of travel for the autonomous vehicle; and

transmit the generated path correction commands for controlling the autonomous vehicle.

13. The system of claim 12 , wherein the determined trajectory of the lateral re-entry planner system and the determined trajectory of the longitudinal re-entry planner system are combined to generate a local plan for serving as a reference for control of the autonomous vehicle.

14. The system of claim 12 , wherein the trajectory determined by the lateral re-entry planner system corrects for the lateral reentry error by solving for a path reentry plan for placing the autonomous vehicle on a correct path plan.

15. The system of claim 12 , wherein the trajectory determined by the longitudinal re-entry planner system corrects for the longitudinal reentry error by solving for a path reentry plan for placing the autonomous vehicle on a correct path plan.

16. The system of claim 12 , wherein the generated path correction commands are transmitted to control steering, braking, and engine components of the autonomous vehicle.

17. An autonomous vehicle comprising:

a camera and lidar sensor that provides sensor data; and

a controller that, by a processor and based on the sensor data, is configured to:

receive data indicative of a vehicle path and data indicative of an initial position from a localization system for determining a lateral reentry error and a longitudinal reentry error;

use a lateral re-entry planner system for determining a trajectory subject to curvature and heading restraints for following the vehicle path based upon the lateral reentry error, to thereby generate a spatial plan using a first convex quadratic cost function;

use a longitudinal re-entry planner system for determining trajectory subject to acceleration, velocity, and travel restraints for following the vehicle path based upon the longitudinal reentry error, to thereby generate a temporal plan using a second convex quadratic cost function;

generate path correction commands based upon the determined trajectory of the lateral re-entry planner system and the determined trajectory of the longitudinal re-entry planner system, by fusing results from the spatial plan with results from the temporal plan, generating a series of points along a path of travel for the autonomous vehicle; and

transmit the generated path correction commands for controlling the autonomous vehicle.