System and Method for Controlling Movement of a Vehicle

A control system for controlling movement of a vehicle, is disclosed. The control system is configured to construct a graph having multiple nodes defining states of the vehicle. The multiple nodes comprise an initial node defining initial state and a target node defining target state. Each pair of nodes is connected by an edge defined by collision-free motion primitives where the nodes comprise motion cusps. Multiple nodes connected through edges form a first path. A first number of motion cusps in the first path is determined and the graph is expanded to add new nodes until a termination condition is met on determining that the first number of motion cusps is above a threshold. The expansion of the graph is subjected to a constraint associated with a total number of motion cusps. Further a second path is determined having lesser motion cusps than the first path.