SLAM是什么？

SLAM（Simultaneous Localization and Mapping）同时定位与建图，是机器人在未知环境中自主导航的关键技术。

SLAM的挑战

想象你被蒙上眼睛放入一个陌生房间：

  • 定位：我在哪里？
  • 建图：周围有什么？
  • 同时性：没有地图无法定位，不知道位置无法建图

这就是”鸡生蛋蛋生鸡”的SLAM问题。

SLAM的分类

按传感器分类

  1. 激光SLAM

    • 传感器：2D/3D激光雷达
    • 优点：精度高，不受光照影响
    • 缺点：价格贵，数据量大
    • 应用：扫地机器人、自动驾驶

  2. 视觉SLAM

    • 传感器：单目/双目/RGB-D相机
    • 优点：成本低，信息丰富
    • 缺点：受光照影响，计算量大
    • 应用：AR/VR、无人机

  3. 融合SLAM

    • 传感器：激光+视觉+IMU
    • 优点：精度高，鲁棒性强
    • 应用：高端机器人

按环境分类

  • 2D SLAM：平面环境
  • 3D SLAM：三维空间

SLAM框架

前端（Frontend）

作用：处理传感器数据，提取特征

主要任务：

  1. 特征提取
  2. 数据关联
  3. 位姿估计

后端（Backend）

作用：优化地图和位姿

主要任务：

  1. 图优化
  2. 滤波
  3. 闭环检测

回环检测（Loop Closure）

识别曾经到过的地方，消除累积误差。

激光SLAM基础

Gmapping算法

基于粒子滤波的2D SLAM：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
import rospy
from nav_msgs.msg import OccupancyGrid
from sensor_msgs.msg import LaserScan

class GmappingSLAM:
    def __init__(self):
        self.particles = []  # 粒子群
        self.map = OccupancyGrid()  # 栅格地图
        
        # 订阅激光数据
        rospy.Subscriber('/scan', LaserScan, self.scan_callback)
    
    def scan_callback(self, scan):
        # 1. 预测：根据里程计移动粒子
        self.predict(odometry)
        
        # 2. 更新：根据激光数据调整权重
        self.update_weights(scan)
        
        # 3. 重采样：保留权重大的粒子
        self.resample()
        
        # 4. 更新地图
        self.update_map(scan)

Cartographer

Google开源的SLAM方案，使用子图优化：

1
2
3
4
5
6
7
8
9
10
11
12
# cartographer配置
TRAJECTORY_BUILDER_2D = {
  use_imu_data = true,
  min_range = 0.3,
  max_range = 30.0,
  
  # 扫描匹配
  ceres_scan_matcher = {
    translation_weight = 10.0,
    rotation_weight = 1.0,
  },
}

视觉SLAM基础

ORB-SLAM2

经典的单目SLAM方案：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
// ORB-SLAM2 主循环
class System {
public:
    void TrackMonocular(cv::Mat &im, double timestamp) {
        // 1. 提取ORB特征
        vector<cv::KeyPoint> vKeys;
        cv::Mat mDescriptors;
        mpORBextractor->operator()(im, vKeys, mDescriptors);
        
        // 2. 跟踪上一帧
        if (mState == OK) {
            TrackWithMotionModel();
        }
        
        // 3. 局部地图跟踪
        TrackLocalMap();
        
        // 4. 关键帧决策
        if (NeedNewKeyFrame()) {
            CreateNewKeyFrame();
        }
    }
};

VINS-Mono

单目视觉-惯性SLAM：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
// 特征跟踪
void FeatureTracker::readImage(const cv::Mat &_img) {
    cv::Mat img = _img;
    
    // 光流跟踪
    vector<uchar> status;
    vector<float> err;
    cv::calcOpticalFlowPyrLK(
        prev_img, img,
        prev_pts, cur_pts,
        status, err
    );
    
    // 提取新特征点
    cv::goodFeaturesToTrack(
        img, n_pts,
        MAX_CNT - cur_pts.size(),
        0.01, MIN_DIST
    );
}

ROS中使用SLAM

启动Gmapping

1
2
3
4
5
# 启动gmapping节点
rosrun gmapping slam_gmapping \
  scan:=/scan \
  _base_frame:=base_link \
  _map_update_interval:=5.0

保存地图

1
2
# 保存生成的地图
rosrun map_server map_saver -f my_map

会生成两个文件：

  • my_map.pgm：地图图片
  • my_map.yaml：地图元数据

加载地图导航

1
2
# 启动导航节点
roslaunch navigation navigation.launch map_file:=my_map.yaml

Python实现简单SLAM

基于特征的SLAM

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
import numpy as np
import cv2

class SimpleSLAM:
    def __init__(self):
        self.orb = cv2.ORB_create()
        self.bf = cv2.BFMatcher(cv2.NORM_HAMMING)
        
        self.trajectory = []
        self.map_points = []
    
    def process_frame(self, image):
        # 提取特征
        kp, des = self.orb.detectAndCompute(image, None)
        
        if len(self.trajectory) == 0:
            # 第一帧，初始化
            self.last_kp = kp
            self.last_des = des
            self.trajectory.append(np.eye(4))
            return
        
        # 特征匹配
        matches = self.bf.knnMatch(self.last_des, des, k=2)
        
        # 比值测试
        good_matches = []
        for m, n in matches:
            if m.distance < 0.75 * n.distance:
                good_matches.append(m)
        
        # 估计位姿
        if len(good_matches) > 10:
            src_pts = np.float32([
                self.last_kp[m.queryIdx].pt for m in good_matches
            ]).reshape(-1, 1, 2)
            
            dst_pts = np.float32([
                kp[m.trainIdx].pt for m in good_matches
            ]).reshape(-1, 1, 2)
            
            # 计算本质矩阵
            E, mask = cv2.findEssentialMat(
                src_pts, dst_pts,
                focal=718.856, pp=(607.1928, 185.2157),
                method=cv2.RANSAC, prob=0.999, threshold=1.0
            )
            
            # 恢复位姿
            _, R, t, mask = cv2.recoverPose(E, src_pts, dst_pts)
            
            # 更新轨迹
            T = np.eye(4)
            T[:3, :3] = R
            T[:3, 3] = t.reshape(3)
            
            self.trajectory.append(
                self.trajectory[-1] @ T
            )
        
        # 更新上一帧
        self.last_kp = kp
        self.last_des = des
    
    def get_trajectory(self):
        return np.array([T[:3, 3] for T in self.trajectory])

SLAM评估指标

绝对轨迹误差（ATE）

1
2
3
4
5
6
7
8
9
10
11
12
def compute_ate(gt_poses, est_poses):
    """
    计算绝对轨迹误差
    gt_poses: 真实位姿
    est_poses: 估计位姿
    """
    ate = np.linalg.norm(gt_poses - est_poses, axis=1)
    return {
        'mean': np.mean(ate),
        'rmse': np.sqrt(np.mean(ate**2)),
        'std': np.std(ate)
    }

相对位姿误差（RPE）

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
def compute_rpe(gt_poses, est_poses, delta=1):
    """
    计算相对位姿误差
    delta: 帧间隔
    """
    rpe_trans = []
    rpe_rot = []
    
    for i in range(len(gt_poses) - delta):
        # 计算相对变换
        gt_rel = np.linalg.inv(gt_poses[i]) @ gt_poses[i + delta]
        est_rel = np.linalg.inv(est_poses[i]) @ est_poses[i + delta]
        
        # 误差
        error = np.linalg.inv(gt_rel) @ est_rel
        
        # 平移误差
        rpe_trans.append(np.linalg.norm(error[:3, 3]))
        
        # 旋转误差
        rpe_rot.append(np.arccos(
            (np.trace(error[:3, :3]) - 1) / 2
        ))
    
    return {
        'trans_mean': np.mean(rpe_trans),
        'rot_mean': np.rad2deg(np.mean(rpe_rot))
    }

常用数据集

TUM RGB-D Dataset

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
# 下载和加载TUM数据集
def load_tum_dataset(path):
    # 读取RGB图像
    rgb_files = sorted(glob.glob(f"{path}/rgb/*.png"))
    
    # 读取深度图像
    depth_files = sorted(glob.glob(f"{path}/depth/*.png"))
    
    # 读取真实轨迹
    with open(f"{path}/groundtruth.txt") as f:
        lines = f.readlines()
        gt_poses = []
        for line in lines:
            if line[0] == '#':
                continue
            data = [float(x) for x in line.split()]
            # timestamp, tx, ty, tz, qx, qy, qz, qw
            gt_poses.append(data)
    
    return rgb_files, depth_files, gt_poses

KITTI Dataset

自动驾驶场景，提供激光雷达、相机等多传感器数据。

实战：基于ROS的导航

1. 建图

1
2
3
4
5
6
7
8
9
10
11
# 启动机器人
roslaunch robot bringup.launch

# 启动gmapping
roslaunch robot gmapping.launch

# 手动遥控建图
roslaunch robot teleop.launch

# 建图完成，保存地图
rosrun map_server map_saver -f office_map

2. 导航

1
2
3
4
5
6
7
8
# 加载地图和导航
roslaunch robot navigation.launch

# 在RViz中设置初始位姿
# 点击"2D Pose Estimate"

# 设置导航目标
# 点击"2D Nav Goal"

3. Python控制导航

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
import rospy
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
import actionlib

def navigate_to_goal(x, y, theta):
    # 创建action客户端
    client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
    client.wait_for_server()
    
    # 构造目标
    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = "map"
    goal.target_pose.header.stamp = rospy.Time.now()
    
    goal.target_pose.pose.position.x = x
    goal.target_pose.pose.position.y = y
    
    # 四元数表示方向
    goal.target_pose.pose.orientation.w = np.cos(theta / 2)
    goal.target_pose.pose.orientation.z = np.sin(theta / 2)
    
    # 发送目标
    client.send_goal(goal)
    client.wait_for_result()
    
    return client.get_result()

# 导航到指定点
rospy.init_node('navigator')
result = navigate_to_goal(2.0, 1.0, 0)  # x, y, theta

进阶话题

1. 多传感器融合

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
// 使用EKF融合IMU和激光
class SensorFusion {
    void predict(IMU imu_data) {
        // 预测步骤
        x_pred = F * x + B * u;
        P_pred = F * P * F.transpose() + Q;
    }
    
    void update(LaserScan scan) {
        // 更新步骤
        K = P_pred * H.transpose() * (H * P_pred * H.transpose() + R).inverse();
        x = x_pred + K * (z - H * x_pred);
        P = (I - K * H) * P_pred;
    }
};

2. 动态环境SLAM

处理移动物体，避免干扰建图。

3. 语义SLAM

结合深度学习，理解场景语义。

常见问题

1. 建图漂移

  • 原因：累积误差
  • 解决：回环检测、图优化

2. 特征匹配失败

  • 原因：光照变化、运动模糊
  • 解决：多传感器融合

3. 计算资源不足

  • 原因：算法复杂度高
  • 解决：降采样、GPU加速

总结

SLAM是机器人自主导航的核心，涉及：

  • 数学基础（线性代数、概率论、优化）
  • 传感器原理
  • 算法实现

建议学习路径：

  1. 掌握数学基础
  2. 学习经典算法（EKF-SLAM、粒子滤波）
  3. 实践开源项目（ORB-SLAM2、Cartographer）
  4. 参加比赛积累经验

推荐资源：