프로그래밍 논리에서 링크 매개 변수와 각도 (운동학)를 변환 행렬로 어떻게 변환합니까?

저는 학부생으로서 로봇 공학 연구를하고 있으며, 대부분 개념 수학을 이해합니다. 그러나 실제로 로봇의 순 운동학을 계산하는 코드를 구현할 때 나는 붙어 있습니다. 내가 찾은 책이나 웹 사이트가 설명하는 방식을 얻지 못했습니다.

나는 같은과 같은 링크 매개 변수 (Denavit-Hartenberg 매개 변수), 주어진 한 각도 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}$$

이 값 테이블을 마지막 링크의 직교 위치 및 회전 ^ 0T_N 을 얻는 데 필요한 적절한 변환 행렬로 바꾸는 방법을 이해하지 못합니다 $^0T_N$. 거기에서, 나는 책을 읽음으로써 XYZ 각도를 알아낼 수 있기를 희망하지만 어떤 도움이라도 감사하겠습니다.

Wikipedia의 DH 페이지에 있는 DH Matrix 섹션에 자세한 내용이 있습니다.

기본적으로 테이블의 정보를 사용하여 동종 변환 행렬 세트를 작성하려고합니다. 하나 이상의 다른 프레임으로 분리 된 프레임 간의 관계를 찾기 위해 동종 변환을 곱할 수 있기 때문에 그렇게합니다. 예를 들어 은 프레임 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$