Project 01 · Phase 1

便利店补货机器人自主导航系统

阶段 1 · 展会级厘米导航

从 0 到 1 搭建轮臂机器人的定位与导航主链路，做成可被上身 VLA / 展会平台调度的完整系统，并完成厘米级对接。

职责：SLAM 与导航负责人

平台：自变量老乌龟（全向 Ranger 底盘 + 方舟无限机械臂）+ Livox MID-360 + 后向 RealSense D435i + Intel NUC

约 10cm

贴边避障

走廊行驶，Collision Monitor 兜底

< 1.5cm

定位 + 导航精度

到货架对接，含盲走

连续运行

展会稳定版本现场手测

2026.09

登陆日本机器人展会

轮臂补货完整任务

整体导航 + VLA 补货

完整任务：导航到货架前停稳，上身 VLA 完成补货。

完整导航 + 补货任务

贴边距离约 10cm

走廊行驶贴边约 10cm。左侧实拍，右侧 RViz2。

实拍

RViz2

定位 + 导航精度 < 1.5cm

到货架对接后，激光点与目标标记偏差约 1.5cm。

项目背景

场景与任务

面向便利店自动补货：轮臂机器人自主行驶到货架前停稳，由上身机械臂完成补货。导航需要同时被展会平台调度和上身 VLA 调用。

阶段定位

阶段 1 的目标是把主链路从 0 搭到可交付：定位、导航、精对准、底盘通信、HTTP API 联调。动态行人此时只能靠 Collision Monitor 逼停，属于 reactive；proactive / learning 留给阶段 2 / 3。

为什么难

补货停靠是厘米级，不能只靠开环停在货架附近。
展会现场是退化场景，定位本身就容易漂；上身作业时雷达还会被挡，LIO 更容易漂。
系统要对外可调用，不能只在本机 RViz 里跑通。

系统架构

感知进入 FAST-LIO2 后分流：一条做 3D 建图，一条做重定位与导航。导航侧由自定义行为树串起 Nav2 粗导航和盲走精对准，平台与上身只通过 HTTP API 下发任务。

系统框图：共享 LIO 与 UDP，建图与导航互斥启动；导航侧定位、感知、执行并行，HTTP 只调度执行 — 建图与导航互斥启动，共用 LIO 和 UDP。导航侧定位、感知、执行三列并行。

六层架构。自下而上是感知底盘、建图定位、导航执行和对外接口。

平台下发目标后，行为树先走 Nav2 粗导航到货架附近，再切到盲走精对准：对齐 yaw → 平移 → 微调 yaw。

两阶段导航时序：平台经 HTTP 与行为树调用 Nav2 再切盲走 — 两阶段导航。到点附近后，行为树从 Nav2 粗导航切换到盲走精对准。

停稳后交还上身 VLA，按到位 → 感知 → 生成任务 → 操作 → 复核走完补货；复核不过就回到感知。

导航加 VLA 补货全流程：到位、感知、生成任务、操作、复核 — 到位后交还上身：感知 → 生成任务 → VLA 操作 → 复核。

核心系统设计

FAST-LIO2：回调拆成三路并行

问题：原生单线程回调里，点云转换和滤波会堵住 IMU / LiDAR 订阅；处理一帧超时后，DDS KEEP_LAST 会丢掉还没取走的 IMU，预积分缺样。

改动：lio_node 改成 3 线程 MultiThreadedExecutor，三个互斥 callback group：传感器组只把 IMU / LiDAR 校验后入缓存；处理组做转换、同步、IMU 传播、LiDAR / ZUPT 更新和发布；轮速组独立入队。滤波状态只允许处理组修改。

效果：LIO 算得再慢也不堵订阅，IMU 不再因为处理超时被覆盖。

FAST-LIO2：机械臂作业时里程计漂

问题：补货时雷达被挡，纯激光–IMU 会漂。

改动：轮速 ZUPT 按 IMU predict → LiDAR update → ZUPT 贯序更新；静止时位姿冻结；高反点旁路给反光柱。

效果：作业停靠时位姿不跟着漂，给滑窗和精对准一个稳的相对运动。

初定位：分级回退

问题：展会重启或丢掉全局位姿后，不能每次靠人在 RViz 里点初值；单一方法在货架退化场景里经常对不上。

改动：按优先级分级尝试：反光柱绝对观测 → ScanContext 粗召回 + ICP → last.bin（last_pose.bin）用上次位姿回退。上一级成功就停，失败才落到下一级。

效果：现场冷启动能自动拉起定位，不依赖人工给初值。

多源定位：单一 ICP 不够稳

问题：货架几何重复，ICP 易假收敛；绝对观测如果谁来谁改 TF，窗口会被污染。

改动：运行期把 LIO 相对边、两阶段点-点 ICP（粗 0.25 m / 细 0.1 m）和反光柱 x/y 全部送进 Pose2 滑窗，只优化 x/y/yaw，并用 NIS / Huber / map→odom 跳变拒绝与 Schur 边缘化守门。

效果：绝对观测统一进一张图，不再直接覆盖 TF。

Nav2：过狭窄走廊

问题：走廊两侧都是货架，全局路径贴边、局部跟踪刮边，碰撞风险高。

改动：按 URDF 做准确 footprint 投影；全局 costmap 双膨胀（引导层约 3.5 m 把路径压向通道中间，安全层约 0.8 m 保碰撞边界）；按实车运动学重调 DWB。

效果：全局路径尽量走通道中线，局部跟踪更贴车体真实轮廓。

Nav2：厘米级对接

问题：只靠 Nav2 开环停在货架附近，停靠误差到不了补货所需的厘米级。

改动：自定义行为树把任务拆成 Nav2 粗导航 + 盲走精对准；盲走按对齐 yaw → 平移 → 微调 yaw 执行。

效果：到点后由盲走收口到厘米级停靠。

Nav2：后向补盲

问题：车体遮挡，前向激光扫不到正后方，倒车和原地转向时 costmap 是盲的。

改动：加后向 RGB-D；局部 costmap 体素层同时订阅前向 3D 激光和后向补盲点云。

效果：后方障碍进入同一张局部地图，倒车和原地转向有覆盖。

个人职责

主责

SLAM 与导航模块主负责人：从 0 到 1 搭建主链路并持续维护，把同事模块并进主系统（反光柱 landmark、底盘 UDP、后向补盲等）。工作重点是现场调试 SLAM 定位精度与导航精度，把对接收口到厘米级。

联调

配合上身 VLA 和展会平台完成接口与任务联调，支持 9 月底展会 Demo。

阶段 2：行人超前预判绕行已经上线。阶段 3：强化学习决策已经上线。