Trajectory planner

Abstract

An autonomous vehicle capable of trajectory prediction may include a first sensor, a second sensor, a processor, a trajectory planner, a low-level controller, and vehicle actuators. The first sensor may be of a first sensor type and may detect an obstacle and a goal. The second sensor may be of a second sensor type and may detect the obstacle and the goal. The processor may perform matching on the obstacle detected by the first sensor and the obstacle detected by the second sensor, model an existence probability of the obstacle based on the matching, and track the obstacle based on the existence probability and a constant velocity model. The trajectory planner may generate a trajectory for the autonomous vehicle based on the tracked obstacle, the goal, and a non-linear model predictive control (NMPC). The low-level controller may implement the trajectory for the autonomous vehicle by driving vehicle actuators.

The invention claimed is: 
     
       1. A system for trajectory prediction, comprising:
 a first sensor of a first sensor type detecting an obstacle and a goal; 
 a second sensor of a second sensor type detecting the obstacle and the goal, wherein the second sensor type is different than the first sensor type; 
 a processor performing matching on the obstacle detected by the first sensor and the obstacle detected by the second sensor, 
 wherein the processor models an existence probability of the obstacle based on the matching, and 
 wherein the processor tracks the obstacle based on the existence probability and a constant velocity model; 
 a trajectory planner generating a trajectory for an autonomous vehicle based on the tracked obstacle, the goal, and a non-linear model predictive control (NMPC); and 
 a low-level controller implementing the trajectory for the autonomous vehicle by driving one or more vehicle actuators, 
 wherein the matching is performed based on a cost function between the obstacle detected by the first sensor and the second sensor and the obstacle tracked by the processor, a cost of the cost function at least partially defined as a distance between a detection of each of the first sensor and the second sensor and the object, the cost function minimizing a time term for tracking the obstacle, facilitating a final position of the autonomous vehicle relative to the goal, and minimizing an area between the goal and the trajectory for the autonomous vehicle. 
 
     
     
       2. The system for trajectory prediction of  claim 1 , wherein the first sensor is an image capture sensor and the second sensor is a light detection and ranging (LiDAR) sensor. 
     
     
       3. The system for trajectory prediction of  claim 1 , wherein the matching is performed based on Hungarian matching. 
     
     
       4. The system for trajectory prediction of  claim 1 , wherein the existence probability is modeled based on a Bayesian function. 
     
     
       5. The system for trajectory prediction of  claim 1 , wherein the tracking is performed based on a Kalman filter. 
     
     
       6. The system for trajectory prediction of  claim 1 , wherein the NMPC is modeled based on a constant initial time, a variable final time, a state, a number of states, a control, a number of controls, a desired initial state vector, an initial state tolerance vector, a desired final state vector, a final state tolerance vector, arrays of slack variables on the initial states and terminal states, and arrays of weights. 
     
     
       7. The system for trajectory prediction of  claim 1 , wherein the cost function is calculated based on constant weight terms, vehicle position coordinates, goal coordinates, a number set associated with minimizing singularities, a desired final heading angle, a steering angle, a steering rate, a longitudinal speed, and a longitudinal acceleration. 
     
     
       8. The system for trajectory prediction of  claim 1 , wherein the low-level controller is a proportional-integral-derivative (PID) controller. 
     
     
       9. The system for trajectory prediction of  claim 1 , wherein the autonomous vehicle is a front-wheel-steered vehicle. 
     
     
       10. A method for trajectory prediction, comprising:
 detecting an obstacle and a goal using a first sensor of a first sensor type; 
 detecting the obstacle and the goal using a second sensor of a second sensor type, wherein the second sensor type is different than the first sensor type; 
 performing matching on the obstacle detected by the first sensor and the obstacle detected by the second sensor via a processor, 
 modeling an existence probability of the obstacle based on the matching via the processor, and 
 tracking the obstacle based on the existence probability and a constant velocity model via the processor; 
 generating a trajectory for an autonomous vehicle based on the tracked obstacle, the goal, and a non-linear model predictive control (NMPC) via a trajectory planner; and 
 implementing the trajectory for the autonomous vehicle by driving one or more vehicle actuators via a low-level controller, 
 wherein the matching is performed based on a cost function between the obstacle detected by the first sensor and the second sensor and the obstacle tracked by the processor, a cost of the cost function at least partially defined as a distance between a detection of each of the first sensor and the second sensor and the object, the cost function minimizing a time term for tracking the obstacle, facilitating a final position of the autonomous vehicle relative to the goal, and minimizing an area between the goal and the trajectory for the autonomous vehicle. 
 
     
     
       11. The method for trajectory prediction of  claim 10 , wherein the first sensor is an image capture sensor and the second sensor is a light detection and ranging (LiDAR) sensor. 
     
     
       12. The method for trajectory prediction of  claim 10 , wherein the matching is performed based on Hungarian matching. 
     
     
       13. The method for trajectory prediction of  claim 10 , wherein the existence probability is modeled based on a Bayesian function. 
     
     
       14. The method for trajectory prediction of  claim 10 , wherein the tracking is performed based on a Kalman filter. 
     
     
       15. The method for trajectory prediction of  claim 10 , wherein the NMPC is modeled based on a constant initial time, a variable final time, a state, a number of states, a control, a number of controls, a desired initial state vector, an initial state tolerance vector, a desired final state vector, a final state tolerance vector, arrays of slack variables on the initial states and terminal states, and arrays of weights. 
     
     
       16. The method for trajectory prediction of  claim 10 , wherein the cost function is calculated based on constant weight terms, vehicle position coordinates, goal coordinates, a number set associated with minimizing singularities, a desired final heading angle, a steering angle, a steering rate, a longitudinal speed, and a longitudinal acceleration. 
     
     
       17. The method for trajectory prediction of  claim 10 , wherein the low-level controller is a proportional-integral-derivative (PID) controller. 
     
     
       18. An autonomous vehicle capable of trajectory prediction, comprising:
 a first sensor of a first sensor type detecting an obstacle and a goal; 
 a second sensor of a second sensor type detecting the obstacle and the goal, wherein the second sensor type is different than the first sensor type; 
 a processor performing matching on the obstacle detected by the first sensor and the obstacle detected by the second sensor, 
 wherein the processor models an existence probability of the obstacle based on the matching, and 
 wherein the processor tracks the obstacle based on the existence probability and a constant velocity model; 
 a trajectory planner generating a trajectory for the autonomous vehicle based on the tracked obstacle, the goal, and a non-linear model predictive control (NMPC); and 
 a low-level controller implementing the trajectory for the autonomous vehicle by driving one or more vehicle actuators, 
 wherein the matching is performed based on a cost function between the obstacle detected by the first sensor and the second sensor and the obstacle tracked by the processor, a cost of the cost function at least partially defined as a distance between a detection of each of the first sensor and the second sensor and the object, the cost function minimizing a time term for tracking the obstacle, facilitating a final position of the autonomous vehicle relative to the goal, and minimizing an area between the goal and the trajectory for the autonomous vehicle.