MPC-based framework for autonomous ground vehicles in a complex environment