9.25 롤-피치-요와 회전 행렬의 상호 변환

1. 변환의 양방향성

롤-피치-요(RPY) 표현과 회전 행렬 사이의 상호 변환은 두 방향이 존재한다.

순방향(Forward): RPY 각도 $(\phi, \theta, \psi)$ 로부터 회전 행렬 $\mathbf{R}$ 을 합성
역방향(Inverse): 회전 행렬 $\mathbf{R}$ 로부터 RPY 각도 $(\phi, \theta, \psi)$ 를 추출

순방향은 삼각 함수의 곱으로 결정적이지만, 역방향은 다가성과 특이점 처리에 주의가 필요하다.

2. 순방향 변환: RPY에서 회전 행렬로

$ZYX$ 테이트-브라이언 이동 축 규약에서 RPY 각도 $(\phi, \theta, \psi)$ (롤, 피치, 요)로부터 회전 행렬은 다음과 같이 합성된다.

$\mathbf{R}_{RPY}(\phi, \theta, \psi) = \mathbf{R}_z(\psi)\mathbf{R}_y(\theta)\mathbf{R}_x(\phi)$

기본 회전 행렬의 곱을 전개하면 $c_\phi = \cos\phi$ , $s_\phi = \sin\phi$ 등을 사용하여 다음의 명시적 형태를 얻는다.

$\mathbf{R}_{RPY} = \begin{bmatrix} c_\psi c_\theta & c_\psi s_\theta s_\phi - s_\psi c_\phi & c_\psi s_\theta c_\phi + s_\psi s_\phi \\ s_\psi c_\theta & s_\psi s_\theta s_\phi + c_\psi c_\phi & s_\psi s_\theta c_\phi - c_\psi s_\phi \\ -s_\theta & c_\theta s_\phi & c_\theta c_\phi \end{bmatrix}$

3. 역방향 변환: 회전 행렬에서 RPY로

주어진 회전 행렬 $\mathbf{R} = [r_{ij}]$ 로부터 RPY 각도를 추출하는 공식은 위 행렬과의 원소별 대응을 통해 유도된다.

3.1 피치의 추출

행렬의 (3,1) 원소는 $-\sin\theta$ 와 일치하므로

$\theta = -\arcsin(r_{31})$

또는 수치적으로 더 안정적인 형태로

$\theta = \mathrm{atan2}\left(-r_{31}, \sqrt{r_{11}^2 + r_{21}^2}\right)$

이다. 후자는 $\arcsin$ 의 정의역 경계에서의 수치 오차에 덜 민감하다. 피치의 값은 관례적으로 $[-\pi/2, \pi/2]$ 로 제한되므로 $\cos\theta \geq 0$ 이다.

3.2 요의 추출

$\cos\theta \neq 0$ 일 때 (1,1)와 (2,1) 원소로부터

$\psi = \mathrm{atan2}(r_{21}, r_{11})$

이 성립한다. 이는 $r_{11} = c_\psi c_\theta$ 와 $r_{21} = s_\psi c_\theta$ 에서 $\cos\theta > 0$ 을 이용하여 직접 얻어진다.

3.3 롤의 추출

$\cos\theta \neq 0$ 일 때 (3,2)와 (3,3) 원소로부터

$\phi = \mathrm{atan2}(r_{32}, r_{33})$

이 성립한다. 이는 $r_{32} = c_\theta s_\phi$ 와 $r_{33} = c_\theta c_\phi$ 에서 직접 얻어진다.

3.4 특이점: $\theta = \pi/2$

$r_{31} = -1$ 이면 $\theta = \pi/2$ 이고 $\cos\theta = 0$ 이 되어 위 공식이 성립하지 않는다. 이 경우 회전 행렬은 다음의 형태를 가진다.

$\mathbf{R} = \begin{bmatrix} 0 & \sin(\phi - \psi) & \cos(\phi - \psi) \\ 0 & \cos(\phi - \psi) & -\sin(\phi - \psi) \\ -1 & 0 & 0 \end{bmatrix}$

여기서 $\phi - \psi$ 만이 의미를 가진다. 관례상 $\psi = 0$ 으로 설정하고 $\phi = \mathrm{atan2}(r_{12}, r_{22})$ 로 결정한다.

3.5 특이점: $\theta = -\pi/2$

$r_{31} = 1$ 이면 $\theta = -\pi/2$ 이고 회전 행렬은

$\mathbf{R} = \begin{bmatrix} 0 & -\sin(\phi + \psi) & -\cos(\phi + \psi) \\ 0 & \cos(\phi + \psi) & -\sin(\phi + \psi) \\ 1 & 0 & 0 \end{bmatrix}$

이 되며, $\phi + \psi$ 의 합만이 의미를 가진다. 관례상 $\psi = 0$ 으로 설정하고 $\phi = -\mathrm{atan2}(r_{12}, r_{22})$ 로 결정한다.

4. 역방향 변환 알고리즘

수치적으로 견고한 RPY 추출 알고리즘은 다음 단계로 구성된다.

$\theta = \mathrm{atan2}(-r_{31}, \sqrt{r_{11}^2 + r_{21}^2})$ 를 계산한다.
if $\cos\theta$ 가 충분히 0에서 떨어져 있으면 ( $|r_{31}| < 1 - \epsilon$ ):

$\psi = \mathrm{atan2}(r_{21}, r_{11})$
$\phi = \mathrm{atan2}(r_{32}, r_{33})$

else if $r_{31} \approx -1$ ( $\theta \approx \pi/2$ ):

$\psi = 0$
$\phi = \mathrm{atan2}(r_{12}, r_{22})$

else ( $r_{31} \approx 1$ , $\theta \approx -\pi/2$ ):

$\psi = 0$
$\phi = -\mathrm{atan2}(r_{12}, r_{22})$

$\epsilon$ 은 수치적 특이점 감지 임계값으로 통상 $10^{-6}$ 정도로 설정된다.

5. 해의 유일성과 다가성

특이점이 아닌 경우 RPY 추출은 $[-\pi, \pi) \times [-\pi/2, \pi/2] \times [-\pi, \pi)$ 의 범위 내에서 유일한 해를 산출한다. 그러나 $\arcsin$ 과 $\mathrm{atan2}$ 의 주기성을 고려하면 대수적으로 $2\pi$ 만큼 이동한 동등한 해가 무한히 많이 존재한다. 관례적인 범위 제한은 이러한 중복 중 하나를 선택하는 규약이다.

또한 피치의 경우 $\theta$ 와 $\pi - \theta$ 의 두 해가 존재할 수 있으나, 관례상 $\theta \in [-\pi/2, \pi/2]$ 에 해당하는 해만이 채택된다.

6. 수치적 유의사항

6.1 $\arcsin$ 대 $\mathrm{atan2}$

피치 추출에서 $\theta = -\arcsin(r_{31})$ 을 단순히 사용하는 경우, $r_{31}$ 이 정확히 $\pm 1$ 에 근접할 때 $\arcsin$ 의 도함수가 발산하여 수치 오차가 증폭된다. $\mathrm{atan2}$ 형태로 대체하면 이 문제가 완화된다.

6.2 범위 벗어남 처리

부동 소수점 오차로 인해 $r_{31}$ 이 $\pm 1$ 을 약간 초과할 수 있다. 이 경우 $\arcsin$ 이 NaN을 반환하므로 사전 클램핑(clamping)이 필요하다.

$r_{31} \leftarrow \max(-1, \min(1, r_{31}))$

6.3 각도 래핑

반복적 계산에서 각도의 누적 값이 $[-\pi, \pi)$ 를 벗어나면 모듈로 연산으로 래핑(wrapping)해야 한다. 이 연산은 $\mathrm{atan2}(\sin\alpha, \cos\alpha)$ 형태로 구현될 수 있다.

7. 왕복 변환의 일관성 검증

순방향과 역방향이 서로의 역함수라는 사실은 다음으로 확인된다. 순방향 $\mathrm{RPY}_{\to\mathbf{R}}$ 과 역방향 $\mathrm{RPY}_{\leftarrow\mathbf{R}}$ 에 대해

$\mathrm{RPY}_{\leftarrow\mathbf{R}}(\mathrm{RPY}_{\to\mathbf{R}}(\phi, \theta, \psi)) = (\phi, \theta, \psi) \quad \text{(특이점 제외)}$

$\mathrm{RPY}_{\to\mathbf{R}}(\mathrm{RPY}_{\leftarrow\mathbf{R}}(\mathbf{R})) = \mathbf{R}$

이 성립한다. 구현 시 이 일관성 테스트를 수치적으로 검증하는 것이 권장된다.

8. 로봇 공학에서의 실용적 적용

8.1 URDF와 로봇 모델 기술

ROS의 URDF 파일은 링크의 상대 자세를 RPY로 기술한다. 내부적으로는 회전 행렬 또는 쿼터니언으로 변환되어 처리되며, 이 과정에 본 장의 상호 변환 공식이 사용된다.

8.2 IMU 자세 추정의 출력

IMU 기반 자세 추정 알고리즘은 내부적으로 쿼터니언 또는 회전 행렬을 유지하나, 사용자 인터페이스와 텔레메트리에는 RPY로 출력한다. 실시간으로 회전 행렬에서 RPY를 추출하는 공식이 주기적으로 호출된다.

8.3 목표 자세 지정

매니퓰레이터의 역기구학에서 목표 자세가 RPY로 지정된 경우, 먼저 회전 행렬로 변환한 후 수치 해법에 공급된다. 이 변환이 순방향 공식이다.

8.4 자세 차이의 계산

두 RPY 자세의 차이는 직접 뺄셈으로 계산할 수 없다(RPY 공간이 선형 공간이 아니기 때문). 각각을 회전 행렬로 변환하여 $\Delta\mathbf{R} = \mathbf{R}_1^T\mathbf{R}_2$ 를 계산하고, 결과를 RPY로 역변환하는 절차가 정확하다.

9. 참고 문헌

Siciliano, B., Sciavicco, L., Villani, L., & Oriolo, G. (2009). Robotics: Modelling, Planning and Control. Springer.
Craig, J. J. (2018). Introduction to Robotics: Mechanics and Control (4th ed.). Pearson.
Diebel, J. (2006). “Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors.” Stanford University Technical Report.
Shuster, M. D. (1993). “A Survey of Attitude Representations.” Journal of the Astronautical Sciences, 41(4), 439–517.
Stevens, B. L., Lewis, F. L., & Johnson, E. N. (2015). Aircraft Control and Simulation (3rd ed.). Wiley.

version: 1.0