3D Rigid Body Transformation & ROS에서 회전을 표현하는법

이 포스트는 “자율주행을 위한 Visual SLAM 제2판” 내용을 정리하였습니다.

사전 지식

• Vector에 대한 기본적인 연산 (내적, 외적, 벡터곱 등등)

Rotation matrix

3차원 공간의 한 점의 좌표 $\mathbf{a} \in \mathbb{R}^3$ 가 있다고 생각해 보자. 3차원 공간에서 base를 구했다고 생각해 보면 $\mathbf{a}$는 아래와 같이 표현할 수 있다.

\begin{equation} \mathbf{a} = \begin{bmatrix} \mathbf{e}_1 & \mathbf{e}_2 & \mathbf{e}_3 \end{bmatrix} \begin{bmatrix} a_1 \\ a_2 \\ a_3 \end{bmatrix} = a_1\mathbf{e}_1 + a_2\mathbf{e}_2 + a_3\mathbf{e}_3 \end{equation}

 

$\mathbb{a}$ 라는 점은 변하지 않고 우리가 구한 base $\begin{bmatrix} \mathbf{e}_1 & \mathbf{e}_2 & \mathbf{e}_3 \end{bmatrix}$ 를 회전시켜 $\begin{bmatrix} \mathbf{e}^{'}_1 & \mathbf{e}^{'}_2 & \mathbf{e}^{'}_3 \end{bmatrix}$ 로 바꿔 보자. 좌표계가 바뀜에 따라 좌표 또한 $\begin{bmatrix} a_1 & a_2 & a_3 \end{bmatrix}^T$ 에서 $\begin{bmatrix} a_1^{'} & a_2^{'} & a_3^{'} \end{bmatrix}^T$ 로 변경하게 된다.

\begin{equation} \begin{bmatrix} \mathbf{e}_1 & \mathbf{e}_2 & \mathbf{e}_3 \end{bmatrix} \begin{bmatrix} a_1 \\ a_2 \\ a_3 \end{bmatrix} =  \begin{bmatrix} \mathbf{e}^{'}_1 & \mathbf{e}^{'}_2 & \mathbf{e}^{'}_3 \end{bmatrix} \begin{bmatrix} a_1^{'} \\ a_2^{'} \\ a_3^{'} \end{bmatrix} \end{equation}

직관적인 설명

멈춰 있는 상태에서 어떤 물건을 보고 시점 기준 대략적으로 (x, y, z) 위치를 생각해 보자. 그 이후 고개를 좌우나 상하로 회전하여 그 물체를 다시 보면 시점 기준 새로운 (x’, y’, z’) 위치를 생각해 볼 수 있다.

 

두 좌표 간의 관계를 설명하기 위해 (2)번 방정식의 양변에 곱하여 base를 단위 행렬(identity matrix)로 바꾼다.

\begin{equation} \begin{aligned} \begin{bmatrix} a_1 \\ a_2 \\ a_3 \end{bmatrix} &= \begin{bmatrix} \mathbf{e}_1^T \\ \mathbf{e}_2^T \\ \mathbf{e}_3^T \end{bmatrix} \begin{bmatrix} \mathbf{e}^{'}_1 & \mathbf{e}^{'}_2 & \mathbf{e}^{'}_3 \end{bmatrix} \begin{bmatrix} a_1^{'} \\ a_2^{'} \\ a_3^{'} \end{bmatrix} \\ &= \begin{bmatrix} \mathbf{e}_1^T\mathbf{e}^{'}_1 & \mathbf{e}_1^T\mathbf{e}^{'}_2 & \mathbf{e}_1^T\mathbf{e}^{'}_3 \\ \mathbf{e}_2^T\mathbf{e}^{'}_1 & \mathbf{e}_2^T\mathbf{e}^{'}_2 & \mathbf{e}_2^T\mathbf{e}^{'}_3 \\ \mathbf{e}_3^T\mathbf{e}^{'}_1 & \mathbf{e}_3^T\mathbf{e}^{'}_2 & \mathbf{e}_3^T\mathbf{e}^{'}_3 \\ \end{bmatrix} \begin{bmatrix} a_1^{'} \\ a_2^{'} \\ a_3^{'} \end{bmatrix} =\mathbf{R}\mathbf{a}^{'} \end{aligned} \end{equation}

$\mathbf{R}$ 는 회전 자체를 설명한다고 말할 수 있으며, 이를 회전행렬이라고 한다. 동시에 이 행렬의 구성 요소는 두 좌표계 기준의 내적이며 길이가 1인 unit vector 이기 때문에 각 기본 벡터 각도의 cos값이다. 따라서 이 행렬을 방향 코사인 행렬(DIrection Cosine Matrix)라고 부르기도 한다.

Rotation Matrix의 특성

Rotation Matrix는 몇 가지 중요한 특성을 가지고 있다.

1. determinant 값이 1인 직교행렬이다. $det(\mathbf{R}) = 1$
2. 회전행렬의 inverse는 transpose와 같다. $\mathbf{R}\mathbf{R}^T = \mathbf{I}$

$n$ 차원의 회전 행렬 컬랙션을 다음과 같이 정의할 수 있다.

\begin{equation} SO(n) = \left\{ \mathbf{R} \in \mathbb{R}^{n \times n} \, \middle| \, \mathbf{R}^T\mathbf{R} = I, \, \det(\mathbf{R}) = 1 \right\} \end{equation}

$SO(n)$는 특수 직교 군(Special Orthogonal Group)을 뜻한다. 만약 $SO(3)$ 라면 3차원 공간에서의 회전을 의미한다.

Transform Matrix

유클리드 공간에서의 회전과 병진을 동차 좌표와 변환 행렬을 사용하여 다음과 같은 식으로 표현 가능하다.

\begin{equation}  \begin{bmatrix}  \mathbf{a}^{'} \\  1  \end{bmatrix} =   \begin{bmatrix}  \mathbf{R} & \mathbf{t} \\  \mathbf{0}^T & 1  \end{bmatrix}  \begin{bmatrix}  \mathbf{a} \\  1  \end{bmatrix} =  \mathbf{T}\begin{bmatrix}  \mathbf{a} \\  1  \end{bmatrix}  \end{equation}

3차원 vector 끝에 1을 추가하여 동차 좌표라고 하는 4차원 벡터로 변환한다. 4차원 벡터의 경우 행렬에 회전 및 병진을 포함하여 작성할 수 있다. 이 행렬 $\mathbf{T}$를 변환 행렬(Transform Matrix) 라고 한다. Transform Matrix는 왼쪽 위 모서리는 회전 행렬, 오른쪽은 병진 벡터, 왼쪽 아래 모서리는 0 벡터, 오른쪽 아래 모서리는 1이다 이와 같은 행렬을 특수 유클리드 군(Special Euclidean Group)이라고 하며 $SE(3)$으로 표기한다.

\begin{equation} SE(3) = \left\{ \mathbf{T} \in \mathbb{R}^{4 \times 4} \, \middle| \, T = \begin{bmatrix} \mathbf{R} & \mathbf{t} \\ 0 & 1 \end{bmatrix}, \, \mathbf{R} \in SO(3), \, \mathbf{t} \in \mathbb{R}^3 \right\} \end{equation}

$SE(3)$의 역행렬은 다음과 같다.

\begin{equation} \mathbf{T}^{-1} = \begin{bmatrix} \mathbf{R}^T & -\mathbf{R}^T\mathbf{t} \\ 0 & 1 \end{bmatrix} \end{equation}

Axis-angle

행렬로 회전을 표현하면 2가지 문제점이 있다.

1. 중복되는 요소: 회전을 표현하기 위해 9가지 요소를 사용하지만 3D 회전은 3 DOF 만 회전한다. 즉 회전행렬에는 중복되는 요소가 포함되어 있다.
2. 제약조건: 회전 및 변환 행렬은 직교 행렬이어야 하며 행렬식은 1이어야 한다. 이런 제약조건은 추정 및 최적화를 어렵게 만든다. (추 후 Lie 대수에서 다룰 예정)

회전축과 동일한 방향의 벡터를 사용하고 길이는 회전하는 각도와 같게 할 수 있다. 이를 회전 벡터(axis-angle)이라고 한다. 회전을 표현하기 위해 3차원의 벡터만 필요하다.

Axis-angle 표현

회전 행렬 $\mathbf{R}$을 회전 벡터로 표현한다면 회전축 $\mathbf{n}$과 회전 각도 $\theta$ 가 있다고 가정하면 $\theta\mathbf{n}$ 로 회전을 표현할 수 있다. 회전 벡터에서 회전 행렬로 변환하는 프로세스는 로드리게스 수식(Rodriges’s Formula)에 의해 이루어 진다.

\begin{equation} \mathbf{R} = cos\theta\mathbf{I} + (1 - cos\theta)\mathbf{n}\mathbf{n}^T + sin\theta\mathbf{n^{\wedge}} \end{equation}

Rodrigues' rotation formula - Wikipedia

반대로 회전 행렬에서 회전 벡터로 변환을 계산할 수도 있다.

\begin{equation}  \begin{aligned}  tr(\mathbf{R}) &= cos\theta tr(\mathbf{I}) + (1-cos\theta)tr(\mathbf{n}\mathbf{n}^T) + sin\theta tr(\mathbf{n}^{\wedge}) \\  &= 3cos\theta + (1 - cos\theta) \\  &= 1 + 2cos\theta  \end{aligned}  \end{equation}

\begin{equation} \theta = arccos\frac{tr(\mathbf{R}) - 1}{2} \end{equation}

회전축 $\mathbf{n}$ 의 경우는 unit vector이다. 동일한 회전을 한다면 회전축이 같다고 생각해 볼 수 있다. 따라서 Rotation matrix의 고유값이 1인 vector가 $\mathbf{n}$ 이 된다.

\begin{equation} \mathbf{R}\mathbf{n} = \mathbf{n} \end{equation}

Euler Angle

오일러 각도는 서로 다른 3개의 축을 이용해 회전을 분해하여 표현하는 방법이다. 회전하는 순서에 따라 다양한 정의 방법이 있다. 예를 들어 $X$축 중심 회전 → $Y$축 중심 회전 → $Z$축 중심 회전하면 $XYZ$축 회전이 발생한다. 마찬가지로 $ZYZ, ZYX$등 다양한 회전 방법을 정의할 수 있다. 일반적으로는 $ZYX$ 회전 (yaw-pitch-roll)이 사용된다.

Euler Angle 단점

오일러 각도는 직관적이긴 하나 짐벌락(Gimbal Lock)문제가 발생하는 큰 문제점이 있다. 회전하는 순서에 따라 자유도를 하나 잃게 된다. 이를 특이점(singularity) 문제라고 한다. 이론적으로는 3차원 회전을 3개의 실수로 표현하려는 경우 필연적으로 특이점 문제가 발생할 수 있음을 증명할 수 있다.

Quaternion

4차원 (한 개의 실수부와 3개의 허수부)로 회전을 표현하는 방식으로 특이점(singularity)이 존재하지 않는다. 쿼터니언과 복소수를 비교하면 이해하는데 도움이 된다. 극좌표계에서 복소수 벡터 $\mathbf{e}^{i\theta}$ 를 표현해 보자.

\begin{equation} \mathbf{e}^{i\theta} = cos\theta + isin\theta \end{equation}

이것은 길이가 1인 복소수이다. 길이가 1인 복소수를 곱하면 길이는 유지되고 각도만 변하는 것을 확인할 수 있다.

예제) 단위 복소수 곱을 통한 회전

어떠 한 복소수 $C = X + iY$ 가 있다고 생각해 보자. 이때 한 단위 복소수를 곱한다.

$$ C\mathbf{e}^{i\theta} = Xcos\theta - Ysin\theta + i(Xsin\theta + Ycos\theta) $$

위 식을 실수부와 허수부로 나누어 행렬식으로 표현해 보자.

$$ X^{'} = Xcos\theta - Ysin\theta \\ Y^{'} = Xsin\theta + Ycos\theta \\ \begin{bmatrix} X^{'} & Y^{'} \end{bmatrix} = \begin{bmatrix} X & Y \end{bmatrix} \begin{bmatrix} cos\theta & sin\theta \\ -sin\theta & cos\theta \end{bmatrix}  $$

2D rotation matrix를 곱한 것과 똑같은 결과가 나오게 된다.

 

마찬가지로 3차원 회전은 단위 쿼터니언으로 설명할 수 있다.

\begin{equation} \mathbf{q} = q_0 + q_ii + q_2j + q_3k \end{equation}

$i, j, k$는 쿼터니언의 3개의 허수이다. 3개의 허수는 다음과 같은 관계를 충족해야 한다.

\begin{equation}  \begin{aligned}  i^2 &= j^2 = k^2 = 1 \\  ij &=k, ji = -k \\  jk &= i, kj = -i \\  ki &= j, ik = -j \\  \end{aligned}  \end{equation}

$i, j, k$를 3개의 축으로 생각해 보면 자기 자신과 곱하는 것은 복소수처럼 보이고 다른것과 곱하는 것은 외적과 비슷하게 보인다. (x축과 y축의 외적을 통해서 z축이 나옴) 때에 따라 스칼라와 벡터를 사용하여 쿼터니언을 표기하기도 한다.

\begin{equation}  \begin{aligned}  \mathbf{q} = \begin{bmatrix}s, \mathbf{v}\end{bmatrix}^T, & s = q_0 \in \mathbb{R}, & \mathbf{v} = \begin{bmatrix}q_1 & q_2 & q_3\end{bmatrix}^T \in \mathbb{R}^3  \end{aligned}  \end{equation}

$s$는 쿼터니언의 실수부이며 $\mathbf{v}$는 허수부이다.

• real quaternion: 쿼터니언의 허수부가 0인 경우
• imaginary quaternion: 쿼터니언의 실수부가 0인 경우

3차원 공간에서의 회전은 단위 쿼터니언(unit quaternion)으로 나타낼 수 있지만 이 표현은 복소수와는 미묘하게 다르다. 복소수에서는 $i$ 를 곱하면 90도 회전된다. 하지만 quaternion에서 $ij = k$의 특성을 보장하려면 $i$를 곱하는 것은 180도 회전에 해당한다. $i^2 = -1$은 $i$ 축으로 360도 회전한 후 반대 방향이 된다는 것을 의미한다. 즉 원래의 모습과 같기 위해서는 720도를 회전해야 한다는 것과 같다. (quaternion의 double cover 특성)

Quaternion 연산

1. 덧셈과 뺄셈

\begin{equation}  \mathbf{q}_a \pm \mathbf{q}_b = \begin{bmatrix}  s_a \pm s_b & \mathbf{v}_a \pm \mathbf{v}_b  \end{bmatrix}^T  \end{equation}

2. 곱셈

\begin{equation}  \begin{aligned}  \mathbf{q}_a\mathbf{q}_b &= (s_as_b - x_ax_b - y_ay_b - z_az_b) \\  &+ (s_ax_b + x_as_b + y_az_b - z_ay_b)i \\  &+ (s_ay_b - x_az_b + y_as_b + z_ax_b)j \\  &+ (s_az_b + x_ay_b - y_ax_b + z_as_b)k  \end{aligned}  \end{equation}

\begin{equation}  \mathbf{q}_a\mathbf{q}_b = \begin{bmatrix}  s_as_b - \mathbf{v}_a^T\mathbf{v}_b & s_a\mathbf{v}_b + s_b\mathbf{v}_a + \mathbf{v}_a \times \mathbf{v}_b  \end{bmatrix}^T  \end{equation}

3. 길이

\begin{equation} ||\mathbf{q}_a|| = \sqrt{s_a^2 + x_a^2 + y_a^2 + z_a^2} \end{equation}

4. 역

\begin{equation} \mathbf{q}^{-1} = \mathbf{q}^* / ||\mathbf{q}||^2 \end{equation}

Quaternion을 이용한 회전 표현

3차원 공간상의 점 $\mathbf{p} = \begin{bmatrix}x & y & z\end{bmatrix} \in \mathbb{R}^3$과 단위 쿼터니언 $\mathbf{q}$로 지정된 회전이 있다고 가정하면 3차원 점 $\mathbf{p}$는 회전한 후 $\mathbf{p}^{'}$가 된다. 행렬을 사용하는 경우 $\mathbf{p}^{'} = \mathbf{Rp}$가  된다. 쿼터니언으로 표현하면 어떻게 될까? 3차원 공간 점을 허수부 쿼터니언으로 확장하자.

$$ \mathbf{p} = \begin{bmatrix} 0 & x & y & z \end{bmatrix}^T = \begin{bmatrix} 0 & \mathbf{v} \end{bmatrix}^T $$

이는 쿼터니언의 실수부는 0으로 그리고 3개의 허수부는 공간의 3축에 해당하는 것과 같다. 그런 다음 회전된 점 $\mathbf{p}^{'}$는 다음과 같은 곱으로 나타낼 수 있다.

\begin{equation} \mathbf{p}^{'} =\mathbf{qpq}^{-1} \end{equation}

Quaternion 참고 자료

Quaternion 회전을 이해하는 것은 어렵다. 나는 아래 영상이 Quaternion을 이해하는데 도움이 됐다.

ROS에서는 어떻게 회전을 표현할까?

ROS에서는 주로 Quaternion으로 회전을 표현하고 있다. 그 이유는 저장해야 될 값이 4가지 밖에 없으며, Rotation matrix와의 변환이 Axis-angle에 비해 간단하기 때문이다. std message를 확인해 보자.

msg/Pose Documentation

Geometry_msgs에서 로봇의 pose 혹은 trajectory를 표현할 때 사용되는 message이다. Quaternion으로 Orentation을 표현하고 있음을 알 수 있다. 뿐 아니라 회전을 Rotation matrix나 euler-angle로 변경하는 Library도 제공해 준다. (tf library)