系统算法数学表达与逻辑总览

日期：2026-03-24

本文档整理当前项目中真正落地使用过的主要算法，并给出它们的数学表达、输入输出和决策逻辑。写法尽量服务于论文与中期汇报，因此重点强调：

1. 系统如何把导航问题降维到可在线预测的拓扑空间；
2. 世界模型如何进入规划与恢复闭环；
3. 各算法之间的关系是什么；
4. 哪些模块是当前主线，哪些属于扩展分支。

---

1. 总体问题形式化

系统把室内导航环境表示为一个带语义的有向图：

$$G=(V,E)$$

其中：

• $V$：节点集合，对应房间、走廊节点、楼梯、电梯、门厅等语义位置；
• $E$：有向边集合，对应可通行通道；
• 每条边 $e=(u,v)$ 具有静态属性与风险属性。

对于每条边 $e$，系统使用如下静态属性：

$$\phi(e)=\{\text{edge\_type},\ \text{distance},\ \text{base\_travel\_time},\ p_b^{cfg}(e,\tau),\ p_f^{cfg}(e,\tau)\}$$

其中：

• $\tau \in \{\text{morning},\text{afternoon},\text{evening}\}$ 是时间桶；
• $p_b^{cfg}$ 是配置层给定的阻塞概率；
• $p_f^{cfg}$ 是配置层给定的失败概率。

一条导航任务可以写成：

$$\mathcal{T}=(s,g,\tau,q)$$

其中：

• $s$：起点；
• $g$：目标节点；
• $\tau$：时间桶；
• $q$：自然语言查询。

系统最终目标是找到一条路径

$$\pi=(v_0=s,v_1,\dots,v_n=g)$$

使得导航成功率尽量高，同时总时间、重规划次数和恢复代价尽量低。

---

2. 关键创新：世界模型的拓扑降维表示

这是当前论文主线里最重要的一点。

传统“世界模型”常面向原始观测或高维连续状态；而本系统把导航预测问题降维为：

对“下一条拓扑边”做未来结果预测。

也就是说，不直接预测原始图像序列，而是把每一次边执行抽象为一个监督样本：

$$x_k \mapsto y_k$$

其中：

• $x_k$：执行第 $k$ 条边之前可获得的低维因果特征；
• $y_k$：执行该边后的真实结果。

标签定义为：

$$y_k = \bigl(t_k,\ b_k,\ f_k,\ r_k\bigr)$$

其中：

• $t_k$：真实通行时间；
• $b_k \in \{0,1\}$：是否被阻塞；
• $f_k \in \{0,1\}$：是否失败；
• $r_k \in \{0,1\}$：是否触发恢复。

所以，系统学习的是一个轻量拓扑世界模型：

$$f_\theta(x_k)=\bigl(\hat t_k,\ \hat p_{b,k},\ \hat p_{f,k}\bigr)$$

这一步的意义在于：

• 把原本高维、难在线部署的预测问题，转化为边级预测；
• 让世界模型第一次可以以毫秒级代价进入在线规划循环；
• 这也是“世界模型降维后能跑导航预测”的核心数学形式。

---

3. 因果特征构造算法

3.1 特征向量定义

当前用于世界模型的输入特征可写为：

$$x_k = \Big[ x_k^{static}, x_k^{cfg}, x_k^{time}, x_k^{hist}, x_k^{nbr}, x_k^{ctx}, x_k^{map}, x_k^{struct}, x_k^{prior} \Big]$$

对应九组特征：

1. 边静态属性；
2. 配置层风险；
3. 时间桶 one-hot；
4. 同边历史窗口；
5. 邻域传播统计；
6. episode 上下文；
7. 地图 one-hot；
8. 图结构特征；
9. 全局先验。

3.2 同边历史窗口

对当前边 $e=(u,v)$，记本 episode 内该有向边的历史事件集合为：

$$\mathcal{H}_e = \{h_1,\dots,h_m\}$$

则同边统计特征为：

$$\mu_t(e)=\frac{1}{m}\sum_{i=1}^{m} t_i$$ $$\sigma_t(e)=\sqrt{\frac{1}{m}\sum_{i=1}^{m}(t_i-\mu_t(e))^2}$$ $$\rho_b(e)=\frac{1}{m}\sum_{i=1}^{m} b_i,\qquad \rho_f(e)=\frac{1}{m}\sum_{i=1}^{m} f_i$$

并保留最近 $K=5$ 次结果作为窗口特征：

$$[t_{m-K+1},\dots,t_m],\quad [b_{m-K+1},\dots,b_m],\quad [f_{m-K+1},\dots,f_m]$$

3.3 邻域传播特征

对当前边 $e=(u,v)$，取与端点 $u,v$ 相邻的近期事件集合 $\mathcal{N}_e$，构造：

$$\rho_b^{nbr}(e)=\frac{1}{|\mathcal{N}_e|}\sum_{h\in\mathcal{N}_e} b(h)$$ $$\rho_f^{nbr}(e)=\frac{1}{|\mathcal{N}_e|}\sum_{h\in\mathcal{N}_e} f(h)$$ $$\mu_t^{nbr}(e)=\frac{1}{|\mathcal{N}_e|}\sum_{h\in\mathcal{N}_e} t(h)$$

它表达“局部区域最近是否整体变差”。

3.4 结构特征

多地图阶段引入的结构特征包括：

$$d_{src}(e),\ d_{dst}(e),\ \min(d_{src},d_{dst}),\ c_{shared}(e)$$

其中：

• $d_{src}$：源节点度数；
• $d_{dst}$：目标节点度数；
• $c_{shared}$：两端点共享邻居数。

这些特征用于表达“这条边是不是脆弱通道、是不是更接近瓶颈”。

3.5 全局先验

系统把“全局先验”和“episode 内历史”显式分开。

对训练集中的边 $e=(u,v)$ 与时间桶 $\tau$，构造：

$$\mu_t^{prior}(e,\tau),\quad \rho_b^{prior}(e,\tau),\quad \rho_f^{prior}(e,\tau),\quad n^{prior}(e,\tau)$$

即：

• 先验平均通行时间；
• 先验阻塞率；
• 先验失败率；
• 样本计数。

这组特征的作用是解决冷启动问题：即使某条边在当前 episode 里没有历史，模型也仍然能拿到稳定先验。

---

4. 动态环境采样算法

仿真中的每一次边执行不是确定的，而是由 DynamicsEngine 按随机机制采样。

4.1 阻塞与失败采样

对边 $e$ 在时间桶 $\tau$ 下：

$$b \sim \mathrm{Bernoulli}(p_b(e,\tau))$$

如果未阻塞，再采样失败事件：

$$f \sim \mathrm{Bernoulli}(p_f(e,\tau))$$

如果已阻塞，则直接令 $f=0$。

4.2 邻域阻塞传播

如果相邻边在本 episode 中曾被阻塞，则当前边的阻塞概率增加：

$$p_b'(e,\tau)=\min\bigl(1,\ p_b(e,\tau)+\lambda_{nbr}\bigr)$$

其中 $\lambda_{nbr}$ 是邻域传播增益。

4.3 通行时间采样

若未阻塞，通行时间为：

$$t = t_{base}(e)\cdot \max(0.3,1+\epsilon),\qquad \epsilon\sim \mathcal{N}(0,\sigma^2)$$

若阻塞，则：

$$t = \kappa_{block}\cdot t_{base}(e)$$

其中 $\kappa_{block}$ 是阻塞时间放大倍数。

---

5. 语言 grounding 算法

系统采用“两阶段 grounding”。

5.1 规则检索

先用规则检索器对每个节点打分：

$$S_{rule}(v,q)= S_{name}+S_{id}+S_{alias}+S_{landmark}+S_{tag}+S_{desc}+S_{roomtype}+S_{floor}+S_{dir}$$

具体逻辑：

• 精确显示名匹配给高分；
• 节点别名匹配给高分；
• landmark、object tag、room type、floor、方向词提供附加分；
• floor 不匹配时会施加惩罚。

这个规则分数本质上是一个人工设计的加法打分器。

5.2 向量召回

节点文本和用户查询都被编码到同一向量空间：

$$z_v = \mathrm{Enc}(v),\qquad z_q=\mathrm{Enc}(q)$$

因为 embedding 已做归一化，召回分数可写成余弦相似度：

$$S_{emb}(v,q)=\cos(z_v,z_q)=z_v^\top z_q$$

然后取 top-$K$ 候选。

5.3 规则重排

对召回候选再做规则重排：

$$S_{final}(v,q)=\alpha S_{emb}(v,q)+(1-\alpha)\tilde S_{rule}(v,q)$$

其中：

• $\tilde S_{rule}$ 是规则分数做截断和归一化后的结果；
• $\alpha\in[0,1]$ 控制语义相似度与规则匹配之间的权重。

这使得 embedding 负责“语义召回”，规则负责“精确校正”。

---

6. 基线规划算法

6.1 几何最短路 shortest_path

在图 $G$ 上对每条边赋予几何代价：

$$c_{geo}(e)=\mathrm{distance}(e)$$

然后用 Dijkstra 求：

$$\pi^\*=\arg\min_{\pi:s\rightarrow g}\sum_{e\in\pi} c_{geo}(e)$$

这是最弱的基线，因为它只看物理距离。

6.2 语义无风险规划 semantic_no_risk

对每条边用基础通行时间作为代价：

$$c_{time}(e)=t_{base}(e)$$

同样使用 Dijkstra：

$$\pi^\*=\arg\min_{\pi:s\rightarrow g}\sum_{e\in\pi} t_{base}(e)$$

这比纯距离更贴近导航实际，但仍不考虑动态风险。

6.3 历史风险规划 historical_risk

对每条边代价定义为：

$$c_{hist}(e,\tau)=t_{base}(e)+\beta p_b^{cfg}(e,\tau)+\gamma p_f^{cfg}(e,\tau)$$

其中：

• $\beta$：把阻塞概率映射成时间惩罚；
• $\gamma$：把失败概率映射成时间惩罚。

然后仍用 Dijkstra 最小化路径总代价。

这个规划器的本质是：不预测未来，只使用手工配置或历史平均风险。

---

7. 世界模型训练算法

7.1 历史基线预测器

训练阶段有一个参考基线 HistoricalBaseline。

如果当前边已有历史，则直接用历史均值：

$$\hat t = \mu_t(e),\qquad \hat p_b=\rho_b(e),\qquad \hat p_f=\rho_f(e)$$

否则退化为配置先验：

$$\hat t=t_{base}(e),\qquad \hat p_b=p_b^{cfg}(e,\tau),\qquad \hat p_f=p_f^{cfg}(e,\tau)$$

7.2 LightGBM 世界模型

系统实际使用的是三头轻量树模型：

$$\hat t = f_t(x),\qquad \hat p_b = f_b(x),\qquad \hat p_f = f_f(x)$$

更具体地，树集成可以形式上写成：

$$f(x)=\sum_{m=1}^{M}\eta f_m(x)$$

其中 $f_m$ 是第 $m$ 棵决策树，$\eta$ 是学习率。

三个头分别对应：

• travel-time 回归；
• block 二分类；
• fail 二分类。

7.3 概率校准

分类头输出的原始概率再经过校准器：

$$\tilde p = c(p_{raw})$$

项目中支持：

• identity calibrator；
• sigmoid / Platt scaling；
• isotonic regression。

当前主线主要使用 isotonic calibration。

7.4 阈值选择

对 fail 头，还会在验证集上选阈值：

$$\theta^\*=\arg\max_{\theta\in\Theta}\ \mathrm{Score}(y,\mathbb{1}[\hat p\ge\theta])$$

其中目标函数可以是：

• $F_1$
• $F_2$
• recall

---

8. Quantile 不确定性估计算法

系统训练两个分位数回归器：

$$\hat q_{0.1}(x),\qquad \hat q_{0.9}(x)$$

然后把不确定性定义为区间宽度：

$$u(x)=\hat q_{0.9}(x)-\hat q_{0.1}(x)$$

这本来打算用于规划器中的不确定性惩罚：

$$c(e)=\alpha \hat t + \beta \hat p_b + \gamma \hat p_f + \delta u$$

但当前实验里多数正式最优结果都把 $\delta$ 压到了 $0$，说明这条线暂时还没有稳定收益。

---

9. 预测规划算法 PredictivePlanner

这是当前世界模型真正进入导航闭环的核心位置。

9.1 边代价

给定世界模型预测结果：

$$f_\theta(x_e)=\bigl(\hat t_e,\hat p_{b,e},\hat p_{f,e},u_e\bigr)$$

则边代价定义为：

$$c_{pred}(e)= \alpha \hat t_e +\beta \hat p_{b,e} +\gamma \hat p_{f,e} +\delta u_e$$

其中：

• $\alpha$：时间项权重；
• $\beta$：阻塞风险惩罚；
• $\gamma$：失败风险惩罚；
• $\delta$：不确定性惩罚。

9.2 路径搜索

规划器仍然使用 Dijkstra，只是把传统手工权重换成了模型预测权重：

$$\pi^\*=\arg\min_{\pi:s\rightarrow g}\sum_{e\in\pi} c_{pred}(e)$$

这就是为什么当前系统虽然没有做强化学习，却已经是“预测驱动规划”：

• 搜索算法没变；
• 但代价函数已经从手工先验变成了 learned world model。

---

10. Reactive Recovery：规则恢复器

当边执行失败后，系统使用规则式恢复器：

$$a = \mathcal{R}(state)$$

动作集合：

$$\mathcal{A}_{reactive}=\{\text{REPLAN},\ \text{BACKTRACK},\ \text{WAIT},\ \text{RELOCALIZE},\ \text{ABORT}\}$$

当前规则优先级是：

1. replan_count 过多 $\rightarrow$ ABORT
2. 当前边 blocked $\rightarrow$ REPLAN
3. 连续失败次数过多 $\rightarrow$ BACKTRACK
4. 当前边 failed $\rightarrow$ REPLAN
5. 定位退化 $\rightarrow$ RELOCALIZE

其中 backtrack 节点选择逻辑是：

从 path_taken 逆序扫描，找到第一个“不是所有出边都已阻塞”的安全节点。

---

11. Proactive Recovery：预测驱动恢复

这是 Phase 8 的核心。

11.1 触发条件

在真正执行下一条边 $e_k$ 之前，先预测：

$$(\hat t_k,\hat p_{b,k},\hat p_{f,k},u_k)=f_\theta(x_k)$$

若满足：

$$\hat p_{b,k}\ge\tau_b \quad \text{or} \quad \hat p_{f,k}\ge\tau_f$$

则进入 proactive decision。

11.2 v1.1 的保守策略

当前最稳的版本 predictive_proactive_v1_1 基本只保留一个主动动作：

$$\text{REPLAN\_NOW}$$

但不是只要高风险就 replan，而是要求替代边满足资格约束：

$$\hat p_b(e_{cur})-\hat p_b(e_{alt}) \ge \Delta_b$$

并且

$$\hat t(e_{alt})-\hat t(e_{cur}) \le \Delta_t$$

也就是说，替代边必须在风险上明显更好，同时时间上不能过分恶化。

11.3 full 的 rollout scoring

完整 Phase 8 会对候选动作做短视 rollout 打分。

对某个候选动作 $a$ 产生的路径 $\pi_a$，定义：

$$J(a)=\lambda_{extra}(a)+\sum_{h=0}^{H-1} c_{pred}(e_h) +\lambda_{bottle}\cdot \mathbf{1}_{bottleneck}(e_h)\cdot \hat p_b(e_h)$$

其中：

• $H$：rollout horizon；
• $\lambda_{extra}$：动作额外代价，例如 wait 和 backtrack 的固定惩罚；
• $\lambda_{bottle}$：瓶颈边惩罚系数。

然后在动作集合中选：

$$a^\*=\arg\min_{a\in\mathcal{A}} J(a)$$

如果最优动作相对当前动作的收益不足最小间隔：

$$J(a^\*) + m \ge J(\text{current})$$

则不触发 proactive intervention。

11.4 为什么 full 不一定更好

因为 full 把预测误差放大到了控制层：

• 若 $\hat p_b$ 误报偏高，就可能过早回避可走边；
• 若新图 OOD，rollout 分数也会跟着偏；
• 所以 full 比纯 planner 更依赖预测质量和跨场景鲁棒性。

---

12. 多地图训练与 map-aware 泛化

为了让世界模型从“单图有效”走向“跨图可用”，系统在多地图阶段显式引入：

$$x^{map}=[\mathbf{1}_{base},\mathbf{1}_{annex}]$$

以及结构特征：

$$x^{struct}=[d_{src},d_{dst},\min(d_{src},d_{dst}),c_{shared}]$$

相当于让模型学习：

$$f_\theta(x,\text{map\_id},\text{structure})$$

而不是默认所有边都来自同一个环境分布。

这一步并没有保证总体成功率一定上升，但它是跨地图泛化必须补上的算法条件。

---

13. 实验统计与比较方法

项目里的正式比较不是只看平均成功率，而是采用 paired evaluation。

对同一组 episode schedule，比较目标方法与基线：

• target-only wins：目标成功、基线失败；
• baseline-only wins：目标失败、基线成功；
• ties：两者同成败。

再用二项检验判断显著性：

$$p = \Pr\bigl(X \text{ 至少像观测结果一样极端}\bigr),\qquad X\sim \mathrm{Binomial}(n,0.5)$$

这保证结果不是只被 seed 波动误导。

---

14. 当前主线与算法定位

从当前整个系统看，各算法的定位可以总结为：

14.1 已经证明有价值的主线算法

1. 拓扑降维世界模型

   • 把高维导航预测问题转成边级预测；
   • 这是论文最核心的算法抽象。

2. predictive_no_unc

   • 这是世界模型接入规划闭环后的主结果；
   • 当前最能代表“世界模型带来可部署导航收益”的版本。

3. 两阶段 grounding

   • embedding recall + rule rerank；
   • 是自然语言目标到拓扑节点的主算法。

14.2 已经打通但尚未成为主结果的算法

1. predictive_proactive_v1_1

   • 在单图上稳定不差；
   • 但跨图还没能稳住。

2. predictive_proactive_full

   • 算法上更完整；
   • 但实验上目前还不是最优规划器。

3. uncertainty 惩罚

   • 量化模型已接入；
   • 但 $\delta$ 目前大多被搜索压成 $0$。

---

15. 一句话总结

如果把整套系统压缩成一句数学化描述，那么当前项目做的事情可以概括为：

$$\text{语言查询 } q \xrightarrow{\text{grounding}} g \xrightarrow{\text{拓扑图 } G} \text{候选边} \xrightarrow{\text{轻量世界模型 } f_\theta} (\hat t,\hat p_b,\hat p_f,u) \xrightarrow{\text{risk-aware planning / recovery}} \pi^\*$$

而论文里最值得强调的核心贡献，是下面这一步：

$$\text{高维导航未来预测} \Longrightarrow \text{拓扑边级低维世界模型预测}$$

也就是：

通过拓扑降维，把世界模型从“难以在线使用的高维预测器”，变成了“可以直接接进导航规划器的轻量决策模块”。