Patent attributes
A system and method are provided and include a subject vehicle having vehicle actuation systems and vehicle sensors. A planning system includes a global route planner module, an inference module, a motion planner module, and a trajectory follower module. The inference module receives a route from the global route planner module and dynamic obstacles data from a perception system and determines a total cost for different sets of motions associated with different trajectories for traveling along the received route. The total cost includes an inferred cost based on a probability of the associated set of motions having an increased or decreased cost based on the dynamic obstacles data. The motion planner selects a particular set of motions based on the total costs and generates a smooth trajectory for the vehicle. The trajectory follower module controls the vehicle actuation systems based on the smooth trajectory.
