Como converter parâmetros e ângulos de link (em cinemática) em matrizes de transformação na lógica de programação?

Estou fazendo pesquisa em robótica na graduação e compreendo a matemática conceitual na maior parte; no entanto, quando se trata de implementar código para calcular a cinemática direta do meu robô, eu estou preso. Só não estou conseguindo explicar como o livro ou os sites que eu encontrei.

Gostaria de calcular os ângulos XYZ considerando os parâmetros de link (parâmetros Denavit-Hartenberg), como os seguintes :

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

Não entendo como transformar essa tabela de valores nas matrizes de transformação adequadas necessárias para obter 0 T N , a posição cartesiana e a rotação do último link. A partir daí, espero poder descobrir o (s) ângulo (s) XYZ da leitura do meu livro, mas qualquer ajuda seria apreciada.$^0T_N$

A seção DH Matrix da página DH na wikipedia possui os detalhes.

Basicamente, você deseja usar as informações da sua tabela para criar um conjunto de matrizes de transformação homogêneas. Fazemos isso porque transformações homogêneas podem ser multiplicadas para encontrar a relação entre os quadros separados por um ou mais outros. Por exemplo, 0 T 1 representa a transformação do quadro 1 para o quadro 0, enquanto 1 T 2 representa a transformação do quadro 2 para o quadro 1. Ao multiplicá-los, obtemos a transformação do quadro 2 para o quadro 0, ou seja, 0 T 2 = 0 T 1 1 T 2 .$^0T_1$$^1T_2$$^0T_2 = ^0T_1^1T_2$

Uma maneira fácil de criar cada uma das transformações é fazer uma transformação homogênea ou matriz de rotação homogênea para cada coluna da tabela e multiplicá-las. Por exemplo, a transformação de 1 em 0 (por exemplo, i - 1 T i , i = 1 ) é$^{i-1}T_i, i = 1$

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

Onde

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

Nesse caso

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

Depois de ter todas as suas transformações, você as multiplica, por exemplo

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

Finalmente, você pode ler o vetor de deslocamento da transformação homogênea $^0T_N$ (ie $d = [^0T_{N,14}, ^0T_{N,24}, ^0T_{N,34}]^T$) Da mesma forma, você pode ler a matriz de rotação de$^0T_N$ para encontrar os ângulos XYZ.