Systems and Methods for Costing Autonomous Vehicle Maneuvers

Systems and methods for autonomous vehicle motion planning are provided. Sensor data describing an environment of an autonomous vehicle and an initial travel path for the autonomous vehicle through the environment can be obtained. A number of trajectories for the autonomous vehicle are generated based on the sensor data and the initial travel path. The trajectories can be evaluated by generating a number of costs for each trajectory. The costs can include a safety cost and a total cost. Each cost is generated by a cost function created in accordance with a number of relational propositions defining desired relationships between the number of costs. A subset of trajectories can be determined from the trajectories based on the safety cost and an optimal trajectory can be determined from the subset of trajectories based on the total cost. The autonomous vehicle can control its motion in accordance with the optimal trajectory.