Як перетворити параметри зв'язку та кути (у кінематиці) в матриці перетворення в логіці програмування?

Я займаюся дослідженнями робототехніки як бакалаврату, і розумію концептуальну математику здебільшого; однак, коли мова йде про фактично реалізований код для обчислення кінематики прямої передачі для мого робота, я застряг. Я просто не розумію, як пояснила книжка чи веб-сайти, які я знайшла.

Я хотів би обчислити XYZ кутів дані параметри зв'язку (параметри Denavit-Хартенберга), наприклад, наступне :

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

Я не розумію, як перетворити цю таблицю значень у правильні матриці перетворення, необхідні для отримання , декартової позиції та обертання останнього посилання. З цього моменту я сподіваюся, що зможу зрозуміти кут XYZ, коли читаю книгу, але будь-яка допомога буде вдячна.$^0T_N$

Розділ " Матриця DH" сторінки DH на wikipedia містить деталі.

В основному ви хочете використовувати інформацію в таблиці, щоб створити набір однорідних матриць перетворення. Ми робимо це тому, що однорідні перетворення можна множити, щоб знайти співвідношення між кадрами, відокремленими одними або кількома іншими. Наприклад, являє собою перетворення з кадру 1 в кадр 0, тоді як являє собою перетворення з кадру 2 в кадр 1. Помноживши їх, ми отримаємо перетворення з кадру 2 в кадр 0, тобто .$^0T_1$$^1T_2$$^0T_2 = ^0T_1^1T_2$

Найпростіший спосіб створити кожне з перетворень - це зробити однорідну матрицю перетворення або однорідну матрицю обертання для кожного стовпця таблиці та помножити їх разом. Наприклад, перетворення від 1 до 0 (наприклад ) є$^{i-1}T_i, i = 1$

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

де

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

В цьому випадку

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

Після всіх перетворень ви множите їх, наприклад,

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

Нарешті, ви можете прочитати вектор зміщення з однорідного перетворення (тобто ). Аналогічно ви можете прочитати матрицю обертання з щоб знайти кути XYZ.$^0T_N$$d = [^0T_{N,14}, ^0T_{N,24}, ^0T_{N,34}]^T$$^0T_N$