Jak przekonwertować parametry łącza i kąty (w kinematyce) na macierze transformacji w logice programowania?

Prowadzę badania z zakresu robotyki jako licencjat i w większości rozumiem matematykę konceptualną; jednak jeśli chodzi o faktyczne wdrażanie kodu do obliczania kinematyki ruchu do przodu dla mojego robota, utknąłem. Po prostu nie rozumiem, w jaki sposób znaleziona książka lub strony internetowe to wyjaśniają.

Ja jak obliczyć XYZ kątów podane parametry łącza (parametry Denavita-Hartenberga), taki jak poniżej :

$$\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}$$

Nie rozumiem, jak zmienić tę tabelę wartości w odpowiednie macierze transformacji potrzebne do uzyskania , pozycji kartezjańskiej i rotacji ostatniego linku. Stamtąd mam nadzieję, że uda mi się ustalić kąt (y) XYZ po przeczytaniu mojej książki, ale każda pomoc byłaby mile widziana.$^0T_N$

Sekcja DH Matrix strony DH na wikipedii zawiera szczegółowe informacje.

Zasadniczo chcesz wykorzystać informacje z tabeli, aby utworzyć zestaw jednorodnych macierzy transformacji. Robimy to, ponieważ homogeniczne transformacje można mnożyć, aby znaleźć relację między ramkami oddzielonymi przez jedną lub więcej innych. Na przykład reprezentuje transformację z klatki 1 do klatki 0, podczas gdy reprezentuje transformację z klatki 2 do klatki 1. Przez pomnożenie ich otrzymujemy transformację z klatki 2 do klatki 0, tj. .$^0T_1$$^1T_2$$^0T_2 = ^0T_1^1T_2$

Łatwym sposobem utworzenia każdej z transformacji jest wykonanie jednorodnej transformacji lub jednorodnej macierzy obrotu dla każdej kolumny w tabeli i pomnożenie ich razem. Na przykład transformacja z 1 na 0 (np. ) wynosi$^{i-1}T_i, i = 1$

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

gdzie

$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}$ .

W tym przypadku

$^0T1 = Rot(\theta_1)$ .

Po zakończeniu wszystkich transformacji mnożymy je razem, np

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

Wreszcie możesz odczytać wektor przemieszczenia z jednorodnej transformacji (tj. ). Podobnie możesz odczytać macierz obrotu z aby znaleźć kąty XYZ.$^0T_N$$d = [^0T_{N,14}, ^0T_{N,24}, ^0T_{N,34}]^T$$^0T_N$