Project 01 · Phase 2

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

阶段 2 · 行人超前预判绕行

阶段 1 只能对行人急停。阶段 2 先检测跟踪行人、再预测、再按规则绕行或停车。

职责：感知与决策链路（设计与实现）

平台：自组四驱四转 + Thor；算法在 Ranger / Nav2 栈上开发并迁移

行人检测有效距离

便利店走廊现场手测

100%

绕障成功率

同场手测

3 × 2

风险 × 区域

Behavior Manager 决策

proactive

避障不再只靠急停

Collision Monitor 仍作兜底

行人超前预判绕行

行人走进路径之前就绕开，绕不开再停。左侧实拍，右侧 RViz2（四目融合 + 预测场）。

实拍

RViz2：四目融合与预测场

项目背景

场景与任务

便利店走廊里会遇到走动的行人。阶段 1 只靠 Collision Monitor：贴上再停，属于 reactive。阶段 2 先外推行人轨迹，在人走进当前路径之前改局部代价、提前绕开；绕不开或风险高再停车。

阶段定位

本人设计并实现感知、预测和规则决策。定位、对接和 HTTP 调度沿用阶段 1，学习式决策留给阶段 3。

为什么难

走廊两侧是货架，行人常从侧向走进路径；单眼前视看到时，已经来不及绕。
行人在动。只标当前占用，车会开到跟前再停；要超前绕行，必须先外推轨迹。
不是所有人都该停：还能绕就绕，绕不开或高风险才停。判错会抖、会堵、会撞。

系统架构

四目鱼眼先 2×2 融合，DeepLabV3 给出行人掩码并重投影到激光；DBSCAN、圆拟合和 6 维 Kalman 把人从瞬时障碍变成可跟踪目标。风险场写入 costmap，再按开阔 / 狭窄分区出正常导航、减速绕行或停车等待。局部跟踪用 CUDA MPPI，减速绕行时把风险场并进全局 costmap 重规划。

阶段 2 感知到决策链路：四目融合、DeepLabV3、恒速预测、双层风险场、Behavior Manager、行为树改 Nav2 — 定位与底盘沿用阶段 1。本阶段新增的是感知到决策这一段。

决策是 3×2：无风险正常行驶；低风险在高速区减速绕行、在低速区停车；高风险立即停车。风险或区域未知一律 FAULT。

风险乘区域的 3×2 决策矩阵，未知输入进入 FAULT — 风险 × 区域。未知输入进 FAULT，速度限制 0。

NORMAL 关掉预测层、速度上限约 0.50 m/s；SLOW_AVOID 打开预测层并建议重规划、速度上限约 0.20 m/s；STOP_WAIT / FAULT 发零速。升级快、降级慢。

四种行为状态如何改预测层和速度上限，以及不对称确认时间 — 高风险立刻停；低风险约 0.2 s 确认；降级约 1.0 s 才放行。

核心系统设计

感知：四目融合 + DeepLabV3，掩码落到激光

问题：货架走廊两侧挡视线，单眼前视看到行人时已经来不及绕；仅用激光点云，行人与货架立柱难以区分。

改动：四路鱼眼对时后拼成一张 2×2 融合图；DeepLabV3（VOC person=15）做语义分割，把行人掩码重投影到激光点云，只留下人身上的点。

效果：两侧行人进入同一条感知链，激光里的人不再和静态货架混在一起。

感知：DBSCAN、圆拟合与 6 维 Kalman 跟踪

问题：分割只给当前帧掩码。人一走，下一帧又是新的一团点，风险场没有速度，也接不住漏检。

改动：对重投影后的点做 DBSCAN 聚类，Kasa 圆拟合给出位置和半径（不稳时退回质心包络）；6 维 Kalman 状态 [x, vx, y, vy, r, ṙ] 做最近邻关联，漏检靠预测顶住。

效果：行人从瞬时障碍变成可持续跟踪的动态目标，位置和速度能送给风险场。

决策：速度 + 协方差写成高斯风险场

问题：只标当前占用格，车会开到跟前再停；速度不确定时，硬切障碍边界会抖。

改动：用跟踪速度外推轨迹，把 Kalman 位置协方差铺成时间衰减高斯核，写入全局 / 局部 costmap（约 3.0 s / 1.2 s）。风险场图层保持加载，只切换是否写入代价、写多强，避免关掉后旧代价清不掉。

效果：人还没走进当前路径，地图上已经有一块会随人移动的软代价。

决策：开阔 / 狭窄分区 + 行为树切策略

问题：同样的行人，开阔处还能绕，狭窄处必须停；只改速度上限、不改规划，仍会贴上去。

改动：风险 × 区域出正常导航 / 减速绕行 / 停车等待。行为树改限速和风险层，等两层 ACK 后再走；卡住时用 WanderingRecovery 脱困（安全则等，堵住则往低代价邻格撤）。

效果：还能绕就绕，绕不开或高风险才停；现场卡死有恢复，不只靠 Collision Monitor。

规划：DWB 换成 CUDA MPPI

问题：阶段 1 的 DWB 按一条参考轨迹打分，吃不下大批带预测代价的候选轨迹，行人一动就只能逼停。

改动：局部控制器换成自研 RangerMPPI：候选轨迹的采样、前向积分和 critic 评分放到 GPU 上按轨迹并行算（约 4096 条）。

效果：同一控制周期里能比较绕、跟、停多组动作，局部跟踪跟得上移动的风险场。

规划：减速绕行时全局重规划

问题：风险场只写在局部图上，全局路径仍朝行人走，MPPI 只能在原路上扭，最后还是急停。

改动：减速绕行时把风险场并进全局 costmap 并触发重规划；高风险则停车等待。Collision Monitor 只作最后兜底。

效果：从贴上再停，升成行人走进路径之前就改路绕开。

个人职责

主责

感知与决策主链路：四目融合与分割、掩码重投影、Kalman 跟踪、风险场、规则决策、行为树策略切换，以及 CUDA MPPI 局部规划。工作重点是现场把「检测跟踪 → 预判绕行」调稳。

联调

定位、对接和 HTTP 沿用阶段 1；和底盘、上身在自组四驱四转 + Thor 上联调。

阶段 3：强化学习决策已经上线。