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
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
