这个项目有两种路径来源：没有路网时，用点云建 Costmap，再跑全局 A*；有路网时，直接拿着路网边的中心线走。两者看起来都是“给一条折线”，但计算路径的位置完全不同。

Costmap：把点云变成可搜索的栅格

点到点导航的目标是地图坐标。要跑 A*，必须先把点云转换成 2D 占据栅格。navigate 在执行线程里按需调用建图逻辑。

关键不是简单地把点投影到网格，而是处理几个工程问题。

首先是密度阈值。RSAIRY 是稀疏固态雷达，单点噪声多。只有某个 cell 内的点数达到阈值，才认为它是障碍。否则一两个飞点就会把地图点得到处是墙。

其次是障碍膨胀。机器人不是质点，有外接半径。障碍 cell 会按机体半径向外膨胀，让路径保持安全距离。

然后是轨迹雕刻。狗走过的 trajectory 是真实可通行路径，即使它看起来离障碍很近，也应该被强制保留为可走。没有这一步，窄走廊会先被膨胀封死，狗明明走过的地方反而规划不出来了。

以前会对整张图建栅格。户外 1km 点云会生成八千多万个 cell，一次就把主机拖垮。现在会先根据“当前位置和目标点”开一个包围盒，再只解析这个范围内的点云分片。这样规划只需要一块局部区域：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
# 栅格按点云全 bbox 开,户外云图跨 1083×791m,@0.1m = 8610 万格
# 窗内即使没有点也要出格子:户外图稀疏,A* 要能在没有点的区域规划通过
MAX_CELLS = 6_000_000
WINDOW_R = 120.0

if bounds is not None:
    xmin, ymin, xmax, ymax = (float(bounds[0]) - margin, float(bounds[1]) - margin,
                              float(bounds[2]) + margin, float(bounds[3]) + margin)
    if len(pts):
        m = ((pts[:, 0] >= xmin) & (pts[:, 0] <= xmax) &
             (pts[:, 1] >= ymin) & (pts[:, 1] <= ymax))
        pts = pts[m]          # 窗外的点不参与

# 超上限就粗化(0.1→0.15→0.2…),保证任何图都能出图、不 OOM
while max_cells > 0 and ((xmax - xmin) / res + 1) * ((ymax - ymin) / res + 1) > max_cells:
    res *= 1.5

全局 A*：8 连通加软惩罚

A* 是纯 Python 和 NumPy 实现，不依赖 ROS。搜索使用 8 连通邻域：

1
2
3
_N8 = [(-1, 0, 1.0), (1, 0, 1.0), (0, -1, 1.0), (0, 1, 1.0),
       (-1, -1, 1.41421356), (-1, 1, 1.41421356),
       (1, -1, 1.41421356), (1, 1, 1.41421356)]

对角线移动代价是 1.414，四方向移动代价是 1.0。这样可以避免路径出现“一步走两格”的奇怪斜切。

除了距离代价，代码还叠加了渐变代价惩罚。Costmap 除了二值占据，还会维护一个渐变 cost 场，表示离墙有多近。A* 计算 step 时：

1
2
3
step = c
if cost is not None:
    step += COST_WEIGHT * (float(cost[nj, ni]) / 99.0)

效果是优先走通道中间，而不是贴着墙。窄道仍然能过，但会被罚分。这个权重是调出来的：太大时狗为了躲墙绕远路，太小时又起不到走中间的效果。

对角线还有一个防角切保护。当对角方向的两个正交邻居都被占据时，不允许斜着挤过去，避免路径从墙角削过去。

视线串拉：让折线更接近人规划的路线

A* 输出的是一串相邻 cell，直接用来跟踪会很碎。项目做了一步“String pulling”，保留真正需要转弯的顶点。

简化从起点开始，尽量找到视线最远的后续点。视线判断用 Bresenham：线段经过 lethal cell，或经过高 cost 的贴墙格，就不可直连。

这里的高 cost 格也当成阻挡。原因是如果不检查高 cost，直线简化会把路径抄近道拉进贴墙区，抵消掉前面 A* 的“走中间”偏好。

简化后的路径是稀疏拐点，而不是密集 cell。控制层沿着这些拐点走，转弯次数明显减少。

路网模式：路径已经存在，不需要再规划

当上层已经按路网算好折线时，navigate 不会再去建 Costmap 和跑 A*。调用参数里直接给 path：

1
{"path": [[1.0, 2.0], [1.5, 2.3], [2.0, 2.8]]}

这个分支有几个原因。一是省掉读点云、建栅格和搜索的成本。二是路网本身就是路径，再去 A* 反而可能因为户外 costmap 为空，把狗从路上拉直线穿出去。

更重要的是，A* 模式依赖真实位姿。取景窗、栅格和全局路径都建立在“我知道自己在哪”之上。如果当前处于推算态，绝对定位不可用，那么算出来的全局路径本身就是错的。路网模式不建栅格，只需要知道狗在这条边的哪一段，所以在推算态下仍然能开工，只是速度和避障会更保守。

动态障碍：从局部重规划到可控停避障

早期方案是实时点云发现 novel 障碍后，把它标进 costmap，然后触发局部重规划。这听起来完整，但真实环境里容易把墙误判成动态障碍，越规划越乱。

后来 navigate 把避障简化成更可控的策略：优先消费 freespace 感知 skill 给出的前方障碍距离、负障和扇区距离。远则全速，近则减速，太近则停。

如果 freespace 契约暂时断开，本地会用 live 点云做一次走廊检测作为回落，但会降速。长时间断开就直接停下，因为回落计算更贵，而且看不到负障。

也就是说，路径规划负责“宏观怎么走”，实时避障负责“这一拍能不能继续走”。两者通过同一套折线衔接，A* 决定大方向，控制器和安全判据决定每一步的实际速度。

关键代码示例

Costmap 不是“把点投到格子”那么简单

先按地面高度取一个高度带，再用点数阈值过滤 RSAIRY 的稀疏噪声：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
FLOOR_PCTL = 2.0      # z percentile taken as the floor

floor = float(np.percentile(pts[:, 2], FLOOR_PCTL)) if len(pts) else 0.0

band = (pts[:, 2] > floor + band_lo) & (pts[:, 2] < floor + band_hi)
p = pts[band]

cnt = np.zeros((H, W), np.int32)
if len(p):
    gx = ((p[:, 0] - xmin) / res).astype(np.int64)
    gy = ((p[:, 1] - ymin) / res).astype(np.int64)
    m = (gx >= 0) & (gx < W) & (gy >= 0) & (gy < H)
    np.add.at(cnt, (gy[m], gx[m]), 1)

occ = (cnt >= min_pts).astype(bool)
occ = _remove_small_blobs(occ, MIN_BLOB_CELLS)   # 抹建图动态点(小孤立团),墙不动 → 少假弯

一两个飞点不应该把一个 cell 点成墙。墙在同一 XY 上会叠很多垂直点，稀疏噪声通常只有 1 个，所以 min_pts=3 是这套固态雷达最重要的鲁棒性旋钮。

建图时人、临时物会散在可走区里，不连大墙。去小连通域用 8 连通 union-find 抹掉这些小团，避免 A* 绕出一堆假弯；它只清理静态规划图，live 点云仍然兜底真障：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
def _remove_small_blobs(occ: np.ndarray, min_cells: int) -> np.ndarray:
    """抹掉小的孤立占据连通域(< min_cells)。
    建图时的动态点(人/临时物)散在可走区、不连大墙,A* 得绕它们=假弯。
    墙是大连通域(远超阈值)不动;贴墙家具连着墙也不动。
    安全:只清静态规划图,实时 live 感知照常兜底真障。"""
    if min_cells <= 1:
        return occ
    H, W = occ.shape
    ys, xs = np.nonzero(occ)
    n = len(ys)
    if n == 0:
        return occ
    idx = -np.ones((H, W), np.int64)
    idx[ys, xs] = np.arange(n)
    parent = np.arange(n)

    def find(a):
        root = a
        while parent[root] != root:
            root = parent[root]
        while parent[a] != root:      # path compression
            parent[a], a = root, parent[a]
        return root

    def union(a, b):
        ra, rb = find(int(a)), find(int(b))
        if ra != rb:
            parent[max(ra, rb)] = min(ra, rb)

    # 8-连通:右、下、右下、左下
    pairs = []
    m = occ[:, :-1] & occ[:, 1:]
    pairs.append((idx[:, :-1][m], idx[:, 1:][m]))
    m = occ[:-1, :] & occ[1:, :]
    pairs.append((idx[:-1, :][m], idx[1:, :][m]))
    m = occ[:-1, :-1] & occ[1:, 1:]
    pairs.append((idx[:-1, :-1][m], idx[1:, 1:][m]))
    m = occ[:-1, 1:] & occ[1:, :-1]
    pairs.append((idx[:-1, 1:][m], idx[1:, :-1][m]))
    for aa, bb in pairs:
        for a, b in zip(aa.tolist(), bb.tolist()):
            union(a, b)
    roots = np.array([find(k) for k in range(n)])
    sizes = np.bincount(roots)
    small = sizes[roots] < min_cells
    out = occ.copy()
    out[ys[small], xs[small]] = False
    return out

轨迹雕刻：狗走过的窄道必须重新变成可通行

否则膨胀 + 噪声会把真实走过的走廊封死：

1
2
3
4
5
6
7
8
9
10
if traj_xy is not None and len(traj_xy):
    t = np.asarray(traj_xy, dtype=np.float32).reshape(-1, 2)
    tube = np.zeros((H, W), bool)
    tgx = ((t[:, 0] - xmin) / res).astype(np.int64)
    tgy = ((t[:, 1] - ymin) / res).astype(np.int64)
    mm = (tgx >= 0) & (tgx < W) & (tgy >= 0) & (tgy < H)
    tube[tgy[mm], tgx[mm]] = True
    tube = _dilate(tube, int(round(carve_r / res)))
    grid[tube] = 0
    cost[tube] = 0        # 走过的道:硬挡+代价都清零(已证实可通,首选)

A* 的 8 连通、软惩罚和防角切

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
COST_WEIGHT = 1.0     # 每单位归一化 cost(0..1)加到步代价
LOS_COST_MAX = 40     # 直线简化只穿 cost ≤ 此的格

for di, dj, c in _N8:
    ni, nj = ci + di, cj + dj
    if ni < 0 or ni >= W or nj < 0 or nj >= H or grid[nj, ni]:
        continue

    # prevent diagonal corner-cutting through a blocked orthogonal neighbour
    if di and dj and (grid[cj, ni] or grid[nj, ci]):
        continue

    step = c
    if cost is not None:
        step += COST_WEIGHT * (float(cost[nj, ni]) / 99.0)

    ng = gc + step
    if ng < gsc.get((ni, nj), 1e18):
        gsc[(ni, nj)] = ng
        came[(ni, nj)] = cur
        heapq.heappush(openh, (ng + h((ni, nj)), ng, (ni, nj)))

grid[nj, ni] 是硬墙，cost[nj, ni] 是“离墙有多近”的渐变代价。软惩罚让路径优先走通道中间，但窄道仍然能过。

直线简化不能只看 lethal

视线判断会把高 cost 格也当阻挡：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
def _los(cm, a, b) -> bool:
    """Bresenham: True if the segment stays clear (no lethal, no high-cost).
    高代价(贴墙)格也当"挡"——否则简化会把路径抄近道拉进贴墙区。"""
    cost = getattr(cm, "cost", None)
    (x0, y0), (x1, y1) = a, b
    dx, dy = abs(x1 - x0), abs(y1 - y0)
    sx = 1 if x0 < x1 else -1
    sy = 1 if y0 < y1 else -1
    err = dx - dy
    x, y = x0, y0
    while True:
        if cm.grid[y, x] or (cost is not None and cost[y, x] > LOS_COST_MAX):
            return False
        if x == x1 and y == y1:
            return True
        e2 = 2 * err
        if e2 > -dy:
            err -= dy
            x += sx
        if e2 < dx:
            err += dx
            y += sy

如果只挡 lethal，A* 好不容易选出的“走中间”路径，会在简化阶段被直线抄近道拉回贴墙区。

小结

路径规划在这个项目里不是单一算法，而是按场景分成两条路：

  • 自由目标点：点云分片、Costmap、8 连通 A*、视线串拉。
  • 路网导航：直接走已经规划好的边中心线，不做二次全局搜索。
  • 动态环境：不依赖频繁全局重规划，而用感知契约做停和减速。

这套设计最值得借鉴的是“不要把规划做得过度聪明”。能沿路走的时候，就不要重新搜索；能停下来的时候，就不要勉强绕一个看不清的障碍。