Optimization-based Safe Trajectory Planning for Autonomous Ground Vehicle in Multi-Floor Scenarios