지난 시간에 이어서 이번 시간에는 차원을 확장하여 3차원 입체 공간에서의 움직임을 다루고자 하며, 3차원 공간에서 각도, 회전을 표현하는 방법에 대해 살펴봅시다.

• x-y축을 갖는 2차원 평면에서 회전 행렬은 다음과 같았습니다.

• 이제, 차원을 확장하여 3차원을 살펴보면, xyz축 각각에 대한 회전각 $\phi, \theta, \psi$ 이 필요하며, 이에 따라 회전 행렬도 3개가 필요함을 알 수 있습니다.

💡 y축에 대한 회전 행렬에서 원소의 부호에 주의합니다.

• 지금과 같이 3차원에서의 각도를 3개의 매개변수로 표현하는 방식을 “오일러 각도 체계”라고 부르며, 축의 순서에 따라 이를 표현하는 여러 조합이 있습니다. 우리는 이중에서 z-y-x 혹은 “3-2-1”이라고 불리는 조합을 사용하겠습니다. ( 어떤 조합을 사용해도 결과만 같으면 무관합니다.)

• 3차원 회전 행렬의 특징들을 살펴보고자 합니다. 첫째로, 회전 행렬의 특성상 자기 자신의 transpose와 곱해지면 identity matrix가 됩니다.
• 더불어, 2D space에서는 $R(\theta_1)R(\theta_2) = R(\theta_1+\theta_2)$ 즉, commutativity가 만족했지만, 3D space에서는 이러한 성질을 적용할 수 없습니다.

• 예를 들어, 아래와 같이 1) x축 90도 회전 2) y축 90도 회전의 결과는 1) y축 90도 회전 2) x축 90도 회전 결과와 전혀 다른 것을 확인 가능합니다. 이러한 특징 때문에 이번 챕터에서 회전에 대해서 자세히 살펴보고자 합니다.

• 앞으로 우리가 사용할 z-y-x 시스템에서는 frame 사이 변환을 아래와 같이 정리하여 사용합니다. body frame에서의 좌표 c3를 world frame 좌표 c0 기준으로 옮기기 위한 회전 행렬 R은 아래와 같이 구할 수 있는 것이지요.

• 실제 회전 행렬들의 곱셈을 모두 계산해보면 다음과 같습니다. (외울 필요는 전혀 없으며, 우리는 sympy를 사용할 것이기 때문에 걱정하지 않으셔도 됩니다.)

equation from : wikipedia

코드 구현

첫번째 살펴볼 코드는 rotation_of_a_box_321_euler.py 입니다. 실행부터 해볼까요?

python3 rotation_of_a_box_321_euler.py

그림에서 빨간색, 초록색, 파란색 막대는 각각 x, y, z 축을 의미합니다. 관습적으로, 로봇 시스템은 대부분 x축을 정면으로 둡니다. (비전 시스템은 z축을 정면으로 두지요.)

• main 함수를 살펴보면, 3개의 각도 $\phi, \theta, \psi$를 설정한 뒤 animate라는 함수에 대입하여 그림을 그리도록 하고 있습니다. (phi, theta, psi는 각각 x,y,z축 기준의 회전각이 됩니다.)

• animate 함수는 다음과 같은 구조를 갖습니다.

1. 꼭짓점을 통해 cube 정의하기
2. 정의된 cube에 3차원 회전을 적용하여 새로운 꼭짓점들을 계산하기
3. cube와 xyz 축, 글자를 시각화하기

• z-y-x 회전 조합을 사용하는 회전 행렬은 세 개의 matrix를 구해 행렬곱을 계산하여 구현합니다.

• 두번째 살펴볼 코드는 main_commutativity.py로, 3차원 회전에 대해 commutativity가 성립하지 않음을 확인하는 예시입니다.

python3 main_commutativity.py

그림에서 보는 바와 같이

1. phi 90도 회전 후 theta 90도 회전의 결과와
2. theta 90도 회전 후 phi 90도 회전의 결과는 전혀 다른 값을 갖습니다.

💡 두번째 예시 코드는 첫번째 예시의 응용이므로 코드 구현에 대한 설명은 생략하겠습니다.

Linear and Angular velocity in 3D space

대부분의 로봇들은 joint angle 제어가 이루어지기 때문에 joint의 각속도를 3차원의 선속도로 변환, 혹은 그 반대 작업이 필요합니다. 하지만 3차원에서의 각속도는 다소 직관적이지 못한 형태를 갖는데요. 차근차근 유도를 통해 이를 도출해보겠습니다.

• 2차원에서부터 시작해보면, 선속도는 각속도와 축의 방향벡터 r의 product form을 갖습니다. (혹은 Jacobian과 velocity state vector $\dot{q}$ 의 곱)

• 이 수식을 그대로 3차원으로 확장해보면, vector와 vector의 곱이 되기 때문에 cross product 계산을 수행해야 하는 복잡한 상황이 발생합니다.

• 벡터의 cross product를 좀 더 빠르게 계산할 수는 없을까요? cross product 시 좌측에 곱해지는 vector를 Skew Symmetric Matrix로 변환하면, Martix x Vector 형태로 정형화된 계산이 가능합니다. Skew Symmetric Matrix의 정의와 특징을 짚고 넘어가겠습니다.

이번에는 3차원에서의 각속도를 구하는 방법에 대해 알아봅시다. (단순히 하나의 축에 대해 회전하는 것이 아니라 자유롭게 회전하는 물체를 상상해보세요!)

• 3차원에서의 각속도는 단순히 각 축의 각속도를 구하고 더한 것이 아닙니다. 이렇게 표현해버리면 덧셈의 순서가 바뀌어도 같은 각속도를 가져야 하는데, 앞서 학습한 내용을 잘 기억해보면 3차원 각도는 commutative 하지 않았었지요.

$$ \vec{w} \neq \dot{\phi} \hat{i} + \dot{\theta} \hat{j} + \dot{\psi} \hat{k} $$

따라서, 이번 챕터에서는 3차원 각속도를 구하기 위한 증명 과정 2가지를 함께 살펴보고자 합니다. 결과는 같지만, 과정에서 차이를 갖게 됩니다.

Method 1

첫번째 방법은 Rotation Matrix의 성질과 Skew symmetric matrix의 특성을 이용하는 방식입니다.

• Rotation Matrix R의 특성을 나타내는 아래 식을 미분해봅시다. (행렬을 미분한다는 개념이 어색할 수도 있는데요. 이번 시간에는 복잡하게 생각하시지 말고 단순히 편미분을 계산한다고 생각하시면 됩니다.)

$$ RR^T = I \\ \dot{R} R^T + R \dot{R}^T = 0 $$

• 앞선 수식의 두번째 항에 강제로 transpose를 두 번 적용하고 식을 정리하면, $\dot{R}R^T$는 skew symmetric matrix임을 확인 가능합니다.

$$ \dot{R}R^T + ((R \dot{R}^T)^T)^T = 0 \\ \dot{R}R^T + (\dot{R} {R}^T)^T = 0 \\ S(a) + S(a)^T = 0 \quad \because S(a) = \dot{R}R^T $$

• skew symmetric matrix이기 때문에 양변의 오른쪽에 rotation martix $R$을 곱하면 아래와 같은 형태도 만들 수 있지요. ( Rotation Matrix의 미분 결과를 얻은 것입니다!)

$$\dot{R}R^T = S(a) \\ \dot{R} R^T R = S(a) R \\ \dot{R} = S(a) R $$

• 이번에는 body frame에서의 방향 벡터 $r_b$를 world frame으로 변환하는 아래 식을 양변 미분해보겠습니다.

• 앞서 구했던 Rotation Matrix의 미분이 등장했네요! 기쁜 마음으로 앞서 계산한 값을 대입한 뒤, 식을 정리해보겠습니다. 정리 후 반복되는 방향 벡터의 변환식을 정리하면, 각속도의 skew symmetric matrix, $S(w)$가 유도됩니다.

• 유도한 $S(w)$의 실질적인 값을 계산함으로 각속도 벡터 $w$를 계산해보겠습니다. z-y-x 각도 체계를 사용하기 때문에 rotation matrix를 세개의 항으로 분리하여 정리합니다.

Method 2

이번에는 보다 직관적인 접근으로 동일한 결과를 도출해보겠습니다.

• 현재 우리가 사용중인 z-y-x 각도 체계를 다시 한 번 생각해보면, z축의 각속도는 그대로 사용할 수 있고, theta의 중심이 되는 축은 초기 $\hat{j}$ 축을 z축으로 변환한 상태에서의 각속도임을, 그리고 phi의 중심이 되는 축은 초기 $\hat{i}$ 축을 z축으로 변환, y축으로 다시 변환한 상태에서의 각속도임을 확인할 수 있습니다. (이 결과는 앞선 복잡한 유도 결과와 동일합니다.)

• 반대로 world frame에서의 euler angle과 velocity를 통해 body frame에서의 각속도를 다음과 같이 유도할 수 있습니다.

• 최종적으로 앞선 벡터 표현식을 계산하고 정리하면 아래와 같이 matrix * velocity 형식으로 표현되며, 결국 현재 좌표계에서 각 축에 대한 euler angle과 euler angular velocity가 주어질 시, world frame에서의 각속도, body frame에서의 각속도로 변환이 모두 가능한 것이지요.

• 반대로, 변환 행렬의 inverse를 사용하면 특정 frame에서 각속도가 주어졌을 때 euler angle을 구할 수 있습니다.

💡 하지만! 그 inverse 계산 시 determinant가 0이 되는 singularity가 발생할 수 있습니다. 이는 $cos(\theta) = 0$일때 발생하며, 이를 방지하기 위해 quarternion이라는 각도 체계가 사용됩니다.

코드 구현

이후 다양한 예시를 구현하기 위해 angular velocity를 sympy로 계산하는 코드를 구현해보겠습니다. world frame과 body frame에서의 각속도를 예시로 준비하였습니다.

• 연산을 위해 필요한 파트인 unit vector와 rotation matrix입니다.

• 수식에 맞추어 world frame angular veolocity와 body frame angular velocity를 계산합니다.

• 결과 출력 시 determinant도 추가하였는데요, 두 행렬 모두 determinant로 cos(theta)를 가짐을 알 수 있습니다. 때문에 theta가 +pi/2, -pi/2가 되어버리면 sigularity가 발생하는 것이지요.

• 각속도가 주어진 현 상황에서 joint의 방향 벡터를 알면 cross product를 통해 선속도까지 구할 수 있을 것입니다.