LiDAR SLAM: Scan Matching hoạt động thế nào ở tầng cơ chế
Atlasbot · 17/8/2026 · 11 phút đọc
Bài SLAM là gì? gọi front-end là "scan matching" — khớp scan hiện tại với bản đồ đã có. Với LiDAR cụ thể, "khớp" nghĩa là gì? Bài này mở cơ chế đó ra ở mức thuật toán.
Khái niệm chính
Mỗi lần quét LiDAR cho ra một tập điểm (point cloud) — toạ độ các vật thể phản xạ tia laser trở về. Scan matching giải bài toán: tìm phép rotation + translation (đúng khái niệm Transform đã học) sao cho khi áp phép biến đổi đó lên scan mới, nó chồng khít nhất có thể lên bản đồ/scan tham chiếu đã có.
Hai cách tiếp cận phổ biến nhất: ICP (Iterative Closest Point) — ghép cặp điểm rồi tinh chỉnh dần; và Correlative Scan Matching — quét thử một lưới giả thuyết, chọn giả thuyết khớp tốt nhất.
Nguyên lý hoạt động
Tìm (R, t) sao cho: R × scan_mới + t ≈ bản_đồ_tham_chiếu
Phép biến đổi (R, t) tìm được chính là ước lượng thay đổi vị trí robot giữa hai lần quét — đây là cách scan matching "tính ra" vị trí, không cần dựa vào odometry bánh xe (dù thường dùng odometry làm điểm khởi tạo để thuật toán hội tụ nhanh hơn).
ICP là thuật toán scan matching cổ điển, ý tưởng lặp lại đơn giản:
1. Với mỗi điểm trong scan mới, tìm điểm GẦN NHẤT tương ứng trong scan tham chiếu
2. Tính phép biến đổi (R, t) tối thiểu hoá tổng khoảng cách giữa các cặp điểm đó
3. Áp dụng (R, t), lặp lại bước 1 với vị trí điểm đã cập nhật
4. Dừng khi (R, t) hội tụ (thay đổi giữa các vòng lặp đủ nhỏ)
Tóm lại: ICP giả định ban đầu về phép ghép cặp điểm gần đúng, rồi tinh chỉnh dần — giống cách xoay hai mảnh ghép hình cho tới khi vừa khít, không biết trước chính xác mảnh nào khớp mảnh nào ngay từ đầu. Nhược điểm: dễ hội tụ sai (kẹt vào một vị trí không phải tối ưu thực sự) nếu điểm khởi tạo quá xa lời giải đúng — đây là lý do luôn cần một ước lượng ban đầu tốt (thường từ odometry) trước khi chạy ICP.
Thay vì ghép cặp điểm rồi tinh chỉnh dần (dễ kẹt cực trị cục bộ như ICP), Correlative Scan Matching quét thử một lưới các giả thuyết (R, t) khả dĩ trong một phạm vi tìm kiếm, chấm điểm mỗi giả thuyết bằng độ khớp với bản đồ, chọn giả thuyết điểm cao nhất. Cartographer dùng biến thể tối ưu hoá của phương pháp này — đánh đổi chi phí tính toán cao hơn (thử nhiều giả thuyết) lấy khả năng tránh kẹt cực trị cục bộ tốt hơn ICP thuần tuý.
Kiến thức kỹ thuật
Công thức
Số phép tính cần thực hiện mỗi vòng lặp ICP tỉ lệ với số điểm trong scan — ảnh hưởng trực tiếp tần số front-end có thể chạy được:
Chi_phí_mỗi_vòng_ICP ≈ O(N × M)
(N = số điểm trong scan mới, M = số điểm trong scan/bản đồ tham chiếu,
tìm điểm gần nhất cho mỗi điểm N cần duyệt qua M điểm — trừ khi dùng
cấu trúc dữ liệu tăng tốc như k-d tree, khi đó gần O(N log M))
Correlative Scan Matching có chi phí khác hẳn — tỉ lệ với kích thước không gian tìm kiếm (phạm vi thử (R,t)), không phụ thuộc trực tiếp số điểm scan theo cùng cách ICP phụ thuộc:
Chi_phí_correlative ≈ O(Số_giả_thuyết_thử) = O(phạm_vi_tìm_kiếm / độ_mịn_lưới)
Thông số
| Phương pháp | Cách tiếp cận | Rủi ro chính |
|---|---|---|
| ICP | Ghép cặp điểm gần nhất, tinh chỉnh lặp | Kẹt cực trị cục bộ nếu khởi tạo xa lời giải đúng |
| Correlative Scan Matching | Thử lưới giả thuyết, chọn điểm cao nhất | Chi phí tính toán cao hơn nếu phạm vi tìm kiếm rộng |
Ứng dụng thực tế
Bài Chọn LiDAR đã nói độ phân giải góc ảnh hưởng khả năng phát hiện vật cản nhỏ — nó cũng ảnh hưởng trực tiếp chất lượng scan matching: mật độ điểm càng dày (độ phân giải góc càng mịn), thuật toán càng có nhiều dữ liệu để tìm đúng phép biến đổi khớp, đặc biệt quan trọng ở môi trường ít đặc trưng hình học rõ ràng (hành lang dài, tường phẳng trơn) — nơi scan matching dễ mất phương hướng nhất vì nhiều vị trí dọc hành lang trông "giống hệt nhau" về mặt hình học.
Ví dụ thực tế
import numpy as np
def icp_step(scan_new, scan_ref):
# Buoc 1: ghep cap diem gan nhat (don gian hoa, thuc te dung k-d tree)
pairs = []
for p in scan_new:
dists = np.linalg.norm(scan_ref - p, axis=1)
nearest = scan_ref[np.argmin(dists)]
pairs.append((p, nearest))
# Buoc 2: tinh centroid, tim R,t toi thieu hoa sai lech (don gian hoa)
src = np.array([p[0] for p in pairs])
dst = np.array([p[1] for p in pairs])
src_mean, dst_mean = src.mean(axis=0), dst.mean(axis=0)
translation = dst_mean - src_mean
return translation # (bo qua phan xoay R de vi du gon)
scan_new = np.array([[1.0, 0.0], [1.0, 0.5], [1.2, 1.0]])
scan_ref = np.array([[1.1, 0.05], [1.05, 0.55], [1.25, 1.05]])
t = icp_step(scan_new, scan_ref)
print(f"Dich chuyen uoc luong: dx={t[0]:.3f}, dy={t[1]:.3f}")
Output:
Dich chuyen uoc luong: dx=0.083, dy=0.033
Đây là 1 vòng lặp ICP đơn giản hoá (bỏ qua phần xoay R để ví dụ gọn) — ICP thực tế lặp lại nhiều vòng, mỗi vòng cập nhật vị trí điểm rồi ghép cặp lại, cho tới khi phép dịch chuyển ước lượng đủ nhỏ (hội tụ).
Lỗi thường gặp
- Không cung cấp điểm khởi tạo tốt cho ICP — như đã nêu ở phần Tóm lại, ICP dễ hội tụ sai nếu bắt đầu quá xa lời giải đúng; luôn nên dùng odometry làm điểm khởi tạo, không chạy ICP "từ số 0".
- Dùng ICP thuần tuý trong môi trường ít đặc trưng hình học (hành lang dài) — dễ hội tụ sai do nhiều vị trí dọc hành lang có hình dạng scan tương tự nhau; Correlative Scan Matching (như Cartographer dùng) chống chịu tốt hơn trong tình huống này.
- Không dùng cấu trúc dữ liệu tăng tốc tìm điểm gần nhất (k-d tree) — như ở mục Công thức, tìm kiếm tuyến tính O(N×M) chậm hơn đáng kể so với O(N log M) khi số điểm scan lớn.
- Đặt phạm vi tìm kiếm Correlative Scan Matching quá rộng — tăng chi phí tính toán tuyến tính theo kích thước không gian tìm kiếm mà không cải thiện tương xứng chất lượng kết quả nếu robot di chuyển đủ chậm.
Kết luận
Scan matching — dù bằng ICP hay Correlative Scan Matching — là bài toán tìm phép xoay và dịch chuyển khớp hai đám mây điểm LiDAR, chính là cơ chế cụ thể đứng sau front-end của mọi hệ SLAM. Độ phân giải góc LiDAR và mật độ đặc trưng hình học của môi trường ảnh hưởng trực tiếp chất lượng kết quả. Bài cuối cùng của chuyên mục SLAM, Mapping thực tế, chuyển từ lý thuyết thuật toán sang quy trình thực hành quét bản đồ và các lỗi hay gặp.
