Ich habe eine Lösung gefunden: Ich berechne die Geschwindigkeiten, die gefahren werden müssen, um von der aktuellen Position zum nächsten interpolierten Punkt zu kommen und gib sie auf die Antriebe. Das Ergebnis ist relativ gut.