[Rigid Body Motions] Linear & Angular Velocities

[Transformation Matrices] Homogeneous Transformation

위의 글을 작년 10월에 쓴 이후로 로봇 관련 글을 하나도 쓰지 않았다.. 스스로를 반성하며 나의 전공에 관한 글도 꾸준히 쓰겠다는 다짐을 해본다.

이번 글에서는 로봇과 같은 rigid body system의 속도, 그 중에서도 각속도를 중심으로 앞서 배운 matrix들을 이용해 표현하는 방법에 대해 정리해보고자 한다.

---

\(\{w\}\)는 world frame, \(\{e\}\)는 end-effector frame.

설명을 위해 위의 그림을 참고하여 설명하겠다.

End-effector의 displacement vector는 \(r^{w}_{e} \in \mathbb{R}^{3}\), rotation matrix는 \( R^{w}_{e} \in SO(3) \)로 정의된다.

- Body Linear Velocity

Linear velocity (선속도)는 세 축 방향 displacement를 시간으로 미분하는 방법을 통해 어렵지 않게 구할 수 있다.

$$ \text{Linear velocity:} \;\, \dot{r}^{w}_{e} = \frac{d}{dt}r^{w}_{e} \in \mathbb{R}^{3} $$

Body frame (위의 예시에서는 end-effector 좌표)에서 속도를 표현하는 방법은 아래와 같다.

$$ \text{Body linear velocity:} \;\, [R^{w}_{e}]^{T} \dot{r}^{w}_{e} \in \mathbb{R}^{3} $$

Body linear velocity를 "Linear velocity of end-effector frame (i.e., \( \{e\}\)) relative to the world frame (i.e., \( \{w\} \)) with respect to the end-effector frame"으로 표현한다. 

 

- Body Angular Velocity

그렇지만 angular velocity는 rotation matrix를 바로 시간 미분하는 것으로 구할 수 없다. 대신 rotation matrix의 특성을 이용한다.

$$ R^{T}R = I \Rightarrow \frac{d}{dt}(R^{T}R) = \dot{R}^{T}R + R^{T}\dot{R} = 0 \Rightarrow \mathbf{\dot{R}^{T}R = -R^{T}\dot{R} = - (\dot{R}^{T}R)^{T}} $$

여기서 \( A^{T} + A = 0 \)인 행렬을 skew-symmetric matix라고 한다. 이는 \(A\)의 요소 \(a_{ij} = -a_{ji} \)와 같다. (\(A\)는 square matrix)

 

Skew-symmetric matrix의 집합을 \( so(3) \)로 쓰며, 수학적인 정의는 \( so(3) = \{ S\in\mathbb{R}^{3 \times 3} | S + S^T = 0\} \)이며, 앞서 공부한 \( SO(3) \)와는 다르다. 

3차원의 skew-symmetric matrix를 간단하게 표현하기 위한 방법으로 vee/hat operator가 있다. 

Skew-symmetric matrix와 vee/hat operator는 외적 (cross product)를 표현하는 것과 같은 기본적인 수학에서도 많이 사용되고 이후에 다루게 될 내용에서도 많이 나오기 때문에 잘 기억해두면 좋을 것이다.

위에서 확인한 \( R^{T}\dot{R} \)의 skew-symmetricity가 rigid body의 angular velocity 계산에 사용될 수 있다.

 

시간에 따라 회전하는 rigid body (rotation matrix가 시간에 대한 함수)에 대해, 고정된 reference frame (world frame)에서 body frame으로의 rotation matrix를 다시 쓰면, \( R(t) = [r_{1}(t) \quad r_{2}(t) \quad r_{3}(t)]\)이다. 이 때 \(r_{1}(t), \; r_{2}(t), \; r_{3}(t) \)는 각각 body frame의 \(x, \;y, \;z\)축의 reference frame에 대한 unit vector이다.

Rotation에 의해 발생하는 linear velocity는 \( \dot{r}_{i} = \omega_{s} \times r_{i}\)이며, 이는 \( r_{1} (t), \;r_{2} (t), \;r_{3} (t) \)에도 적용이 가능하다. 그러면 \( R ̇= \omega_{s} \times R = \hat{\omega}_{s} R\)이 된다. \( \omega_{s} \)는 reference frame에서 표현되는 angular velocity이다.

 

여기서 앞서 구했던 skew-symmetric matrix처럼 만들어 주기 위해 양변의 뒤에 \( R^T \)를 곱한다.

$$ \dot{R}R^{T} = \hat{\omega}_{s} R R^{T} = \hat{\omega}_{s} RR^{-1} = \hat{\omega}_{s}$$

 

\(R^T\)를 앞에 곱해주면 다음과 같다.

$$ R^{T}\dot{R} = R^{T} \hat{\omega}_{s} R = \widehat{R^{T}\omega_{s}} $$

 

그런데 \( R^{T}\omega_{s} = \omega_{b} \)이므로 (\( \omega_{b} \)는 body frame에서의 angular velocity), hat operator와 함께 정리하면 다음과 같이 정리 가능하다.

$$ \hat{\omega}_{b} = \widehat{R^{T}\omega_{s}} = R^{T}\hat{\omega}_{s}R = R^{T}(\dot{R}R^{T})R = R^{T} \dot{R} $$

 

정리하면, \( \mathbf{\dot{R}R^{T} = \hat{\omega}_{s}, \: R^{T}\dot{R} = \hat{\omega}_{b}} \)가 된다.

 

- Simple example in 2D

\( R^{w}_{tar} = \begin{bmatrix} cos\theta & -sin\theta \\ sin\theta & cos\theta \end{bmatrix} \)

 

\( \dot{R}^{w}_{tar} = \frac{d}{dt}\left(\begin{bmatrix} cos\theta & -sin\theta \\ sin\theta & cos\theta \end{bmatrix}\right)  = \begin{bmatrix} -sin\theta & -cos\theta \\ cos\theta & -sin\theta \end{bmatrix}\dot{\theta} \)

 

\( (R^{w}_tar)^{T} \dot{R}^{w}_{tar} = \begin{bmatrix} 0 & 1 \\ -1 & 0 \end{bmatrix} \dot{\theta} \)

 

$$ \therefore \omega = \dot{\theta} $$

- Simple example in 3D

\( R = \begin{bmatrix} cos\theta & -sin\theta & 0 \\ sin\theta & cos\theta & 0 \\ 0 & 0 & 1 \end{bmatrix} = rot(z,\theta)\)에 대해,

$$ \dot{R}R^T = \begin{bmatrix} -\dot{\theta}sin\theta & -\dot{\theta}cos\theta & 0 \\ \dot{\theta}cos\theta & -\dot{\theta}sin\theta & 0 \\ 0 & 0 & 0 \end{bmatrix} \begin{bmatrix} cos\theta & sin\theta & 0 \\ -sin\theta & cos\theta & 0 \\ 0 & 0 & 1 \end{bmatrix} = \begin{bmatrix} 0 & -\dot{\theta} & 0 \\ \dot{\theta} & 0 & 0 \\ 0 & 0 & 0 \end{bmatrix}$$

 

정리된 행렬이 skew-symmetric matrix임을 확인했다. 여기에 vee operator를 쓰면,

$$ (\dot{R}R^T)^{\vee} = \begin{bmatrix} 0 \\ 0 \\ \dot{\theta} \end{bmatrix} $$

 

---

Rigid body의 linear/angular velocity를 rotation matrix로 표현하는 방법에 대해 정리하였다. Skew-symmetric matrix를 포함하여 수학적인 내용이 많이 나왔다. 이 모든 수식을 기억하고 있을 필요는 없지만 이런 공식과 성질들을 사용하게 된 배경에 대해서는 기억하면 좋을 것 같다. (e.g., Rotation matrix를 이용하여 skew-symmetric matrix를 만들고 이를 이용해 body frame과 reference frame의 angular velocity를 구할 수 있다는 것, 등등)

\( R^{T}\hat{\omega}R = \widehat{R^{T}\omega} \)의 증명은 아래 글에 따로 작성하였으니 궁금하면 확인해보시길..

[Rotation Matrix] Properties of vee/hat operator