SLAM từ A đến Z: LiDAR, Visual và cách robot định vị

SLAM là gì và tại sao quan trọng?

SLAM (Simultaneous Localization and Mapping) là bài toán cơ bản nhất trong robot navigation: robot vừa xây dựng bản đồ môi trường, vừa xác định vị trí của mình trên bản đồ đó -- đồng thời. Đây là bài toán "con gà và quả trứng" kinh điển: để định vị cần bản đồ, nhưng để xây bản đồ cần biết vị trí.

Mọi AMR (Autonomous Mobile Robot) hoạt động trong nhà máy, kho hàng, hay ngoài trời đều cần giải SLAM. Không có SLAM, robot chỉ là một chiếc xe điều khiển từ xa.

Trong bài này, mình sẽ đi từ lý thuyết cơ bản (EKF-SLAM, particle filter) đến các framework hiện đại (ORB-SLAM3, Cartographer, LIO-SAM), và giúp bạn chọn đúng phương pháp cho từng use case.

Robot sử dụng LiDAR để scan và xây dựng bản đồ môi trường

Nền tảng lý thuyết SLAM

Bài toán SLAM dưới góc nhìn xác suất

SLAM có thể được mô hình hóa như bài toán ước lượng Bayesian. Tại mỗi thời điểm t, robot cần ước lượng:

Trạng thái (state): vị trí và hướng của robot (x, y, theta)
Bản đồ (map): vị trí của các landmark hoặc occupancy grid
Observations: dữ liệu từ sensor (LiDAR scan, camera frame)
Controls: lệnh điều khiển (vận tốc, góc lái)

Mục tiêu: tìm phân phối xác suất P(x_t, m | z_{1:t}, u_{1:t}) -- xác suất của vị trí robot và bản đồ, cho biết toàn bộ quan sát và điều khiển từ đầu đến thời điểm t.

EKF-SLAM -- Extended Kalman Filter

EKF-SLAM là phương pháp kinh điển nhất, sử dụng Extended Kalman Filter để ước lượng đồng thời vị trí robot và vị trí các landmark.

Cách hoạt động:

Predict: sử dụng motion model để dự đoán vị trí mới của robot
Update: khi quan sát được landmark, cập nhật cả vị trí robot và vị trí landmark

State vector chứa vị trí robot (3 giá trị) + vị trí tất cả landmarks (2N giá trị cho N landmarks). Covariance matrix có kích thước (3+2N) x (3+2N).

# EKF-SLAM simplified pseudocode
def ekf_slam_predict(mu, sigma, u):
    # Motion model: predict new robot pose
    mu_bar = motion_model(mu, u)
    G = jacobian_motion(mu, u)
    sigma_bar = G @ sigma @ G.T + R  # R: motion noise
    return mu_bar, sigma_bar

def ekf_slam_update(mu_bar, sigma_bar, z, landmark_id):
    # Observation model: update with landmark measurement
    z_hat = observation_model(mu_bar, landmark_id)
    H = jacobian_observation(mu_bar, landmark_id)
    K = sigma_bar @ H.T @ inv(H @ sigma_bar @ H.T + Q)
    mu = mu_bar + K @ (z - z_hat)
    sigma = (I - K @ H) @ sigma_bar
    return mu, sigma

Ưu điểm: lý thuyết vững chắc, dễ hiểu, convergent với số landmark nhỏ.

Nhược điểm: độ phức tạp O(N^2) với N landmarks -- không scale được cho bản đồ lớn. Giả định Gaussian không phù hợp với môi trường phức tạp.

Particle Filter SLAM (FastSLAM)

FastSLAM (Montemerlo et al., 2002) giải quyết vấn đề scalability bằng cách sử dụng particle filter cho robot pose và EKF riêng biệt cho mỗi landmark trong mỗi particle.

Cách hoạt động:

Mỗi particle đại diện một giả thuyết về đường đi của robot
Mỗi particle có riêng bộ EKF cho từng landmark
Resampling chọn các particle có xác suất cao nhất

Độ phức tạp: O(M * N) với M particles và N landmarks -- tốt hơn nhiều so với EKF-SLAM.

FastSLAM 2.0 cải tiến thêm bằng cách sử dụng observation mới nhất để sample particle, giảm số particle cần thiết.

LiDAR SLAM -- Chuẩn công nghiệp

Google Cartographer

Cartographer là hệ thống SLAM 2D/3D do Google phát triển, open-source từ 2016. Đây là một trong những LiDAR SLAM được dùng rộng rãi nhất trong công nghiệp.

Kiến trúc:

Local SLAM (Frontend): scan matching bằng Ceres Solver (nonlinear least squares), tạo các submaps từ LiDAR scans liên tiếp
Global SLAM (Backend): pose graph optimization để đóng loop closures, sử dụng branch-and-bound scan matching

Điểm mạnh:

Real-time 2D SLAM trên hardware bình thường
3D SLAM với multi-LiDAR setup
Tích hợp tốt với ROS 1 và ROS 2
Pure LiDAR, không cần IMU (nhưng IMU cải thiện độ chính xác)

# Chạy Cartographer với ROS 2
ros2 launch cartographer_ros cartographer.launch.py \
  use_sim_time:=true

LIO-SAM -- LiDAR-Inertial Odometry via Smoothing and Mapping

LIO-SAM (Shan et al., 2020) là framework tightly-coupled LiDAR-inertial SLAM sử dụng factor graph optimization. Paper gốc: arXiv:2007.00258.

Kiến trúc:

IMU pre-integration factor: tính toán relative motion giữa các LiDAR scans
LiDAR odometry factor: point-to-plane và point-to-edge matching
GPS factor (optional): cho outdoor navigation
Loop closure factor: phát hiện và correct drift

Tại sao LIO-SAM mạnh:

IMU giúp de-skew LiDAR point cloud (rất quan trọng khi robot di chuyển nhanh)
Factor graph cho phép thêm nhiều nguồn thông tin (GPS, wheel odometry)
Keyframe-based: chỉ lưu các frame quan trọng, giảm bộ nhớ

# LIO-SAM config (params.yaml)
lio_sam:
  pointCloudTopic: "points_raw"
  imuTopic: "imu_correct"
  gpsTopic: "odometry/gps"
  
  # Lidar sensor
  sensor: velodyne  # velodyne, ouster, livox
  N_SCAN: 16
  Horizon_SCAN: 1800
  
  # IMU
  imuAccNoise: 3.9939570888238808e-03
  imuGyrNoise: 1.5636343949698187e-03

Pointcloud 3D từ LiDAR SLAM tạo bản đồ chi tiết của môi trường

Visual SLAM -- Camera là đủ

ORB-SLAM3

ORB-SLAM3 (Campos et al., 2021) là hệ thống Visual-Inertial SLAM toàn diện nhất hiện nay. Paper: arXiv:2007.11898.

Hỗ trợ:

Monocular: 1 camera (có scale ambiguity)
Stereo: 2 camera (full metric scale)
RGB-D: depth camera (như Intel RealSense)
Visual-Inertial: camera + IMU (monocular-inertial hoặc stereo-inertial)

Pipeline:

Tracking: extract ORB features, match với local map, estimate camera pose
Local Mapping: triangulate new map points, local bundle adjustment
Loop Closing: DBoW2 bag-of-words cho place recognition, pose graph optimization
Multi-map: quản lý nhiều bản đồ khi tracking mất, merge khi reconnect

Điểm mạnh: accurate, robust, hỗ trợ nhiều loại camera, multi-map system.

Nhược điểm: cần texture-rich environment (thất bại trong corridor trắng), computational cost cao hơn LiDAR SLAM.

// ORB-SLAM3 basic usage
ORB_SLAM3::System SLAM(
    vocab_path,           // ORB vocabulary
    settings_path,        // Camera + ORB params
    ORB_SLAM3::System::RGBD,  // Sensor type
    true                  // Visualization
);

// Process each frame
cv::Mat Tcw = SLAM.TrackRGBD(imRGB, imD, timestamp);

RTAB-Map -- Real-Time Appearance-Based Mapping

RTAB-Map là Visual SLAM được tích hợp tốt nhất với ROS 2. Hỗ trợ nhiều loại sensor và có memory management thông minh.

Điểm nổi bật:

Multi-session mapping: lưu và tải bản đồ giữa các lần chạy
Hỗ trợ 2D occupancy grid (cho navigation) và 3D point cloud
Graph-based SLAM với visual loop closure
ROS 2 package sẵn sàng dùng

Bảng so sánh tổng hợp

Phương pháp	Sensor	Độ chính xác	Tốc độ	Outdoor	Indoor	Scale
EKF-SLAM	Any	Trung bình	Nhanh	Có	Có	Nhỏ (<100 landmarks)
FastSLAM	Any	Cao	Trung bình	Có	Có	Trung bình
Cartographer	LiDAR	Cao	Real-time	Hạn chế	Tốt nhất	Lớn
LIO-SAM	LiDAR+IMU	Rất cao	Real-time	Tốt	Tốt	Rất lớn
ORB-SLAM3	Camera(+IMU)	Cao	Real-time	Có	Tốt	Trung bình
RTAB-Map	Camera/LiDAR	Cao	Real-time	Có	Tốt	Lớn

Khi nào dùng phương pháp nào?

Chọn LiDAR SLAM (Cartographer / LIO-SAM) khi:

Môi trường trong nhà có cấu trúc (nhà máy, kho hàng)
Cần độ chính xác cao và độ tin cậy cho production
Robot di tốc độ cao (LIO-SAM với IMU de-skew)
Budget cho phép mua LiDAR (từ $200 cho 2D đến $5,000+ cho 3D)

Chọn Visual SLAM (ORB-SLAM3 / RTAB-Map) khi:

Budget hạn chế -- camera rẻ hơn LiDAR nhiều
Cần semantic information (nhận diện object, text)
Môi trường có nhiều texture (không phải corridor trắng)
Kết hợp với computer vision tasks khác

Chọn Hybrid (Visual-Inertial hoặc LiDAR-Inertial) khi:

Cần độ tin cậy cao nhất -- sensor fusion tăng robustness
Môi trường thay đổi nhiều (ánh sáng, dynamic objects)
Robot hoạt động cả trong nhà và ngoài trời

Xu hướng mới: Learning-based SLAM

Các phương pháp SLAM truyền thống dựa trên geometric constraints. Nhưng deep learning đang thay đổi cuộc chơi:

DROID-SLAM (Teed & Deng, 2021): end-to-end differentiable SLAM, độ chính xác vượt trội ORB-SLAM3 trên nhiều benchmark. Paper: arXiv:2108.10869.
Gaussian Splatting SLAM: sử dụng 3D Gaussian Splatting để tạo bản đồ photorealistic, cho phép novel view synthesis.
Foundation model-based place recognition: dùng CLIP, DINOv2 cho loop closure tốt hơn bag-of-words truyền thống.

Chúng ta sẽ tìm hiểu sâu hơn về learning-based navigation trong Part 3 của series này.

Tiếp theo trong series

Đây là Part 1 của series Navigation hiện đại. Trong các bài tiếp theo:

Part 2: ROS 2 Nav2: Navigation hoàn chỉnh cho AMR -- Nav2 stack, path planning, behavior trees
Part 3: Learning-based Navigation: GNM, ViNT và NoMaD -- Foundation models cho robot navigation
Part 4: Vision-Language Navigation: Robot đi theo chỉ dẫn -- VLN task và LLM-based planning
Part 5: Outdoor Navigation và Multi-Robot coordination -- GPS-denied nav, MAPF algorithms

SLAM là gì và tại sao quan trọng?

Mọi AMR (Autonomous Mobile Robot) hoạt động trong nhà máy, kho hàng, hay ngoài trời đều cần giải SLAM. Không có SLAM, robot chỉ là một chiếc xe điều khiển từ xa.

Robot sử dụng LiDAR để scan và xây dựng bản đồ môi trường

Nền tảng lý thuyết SLAM

Bài toán SLAM dưới góc nhìn xác suất

SLAM có thể được mô hình hóa như bài toán ước lượng Bayesian. Tại mỗi thời điểm t, robot cần ước lượng:

Trạng thái (state): vị trí và hướng của robot (x, y, theta)
Bản đồ (map): vị trí của các landmark hoặc occupancy grid
Observations: dữ liệu từ sensor (LiDAR scan, camera frame)
Controls: lệnh điều khiển (vận tốc, góc lái)

EKF-SLAM -- Extended Kalman Filter

EKF-SLAM là phương pháp kinh điển nhất, sử dụng Extended Kalman Filter để ước lượng đồng thời vị trí robot và vị trí các landmark.

Cách hoạt động:

Predict: sử dụng motion model để dự đoán vị trí mới của robot
Update: khi quan sát được landmark, cập nhật cả vị trí robot và vị trí landmark

State vector chứa vị trí robot (3 giá trị) + vị trí tất cả landmarks (2N giá trị cho N landmarks). Covariance matrix có kích thước (3+2N) x (3+2N).

# EKF-SLAM simplified pseudocode
def ekf_slam_predict(mu, sigma, u):
    # Motion model: predict new robot pose
    mu_bar = motion_model(mu, u)
    G = jacobian_motion(mu, u)
    sigma_bar = G @ sigma @ G.T + R  # R: motion noise
    return mu_bar, sigma_bar

def ekf_slam_update(mu_bar, sigma_bar, z, landmark_id):
    # Observation model: update with landmark measurement
    z_hat = observation_model(mu_bar, landmark_id)
    H = jacobian_observation(mu_bar, landmark_id)
    K = sigma_bar @ H.T @ inv(H @ sigma_bar @ H.T + Q)
    mu = mu_bar + K @ (z - z_hat)
    sigma = (I - K @ H) @ sigma_bar
    return mu, sigma

Ưu điểm: lý thuyết vững chắc, dễ hiểu, convergent với số landmark nhỏ.

Nhược điểm: độ phức tạp O(N^2) với N landmarks -- không scale được cho bản đồ lớn. Giả định Gaussian không phù hợp với môi trường phức tạp.

Particle Filter SLAM (FastSLAM)

Cách hoạt động:

Mỗi particle đại diện một giả thuyết về đường đi của robot
Mỗi particle có riêng bộ EKF cho từng landmark
Resampling chọn các particle có xác suất cao nhất

Độ phức tạp: O(M * N) với M particles và N landmarks -- tốt hơn nhiều so với EKF-SLAM.

FastSLAM 2.0 cải tiến thêm bằng cách sử dụng observation mới nhất để sample particle, giảm số particle cần thiết.

LiDAR SLAM -- Chuẩn công nghiệp

Google Cartographer

Cartographer là hệ thống SLAM 2D/3D do Google phát triển, open-source từ 2016. Đây là một trong những LiDAR SLAM được dùng rộng rãi nhất trong công nghiệp.

Kiến trúc:

Local SLAM (Frontend): scan matching bằng Ceres Solver (nonlinear least squares), tạo các submaps từ LiDAR scans liên tiếp
Global SLAM (Backend): pose graph optimization để đóng loop closures, sử dụng branch-and-bound scan matching

Điểm mạnh:

Real-time 2D SLAM trên hardware bình thường
3D SLAM với multi-LiDAR setup
Tích hợp tốt với ROS 1 và ROS 2
Pure LiDAR, không cần IMU (nhưng IMU cải thiện độ chính xác)

# Chạy Cartographer với ROS 2
ros2 launch cartographer_ros cartographer.launch.py \
  use_sim_time:=true

LIO-SAM -- LiDAR-Inertial Odometry via Smoothing and Mapping

LIO-SAM (Shan et al., 2020) là framework tightly-coupled LiDAR-inertial SLAM sử dụng factor graph optimization. Paper gốc: arXiv:2007.00258.

Kiến trúc:

IMU pre-integration factor: tính toán relative motion giữa các LiDAR scans
LiDAR odometry factor: point-to-plane và point-to-edge matching
GPS factor (optional): cho outdoor navigation
Loop closure factor: phát hiện và correct drift

Tại sao LIO-SAM mạnh:

IMU giúp de-skew LiDAR point cloud (rất quan trọng khi robot di chuyển nhanh)
Factor graph cho phép thêm nhiều nguồn thông tin (GPS, wheel odometry)
Keyframe-based: chỉ lưu các frame quan trọng, giảm bộ nhớ

# LIO-SAM config (params.yaml)
lio_sam:
  pointCloudTopic: "points_raw"
  imuTopic: "imu_correct"
  gpsTopic: "odometry/gps"
  
  # Lidar sensor
  sensor: velodyne  # velodyne, ouster, livox
  N_SCAN: 16
  Horizon_SCAN: 1800
  
  # IMU
  imuAccNoise: 3.9939570888238808e-03
  imuGyrNoise: 1.5636343949698187e-03

Pointcloud 3D từ LiDAR SLAM tạo bản đồ chi tiết của môi trường

Visual SLAM -- Camera là đủ

ORB-SLAM3

ORB-SLAM3 (Campos et al., 2021) là hệ thống Visual-Inertial SLAM toàn diện nhất hiện nay. Paper: arXiv:2007.11898.

Hỗ trợ:

Monocular: 1 camera (có scale ambiguity)
Stereo: 2 camera (full metric scale)
RGB-D: depth camera (như Intel RealSense)
Visual-Inertial: camera + IMU (monocular-inertial hoặc stereo-inertial)

Pipeline:

Tracking: extract ORB features, match với local map, estimate camera pose
Local Mapping: triangulate new map points, local bundle adjustment
Loop Closing: DBoW2 bag-of-words cho place recognition, pose graph optimization
Multi-map: quản lý nhiều bản đồ khi tracking mất, merge khi reconnect

Điểm mạnh: accurate, robust, hỗ trợ nhiều loại camera, multi-map system.

Nhược điểm: cần texture-rich environment (thất bại trong corridor trắng), computational cost cao hơn LiDAR SLAM.

// ORB-SLAM3 basic usage
ORB_SLAM3::System SLAM(
    vocab_path,           // ORB vocabulary
    settings_path,        // Camera + ORB params
    ORB_SLAM3::System::RGBD,  // Sensor type
    true                  // Visualization
);

// Process each frame
cv::Mat Tcw = SLAM.TrackRGBD(imRGB, imD, timestamp);

RTAB-Map -- Real-Time Appearance-Based Mapping

RTAB-Map là Visual SLAM được tích hợp tốt nhất với ROS 2. Hỗ trợ nhiều loại sensor và có memory management thông minh.

Điểm nổi bật:

Multi-session mapping: lưu và tải bản đồ giữa các lần chạy
Hỗ trợ 2D occupancy grid (cho navigation) và 3D point cloud
Graph-based SLAM với visual loop closure
ROS 2 package sẵn sàng dùng

Bảng so sánh tổng hợp

Phương pháp	Sensor	Độ chính xác	Tốc độ	Outdoor	Indoor	Scale
EKF-SLAM	Any	Trung bình	Nhanh	Có	Có	Nhỏ (<100 landmarks)
FastSLAM	Any	Cao	Trung bình	Có	Có	Trung bình
Cartographer	LiDAR	Cao	Real-time	Hạn chế	Tốt nhất	Lớn
LIO-SAM	LiDAR+IMU	Rất cao	Real-time	Tốt	Tốt	Rất lớn
ORB-SLAM3	Camera(+IMU)	Cao	Real-time	Có	Tốt	Trung bình
RTAB-Map	Camera/LiDAR	Cao	Real-time	Có	Tốt	Lớn

Khi nào dùng phương pháp nào?

Chọn LiDAR SLAM (Cartographer / LIO-SAM) khi:

Môi trường trong nhà có cấu trúc (nhà máy, kho hàng)
Cần độ chính xác cao và độ tin cậy cho production
Robot di tốc độ cao (LIO-SAM với IMU de-skew)
Budget cho phép mua LiDAR (từ $200 cho 2D đến $5,000+ cho 3D)

Chọn Visual SLAM (ORB-SLAM3 / RTAB-Map) khi:

Budget hạn chế -- camera rẻ hơn LiDAR nhiều
Cần semantic information (nhận diện object, text)
Môi trường có nhiều texture (không phải corridor trắng)
Kết hợp với computer vision tasks khác

Chọn Hybrid (Visual-Inertial hoặc LiDAR-Inertial) khi:

Cần độ tin cậy cao nhất -- sensor fusion tăng robustness
Môi trường thay đổi nhiều (ánh sáng, dynamic objects)
Robot hoạt động cả trong nhà và ngoài trời

Xu hướng mới: Learning-based SLAM

Các phương pháp SLAM truyền thống dựa trên geometric constraints. Nhưng deep learning đang thay đổi cuộc chơi:

DROID-SLAM (Teed & Deng, 2021): end-to-end differentiable SLAM, độ chính xác vượt trội ORB-SLAM3 trên nhiều benchmark. Paper: arXiv:2108.10869.
Gaussian Splatting SLAM: sử dụng 3D Gaussian Splatting để tạo bản đồ photorealistic, cho phép novel view synthesis.
Foundation model-based place recognition: dùng CLIP, DINOv2 cho loop closure tốt hơn bag-of-words truyền thống.

Chúng ta sẽ tìm hiểu sâu hơn về learning-based navigation trong Part 3 của series này.

Tiếp theo trong series

Đây là Part 1 của series Navigation hiện đại. Trong các bài tiếp theo:

Part 2: ROS 2 Nav2: Navigation hoàn chỉnh cho AMR -- Nav2 stack, path planning, behavior trees
Part 3: Learning-based Navigation: GNM, ViNT và NoMaD -- Foundation models cho robot navigation
Part 4: Vision-Language Navigation: Robot đi theo chỉ dẫn -- VLN task và LLM-based planning
Part 5: Outdoor Navigation và Multi-Robot coordination -- GPS-denied nav, MAPF algorithms

SLAM là gì và tại sao quan trọng?

Nền tảng lý thuyết SLAM

Bài toán SLAM dưới góc nhìn xác suất

EKF-SLAM -- Extended Kalman Filter

Particle Filter SLAM (FastSLAM)

LiDAR SLAM -- Chuẩn công nghiệp

Google Cartographer

LIO-SAM -- LiDAR-Inertial Odometry via Smoothing and Mapping

Visual SLAM -- Camera là đủ

ORB-SLAM3

RTAB-Map -- Real-Time Appearance-Based Mapping

Bảng so sánh tổng hợp

Khi nào dùng phương pháp nào?

Chọn LiDAR SLAM (Cartographer / LIO-SAM) khi:

Chọn Visual SLAM (ORB-SLAM3 / RTAB-Map) khi:

Chọn Hybrid (Visual-Inertial hoặc LiDAR-Inertial) khi:

Xu hướng mới: Learning-based SLAM

Tiếp theo trong series

Bài viết liên quan

Nguyễn Anh Tuấn

Bài viết liên quan

ROS 2 Nav2: Navigation hoàn chỉnh cho AMR

ROS 2 từ A đến Z (P6): Nav2 — Robot tự hành

Outdoor Navigation và Multi-Robot Coordination

SLAM là gì và tại sao quan trọng?

Nền tảng lý thuyết SLAM

Bài toán SLAM dưới góc nhìn xác suất

EKF-SLAM -- Extended Kalman Filter

Particle Filter SLAM (FastSLAM)

LiDAR SLAM -- Chuẩn công nghiệp

Google Cartographer

LIO-SAM -- LiDAR-Inertial Odometry via Smoothing and Mapping

Visual SLAM -- Camera là đủ

ORB-SLAM3

RTAB-Map -- Real-Time Appearance-Based Mapping

Bảng so sánh tổng hợp

Khi nào dùng phương pháp nào?

Chọn LiDAR SLAM (Cartographer / LIO-SAM) khi:

Chọn Visual SLAM (ORB-SLAM3 / RTAB-Map) khi:

Chọn Hybrid (Visual-Inertial hoặc LiDAR-Inertial) khi:

Xu hướng mới: Learning-based SLAM

Tiếp theo trong series

Bài viết liên quan

Nguyễn Anh Tuấn

Bài viết liên quan

ROS 2 Nav2: Navigation hoàn chỉnh cho AMR

ROS 2 từ A đến Z (P6): Nav2 — Robot tự hành

Outdoor Navigation và Multi-Robot Coordination