3.2 순기구학

3.2.1 동차변환 체인

관절 벡터를 $q=[q_1,\ldots,q_n]^T$라 하자. 베이스 프레임 $\{0\}$에서 엔드이펙터 프레임 $\{E\}$까지의 변환은

$${}^{0}T_E(q) = {}^{0}T_1(q_1){}^{1}T_2(q_2)\cdots{}^{n-1}T_E(q_n)$$

이다. 변환행렬은

$$T=\begin{bmatrix}R & p\\0&1\end{bmatrix},$$

여기서 $R\in SO(3)$는 회전, $p\in\mathbb{R}^3$는 위치다. 행렬 곱은 교환법칙이 성립하지 않으므로 관절 순서가 곧 모델이다.

3.2.2 표준 DH 표현

표준 DH 파라미터 $(a_i,\alpha_i,d_i,\theta_i)$의 한 링크 변환은

$$A_i = R_z(\theta_i)T_z(d_i)T_x(a_i)R_x(\alpha_i)$$

이며, 회전 관절에서는 $\theta_i=q_i+\theta_{0,i}$가 된다. DH는 계산과 교육에 편리하지만, URDF의 임의 축·오프셋을 변환하는 과정에서 실수가 생길 수 있다. 실무에서는 다음 두 모델을 나란히 둔다.

• 권위 모델(authoritative model): 버전을 고정한 URDF와 캘리브레이션 파일
• 계산 모델(computational model): DH 또는 Product of Exponentials로 정리한 코드

무작위 관절 자세 여러 개에서 두 모델의 엔드이펙터 위치와 회전을 비교해야 한다. 이 장의 code/fk_dls_ik.py는 알고리즘 학습용 5축 DH 예제이며, 실제 SO-101 치수를 주장하지 않는다.

3.2.3 기하 자코비안

베이스 좌표계에서 관절 $i$의 회전축을 $z_{i-1}$, 관절 원점을 $o_{i-1}$, 엔드이펙터 원점을 $o_E$라 하면 회전 관절의 자코비안 열은

$$J_{v,i}=z_{i-1}\times(o_E-o_{i-1}),\qquad J_{\omega,i}=z_{i-1}.$$

따라서

$$\begin{bmatrix}v_E\\\omega_E\end{bmatrix}=J(q)\dot q.$$

5축 팔의 전체 pose 자코비안은 $6\times5$다. 모든 6차원 twist를 독립적으로 만들 수 없으므로 위치만 사용하는 $3\times5$ 자코비안이나 방향 오차에 낮은 가중치를 둔 가중 자코비안을 선택한다.