4. 多路径干扰消除策略

任务定义

在ToF（飞行时间）深度相机中，多路径干扰（Multi-Path Interference, MPI）是指发射光经多条不同路径反射后到达同一像素，导致深度测量值偏离真实距离的系统性误差。本案例要求实现一种基于连续波调制的多路径干扰消除算法：给定像素接收到的多个频率下的相位测量值，估计并分离出直达路径与二次反射路径的贡献，输出校正后的深度值。

问题建模

对于连续波ToF，每个调制频率 f 下测得的相位 φ(f) 与光飞行距离 d 的关系为：φ = 4πfd/c（模2π）。当存在两条反射路径时，接收信号为两路径信号的叠加，等效相位为：

φ_meas(f) = atan2( A1·sin(4πf·d1/c) + A2·sin(4πf·d2/c), A1·cos(4πf·d1/c) + A2·cos(4πf·d2/c) )

其中 d1 为直达路径距离，d2 为二次反射路径距离，A1、A2 为对应幅度。目标是从多个频率的 φ_meas 中解出 d1（真实深度）。

实现方案

采用非线性最小二乘拟合：使用至少4个不同调制频率的相位测量值，通过Levenberg-Marquardt算法迭代优化参数 (d1, d2, A1, A2)，使模型预测相位与实测相位残差最小。初始化时以最低频率的相位直接换算距离作为 d1 初值，d2 设为 2×d1（常见墙角二次反射路径近似），A1:A2 初始为 3:1。

import numpy as np
from scipy.optimize import least_squares

C = 299792458.0  # 光速 m/s

def phase_model(params, freqs):
    """计算给定参数下各频率的预测相位（弧度）"""
    d1, d2, A1, A2 = params
    phi1 = 4 * np.pi * freqs * d1 / C
    phi2 = 4 * np.pi * freqs * d2 / C
    # 叠加后取幅角
    real = A1 * np.cos(phi1) + A2 * np.cos(phi2)
    imag = A1 * np.sin(phi1) + A2 * np.sin(phi2)
    return np.arctan2(imag, real)

def residual(params, freqs, meas_phase):
    """计算预测与实测相位差，处理2π环绕"""
    pred = phase_model(params, freqs)
    diff = pred - meas_phase
    # 将差值映射到 [-π, π]
    return (diff + np.pi) % (2 * np.pi) - np.pi

def mpi_correct(freqs, meas_phase):
    """
    多路径干扰消除主函数
    参数:
        freqs: 调制频率数组 (Hz)，至少4个不同频率
        meas_phase: 对应各频率的实测相位 (弧度)
    返回:
        d1_est: 校正后的直达距离 (m)
        d2_est: 二次反射路径距离 (m)
        success: 是否收敛
    """
    # 初值：最低频相位直接换算距离
    f_min_idx = np.argmin(freqs)
    d1_init = meas_phase[f_min_idx] * C / (4 * np.pi * freqs[f_min_idx])
    if d1_init < 0:
        d1_init += C / (2 * freqs[f_min_idx])  # 相位模糊修正
    d2_init = 2.0 * d1_init
    A1_init, A2_init = 0.75, 0.25
    p0 = np.array([d1_init, d2_init, A1_init, A2_init])

    # 边界约束：距离非负，幅度非负且A1>A2
    lower = [0, 0, 0, 0]
    upper = [10.0, 20.0, 1.0, 1.0]

    result = least_squares(residual, p0, args=(freqs, meas_phase),
                           bounds=(lower, upper), method='trf', max_nfev=200)
    d1_est, d2_est = result.x[0], result.x[1]
    success = result.success
    return d1_est, d2_est, success

# ===== 示例输入：模拟真实场景 =====
# 真实参数：直达距离 1.20m，二次反射路径 2.50m，幅度比 0.7:0.3
true_d1, true_d2, true_A1, true_A2 = 1.20, 2.50, 0.7, 0.3
freqs = np.array([20e6, 25e6, 30e6, 35e6])  # 4个调制频率

# 生成无噪声的"实测"相位（用于验证算法正确性）
np.random.seed(42)
phi_true = phase_model([true_d1, true_d2, true_A1, true_A2], freqs)
# 添加少量高斯噪声模拟真实测量
meas_phase = phi_true + np.random.normal(0, 0.02, size=len(freqs))

# 执行多路径消除
d1_corrected, d2_est, ok = mpi_correct(freqs, meas_phase)

# 对比：未校正（直接用最低频相位换算）
d1_uncorrected = meas_phase[np.argmin(freqs)] * C / (4 * np.pi * np.min(freqs))
if d1_uncorrected < 0:
    d1_uncorrected += C / (2 * np.min(freqs))

print(f"真实直达距离: {true_d1:.4f} m")
print(f"未校正估计:   {d1_uncorrected:.4f} m (误差 {abs(d1_uncorrected-true_d1)*100:.1f} cm)")
print(f"校正后估计:   {d1_corrected:.4f} m (误差 {abs(d1_corrected-true_d1)*100:.1f} cm)")
print(f"二次路径估计: {d2_est:.4f} m (真实 {true_d2:.4f} m)")
print(f"收敛状态: {ok}")

落地与验证

依赖安装：需要 Python 3.8+ 及 NumPy、SciPy 库，执行 pip install numpy scipy。

运行方式：将上述代码保存为 mpi_correction.py，在终端执行 python mpi_correction.py。

验证方法：观察输出中“校正后估计”与“真实直达距离”的误差。由于代码中已设置随机种子，每次运行结果一致。判定标准：校正后误差应显著小于未校正误差（本示例中未校正误差通常大于10cm，校正后应小于1cm）。若需验证算法对不同距离的适应性，可修改 true_d1 值（如0.5~5.0m范围内）并重新运行，观察校正误差是否保持在厘米级以内。

关键说明

本实现针对双路径干扰模型（一个直达路径加一个二次反射路径），这是室内场景中最常见的多路径情形。算法要求至少4个调制频率以保证参数可辨识性。实际部署时，若场景存在三条及以上显著路径，需扩展模型参数数量并增加频率数。代码中相位噪声标准差设为0.02弧度（约1.1°），对应典型ToF传感器在中等光照下的噪声水平。若传感器噪声更大，建议增加频率数量或采用鲁棒估计方法。