Hallo Devil!

Deine Idee mit der Vektorrechnung ist schon recht solide. Rotationsmatrizen sind auch nur eine Form von Vektorrechnung und die machen nichts, was du nicht auch durch Addition von einzelnen Vektoren erreichen kannst.
Das System hat mit einem Arm 5 Freiheitsgrade. Das heißt, dass der Punkt, an dem die Hand des Roboterarms gerade ist, eine Funktion von 5 Variablen ist, die auch eine Zeitableitung hat (bzw. 5 partielle) (welche wichtig sind, wenn du den schnellsten Weg haben willst).
Anfangs würde ich das Problem auf "Finde überhaupt den Punkt" reduzieren. Dazu das Problem erstmal unterteilen in Winkeländerung und Positions- und Winkelveränderung. Bei erstem drehst du einfach an den zuständigen Motoren. Beim zweiten reduzierst du das Problem auf eine Winkeländerung indem du die Hand an ihre neue Position fährst, was ja das eigentliche Problem ist. Das könnte man anfangs machen, indem man erstmal überprüft ob man schon durch Veränderung einzelner Freiheitsgrade sein Ziel erreicht. Wenn nicht (was wahrscheinlich ist), dann schiebt man die Hand an einen Punkt, der auf einer Linie wäre mit dem Bewegungsbogen der Hand, wenn man einen anderen Freiheitsgrad verändert, um dann hinterher genau diesen so zu verändern...
Also es ist ganz praktisch das Problem in viele kleinere Probleme zu zerlegen. So kommt man u.U. auch auf die eine Wunderformel....

Hoffe, du ziehst einen Denkanstoß aus meinem Text,
Markus