A method of operation in a processor-based system to determine paths for vehicles via planning graphs, where each planning graph respectively comprises a plurality of nodes connected by edges, each node which represents, implicitly or explicitly, time and variables that characterize a state of a vehicle which operates in an environment and which environment includes one or more dynamic objects, and each edge represents a transition between a respective pair of nodes, the method comprising:
- receiving perception information that represents the environment in which the vehicle operates;
- performing a collision assessment on each of two or more of the edges of a planning graph;
- for each of two or more edges of the planning graph, setting a probability of collision based at least in part on the collision assessment;
- performing an optimization to identify a path in the resulting planning graph that provides a longest route of travel of the vehicle in two-dimensional or three-dimensional space as specified by the path with a relatively low potential of a collision with one or more dynamic objects in the environment in which the vehicle operates;
- and
- implementing a motion plan with the relatively low potential of a collision with one or more dynamic objects in the environment in which the vehicle operates based at least in part on the optimization.