A system for motion planning for an autonomous vehicle, the system comprising: a plurality of sensors operable to capture environmental information from an environment of the autonomous vehicle; a plurality of detectors (202; 203) in electrical communication with the plurality of sensors to receive sensor data therefrom, the detectors operable to determine whether there are any obstacles in the environment from the sensor data; wherein the plurality of detectors (202; 203) comprises an object detector operable to generate perception data comprising voxels of obstacles representing identified objects of the environment, wherein the plurality of detectors (202; 203) comprises an object tracker operable to identify a dynamic obstacle with uncertain movement in the environment and to output a probabilistic estimate comprising a probability distribution of future trajectories for the dynamic obstacle; a cost grid operable to receive the voxels of obstacles from the object detector, assign each voxel a cost value indicative of a cost of collision, and output the voxels and assigned cost values; and a motion planning module in electrical communication with the plurality of detectors (202; 203), and in electrical communication with a computing system of the autonomous vehicle, wherein the motion planning module (205; 300) is operable to store a planning graph comprising a plurality of nodes connected by edges, wherein each node represents, implicitly or explicitly, time and variables defining a state of the autonomous vehicle, an operating environment, or both the state of the autonomous vehicle and the operating environment, each edge represents a transition between respective states of the autonomous vehicle represented by a respective pair of nodes which the edge connects, and wherein the motion planning module (205; 300) is operable to receive the voxels and assigned cost values from the cost grid corresponding to detected obstacles and to sample over the distribution of trajectories from the probabilistic estimate and calculate a probability of collision for each edge in the planning graph, adjust a respective value to represent a cost of collision and probability of collision along each edge in the planning graph that results in a possible collision with obstacles in the perception data to account for the sample trajectories, determine a path for the autonomous vehicle considering the cost of collision and probability of collision, and output the path to the computing system. a plurality of sensors operable to capture environmental information from an environment of the autonomous vehicle; a plurality of detectors (202; 203) in electrical communication with the plurality of sensors to receive sensor data therefrom, the detectors operable to determine whether there are any obstacles in the environment from the sensor data; wherein the plurality of detectors (202; 203) comprises an object detector operable to generate perception data comprising voxels of obstacles representing identified objects of the environment, wherein the plurality of detectors (202; 203) comprises an object tracker operable to identify a dynamic obstacle with uncertain movement in the environment and to output a probabilistic estimate comprising a probability distribution of future trajectories for the dynamic obstacle; a cost grid operable to receive the voxels of obstacles from the object detector, assign each voxel a cost value indicative of a cost of collision, and output the voxels and assigned cost values; and a motion planning module in electrical communication with the plurality of detectors (202; 203), and in electrical communication with a computing system of the autonomous vehicle, wherein the motion planning module (205; 300) is operable to store a planning graph comprising a plurality of nodes connected by edges, wherein each node represents, implicitly or explicitly, time and variables defining a state of the autonomous vehicle, an operating environment, or both the state of the autonomous vehicle and the operating environment, each edge represents a transition between respective states of the autonomous vehicle represented by a respective pair of nodes which the edge connects, and wherein the motion planning module (205; 300) is operable to receive the voxels and assigned cost values from the cost grid corresponding to detected obstacles and to sample over the distribution of trajectories from the probabilistic estimate and calculate a probability of collision for each edge in the planning graph, adjust a respective value to represent a cost of collision and probability of collision along each edge in the planning graph that results in a possible collision with obstacles in the perception data to account for the sample trajectories, determine a path for the autonomous vehicle considering the cost of collision and probability of collision, and output the path to the computing system.