Wie konvertiere ich Verbindungsparameter und Winkel (in der Kinematik) in Transformationsmatrizen in der Programmierlogik?

Ich mache als Student Robotikforschung und verstehe die konzeptionelle Mathematik größtenteils. Wenn es jedoch darum geht, Code zur Berechnung der Vorwärtskinematik für meinen Roboter tatsächlich zu implementieren, stecke ich fest. Ich verstehe einfach nicht, wie das Buch oder die Websites, die ich gefunden habe, es erklären.

Ich möchte das XYZ berechnen , die Verbindungsparameter gegebene Winkel (Denavit-Harte Parameter), wie beispielsweise die folgenden :

$$\begin{array}{ccc} \bf{i} & \bf{\alpha_i-1} & \bf{a_i-1} & \bf{d_i} & \bf{\theta_i}\\ \\ 1 & 0 & 0 & 0 & \theta_1\\ 2 & -90^{\circ} & 0 & 0 & \theta_2\\ 3 & 0 & a_2 & d_3 & \theta_3\\ 4 & -90^{\circ} & a_3 & d_4 & \theta_4\\ 5 & 90^{\circ} & 0 & 0 & \theta_5\\ 6 & -90^{\circ} & 0 & 0 & \theta_6\\ \end{array}$$

Ich verstehe nicht, wie man diese Wertetabelle in die richtigen Transformationsmatrizen , die benötigt werden, um , die kartesische Position und die Drehung des letzten Links zu erhalten. Von dort aus hoffe ich, dass ich die XYZ-Winkel beim Lesen meines Buches herausfinden kann, aber jede Hilfe wäre willkommen.$^0T_N$

Der DH Matrix- Abschnitt der DH-Seite auf Wikipedia enthält die Details.

Grundsätzlich möchten Sie die Informationen in Ihrer Tabelle verwenden, um eine Reihe homogener Transformationsmatrizen zu erstellen. Wir tun dies, weil homogene Transformationen multipliziert werden können, um die Beziehung zwischen Frames zu finden, die von einem oder mehreren anderen getrennt sind. Zum Beispiel repräsentiert 0 T 1 die Transformation von Frame 1 zu Frame 0, während 1 T 2 die Transformation von Frame 2 zu Frame 1 darstellt. Durch Multiplizieren erhalten wir die Transformation von Frame 2 zu Frame 0, dh 0 T 2 = 0 T. 1 1 T 2 .$^0T_1$$^1T_2$$^0T_2 = ^0T_1^1T_2$

Eine einfache Möglichkeit, jede der Transformationen zu erstellen, besteht darin, für jede Spalte in der Tabelle eine homogene Transformation oder eine homogene Rotationsmatrix zu erstellen und diese miteinander zu multiplizieren. Zum Beispiel ist die Transformation von 1 nach 0 (zB i - 1 T i , i = 1 )$^{i-1}T_i, i = 1$

$^0T_1 = Trans(d_1)*Rot(\theta_1)*Trans(a_2)*Rot(\alpha_2)$

wo

$Trans(d_1) = \begin{bmatrix}1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & \bf{d_1 = 0} \\ 0 & 0 & 0 & 1 \end{bmatrix},$

$Rot(\theta_1) = \begin{bmatrix} \text{cos}(\bf{\theta_1}) & - \text{sin}(\bf{\theta_1}) & 0 & 0 \\ \text{sin}(\bf{\theta_1}) & \text{cos}(\bf{\theta_1}) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix},$

$Trans(a_2) = \begin{bmatrix} 1 & 0 & 0 & \bf{a_2 = 0} \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix},$

$Rot(\alpha_2) = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & \text{cos}(\bf{\alpha_2 = 0}) & -\text{sin}(\bf{\alpha_2 = 0}) & 0 \\ 0 & \text{sin}(\bf{\alpha_2 = 0}) & \text{cos}(\bf{\alpha_2 = 0}) & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$

In diesem Fall

$^0T1 = Rot(\theta_1)$

Sobald Sie alle Ihre Transformationen haben, multiplizieren Sie sie zusammen, z

$^0T_N = ^0T_1*^1T_2...^{N-1}T_N$

$^0T_N$$d = [^0T_{N,14}, ^0T_{N,24}, ^0T_{N,34}]^T$$^0T_N$