Làm cách nào để chuyển đổi các tham số và góc liên kết (trong động học) thành ma trận biến đổi trong logic lập trình?

Tôi đang làm nghiên cứu về robot như một sinh viên, và tôi hiểu phần lớn toán học khái niệm; tuy nhiên, khi thực sự triển khai mã để tính toán động học chuyển tiếp cho robot của tôi, tôi bị mắc kẹt. Tôi chỉ không hiểu cách mà cuốn sách hoặc trang web mà tôi tìm thấy giải thích nó.

Tôi muốn tính toán các góc XYZ cho các tham số liên kết (tham số Denavit-Hartenberg), chẳng hạn như sau :

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

$^0T_N$

Phần DH Matrix của trang DH trên wikipedia có các chi tiết.

$^0T_1$$^1T_2$$^0T_2 = ^0T_1^1T_2$

$^{i-1}T_i, i = 1$

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

Ở đâu

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

Trong trường hợp này

$^0T1 = Rot(\theta_1)$

Khi bạn có tất cả các biến đổi của mình, bạn nhân chúng lên, ví dụ:

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