甘蔗田里的自主探索——通道感知的前沿探索算法

通用的前沿探索算法不管用——至少在甘蔗田里不管用。

甘蔗田的结构是平行的狭长通道，行距大约 1.5 米，行长约 100 米。通用算法选前沿点只看距离和簇大小，结果车走到一条通道尽头，转 180° 回头，走回起点，再进下一条——效率低到令人发指。更糟的是，它会在两条通道的交叉口反复选点，来回兜圈。

后来重写了探索算法，加了通道方向连续性和通道尽头检测。效果立竿见影：车走到通道尽头自然转入相邻通道，像蛇形走位一样逐行扫描，效率翻了三倍。

前沿提取：OpenCV 一把梭

前沿的定义：free 栅格（值=0）且相邻有 unknown 栅格（值=-1）。提取方法很暴力：

1
2
3

unknown_u8 = unknown.astype(np.uint8)
adj_unknown = cv2.dilate(unknown_u8, np.ones((3, 3), np.uint8), iterations=1).astype(bool)
frontier_mask = free & adj_unknown

膨胀一次 unknown，跟 free 做 AND——相邻有 unknown 的 free 格子就是前沿。然后用 cv2.connectedComponents 提取连通簇，每个簇就是一个候选探索区域。

BFS 可达性：别选到墙后面的前沿

前沿点可能在墙的另一面——看起来近，实际上到不了。通用算法不管这个，直接选最近的前沿，然后 Nav2 规划失败，重试，又失败。

解法：从机器人位置做 BFS，计算到所有 free 栅格的距离。只选可达的前沿点。

def _bfs_distance(self, free, start_x, start_y):
    dist = np.full((h, w), -1, dtype=np.int32)
    queue = deque()
    dist[start_y, start_x] = 0
    queue.append((start_x, start_y))

    while queue:
        cx, cy = queue.popleft()
        cd = dist[cy, cx]
        for dx, dy in ((-1, 0), (1, 0), (0, -1), (0, 1)):
            nx, ny = cx + dx, cy + dy
            if 0 <= nx < w and 0 <= ny < h and free[ny, nx] and dist[ny, nx] < 0:
                dist[ny, nx] = cd + 1
                queue.append((nx, ny))
    return dist

4 邻域 BFS，O(W×H) 时间。地图一般 200×200，毫秒级完成。

有个细节：如果机器人当前位置不在 free 上（比如刚好在 unknown 区域），算法会搜索周围 20 格内最近的 free 格子作为 BFS 起点。这在建图初期很常见——车刚启动，周围大部分都是 unknown。

通道方向连续性：顺着走，别回头

这是最关键的创新。评分公式：

1
2
3

score = reachable_dist
      + 0.4 * heading_penalty * reachable_dist   # 方向偏差惩罚
      - 0.03 * cluster_size                       # 大簇奖励

heading_penalty = |angle_to_goal - robot_yaw| / π，范围 0~1。正前方是 0，正后方是 1。

0.4 的权重意味着：正前方 10 米远的前沿和正后方 6 米远的前沿得分相同。车会优先往前走，而不是回头走已经走过的路。

大簇奖励 -0.03 × size 是因为：大前沿簇通常是未探索通道的入口，小簇可能是噪声或通道边缘的零碎前沿。给大簇加分，引导车进入新通道。

通道尽头检测：前方没路了，转隔壁

光有方向连续性还不够。车走到通道尽头，前方没有前沿了，但左右两侧可能有——那是相邻通道。如果方向惩罚太重，车会一直回头，永远不转入相邻通道。

解法：检测前方 ±45° 扇区内是否有前沿。如果没有，降低侧面前沿的方向惩罚：

has_forward = any(
    abs(angle_diff) < π/4 for _, angle_diff in candidates
)

for goal, angle_diff in candidates:
    heading_penalty = angle_diff / π
    if not has_forward:
        heading_penalty *= 0.3  # 通道尽头，大幅降低方向惩罚
    score = (
        world_dist
        + 0.4 * heading_penalty * world_dist
        - 0.03 * goal.size
    )

heading_penalty *= 0.3 把 90° 侧向的惩罚从 0.5 降到 0.15。侧面前沿的得分一下子就比回头的前沿高了——车自然转入相邻通道。

效果：车走到通道尽头，不回头，直接右转进下一条通道，像犁田一样逐行扫描。

黑名单：别再去失败的地方

Nav2 有时候规划失败——前沿点看起来可达，但路径上有窄缝过不去。不加处理的话，探索算法下一轮又选同一个点，又失败，死循环。

def add_failed_goal(self, x, y):
    self._failed_goals.append((x, y))
    if len(self._failed_goals) > 100:
        self._failed_goals = self._failed_goals[-100:]

def _is_blacklisted(self, point):
    for fg in self._failed_goals:
        if distance(point, fg) < 0.5:  # 0.5 米半径内
            return True
    return False

失败目标 0.5 米半径内的新候选全部跳过。最多记 100 个，防止内存泄漏。

黑名单没有过期时间——因为如果这个点真的到不了，永远不该再去；如果后来地图更新了，通道变宽了，BFS 可达性会自然找到新的前沿点。

地图边界过滤

前沿点如果紧贴地图边缘，Nav2 的 worldToMap 容易越界抖动。过滤掉距离边缘 1 格以内的候选：

margin = 1
if goal_mx <= margin or goal_mx >= (w - 1 - margin) or \
   goal_my <= margin or goal_my >= (h - 1 - margin):
    continue

跟通用算法比

	通用前沿探索	通道感知探索
选点策略	最近距离优先	方向连续 + 通道尽头自适应
通道尽头	掉头回头	自然转入相邻通道
不可达前沿	反复尝试直到超时	BFS 预过滤 + 黑名单
100m 通道扫描	~3 次回头	0 次回头，蛇形扫完
兜圈概率	高（交叉口反复选点）	低（方向惩罚抑制回头）

甘蔗田里跑下来，同样 2000 平方米的区域，通用算法要 25 分钟，通道感知版 8 分钟。差距主要来自消除了回头和兜圈。