Aggressive terrain following for motion-constrained vehicles in uncertain environments