Bagaimana cara mengubah parameter tautan dan sudut (dalam kinematika) menjadi matriks transformasi dalam logika pemrograman?

Saya sedang melakukan penelitian robotika sebagai sarjana, dan saya memahami matematika konseptual untuk sebagian besar; Namun, ketika benar-benar menerapkan kode untuk menghitung kinematika maju untuk robot saya, saya terjebak. Saya hanya tidak mengerti cara buku atau situs web yang saya temukan menjelaskannya.

Saya ingin menghitung sudut XYZ mengingat parameter tautan (parameter Denavit-Hartenberg), seperti berikut :

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

Saya tidak mengerti bagaimana mengubah tabel nilai ini menjadi matriks transformasi yang tepat yang diperlukan untuk mendapatkan , posisi Cartesian dan rotasi tautan terakhir. Dari sana, saya berharap dapat mengetahui sudut XYZ dari membaca buku saya, tetapi bantuan apa pun akan dihargai.$^0T_N$

Bagian DH Matrix dari halaman DH di wikipedia memiliki perinciannya.

Pada dasarnya Anda ingin menggunakan informasi dalam tabel Anda untuk membuat satu set matriks transformasi yang homogen. Kami melakukannya karena transformasi homogen dapat dikalikan untuk menemukan hubungan antara frame yang dipisahkan oleh satu atau lebih yang lain. Sebagai contoh, mewakili transformasi dari frame 1 ke frame 0 sementara mewakili transformasi dari frame 2 ke frame 1. Dengan mengalikannya kita mendapatkan transformasi dari frame 2 ke frame 0, yaitu .$^0T_1$$^1T_2$$^0T_2 = ^0T_1^1T_2$

Cara mudah untuk membuat setiap transformasi adalah dengan membuat transformasi homogen atau matriks rotasi homogen untuk setiap kolom dalam tabel dan mengalikannya bersama. Misalnya, transformasi dari 1 ke 0 (mis. ) adalah$^{i-1}T_i, i = 1$

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

dimana

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

Pada kasus ini

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

Setelah Anda memiliki semua transformasi Anda, Anda melipatgandakan mereka ke sana, misalnya

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

Akhirnya Anda dapat membaca vektor perpindahan dari transformasi homogen (yaitu ). Demikian pula, Anda dapat membaca matriks rotasi dari untuk menemukan sudut XYZ.$^0T_N$$d = [^0T_{N,14}, ^0T_{N,24}, ^0T_{N,34}]^T$$^0T_N$