移动机器人SLAM建模仿真与车间管网地图构建 —— 基于激光雷达与占据栅格的OOP实战

 

“第一次带着移动机器人在老厂区转，原本以为拿着CAD图纸就能直接导航，结果现场一比对，发现十年间新增的工艺管道、临时储罐、检修平台根本没在图里，机器人走着走着就撞上了悬空的管廊。后来我们给机器人加装了2D激光雷达，跑了一套基于占据栅格（Occupancy Grid）的SLAM建图算法，让它自己‘摸’着走一圈，自动把管道、储罐、立柱全部画进地图。现在生成的PGM地图直接导入导航栈，再也没出现过‘幽灵障碍’撞车事故。”

—— 哈尔滨工程大学《工业过程控制》课程核心思想延伸

 

一、实际应用场景描述

 

在石油化工、制药、能源等存量工厂的智能化改造中，移动机器人承担着巡检、应急、物资运输等关键任务。然而，老旧厂区普遍存在图纸缺失、现场变更频繁、管网错综复杂的问题，传统的预先建模方式难以满足实际需求。

 

                    ┌──────────────────────────────────────────────┐

                    │ 移动机器人SLAM车间建图系统 │

                    │ │

                    │ [上位机建图与导航系统] │

                    │ │ ROS/LCM/WebSocket │

                    │ ▼ │

                    │ ┌────────────────────────────┐ │

                    │ │ SLAM 前端处理引擎 │ │

                    │ │ ┌──────────────────────┐ │ │

                    │ │ │ 1. 激光雷达数据处理 │ │ │

                    │ │ │ (去畸变/滤波) │ │ │

                    │ │ └──────────────────────┘ │ │

                    │ │ ┌──────────────────────┐ │ │

                    │ │ │ 2. 里程计融合 │ │ │

                    │ │ │ (Encoder/IMU) │ │ │

                    │ │ └──────────────────────┘ │ │

                    │ │ ┌──────────────────────┐ │ │

                    │ │ │ 3. 扫描匹配 │ │ │

                    │ │ │ (ICP/Hector/GMapping)│ │

                    │ │ └──────────────────────┘ │ │

                    │ └────────────┬───────────────┘ │

                    │ │ 位姿+激光数据 │

                    │ ┌───────┴───────┐ │

                    │ ▼ ▼ │

                    │ ┌─────────┐ ┌─────────┐ │

                    │ │ 2D激光雷达 │ │ 里程计/IMU │ │

                    │ │ (LiDAR) │ │ (Odometry)│ │

                    │ └────┬────┘ └────┬────┘ │

                    │ │ Scan │ Delta Pose │

                    │ ▼ ▼ │

                    │ ┌────────────────────────────┐ │

                    │ │ 车间物理环境 │ │

                    │ │ ┌───┐ ┌───┐ ┌───┐ │ │

                    │ │ │罐体├──►管道├──►立柱├──... │ ← 待建图 │

                    │ │ └─┬─┘ └─┬─┘ └───┘ │ │

                    │ │ └──────┴──────── │ │

                    │ └───────────────────────────┘ │

                    │ │ 观测数据 │

                    │ ▼ │

                    │ ┌────────────────────────────┐ │

                    │ │ 后端优化与地图构建 │ │

                    │ │ ┌──────────────────────┐ │ │

                    │ │ │ 4. 位姿图优化 │ │ │

                    │ │ │ (GraphSLAM) │ │ │

                    │ │ └──────────────────────┘ │ │

                    │ │ ┌──────────────────────┐ │ │

                    │ │ │ 5. 占据栅格地图更新 │ │ │

                    │ │ │ (Bayesian Update) │ │ │

                    │ │ └──────────────────────┘ │ │

                    │ └────────────┬───────────────┘ │

                    │ │ 概率地图 (.pgm/.yaml) │

                    │ ▼ │

                    │ ┌────────────────────────────┐ │

                    │ │ Navigation Stack │ │

                    │ │ (Path Planning & AMCL) │ │

                    │ └───────────────────────────┘ │

                    │ │

                    │ 核心: 激光匹配 + 概率建图 + 位姿优化 │

                    └──────────────────────────────────────────────┘

 

传统人工建模 vs SLAM自动建图

 

维度 人工CAD建模 SLAM自动建图

准确性 ❌ 依赖旧图纸 ✅ 现场实测

时效性 ❌ 更新滞后 ✅ 实时构建

管道识别 ❌ 容易遗漏 ✅ 自动探测

成本 ❌ 高昂（测绘） ✅ 一次投入

适用场景 新建项目 存量改造

 

二、引入痛点

 

2.1 现场的真实困境

 

场景 现场发生了什么 根因

“撞上悬空管道” “图纸没标，雷达扫不到” 2D雷达垂直盲区

“定位漂移” “走着走着就丢了位置” 里程计累积误差

“地图鬼影” “明明没墙却画出一道墙” 动态物体干扰

“储罐边缘模糊” “圆形罐画成了多边形” 分辨率不足

“建图卡顿” “数据量大，CPU跑满” 算法复杂度高

 

2.2 核心矛盾

 

车间环境的非结构化特征（大量圆柱形储罐、不规则管道、金属反光）与传统栅格地图的离散化表示之间存在矛盾。 同时，2D激光雷达虽然成本低，但对悬空障碍物（如管廊）存在天然盲区。解决方案必须结合多传感器融合（里程计+IMU）和鲁棒的扫描匹配算法，并在地图构建中引入贝叶斯概率更新，以区分静态结构与动态干扰。

2.3 我们要解决什么

 

用一段精简的 Python 程序，构建一个移动机器人SLAM建模仿真系统，实现：

 

1. 激光雷达仿真模型 —— 模拟2D LiDAR在车间环境中的扫描

2. 占据栅格地图（OGM） —— 基于概率的地图表示

3. 简易扫描匹配 —— 基于相关性或特征的位姿估计

4. 里程计运动模型 —— 模拟机器人运动与噪声

5. 贝叶斯地图更新 —— 递归更新栅格占用概率

6. 可视化 —— 实时显示机器人轨迹、激光束和地图

 

三、核心逻辑讲解

 

3.1 理论基础：占据栅格与贝叶斯更新

 

本工具基于哈工程《工业过程控制》第十三章“状态估计与滤波”和概率机器人学：

 

① 占据栅格地图（Occupancy Grid Map）

 

将环境划分为 M \times N 个栅格，每个栅格 m_{ij} 存储被占据的概率 p(m_{ij}) 。

 

② 贝叶斯更新规则

 

利用传感器观测数据 z 递归更新概率：

 

p(m|z_{1:t}) = \frac{p(z_t|m) \cdot p(m|z_{1:t-1})}{p(z_t|z_{1:t-1})}

 

对数 odds 形式（避免小数乘法）：

 

l(m|z_{1:t}) = l(m|z_{1:t-1}) + \log\frac{p(z_t|m)}{1-p(z_t|m)}

 

③ 激光雷达逆传感器模型

 

给定激光测距值 r 和角度 \theta ，计算光束路径上各点的占用概率：

 

- 端点处： p_{occ} = 0.7 \sim 0.9 

- 路径上： p_{free} = 0.3 \sim 0.1 

 

四、代码讲解（面向对象设计）

 

4.1 类结构总览

 

类名 职责 设计模式

 

"GridCell" 栅格单元（dataclass） 值对象

 

"LaserScan" 激光扫描数据（dataclass） 值对象

 

"Pose2D" 机器人位姿（dataclass） 值对象

 

"PlantFeature" 车间特征（储罐/管道） 封装

 

"SimulatedLiDAR" 模拟激光雷达 策略模式

 

"OdometryModel" 里程计运动模型 封装

 

"OccupancyGridMap" 占据栅格地图 观察者模式

 

"ScanMatcher" 扫描匹配器 策略模式

 

"SLAMEngine" SLAM核心引擎 模板方法

 

"VisualizationEngine" 可视化引擎 封装

 

"MappingSystem" 建图系统编排器（聚合根） 聚合根

 

4.2 数据模型层

 

from dataclasses import dataclass, field

from typing import List, Tuple, Optional, Dict, Set

from enum import Enum, auto

import numpy as np

import math

import matplotlib.pyplot as plt

from matplotlib.patches import Circle, Rectangle

import cv2

 

@dataclass

class Pose2D:

    """2D位姿 —— 值对象"""

    x: float = 0.0

    y: float = 0.0

    theta: float = 0.0 # 弧度

 

    def transform(self, point: np.ndarray) -> np.ndarray:

        """将点从机器人坐标系转换到世界坐标系"""

        cos_t, sin_t = math.cos(self.theta), math.sin(self.theta)

        rot_matrix = np.array([[cos_t, -sin_t], [sin_t, cos_t]])

        translated = rot_matrix @ point + np.array([self.x, self.y])

        return translated

 

@dataclass

class LaserScan:

    """激光扫描数据 —— 值对象"""

    ranges: np.ndarray # 距离数组

    angles: np.ndarray # 角度数组

    pose: Pose2D # 扫描时的位姿

    timestamp: float = 0.0

    max_range: float = 10.0

    min_range: float = 0.1

 

@dataclass

class GridCell:

    """栅格单元 —— 值对象"""

    log_odds: float = 0.0

    occupied: bool = False

 

    def update(self, inverse_sensor_model_value: float):

        """使用逆传感器模型更新log-odds"""

        self.log_odds += inverse_sensor_model_value

        # 限幅

        self.log_odds = max(-10.0, min(10.0, self.log_odds))

        self.occupied = self.log_odds > 0.0

 

    @property

    def probability(self) -> float:

        """转换为概率值"""

        return 1.0 - 1.0 / (1.0 + math.exp(self.log_odds))

 

4.3 车间环境特征建模

 

class PlantFeature:

    """车间环境特征基类"""

 

    def __init__(self, feature_id: str):

        self.id = feature_id

 

    def intersect_with_ray(self, origin: np.ndarray, direction: np.ndarray,

                          max_range: float) -> Optional[float]:

        """计算射线与该特征的交点距离"""

        raise NotImplementedError

 

    def draw(self, ax):

        """在matplotlib上绘制该特征"""

        raise NotImplementedError

 

 

class CylindricalTank(PlantFeature):

    """圆柱形储罐"""

 

    def __init__(self, feature_id: str, center: Tuple[float, float], radius: float):

        super().__init__(feature_id)

        self.center = np.array(center)

        self.radius = radius

 

    def intersect_with_ray(self, origin: np.ndarray, direction: np.ndarray,

                          max_range: float) -> Optional[float]:

        """射线与圆的交点计算"""

        # 向量 OC

        oc = origin - self.center

 

        # 二次方程系数: a*t^2 + b*t + c = 0

        a = np.dot(direction, direction)

        b = 2.0 * np.dot(oc, direction)

        c = np.dot(oc, oc) - self.radius**2

 

        discriminant = b*b - 4*a*c

 

        if discriminant < 0:

            return None

 

        sqrt_disc = math.sqrt(discriminant)

        t1 = (-b - sqrt_disc) / (2*a)

        t2 = (-b + sqrt_disc) / (2*a)

 

        # 选择最近的正解

        t = None

        if t1 > 0.01 and t1 < max_range:

            t = t1

        if t2 > 0.01 and t2 < max_range and (t is None or t2 < t1):

            t = t2

 

        return t

 

    def draw(self, ax):

        circle = Circle(self.center, self.radius, fill=False,

                      edgecolor='black', linewidth=2, label=f'Tank {self.id}')

        ax.add_patch(circle)

 

 

class PipelineSegment(PlantFeature):

    """管道线段"""

 

    def __init__(self, feature_id: str, p1: Tuple[float, float],

                 p2: Tuple[float, float], thickness: float = 0.1):

        super().__init__(feature_id)

        self.p1 = np.array(p1)

        self.p2 = np.array(p2)

        self.thickness = thickness

 

    def intersect_with_ray(self, origin: np.ndarray, direction: np.ndarray,

                          max_range: float) -> Optional[float]:

        """射线与线段的交点（简化版，忽略厚度）"""

        # 使用参数方程求解

        v1 = origin - self.p1

        v2 = self.p2 - self.p1

        v3 = np.array([-direction[1], direction[0]])

 

        dot = np.dot(v2, v3)

        if abs(dot) < 1e-6:

            return None

 

        t1 = np.cross(v2, v1) / dot

        t2 = np.dot(v1, v3) / dot

 

        if t1 > 0.01 and t1 < max_range and 0 <= t2 <= 1:

            return t1

 

        return None

 

    def draw(self, ax):

        x = [self.p1[0], self.p2[0]]

        y = [self.p1[1], self.p2[1]]

        ax.plot(x, y, 'k-', linewidth=self.thickness*20, label=f'Pipe {self.id}')

 

4.4 模拟激光雷达（策略模式）

 

class SimulatedLiDAR:

    """

    模拟激光雷达 —— 策略模式

 

    在虚拟车间环境中生成激光扫描数据

    """

 

    def __init__(self, max_range: float = 10.0, num_beams: int = 360,

                 angle_min: float = -math.pi, angle_max: float = math.pi):

        self.max_range = max_range

        self.num_beams = num_beams

        self.angle_min = angle_min

        self.angle_max = angle_max

        self.angle_increment = (angle_max - angle_min) / (num_beams - 1)

 

        self.plant_features: List[PlantFeature] = []

 

    def set_environment(self, features: List[PlantFeature]):

        """设置车间环境特征"""

        self.plant_features = features

 

    def scan(self, pose: Pose2D) -> LaserScan:

        """执行一次扫描"""

        ranges = np.full(self.num_beams, self.max_range)

        angles = np.linspace(self.angle_min, self.angle_max, self.num_beams)

 

        origin = np.array([pose.x, pose.y])

 

        for i in range(self.num_beams):

            angle = pose.theta + angles[i]

            direction = np.array([math.cos(angle), math.sin(angle)])

 

            min_dist = self.max_range

 

            # 与所有环境特征求交

            for feature in self.plant_features:

                dist = feature.intersect_with_ray(origin, direction, self.max_range)

                if dist is not None and dist < min_dist:

                    min_dist = dist

 

            ranges[i] = min_dist

 

        return LaserScan(ranges=ranges, angles=angles, pose=pose,

                        timestamp=0.0, max_range=self.max_range)

 

 

class NoisyLiDAR(SimulatedLiDAR):

    """带噪声的激光雷达（模拟真实传感器）"""

 

    def __init__(self, max_range: float = 10.0, num_beams: int = 360,

                 angle_min: float = -math.pi, angle_max: float = math.pi,

                 range_noise_std: float = 0.02, dropout_prob: float = 0.01):

        super().__init__(max_range, num_beams, angle_min, angle_max)

        self.range_noise_std = range_noise_std

        self.dropout_prob = dropout_prob

 

    def scan(self, pose: Pose2D) -> LaserScan:

        """带噪声的扫描"""

        scan = super().scan(pose)

 

        # 添加高斯噪声

        noise = np.random.normal(0, self.range_noise_std, scan.ranges.shape)

        scan.ranges = np.clip(scan.ranges + noise, self.max_range * 0.01, self.max_range)

 

        # 随机丢点

        dropout_mask = np.random.rand(self.num_beams) < self.dropout_prob

        scan.ranges[dropout_mask] = self.max_range

 

        return scan

 

4.5 里程计运动模型

 

class OdometryModel:

    """

    里程计运动模型

 

    模拟差分驱动机器人的运动与噪声

    """

 

    def __init__(self, alpha: List[float] = None):

        # 噪声参数 [alpha1, alpha2, alpha3, alpha4]

        # 对应旋转噪声、平移噪声等

        self.alpha = alpha or [0.01, 0.01, 0.01, 0.01]

 

    def sample_motion_model(self, prev_pose: Pose2D,

                           control: Tuple[float, float, float],

                           dt: float) -> Pose2D:

        """

        采样运动模型

 

        Args:

            prev_pose: 上一时刻位姿

            control: (v, w, dt) 线速度、角速度、时间

            dt: 时间步长

 

        Returns:

            新的带噪声位姿

        """

        v, w, _ = control

 

        # 真实运动（无噪声）

        delta_theta = w * dt

        delta_x = v * dt * math.cos(prev_pose.theta + delta_theta / 2)

        delta_y = v * dt * math.sin(prev_pose.theta + delta_theta / 2)

 

        # 添加噪声

        noisy_delta_x = delta_x + np.random.normal(0, self.alpha[0] * abs(v) + self.alpha[1] * abs(w))

        noisy_delta_y = delta_y + np.random.normal(0, self.alpha[0] * abs(v) + self.alpha[1] * abs(w))

        noisy_delta_theta = delta_theta + np.random.normal(0, self.alpha[2] * abs(v) + self.alpha[3] * abs(w))

 

        # 新位姿

        new_pose = Pose2D(

            x=prev_pose.x + noisy_delta_x,

            y=prev_pose.y + noisy_delta_y,

            theta=prev_pose.theta + noisy_delta_theta

        )

 

        return new_pose

 

4.6 占据栅格地图（观察者模式）

 

class OccupancyGridMap:

    """

    占据栅格地图 —— 观察者模式

 

    维护环境的概率表示

    """

 

    def __init__(self, width: int, height: int, resolution: float = 0.05):

        self.width = width

        self.height = height

        self.resolution = resolution # 米/像素

        self.origin = np.array([-width * resolution / 2, -height * resolution / 2])

 

        # 初始化栅格

        self.grid: List[List[GridCell]] = [

            [GridCell() for _ in range(width)] for _ in range(height)

        ]

 

        # 逆传感器模型参数

        self.l_occ = math.log(0.7 / 0.3) # 占用对数几率

        self.l_free = math.log(0.3 / 0.7) # 空闲对数几率

 

    def world_to_map(self, point: np.ndarray) -> Tuple[int, int]:

        """世界坐标转栅格坐标"""

        mx = int((point[0] - self.origin[0]) / self.resolution)

        my = int((point[1] - self.origin[1]) / self.resolution)

        return mx, my

 

    def map_to_world(self, mx: int, my: int) -> np.ndarray:

        """栅格坐标转世界坐标"""

        x = mx * self.resolution + self.origin[0]

        y = my * self.resolution + self.origin[1]

        return np.array([x, y])

 

    def is_valid(self, mx: int, my: int) -> bool:

        """检查栅格索引是否有效"""

        return 0 <= mx < self.width and 0 <= my < self.height

 

    def update_map(self, scan: LaserScan):

        """

        使用激光扫描数据更新地图（贝叶斯更新）

        """

        pose = scan.pose

        origin = np.array([pose.x, pose.y])

 

        for i in range(len(scan.ranges)):

            range_val = scan.ranges[i]

            angle = pose.theta + scan.angles[i]

 

            # 计算端点

            endpoint = origin + range_val * np.array([math.cos(angle), math.sin(angle)])

 

            # 将光束路径上的栅格标记为free

            self._raytrace(origin, endpoint, free=True)

 

            # 如果测距值在有效范围内，标记端点为occupied

            if range_val < scan.max_range:

                ex, ey = self.world_to_map(endpoint)

                if self.is_valid(ex, ey):

                    self.grid[ey][ex].update(self.l_occ)

 

    def _raytrace(self, start: np.ndarray, end: np.ndarray, free: bool = True):

        """

        沿射线遍历栅格

        """

        start_cell = self.world_to_map(start)

        end_cell = self.world_to_map(end)

 

        # Bresenham直线算法

        x0, y0 = start_cell

        x1, y1 = end_cell

 

        dx = abs(x1 - x0)

        dy = abs(y1 - y0)

        sx = 1 if x0 < x1 else -1

        sy = 1 if y0 < y1 else -1

        err = dx - dy

 

        while True:

            if not self.is_valid(x0, y0):

                break

 

            if free:

                self.grid[y0][x0].update(self.l_free)

 

            if x0 == x1 and y0 == y1:

                break

 

            e2 = 2 * err

            if e2 > -dy:

                err -= dy

                x0 += sx

            if e2 < dx:

                err += dx

                y0 += sy

 

    def get_probability_grid(self) -> np.ndarray:

        """获取概率网格（用于可视化）"""

        grid = np.zeros((self.height, self.width))

        for y in range(self.height):

            for x in range(self.width):

                grid[y, x] = self.grid[y][x].probability

        return grid

 

    def save_map(self, filename: str = "occupancy_map.pgm"):

        """保存为PGM格式（ROS标准）"""

        prob_grid = self.get_probability_grid()

        # 转换为0-255灰度图（0=黑=占用, 255=白=空闲）

        img = (1.0 - prob_grid) * 255

        img = img.astype(np.uint8)

        cv2.imwrite(filename, img)

        print(f"💾 地图已保存至: {filename}")

 

        # 保存YAML配置文件

        yaml_content = f"""

image: {filename}

resolution: {self.resolution}

origin: [{self.origin[0]}, {self.origin[1]}, 0.0]

negate: 0

occupied_thresh: 0.65

free_thresh: 0.196

"""

        with open(filename.replace('.pgm', '.yaml'), 'w') as f:

            f.write(yaml_content.strip())

        print(f"💾 地图配置已保存至: {filename.replace('.pgm', '.yaml')}")

 

4.7 扫描匹配器（策略模式）

 

class ScanMatcher:

    """

    扫描匹配器 —— 策略模式

 

    基于相关性优化的位姿估计

    """

 

    def __init__(self, map_ref: OccupancyGridMap, max_iterations: int = 10):

        self.map = map_ref

        self.max_iterations = max_iterations

 

    def match(self, scan: LaserScan, initial_pose: Pose2D) -> Pose2D:

        """

        执行扫描匹配

 

        Args:

            scan: 当前激光扫描

            initial_pose: 初始位姿估计（来自里程计）

 

        Returns:

            优化后的位姿

        """

        best_pose = initial_pose

        best_score = self._score_scan(scan, initial_pose)

 

        # 简单的爬山优化（实际应使用高斯牛顿或Ceres）

        search_steps = [

            (0.05, 0), (0, 0.05), (0, -0.05), (-0.05, 0), # x,y平移

            (0, 0, 0.05), (0, 0, -0.05) # 旋转

        ]

 

        for _ in range(self.max_iterations):

            improved = False

            for dx, dy, dtheta in search_steps:

                test_pose = Pose2D(

                    best_pose.x + dx,

                    best_pose.y + dy,

                    best_pose.theta + dtheta

                )

                score = self._score_scan(scan, test_pose)

 

                if score > best_score:

                    best_score = score

                    best_pose = test_pose

                    improved = True

 

            if not improved:

                break

 

        return best_pose

 

    def _score_scan(self, scan: LaserScan, pose: Pose2D) -> float:

        """

        计算扫描在给定位姿下的匹配得分

        """

        score = 0.0

        origin = np.array([pose.x, pose.y])

 

        for i in range(len(scan.ranges)):

            range_val = scan.ranges[i]

            if range_val >= scan.max_range:

                continue

 

            angle = pose.theta + scan.angles[i]

            endpoint = origin + range_val * np.array([math.cos(angle), math.sin(angle)])

 

            mx, my = self.map.world_to_map(endpoint)

            if self.map.is_valid(mx, my):

                prob = self.map.grid[my][mx].probability

                # 越接近1（占用），得分越高

                score += prob

 

        return score

 

4.8 SLAM核心引擎（模板方法）

 

class SLAMEngine:

    """

    SLAM核心引擎 —— 模板方法模式

 

    协调传感器数据、运动模型和地图更新

    """

 

    def __init__(self, map_width: int = 400, map_height: int = 400,

                 resolution: float = 0.05):

        self.odom_model = OdometryModel()

        self.grid_map = OccupancyGridMap(map_width, map_height, resolution)

利用AI解决实际问题，如果你觉得这个工具好用，欢迎关注长安牧笛！