Matrix trong Robotics: biến đổi hàng loạt điểm cùng lúc

Bài Vector trong Robotics đã nói vector biểu diễn một điểm hoặc một hướng. Câu hỏi tiếp theo: làm sao xoay/dịch chuyển hàng nghìn điểm cùng lúc (toàn bộ đám mây điểm LiDAR, toàn bộ bản đồ occupancy grid) theo cùng một phép biến đổi, mà không cần viết vòng lặp tính riêng từng điểm theo công thức lượng giác? Câu trả lời là matrix.

Khái niệm chính

Một matrix 2×2 nhân với một vector 2D cho ra một vector 2D khác — quy tắc nhân cố định:

| a  b |   | x |   | ax + by |
| c  d | × | y | = | cx + dy |

Điều quan trọng: cùng một matrix M, nhân với vector nào cũng áp dụng đúng một phép biến đổi đó. Nghĩa là nếu cần xoay 5.000 điểm LiDAR đi 15°, chỉ cần tính một matrix xoay 15° duy nhất, rồi nhân lần lượt với cả 5.000 vector — không cần suy luận lại công thức lượng giác cho từng điểm.

Nguyên lý hoạt động

Ba phép biến đổi matrix hay gặp nhất trong robot 2D:

Ma trận đơn vị (identity) — không đổi gì:
| 1  0 |
| 0  1 |

Ma trận xoay (rotation) góc θ:
| cos θ  −sin θ |
| sin θ   cos θ |

Ma trận co giãn (scale) theo hệ số k:
| k  0 |
| 0  k |

Ma trận xoay là ma trận dùng nhiều nhất trong robot di động — chính là công thức đứng sau bài Rotation khi cần xoay một vector theo góc heading hiện tại của robot.

Tóm lại: Phép biến đổi bằng matrix tuyến tính — nghĩa là không thể dùng nó để dịch chuyển (translate) một điểm, chỉ xoay/co giãn quanh gốc toạ độ (0,0). Đây chính là lý do vì sao robot học cần thêm khái niệm homogeneous transform (bài Transform) — mở rộng matrix để xử lý được cả phép dịch chuyển.

Kiến thức kỹ thuật

Công thức

Nhân matrix không có tính giao hoán — thứ tự nhân quyết định kết quả cuối cùng khác nhau, một điểm rất dễ nhầm lẫn khi ghép nhiều phép biến đổi:

M1 × M2 ≠ M2 × M1 (nói chung)

VD: Xoay 90° rồi co giãn 2 lần, áp dụng lên điểm (1, 0)
Cách 1 (xoay trước): M_scale × (M_rotate × p) = M_scale × (0, 1) = (0, 2)
Cách 2 (co giãn trước): M_rotate × (M_scale × p) = M_rotate × (2, 0) = (0, 2)

→ Trường hợp này trùng kết quả (do co giãn đều k theo mọi trục), nhưng với
co giãn không đều theo từng trục (kx ≠ ky) hoặc phép biến đổi phức tạp hơn,
2 thứ tự cho ra 2 kết quả khác nhau hoàn toàn

Khi ghép nhiều phép biến đổi (M_tong_hop = M_scale × M_rotate), luôn giữ đúng thứ tự dự định — đổi thứ tự nhân là đổi luôn ý nghĩa hình học của phép biến đổi.

Thông số

Chi phí tính toán khi biến đổi hàng loạt điểm — lý do các thư viện robot luôn gộp matrix trước rồi mới áp dụng lên toàn bộ dữ liệu:

Cách làm	Chi phí cho N điểm
Tính lại sin/cos (hàm siêu việt, chậm) cho từng điểm	N lần gọi hàm lượng giác — tốn kém
Tính 1 matrix xoay 1 lần, nhân matrix (chỉ nhân/cộng) cho N điểm	1 lần gọi lượng giác + N × (4 phép nhân + 2 phép cộng) — rẻ hơn nhiều

Ứng dụng thực tế

Đây chính xác là việc SLAM làm ở mỗi lần quét LiDAR mới: dữ liệu quét thô nằm trong hệ toạ độ cục bộ của LiDAR (lidar_link, đã học ở bài TF/TF2 trong chuyên mục ROS/ROS2) — trước khi gộp vào bản đồ toàn cục, toàn bộ hàng nghìn điểm của 1 lần quét phải được nhân với ma trận biến đổi ứng với pose (vị trí + hướng) hiện tại của robot, chuyển từ hệ cục bộ sang hệ map toàn cục. Với LiDAR quét hàng chục lần mỗi giây, mỗi lần hàng nghìn điểm, việc gộp matrix trước khi áp dụng hàng loạt (thay vì tính lượng giác lại cho từng điểm) là yếu tố quyết định SLAM có chạy đủ nhanh theo thời gian thực hay không.

Ví dụ thực tế

import numpy as np

theta = np.radians(90)  # xoay 90 độ
M_rotate = np.array([
    [np.cos(theta), -np.sin(theta)],
    [np.sin(theta),  np.cos(theta)]
])

# 3 điểm quét LiDAR mẫu (toạ độ cục bộ)
points = np.array([[1, 0], [2, 0], [0, 1]])

# Xoay toàn bộ 3 điểm cùng lúc bằng 1 phép nhân matrix
points_rotated = points @ M_rotate.T

print(points_rotated)

Output:
[[ 6.123234e-17  1.000000e+00]
 [ 1.224647e-16  2.000000e+00]
 [-1.000000e+00  6.123234e-17]]

Kết quả gần đúng [(0,1), (0,2), (-1,0)] (sai số cực nhỏ do làm tròn số thực dấu phẩy động) — xác nhận đúng: điểm (1,0) sau khi xoay 90° trở thành (0,1), khớp với quy tắc xoay ngược chiều kim đồng hồ đã nêu ở công thức ma trận xoay.

Lỗi thường gặp

• Đổi nhầm thứ tự khi ghép nhiều matrix — như đã nói ở mục Công thức, nhân matrix không giao hoán — ghép sai thứ tự (M_rotate × M_scale thay vì M_scale × M_rotate) cho kết quả hình học khác hẳn dự định.
• Kỳ vọng matrix tuyến tính tự dịch chuyển được điểm — như đã nói ở phần Nguyên lý hoạt động, matrix 2×2/3×3 tuyến tính chỉ xoay/co giãn quanh gốc toạ độ, không thể dịch chuyển (translate) — cần homogeneous transform (bài tiếp theo) cho việc này.
• Trộn độ và radian khi tính ma trận xoay — giống lỗi đã nói ở bài Vector, hàm sin/cos trong hầu hết thư viện nhận radian, quên đổi đơn vị cho kết quả sai hoàn toàn.
• Nhân 2 matrix sai kích thước — matrix 2×2 chỉ nhân được với vector 2D hoặc matrix 2×N, cố nhân với vector/matrix sai số chiều gây lỗi runtime ở hầu hết thư viện toán học (NumPy báo ValueError: matmul: Input operand has a mismatch...).

Kết luận

Matrix cho phép áp dụng cùng 1 phép biến đổi hình học lên hàng nghìn điểm chỉ bằng 1 phép nhân lặp lại, tránh phải tính lại hàm lượng giác tốn kém cho từng điểm — nền tảng tính toán của mọi pipeline SLAM/point cloud xử lý dữ liệu LiDAR thời gian thực. Nắm vững tính không giao hoán khi ghép nhiều phép biến đổi là chìa khoá tránh lỗi hình học khó debug. Bài tiếp theo trong chuyên mục nói về Hệ toạ độ (Coordinate System) — nền tảng để hiểu matrix/vector đang được biểu diễn trong hệ quy chiếu nào.