Transform: ghép Rotation và Translation thành một phép biến đổi

Bài Matrix trong Robotics đã để lại một câu hỏi mở: ma trận xoay/co giãn thuần chỉ biến đổi được vector quanh gốc toạ độ (0,0) — không thể dùng nó để dịch chuyển một điểm sang vị trí khác. Nhưng một cảm biến gắn trên robot vừa lệch vị trí (translation) vừa lệch hướng (rotation) so với tâm robot. Homogeneous transform là cách gộp cả hai phép biến đổi vào đúng một ma trận.

Khái niệm chính

Mẹo toán học: thêm một thành phần thứ 3 (luôn bằng 1) vào vector 2D, biến (x, y) thành (x, y, 1) — gọi là toạ độ đồng nhất (homogeneous coordinates). Với vector mở rộng này, một ma trận 3×3 có thể biểu diễn cả xoay lẫn dịch chuyển trong đúng một phép nhân:

| cos θ  −sin θ  tx |   | x |   | x' |
| sin θ   cos θ  ty | × | y | = | y' |
|   0       0     1 |   | 1 |   | 1  |

Hai cột tx, ty chính là phần dịch chuyển (translation); khối 2×2 góc trên-trái là phần xoay (rotation) đã học ở bài Matrix. Đây chính xác là loại ma trận tf2 dùng nội bộ để lưu và tính toán mọi phép biến đổi giữa các hệ toạ độ.

Tóm lại: "Transform" giữa hai hệ toạ độ = một cặp (rotation, translation) gộp chung vào một ma trận homogeneous — trả lời đúng câu hỏi "điểm này lệch bao xa và lệch hướng bao nhiêu giữa hệ A và hệ B".

Nguyên lý hoạt động

Lợi ích lớn nhất của homogeneous transform: nối nhiều phép biến đổi liên tiếp chỉ là nhân các ma trận lại với nhau:

T_map→base_link = T_map→odom × T_odom→base_link
T_map→lidar     = T_map→base_link × T_base_link→lidar

Đây chính là cách tf2 trả lời câu hỏi "điểm này trong hệ lidar_link tương ứng toạ độ bao nhiêu trong hệ map" — chỉ cần nhân liên tiếp các transform dọc theo đường đi giữa hai node trong TF tree.

Không phải transform nào cũng đổi liên tục:

static_transform_publisher   →  base_link → lidar_link (cố định, đo 1 lần lúc lắp ráp)
odom_publisher (chạy liên tục) →  odom → base_link (đổi liên tục theo odometry)
amcl (chạy định kỳ)            →  map → odom (đổi mỗi khi AMCL hiệu chỉnh)

Kiến thức kỹ thuật

Công thức

tf2 thường cần tính theo cả 2 chiều — biết T_A→B, đôi khi cần T_B→A (chiều ngược lại). Vì ma trận xoay có tính chất trực giao (orthogonal), nghịch đảo của phần xoay đơn giản chỉ là chuyển vị (transpose), không cần tính nghịch đảo ma trận đầy đủ tốn kém:

T⁻¹: rotation R⁻¹ = Rᵀ (chuyển vị, không phải nghịch đảo từng phần tử)
     translation t' = −Rᵀ × t

VD: T_base_link→lidar có R xoay 0° (không xoay), t = (0.1, 0, 0.15)
T_lidar→base_link có R⁻¹ = Rᵀ = R (không xoay nên chuyển vị = chính nó)
t' = −(0.1, 0, 0.15) = (−0.1, 0, −0.15)

Thông số

Trong ROS2 thực tế, tf2 làm việc với không gian 3D đầy đủ, không chỉ 2D như các ví dụ minh hoạ trong chuyên mục này:

Chiều không gian	Kích thước matrix homogeneous	Phần rotation dùng
2D (minh hoạ trong bài)	3×3	Góc θ đơn giản
3D (tf2 thực tế dùng)	4×4	Quaternion (đã học ở bài trước), không phải Euler

Ứng dụng thực tế

Khi robot phát hiện vật cản qua LiDAR ở toạ độ cục bộ trong hệ lidar_link, việc xác định vị trí thật của vật cản đó trên bản đồ toàn cục chính là phép nhân chuỗi transform đã nêu ở phần Nguyên lý hoạt động: T_map→lidar × điểm_cục_bộ. Toàn bộ pipeline Nav2 cập nhật costmap (bản đồ chi phí dùng để né vật cản) hàng chục lần mỗi giây đều dựa trên đúng phép nhân ma trận homogeneous này — hiểu cơ chế toán học bên dưới giúp chẩn đoán chính xác khi vật cản "hiện sai vị trí" trên bản đồ: lỗi có thể nằm ở bất kỳ transform nào trong chuỗi (sai static transform lúc lắp cảm biến, hoặc lỗi tích luỹ trong odom→base_link), không nhất thiết là lỗi cảm biến.

Ví dụ thực tế

import numpy as np

def homogeneous_transform(theta, tx, ty):
    return np.array([
        [np.cos(theta), -np.sin(theta), tx],
        [np.sin(theta),  np.cos(theta), ty],
        [0, 0, 1]
    ])

# odom -> base_link: robot đã đi tới (2, 0), xoay 90 độ
T_odom_base = homogeneous_transform(np.radians(90), 2, 0)
# base_link -> lidar_link: LiDAR lắp cố định, lệch 0.1m theo x
T_base_lidar = homogeneous_transform(0, 0.1, 0)

T_odom_lidar = T_odom_base @ T_base_lidar
point_lidar = np.array([1, 0, 1])   # điểm cách LiDAR 1m phía trước
point_odom = T_odom_lidar @ point_lidar

print(f"Diem trong he odom: ({point_odom[0]:.2f}, {point_odom[1]:.2f})")

Output:
Diem trong he odom: (2.00, 3.10)

Kết quả gộp đúng cả 2 tầng transform trong 1 phép tính: vị trí robot (2,0) xoay 90°, cộng thêm độ lệch LiDAR 0.1m và điểm cách LiDAR 1m phía trước — tất cả được tính đúng chỉ bằng cách nhân liên tiếp các ma trận homogeneous, không cần viết riêng logic cộng/trừ toạ độ thủ công cho từng bước.

Lỗi thường gặp

• Tính nghịch đảo transform bằng cách nghịch đảo từng phần tử — như đã nói ở mục Công thức, nghịch đảo đúng của phần rotation là chuyển vị, không phải lấy 1/giá_trị từng phần tử — tính sai cách này cho kết quả hoàn toàn vô nghĩa về mặt hình học.
• Nhầm chiều transform (parent→child hay child→parent) — áp dụng T_map→odom khi thực ra cần T_odom→map (chiều ngược lại) cho kết quả toạ độ sai hoàn toàn dù không có lỗi cú pháp nào.
• Coi static transform là "không bao giờ cần cập nhật" — nếu cảm biến bị tháo lắp lại (bảo trì, thay thế) mà không đo và cập nhật lại giá trị static transform tương ứng, mọi phép tính vị trí dựa trên transform cũ sẽ lệch một lượng cố định, khó phát hiện vì không có lỗi runtime nào.
• Tích luỹ sai số nổi (floating-point) qua chuỗi transform dài — nhân liên tiếp nhiều ma trận qua thời gian dài có thể khiến phần rotation dần mất tính trực giao hoàn hảo (không còn đúng RRᵀ = I) — cần chuẩn hoá định kỳ, tương tự vấn đề chuẩn hoá quaternion đã nói ở bài trước.

Kết luận

Homogeneous transform là công cụ toán học thống nhất gộp rotation và translation vào 1 phép nhân matrix, cho phép nối chuỗi nhiều hệ toạ độ chỉ bằng cách nhân ma trận liên tiếp — đây chính xác là cơ chế tf2 dùng để trả lời mọi câu hỏi "điểm này ở đâu trong hệ kia". Đây cũng là bài cuối cùng của chuyên mục Toán cho Robotics — Vector, Matrix, Hệ toạ độ, Rotation, Quaternion, và Transform vừa học là bộ công cụ toán học đầy đủ cho chuyên mục tiếp theo: Động học Robot, nơi các công thức này được áp dụng trực tiếp vào chuyển động thật của robot di động.