拖地机器人开发

项目概述

TD25A 是一套基于 ROS 2 Humble 的拖地机器人系统，覆盖底盘执行、多源感知、建图定位、自主导航、覆盖清扫、自动回充、远程交互与安全诊断。

文章上方的交互图谱来自 2026-07-23 对 Jetson 的分模式启动、运行图采集、源码交叉检查和清理后复测。图谱把每项功能的实现方法、话题链、TF 框架、实际工作链和源码/记录位置放在同一处；鼠标悬浮或键盘聚焦话题，可以查看它在系统中的职责。

TF 坐标体系

系统的空间主链是 map → odom → base_footprint → base_link：

• map → odom 由 GPU 全局重定位维护，用于修正长期位置漂移。
• odom → base_footprint 由 FAST-LIO 维护，保证局部运动连续。
• base_footprint → base_link 以及雷达、相机、IMU、车轮和机构安装关系属于静态 TF。
• 地图目标、覆盖路线保存在 map；局部代价地图工作在 odom；碰撞几何和安全距离以 base_footprint 为准。

首次快照发现 motion_link 同时存在静态 body → motion_link 与动态 map → motion_link。清理后重新采集 23 条 TF 边，已经没有多父 child；motion_link 只保留静态 body → motion_link，定位节点的 /motionlink2map 仅作为诊断数值，不再广播第二条 TF 父链。

2026-07-29 设计记录： 后续定位 T2 已确定采用轮速、IMU、激光持续融合， 并只对导航发布严格二维的 odom → base_footprint。这仍是待实现、待实车验收的 方案，不代表下方 2026-07-23 运行图已经具备全部行为。详见 《TD25A 持续融合定位：待解决问题与当前替补方案》。

功能分类

一、底盘与清洁执行

采用差速运动学、统一的线角独立梯形斜坡和 STM32 V5.4 反馈。所有导航、遥控和维护速度最终汇入同一底盘命令入口；超时、急停和故障停车仍可立即生效。

二、环境感知

对 Livox 和 D435i 数据分别做地面、反光和无效点过滤，再产生障碍标记云与清除云。两路数据按运行开关融合后进入 STVL 和 inflation，形成控制器使用的局部/全局代价地图。

三、建图与定位

FAST-LIO 负责短时连续的激光惯性里程计，GPU ICP 负责对三维地图做全局重定位。两者不争抢同一 TF：前者维护 odom → base_footprint，后者维护 map → odom。

四、自主导航与运动控制

导航由行为树组织，Theta* 生成全局参考路径，CUDA MPPI 在局部代价地图中滚动采样、判断 footprint 碰撞并输出速度。MPPI 最优预测轨迹经转换节点保存在独立 Path 话题中，供 RViz 粉色轨迹显示和日志复盘。

2026-08-09 9F 地图入口与门洞修正（UI 稳定 / 导航待实车）： 原导航启动遮罩在地图/YAML握手已成功后仍等待定位恢复或 长运行 ros2 launch 进程退出，因而卡在 96%，操作员连地图和 “重定位”工具都无法进入。MainWindow._on_verified_map() 现在以地图 握手作为进入工作台的条件；即使定位为 LOST/STALE，也会先 显示地图和重定位入口，但导航目标仍由定位健康门失败关闭。

同次无运动复核中，依用户 500% 示例图对齐原始 maps/9.png， 只把门洞范围 46 个占据像素改为自由（其中包含最后指出的 两个残留黑点），没有改动 9.yaml 或 9.pcd。最终 PNG SHA256 为 f1b6b57582795c8a6b548dbea4940216bdfb8f5a214b4f319d14a62c8b1f89d3； 开口为 18 像素 × 0.08 m，约 1.44 m，位于地图坐标 x≈2.3 m, y≈0.1 m，连通左侧房间和中部竖向通道。

操作顺序是：选择 9.yaml 并点击“应用并启动导航” → 地图 工作台出现 → 定位非健康时先用左侧“重定位” → 只有健康门 恢复后才能规划/发送目标。本次只验证了工作台入口、地图重载和 静态开口；没有发送 /initialpose、导航或清扫目标，左右轮目标/ 实测均为 0 RPM，XY running_mask=0，2 秒 /cmd_vel 窗口无消息。 因此“新门洞已载入静态地图”已通过，“重定位后规划器和实车能穿过” 仍待后续有人持急停的低速验收；现场传感器若观测到真实障碍， 动态代价地图仍会重新闭锁该通道。

五、覆盖清扫与任务管理

覆盖层负责圈区、边界、弓字/BCD 路线、活动窗口、局部绕行、堵塞登记和遗漏补扫。完整路线、当前窗口、已完成、跳过、待恢复和进度均使用独立输出保存，避免长任务只剩一次性控制命令而无法复盘。

2026-08-09 生产 UI 填充规划复测（降级 / 待实车）： 从主启动器进入导航、 加载 9.yaml、应用定位并打开“任务 → 全图覆盖”，已经完整走通到路径规划结束。 当前普通填充主链为“UI 任务编排 → OpenNav /compute_coverage_path → Fields2Cover 2.0 → TD25A 转场与最终车体校验 → /ui/coverage_path 预览”；只有当前位姿和整条路径均通过安全门、操作者再次确认后， 才允许交给 Nav2 FollowPath。本次 9F 生成 1 个区域、48 条等距作业线、 2810 个路径点和 45 个相位边界，规划耗时 197517 ms；footprint 扫掠为 0 碰撞且路径连续，转弯安全覆盖率 93.6%，机构可达覆盖率 81.6%，实际刷面 覆盖率 69.5%。

实测定位状态为 LOST，机器人当前位姿 (5.73, 2.29) 的完整 footprint 与 静态地图冲突，正式请求按 start_pose_not_footprint_safe 闭锁。UI 仅以距当前点 0.73 m 的安全锚点 (5.26, 2.86) 重算只读预览，不写入待执行路径并禁用“执行” 按钮；全程未发送导航目标、初始位姿或非零速度，左右轮目标/实测 RPM 均为 0。 另有 10 段 / 153.14 m 沿边候选，但 8 个墙面引用尚未连通，执行链保持 WAIT(edge_unconnected_wall_references)。导航组合目前也不会自动启动 lifecycle active 的 coverage_server，本次只为计算临时手动启动并在验证后清理。因此这次 结果证明“UI 到填充路径规划”可用，不代表覆盖执行或沿边清扫已经通过实车验收。

2026-08-04 覆盖规划进度： 当前新增的是离线审图层：OccupancyGrid、 车体中心可通行域、全车体转弯安全域、平整化 clean_field、常规矩形清扫域、 矩形取尽、每区名义路径、区域顺序与分簇预览。分簇已经取消“最多 4 区”的 固定上限。物理沿边区域、固定朝向直进倒出死区和拒绝域已经完成分类；按簇的 绿色沿边、蓝色转场、青色直进、橙色同路倒出以及硬停车点也已通过离线精确车体 校验。分段状态机、控制器 ID、清扫机构入口状态与硬停前置检查已经接到主 UI 和 ROS action 回调；新分段任务有意不压平成长路径交给旧滚动 PathManager， 以免跨过硬停、X 轴伸缩和正倒车语义。逐段恢复/重规划、覆盖记账和实车验收仍 未完成，不能把 RViz2 预览或代码接入写成完整清扫已完成。图层、颜色、话题及 9F/4F 验证结果见 《TD25A 覆盖规划：从地图图层到矩形分区》。

2026-08-04 沿边方向约束： 清洁机构只能向车体右侧伸出，沿边路径因此 固定为“可清扫空间在左、墙面或障碍在右”。房间外轮廓表现为逆时针，柱子等 障碍岛则按保持障碍在右侧的方向绕行；调度器不再为了缩短转场提供沿边反向候选。 二次复核发现原绿色线位于橙色转弯安全边界，很多位置超过已标定 250 mm 行程，不能算贴墙清扫。当前只保留真实墙面在右侧、机构够得到且完整蓝色机构 矩形校验通过的中心线。进一步按真实硬件动作改为“停车伸出—固定 X 沿边—停车 收回”后，9F 生成 44 段任务并保留 2 段固定 250 mm 沿边；4F 生成 205 段， 保留 8 段沿边和 4 段死区倒车，但仍被 1 个簇间衔接和倒车授权阻断。相关测试为 40 passed，Jetson 构建通过；本轮没有发送任何底盘或机构运动命令。

六、自动回充

后置相机直接发布压缩 JPEG 和 CameraInfo，官方 apriltag_ros 识别 tag36h11 / id=0 / 150 mm，再由 PnP、地图绑定 anchor、Nav2 预对接和 IBVS 视觉 PID 完成回充。实际检测输出为 /td25a/rear_apriltag/detections；标定、开始和取消分别使用 /td25a/calibrate_dock、/td25a/start_dock 和 /td25a/abort_dock。

对接节点和 PyQt UI 都直接订阅官方 apriltag_msgs/AprilTagDetectionArray；标记生成页对应 dock_apriltag.py、dock_apriltag_panel.py 和 DockAprilTagPanel。当前没有可切换的第二套回充视觉实现。

后方超声和充电触点闭环目前仍明确关闭，因此“视觉链能启动”不能替代真实 接近、对桩、触点和失败退出验收。

七、人机交互与远程控制

PyQt、RViz、键盘/手柄、MQTT 与微信端共享任务和状态接口。地图点击统一转换到 map，遥控使用控制权和超时机制，避免与自主导航同时争用底盘。MQTT 状态桥已改为直接读取 /chassis/ultrasonic，实测返回 con1 至 con10 共十路有效数据。

2026-08-04：硬件标定 3D 只读模型

状态为稳定（只读显示）。硬件测试/标定窗口右侧原“目标 / 实际轨迹”画布 已经替换为和 RViz RobotModel 读取同一份 td25a.urdf、同一组 STL 的可旋转 机器人模型。左上角“显示静态 TF”控制 46 组红绿蓝坐标轴；取消勾选只隐藏 坐标轴，不隐藏机器人。鼠标左键旋转、右键或中键平移、滚轮缩放，俯视、斜视、 重置和重载模型都有独立按钮。这里没有新增或改写 TF 广播，只是读取既有 URDF 固定关系进行显示，因此不改变 map → odom → base_footprint → base_link 的 所有权。

模型共有 7 个有外观的部件。左右驱动轮由 /chassis/status 的编码器反馈更新； X 机构把 /chassis/xy_status 的行程换算为 0～-250 mm 横移，Y 机构在完成 回零后换算为 -38～+62 mm 升降。两个万向轮虽然在 URDF 中是连续关节，但 当前没有转角传感器，显示值保持 0，不能当作真实转角。底盘或 XY 状态超过 新鲜度窗口后，模型保留最后可知姿态并显示“无实时数据”，不会伪造零位。

实现位于 stm32_chassis_ui/robot_model_view.py 和 dashboard.py；新视图没有 ROS publisher、service client 或运动命令入口，原轨迹数据接收对象仅在后台保留， 不会因为换图而改变轮式标定逻辑。Jetson source 与 symlink-install 构建树哈希 一致，实际加载 7 个模型部件和 46 个静态 TF；显示/隐藏与鼠标旋转均在 正式窗口截图复核，相关回归为 37 passed。验证期间未启动底盘驱动，也没有 驱动车轮或 XY 机构，所以这里只能确认显示链和反馈换算，不能替代实车运动闭环。

本次还发现传感器标定页存在旧面板与新 Dashboard 构造参数不一致。为恢复窗口 启动，Dashboard 只在面板确实支持时才传入停车、心跳和运行状态回调；当前旧 面板仍保持原“静止采集”行为，没有擅自补写小范围动态标定。全包测试仍有一个 与本功能无关的旧文案合同失败，因此动态传感器标定继续按未完成处理。

八、安全、诊断与数据闭环

系统检查急停、通信新鲜度、轮速、堵转、定位/TF 和 Nav2 活性。测试脚本同步记录控制器命令、底盘实际命令、里程计、轮速、路径、代价地图和导航日志，形成“现象—参数—数据—结论”的闭环。

运行图审计结论

2026-07-23 的历史快照不是只看一张静态图，而是分别启动默认导航组合、深度 避障组合、手动建图、UI、回充和 MQTT 后重新审计。该日默认组合 （导航 + UI + 回充 + MQTT，深度默认关闭）为 39 个节点、140 个话题；显式 开启逐探头超声诊断后为 40 个节点、151 个话题。全程没有发送速度、导航、 清扫或回充目标，/chassis/cmd_vel 实测保持为零。

确认并收口的旧结构包括：

• 删除 UI 对 /scan、/points 的空兼容订阅；导航和手动建图均直接使用 /cloud_registered。
• NavigateCompleteCoverage 一体化导航 action 与 MQTT 旧 action 状态订阅仍保持删除；2026-08-09 已恢复生产 ComputeCoveragePath 客户端，普通填充由 OpenNav/Fields2Cover 2.0 只负责计算，执行仍统一使用 Nav2 FollowPath，并受当前位姿、完整 footprint 与人工确认安全门约束。
• 回充主链固定为“压缩 JPEG + CameraInfo → /td25a/rear_apriltag/detections → PnP → IBVS”，UI 与对接状态机直接 消费同一官方检测数组。
• 旧 1–6 数字编号超声链已连同剩余测试引用彻底删除；MQTT 改为直接读取当前 STM32 的聚合超声消息。
• /ultrasonic/con1 至 /ultrasonic/con10 作为后续诊断接口保留，名称固定，不再做可编辑话题配置。/ultrasonic_range_bridge 默认关闭，只有显式按需启动时才创建 10 路 sensor_msgs/Range；实测十路均由该桥发布。

仍然保留的零连接接口都有明确理由：/projected_map 属于手动建图；/explore/* 属于自动探索；深度障碍话题由深度开关控制；costmap footprint、/goal_update、/speed_limit 和 /joint_states 是 Nav2/ROS 标准可选输入；覆盖、定位、MPPI 与图像传输中的零订阅输出则供 RViz、晚加入 UI 或离线诊断按需消费。

判断“无用”采用三重证据：对应功能已启动、运行图仍断链、源码没有有效工作路径。只满足“当前没人订阅”不会删除。

2026-07-26：模式、摄像头与人工流程复核

先说结论

当前可以确认导航后台、人工建图后台、D435i、左右建图相机、后置回充相机和 AprilTag3 识别后台都能启动；导航运行中动态加入回充相机不会让 D435i 掉流。 但还不能宣称“所有功能正常”：地图 9 当前没有回充标定；2026-08-09 导航 地图入口已修复，9F 门洞也已重载，但定位仍为 LOST/STALE，未做重定位或 穿门规划。普通填充已经通过生产 UI 生成只读预览，但定位 footprint 与 静态地图冲突，沿边引用和 coverage_server 自动启动仍未收口。自动探索、微信非零遥控、 覆盖执行和真实 回充也没有在无运动审计中执行。

操作者入口和内部状态

主启动器给人使用的正式入口是 5 个，运行时状态机则有 8 个状态：

覆盖清扫和回充属于导航工作台内的子流程；“清理残留节点”属于维护动作，不是 第六个业务模式。

这 8 个值是任务互斥、UI/MQTT 状态同步、取消清理和故障安全使用的内部状态， 不是 8 个可点击按钮。CLEANING 与 DOCKING 是 NAVIGATION 底座上的 子流程；FAULT 由系统进入，IDLE 是默认落点。

无运动实测

后置相机的正式图像接口是 /rear_camera/image_raw/compressed，不是未压缩 /rear_camera/image_raw。D435i 位于独立 USB3 链，可以和后置相机并发； 后置相机与左侧建图相机共享 USB2 hub，开启回充时会停止侧相机对齐/采集栈， 所以正式组合是“导航 D435i + 后置回充”，不是建图三摄加后置四摄全开。

从人工操作角度怎么跑

导航并开启回充相机的流程是：

1. 从“导航”进入工作台，在地图页选择地图并点击“应用并启动导航”。
2. 等 Nav2 lifecycle 全部 active，并确认 D435i 深度、融合障碍和底盘状态 正常。
3. 打开“回充”页，点“打开摄像头并开启回充”。UI 会把当前地图名通过 DOCK_MAP_NAME 传给 start_all.sh --add-docking，只新增 docking 后台，不重启导航。
4. 观察后置压缩画面和 /td25a/dock_scan_status。只有完整看到 tag36h11 ID=0、画面稳定且状态为 OK 时才能点击“标定回充位”。
5. 重新标定后，另选有人持急停的低速维护窗口验证“开始回充”、取消、Tag 丢失、视觉伺服、最终接触和失败退出。

相机内参和“回充位标定”是两件事。四路相机内参文件当前都存在；硬件测试页 可用棋盘格逐台重新计算内参。回充位标定还要求 3 秒内采满 10 个稳定样本、 角点标准差不大于 1.5 px、重投影误差不大于 2 px、Tag 距离/居中/透视通过， 并且 map → base_footprint 可用。

地图 9 当前没有回充标定。下一次在地图 9 成功标定时才会创建 ~/.config/td25a/dock_calibration__9.yaml；未检测到 Tag 时服务返回 success=False，不会生成文件，也不能开始回充。

2026-07-26 已完成 AprilTag3 单链代码收口：对接与 UI 定向测试 30 passed，td25a_recharge 和 td25a_robot_ui 的 --symlink-install 构建成功。构建后无运动烟测中，后置压缩流约 15. 15 Hz、官方检测数组约 22.72 Hz，四个回充服务存在，状态保持 idle， 没有生成标定文件；测试结束后临时回充栈已停止。该结果证明源码、依赖、安装 空间和等待操作链一致，不代表机器人已完成实际接近、触点或完整回充。

当前阻断项

模式、覆盖、回充和相机聚焦回归共 143 项，结果为 140 passed / 3 failed：

1. 正式覆盖入口开始新任务时没有重置 _want_perimeter=False；
2. MPPI 稳定配置还没有 VelocityDeadbandCritic，低于 STM32 执行死区的 命令惩罚合同失败；
3. Nav2 覆盖终点容差仍为 0.12 m，而 PathManager 为 0.22 m，source 和 install 都不一致。

第三项与 7 月 25 日实车记录的“两处已同步 0.22 m”冲突，重启后会重新加载 0. 12 m。修复并重建前不能继续把完整覆盖清扫写成稳定功能；CON9、十路超声、 X/Y 和完整区域闭环仍按原安全边界保留为待验。