Schonmal probiert was passiert wenn de an deinen Roboter einfach oben hin n Besenstiel schraubst? Soll in Ruheposition einfach gerade nach oben zeigen.
Sieht nicht schön aus, aber für Versuchszwecke möglich.

Wenns dann funktioniert is deine Konstruktion zu instabil. Mehr Gewicht und ein höherer Schwerpunkt (bei deinen Roboterausmaßen is n Besenstiel sicher nicht nötig, da tuts auch was kleineres. war nur n Beispiel) sollten da schon viel helfen.

Ansonsten bleibt die Frage wie sehr er in eine Richtung kippt bei vorgegebener Geschwindigkeit. Bzw hast du mal versucht als Zielgeschwindigkeit nicht P*x sondern einen niedrigeren Wert zu nehmen (edit: gemeint ist hier: kleinerer Wert für P als den momentan verwendeten)?

MfG Daniel