4D毫米波点云预处理

m0_60857098

已于 2024-08-21 15:31:03 修改

阅读量1.3k

点赞数 12

分类专栏： 4D毫米波文章标签： 3d 自动驾驶

于 2024-08-20 15:57:14 首次发布

本文链接：https://blog.youkuaiyun.com/m0_60857098/article/details/141352127

版权

4D毫米波专栏收录该内容

2 篇文章

订阅专栏

3-D Object Detection for Multiframe 4-D Automotive Millimeter-Wave Radar Point Cloud （RadarMFNet）sensors2023

摘要

目标检测是自动驾驶中的一项重要任务。目前，自动驾驶系统的目标检测方法主要基于摄像机和光探测和测距(LiDAR)的信息，这可能会经历复杂照明或恶劣天气的干扰。目前，4-D (x, y, z, v)毫米波雷达可以提供更密集的点云，实现传统毫米波雷达难以完成的三维目标检测任务。现有的三维目标点云检测算法大多基于三维激光雷达;这些方法不一定适用于数据稀疏、噪声较多的毫米波雷达，包括速度信息。本研究提出了一种基于多帧4D毫米波雷达点云的三维目标检测框架。首先，利用毫米波雷达估计自我车辆速度信息，对毫米波雷达点云的相对速度信息进行绝对速度补偿。其次，通过毫米波雷达帧之间的匹配，将多帧毫米波雷达点云与最后一帧进行匹配。最后，利用所提出的多帧毫米波雷达点云检测网络检测目标。实验使用我们新记录的 TJ4DRadSet 数据集在复杂交通环境中执行。结果表明，所提出的目标检测框架优于基于 3-D 平均精度的比较方法。实验结果和方法可作为其他多帧4-D毫米波雷达检测算法的基线。

A. Radar Point-Cloud Velocity Compensation

用毫米波估计自车速度：

①利用高精度定位信息得到自我车辆质心的速度和角速度，然后利用4-D雷达相对于车辆质心的外参矩阵得到4-D雷达处的车辆速度。

②利用4-D雷达点云的信息估计4-D雷达处的自我车辆速度。

radarMFNet提出了一种基于②的方法。

4-D雷达测得的点云径向速度 $v_d^r$ 可表示为点云方向与目标速度 $v_d$ 的点积

R 是目标的径向距离。

如图 5 所示，所有雷达静态对象点的相对速度与，这与自我车辆 vc 在 4D 雷达传感器位置的速度相反。给定一组 N 个测量值，应用上述表达式

方程(2)可以用线性最小二乘法求解

从(3)中，根据雷达静态目标点的信息估计自我车辆ˆvc的速度。当使用所有测量求解上述公式时，需要考虑误差，因为雷达测得的点不是所有静态点;因此，利用随机样本和共识方法[32]求解上述公式。该模型用于拟合实验数据，并且能够平滑包含特定数量错误的数据。在本研究提出的模型中，这些错误是由运动物体引起的，会产生倾斜的速度分布。

在求解自我车辆速度 ^vc 后，可以使用 (1) 计算目标的径向速度补偿 ^vr c，每个目标点 vr a 的速度可补偿为

B. Radar-Point-Cloud Self-Motion Compensation

通过多帧点云匹配对点云进行自我运动补偿，以减少车辆自我运动对网络检测效果的影响。对于雷达的第n帧点云，点集 $P_n^m$ 可以表示为

其中 pm n 是第 m 个点的信息，xm n , ym n , zm n 是第 m 个点的 xyz 坐标信息。

利用(4)得到雷达点云的绝对速度，通过滤波速度阈值得到静态点云。静态点集 Q ln 表示为

其中 qln 是第 l 个点的信息。

对于雷达帧之间的点云匹配，我们需要确定欧氏变换R, t，使得

上述变换矩阵使用迭代最近点 (ICP) 算法 [33] 求解。该算法基于最小二乘法进行迭代计算，使误差的平方和达到最小值

雷达点云转换关系可以表示为

Y nn−1 是第 (n − 1) 帧的点云注册到第 n 帧的点云后的点集。

对于第(n−m)帧转换为第th帧点云后的点集

在多帧雷达检测中，将配准后的第n帧和第m帧输入到第n帧中的网络

从 (12) 和 (13) 中，ICP 算法仅在每一帧迭代中计算一次以获得 H n-1 ，然后从前一帧 I n-1 的点集计算。

目标检测网络

提出的多帧4-D雷达点云检测网络RadarMFNet由三个过程组成:1)多帧点云特征编码和伪图像生成;2)时空骨干网;3)检测头回归。点云编码是提取多帧点云特征的过程，它还根据特定规则将不规则点云输入处理成相同的数据格式。时空骨干网主要用于特征提取，利用三维CNN处理时空伪图像网格数据。检测头模块主要在对象的 3D 框位置、方向和类型上执行回归任务检测。具体网络结构如图6所示。

1)点云到伪图像:目前主要有两种点云编码方法。第一种是基于点的方法，它使用球查询进行点云采样，其代表性网络为PointRCNN[30]。它具有灵活的感受野范围和更好的小物体检测结果。然而，使用这种方法，大规模点云计算效率低下，需要对象具有良好的点云分布特征。第二种是基于体素的方法，它将大量的点云转换为固定大小的网格组合的张量，其代表性网络为VoxelNet[25]和SECOND[26]。实验表明，毫米波雷达点云分布的稀疏性使得使用基于点的编码方法提取目标特征具有挑战性，因为毫米波雷达点的分布包含更多的噪声。因此，基于体素的方法用于对点云进行编码。此外，忽略了高度方向的编码，采用基于柱的编码来减少计算量。我们的方法如图7所示。

首先，将多帧4-D雷达点云每一帧的原始4-D雷达点云裁剪到沿X轴、Y轴和Z轴的R x × R y × R z范围。使用每个雷达点云的原始信息，扩展维数生成增强点li

其中[x, y, z, v, I]分别表示雷达点云、多普勒速度和反射强度信息的原始测量坐标。c 表示每个点云相对于支柱中所有点的算术平均值距离，p 表示从点云到支柱中心偏移的距离。因此，每个增强点都由一个 D = 13 维向量描述。最后，可以为每一帧点云得到一个大小为(D, Pi, N)的密集张量，其中P是非空柱子的数量，N是每个柱子的点数，i表示第i帧。

通过结合 T 帧的点云来获得大小为 (D, P, N) 的张量。然后，对D维中的点进行操作，通过线性层、批处理归一化(BN)层和ReLU层连续得到大小为(C, P, N)的张量。然后，对点云数量维度执行最大池化以获得大小为 (C, P) 的输出张量。编码的特征分散到原来的柱子位置，最后得到一个大小为(C, T, H, W)的伪图像，其中C表示通道数，T表示帧数，H和W分别为伪图像的高度和宽度。

2)骨干:检测阶段的骨干网用于提取时间和空间特征，网络结构如图8所示。

骨干网络主要由两个子网络组成：自上而下的子网络以降低空间分辨率提取相同时间分辨率的特征，另一个网络执行自上而下的特征上采样和连接。

自顶向下子网可以用一系列ST块表示:层中第一个卷积的步幅为2，第一层的结果通过添加1 × 3 × 3和3 × 3 × 3卷积得到，除以2。模块中后续卷积的步幅为1，由L 1 × 3 × 3卷积组成。模块中的每个卷积后面都是 BN 和 ReLU 层。

每个自顶向下模块的输出特征通过时间池化、上采样和连接相结合。首先，4-D 张量 (C, T, H, W ) 通过时间维度上的最大池化转换为 3D 张量 (C, H, W )。然后使用二维转置卷积对初始步幅Sin和最终步幅Sout的特征图进行上采样。然后，通过 BN 和 ReLU 层对特征进行顺序上采样；最终输出结果是来自不同空间分辨率的特征的串联结果。由于 3-D 卷积用于自上而下的模块，因此可以提取目标的时空特征。

补充

最小二乘法解矩阵方程 pyhon

在Python中，可以使用NumPy库来解一个线性方程组。NumPy的numpy.linalg模块提供了lstsq函数，该函数可以计算最小二乘解。以下是一个使用lstsq函数解线性方程组的例子：

import numpy as np
 
# 假设A是一个m x n矩阵，b是一个m x 1矩阵
A = np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]])
b = np.array([10, 20, 30])
 
# 使用numpy.linalg.lstsq计算最小二乘解
x, res, rank, s = np.linalg.lstsq(A, b, rcond=None)
 
# x是方程的解向量
print(x)

np.linalg.lstsq()的输入包括三个参数，A为自变量X，b为因变量Y，rcond用来处理回归中的异常值，一般不用。

lstsq的输出包括四部分：回归系数、残差平方和、自变量X的秩、X的奇异值。一般只需要回归系数就可以了。

迭代最近点 (ICP) 算法

ICP（迭代最近点）算法‌是一种迭代计算方法，主要用于计算机视觉中深度图像的精确拼合，通过不断迭代最小化源数据与目标数据对应点来实现精确地拼合。ICP算法通过寻找两组点云之间的最近点，通过迭代的方式逐步调整点云的位置和姿态，使得两组点云之间的误差不断减小，最终达到精确配准的目的。这种算法在计算机视觉、机器人技术和3D建模等领域有广泛应用，尤其是在需要精确对齐或拼接3D点云数据时‌。

ICP算法的基本步骤包括：

‌点云匹配‌：在源曲面和目标曲面上找到对应的点。
‌最近点搜索‌：通过计算每个点与另一个点云中所有点的距离，找到最近的点作为对应点。
‌位姿估计‌：根据找到的对应点，估计两组点云之间的相对位姿变换（旋转和平移）。
‌迭代优化‌：根据估计的位姿对点云进行调整，然后重复上述步骤，直到满足一定的收敛条件或达到最大迭代次数。

import numpy as np

def icp(source_points, target_points, max_iterations=50, tolerance=1e-5):
    """
    Iterative Closest Point (ICP) algorithm implementation.
    
    Args:
        source_points (np.array): Source points (n x 3).
        target_points (np.array): Target points (n x 3).
        max_iterations (int): Maximum number of iterations.
        tolerance (float): Convergence threshold.
    
    Returns:
        R (np.array): Rotation matrix.
        T (np.array): Translation vector.
    """
    R = np.eye(3)
    T = np.zeros(3)
    
    for _ in range(max_iterations):
        # Find the nearest neighbors
        distances, indices = find_nearest_neighbors(source_points, target_points)
        
        # Compute the transformation
        R_new, T_new = compute_transformation(source_points, target_points, indices)
        
        # Update the transformation
        source_points = np.dot(source_points, R_new.T) + T_new
        
        # Check for convergence
        if np.abs(np.sum(R_new - R)) < tolerance and np.abs(np.sum(T_new - T)) < tolerance:
            break
        
        R = R_new
        T = T_new
    
    return R, T

def find_nearest_neighbors(source_points, target_points):
    # Find the nearest neighbors using a distance metric
    distances = np.linalg.norm(source_points[:, np.newaxis] - target_points, axis=-1)
    indices = np.argmin(distances, axis=1)
    
    return distances, indices

def compute_transformation(source_points, target_points, indices):
    # Compute the transformation between source and target points
    T = np.mean(target_points[indices] - source_points, axis=0)
    
    source_centered = source_points - np.mean(source_points, axis=0)
    target_centered = target_points[indices] - np.mean(target_points[indices], axis=0)
    
    W = np.dot(source_centered.T, target_centered)
    U, _, VT = np.linalg.svd(W)
    
    R = np.dot(U, VT)
    
    return R, T

# 示例用法
source_points = np.random.rand(100, 3)
R, T = icp(source_points, source_points + 0.1 * np.random.randn(100, 3))

print("Rotation matrix:")
print(R)
print("\nTranslation vector:")
print(T)

这段代码实现了ICP算法的核心步骤，包括计算最近邻点，平均点位置、移除平均位置、计算协方差矩阵、旋转矩阵和平移向量。最终，它返回了两帧点云的准确配准变换矩阵T。