[SLAM] 1. 3D Rigid Body Transformation, 좌표계 변환

안녕하세요. 후니대디입니다.

이번에는 3D 좌표계와 좌표계 변환에 대해서 포스팅하겠습니다.

---

Fixd RPY axis angle

3차원 공간을 기준으로 알아보기 전에 쉽게 2차원 공간부터 살펴보죠.

rotation

위의 그림은 xy평면에서 z축 기준으로 하는 2차원 회전행렬(rotation matrix)을 표현한 것입니다. 만약 기존의 xy평면의 (a,b)라는 점(혹은 벡터)가 있다고 하면, z축으로 세타만큼 이동한 좌표에서는 (a,b)는 회전행렬과 (a,b)벡터의 곱셈을 통해 나온(a`,b`)라고 표현할 수 있습니다. 어떻게 보면 절대적인 (a,b)의 점이 이동한 것이 아닌 coordinate마다 해당 좌표를 표현하는 방법이 달라진 것 뿐입니다.

translation

이동행렬(translation)은 linear한 움직임으로 위와 같이 덧셈으로 표현할 수 있습니다.

이를 3차원으로 확장해봅시다. 그럼 각축마다의 회전행렬은 아래와 같이 표현할 수 있습니다. (사실 아래의 matrix는 Homogeneous 형식으로 되어있습니다. 밑에서 그에 대해 살짝 언급하겠습니다.)

3차원 회전행렬

회전행렬끼리의 곱은 닫혀있습니다. 간단한 예제를 아래 보도록 하겠습니다. 기준 좌표기준으로 x축으로 세타1, y축으로 세타2만큼 회전한 rotation matrix를 구하는 예제입니다.

간단하게 x축 회전행렬에 y축 회전행렬을 곱하면 해결됩니다. 하지만 실제 application에서는 매우 불편합니다. 이유는 다음 Euler Axis에서 보겠습니다.

---

Euler Axis

실제 회전 행렬, 이동 행렬을 적용하는 application에서는 위의 axis angle은 매우 불편합니다. 로봇팔을 예로 들면 base를 기준으로 end effector의 회전행렬을 알고 싶다고 하면 한번에 알 수는 없습니다. 각각의 조인트에서 엔코더를 통해 각도를 뽑아내고 차례차례 회전행렬을 구해나갑니다. 이 과정에서 엔코더의 각도는 인접한 조인트의 각도이기 때문에 base와의 상대적인 각도를 알기는 힘듭니다.(한번 더 계산이 필요합니다). 이런 상황에서 오일러 공식을 이용하면 손쉽게 계산이 가능합니다. 글로는 설명을 하기가 힘들어서 아래의 그림을 보시면 이해가 쉽습니다.

각각의 조인트의 각도를 알고 있는 상황에서 base와 endeffector의 회전행렬을 구하는 예제입니다. 회전행렬을 구할때 fixed aixs와는 다르게 이전에 변환된 좌표를 기준으로 회전을 합니다. 또한  fixed axis에서와는 다르게 왼쪽으로 회전행렬을 곱해갑니다. 만약 X축 -> Y축 -> Z축으로 회전한다면 ZYX 으로 곱셈이 이루어지게 됩니다. 오일러 각에 의한 회전은 항상 물체의 현재 모델 좌표계를 기준으로 합니다. 이 경우 유의해야 할 점은 회전의 결과 좌표축 방향이 바뀐다는 점입니다. 그렇기 때문에 순서가 바뀌면 안됩니다.아래는 일반화한 그림입니다.

 

 

 위의 표현에서 기존 3x3행렬과는 다르게 4x4행렬을 사용하였습니다. Homogeneous form이라고 하며 한차원 더 늘려서 나타내므로써 Rotation matrix와 translation을 한번에 표현하기 용이합니다. 이는 se3로 표현할 수 있고 se3끼리의 연산은 닫혀있습니다.

Homogeneous Form

(SO3, SE3에 대해서는 Lie Group 주체로 이후 SLAM 시리즈에서 다룰때 더 자세히 언급하겠습니다.)

[SLAM] 2.리군, 리대수(Lie Group, Lie Algebra)

 

물론 오일러 각도표현에도 문제가 존재합니다.

복합 변환의 결과값을 예측하기 어렵게 됩니다.

예를 들어, 어떤 물체를 오일러 각(10도, 20도, 30도)에 의해 회전하고 이어서 회전한 좌표축을 기준으로 다시 (40도, 50도, 60도)로 회전할 경우 이는 원래 물체를 (50도, 70도, 90도) 로 회전한 결과와는 다르게 됩니다. 그 이유는 이미 첫번째 회전변환에서 좌표축의 방향이 변했기 때문입니다.

또한 대표적인 문제인 짐벌락이 발생합니다.

 

짐벌락 video

---

Fix Axis와 Euler Axis 차이점 Recap

 두 axis의 차이는 변환된 Axis기준으로 회전을 하는지 고정된 axis를 기준으로 회전을 하는지의 차이이고 이에 따라 Rotation Matrix를 곱하는 위치가 달라집니다. 그림으로는 아래와 같습니다.

fix-axis, euler-axis

---

Rodrigues formula(로드리게스 회전변환)

로드리게스 화전변환은 X, Y, Z축이 아닌 임의의 축을 기준으로 회전 변환을 하고 싶을때 사용합니다. X, Y, Z축의 회전 변환을 통해 나온 벡터는 사실 임의의 회전축에 의한 변환으로 표시할 수 있습니다. 따라서 로드리게스 angle로 회전변환을 간단하게 표시할 수 있습니다.

아래는 임의의 벡터 V가 회전축 N을 기준으로 회전하는 상황에서 로드리게스 eq을 증명한 내용입니다.

 

---

Quaternion

 

정 의

쿼터니언은 해밀턴이 발견한 complex number입니다. 4개의 float로 이루어져 rotation matrix대비 연산이 가볍고 오류가 발생할 확률이 낮습니다.(짐벌락 등- 확률이 낮은거지 없는건 아닙니다.) 4개의 float를 사원수라고 하며, 아래와 같이 나타낼 수 있습니다.

쿼터니언 definition

---

연 산

다음으로 연산을 살펴보면 뎃셈은 기존 벡터처럼 하면 되서 특별할 건 없고, 곱셈에 대해 보겠습니다.

사원수는 곱셈에 대해 닫혀있습니다. 따라서 항끼리 곱셈을 진행하면 아래와 같습니다.

쿼터니언 곱셈1

나온 결과물을 좀 더 보기 좋게 정리하면 아래와 같습니다.

쿼터니언 곱셈2

---

특 성

복소수와 같이 켤레(Conjugate)개념이 있습니다. 허수부의 부호만 변경합니다.

켤레, 성질

크기는 다음과 같이 4차원 길이를 구하는 것과 같습니다.

norm

항등원과 역원의 정의입니다.

항등원 identity

역원 inverse

---

회 전

1. 임의의 단위벡터를 기준으로 세타만큼 회전하는 회전변환을 사원수로 나타내면 다음과 같습니다.

 

2. 오일러 회전 

오일러 eq

위와 같은 오일러 공식을 아래와 같이 사원수로 표현할 수 있습니다.

쿼터니언 & 오일러 각

cos이 실수부(크기), u*sin이 허수부 삼원수(방향)를 의미합니다.

 

*참고

 

3. 쿼터니언을 이용해 벡터 P를 회전하면 qpq-1로 표현 할 수 있습니다. 1번과 다른점은 벡터 P의 변화를 나타냅니다. 보통의 회전행렬과 같은 상황입니다. 결국 이 3번을 알기 위에 1,2번을 살펴본 것 입니다.

위에서 배운 내용들을 가지고 증명하면 다음과 같습니다.

사원수 회전 증명

참고로 허수부가 방향을 나타내는 부분이니, 실수부는 0이나오고 허수부와 rodrigues값과 비교를 하며 증명은 하였습니다.

 

4. 회전 행렬로의 변환

위에서 구한 회전 사원수를 rotation matrix로 표현하면 아래와 같습니다.

사원수 ->회전행렬 1

증명)

증명

위 matrix을 정리하면 위의 정리된 식과 동일합니다.(q0:w, q1:x,q2:y,q3:z)

*위키피디아 참고하였습니다.

Conversion between quaternions and Euler angles - Wikipedia

 

*참고

회전행렬(rotation matrix) -> 쿼터니언 

rotation matrix -> quaternion

유클리드 공간에 대해 더 깊이있게 알고 싶으신 분은 아래 링크 추천드립니다.

Maths - Transforms in 3D - Martin Baker

 

다음 포스팅은 좌표변환의 또 다른 주제인 리대수관련하여 포스팅하겠습니다.