Hierarchical Optimal Maneuver Planning and Trajectory Control at On-Ramps With Multiple Mainstream Lanes