9.53 연쇄 DH 변환과 말단 장치 위치 계산

1. 순기구학 문제

매니퓰레이터의 순기구학(forward kinematics) 문제는 주어진 관절 변수 $\boldsymbol{\theta} = (\theta_1, \theta_2, \ldots, \theta_n)^T$ 에 대해 말단 장치(end-effector)의 위치와 자세를 계산하는 것이다. 연쇄형 매니퓰레이터에서 이 문제는 DH 변환의 연쇄 곱으로 체계적으로 해결된다.

2. 연쇄 곱의 정식

$n$ 개의 관절을 가진 매니퓰레이터의 베이스 좌표계 $\{0\}$ 에서 말단 장치 좌표계 $\{n\}$ 까지의 변환은 모든 인접 변환의 곱이다.

${}^0\mathbf{T}_n(\boldsymbol{\theta}) = {}^0\mathbf{T}_1(\theta_1) \cdot {}^1\mathbf{T}_2(\theta_2) \cdot {}^2\mathbf{T}_3(\theta_3) \cdots {}^{n-1}\mathbf{T}_n(\theta_n)$

또는 곱 표기로

${}^0\mathbf{T}_n(\boldsymbol{\theta}) = \prod_{i=1}^n {}^{i-1}\mathbf{T}_i(\theta_i)$

각 ${}^{i-1}\mathbf{T}_i(\theta_i)$ 는 $i$ 번째 링크의 DH 매개변수와 관절 변수 $\theta_i$ (회전 관절의 경우)에 의해 결정되는 동차 변환 행렬이다.

3. 말단 장치 자세 행렬의 구조

연쇄 곱의 결과 ${}^0\mathbf{T}_n$ 은 다음의 블록 형태를 가진다.

${}^0\mathbf{T}_n(\boldsymbol{\theta}) = \begin{bmatrix}\mathbf{R}(\boldsymbol{\theta}) & \mathbf{p}(\boldsymbol{\theta}) \\ \mathbf{0}^T & 1\end{bmatrix}$

여기서

$\mathbf{R}(\boldsymbol{\theta}) \in SO(3)$ : 말단 장치의 자세(orientation)
$\mathbf{p}(\boldsymbol{\theta}) \in \mathbb{R}^3$ : 말단 장치의 위치(position)

말단 장치의 위치 벡터는 행렬의 우상단 $3 \times 1$ 블록을 추출하여 얻는다.

4. 계산의 단계별 절차

4.1 단계: DH 매개변수 표 준비

매니퓰레이터의 DH 매개변수 표 $(a_i, \alpha_i, d_i, \theta_i)_{i=1}^n$ 를 준비한다. 회전 관절에서는 $\theta_i$ 가 입력 변수이고, 직선 관절에서는 $d_i$ 가 입력 변수이다.

4.2 단계: 인접 변환 행렬 계산

각 링크 $i = 1, 2, \ldots, n$ 에 대해 다음의 변환 행렬을 계산한다.

${}^{i-1}\mathbf{T}_i = \begin{bmatrix} \cos\theta_i & -\sin\theta_i\cos\alpha_i & \sin\theta_i\sin\alpha_i & a_i\cos\theta_i \\ \sin\theta_i & \cos\theta_i\cos\alpha_i & -\cos\theta_i\sin\alpha_i & a_i\sin\theta_i \\ 0 & \sin\alpha_i & \cos\alpha_i & d_i \\ 0 & 0 & 0 & 1 \end{bmatrix}$

이 단계에서 $n$ 개의 $4 \times 4$ 행렬이 산출된다.

4.3 단계: 행렬의 연쇄 곱

$n$ 개의 행렬을 차례로 곱한다. 결합 법칙에 의해 곱의 순서는 결과에 영향을 주지 않으나, 일반적으로 왼쪽에서 오른쪽으로 누적적으로 곱하는 것이 효율적이다.

${}^0\mathbf{T}_1 \to {}^0\mathbf{T}_1 {}^1\mathbf{T}_2 \to ({}^0\mathbf{T}_1 {}^1\mathbf{T}_2) {}^2\mathbf{T}_3 \to \cdots$

4.4 단계: 위치와 자세 추출

최종 행렬 ${}^0\mathbf{T}_n$ 로부터 위치 벡터와 자세 행렬을 추출한다.

$\mathbf{p} = {}^0\mathbf{T}_n[1:3, 4], \quad \mathbf{R} = {}^0\mathbf{T}_n[1:3, 1:3]$

5. 부분 변환의 활용

자코비안 계산이나 동역학 계산에서 부분 변환 ${}^0\mathbf{T}_i$ ( $i = 1, 2, \ldots, n-1$ )이 필요하다. 이러한 경우 연쇄 곱을 누적적으로 계산하면서 각 단계의 부분 변환을 저장한다.

${}^0\mathbf{T}_i = {}^0\mathbf{T}_{i-1} \cdot {}^{i-1}\mathbf{T}_i$

이 점진적 계산은 $n$ 개의 부분 변환을 모두 한 번의 순회로 산출하므로 효율적이다.

6. 연쇄 곱의 의미

연쇄 곱은 좌표계 사이의 누적적 변환을 의미한다. ${}^0\mathbf{T}_n$ 의 각 열은 다음과 같이 해석된다.

첫 번째 열: 말단 장치 좌표계의 $x$ 축이 베이스 좌표계에서 표현된 단위 벡터
두 번째 열: 말단 장치 좌표계의 $y$ 축이 베이스 좌표계에서 표현된 단위 벡터
세 번째 열: 말단 장치 좌표계의 $z$ 축이 베이스 좌표계에서 표현된 단위 벡터
네 번째 열: 말단 장치 좌표계의 원점이 베이스 좌표계에서 표현된 위치(동차 좌표)

7. 예시: 2자유도 평면 매니퓰레이터

링크 길이 $L_1$ , $L_2$ 를 가진 2자유도 평면 매니퓰레이터를 고려한다. DH 매개변수 표는 다음과 같다.

링크 $i$	$a_i$	$\alpha_i$	$d_i$	$\theta_i$
1	$L_1$	0	0	$\theta_1$
2	$L_2$	0	0	$\theta_2$

각 인접 변환은 다음과 같다.

${}^0\mathbf{T}_1 = \begin{bmatrix} \cos\theta_1 & -\sin\theta_1 & 0 & L_1\cos\theta_1 \\ \sin\theta_1 & \cos\theta_1 & 0 & L_1\sin\theta_1 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$

${}^1\mathbf{T}_2 = \begin{bmatrix} \cos\theta_2 & -\sin\theta_2 & 0 & L_2\cos\theta_2 \\ \sin\theta_2 & \cos\theta_2 & 0 & L_2\sin\theta_2 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$

연쇄 곱을 계산하면

${}^0\mathbf{T}_2 = {}^0\mathbf{T}_1 \cdot {}^1\mathbf{T}_2 = \begin{bmatrix} \cos(\theta_1 + \theta_2) & -\sin(\theta_1 + \theta_2) & 0 & L_1\cos\theta_1 + L_2\cos(\theta_1 + \theta_2) \\ \sin(\theta_1 + \theta_2) & \cos(\theta_1 + \theta_2) & 0 & L_1\sin\theta_1 + L_2\sin(\theta_1 + \theta_2) \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$

이 된다. 말단 장치의 위치는

$\mathbf{p}(\theta_1, \theta_2) = \begin{bmatrix}L_1\cos\theta_1 + L_2\cos(\theta_1 + \theta_2) \\ L_1\sin\theta_1 + L_2\sin(\theta_1 + \theta_2) \\ 0\end{bmatrix}$

이며, 이는 평면 2자유도 매니퓰레이터의 잘 알려진 순기구학 공식이다.

8. 자유도 매니퓰레이터의 경우

PUMA 560과 같은 6자유도 매니퓰레이터의 순기구학은 6개의 인접 변환 행렬의 연쇄 곱이다. 대수적으로 전개하면 매우 복잡한 식이 되지만, 행렬 곱을 수치적으로 수행하면 효율적으로 계산할 수 있다.

9. 계산 비용

9.1 단일 행렬 곱

$4 \times 4$ 행렬 곱은 일반적으로 64회의 곱셈과 48회의 덧셈을 요구한다. 그러나 동차 변환 행렬의 마지막 행이 $(0, 0, 0, 1)$ 로 고정되어 있으므로, 마지막 행을 명시적으로 계산하지 않으면 약 36회 곱셈과 27회 덧셈으로 충분하다.

9.2 전체 연쇄 곱

$n$ 개의 인접 변환을 곱하는 데 약 $36(n-1)$ 회 곱셈과 $27(n-1)$ 회 덧셈이 필요하다. $n = 6$ 의 경우 약 180회 곱셈과 135회 덧셈으로, 현대 컴퓨터에서 수 마이크로초 이내에 완료된다.

9.3 삼각 함수 계산

각 인접 변환에서 $\sin\theta_i$ , $\cos\theta_i$ , $\sin\alpha_i$ , $\cos\alpha_i$ 의 4개의 삼각 함수가 필요하다. $\sin\alpha_i$ 와 $\cos\alpha_i$ 는 상수이므로 사전 계산하여 저장해 둘 수 있다. 관절 변수 $\theta_i$ 의 삼각 함수만 매번 새로 계산하면 된다.

10. 효율적 구현 기법

10.1 부분 변환의 캐싱

순기구학과 자코비안을 동시에 계산할 때, 부분 변환 ${}^0\mathbf{T}_i$ 를 캐싱하여 재사용한다. 이는 자코비안의 각 열이 부분 변환에 의존하기 때문이다.

10.2 사전 계산된 상수

$\sin\alpha_i$ , $\cos\alpha_i$ , $a_i$ , $d_i$ 등의 상수는 매니퓰레이터의 초기화 시 한 번 계산하여 저장한다.

10.3 SIMD 최적화

현대 프로세서의 SIMD(Single Instruction Multiple Data) 명령을 활용하면 행렬 곱을 더욱 효율적으로 수행할 수 있다. 라이브러리 수준에서 자동으로 활용되는 경우가 많다.

11. 정확성 검증

계산된 ${}^0\mathbf{T}_n$ 의 정확성을 다음의 검증으로 확인할 수 있다.

11.1 정규 직교성

$\mathbf{R}^T\mathbf{R} = \mathbf{I}_3$ 가 수치적으로 만족되는지 확인한다.

11.2 행렬식

$\det(\mathbf{R}) \approx +1$ 인지 확인한다.

11.3 극한 케이스

특별히 알려진 자세(예: 모든 관절 각이 0 또는 명목 자세)에서 결과가 예상값과 일치하는지 확인한다.

12. 응용

12.1 시뮬레이션

순기구학 계산은 매니퓰레이터 시뮬레이션의 핵심 모듈이다. 시뮬레이션 매 시각에 현재 관절 각을 입력으로 받아 말단 장치 위치를 계산한다.

12.2 시각화

로봇 모델의 시각적 렌더링에서 각 링크의 자세를 결정하기 위해 부분 변환 ${}^0\mathbf{T}_i$ 가 사용된다.

12.3 작업 영역 분석

매니퓰레이터의 가능한 모든 자세를 탐색하여 작업 영역(workspace)을 결정하는 데 순기구학이 반복적으로 호출된다.

12.4 충돌 검사

매니퓰레이터의 각 링크가 환경의 장애물과 충돌하는지 확인하기 위해 각 링크의 자세를 알아야 하며, 이는 부분 변환에서 얻는다.

12.5 자코비안 계산

매니퓰레이터의 자코비안 행렬은 부분 변환의 회전 부분과 위치 벡터의 차이로부터 직접 계산된다.

13. 참고 문헌

Denavit, J., & Hartenberg, R. S. (1955). “A Kinematic Notation for Lower-Pair Mechanisms Based on Matrices.” Journal of Applied Mechanics, 22, 215–221.
Craig, J. J. (2018). Introduction to Robotics: Mechanics and Control (4th ed.). Pearson.
Siciliano, B., Sciavicco, L., Villani, L., & Oriolo, G. (2009). Robotics: Modelling, Planning and Control. Springer.
Spong, M. W., Hutchinson, S., & Vidyasagar, M. (2020). Robot Modeling and Control (2nd ed.). Wiley.
Featherstone, R. (2008). Rigid Body Dynamics Algorithms. Springer.

version: 1.0