lastnamesong
[Coordinate] Exponential Coordinates 본문
이전 글들에서 회전을 표현하는 방법으로 \( SO(3) \)의 정의와 rotation matrix의 성질에 대해 알아보았다. Rotation matrix를 parameterize하는 방법으로는, roll-pitch-yaw (RPY), 오일러 각 (Euler angle)가 있었으며 각각에 대해서도 이전 글에 소개했던 바가 있다. 또한 rigid body의 twist를 rotation matrix의 미분과 skew-symmetric matrix로 표현하는 방법을 학습했다.
이번 글에서는 이를 위한 방법 중 하나인 exponential coordinate에 대해 정리하고 matrix exponential을 이용해 rigid body의 rotation을 구하는 방법을 소개한다.
Matrix exponential에 대해서는 별도의 글로 정리해두었으니 읽어보시길..
- Exponential Coordinates of Rotations & Rodrigues' Formula
아래 그림에서 unit magnitude를 가지는 회전축 \( \omega \)에 대해 시간 \( t \)에 따라 벡터 \( p \)가 회전하는 경우를 가정해보자. 이런 상황이 가능하기 위해 \( p(0) \)가 \( 1rad/s \)의 속도로 (\( \omega \)를 unit magnitude로 정의해서 이렇게 생각할 수 있다.) \( t = 0 \)에서 \( t = \theta \)까지 회전하는 것을 생각해볼 수 있다.
\( p \)의 속도는 회전원의 접선 방향으로 다음 수식과 같이 구할 수 있다.
$$ \dot{p}(t) = \omega \times p(t) = \hat{\omega}p(t) $$
위의 수식을 보면 1차 선형미분방정식의 형태 (\( \dot{x} = Ax\))를 갖는 것을 확인할 수 있으며, 그 해는 exponential하게 구해질 수 있다.
$$ p(t) = e^{\hat{\omega}t}p(0) = e^{\hat{\omega}\theta}p(0) $$
Matrix exponential을 테일러 급수 (Taylor series)를 이용해 아래와 같이 전개할 수 있다.
$$ e^{\hat{\omega}\theta} = I + \hat{\omega}\theta + \hat{\omega}^2\dfrac{\theta^2}{2!} + \hat{\omega}^3\dfrac{\theta^3}{3!} + \cdots $$
여기서 정의한 unit magnitude를 갖는 벡터의 skew-symmetric matrix는 \( \hat{\omega}^3 = -\hat{\omega} \)라는 특징을 갖는다. (다른 글에 정리해두었다.) 따라서 위의 수식에서 \( \hat{\omega}^3 \) 이상의 차수를 2차나 1차로 만들어버릴 수 있다. 차수를 낮추고 \( \hat{\omega}^2 \)과 \hat{\omega}\)로 묶어서 정리하면 다음과 같다.
$$ e^{\hat{\omega}\theta} = I + \left( \theta - \dfrac{\theta^3}{3!} + \dfrac{\theta^5}{5!} - \cdots \right)\hat{\omega} + \left( \dfrac{\theta^2}{2!} - \dfrac{\theta^4}{4!} + \dfrac{\theta^6}{6!} - \cdots \right)\hat{\omega}^2 $$
위 수식에서 \( \sin\theta \)와 \( \cos\theta \)의 Taylor expansion을 확인할 수 있다.
$$ \begin{matrix} \sin\theta = \theta - \dfrac{\theta^3}{3!} + \dfrac{\theta^5}{5!} - \cdots \\ \cos\theta = 1 - \dfrac{\theta^2}{2!} + \dfrac{\theta^4}{4!} - \cdots \end{matrix}$$
따라서 \( \omega \)를 축으로 \( \theta \)만큼의 회전인 \( e^{\hat{\omega}\theta} \)를 다음과 같이 정리할 수 있다.
$$ Rot(\omega, \, \theta) = e^{\hat{\omega}\theta} = I + \sin\theta\hat{\omega} + (1-\cos\theta)\hat{\omega}^2 \in SO(3) $$
시작할 때 주어진 조건 (i.e., \( \theta \in \mathbb{R}, \, \omega \in \mathbb{R}^3, || \omega || = 1 \))은 변함이 없어야 한다. 이를 Rodrigues' formula라고 부르기도 한다. 이렇게 구한 exponential matrix를 벡터에 곱하면 rotation을 의미한다는 것은 확인하였다. 구체적으로는 \( e^{\hat{\omega}\theta}p \)는 벡터 \( p \in \mathbb{R}^3 \)을 \( \theta \)만큼 fixed-frame의 축 \( \omega \)만큼 회전한 것이라고 볼 수 있다. 비슷하게 이 matrix exponential이 앞/뒤로 곱해지는 것에 대한 의미를 정리할 수있다.
- \( R^{'} = e^{\hat{\omega}\theta}R = Rot(\omega,\,\theta)R \): orientation achieved by rotation \( R \) by \( \theta \) about the axis \( \omega \) in the fixed frame
- \( R^{''} = Re^{\hat{\omega}\theta} = R\,Rot(\omega,\,\theta) \): orientation achieved by rotation \( R \) by \( \theta \) about the axis \( \omega \) in the body frame
Rodrigues' formula는 임의의 회전축을 기준으로 하는 회전에 대한 rotation matrix를 제공할 수 있다는 점에서 의미가 있다. 그리고 skew-symmetric matrix인 \( \hat{\omega}\theta \in so(3) \)에 대한 rotation matrix인 \( Rot(\omega, \, \theta) \)로의 mapping이라고 할 수도 있다. 그래서 다르게 표현하면 \( \xi = \omega \theta \in \mathbb{R}^3 \)에 대해 \( e^{\hat{\xi}} = I + \dfrac{\sin||\xi||}{||\xi ||}\hat{\xi} + \dfrac{(1-\cos||\xi||)}{||\xi||^2}\hat{\xi}^2 \in SO(3) \)로 쓸 수 있다.
이를 exponential mapping이라고 하며, 그의 반대 mapping인 lograithm map은 \( SO(3) \)를 \( so(3) \)로 변환하는 mapping이 된다.
- Logarithmic map of \( SO(3) \)
\( \omega = \begin{bmatrix} \omega_1 \\ \omega_2 \\ \omega_3 \end{bmatrix} \)에 대해 Rodrigues' formula를 다시 쓰면 아래와 같다.
$$ e^{\hat{\omega}\theta} = I + \sin\theta\hat{\omega} + (1-\cos\theta)\hat{\omega}^2 = \begin{bmatrix} \cos\theta + \omega^{2}_{1}(1-\cos\theta) & \omega_1 \omega_2 (1 - \cos\theta) - \omega_3 \sin\theta & \omega_{1}\omega_{3} (1 - \cos\theta) + \omega_2 \sin\theta \\ \omega_1 \omega_2 (1 - \cos\theta) - \omega_3 \sin\theta & \cos\theta + \omega^{2}_{2}(1-\cos\theta) & \omega_{2}\omega_{3} (1 - \cos\theta) + \omega_1 \sin\theta \\ \omega_{1}\omega_{3} (1 - \cos\theta) - \omega_2 \sin\theta & \omega_{2}\omega_{3} (1 - \cos\theta) + \omega_1 \sin\theta & \cos\theta + \omega^{2}_{3}(1-\cos\theta) \end{bmatrix} $$
이렇게 구한 rotation matrix를 \( R = \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix} \)로 정의하면 \( R \)의 요소들을 더하고 뺌으로써 \( \hat{\omega} \)와 \( \theta \)를 구할 수 있다.
1) \( \theta \)
행렬의 대각합 (trace)를 계산하면 다음과 같다.
$$ tr(R) = r_{11} + r_{22} + r_{33} = 3\cos\theta + (\omega^{2}_{1} + \omega^{2}_{2} + \omega^{2}_{3})(1-\cos\theta) = 1 + 2\cos\theta $$
정리하면 \( \theta = \cos^{-1}\left( \dfrac{tr(R)-1}{2} \right) \)가 된다. \( || \xi || \)가 angle of rotation을 나타내므로, 이 때의 \( \theta \)는 적당히 양수로 잡으면 된다.
몇 가지 예외적인 경우가 존재한다.
- \( R\, \)이 identity matrix (\( I \))일 때, \( \theta = 0 \)이 되고, \( \omega \)는 임의의 벡터가 가능하다.
- \( \theta = \pm \pi \)일 때 singularity가 발생한다.
2) \( \hat{\omega} \)
\( \omega \)를 구하기 위해 \( \sin\theta \)를 남겨볼 수 있다.
$$ \begin{matrix} r_{32} - r_{23} = 2\omega_1\sin\theta, & r_{13}-r_{31} = 2\omega_2\sin\theta, & r_{21}-r_{12} = 2\omega_3\sin\theta \end{matrix} $$
\( \sin\theta \neq 0 \)이면 \( \omega \)의 각 요소들을 구할 수 있다.
$$ \begin{matrix} \omega_1 = \dfrac{1}{2\sin\theta}(r_{32} - r_{23}), & \omega_2 = \dfrac{1}{2\sin\theta}(r_{13}-r_{31}), & \omega_3 = \dfrac{1}{2\sin\theta}(r_{21}-r_{12}) \end{matrix} $$
Skew-symmetric matrix로 정리하면 \( R \)과 \(\theta\)로 나타낼 수 있다.
$$ \hat{\omega} = \begin{bmatrix} 0 & -\omega_3 & \omega_2 \\ \omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0 \end{bmatrix} = \dfrac{1}{2\sin\theta}(R-R^T)$$
종합하면, \( \theta \)에 따라서 \( SO(3) \)의 logarithmic map을 정의할 수 있다.
- \( \theta = 0\) (equivalently, \(R = I \) or \(tr(R) = 3 \))
$$ log(R) = 0 \in \mathbb{R}^3 $$
회전이 없으므로 rotation matrix는 identitiy이다. - \( \theta \neq 0 \)
$$ \hat{\omega}=\dfrac{\theta}{2\sin\theta}(R-R^T) \quad where \quad \theta = \cos^{-1}\left(\dfrac{tr(R)-1}{2}\right) $$ - \( \theta = \pm \pi \) (equivalently, \(tr(R)=-1\))
$$ Singularity $$
\( +\pi \)와 \( -\pi \)의 결과가 같으므로 singularity가 발생한다. (로봇 입장에서 풀 수 없다.)
- Representation of \( SE(3) \)
Homogeneous transformation matrix group에 대해서도 exponential / logarithmic map이 가능하다.
\( \lambda = \begin{bmatrix} \eta \\ \xi \end{bmatrix} \in \mathbb{R}^6 \)이 위치와 회전을 표현하는 벡터라고 해보자. 이에 대한 homogeneous transformation은 \( \hat{\lambda}=\begin{bmatrix} \hat{\xi} & \eta \\ 0 & 1 \end{bmatrix} \in \mathbb{R}^{4 \times 4} \)가 된다. 이에 대한 Taylor series expansion을 수행하면 아래와 같다.
$$ exp(\hat{\lambda}) = I + \hat{\lambda} + \dfrac{\hat{\lambda}^2}{2!} + \dfrac{\hat{\lambda}^3}{3!} + \cdots = \begin{bmatrix} exp(\hat{\xi}) & A(\xi) \eta \\ 0 & 1 \end{bmatrix} $$
여기서, \( A(\xi) = I + \left( \dfrac{1-cos||\xi||}{||\xi||} \right) \dfrac{\hat{\xi}}{||\xi||} + \left( 1-\dfrac{sin||\xi||}{||\xi||} \right) \dfrac{\hat{\xi}^2}{||\xi||^2} \)이다.
Logarithmic map의 경우, \( T \in SE(3) \)에 대해 \( \lambda \in \mathbb{R}^6 \)로의 mapping을 정의할 수 있다.
\( SO(3) \)의 mapping과 마찬가지로 \(R\)의 조건에 따라 다르게 정리가 가능하다.
- \(R = I \): \(\xi = 0,\, \eta = r \)
- \( tr(R)=-1 \): singularity
- Otherwise: \( \xi = \log(R)^{\wedge},\, \eta = A^{-1}(\xi)r \)
where \( A^{-1}(\xi) = I-\dfrac{1}{2}\hat{\xi} + (1-\alpha(||\xi||))\dfrac{\hat{\xi}^2}{||\xi||^2} \), \( \alpha(y) = \dfrac{y}{2}\cot\dfrac{y}{2} \)
정리하면 다음과 같다.
속도 (미분)와 적분에 대해서는 따로 다루지는 않았다. 자세한 내용은 아래 참고한 원문(F. Nullo and R. M. Murray, "Proportional derivative (PD) control on the Euclidean group", CDS Technical Report, 1995)을 참고하시길
'Robotics > Geometry' 카테고리의 다른 글
| [Coordinate] Quaternion (쿼터니언, 사원수) (0) | 2024.07.26 |
|---|---|
| [Rigid Body Motions] Body Twist (0) | 2024.07.19 |
| [Rigid Body Motions] Linear & Angular Velocities (0) | 2024.07.16 |
| [Transformation Matrices] Homogeneous Transformation (2) | 2023.10.15 |
| [Coordinate] 오일러 각 (Euler Angle) (1) | 2023.10.06 |
