扫地机器人核心是全覆盖路径规划 CCPP（Complete Coverage Path Planning），分为：综述类、国内期刊（全覆盖 / 改进算法）、外文经典、SLAM + 规划工程类，适合毕设、课题调研，全部聚焦家庭室内家居场景。
关键指标：覆盖率、路径重复率、运行时间、内存开销，是扫地机器人区别普通点到点路径规划的重点。
一、中文综述（快速入门，必看）
- 移动机器人全覆盖路径规划算法综述
适合开题，梳理 Boustrophedon 分割、细胞分解、栅格遍历、智能算法、SLAM 融合；讲清扫地机主流：弓字形、随机、分区遍历优缺点。 检索：知网，大量中文毕设引用。
- 扫地机器人路径规划算法研究综述J 科技与创新
专门针对家庭扫地机器人，对比随机碰撞、螺旋、Boustrophedon、A*‑DWA、蚁群，分析实际家用环境（家具、狭小房间）痛点。
二、国内核心期刊论文（家庭扫地机，全覆盖 CCPP，可直接做复现）
1）分区 + 细胞分解（主流商用思路，石头 / 科沃斯底层思想）
- 许伦辉，林世城. 基于分治思想的扫地机器人全覆盖路径规划算法研究J. 广西师范大学学报 (自然科学版),2021,39 (6):54‑62.
- 亮点：针对家庭零散障碍物，分治分割生成弓形清扫，降低路径重复率；栅格仿真，适合毕设复现。
- 郭峻宇等. 基于静态地图扫地机器人的路径规划J. 上海工程技术大学学报，2026.
- 亮点：细胞自动机 + 改进蚁群，子区域串联，TSP 旅行商问题优化房间遍历顺序；重复率对比实验完整。
2）启发式 A*/DWA 融合（全局 + 局部避障，SLAM 地图后规划）
- 谢坤霖，李宗根. 基于启发式搜索算法的扫地机器人路径规划J. 西华大学学报，2019,38 (4):69‑76.
- 亮点：改进 A * 用于扫地机全覆盖，栅格仿真，分析家庭多障碍物场景，算法对比清晰，适合入门复现。
- 刘智超. Indoor Robot Path Planning Incorporating Improved A Algorithms and DWA*J. 制造业自动化，中文核心.
- 亮点：全局改进 A* + 局部 DWA 动态避障；适配家庭动态障碍物（椅子、拖鞋），解决静态规划无法应对家中移动物体。
3）仿生智能算法（蚁群、遗传，适合做改进创新点）
- 蒋玉杰，曾岑. 清洁机器人基于遗传算法的全区域路径规划J. 机械制造，2009.
早期经典，遗传算法做全覆盖，适合做对比基线。
三、外文经典论文（CCPP 基础 + 现代改进）
✨全覆盖经典奠基（必引）
- Boustrophedon Cellular Decomposition for Coverage Path Planning（1998）
Choset H, Pignon P. 扫地机器人分区清扫的鼻祖，Boustrophedon 牛耕式细胞分解，几乎所有商用扫地机分区清扫的理论源头。
- Complete Coverage Path Planning: A Survey
全覆盖综述，梳理细胞分解、栅格遍历、随机遍历，适合外文参考文献。
✨面向家庭扫地机器人
- Path planning algorithm development for autonomous vacuum cleaner robots (2014)ResearchGa...
对比随机行走、螺旋、S 型弓字形、沿墙行走 4 种早期扫地机策略；理解初代无 SLAM 扫地机工作原理。
- Si H, Miao Z. A Map Segmentation Method Based on Image Processing for Robot Complete Coverage OperationJ.Journal of Field Robotics,2025Wiley Onli....
2025 最新，基于图像处理做地图分割，处理家庭凹型房间、家具遮挡，适合前沿参考。
- Liu H, Zhang Y. ASL‑DWA: An Improved A‑Star Algorithm for Indoor Cleaning Robots, IEEE Access,2022Nature.
改进 A*‑DWA，面向室内清洁机器人；降低路径冗余，动态障碍物响应，工程性强。
四、SLAM + 规划联合（真实扫地机器人完整链路，工程方向）
家用扫地机器人不是单纯路径规划，是SLAM 建图 →地图分割 →全覆盖路径 →局部动态避障完整链路。
- Autonomous Visual Navigation System Based on a Single Camera for Floor‑Sweeping Robot, Applied Sciences,2023ResearchGa... 单目视觉 SLAM + 垃圾识别 + 局部动态规划；仿真基于 TurtleBot，适合做视觉扫地机器人课题。
注意：激光 SLAM 常用 GMapping、Cartographer，再在上层做 CCPP 全覆盖。
五、选题 / 复现建议（毕设参考）
- 简单复现：Boustrophedon 分割 + 弓字形遍历（Matlab/Python 栅格仿真）
- 中等创新：改进 A*+DWA，全局全覆盖，局部动态避障；对比覆盖率、重复率
- 高级创新：地图分割 + TSP 优化房间清扫顺序，模拟多房间家庭；加入电池回充约束
六、检索关键词（知网、IEEE Xplore、Semantic Scholar）
中文：
扫地机器人；全覆盖路径规划；CCPP；Boustrophedon；细胞分解；栅格地图；路径重复率
英文：
Sweeping robot / Vacuum cleaner robot; Complete Coverage Path Planning(CCPP); Boustrophedon cellular decomposition; indoor household environment
python
"""Furnished-apartment coverage planning and 3D robot-vacuum simulation."""
from __future__ import annotations
import argparse
import csv
import heapq
import math
import time
from collections import deque
from dataclasses import dataclass
from pathlib import Path
from typing import Iterable, Sequence
import matplotlib
import matplotlib.pyplot as plt
import numpy as np
from matplotlib.animation import FuncAnimation, PillowWriter
from matplotlib.collections import LineCollection
from matplotlib.font_manager import fontManager
from matplotlib.lines import Line2D
from matplotlib.patches import Circle, Rectangle
from mpl_toolkits.mplot3d.art3d import Poly3DCollection
GridCell = tuple[int, int]
@dataclass(frozen=True)
class RectObject:
name: str
xmin: float
xmax: float
ymin: float
ymax: float
height: float
color: str
kind: str = "furniture"
@dataclass(frozen=True)
class Room:
name: str
xmin: float
xmax: float
ymin: float
ymax: float
color: str
@dataclass
class PlanResult:
path: list[GridCell]
reachable: np.ndarray
cleaned: np.ndarray
dock: GridCell
path_length_m: float
coverage_percent: float
reachable_area_m2: float
planning_time_s: float
cleanup_targets: int
class Apartment:
"""Apartment geometry and its collision-inflated occupancy grid."""
width = 12.0
depth = 9.0
def __init__(
self,
resolution: float = 0.25,
robot_radius: float = 0.23,
safety_margin: float = 0.06,
) -> None:
self.resolution = resolution
self.robot_radius = robot_radius
self.safety_margin = safety_margin
self.clearance = robot_radius + safety_margin
self.rows = int(round(self.depth / resolution))
self.cols = int(round(self.width / resolution))
self.x_centers = (np.arange(self.cols) + 0.5) * resolution
self.y_centers = (np.arange(self.rows) + 0.5) * resolution
self.rooms = [
Room("客厅", 0.15, 7.12, 0.15, 5.72, "#e8f0ed"),
Room("卧室", 7.28, 11.85, 0.15, 5.72, "#e9edf6"),
Room("餐厅与玄关", 0.15, 11.85, 5.88, 8.85, "#f4eee3"),
]
self.walls = self._build_walls()
self.furniture = self._build_furniture()
self.objects = self.walls + self.furniture
self.occupancy = self._build_occupancy()
def _build_walls(self) -> list[RectObject]:
wall_color = "#697078"
walls = [
RectObject("左外墙", 0.00, 0.15, 0.00, 9.00, 0.72, wall_color, "wall"),
RectObject("右外墙", 11.85, 12.00, 0.00, 9.00, 0.72, wall_color, "wall"),
RectObject("下外墙", 0.00, 12.00, 0.00, 0.15, 0.72, wall_color, "wall"),
RectObject("上外墙", 0.00, 12.00, 8.85, 9.00, 0.72, wall_color, "wall"),
# Horizontal wall, with two 1.6 m door openings. The openings
# remain traversable after robot-radius collision inflation.
RectObject("客厅北墙", 0.00, 2.45, 5.72, 5.88, 0.62, wall_color, "wall"),
RectObject("中部北墙", 4.05, 8.00, 5.72, 5.88, 0.62, wall_color, "wall"),
RectObject("卧室北墙", 9.60, 12.00, 5.72, 5.88, 0.62, wall_color, "wall"),
# Vertical wall, with a 1.2 m living-room/bedroom door opening.
RectObject("卧室西墙下", 7.12, 7.28, 0.00, 2.10, 0.62, wall_color, "wall"),
RectObject("卧室西墙上", 7.12, 7.28, 3.35, 5.88, 0.62, wall_color, "wall"),
]
return walls
def _build_furniture(self) -> list[RectObject]:
return [
RectObject("沙发", 0.65, 3.20, 4.30, 5.35, 0.78, "#477f7a"),
RectObject("茶几", 2.10, 3.65, 2.65, 3.55, 0.42, "#b77a42"),
RectObject("电视柜", 5.95, 6.75, 4.45, 5.42, 0.52, "#454b50"),
RectObject("落地灯", 0.70, 1.15, 2.45, 2.90, 1.35, "#d0a83f"),
RectObject("床", 8.30, 11.35, 3.10, 5.00, 0.48, "#587ba8"),
RectObject("床头柜", 7.55, 8.15, 4.25, 5.00, 0.58, "#9a704f"),
RectObject("衣柜", 10.75, 11.65, 0.55, 2.40, 1.75, "#826d5f"),
RectObject("书桌", 7.65, 9.45, 0.55, 1.25, 0.76, "#a06c3f"),
RectObject("餐桌", 4.35, 6.20, 6.55, 8.05, 0.74, "#a66c3f"),
RectObject("餐椅1", 3.70, 4.20, 6.75, 7.30, 0.82, "#66806a"),
RectObject("餐椅2", 6.35, 6.85, 6.75, 7.30, 0.82, "#66806a"),
RectObject("餐椅3", 7.20, 7.80, 7.95, 8.45, 0.82, "#66806a"),
RectObject("鞋柜", 0.55, 1.90, 7.95, 8.58, 1.05, "#80746a"),
RectObject("边柜", 9.80, 11.35, 7.90, 8.55, 0.88, "#9b7d59"),
]
def _build_occupancy(self) -> np.ndarray:
x_grid, y_grid = np.meshgrid(self.x_centers, self.y_centers)
occupied = np.zeros((self.rows, self.cols), dtype=bool)
for item in self.objects:
occupied |= (
(x_grid >= item.xmin - self.clearance)
& (x_grid <= item.xmax + self.clearance)
& (y_grid >= item.ymin - self.clearance)
& (y_grid <= item.ymax + self.clearance)
)
return occupied
def cell_to_xy(self, cell: GridCell) -> tuple[float, float]:
row, col = cell
return float(self.x_centers[col]), float(self.y_centers[row])
def nearest_free_cell(self, x: float, y: float) -> GridCell:
free_rows, free_cols = np.where(~self.occupancy)
if free_rows.size == 0:
raise RuntimeError("地图中没有可用的自由栅格。")
distances = (
(self.x_centers[free_cols] - x) ** 2
+ (self.y_centers[free_rows] - y) ** 2
)
index = int(np.argmin(distances))
return int(free_rows[index]), int(free_cols[index])
def valid_neighbors(self, cell: GridCell) -> Iterable[tuple[GridCell, float]]:
row, col = cell
for dr, dc in (
(-1, 0),
(1, 0),
(0, -1),
(0, 1),
(-1, -1),
(-1, 1),
(1, -1),
(1, 1),
):
nr, nc = row + dr, col + dc
if not (0 <= nr < self.rows and 0 <= nc < self.cols):
continue
if self.occupancy[nr, nc]:
continue
if dr != 0 and dc != 0:
# A diagonal move may not cut through a furniture/wall corner.
if self.occupancy[row, nc] or self.occupancy[nr, col]:
continue
yield (nr, nc), math.sqrt(2.0) if dr and dc else 1.0
def room_at(self, x: float, y: float) -> str:
for room in self.rooms:
if room.xmin <= x <= room.xmax and room.ymin <= y <= room.ymax:
return room.name
return "门厅/通道"
class CoveragePlanner:
"""Boustrophedon room sweeps connected by collision-safe A* paths."""
def __init__(self, apartment: Apartment, cleaning_width: float = 0.52) -> None:
self.apartment = apartment
self.cleaning_width = cleaning_width
self.cleaning_radius = cleaning_width / 2.0
self.row_stride = max(
1, int(math.floor(cleaning_width / apartment.resolution + 1e-9))
)
def astar(self, start: GridCell, goal: GridCell) -> list[GridCell]:
if start == goal:
return [start]
def heuristic(cell: GridCell) -> float:
dr = abs(cell[0] - goal[0])
dc = abs(cell[1] - goal[1])
return max(dr, dc) + (math.sqrt(2.0) - 1.0) * min(dr, dc)
open_heap: list[tuple[float, float, GridCell]] = []
heapq.heappush(open_heap, (heuristic(start), 0.0, start))
best_cost: dict[GridCell, float] = {start: 0.0}
parent: dict[GridCell, GridCell] = {}
closed: set[GridCell] = set()
while open_heap:
_, cost, current = heapq.heappop(open_heap)
if current in closed:
continue
if current == goal:
path = [goal]
while path[-1] != start:
path.append(parent[path[-1]])
path.reverse()
return path
closed.add(current)
for neighbor, move_cost in self.apartment.valid_neighbors(current):
candidate = cost + move_cost
if candidate + 1e-12 < best_cost.get(neighbor, math.inf):
best_cost[neighbor] = candidate
parent[neighbor] = current
heapq.heappush(
open_heap,
(candidate + heuristic(neighbor), candidate, neighbor),
)
raise RuntimeError(f"A* 无法连接栅格 {start} 和 {goal}。")
def reachable_from(self, start: GridCell) -> np.ndarray:
reachable = np.zeros_like(self.apartment.occupancy)
reachable[start] = True
queue: deque[GridCell] = deque([start])
while queue:
cell = queue.popleft()
for neighbor, _ in self.apartment.valid_neighbors(cell):
if not reachable[neighbor]:
reachable[neighbor] = True
queue.append(neighbor)
return reachable
@staticmethod
def _extend_path(path: list[GridCell], addition: Sequence[GridCell]) -> None:
if not addition:
return
start_index = 1 if path and path[-1] == addition[0] else 0
for cell in addition[start_index:]:
if not path or path[-1] != cell:
path.append(cell)
def _room_sweep_segments(
self, room: Room, reachable: np.ndarray
) -> list[list[GridCell]]:
a = self.apartment
row_candidates = np.where(
(a.y_centers >= room.ymin) & (a.y_centers <= room.ymax)
)[0]
valid_rows = [
int(row)
for row in row_candidates
if np.any(
reachable[row]
& (a.x_centers >= room.xmin)
& (a.x_centers <= room.xmax)
)
]
if not valid_rows:
return []
selected_rows = valid_rows[:: self.row_stride]
# Ensure the far room edge remains within one cleaning radius.
if valid_rows[-1] - selected_rows[-1] > self.row_stride // 2:
selected_rows.append(valid_rows[-1])
segments: list[list[GridCell]] = []
for row_number, row in enumerate(selected_rows):
valid_columns = np.where(
reachable[row]
& (a.x_centers >= room.xmin)
& (a.x_centers <= room.xmax)
)[0]
if valid_columns.size == 0:
continue
split_points = np.where(np.diff(valid_columns) > 1)[0] + 1
runs = [part for part in np.split(valid_columns, split_points) if part.size]
left_to_right = row_number % 2 == 0
ordered_runs = runs if left_to_right else list(reversed(runs))
for run in ordered_runs:
columns = run if left_to_right else run[::-1]
segments.append([(row, int(col)) for col in columns])
return segments
def _mark_cleaned(
self, cleaned: np.ndarray, cells: Iterable[GridCell], reachable: np.ndarray
) -> None:
a = self.apartment
offset_limit = int(math.ceil(self.cleaning_radius / a.resolution))
offsets: list[tuple[int, int]] = []
for dr in range(-offset_limit, offset_limit + 1):
for dc in range(-offset_limit, offset_limit + 1):
distance = math.hypot(dr * a.resolution, dc * a.resolution)
if distance <= self.cleaning_radius + 1e-9:
offsets.append((dr, dc))
for row, col in cells:
for dr, dc in offsets:
nr, nc = row + dr, col + dc
if (
0 <= nr < a.rows
and 0 <= nc < a.cols
and reachable[nr, nc]
):
cleaned[nr, nc] = True
def plan(self, dock_xy: tuple[float, float] = (0.65, 0.65)) -> PlanResult:
started_at = time.perf_counter()
a = self.apartment
dock = a.nearest_free_cell(*dock_xy)
reachable = self.reachable_from(dock)
path: list[GridCell] = [dock]
# Each room is a coverage cell. Within it, alternate sweep direction.
for room in a.rooms:
for segment in self._room_sweep_segments(room, reachable):
connector = self.astar(path[-1], segment[0])
self._extend_path(path, connector)
self._extend_path(path, segment)
cleaned = np.zeros_like(reachable)
self._mark_cleaned(cleaned, path, reachable)
# Door thresholds and narrow residual pockets may fall between nominal
# sweep rows. Visit only those residual cells before returning to dock.
cleanup_targets = 0
max_cleanup_targets = int(np.count_nonzero(reachable))
while cleanup_targets < max_cleanup_targets:
uncovered = np.argwhere(reachable & ~cleaned)
if uncovered.size == 0:
break
current = np.array(path[-1])
nearest_index = int(
np.argmin(np.sum((uncovered - current[None, :]) ** 2, axis=1))
)
target = tuple(int(value) for value in uncovered[nearest_index])
connector = self.astar(path[-1], target)
self._extend_path(path, connector)
self._mark_cleaned(cleaned, connector, reachable)
cleanup_targets += 1
self._extend_path(path, self.astar(path[-1], dock))
self._mark_cleaned(cleaned, path[-1:], reachable)
path_length = sum(
math.hypot(b[0] - c[0], b[1] - c[1]) * a.resolution
for c, b in zip(path, path[1:])
)
reachable_count = int(np.count_nonzero(reachable))
cleaned_count = int(np.count_nonzero(cleaned & reachable))
coverage = 100.0 * cleaned_count / reachable_count
result = PlanResult(
path=path,
reachable=reachable,
cleaned=cleaned,
dock=dock,
path_length_m=path_length,
coverage_percent=coverage,
reachable_area_m2=reachable_count * a.resolution**2,
planning_time_s=time.perf_counter() - started_at,
cleanup_targets=cleanup_targets,
)
self.validate(result)
return result
def validate(self, result: PlanResult) -> None:
a = self.apartment
if result.path[0] != result.dock or result.path[-1] != result.dock:
raise AssertionError("规划路径没有从充电座出发并返回充电座。")
x_grid, y_grid = np.meshgrid(a.x_centers, a.y_centers)
for room in a.rooms:
room_free = (
~a.occupancy
& (x_grid >= room.xmin)
& (x_grid <= room.xmax)
& (y_grid >= room.ymin)
& (y_grid <= room.ymax)
)
free_count = int(np.count_nonzero(room_free))
reachable_count = int(np.count_nonzero(room_free & result.reachable))
if free_count and reachable_count / free_count < 0.75:
raise AssertionError(
f"{room.name} 只有 {reachable_count / free_count:.1%} 的自由区域"
"与充电座连通。"
)
for current, following in zip(result.path, result.path[1:]):
if a.occupancy[current] or a.occupancy[following]:
raise AssertionError("规划路径与膨胀后的障碍物碰撞。")
dr = following[0] - current[0]
dc = following[1] - current[1]
if max(abs(dr), abs(dc)) > 1 or (dr == 0 and dc == 0):
raise AssertionError("规划路径含有不连续的栅格跳跃。")
if dr and dc:
if a.occupancy[current[0], following[1]] or a.occupancy[
following[0], current[1]
]:
raise AssertionError("规划路径斜向穿过了障碍物拐角。")
if result.coverage_percent < 99.999:
raise AssertionError(
f"覆盖率仅为 {result.coverage_percent:.3f}%，未达到完整覆盖。"
)
def configure_chinese_font() -> None:
available = {font.name for font in fontManager.ttflist}
candidates = [
"Microsoft YaHei",
"SimHei",
"Noto Sans CJK SC",
"Source Han Sans SC",
"Arial Unicode MS",
]
selected = next((name for name in candidates if name in available), "DejaVu Sans")
matplotlib.rcParams["font.sans-serif"] = [selected, "DejaVu Sans"]
matplotlib.rcParams["axes.unicode_minus"] = False
def box_faces(item: RectObject) -> list[list[tuple[float, float, float]]]:
x0, x1, y0, y1, z1 = (
item.xmin,
item.xmax,
item.ymin,
item.ymax,
item.height,
)
p000, p100 = (x0, y0, 0.0), (x1, y0, 0.0)
p110, p010 = (x1, y1, 0.0), (x0, y1, 0.0)
p001, p101 = (x0, y0, z1), (x1, y0, z1)
p111, p011 = (x1, y1, z1), (x0, y1, z1)
return [
[p000, p100, p110, p010],
[p001, p101, p111, p011],
[p000, p100, p101, p001],
[p100, p110, p111, p101],
[p110, p010, p011, p111],
[p010, p000, p001, p011],
]
def cylinder_faces(
x: float, y: float, radius: float, z0: float, z1: float, sides: int = 24
) -> list[list[tuple[float, float, float]]]:
angles = np.linspace(0.0, 2.0 * math.pi, sides, endpoint=False)
bottom = [(x + radius * math.cos(t), y + radius * math.sin(t), z0) for t in angles]
top = [(px, py, z1) for px, py, _ in bottom]
faces: list[list[tuple[float, float, float]]] = [bottom[::-1], top]
for index in range(sides):
next_index = (index + 1) % sides
faces.append(
[
bottom[index],
bottom[next_index],
top[next_index],
top[index],
]
)
return faces
def path_xy(apartment: Apartment, path: Sequence[GridCell]) -> np.ndarray:
return np.array([apartment.cell_to_xy(cell) for cell in path], dtype=float)
def sample_polyline(points: np.ndarray, frame_count: int) -> np.ndarray:
if len(points) <= 1:
return points.copy()
segment_lengths = np.linalg.norm(np.diff(points, axis=0), axis=1)
cumulative = np.concatenate(([0.0], np.cumsum(segment_lengths)))
if cumulative[-1] == 0.0:
return np.repeat(points[:1], frame_count, axis=0)
distances = np.linspace(0.0, cumulative[-1], frame_count)
sampled_x = np.interp(distances, cumulative, points[:, 0])
sampled_y = np.interp(distances, cumulative, points[:, 1])
return np.column_stack((sampled_x, sampled_y))
def draw_2d(
ax: plt.Axes, apartment: Apartment, result: PlanResult, points: np.ndarray
) -> tuple[Line2D, Circle]:
for room in apartment.rooms:
ax.add_patch(
Rectangle(
(room.xmin, room.ymin),
room.xmax - room.xmin,
room.ymax - room.ymin,
facecolor=room.color,
edgecolor="none",
zorder=0,
)
)
ax.text(
(room.xmin + room.xmax) / 2.0,
(room.ymin + room.ymax) / 2.0,
room.name,
color="#6a7075",
fontsize=11,
ha="center",
va="center",
zorder=1,
)
x_grid, y_grid = np.meshgrid(apartment.x_centers, apartment.y_centers)
ax.contour(
x_grid,
y_grid,
apartment.occupancy.astype(float),
levels=[0.5],
colors=["#b4413e"],
linewidths=0.65,
linestyles="--",
alpha=0.85,
zorder=2,
)
for item in apartment.objects:
ax.add_patch(
Rectangle(
(item.xmin, item.ymin),
item.xmax - item.xmin,
item.ymax - item.ymin,
facecolor=item.color,
edgecolor="#30343a",
linewidth=0.5,
alpha=0.96,
zorder=3,
)
)
if item.kind == "furniture" and (item.xmax - item.xmin) > 0.55:
ax.text(
(item.xmin + item.xmax) / 2.0,
(item.ymin + item.ymax) / 2.0,
item.name,
fontsize=6.7,
ha="center",
va="center",
color="white",
zorder=4,
)
segments = np.stack((points[:-1], points[1:]), axis=1)
ax.add_collection(
LineCollection(
segments,
colors="#147d92",
linewidths=0.8,
alpha=0.78,
zorder=5,
)
)
dock_x, dock_y = apartment.cell_to_xy(result.dock)
ax.scatter(
[dock_x],
[dock_y],
marker="s",
s=58,
color="#292d32",
edgecolor="white",
linewidth=0.8,
zorder=7,
)
robot_marker = Line2D(
[dock_x],
[dock_y],
marker="o",
markersize=7,
markerfacecolor="#e4583e",
markeredgecolor="white",
linewidth=1.5,
color="#e4583e",
zorder=8,
)
ax.add_line(robot_marker)
safety_circle = Circle(
(dock_x, dock_y),
apartment.robot_radius,
facecolor="#e4583e",
edgecolor="#973324",
alpha=0.18,
zorder=6,
)
ax.add_patch(safety_circle)
legend_items = [
Line2D([0], [0], color="#147d92", lw=1.8, label="规划清扫路径"),
Line2D(
[0], [0], color="#b4413e", lw=1.2, ls="--", label="膨胀后安全边界"
),
Line2D(
[0],
[0],
marker="s",
color="none",
markerfacecolor="#292d32",
label="充电座",
),
]
ax.legend(handles=legend_items, loc="upper right", fontsize=8, framealpha=0.92)
ax.set_xlim(0.0, apartment.width)
ax.set_ylim(0.0, apartment.depth)
ax.set_aspect("equal", adjustable="box")
ax.set_xlabel("x / m")
ax.set_ylabel("y / m")
ax.set_title("2D 占据栅格与覆盖路径", fontsize=12)
ax.grid(color="#c6cbd0", linewidth=0.35, alpha=0.45)
return robot_marker, safety_circle
def draw_3d(
ax: plt.Axes, apartment: Apartment, result: PlanResult, points: np.ndarray
) -> tuple[Poly3DCollection, Line2D, Line2D]:
for room in apartment.rooms:
floor = [
(room.xmin, room.ymin, 0.0),
(room.xmax, room.ymin, 0.0),
(room.xmax, room.ymax, 0.0),
(room.xmin, room.ymax, 0.0),
]
ax.add_collection3d(
Poly3DCollection(
[floor],
facecolors=room.color,
edgecolors="#c1c5c8",
linewidths=0.4,
alpha=0.94,
)
)
for item in apartment.objects:
alpha = 0.72 if item.kind == "wall" else 0.94
box = Poly3DCollection(
box_faces(item),
facecolors=item.color,
edgecolors="#33383c",
linewidths=0.35,
alpha=alpha,
)
ax.add_collection3d(box)
if item.kind == "furniture" and (item.xmax - item.xmin) > 0.8:
ax.text(
(item.xmin + item.xmax) / 2.0,
(item.ymin + item.ymax) / 2.0,
item.height + 0.06,
item.name,
fontsize=6.2,
ha="center",
va="bottom",
color="#33363a",
)
ax.plot(
points[:, 0],
points[:, 1],
np.full(len(points), 0.035),
color="#147d92",
linewidth=0.72,
alpha=0.52,
)
trail, = ax.plot([], [], [], color="#f06445", linewidth=2.1, alpha=0.95)
dock_x, dock_y = apartment.cell_to_xy(result.dock)
robot = Poly3DCollection(
cylinder_faces(
dock_x, dock_y, apartment.robot_radius, z0=0.045, z1=0.19
),
facecolors="#e4583e",
edgecolors="#8c3024",
linewidths=0.4,
alpha=1.0,
)
ax.add_collection3d(robot)
heading, = ax.plot(
[dock_x, dock_x + apartment.robot_radius],
[dock_y, dock_y],
[0.20, 0.20],
color="#f7e8d2",
linewidth=2.0,
)
ax.set_xlim(0.0, apartment.width)
ax.set_ylim(0.0, apartment.depth)
ax.set_zlim(0.0, 2.05)
ax.set_box_aspect((apartment.width, apartment.depth, 4.5))
ax.view_init(elev=37, azim=-57)
ax.set_xlabel("x / m", labelpad=5)
ax.set_ylabel("y / m", labelpad=5)
ax.set_zlabel("高度 / m", labelpad=4)
ax.set_title("3D 家居环境与机器人运动", fontsize=12)
ax.grid(False)
ax.xaxis.pane.fill = False
ax.yaxis.pane.fill = False
ax.zaxis.pane.fill = False
return robot, heading, trail
def render_simulation(
apartment: Apartment,
result: PlanResult,
output_file: Path,
animate: bool,
frame_count: int,
interval_ms: int,
gif_file: Path | None,
) -> tuple[plt.Figure, FuncAnimation | None]:
configure_chinese_font()
points = path_xy(apartment, result.path)
figure = plt.figure(figsize=(15.5, 7.2), constrained_layout=True)
grid = figure.add_gridspec(1, 2, width_ratios=(1.0, 1.28))
ax_2d = figure.add_subplot(grid[0, 0])
ax_3d = figure.add_subplot(grid[0, 1], projection="3d")
robot_2d, safety_circle = draw_2d(ax_2d, apartment, result, points)
robot_3d, heading, trail_3d = draw_3d(ax_3d, apartment, result, points)
status = figure.suptitle(
(
f"扫地机器人覆盖规划 | 路径 {result.path_length_m:.1f} m | "
f"覆盖率 {result.coverage_percent:.1f}%"
),
fontsize=14,
fontweight="bold",
color="#2d3338",
)
output_file.parent.mkdir(parents=True, exist_ok=True)
figure.savefig(output_file, dpi=170, facecolor="white")
animation: FuncAnimation | None = None
if animate:
sampled = sample_polyline(points, max(2, frame_count))
def update(frame: int) -> tuple[object, ...]:
x, y = sampled[frame]
if frame + 1 < len(sampled):
dx, dy = sampled[frame + 1] - sampled[frame]
else:
dx, dy = sampled[frame] - sampled[frame - 1]
angle = math.atan2(dy, dx) if abs(dx) + abs(dy) > 1e-12 else 0.0
robot_2d.set_data([x], [y])
safety_circle.center = (x, y)
robot_3d.set_verts(
cylinder_faces(
x, y, apartment.robot_radius, z0=0.045, z1=0.19
)
)
heading.set_data_3d(
[x, x + apartment.robot_radius * math.cos(angle)],
[y, y + apartment.robot_radius * math.sin(angle)],
[0.20, 0.20],
)
trail_3d.set_data_3d(
sampled[: frame + 1, 0],
sampled[: frame + 1, 1],
np.full(frame + 1, 0.045),
)
progress = 100.0 * frame / (len(sampled) - 1)
status.set_text(
f"正在清扫：{apartment.room_at(x, y)} | 任务进度 {progress:5.1f}% | "
f"规划覆盖率 {result.coverage_percent:.1f}%"
)
return robot_2d, safety_circle, robot_3d, heading, trail_3d, status
animation = FuncAnimation(
figure,
update,
frames=len(sampled),
interval=interval_ms,
repeat=True,
blit=False,
)
if gif_file is not None:
gif_file.parent.mkdir(parents=True, exist_ok=True)
fps = max(1, int(round(1000.0 / interval_ms)))
animation.save(gif_file, writer=PillowWriter(fps=fps), dpi=100)
return figure, animation
def export_path(
output_file: Path, apartment: Apartment, result: PlanResult
) -> None:
output_file.parent.mkdir(parents=True, exist_ok=True)
with output_file.open("w", newline="", encoding="utf-8-sig") as file_handle:
writer = csv.writer(file_handle)
writer.writerow(["step", "grid_row", "grid_col", "x_m", "y_m", "room"])
for step, cell in enumerate(result.path):
x, y = apartment.cell_to_xy(cell)
writer.writerow([step, cell[0], cell[1], f"{x:.3f}", f"{y:.3f}", apartment.room_at(x, y)])
def parse_arguments() -> argparse.Namespace:
parser = argparse.ArgumentParser(
description="带家具户型中的扫地机器人覆盖路径规划与 3D 动画仿真"
)
parser.add_argument(
"--output-dir",
type=Path,
default=Path(__file__).resolve().parent / "output",
help="图片、路径和动画输出目录",
)
parser.add_argument(
"--no-show", action="store_true", help="不打开 Matplotlib 交互窗口"
)
parser.add_argument(
"--no-animation", action="store_true", help="只绘制静态 2D/3D 仿真结果"
)
parser.add_argument(
"--save-gif",
action="store_true",
help="将 3D 运动过程保存为 robot_vacuum.gif（耗时较长）",
)
parser.add_argument(
"--frames", type=int, default=600, help="动画采样帧数，默认 600"
)
parser.add_argument(
"--interval", type=int, default=35, help="动画帧间隔（毫秒），默认 35"
)
return parser.parse_args()
def main() -> None:
args = parse_arguments()
if args.no_show:
plt.switch_backend("Agg")
apartment = Apartment()
planner = CoveragePlanner(apartment)
result = planner.plan()
args.output_dir.mkdir(parents=True, exist_ok=True)
image_file = args.output_dir / "vacuum_simulation_overview.png"
path_file = args.output_dir / "planned_path.csv"
gif_file = args.output_dir / "robot_vacuum.gif" if args.save_gif else None
export_path(path_file, apartment, result)
should_animate = not args.no_animation and (not args.no_show or args.save_gif)
figure, animation = render_simulation(
apartment=apartment,
result=result,
output_file=image_file,
animate=should_animate,
frame_count=max(2, args.frames),
interval_ms=max(1, args.interval),
gif_file=gif_file,
)
print("路径规划完成")
print(" 算法：分区往复式覆盖（Boustrophedon）+ 8 邻域 A*")
print(f" 可达清扫面积：{result.reachable_area_m2:.2f} m^2")
print(f" 总路径长度：{result.path_length_m:.2f} m")
print(f" 覆盖率：{result.coverage_percent:.2f}%")
print(f" 路径节点数：{len(result.path)}")
print(f" 窄区补扫目标数：{result.cleanup_targets}")
print(f" 规划耗时：{result.planning_time_s:.3f} s")
print(f" 仿真总览：{image_file.resolve()}")
print(f" 路径坐标：{path_file.resolve()}")
if gif_file is not None:
print(f" 动画文件：{gif_file.resolve()}")
if not args.no_show:
# Keep a local reference so Matplotlib does not collect the animation.
_animation_reference = animation
plt.show()
del _animation_reference
else:
plt.close(figure)
if __name__ == "__main__":
main()当前使用的算法
代码采用混合覆盖规划：
占据栅格地图：将房间、墙体和家具离散为 0.25 m 栅格，并按照"机器人半径 + 安全距离"膨胀障碍物。Apartment
可达区域搜索：从充电座进行洪泛搜索，只统计机器人真正能够到达的区域。reachable_from
Boustrophedon 往复式覆盖：在各房间生成交替方向的"弓字形"清扫路径。room_sweep_segments
8 邻域 A*：连接不同清扫条带、绕开家具，并规划返回充电座的路径。astar
遗漏区域补扫：检测尚未被机器人清扫半径覆盖的栅格，使用 A* 前往补扫。plan
因此整体算法是：
障碍膨胀占据栅格 + Boustrophedon 覆盖规划 + A* 路径连接
没有户型图怎么办
当前程序属于"已知地图下的全局规划"。没有户型图时，不能在开始清扫前保证整个真实房屋的覆盖率，需要改成：
SLAM 在线建图 + 前沿探索 + 在线覆盖规划
工作流程如下：
通过激光雷达、深度相机、里程计和碰撞传感器采集环境信息。
使用 SLAM 实时生成占据栅格地图并定位机器人。
使用 Frontier Exploration 搜索"已知自由区域和未知区域的边界"，不断探索新房间。
对已建图区域执行往复式覆盖或 STC 覆盖规划。
地图发生变化时使用 A* 或 D* Lite 重新规划。
单独维护清扫覆盖栅格，记录机器人清扫刷实际扫过的区域。
当不存在可达探索边界、地图连续多轮不再变化，并且所有已知可达栅格均被覆盖时结束。
覆盖率计算为：
覆盖率 = 已清扫的可达自由栅格数 / 已发现的可达自由栅格总数
需要注意：未知地图下无法对尚未发现的隐藏房间给出绝对覆盖保证。只有在传感器能够发现所有入口、定位误差受控、环境基本静态且所有区域物理可达的条件下，才能近似保证完整覆盖。