2. 巷道退化环境下激光里程计特征提取与匹配

为什么长直巷道会让激光里程计“失明”

在长直巷道中，激光雷达扫到的点云高度重复：两侧墙面近似平行平面，顶板近似水平平面，管道近似圆柱面。沿巷道轴向（设为 x 轴）平移时，墙面与顶板的几何形状几乎不变，点到面残差对 x 位移不敏感，导致沿轴向的约束极弱。此时若把全部点直接送入 ICP，求解器会在退化方向上产生大幅漂移。工程上的对策是：先提取结构化特征（墙面、顶板、管道轴线），再按特征类型分配约束，并对退化方向做显式抑制。

特征提取：三类结构化基元

设雷达坐标系为右手系，x 向前、y 向左、z 向上，地面分割后保留侧壁与顶板点。对每一帧做如下处理：

• 墙面提取：对点云按方位角分扇区，每个扇区用 RANSAC 拟合竖直平面（法向近似水平），保留内点数 ≥ 30、平面度残差 RMS ≤ 0.03 m 的平面，记录法向 n 与距离 d。
• 顶板提取：筛选 z 高于地面 1.8 m 以上的点，拟合近水平平面（法向与 z 轴夹角 < 10°），得到顶板高度 h。
• 管道提取：对剩余点做欧氏聚类，对每个簇在横截面（y-z 平面）内拟合圆，半径 0.05–0.4 m、圆度残差 ≤ 0.02 m 的簇判定为管道，输出圆心 (y₀,z₀) 与轴向（近似 x 向）。

三类特征分别提供不同方向的约束：墙面法向约束横向 y 与偏航，顶板约束 z 与俯仰/横滚，管道轴线约束横向位置并可作为长直巷道中少有的“纵向可分辨”参照（管道接头、支架间距）。

帧间关联与位姿初值估计

关联采用“类型内匹配 + 类型间校验”的策略，避免把墙面误配到管道：

1. 墙面匹配：以法向夹角 < 8°、距离差 < 0.15 m 为门限，用匈牙利算法做一对一匹配，得到 2 组平面约束。
2. 顶板匹配：直接比较高度差，< 0.05 m 视为同一顶板。
3. 管道匹配：比较横截面圆心距 < 0.1 m 且半径差 < 0.03 m；若管道带支架，用支架沿 x 的间距序列做一维互相关，得到轴向平移初值 Δx。
4. 初值求解：先用顶板与墙面约束解出 (y, z, roll, pitch, yaw)，再用管道/支架的 Δx 补上 x，得到 6 自由度初值 T₀，送入点云配准做精化。

下面给出可运行的特征提取与初值估计核心代码（Python + NumPy，输入为已去噪、已分割的点云数组，形状 N×3）：

import numpy as np

def fit_plane_ransac(pts, thresh=0.03, iters=200, rng=None):
    rng = rng or np.random.default_rng(0)
    best_inl, best_model = None, None
    for _ in range(iters):
        idx = rng.choice(len(pts), 3, replace=False)
        p0, p1, p2 = pts[idx]
        n = np.cross(p1 - p0, p2 - p0)
        norm = np.linalg.norm(n)
        if norm < 1e-6:
            continue
        n /= norm
        d = -n @ p0
        dist = np.abs(pts @ n + d)
        inl = dist < thresh
        if best_inl is None or inl.sum() > best_inl.sum():
            best_inl, best_model = inl, (n, d)
    return best_model, best_inl

def extract_features(pts, z_ground=0.0):
    feats = {"walls": [], "ceiling": None, "pipes": []}
    # 顶板：z 高于地面 1.8 m
    high = pts[pts[:, 2] > z_ground + 1.8]
    if len(high) > 50:
        (n, d), inl = fit_plane_ransac(high, thresh=0.04)
        if abs(n[2]) > 0.98 and inl.sum() >= 50:
            feats["ceiling"] = {"n": n, "d": d, "h": -d}
    # 墙面：按方位角分 8 扇区，拟合竖直平面
    ang = np.arctan2(pts[:, 1], pts[:, 0])
    for k in range(8):
        m = (ang >= -np.pi + k * np.pi / 4) & (ang < -np.pi + (k + 1) * np.pi / 4)
        seg = pts[m]
        if len(seg) < 40:
            continue
        (n, d), inl = fit_plane_ransac(seg, thresh=0.03)
        if abs(n[2]) < 0.15 and inl.sum() >= 30:
            feats["walls"].append({"n": n, "d": d, "count": int(inl.sum())})
    # 管道：横截面圆拟合（简化：用 y-z 投影的均值与半径估计）
    rest = pts[pts[:, 2] < z_ground + 1.8]
    if len(rest) > 30:
        yz = rest[:, 1:3]
        c = yz.mean(axis=0)
        r = np.linalg.norm(yz - c, axis=1).mean()
        if 0.05 < r < 0.4:
            feats["pipes"].append({"center": c, "r": float(r)})
    return feats

def match_and_init(fa, fb):
    # 墙面匹配：法向夹角 + 距离差
    pairs = []
    for i, wa in enumerate(fa["walls"]):
        for j, wb in enumerate(fb["walls"]):
            cosang = abs(wa["n"] @ wb["n"])
            if cosang > np.cos(np.deg2rad(8)) and abs(wa["d"] - wb["d"]) < 0.15:
                pairs.append((i, j))
    # 顶板高度差
    dz = 0.0
    if fa["ceiling"] and fb["ceiling"]:
        dz = fb["ceiling"]["h"] - fa["ceiling"]["h"]
    # 管道圆心差 → 横向初值
    dy = 0.0
    if fa["pipes"] and fb["pipes"]:
        dy = fb["pipes"][0]["center"][0] - fa["pipes"][0]["center"][0]
    # 轴向初值：无支架时置 0，交由配准精化
    dx = 0.0
    T0 = np.eye(4)
    T0[0, 3], T0[1, 3], T0[2, 3] = dx, dy, dz
    return T0, len(pairs)

# 示例：两帧各 2000 点（教学假设数据）
rng = np.random.default_rng(1)
frame_a = rng.normal(size=(2000, 3)) * [8, 2, 1.5]
frame_b = frame_a + [0.3, 0.02, 0.01]  # 假设沿 x 平移 0.3 m
fa = extract_features(frame_a)
fb = extract_features(frame_b)
T0, n_pairs = match_and_init(fa, fb)
print("墙面匹配对数:", n_pairs, "初值平移:", T0[:3, 3])

上述示例数据为教学假设，输出仅用于说明流程：墙面匹配对数反映横向约束是否充足，初值平移中的 x 分量在无支架时默认为 0，必须依赖后续配准或支架间距序列补齐。

特征数量对漂移的影响分析

为量化“特征稀疏”带来的影响，可在同一段巷道数据上做消融：分别只用墙面、墙面+顶板、墙面+顶板+管道，统计每帧有效约束数与 100 帧累计漂移。下表为教学示例的预期趋势（非实测）：