Points, Vectors, and Coordinate Systems
로봇의 3차원 운동을 설명하려면, 우선 가장 기본적인 개념인 점, 벡터, 그리고 좌표계에서부터 시작해야 한다. 3차원 공간에서 점은 3개의 수로 표현될 수 있다.
벡터는 점과 마찬가지로 3개의 수로 표현되지만, 한 점에서 다른 점으로의 방향과 거리를 의미한다.
$\mathbf{a} = \begin{bmatrix} a_1 \\ a_2 \\ a_2 \end{bmatrix}$
3차원 공간에서 좌표계는 3개의 base $\mathbf{e}_1, \mathbf{e}_2, \mathbf{e}_3$로 구성된다. 좌표계의 base가 정의됨으로 인하여 벡터는 좌표를 가지게 된다.
$\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$
즉, 벡터 그 자체뿐만 아니라 base의 선택에도 벡터의 좌표는 영향을 받는다.
벡터의 외적에서 한 가지 알아 두면 좋은 것이 있는데, 외적을 행렬과 벡터의 곱으로 표현하는 것이다. 이를 통해 벡터 연산을 쉽게 할 수 있다.
$\mathbf{a} \times \mathbf{b} = \begin{Vmatrix} \mathbf{e}_1 & \mathbf{e}_2 & \mathbf{e}_3 \\ a_1 & a_2 & a_3 \\ b_1 & b_2 & b_3 \end{Vmatrix} = \begin{bmatrix} a_2b_3 - a_3b_2 \\ a_3b_1 - a_1b_3 \\ a_1b_2 - a_2b_1 \end{bmatrix} = \begin{bmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{bmatrix} \begin{bmatrix} b_1 \\ b_2 \\ b_3 \end{bmatrix}$
$\mathbf{a}^\wedge = \begin{bmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{bmatrix}$
Euclidean Transforms Between Coordinate Systems
로보틱스에서는 주변환경을 기준으로 로봇 몸체, 로봇 팔, 그리고 센서들이 복합적으로 움직인다. 이로 인해 외부 좌표계로 모든 움직임을 표현하는 것은 어렵다. 로봇 몸체가 이동하면 그에 따른 로봇팔, 관절, 센서들의 위치가 전부 바뀌기 때문이다. 따라서 로봇의 각 부분에 개별적인 좌표계를 부여하고, 연결된 부품들 간의 좌표계 변환을 통해 로봇의 움직임을 간단히 표현할 수 있다.
이로 인해 하나의 벡터임에도 카메라 좌표계, 로봇 몸체 좌표계, 월드 좌표계에서 각각 다른 좌표를 가질 수 있다. 예를 들어, 월드 좌표계에서는 $\begin{bmatrix} 1 & 1 & 1 \end{bmatrix}^T$이었던 벡터가 카메라 좌표계에서는 $\begin{bmatrix} \sqrt{3} & 0 & 0 \end{bmatrix}^T$이 될 수 있다. 즉, 좌표계의 base가 달라지는 것이다.
따라서, 카메라 좌표계를 기준으로 한 이미지, 라이다 좌표계를 기준으로 한 pointcloud, SLAM의 결과물로 나온 월드 좌표계 등을 이용하여 종합적인 로봇 알고리즘을 짜려면, 한 좌표계에서 다른 좌표계로의 변환 과정을 알아야 한다. 그리고 이 변환을 하나의 transform matrix $\mathbf{T}$로 표현할 수 있다.
Rotation Matrix
직관적으로, 한 좌표계에서 다른 좌표계로의 변환은 rotation과 translation으로 나눌 수 있다. 변환 과정에서 벡터의 크기나 벡터 간의 각도는 변하지 않는다. 우선 rotation에 대해 살펴보자.
기존 좌표계의 base를 $\mathbf{e}_1, \mathbf{e}_2, \mathbf{e}_3$, 새로운 좌표계의 base를 $\mathbf{e}_1', \mathbf{e}_2', \mathbf{e}_3'$라고 하자. 이때 기존 좌표계상의 벡터 $\mathbf{a} = a_1\mathbf{e}_1 + a_2\mathbf{e}_2 + a_3\mathbf{e}_3$를 정의할 수 있다. 제3자의 입장에서 볼 때 좌표계만 회전하고 벡터의 크기나 각도는 변하지 않는다고 가정하면 (로봇이 제자리에서 회전하는 상황에서 로봇과 목적지까지의 벡터를 생각할 수 있다), 새로운 좌표계에서의 벡터는 $\mathbf{a} = a_1'\mathbf{e}_1' + a_2'\mathbf{e}_2' + a_3'\mathbf{e}_3'$로 표현할 수 있다.
$\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}$
양변에 $\begin{bmatrix} \mathbf{e}_1^T \\ \mathbf{e}_2^T \\ \mathbf{e}_3^T \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}_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} 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}$
가 된다. 이때 우변의 3x3 행렬을 rotation matrix $\mathbf{R}^T$이라고 정의할수 있다.
$\mathbf{R}^T = \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}^T \begin{bmatrix} a_1' \\ a_2' \\ a_3'
\end{bmatrix}$
즉, rotation matrix는 기존 좌표계의 base와 새로운 좌표계의 base 사이의 관계를 나타낸다. 제3자의 시점에서 동일한 벡터를 표현할 때, 변환된 좌표계에서의 벡터 좌표에 rotation matrix를 곱하면 기존 좌표계에서의 벡터 좌표를 얻을 수 있다.
rotation matrix는 다음과 같은 특성을 가진다.
- orthogonal: $\mathbf{R}^T\mathbf{R} = \mathbf{R}\mathbf{R}^T = \mathbf{I}$
- det($\mathbf{R}$) = 1
즉, $\mathbf{R}^T$는 $\mathbf{R}$과 방향이 반대인 회전을 나타낸다.
Translation Vector
Translation vector $\mathbf{t}$를 이용하여 벡터의 위치를 변화시킬 수 있다.
$\begin{bmatrix} a_1' \\ a_2' \\ a_3' \end{bmatrix} = \mathbf{R} \begin{bmatrix} a_1 \\ a_2 \\ a_3 \end{bmatrix} + \mathbf{t}$
Notation
좌표계 1과 2가 있고, 하나의 벡터 $\mathbf{a}$를 각 좌표계에서 본 벡터를 $\mathbf{a}_1$, $\mathbf{a}_2$라 하자. 이때 $\mathbf{a}_1$과 $\mathbf{a}_2$ 사이의 변환은 아래의 수식으로 표현된다.
$\mathbf{a}_1 = \mathbf{R}_{12}\mathbf{a}_2 + \mathbf{t}_{12}$
즉, 좌표계 2에서의 벡터를 좌표계 1로 변환하기 때문에 $\mathbf{R}_{12}$의 아래 첨자 12는 오른쪽에서 왼쪽으로 읽어서 좌표계 2에서 좌표계 1로 변환한다는 것을 의미한다.
Transform Matrix and Homogeneous Coordinates
3D 벡터의 끝에 1을 추가하여 4차원 벡터로 만들면, rotation과 translation을 하나의 행렬로 표현할 수 있다. 이처럼 1이 추가되어 4차원 벡터로 만들어진 좌표를 homogeneous coordinates라고 하고, 이를 이용하여 rotation과 translation이 결합된 행렬을 transform matrix $\mathbf{T}$라고 한다.
$\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}$
이처럼 homogeneous coordinates를 이용하면 transformation 계산을 간편하게 할 수 있다. 이뿐만 아니라, 두 개 이상의 transformation을 결합할 때도 편리하다.
$\mathbf{b} = \mathbf{T}_1\mathbf{a}, \quad \mathbf{c} = \mathbf{T}_2\mathbf{b} \quad \Rightarrow \mathbf{c} = \mathbf{T}_2\mathbf{T}_1\mathbf{a}$
Transform matrix는 그 역행렬이 역방향의 transformation을 나타낸다는 특성이 있다.
$\mathbf{T}^{-1} = \begin{bmatrix} \mathbf{R}^T & -\mathbf{R}^T\mathbf{t} \\ \mathbf{0}^T & 1 \end{bmatrix}$
또한, rotation matrix와 마찬가지로 $\mathbf{T}_{12}$의 아래 첨자 12는 오른쪽에서 왼쪽으로 읽어서 좌표계 2에서 좌표계 1로 변환한다는 것을 의미한다.