SYSTEMS, METHODS, AND COMPUTER-READABLE MEDIA FOR SPATIO-TEMPORAL MOTION PLANNING

Systems, methods and computer-readable media for spatio-temporal motion planning, including: receiving data defining a drivable area within a spatio-temporal space; generating a cost function to be minimized that includes one or more upper-bound cost terms, each upper-bound cost term approximating the upper-bound of a cost term for the trajectory for a spatial frame of the spatio-temporal space, each upper-bound cost term being expressed as one or more longitudinal-temporal cost terms and one or more lateral-temporal cost terms for the trajectory; and further cost terms for the trajectory for the spatio-temporal frames; computing, based on the constraints and cost function, a planned trajectory through the drivable area, the planned trajectory directory being optimized with respect to both spatial frame and the spatio-temporal frames. Optimizing the planned trajectory in the spatial frame in addition to the spatio-temporal frames can enable planning objectives for the spatial frame to be optimized, for example maintaining closeness to a reference path and minimizing lateral acceleration and directional oscillation.