CORE Planner 深度解读：基于上下文记忆的强化学习未知环境导航

Hermit_Rabbit

171人浏览 · 2026-07-26 16:48:57

Hermit_Rabbit · 2026-07-26 16:48:57 发布

0. 引子

这篇文章的切入点非常具体——它不是在做"更好的感知"或"更大的模型"，而是在解决一个几乎所有未知环境导航方法都会遇到的老问题：导航死锁振荡（Navigation Deadlock Oscillation, NDO）。机器人在部分可观测环境中反复做出相互矛盾的局部决策，导致原地打转。这个现象在 FAR Planner、RRT 变体、乃至大部分基于学习的局部规划器中都广泛存在，但此前没有工作把它形式化并给出系统性的解决方案。CORE 的回答是：把机器人的历史轨迹编码为图节点上的上下文记忆，让策略网络在每一步决策时都能"看见"自己曾经走过哪里、走过多少次，从而避免重蹈覆辙。

这里的关键是，CORE 并没有发明新的网络架构——它用的是标准 Transformer 编码器-解码器加 Pointer Network，这些都是 2015-2017 年的技术。它的贡献在于环境表示和记忆机制的设计：用稀疏可见性图替代密集栅格地图，用节点级访问计数替代固定长度的历史缓冲区。这种设计让模型能够在纯图像仿真环境中训练，然后零样本迁移到真实机器人上——不需要 Gazebo、不需要 Isaac Sim、不需要任何 fine-tuning。代码目前已经开源：https://github.com/BBD00/core_planner

在这里插入图片描述

1. 问题定义与核心挑战

1.1 未知环境导航的形式化

未知环境导航的核心目标可以用一句话概括：机器人在没有先验地图的情况下，仅依靠实时传感器数据，找到一条从当前位置到目标点的最短无碰撞路径。形式上，环境被建模为二维占据栅格地图 $E$ ，包含自由空间 $E_f$ 和障碍空间 $E_o$ 。机器人维护一个环境信念（belief） $B$ ，由已知障碍 $B_o$ 、未知区域 $B_u$ 和已知自由区域 $B_f$ 组成。每一步，机器人通过传感器观测 $M$ 更新信念 $\cup M$ ，直到到达目标。
$p^{*}(t) = \arg\min_{p \in \mathcal{P}(t)} \mathrm{Cost}\big(p;\; S(t-1), B(t-1)\big)$
这里 $\mathcal{P}(t)$ 是时刻 $t$ 的候选路径点集合， $\mathrm{Cost}$ 是一个综合了到目标点的欧氏距离、当前航向偏差角度、路径曲率平滑度等多项指标的加权目标函数。这个公式看起来是路径规划领域的标准范式，但它隐含了一个对后续讨论至关重要的假设：代价函数只依赖当前时刻的环境信念，完全不显式编码机器人此前的运动历史和决策记录。

1.2 导航死锁振荡的根因

在部分可观测环境下，仅依赖当前信念的贪心决策会导致连续步骤之间的评估不一致。考虑一个多房间场景：机器人贪心选择直线距离最短的路径点 A，但移动过程中新的传感器数据揭示 A 方向存在障碍，于是之前被忽略的路径点 B 重新变成最优选择。机器人掉头走向 B，但走了几步后信念再次更新，A 又变得更优。这种循环无限重复——这就是论文定义的 导航死锁振荡（NDO） 现象。

直觉理解：这类似于导航软件在两条路线之间反复切换的情形。假设你开车到一个路口，导航说"左转"；你左转后发现前方堵车，导航重新规划说"调头右转"；你调头后发现右边也堵了，导航又说"调头左转"。问题的根源不是算法错了，而是算法没有"记忆"——它不知道自己刚刚已经尝试过左转并失败了。

1.3 CORE 的解决思路

CORE 的核心洞察非常明确：把历史轨迹信息紧凑地编码到图节点的特征向量中，而不是作为一个不断增长的时间序列交给循环网络来处理。具体来说，图中每个节点都维护一个"访问计数"（trajectory indicator），记录机器人曾经经过该节点附近多少次。这样，目标函数从公式 (1) 扩展为：
$p^{*}(t) = \arg\min_{p \in \mathcal{P}(t)} \mathrm{Cost}\big(p;\; S(0), \ldots, S(t-1), B(t-1)\big)$
这里 $\ldots, S(t-1)$ 代表了从任务初始化到当前时刻的完整历史状态序列，但在实际工程实现中它被压缩为节点级的标量访问计数 $v_i$ ，不会随轨迹长度增长而增加推理延迟。这意味着模型能利用任意长度的历史信息，同时保持常数级别的推理开销。

2. 整体框架与系统流水线

2.1 从传感器到决策的数据流

CORE Planner 的整体流水线可以分为四个阶段：感知、图构建、特征编码、动作选择。传感器（LiDAR 或 RGB-D 相机）获取点云数据后，系统首先在局部地图上提取障碍物轮廓的可见性图节点；然后将这些节点与机器人节点、目标节点、前沿聚类节点合并为一张增量更新的全局稀疏图；接着，每个节点的 5 维特征向量（相对坐标、效用值、目标距离、访问计数）被送入 Transformer 编码器提取全局特征；最后，解码器以机器人节点为查询，通过 Pointer Network 输出邻居节点上的概率分布，选择下一个路径点。

在这里插入图片描述

2.2 关键设计决策：为什么是可见性图

传统导航方法大多使用密集栅格地图作为环境表示。一张 500x500 的栅格地图有 25 万个像素点，如果直接送入神经网络，输入维度巨大且包含大量冗余信息。FAR Planner 率先引入可见性图来降低计算开销，但它仍然依赖手工规则来选择路径点。CORE 的选择是：保留可见性图的稀疏表示优势，但把路径点选择这一步交给强化学习策略来做。这样既避免了密集栅格的计算瓶颈，又摆脱了手工启发式函数的局限性。

进一步看，可见性图还有一个对强化学习极为友好的特性：它天然定义了"动作空间"。在可见性图中，机器人的当前位置节点与其他节点之间的无碰撞连接（即可见边）就是所有合法的下一步选择。这意味着动作空间是离散且动态的——节点数量随探索进展而变化，而 Pointer Network 恰好能处理这种变长输入输出。

2.3 训练与部署的解耦

CORE 的训练完全在基于图像的二维仿真环境中完成——不需要物理引擎、不需要动力学模型、不需要域随机化。训练环境就是一张 500x500 像素的灰度图像，黑色像素是障碍物，白色是自由空间，灰色是未知区域。这种极度简化的训练设置能够实现零样本迁移的原因在于：可见性图作为中间表示，隔离了传感器差异。无论输入是 LiDAR 点云还是 RGB-D 深度图，只要能从中提取出障碍物轮廓并构建可见性图，后续的决策流程就完全相同。

3. 环境表示：稀疏可见性图的构建

3.1 从栅格地图到轮廓点

可见性图构建的第一步是从当前信念地图中提取障碍物轮廓。在代码实现中，这通过 OpenCV 的轮廓检测完成，然后对轮廓进行等间距采样得到一组代表性的"可见性节点"。每个节点代表障碍物边缘上的一个关键几何位置——这些位置决定了机器人的可通行路径。

# utils.py — 可见性图提取核心逻辑
def extract_visible_graph_from_map(map_info):
    grid_map = map_info.map
    binary_map = ((grid_map == OCCUPIED) | (grid_map == UNKNOWN)).astype(np.uint8) * 255
    contours, _ = cv2.findContours(binary_map, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

    # 对每个轮廓等间距采样
    contour_sampled_points = []
    for contour in contours:
        perimeter = cv2.arcLength(contour, True)
        num_samples = max(3, int(perimeter / NODE_RESOLUTION))
        # ... 采样逻辑 ...

    # 批量碰撞检测建立可见边
    all_points_array = np.array(all_points, dtype=np.float64)
    starts = all_points_array[pairs_i]
    ends = all_points_array[pairs_j]
    collision_results = _batch_collision_check(starts, ends, map_info.map, 0.0, 0.0, 1.0)

    # 构建可见性矩阵
    visibility_matrix = np.zeros((n, n), dtype=bool)
    for idx, (i, j) in enumerate(zip(pairs_i, pairs_j)):
        if collision_results[idx] == 0:
            visibility_matrix[i, j] = True
            visibility_matrix[j, i] = True
    return visible_graphs

这段代码的工程细节值得注意。首先，碰撞检测使用了 Numba 的 @njit 加速的 Bresenham 直线算法——对于每一对候选节点，沿连线逐像素检查是否穿过障碍物或未知区域。其次，所有点对之间的碰撞检测被组织为批量并行操作（_batch_collision_check），利用 NumPy 向量化和 Numba 的 parallel=True 来加速。这种设计使得即使在包含数百个轮廓点的复杂地图中，图构建的耗时也能控制在毫秒级别。

3.2 增量式图维护

CORE 的可见性图不是每一步从头构建的——那样计算开销太大。它采用增量更新策略：每一步只处理传感器新观测到的区域变化，添加新节点、移除失效节点、更新受影响的边。这个逻辑由 NodeManager 类的 update_graph 方法实现：这一点在实际部署中具有重要意义。这种增量式的处理方式是保证系统在大规模环境中实时响应的基础性工程决策，避免了全局重建带来的计算瓶颈。

# node_manager.py — 增量图更新
def update_graph(self, robot_location, frontiers, updating_map_info, map_info):
    self._update_counter += 1
    # 1. 检测地图变化区域
    changed_coords = self.get_changed_region_mask(updating_map_info)
    # 2. 提取当前局部地图的可见性图
    visible_graphs = extract_visible_graph_from_map(updating_map_info)
    # 3. 移除与新观测矛盾的历史节点
    self.remove_history_node(robot_location, new_visible_points, updating_map_info)
    # 4. 仅在变化区域添加新节点
    for contour in visible_graphs:
        for index in range(len(contour_edges)):
            point_coord = get_coords_from_cell_position(...)
            node = self.check_node_exist_in_dict(point_coord)
            if node is None:
                if self.is_near_changed_region(point_coord, changed_coords, threshold=5.0):
                    node = self.add_node_to_dict(point_coord, frontiers, egdes_coord, updating_map_info)
    # 5. 更新机器人节点、目标节点、前沿聚类节点
    self.add_node_to_dict(robot_location, frontiers, [], updating_map_info, is_robot=True)

这里的关键是第 4 步的条件判断 is_near_changed_region：只有当候选节点位于本帧新观测的"变化区域"附近时，才会被添加到图中。这避免了在已经稳定的区域重复添加冗余节点，是保证图稀疏性的重要工程手段。图的空间索引使用 R-tree（rtree.index.Index），支持高效的范围查询和最近邻搜索。

工程价值：增量更新策略是 CORE 能在边缘设备（Jetson Orin NX）上实时运行的关键。如果每一步都重建完整的可见性图，在包含 1500 个节点的森林环境中耗时将从 10ms 飙升到数百毫秒，远超实时要求。

3.3 节点数据结构与邻接关系

每个图节点由 LocalNode 类表示，维护着自身坐标、邻居节点集合、可观测前沿点集合和效用值。下面的代码展示了节点的完整数据结构定义，可以看到每个节点本质上是一个带邻接信息的空间实体：这是该方法能够在边缘设备上实时运行的前提条件之一。

# node_manager.py — 节点数据结构
class LocalNode:
    def __init__(self, id, coords, frontiers, edge_coords, updating_map_info):
        self.coords = coords            # 世界坐标 (x, y)
        self.utility_range = UTILITY_RANGE  # 效用计算半径 = 0.8 * SENSOR_RANGE
        self.utility = 0                # 可观测前沿点数量
        self.id = id
        self.neighbor_ids = set()       # 邻居节点 ID 集合
        self.neighbor_coords_dist = dict()  # 邻居坐标字典
        self.observable_frontiers = self.initialize_observable_frontiers(frontiers, updating_map_info)

节点的效用值（utility）定义为该节点能"看到"的前沿点数量——前沿点是已知区域与未知区域的边界，代表探索的潜力方向。这个设计受 frontier-based exploration 文献的启发：一个效用值高的节点意味着走向它可能揭示大量新的未知区域，有助于找到通往目标的路径。这种设计选择体现了工程实践中对计算效率和决策质量的平衡考量。

4. 上下文记忆机制：让策略"记住"走过的路

4.1 从二值标记到访问计数

CORE 之前最接近的工作是 CADRL（Context-Aware Deep RL），它也尝试在状态中加入轨迹信息，但只使用了一个二值标记——节点要么"去过"（1）要么"没去过"（0）。这种粗粒度的编码在短距离导航中够用，但在长距离多房间场景中信息量不足。如果机器人在某个区域反复打转 10 次，二值标记和仅打转 1 次是无法区分的。

CORE 的改进是用整数访问计数 $v_i$ 替代二值标记，使策略网络能够精确区分一个区域被经过一次还是十次的显著差异，从而做出更加精细化的路径回避决策，有效打破反复徘徊的恶性循环。形式上，每个节点的访问计数按照如下数学公式从完整历史轨迹中统计得到：
$v_i = \sum_{\tau=1}^{t} \mathbb{I}\big(D(r_\tau, p_i) < \epsilon\big)$
其中 $D(\cdot, \cdot)$ 表示两个空间坐标之间的欧几里得距离度量， $\mathbb{I}(\cdot)$ 是布尔指示函数（条件为真返回一否则返回零）， $r_\tau$ 是机器人在历史时刻 $\tau$ 记录下的实际世界坐标位置， $p_i$ 是当前被评估节点 $n_i$ 的空间坐标， $\epsilon$ 是判定一次经过是否算作有效访问的距离阈值参数（代码中设定为三个栅格单元即一点二米的物理距离）。换句话说， $v_i$ 统计了从任务启动到当前时刻为止机器人经过节点 $n_i$ 邻域范围内的累积总次数。

4.2 代码实现：stay_count 的计算

在实际代码中，访问计数通过 get_stay_count 函数实现。这个函数遍历机器人从 episode 开始到当前时刻的完整历史轨迹，统计有多少个历史位置点落在当前评估节点的阈值距离范围内，返回的整数计数就是该节点的上下文记忆值：从工程角度来看，这种实现方式在保证正确性的同时最大化了运行效率。

# utils.py — 上下文记忆的核心计算
def get_stay_count(current_position, past_trajectory_x, past_trajectory_y,
                   window_size=STAY_WINDOW_SIZE, threshold=STAY_DIS_THRESHOLD):
    if len(past_trajectory_x) < window_size:
        window_size = len(past_trajectory_x)
    recent_x = past_trajectory_x
    recent_y = past_trajectory_y
    distances = np.hypot(
        np.array(recent_x) - current_position[0],
        np.array(recent_y) - current_position[1]
    )
    close_count = np.sum(distances < threshold)
    return close_count

这里有一个工程上的考量：默认参数 STAY_WINDOW_SIZE=20，STAY_DIS_THRESHOLD=3（对应物理距离 3 x 0.4m = 1.2m）。窗口大小限制了回溯的深度——虽然论文声称使用"完整历史"，但代码中实际使用了整条轨迹（past_trajectory_x 是从 episode 开始到当前的所有位置），因为 recent_x = past_trajectory_x 没有截断。阈值距离 1.2m 大约是两个栅格节点的间距，这意味着机器人只要经过一个节点 1.2m 范围内就算"访问过"。

难点提示（为什么 stay_count 能解决 NDO）：想象你在一个陌生商场里找出口。如果你记不住自己走过哪些通道，你可能在同一个环形走廊里绕圈——每次到一个岔路口都觉得"看起来那边更近"，但那边其实是死路。现在假设地板上会留下你的脚印，脚印越多说明你来过越多次。一个理性的策略会主动避开脚印密集的区域，因为那意味着那个方向很可能不通。CORE 的 stay_count 就是这个"脚印计数"，它被编码到节点特征中让网络学会"看到高访问计数就降低选择概率"。

4.3 特征向量的组装

每个节点最终被表示为 5 维特征向量 $\mathbf{x}_i = [p_i \oplus u_i \oplus d_i \oplus v_i]$ ，其中 $p_i$ 是相对于机器人的归一化坐标（2 维）， $u_i$ 是归一化效用值， $d_i$ 是到目标的欧几里得距离， $v_i$ 是访问计数。归一化策略如下：这种机制确保了系统在长时间运行中的稳定性和可靠性。

# agent.py — 观测特征组装
def get_observation(self):
    current_node_coords = node_coords[self.current_index]
    # 坐标归一化：相对于机器人位置，除以最大绝对值
    relative_coords = np.concatenate((
        node_coords[:, 0].reshape(-1, 1) - current_node_coords[0],
        node_coords[:, 1].reshape(-1, 1) - current_node_coords[1]
    ), axis=-1)
    max_abs_coord = np.max(np.abs(relative_coords))
    if max_abs_coord > 1e-6:
        node_coords = relative_coords / max_abs_coord

    # 效用值归一化：除以传感器扇形面积内的最大前沿点数
    node_utility = node_utility / (SENSOR_RANGE * 3.14 // FRONTIER_CELL_SIZE)

    # 拼接 5 维特征
    node_inputs = np.concatenate(
        (node_coords, node_utility, node_goal_distance, node_stay_count), axis=1
    )
    node_inputs = torch.FloatTensor(node_inputs).unsqueeze(0).to(self.device)

坐标的归一化方式值得关注：它以机器人为原点，然后除以当前图中所有节点的最大坐标绝对值。这意味着无论环境尺度多大，坐标特征始终落在 [-1, 1] 范围内。这种自适应归一化是 CORE 能在不同尺度环境中泛化的原因之一——训练时的 100m x 100m 地图和部署时的走廊、森林在归一化后具有相似的特征分布。

5. 网络架构：Transformer 编码全局结构，Pointer 输出局部决策

5.1 编码器：图结构约束的注意力

在整个网络架构中，编码器承担的核心任务是将可见性图中所有节点的原始五维低级特征映射为高维的环境感知特征表示，使得每个节点的嵌入向量不仅包含自身的局部信息，还融合了来自相邻节点的全局上下文。具体实现上，它首先通过一个带有激活函数的线性投射层将输入从五维原始空间映射到一百二十八维的隐式嵌入空间，然后通过多层堆叠的标准多头自注意力编码层进行全图范围内的信息聚合与交互：
$\mathbf{H}^{(0)} = \text{ReLU}(\mathbf{X} \cdot W_{emb} + \mathbf{B}_{emb}), \quad W_{emb} \in \mathbb{R}^{5 \times 128}$
这里的关键设计是编码器掩码（edge_mask）：它基于可见性图的邻接矩阵构建，使得每个节点在自注意力计算中只能关注（attend to）与它有可见边相连的节点。这不是标准 Transformer 的全连接注意力——它引入了图结构先验，防止不相邻的节点之间产生虚假的注意力关联。

# model.py — 多头注意力（图约束版）
class MultiHeadAttention(nn.Module):
    def forward(self, q, k=None, v=None, key_padding_mask=None, attn_mask=None):
        # ...
        U = self.norm_factor * torch.matmul(Q, K.transpose(2, 3))
        if attn_mask is not None:
            U = U.masked_fill(attn_mask == 1, float('-1e8'))  # 图结构掩码
        attention = torch.softmax(U, dim=-1)
        # ...

编码器使用 8 头注意力、128 维嵌入空间，经过多层自注意力编码后输出环境感知特征 $h_e \in \mathbb{R}^{N \times 128}$ ，其中 $N$ 是当前图的节点数。由于 batch 训练要求固定张量形状，节点数统一填充到 512（NODE_PADDING_SIZE），填充位置用 padding mask 屏蔽。这一设计决策直接影响了后续模块的输入格式和处理逻辑。

5.2 解码器：机器人视角的交叉注意力

解码器的设计相对简洁。它首先从编码器输出中提取机器人节点的特征 $h_r$ ，然后以 $h_r$ 为 query、全局特征 $h_e$ 为 key/value 执行一层交叉注意力。这一步的语义是：站在机器人的视角，审视整个已知环境的结构。这种方式在保持代码简洁性的同时提供了足够的灵活性。

# model.py — 解码器
class GraphDecoder(nn.Module):
    def forward(self, current_node_feature, enhanced_node_feature, node_padding_mask):
        # current_node_feature: 机器人节点特征 [batch, 1, 128]
        # enhanced_node_feature: 全局编码特征 [batch, N, 128]
        output = self.decoder_layer(
            current_node_feature,     # query
            enhanced_node_feature,    # key & value
            node_padding_mask         # padding mask
        )
        return output

解码器输出的增强特征 $\tilde{h}_r$ 与原始机器人节点特征 $h_r$ 拼接后形成 256 维向量，再通过一个线性层 current_embedding 压缩回 128 维，作为融合了全局上下文信息的最终状态表示送入 Pointer Network 进行动作概率计算。从系统集成的角度来看，这种方案降低了模块间的耦合度。

5.3 Pointer Network：变长动作空间的概率输出

Pointer Network 是这个架构中处理动态动作空间的关键组件。不同于标准 RL 中固定大小的动作空间（如 Atari 的若干按键），CORE 的动作空间是机器人当前节点的所有邻居节点——这个数量随图结构变化而不同。Pointer Network 通过注意力机制在可变数量的候选项上产生概率分布：

# model.py — Pointer Network（单头注意力实现）
class SingleHeadAttention(nn.Module):
    def __init__(self, embedding_dim):
        super().__init__()
        self.tanh_clipping = 10
        self.norm_factor = 1 / math.sqrt(embedding_dim)
        self.w_query = nn.Parameter(torch.Tensor(embedding_dim, embedding_dim))
        self.w_key = nn.Parameter(torch.Tensor(embedding_dim, embedding_dim))

    def forward(self, q, k, mask=None):
        # q: 机器人状态特征 [batch, 1, 128]
        # k: 邻居节点特征 [batch, K, 128]
        Q = torch.matmul(q.reshape(-1, n_dim), self.w_query).view(shape_q)
        K = torch.matmul(k.reshape(-1, n_dim), self.w_key).view(shape_k)
        U = self.norm_factor * torch.matmul(Q, K.transpose(1, 2))
        U = self.tanh_clipping * torch.tanh(U)  # 裁剪到 [-10, 10]
        if mask is not None:
            U = U.masked_fill(mask == 1, -1e8)
        attention = torch.log_softmax(U, dim=-1)
        return attention

这里有两个设计细节。第一，使用 tanh_clipping 将原始分数裁剪到 [-10, 10] 范围——这是 Pointer Network 在组合优化问题中的标准做法，防止 logits 过大导致概率分布退化为 one-hot。第二，输出的是 log_softmax 而非 softmax——这为后续 SAC 训练中的熵计算和对数概率求导提供了数值稳定性。这个数值选择经过了大量实验调优，在多种环境中表现稳健。

直觉理解：可以把 Pointer Network 想象成一个"指向"操作。机器人站在路口，面前有若干条可走的路。网络不是从一个固定的列表里选"第 3 条路"，而是直接"指向"具体的邻居节点——无论有 3 个邻居还是 30 个邻居，指向机制都适用。这种设计天然适配图结构中动态变化的邻接关系。

6. 图稀疏化：大规模环境的可扩展性

6.1 为什么需要稀疏化

随着机器人探索范围的扩大，可见性图的节点数量会持续增长。在森林环境中，节点数可以达到 1500 以上。Transformer 的自注意力复杂度是 $O(N^2)$ ——1500 个节点意味着超过 200 万次注意力计算，这在 Jetson Orin NX 这类边缘设备上是不可接受的。CORE 的解决方案是距离相关的图稀疏化：保留机器人附近区域的完整细节，对远处区域进行节点聚合。
$T_{\text{total}} = O(N \log N + N'^2), \quad \text{vs} \quad T_{\text{raw}} = O(N^2)$
其中 $N$ 是原始节点数， $N^{'}$ 是稀疏化后的节点数。经验上稀疏化比率 $\alpha = N'/N \leq 0.5$ ，即 Transformer 编码器中自注意力矩阵的计算量至少减半。在论文报告的森林仿真环境的实际测试中，原始可见性图的节点总数从约 1500 个被有效压缩到 750 个以下，推理时间从潜在的 20ms 压缩到实际的 9ms。

6.2 聚类实现

在代码中，图稀疏化主要通过前沿点聚类函数 cluster_frontiers 和节点管理器的增量过滤逻辑共同实现。前沿点首先被一个基于网格的连通域聚类算法按空间邻近性分组，然后每组内取质心坐标作为该区域的代表节点，显著减少了图中的冗余节点数量：

# utils.py — 基于网格的前沿点聚类
def cluster_frontiers(frontier_set, distance_threshold=2.0,
                      min_points=MIN_CLUSTER_NUM, max_cluster_size=MAX_CLUSTER_NUM):
    grid_size = distance_threshold
    grid_dict = {}
    for point in points:
        grid_x = int(point[0] // grid_size)
        grid_y = int(point[1] // grid_size)
        grid_key = (grid_x, grid_y)
        if grid_key not in grid_dict:
            grid_dict[grid_key] = []
        grid_dict[grid_key].append(point)

    # BFS 连通域扩展
    clusters = []
    processed_grids = set()
    for grid_key in grid_dict:
        if grid_key in processed_grids:
            continue
        cluster_points = []
        queue = deque([grid_key])
        processed_grids.add(grid_key)
        while queue:
            current_grid = queue.popleft()
            cluster_points.extend(grid_dict[current_grid])
            for dx in [-1, 0, 1]:
                for dy in [-1, 0, 1]:
                    neighbor_grid = (current_grid[0] + dx, current_grid[1] + dy)
                    if neighbor_grid in grid_dict and neighbor_grid not in processed_grids:
                        processed_grids.add(neighbor_grid)
                        queue.append(neighbor_grid)

    # 取质心
    centroids = set()
    for cluster in final_clusters:
        if len(cluster) >= min_points:
            arr = np.array(cluster)
            centroids.add(tuple(arr.mean(axis=0)))
    return centroids

这里的聚类阈值 distance_threshold=2.0 对应物理距离 2 x 0.4m = 0.8m。最小簇大小 MIN_CLUSTER_NUM=7 过滤掉孤立的噪声点，最大簇大小 MAX_CLUSTER_NUM=25 防止产生过大的代表区域。超过 25 个点的大簇会被进一步递归切分。这种归一化策略是模型能够跨环境泛化的重要技术保障。

6.3 稀疏化的边界：非稀疏区域

论文中明确提到需要在机器人附近定义一个非稀疏区域来保留局部环境的完整细节，确保近距离的精细导航不受稀疏化影响。在代码中，这个非稀疏区域的大小通过 UPDATING_MAP_SIZE 参数控制，其物理含义和计算方式如下：这意味着该组件在整体架构中扮演着不可替代的角色。

# parameter.py
UPDATING_MAP_SIZE = 2 * SENSOR_RANGE + 4 * NODE_RESOLUTION  # = 2*30 + 4*4 = 76 格 = 30.4m

在这个 30.4m x 30.4m 的局部窗口内，所有可见性图节点都被完整保留；窗口外的节点则可能被聚合。这意味着机器人在做决策时，对近距离环境有像素级精度的感知，对远距离环境则只有拓扑级的粗略理解——这恰好符合导航决策的实际需求。这体现了作者在简洁性和表达力之间找到的实用平衡点。

7. 传感器仿真与信念更新

7.1 光线投射模型

训练环境中的传感器仿真采用了一种极简但有效的实现方式：一个 360 度全向的光线投射模型，以 0.5 度为角度增量发射 720 条射线，每条射线沿 Bresenham 直线栅格化路径前进，直到碰到障碍物像素或达到传感器的最大探测距离为止，具体实现如下：这种分层设计使得各模块可以独立优化而不互相干扰。

# sensor.py — 360 度光线投射传感器
def sensor_work(robot_position, sensor_range, robot_belief, ground_truth):
    sensor_angle_inc = 0.5 / 180 * np.pi  # 0.5 度增量
    sensor_angle = 0
    x0 = robot_position[0]
    y0 = robot_position[1]
    while sensor_angle < 2 * np.pi:
        x1 = x0 + np.cos(sensor_angle) * sensor_range
        y1 = y0 + np.sin(sensor_angle) * sensor_range
        robot_belief = collision_check(x0, y0, x1, y1, ground_truth, robot_belief)
        sensor_angle += sensor_angle_inc
    return robot_belief

每条射线调用 collision_check，它用 Bresenham 算法逐像素推进，将射线经过的每个像素从"未知"（127）更新为地面真值（255=自由或 1=障碍）。当射线碰到障碍物时停止——这模拟了 LiDAR 的遮挡效应。整个传感器仿真发射 720 条射线（360度 / 0.5度），覆盖机器人周围 30 格（12m）范围内的所有可见区域。

工程价值：这种极简的传感器模型是 CORE 能在 5 小时内完成训练的核心原因。相比 Gazebo 中的光线追踪或 Isaac Sim 的物理渲染，基于图像的 Bresenham 投射快了 3-4 个数量级——一次 sensor_work 调用在 CPU 上只需微秒级延迟。而由于 CORE 的策略只看可见性图而不看原始像素，这种简化不影响策略的迁移能力。

7.2 前沿点检测

前沿点（frontier）是已知自由区域与未知区域的边界像素，它标志着探索的最前沿。前沿点的检测使用 OpenCV 的卷积滤波器统计每个自由像素 3x3 邻域中的未知像素数量，只有邻域内未知数处于合理范围的自由像素才被标记为前沿：这个设计在消融实验中被验证为提升性能的关键因素之一。

# utils.py — 前沿点检测
def get_frontier_in_map(map_info):
    unknown = (map_info.map == UNKNOWN).astype(np.uint8)
    kernel = np.ones((3, 3), dtype=np.uint8)
    kernel[1, 1] = 0
    unknown_neighbor = cv2.filter2D(unknown, -1, kernel)
    # 自由像素 + 1 < 未知邻居数 < 8 → 前沿点
    frontier_indices = _find_frontier_indices(map_info.map, unknown_neighbor)

只有未知邻居数在 (1, 8) 之间的自由像素才被认为是前沿点——邻居数为 0 意味着完全在已知区域内部，邻居数为 8 意味着被未知区域包围（不太可能是真正的边界）。检测到的前沿点随后被下采样到 FRONTIER_CELL_SIZE = 0.8m 分辨率，避免产生过多的密集点。这种渐进式的策略在强化学习训练中被广泛证明有效。

在这里插入图片描述

8. 奖励设计：五项信号的平衡

8.1 奖励函数的组成

CORE 的奖励函数由五个独立的分项构成，每一项都针对导航任务的一个特定行为目标进行引导或惩罚，它们经过精心设定的系数加权组合后，为策略梯度优化算法提供了从全局方向引导到局部死锁检测的多维度复合学习信号，其数学形式如下所示：
$\lambda_1 r_{\text{goal}} + \lambda_2 r_{\text{frontier}} + \lambda_3 r_{\text{ds}} + \lambda_4 r_{\text{stay}} + \lambda_5 r_f$
在代码实现中，这五项奖励信号被统一整合在环境类 Env 的 calculate_reward 方法中，由环境在每一步状态转移完成后自动计算各分项数值并加权求和，最终作为标量即时反馈信号返回给智能体用于策略梯度更新。下面我们逐项深入分析各个分项的设计意图、数值量级选择背后的工程考量以及具体的代码实现细节和参数敏感性。