1. A processor-implemented method for automated driving of a vehicle, comprising: receiving, by one or more data processors, vehicle state data, driving scenario plan, behavior control data and vehicle object environment data; generating, by the one or more data processors, a search graph based upon the received vehicle state data and driving scenario plan; wherein the search graph contains a grid of points for locating objects relative to a reference trajectory path; wherein the search graph uses a coordinate system that indicates lateral offsets of the objects relative to the reference trajectory path stations; and determining, by the one or more data processors, a trajectory path that is optimal with respect to a specific cost function; wherein the optimal trajectory path is provided to a controller in the vehicle for implementing the driving scenario plan.