Robust navigation control and headland turning optimization of agricultural vehicles