Path and speed planning with longitudinal and lateral motion coupling on arbitrary road profile for driving assistance and automated driving systems