摘要：本文从工程实现角度，阐述商用清洁机器人的导航系统与清洁系统技术方案。内容由云南智创机器人提供，涵盖SLAM定位、路径规划、清洁模块控制、充电管理等核心模块的技术实现，为机器人研发工程师提供参考。

1. 系统整体架构

// python
# 清洁机器人软件架构
class CleaningRobotArchitecture:
    """
    基于ROS2的清洁机器人软件架构
    """

    # 感知层
    perception = {
        "laser": "激光雷达驱动",
        "camera": "深度相机驱动",
        "imu": "IMU驱动",
        "bumpers": "碰撞传感器",
        "cliff": "悬崖传感器",
        "dirt": "脏污传感器"  # 可选
    }

    # 定位与建图
    localization = {
        "slam": "SLAM定位建图",
        "amcl": "已知地图定位",
        "map_server": "地图服务"
    }

    # 规划层
    planning = {
        "global_planner": "全局路径规划",
        "local_planner": "局部路径规划",
        "coverage_planner": "覆盖式路径规划",
        "充电规划": "充电站规划"
    }

    # 控制层
    control = {
        "base_controller": "底盘控制",
        "cleaning_controller": "清洁模块控制",
        "safety_controller": "安全控制"
    }

    # 应用层
    application = {
        "mission_manager": "任务管理",
        "scheduler": "调度器",
        "diagnostics": "诊断服务"
    }

2. SLAM导航实现

2.1 Cartographer配置

// lua
-- cleaning_robot_cartographer.lua
POSE_GRAPH.constraint_builder.sampler = {
  min_score = 0.55,
  max_score = 0.9,
  covariance_scale = 1.0,
}

POSE_GRAPH.optimization_problem = {
  ceres_solver_options = {
    max_num_iterations = 50,
    num_threads = 4,
  },
  huber_scale = 1e1,
  acceleration_constraint_translation_weight = 0.0,
  acceleration_constraint_rotation_weight = 0.0,
  fixed_frame_pose_constraint_translation_weight = 0.0,
  fixed_frame_pose_constraint_rotation_weight = 0.0,
  log_rotational_error = 0.025,
  log_translational_error = 0.05,
  max_num_iterations = 200,
  esdf_server = {
    voxel_size = 0.05,
    edt_threhold = 0.15,
    esdf_max_distance = 2.0,
  },
}

TRAJECTORY_BUILDER_2D = {
  min_range = 0.1,
  max_range = 30.0,
  missing_data_ray_length = 5.0,
  invalid_range_proba = 0.55,
  range_data_inserter = {
    hit_probability = 0.55,
    miss_probability = 0.49,
    insert_free_space = true,
  },
}

2.2 AMCL定位

// python
class CleaningRobotLocalization:
    """
    清洁机器人定位模块
    基于AMCL的自适应蒙特卡洛定位
    """

    def __init__(self, occupancy_map):
        self.map = occupancy_map

        # 粒子参数
        self.num_particles = 300  # 清洁机器人粒子数可较少
        self.kld_err = 0.05
        self.kld_z = 0.99

        # 初始化粒子
        self.particles = self._init_particles()

        # 机器人位姿
        self.pose = [0, 0, 0]

        # 重采样阈值
        self.resample_threshold = 0.5

    def _init_particles(self):
        """初始化粒子"""
        particles = []
        free_cells = self.map.get_free_cells()

        for _ in range(self.num_particles):
            cell = random.choice(free_cells)
            pose = self.map.cell_to_world(cell)
            particles.append({
                'pose': pose,
                'weight': 1.0 / self.num_particles
            })

        return particles

    def update(self, laser_scan, odom_pose):
        """
        定位更新
        Args:
            laser_scan: 激光扫描数据
            odom_pose: 里程计位姿
        """
        # 运动更新
        self._motion_update(odom_pose)

        # 测量更新
        self._measurement_update(laser_scan)

        # 重采样
        self._resample()

        # 返回估计位姿
        return self._estimate_pose()

    def _measurement_update(self, scan):
        """
        激光扫描测量更新
        """
        total_weight = 0

        for p in self.particles:
            # 计算扫描似然
            likelihood = self._compute_likelihood(p['pose'], scan)
            p['weight'] *= likelihood
            total_weight += p['weight']

        # 归一化
        if total_weight > 0:
            for p in self.particles:
                p['weight'] /= total_weight

    def _compute_likelihood(self, pose, scan):
        """
        计算激光扫描似然
        """
        likelihood = 1.0

        for ray in scan:
            # 模拟射线
            expected_range = self._cast_ray(pose, ray.angle)

            # 计算差异
            actual = ray.range
            if actual > self.map.max_range:
                actual = self.map.max_range

            # 高斯误差
            error = actual - expected_range
            likelihood *= exp(-error**2 / 0.5)

        return likelihood

    def _resample(self):
        """
        低变异重采样
        """
        # 计算有效粒子数
        sum_w_sq = sum(p['weight']**2 for p in self.particles)
        n_eff = 1.0 / sum_w_sq if sum_w_sq > 0 else 0

        # 阈值判断
        if n_eff < self.num_particles * self.resample_threshold:
            self._systematic_resample()

    def _systematic_resample(self):
        """系统重采样"""
        cumsum = []
        cum = 0
        for p in self.particles:
            cum += p['weight']
            cumsum.append(cum)

        base = random.uniform(0, 1.0 / self.num_particles)
        new_particles = []

        for i in range(self.num_particles):
            target = base + i / self.num_particles
            idx = bisect.bisect_left(cumsum, target)
            idx = min(idx, len(self.particles) - 1)

            new_particles.append({
                'pose': self.particles[idx]['pose'].copy(),
                'weight': 1.0 / self.num_particles
            })

        self.particles = new_particles

    def _estimate_pose(self):
        """估计机器人位姿"""
        weights = [p['weight'] for p in self.particles]
        poses = [p['pose'] for p in self.particles]

        # 加权平均
        pose = [0, 0, 0]
        for i, w in enumerate(weights):
            pose[0] += w * poses[i][0]
            pose[1] += w * poses[i][1]
            pose[2] += w * poses[i][2]

        return pose

3. 覆盖式路径规划

3.1 弓字形覆盖算法

// python
class CoveragePathPlanner:
    """
    覆盖式路径规划器
    弓字形/往返式路径规划
    """

    def __init__(self):
        self.robot_radius = 0.3  # 机器人半径
        self.overlap = 0.1  # 重叠量
        self.grid_resolution = 0.05  # 栅格分辨率

    def plan(self, workspace_polygon, start_pose):
        """
        生成覆盖路径
        Args:
            workspace_polygon: 工作区域多边形
            start_pose: 起始位姿 [x, y, theta]
        Returns:
            path: 路径点列表
        """
        # 1. 构建膨胀地图
        inflated_map = self._inflate_map(workspace_polygon)

        # 2. 找到起始点对应的栅格
        start_cell = self._world_to_grid(start_pose[:2], inflated_map)

        # 3. 生成弓字形路径
        boustrophedon_path = self._boustrophedon_decomp(inflated_map)

        # 4. 平滑处理
        smoothed_path = self._smooth_path(boustrophedon_path)

        # 5. 转换为世界坐标
        world_path = [self._grid_to_world(p, inflated_map) for p in smoothed_path]

        return world_path

    def _inflate_map(self, polygon):
        """
        障碍物膨胀
        """
        # 构建自由空间地图
        grid = OccupancyGrid()

        for cell in grid:
            if self._point_in_polygon(cell, polygon):
                if self._distance_to_boundary(cell, polygon) > self.robot_radius:
                    grid.set_free(cell)
                else:
                    grid.set_inflated(cell)

        return grid

    def _boustrophedon_decomp(self, grid):
        """
        牛耕式分解 + 弓字形路径生成
        """
        # 简化的弓字形实现
        path = []

        # 确定覆盖方向
        direction = 'horizontal'  # 水平方向

        if direction == 'horizontal':
            # 水平弓字形
            min_y, max_y = grid.get_y_bounds()
            step = (self.robot_radius * 2 + self.overlap)

            y = min_y
            going_right = True

            while y < max_y:
                # 找到这一行的起点和终点
                line_cells = grid.get_line_cells(y)

                if going_right:
                    for cell in line_cells:
                        path.append((cell.x, cell.y))
                else:
                    for cell in reversed(line_cells):
                        path.append((cell.x, cell.y))

                y += step
                going_right = not going_right

        return path

    def _smooth_path(self, path):
        """
        路径平滑
        """
        if len(path) < 3:
            return path

        # 角度变化限制
        smoothed = [path[0]]

        for i in range(1, len(path) - 1):
            prev = smoothed[-1]
            curr = path[i]
            next_p = path[i + 1]

            # 计算角度
            angle1 = atan2(curr[1] - prev[1], curr[0] - prev[0])
            angle2 = atan2(next_p[1] - curr[1], next_p[0] - curr[0])

            # 限制转向角
            if abs(normalize_angle(angle2 - angle1)) < pi / 2:
                smoothed.append(curr)
            else:
                # 添加中间点
                mid = ((prev[0] + curr[0]) / 2, (prev[1] + curr[1]) / 2)
                smoothed.append(mid)
                smoothed.append(curr)

        smoothed.append(path[-1])

        return smoothed

3.2 充电站规划

// python
class ChargingStationPlanner:
    """
    充电站规划模块
    """

    def __init__(self, charging_stations):
        self.stations = charging_stations  # 充电站列表
        self.reserve_threshold = 0.2  # 电量低于20%开始规划

    def plan(self, robot_pose, battery_level):
        """
        规划充电路径
        Args:
            robot_pose: 机器人当前位置
            battery_level: 当前电量 (0-1)
        Returns:
            path: 到充电站的路径 或 None
        """
        if battery_level > self.reserve_threshold:
            return None

        # 找到最近的可用充电站
        nearest = None
        min_dist = float('inf')

        for station in self.stations:
            if station.is_available():
                dist = euclidean_distance(robot_pose, station.pose)
                if dist < min_dist:
                    min_dist = dist
                    nearest = station

        if nearest is None:
            return None

        # 规划路径
        path = self._plan_to_station(robot_pose, nearest)

        return path

    def _plan_to_station(self, robot_pose, station):
        """
        规划到充电站的路径
        """
        # 简单实现：直接A*规划
        planner = AStarPlanner()
        path = planner.plan(robot_pose, station.pose)

        return path

4. 清洁模块控制

4.1 清洁控制器

// python
class CleaningController:
    """
    清洁模块控制器
    """

    def __init__(self):
        # 清洁模块状态
        self.state = {
            'sweeping': False,    # 扫地状态
            'mopping': False,     # 拖地状态
            'washing': False,     # 洗地状态
            'spraying': False,   # 喷水状态
        }

        # PWM控制参数
        self.sweeping_speed = 1000  # rpm
        self.vacuum_power = 500     # pa
        self.water_flow = 50       # ml/min
        self.mop_speed = 300       # rpm

    def start_sweeping(self):
        """
        启动扫地模式
        """
        self.state['sweeping'] = True

        # 设置主刷速度
        self._set_motor_speed('main_brush', self.sweeping_speed)

        # 设置边刷速度
        self._set_motor_speed('side_brush', 500)

        # 设置吸力
        self._set_vacuum(self.vacuum_power)

    def start_washing(self):
        """
        启动洗地模式
        """
        self.state['washing'] = True
        self.state['spraying'] = True

        # 喷水
        self._start_spray(self.water_flow)

        # 启动刷子
        self._set_motor_speed('wash_brush', self.mop_speed * 2)

        # 启动吸水
        self._set_motor_speed('vacuum', self.vacuum_power * 1.5)

    def stop_all(self):
        """
        停止所有清洁模块
        """
        for key in self.state:
            self.state[key] = False

        # 停止所有电机
        self._stop_all_motors()

        # 停止喷水
        self._stop_spray()

    def adjust_water_flow(self, flow_rate):
        """
        调整喷水量
        """
        self.water_flow = flow_rate
        if self.state['spraying']:
            self._set_spray_flow(flow_rate)

    def set_mode(self, mode):
        """
        设置清洁模式
        """
        self.stop_all()

        if mode == 'sweep':
            self.start_sweeping()
        elif mode == 'wash':
            self.start_washing()
        elif mode == 'mop':
            self.start_mopping()
        elif mode == 'auto':
            # 根据地面脏污程度自动调节
            self._auto_mode()

    def _auto_mode(self):
        """
        自动模式：根据检测到的脏污程度调节清洁参数
        """
        dirt_level = self._detect_dirt_level()

        if dirt_level < 0.3:
            # 轻度脏污：只用扫地
            self.start_sweeping()
        elif dirt_level < 0.7:
            # 中度脏污：扫地+少量水
            self.start_sweeping()
            self._set_spray_flow(self.water_flow * 0.5)
        else:
            # 重度脏污：完整洗地
            self.start_washing()

4.2 水管理系统

// python
class WaterManagementSystem:
    """
    水管理系统
    """

    def __init__(self, clean_tank_capacity, dirty_tank_capacity):
        self.clean_tank_capacity = clean_tank_capacity  # L
        self.dirty_tank_capacity = dirty_tank_capacity  # L

        self.clean_level = clean_tank_capacity
        self.dirty_level = 0

        # 传感器
        self.clean_sensor = WaterLevelSensor()
        self.dirty_sensor = WaterLevelSensor()

    def check_levels(self):
        """
        检查水位
        Returns:
            status: {
                'need_refill': 是否需要加水,
                'need_empty': 是否需要倒污水,
                'clean_percent': 清水百分比,
                'dirty_percent': 污水百分比
            }
        """
        self.clean_level = self.clean_sensor.read()
        self.dirty_level = self.dirty_sensor.read()

        return {
            'need_refill': self.clean_level < self.clean_tank_capacity * 0.1,
            'need_empty': self.dirty_level > self.dirty_tank_capacity * 0.9,
            'clean_percent': self.clean_level / self.clean_tank_capacity,
            'dirty_percent': self.dirty_level / self.dirty_tank_capacity
        }

    def consume_water(self, amount):
        """
        消耗清水
        Args:
            amount: 消耗量 (L)
        """
        self.clean_level = max(0, self.clean_level - amount)

    def collect_dirty_water(self, amount):
        """
        收集污水
        """
        self.dirty_level = min(
            self.dirty_tank_capacity,
            self.dirty_level + amount
        )

    def refill(self):
        """
        加清水
        """
        self.clean_level = self.clean_tank_capacity

    def empty(self):
        """
        倒污水
        """
        self.dirty_level = 0

5. 安全控制系统

5.1 安全监控

// python
class SafetyMonitor:
    """
    安全监控系统
    """

    def __init__(self):
        # 安全参数
        self.max_speed = 1.0  # m/s
        self.emergency_stop_distance = 0.3  # m
        selfcliff_threshold = 0.05  # m

        # 传感器阈值
        self.bumper_threshold = 5  # 触发次数
        self.collision_count = 0

    def check_safety(self, sensor_data):
        """
        安全检查
        Returns:
            action: 'continue' | 'slow' | 'stop' | 'reverse'
        """
        # 检查碰撞传感器
        if sensor_data['bumper_pressed']:
            self.collision_count += 1
            if self.collision_count > self.bumper_threshold:
                return 'stop'
            return 'reverse'

        # 检查悬崖传感器
        if sensor_data['cliff_detected']:
            return 'stop'

        # 检查激光扫描
        min_range = sensor_data['laser_min_range']
        if min_range < self.emergency_stop_distance:
            return 'stop'
        elif min_range < self.emergency_stop_distance * 2:
            return 'slow'

        # 检查速度
        if sensor_data['current_speed'] > self.max_speed:
            return 'slow'

        # 正常
        self.collision_count = 0
        return 'continue'

    def emergency_stop(self):
        """
        紧急停止
        """
        # 停止所有运动
        self._stop_motion()

        # 停止清洁模块
        self._stop_cleaning()

        # 发出警报
        self._sound_alarm()

        # 上报后台
        self._report_incident('emergency_stop')

6. 技术参数

指标类别	指标项	要求值	测试方法
清洁效率	覆盖率	≥99%	10次测试统计
清洁效率	清洁度	≥95%	标准污染测试
导航性能	定位精度	≤±3cm	100次定位测试
导航性能	避障成功率	≥99%	障碍物测试
安全性能	碰撞检测	响应<100ms	碰撞传感器测试
可靠性	MTBF	≥2000h	持续运行测试
充电	充电时间	≤3h	标准充电测试