Hallo Payot,
Den Teil a) hast Du richtig gelöst$${}^BT_{W}=\begin{pmatrix} \cos(\Theta) & -\sin(\Theta)& 0& X \\ \sin(\Theta) & \cos(\Theta) & 0& Y \\ 0& 0& 1& 0 \\ 0& 0& 0& 1\end{pmatrix}$$
Für den Teil b) sind zunächst die einzelnen Transformationsmatrizen aufzustellen. Ich benutze dazu die Syntax aus dem Wikipedia-Artikel über die Denativ-Hartenberg Transformation. Es ist:$${}^BT_{1}=\begin{pmatrix} \cos(\vartheta_1) & -\sin(\vartheta_1)& 0& 0 \\ \sin(\vartheta_1) & \cos(\vartheta_1) & 0& 0 \\ 0& 0& 1& 0 \\ 0& 0& 0& 1\end{pmatrix} \\{}^1T_2 = \begin{pmatrix} \cos(\vartheta_2) & \sin(\vartheta_2)& 0& L_1 \\ -\sin(\vartheta_2) & \cos(\vartheta_2) & 0& 0 \\ 0& 0& 1& 0 \\ 0& 0& 0& 1\end{pmatrix} \\ {}^2T_W = \begin{pmatrix} \cos(\vartheta_3) & -\sin(\vartheta_3)& 0& L_2 \\ \sin(\vartheta_3) & \cos(\vartheta_3) & 0& 0 \\ 0& 0& 1& 0 \\ 0& 0& 0& 1\end{pmatrix}$$Beachte bitte, dass \(\vartheta_2\) links herum positiv definiert ist. Daher wandert dort das Vorzeichen.
Um nun die Matrix \({}^{B}T_{W}\) in Abhängigkeit der Gelenkwinkel und Längen aufzustellen, werden diese Matrizen miteinander multipliziert. $${}^{B}T_{W} = {}^{B}T_{1} \cdot {}^{1}T_{2} \cdot {}^{2}T_{W}$$Hier müsen die Indizes rechts und links vom Punkt immer identisch sein! Bei der Multiplikation mache ich noch von den Additionstheoremen Gebrauch$$\cos(a)\cos(b) \mp \sin(a)\sin(b) = \cos(a\pm b)\\ \sin(a)\cos(b) \pm \cos(a)\sin(b) = \sin(a\pm b)$$Dann ist das Resulttat der Multiplikation$${}^BT_W = \begin{pmatrix} \cos(\vartheta_1-\vartheta_2+\vartheta_3) & -\sin(\vartheta_1-\vartheta_2+\vartheta_3)& 0& L_1\cos(\vartheta_1) +L_2\cos(\vartheta_1-\vartheta_2)\\ \sin(\vartheta_1-\vartheta_2+\vartheta_3) & \cos(\vartheta_1-\vartheta_2+\vartheta_3)& 0& L_1\sin(\vartheta_1) +L_2\sin(\vartheta_1-\vartheta_2)\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{pmatrix}$$Gleichsetzen dieser Matrizen liefert ein Gleichungssystem mit den drei unbekannten Winkeln$$\vartheta_1-\vartheta_2+\vartheta_3 = \Theta \\ L_1\cos(\vartheta_1) +L_2\cos(\vartheta_1-\vartheta_2) = X\\ L_1\sin(\vartheta_1) +L_2\sin(\vartheta_1-\vartheta_2) = Y$$Die letzten beiden sind nicht von \(\vartheta_3\) abhängig und könnte man mit 'Schulmitteln' lösen. Das mache ich jetzt aber nicht, sondern erinnere mich an den Kosinussatz ;-)
Die beiden Längen \(L_1\) und \(L_2\) sowie der Abstand des Handgelenks von der Basis \(\sqrt{X^2+Y^2}\) bilden ein Dreieck, bei dem sich die Winkel berechnen lassen:$$\vartheta_1= \arctan\left(\frac{Y}{X}\right) \pm \arccos\left(\frac{L_{1}^{2}+\left(X^2+Y^2\right)-L_{2}^{2}}{2L_{1}\cdot \sqrt{X^2+Y^2}}\right)\\ \vartheta_2 = \pm\arccos\left(\frac{\left(X^2+Y^2\right)-L_{1}^{2}-L_{2}^{2}}{2L_{1}L_{2}}\right)$$und \(\vartheta_3\) ist jetzt kein Problem mehr:$$\vartheta_3= \Theta - \vartheta_1 + \vartheta_2$$Das Desmos-Applet zeigt eines der beiden Ergebnise
https://www.desmos.com/calculator/uq1hf9ypug
Die Position des Greifers und sein Winkel lassen sich mit der Maus verschieben (die roten Punkte). Ebenso kann man links im Bild die Längen \(L_{1,2}\) durch vertkales Verschieben ändern. Die Gelenkwinkel \(\vartheta_{1,2,3}\)werden angezeigt. Wandert der Greifer aus dem Bereich des Roboters, so verschwindet das Knickgelenk.
Gruß Werner
