[Physical AI W2D1] 5/6 — 좌표계와 동차 변환 행렬 ②: 3D·TF·Quaternion·기구학

[Physical AI W2D1 · 5/6]

2D를 3D로 확장한다. 4×4 동차 변환 행렬이 곧 ROS 2 TF이고, 회전은 Quaternion으로 표현되며, 이 변환을 누적하면 순기구학(관절→끝단), 되돌리면 역기구학(목표→관절)이 된다. RViz TF 축의 정체를 마무리한다.

이 글에서 잡는 개념
1. 3D 회전 행렬(Roll·Pitch·Yaw)과 4×4 동차 변환 행렬
2. 4×4 행렬 = ROS 2 TF (translation + rotation + 시간)
3. Quaternion(x,y,z,w)과 yaw 변환, RPY와의 관계
4. 부모/자식 좌표계·좌표계 체인
5. 순기구학(관절→끝단)과 역기구학(목표→관절), 그리고 피지컬 AI 연결

(4편의 2D를 3D로 확장하고, RViz에서 본 TF 축이 곧 동차 변환 행렬임을 마무리합니다.)

---

1. 3D 회전 행렬 — Roll·Pitch·Yaw

3D 회전은 세 축에 대한 회전으로 표현합니다.

Roll (x축):              Pitch (y축):             Yaw (z축):
[ 1    0      0   ]      [  cosp  0  sinp ]       [ cosy  -siny  0 ]
[ 0  cosr  -sinr ]      [   0    1   0   ]       [ siny   cosy  0 ]
[ 0  sinr   cosr ]      [ -sinp  0  cosp ]       [  0      0    1 ]

조합(자주 쓰는 RPY 순서):

R = R_z(yaw) · R_y(pitch) · R_x(roll)

회전 순서에 따라 결과가 달라지므로 어떤 순서인지 명확히 해야 합니다. (Quaternion은 계산 안정·보간에 유리, 회전 행렬은 축이 어떻게 변하는지 이해에 유리.)

---

2. 3D 동차 변환 행렬 — 4×4

3D 점은 동차 좌표로 [x, y, z, 1]ᵀ. 동차 변환 행렬은 4×4입니다.

T =
[ r11  r12  r13  tx ]      간단히:   T = [ R  t ]
[ r21  r22  r23  ty ]                     [ 0  1 ]
[ r31  r32  r33  tz ]
[  0    0    0    1 ]

• 왼쪽 위 3×3 = 회전 R, 오른쪽 3×1 = 이동 t, 마지막 행 [0 0 0 1] 고정.
• 점 변환: p_A = T_A_B · p_B.

예시 — base_link → laser

LiDAR가 본체 앞 0.25m·위 0.15m, 방향 동일(회전 없음 → R=단위행렬):

T_base_laser =
[ 1  0  0  0.25 ]
[ 0  1  0  0.00 ]
[ 0  0  1  0.15 ]
[ 0  0  0  1.00 ]

LiDAR 기준 앞 1m 점 p_laser = [1, 0, 0, 1]ᵀ → base 기준:

p_base = T_base_laser · p_laser = [ 1.25, 0.00, 0.15, 1.00 ]ᵀ

→ LiDAR 기준 앞 1m가 본체 기준 x=1.25, z=0.15. (3편 Publisher의 static TF base_link→laser 값과 정확히 일치합니다!)

---

3. 4×4 동차 변환 행렬 = ROS 2 TF

ROS 2의 TF는 수학적으로 동차 변환 행렬과 같습니다. TF 트리

odom └── base_link └── laser

는 T_odom_base, T_base_laser 두 변환을 뜻하고, LiDAR 점을 odom까지:

p_odom = T_odom_base · T_base_laser · p_laser

다만 TF는 4×4 행렬을 직접 쓰게 하지 않고 메시지로 발행합니다.

parent frame (header.frame_id)
child frame  (child_frame_id)
translation: x, y, z
rotation:    x, y, z, w   (Quaternion)
timestamp

예 — base_link → laser:

parent: base_link, child: laser
translation: (0.25, 0.0, 0.15)
rotation: (0, 0, 0, 1)   # 회전 없음

이건 위의 T_base_laser 4×4와 정확히 대응됩니다. 즉 TF = 동차 변환 행렬 + 시간 정보.

💡 Translation·Rotation 둘 다 — Translation만 맞고 Rotation이 틀리면 센서 위치는 맞아도 방향이 틀어지고, 반대면 방향은 맞아도 위치가 틀립니다. 항상 함께 확인.

 

---

4. Quaternion — ROS 2의 회전 표현

Quaternion은 3D 회전을 네 값 (x, y, z, w)로 표현합니다. ROS 2의 Pose·Odometry·TF는 회전을 Quaternion으로 씁니다.

회전 없음(단위 회전): x=0, y=0, z=0, w=1

2D 이동 로봇은 z축 회전(yaw)이 핵심이고, yaw → Quaternion 변환은:

x = 0
y = 0
z = sin(yaw / 2)
w = cos(yaw / 2)

예: yaw = 90° → yaw/2 = 45° → z = sin45° ≈ 0.707, w = cos45° ≈ 0.707.

3편 Publisher 코드의 rotation.z = math.sin(yaw/2), rotation.w = math.cos(yaw/2)가 바로 이것이었습니다.

Quaternion은 직관적이진 않지만 3D 회전 계산에서 안정적(짐벌락 없음, 보간 유리)이라 로봇팔·드론·카메라 자세에 자주 쓰입니다.

---

5. Roll·Pitch·Yaw와 Quaternion — 세 가지 회전 표현

표현	장점	주 사용
Roll·Pitch·Yaw	사람이 이해하기 쉬움	방향 설명, 설정값
회전 행렬	좌표축 변환 이해 쉬움	수학 계산, 동차 변환 행렬
Quaternion	3D 회전 계산 안정적	ROS 2 Pose·Odometry·TF

Roll → x축 회전,  Pitch → y축 회전,  Yaw → z축 회전

이동 로봇은 바닥 위 좌우 회전이라 yaw가 가장 중요합니다. 이론은 회전 행렬로 의미를 잡고, ROS 2 실습은 Quaternion으로 메시지를 구성하는 흐름을 이해하면 됩니다.

---

6. 부모·자식 좌표계와 좌표계 체인

TF는 좌표계 관계를 부모(parent)·자식(child) 으로 표현합니다.

base_link → laser     # header.frame_id: base_link, child_frame_id: laser

"laser가 base_link 기준 어디에 있는지"를 뜻합니다. 하나의 자식은 보통 하나의 부모를 가져, 전체가 트리가 됩니다.

odom └── base_link ├── laser ├── camera_link └── imu_link

⚠️ 부모/자식을 거꾸로 설정하면(예: laser를 부모로) "로봇이 센서 기준으로 움직이는" 잘못된 구조가 됩니다.

좌표계 체인 — 두 좌표계가 직접 연결돼 있지 않아도 중간을 거쳐 변환합니다.

p_odom = T_odom_base · T_base_laser · p_laser     # odom ← base ← laser

체인이 끊기면(중간 frame 미발행, 이름 불일치) RViz는 No transform — 2편의 디버깅이 여기 연결됩니다.

---

7. 순기구학 — 관절각에서 끝단으로

순기구학(Forward Kinematics) 은 관절 값이 주어졌을 때 로봇 끝단(엔드이펙터) 위치·자세를 계산합니다. 2링크 평면 로봇팔이라면:

T_base_link1 = 회전 θ1 + 이동 L1
T_link1_link2 = 회전 θ2 + 이동 L2
T_base_ee = T_base_link1 · T_link1_link2

링크가 많아져도 동차 변환 행렬을 누적하면 됩니다.

T_base_ee = T_base_1 · T_1_2 · T_2_3 · … · T_n_ee

물체를 집고, 용접선을 따라가고, 카메라를 옮기려면 끝단 위치를 정확히 계산해야 합니다 — 그 기본이 순기구학, 핵심 도구가 동차 변환 행렬입니다.

---

8. 역기구학 — 목표에서 관절각으로

역기구학(Inverse Kinematics) 은 반대 문제 — 목표 끝단 위치가 주어졌을 때 필요한 관절각을 계산합니다.

순기구학:  관절각  →  끝단 위치
역기구학:  끝단 목표  →  관절각

역기구학은 더 어렵습니다(해가 여러 개거나, 도달 불가능하거나, 관절 제한·충돌 고려). 그래도 좌표 변환이 핵심입니다 — 카메라가 인식한 물체 p_camera를 먼저 로봇팔 base 기준으로 변환해야 합니다.

p_base = T_base_camera · p_camera

이 변환 뒤에야 역기구학이 목표를 계산할 수 있습니다. 좌표 변환이 비전 인식과 실제 조작을 잇는 것이죠.

---

9. 피지컬 AI에서 좌표 변환

피지컬 AI는 인식 → 판단 → 행동의 시스템입니다(입문 ①편의 순환 고리). 여기서 좌표 변환이 인식과 행동을 연결합니다.

카메라가 물체를 인식해도, 로봇이 집으려면 여러 좌표 변환이 필요합니다.

이미지 좌표 → 카메라 좌표 → 로봇 기준 → 로봇팔 기준 → 엔드이펙터 목표 자세

자율주행도 마찬가지:

LiDAR 기준 장애물 → 로봇 기준 → 지도 기준 → 경로 계획

핵심: 좌표계가 틀리면 AI가 올바르게 인식해도 로봇은 엉뚱한 곳으로 움직입니다. 그래서 피지컬 AI는 AI 모델뿐 아니라 좌표계·변환 행렬·TF 구조를 함께 이해해야 합니다.

---

5편 정리

• 3D 회전 = Roll·Pitch·Yaw 행렬 조합, 동차 변환은 4×4 [R t; 0 1].
• 4×4 동차 변환 = ROS 2 TF(translation+rotation+시간). base_link→laser 예시가 3편 코드와 일치.
• 회전은 Quaternion(z=sin(yaw/2), w=cos(yaw/2)). RPY(사람)·회전행렬(수학)·Quaternion(ROS) 세 표현.
• 부모/자식 트리 + 좌표계 체인(끊기면 No transform).
• 순기구학(관절→끝단, 동차변환 누적) ↔ 역기구학(목표→관절, 좌표변환 필요).
• 피지컬 AI = 좌표 변환이 인식과 행동을 연결.

다음 편 예고

이론을 마쳤으니, 6편에서 직접 계산합니다 — Colab/Python으로 2D·3D 동차 변환과 역변환, 좌표계 체인을 코드로 풀고, TF Publisher 노드를 만들어 tf2_echo와 RViz로 좌표축을 확인합니다. 손으로 굳히는 마지막 편입니다.

📚 Week2 Day1 전체 목차 (총 6편)

• 1/6 RViz로 로봇을 눈으로 보다 — 시각화 흐름·Fixed Frame·Display
• 2/6 RViz 개념 ② — TF·좌표계·URDF·센서 Display·디버깅
• 3/6 RViz2 실습 — GCP VM 헤드리스 GUI + Publisher + 시각화
• 4/6 좌표계와 동차 변환 행렬 ① — 좌표계·2D 변환·동차좌표
• 5/6 좌표계와 동차 변환 행렬 ② — 3D·TF·Quaternion·기구학 — 이번 글
• 6/6 좌표 변환 실습 — Python 변환 + TF Publisher + RViz