Как преобразовать параметры и углы связи (в кинематике) в матрицы преобразования в логике программирования?

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

Я хотел бы рассчитать углы XYZ, учитывая параметры связи (параметры Денавита-Хартенберга), такие как следующее :

$$\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 Matrix на странице DH в Википедии содержит подробную информацию.

В основном вы хотите использовать информацию в вашей таблице для создания набора однородных матриц преобразования. Мы делаем это потому, что однородные преобразования можно умножить, чтобы найти связь между фреймами, разделенными одним или несколькими другими. Например, представляет преобразование из кадра 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$