I think just adding a simple low-pass filter on your distance measurements and tracking the derivative for PD control would cure that problem. Try it!