机器人怎么从客厅走到卧室？一个纯 Python 家庭导航仿真

想象一个蒙着眼的人，手里拿着激光测距仪，轮子上装着计数器，要去卧室拿东西。

他不知道自己在哪，不知道前面有没有人；他只知道轮子转了多少、激光打到墙有多远。

家庭机器人导航，就是把这件事拆成一条能跑起来的链路。

仿真画面：左边是家、路径、激光点和行人；右边是实时状态、定位误差、候选轨迹和各项指标。

这是一个纯 Python 的 2D 家庭导航仿真：14 m × 10 m 的家，机器人从客厅出发，穿过 0.9 m 的走廊，到达卧室。它不依赖 ROS2、Gazebo 或 YOLO 权重，只需要 Python 3.12 + NumPy + OpenCV。

项目地址：KissSheep666/home-nav-sim

一、先看它跑成什么样

三个场景，各跑 20 个随机 episode（固定 seed，可复现）：

这是仿真里的结果，不是真机。crossing 的 20 次里有一次为了让行耗时 86.8 秒，最紧的一次表面间距只有 0.06 m——没有碰撞，但说明策略偏保守。做机器人项目，主动说边界比只报好看数字更重要。

演示视频：窄走廊 · 行人横穿 · 走廊阻塞

二、导航到底难在哪

很多人对机器人的想象是「输入起点和终点，输出一条线」。真实问题至少有四层不确定性：

导航不是一条算法，而是一条链路。 定位错了，规划就会基于错误的位置；感知漏了人，避障就会撞上去；局部规划选错速度，控制再准也没用。

所以这个项目按照一条真实的导航链路来搭：

flowchart LR
    W[世界 / 激光] --> L[粒子滤波定位]
    O[里程计 + IMU] --> L
    L --> P[语义感知]
    P --> G[A* 全局规划]
    G --> D[DWA 局部避障]
    D --> C[PID + 差速控制]
    C --> W

每一层只回答一个问题，层与层之间用「位姿、路径、速度」这些明确的接口连起来。下面按机器人走一趟的顺序讲。

三、把家变成一张可搜索的地图

3.1 占据栅格：把连续世界切成 5 cm 的格子

机器人不会直接理解「沙发在左边」这种描述。它需要一张统一的世界表示：占据栅格（occupancy grid）。

• 整个家 14 m × 10 m；
• 每格 5 cm，所以是 280 × 200 个格子；
• 每格三种状态：能走、障碍、未知（还没看到）。

地图上还会额外算一张 clearance map：每个空格到最近障碍的距离。规划时，机器人中心不能离墙太近，所以「到墙距离 < 机器人半径 + 安全余量」的格子会被当成不可走。

这带来一个很现实的取舍：

• 膨胀太小 → 路径贴墙，转弯容易蹭；
• 膨胀太大 → 0.9 m 的走廊可能直接变成「无路可走」。

项目里机器人半径 0.22 m、安全余量 0.08 m，规划要求 clearance ≥ 0.30 m。0.9 m 走廊 = 18 格，机器人直径约 9 格，膨胀后中间还剩约 6 格可走——刚好够，但不宽裕。

窄走廊场景：HUD 显示 narrow yes，机器人自动限速 0.25 m/s，路径尽量走走廊中间。

3.2 为什么不能只在原始地图上跑 A*

因为 A* 把机器人当成一个点。如果不膨胀，它会算出一条贴着墙角的路径，几何上「能过」，物理上机器人会蹭墙。

膨胀的本质是：把机器人的体积提前算进地图里，让规划器可以把它当成一个点来搜索。 代价是窄通道会变窄，甚至消失。

四、我在哪：粒子滤波

4.1 里程计会漂，激光能纠正

轮子转了多远、转了多少度，可以估出机器人走了多少——这叫里程计。IMU 再补一个朝向角速度。问题是：

• 轮子半径有误差；
• 地毯、打滑、负载变化都会让里程计漂；
• 时间越长，误差越大。

所以需要一个「绝对参考」：激光扫一圈，和已知地图比对。

4.2 用 300 个「猜测」表示位置

粒子滤波的思路很直观：

1. 撒点：在初始位置附近撒 300 个粒子，每个粒子是一个可能的位姿 (x, y, θ)；
2. 预测：按里程计和 IMU 的增量移动每个粒子，加一点噪声；
3. 观测：对每个粒子问「如果我在这个位置，激光应该看到什么？」，和真实激光比，越像权重越高；
4. 重采样：淘汰权重低的，在权重高的附近重新撒点。

重复几轮，300 个猜测会聚到真实位置附近。

项目用 likelihood field 来算「像不像」：预先在地图上算好每个格子到最近障碍的距离，激光端点落在障碍附近就得高分。这样不用每次逐条射线做 ray casting，速度更快。

4.3 粒子滤波什么时候会失效

• 长廊/对称房间：多个位置看起来都像，粒子可能聚错；
• 机器人被抱走（kidnapped robot）：所有粒子都错，需要全局重定位；
• 粒子太少：收敛慢、抖动；太多：计算变慢。

这也是为什么真实机器人不能只靠一种传感器：激光负责几何，IMU 负责短时朝向，里程计负责连续性，谁都不是万能的。

一句话：粒子滤波不是「知道自己在哪」，而是「用一堆带权重的猜测逼近自己在哪」。

五、前面是谁：激光 + 语义感知

5.1 激光回答「哪里有东西」，相机回答「那是什么」

激光很擅长回答：

• 前面 0.8 m 有障碍；
• 左边墙多远、右边墙多远；
• 这条走廊有多宽。

但它不知道那是人、椅子还是门。相机 + 检测模型才能回答「那是大人、小孩还是宠物」。

项目里用 SimDetector 模拟 YOLO + ByteTrack 的输出：

• 按 FOV、距离、墙体遮挡生成检测；
• 给每个目标类别、位置、速度、confidence；
• 跟踪器用最近邻 + 恒定速度估计运动方向。

接口和真实感知保持一致，以后换成 YOLO11 + ByteTrack，后面的规划器不需要改。

5.2 为什么小孩的安全半径更大

这是语义感知最直观的价值：

小孩的物理尺寸更小，但行为更不可预测，所以 DWA 给他更大的安全距离和更高的动态代价。

行人横穿场景：红色的 adult 进入视野，HUD 里的 min gap 实时变化；DWA 会把「未来 1.5 秒可能撞上」的轨迹提前丢掉。

语义不是「识别出类别」就结束了，它要变成规划器能用的数字。

六、走哪条路：A* 全局规划

全局规划要回答的是「大概走哪条路」，而不是「这一秒怎么躲人」。

项目里的 A*：

• 8 邻接（上下左右 + 斜角），斜角代价 √2；
• 启发式用 octile 距离，让搜索有方向感；
• 可通行条件：clearance ≥ 0.30 m；
• 额外加一个「居中惩罚」，让路径穿过走廊/门洞中心，而不是贴着门框；
• 路径平滑：用 Bresenham 检查两点之间是否全程有足够 clearance，能直连就删掉中间点。

全局路径 1 Hz 重规划，或者在卡住、局部无解时重规划。blocked 场景里，走廊被静态障碍堵死，A* 会自动选择左侧备用路线。

走廊阻塞场景：中间通道被静态障碍堵住，A 重新规划，从客厅左侧绕行。*

A* 不知道行人下一秒在哪。它只负责「路线」，动态避障是下一层的事。

七、这一秒怎么走：DWA 局部避障

7.1 在速度空间里采样

DWA（Dynamic Window Approach）做的事：

1. 在当前速度附近采样很多组 (v, ω)：前进速度 + 转向角速度；
2. 每组按差速运动学向前模拟 1.5 秒；
3. 把会撞墙、撞人的轨迹扔掉；
4. 剩下的按多项代价打分，选最好的一组。

项目采样 6 档 v、11 档 ω，只考虑加速度能到达的范围——这就是「动态窗口」。

7.2 代价函数

这些项归一化后加权求和。权重调不好会出现两种典型问题：

• 太怕撞 → 原地不动；
• 太激进 → 贴着人走。

7.3 TTC 与窄通道

TTC（Time To Collision）用「距离 / 接近速度」估算碰撞时间：

• TTC < 1.5 s：减速；
• TTC < 0.8 s：停车。

窄通道里，机器人会根据激光左右距离估算自由宽度；小于 1.2 m 时降速到 0.25 m/s，提高 clearance 权重，尽量居中通过。

7.4 一个有意思的状态：让行

如果行人停在窄走廊里不动，机器人也停下，就会死锁——「人礼貌地让机器人，机器人也礼貌地让人」。

项目里加了一个 yield 状态：当机器人发现自己卡住、前方近距离有人时，安全倒车退出窄通道，给对方让路。倒车前会检查后方 clearance，避免盲倒撞墙。

这种「社交行为」不是导航算法本身，但它决定了机器人能不能在真实家庭里工作。

八、让轮子转：PID + 差速运动学

DWA 输出的是「前进速度 v + 转向角速度 ω」，但机器人真正能控制的是左右两个轮子。

差速底盘的换算：

v_l = v - ωL/2
v_r = v + ωL/2

再除以轮半径，得到左右轮的目标角速度。每个轮子一个 PID：

• P：误差越大，修正越强；
• I：消除长期偏差；
• D：抑制震荡。

项目参数是 Kp=8, Ki=2, Kd=0.1，带抗积分饱和和加速度限幅。另外还有一个 pure pursuit 基线，用来对比「没有 DWA 时动态场景是不是更容易撞」。

DWA 是决策层，PID 是执行层：DWA 决定「这一秒想怎么走」，PID 保证「轮子真的跟得上」。

九、三个场景，各自考什么

三个场景分别对应三类问题：能不能过、会不会撞人、路被堵了怎么办。

十、为什么它「不是」什么

这部分很重要，因为机器人项目最容易过度包装。

为什么不做完整 SLAM？ 因为完整 SLAM 要处理 scan matching、回环检测、位姿图优化，调试成本很高；而这个项目的重点是把「定位 → 感知 → 规划 → 控制 → 评估」这条导航链路完整跑通。

为什么不用 ROS2？ ROS2 + Gazebo + DDS + launch + tf 的环境问题很耗时间；算法链路才是重点。代码按节点/话题边界拆好，接口表和 ROS2 的对应关系都写在文档里，移植是工程工作，不是重写算法。

建图模式也做了，但定位是「已知位姿」的：110 个位姿、68.5% 地图被观测、已观测格子的建图准确率 100%；没走到的区域保持 unknown。它是 SLAM 的两个半边之一，不是完整 SLAM。

十一、怎么自己跑

依赖只有 Python 3.12 + NumPy + OpenCV：

cd D:\CodeSpace\home-nav-sim
pip install -r requirements.txt

# 交互 demo：q 退出 / space 暂停 / r 重置 / 1/2/3 切场景
python run_demo.py --scenario corridor --live
python run_demo.py --scenario crossing --seed 5 --live
python run_demo.py --scenario blocked --live

# 录像
python run_demo.py --scenario crossing --seed 5 --record results/demo_crossing.mp4

# 批量评估：3 场景 × 20 episodes
python run_eval.py --scenarios corridor,crossing,blocked --episodes 20

# 建图 demo
python run_map.py --out results/map_built.png

# 18 个单元测试
python -m unittest discover -s tests

不需要下载 ROS2、Gazebo、YOLO 权重或数据集。

十二、写完之后，我最大的感受

机器人导航真正难的不是某一个算法，而是把不确定性一层层收敛：

• 里程计不可信 → 用激光纠正；
• 激光不知道类别 → 用语义补充；
• 全局路径不知道动态障碍 → 用 DWA 实时避障；
• DWA 输出的是速度 → 用 PID 让轮子执行；
• 一次跑通不算数 → 用 20 次随机 episode 量化。

每一层都在替下一层「擦屁股」，每一层也都有自己的失败模式。一个好的导航系统，不是某个算法特别强，而是每一层都知道自己不该负责什么。

如果你也想做一个，建议从 2D 仿真开始：不需要真机、不需要 ROS2，先把这条链路跑通，再谈移植。

参考

• 项目代码：KissSheep666/home-nav-sim
• 概念手册：docs/concepts.md
• 工程链路与 ROS2 映射：docs/pipeline.md
• 评估结果：results/summary.md
• 演示视频：窄走廊 · 行人横穿 · 走廊阻塞