CN116978009A - 基于4d毫米波雷达的动态物体滤除方法 - Google Patents

Abstract

本发明公开了一种基于4D毫米波雷达的动态物体滤除方法，包括以下步骤：步骤一、通过手动/自动触发自动在线激光雷达‑相机与激光雷达‑4D毫米波外参校准模块，实现多传感器在空间上以及时间上的同步，对数据进行预处理；步骤二、运动物体识别与分割机制根据环境工况选择图像分割网络实现或者点云语义分割网络，对目标进行识别分割，并对物体的运动属性与特征初步推断；步骤三、基于4D毫米波雷达的运动特征检测与判别机制；步骤四、外点判断与去除模块通过设定动态外点的判断与去除策略，对图像中的运动点进行去除，消除因运动物体引入的不稳定的数据关联。本发明方法，提升SLAM系统在动态环境下的鲁棒性，使系统在动态环境下具有较好的性能。

Description

基于4D毫米波雷达的动态物体滤除方法

技术领域

本发明涉及即时定位与地图创建技术领域，特别涉及一种基于4D毫米波雷达的动态物体滤除方法。

背景技术

即时定位与地图创建(Simultaneous Localization and Mapping，SLAM)所关注的问题是载有传感器的移动机器人在未知的环境中定位并构建环境地图，是移动机器人感知自身状态和外部环境的关键技术。良好地图与感知系统的融合对于提高无人驾驶车辆在城市环境中的安全性至关重要，它为车辆提供了周围环境的先验信息，实现了可靠的高精度定位，并提供了超视距感知能力，最终提高无人驾驶车在城市环境中运行的安全性。

目前，很多研究机构对于视觉SLAM与激光SLAM技术已经做了较广泛深入的研究并提出一些相对成熟系统。一方面，现在多数系统都是基于静止环境下的假设基础上构成的，但是现实世界却是复杂而动态的。当场景中出现动态物体，例如行人、宠物和车辆等时，特征匹配、位姿估计、回环检测和地图构建可能会出错，动态物体的引入不仅会影响里程计的定位精度，而且如果不能及时有效处理动态物体带来的影响，将会增加了数据关联的难度，严重影响SLAM系统的运行性能，造成创建地图效果并不理想，最终导致算法失败。另一方面，随着科技发展，单个传感器性能也越来越强大，但是在复杂动态的城市环境中基于单一传感器的SLAM方案无法满足感知需求，不能解决全场景下的感知需求。基于视觉相机SLAM方案成本低、轻，且能够使用图像中含有丰富的信息，但是过于依赖于图像场景文理特征，很大程度上局限于光照条件差的情况，例如，夜晚场景下相机数据是不可靠的。基于激光雷达的SLAM能够提供高保真的3D测量，但是方法依赖于场景的几何特征，如长隧道或存在遮蔽物(如雾、灰尘、烟雾)，其方法将难以提供可靠的计算估计。

目前的SLAM系统主要研究集中在前端视觉里程计和后端图优化方面，而对动态场景下的三维地图重建关注较少。然而现有方法尽管它们可以在静态环境下构建出较好的稀疏或半稠密地图，但是在动态环境下表现不佳，而且实际生活中的场景往往是动态的，动态目标会对里程计的定位结果产生影响，未经处理的动态目标也会被添加到地图中，导致地图质量下降。而且只依赖于单一传感器数据，无法提供全面的环境信息，以至于无法有效处理多样化动态环境信息和准确建立全面的地图。

针对这些问题，在进行动态环境SLAM研究时，可以对系统前端里程计进行改进，消除动态物体的影响。一般动态环境下的视觉里程计组成框图如图6所示。动态环境下的视觉里程计中改进的关键部分为增加了运动特征检测与处理模块，该模块主要实现对环境中运动特征的识别与处理，将运动特征去除，消除运动物体的影响。现有技术通过从基于几何的方法和结合深度学习的方法等从数据中提取动态成分，并将其作为异常值而明确丢弃，实现精准定位，并在建图时剔除动态物体，构建三维稠密静态背景地图或语义地图。在视觉SLAM中，基于几何的方法和基于光流的方法都具有较好的实时性，而且不用借助物体先验类别就能判断运动状态，还可以分割未知动态物体。但由于分割的确定仅基于运动并通过高几何误差来表示，所以不能处理移动物体临时停止的情况，无法区分由运动物体引起的残差和错误匹配引起的残差而且难以处理退化运动。例如，物体沿对极平面移动，并且其方向和速度与摄像机相似，运动矢量很小，移动的物体容易被看成静态背景的一部分。当前利用深度学习来处理动态视觉SLAM问题，优点主要在于目标检测、语义分割等方面的深度神经网络方法能以较高的准确率和效率来识别、分割物体。利用深度神经网络提供的物体语义信息，结合运动先验知识将场景中的物体分为动态、静态和潜在移动物体。基于深度学习的动态SLAM方法在处理特定场景中特定动态物体时获得了非常有效的结果。然而，单一地利用深度学习和物体先验知识来识别移动物体，会受限于预先训练数据集中的物体类别结果，不属于训练类别中的运动物体不会被检测到，并会被错误地融合到背景地图中，导致定位精度低和建图效果差，这也是基于学习的方法的一个重要的实际限制。而且，此类方法只能检测先验动态物体，并不能识别任意移动物体。

在激光SLAM中，近年基于激光SLAM方案随着深度学习的发展在动态环境下开始逐渐流行，除了高精度的几何信息以外，使用语义分割网络后，地图中还包含语义信息，为捕捉场景中的动态对象提供依据，通过当前时刻观测与世界模型的语义一致性能够过滤场景中运动物体，缺点与视觉深度学习方法类似。

公开于该背景技术部分的信息仅仅旨在增加对本发明的总体背景的理解，而不应当被视为承认或以任何形式暗示该信息构成已为本领域一般技术人员所公知的现有技术。

发明内容

本发明的目的在于提供一种基于4D毫米波雷达的动态物体滤除方法，从而克服现有SLAM系统鲁棒性差和精度低等的缺点。

为实现上述目的，本发明提供了一种基于4D毫米波雷达的动态物体滤除方法，包括以下步骤：

步骤一、通过手动/自动触发自动在线激光雷达-相机与激光雷达-4D毫米波外参校准模块，实现多传感器在空间上以及时间上的同步，对相机雷达融合RGBD数据和4D毫米波雷达数据进行预处理；

步骤二、运动物体识别与分割机制根据环境工况选择图像分割网络实现或者点云语义分割网络，对目标进行识别分割，获取图像数据或者点云数据中可运动物体的语义标签信息，根据语义标签信息初步推测出物体的运动属性；

步骤三、基于4D毫米波雷达的运动特征检测与判别机制，利用4D毫米波雷达测出目标物体的方位信息和速度信息，结合相机运动物体识别分割数据，对目标物体进行运动特征检测与判断；

步骤四、根据运动物体识别分割与运动特征检测判别结果，外点判断与去除模块通过设定动态外点的判断与去除策略，对图像中的运动点进行去除，消除因运动物体引入的不稳定的数据关联。

优选地，上述技术方案中，步骤一中实现多传感器在空间上以及时间上的同步包括以下步骤：

(1)安装4D毫米波雷达、相机和激光雷达；

(2)对相机的内参、激光雷达-4D毫米波雷达外参、相机-激光雷达外参进行标定，实现多传感器空间上的同步；

(3)通过操作系统ROS消息过滤器保证多传感器在时间上的同步。

优选地，上述技术方案中，相机的内参标定包括，采用张正有标定法，通过对至少三幅不同姿态平面棋盘格标定板，求解相机内参数和畸变参数。

优选地，上述技术方案中，步骤(2)中激光雷达-4D毫米波雷达外参标定为在线标定，方法包括：

1)采用NDT算法实现在线配准激光雷达和4D毫米波雷达点云位置，通过匹配NDT算法中平移参数(x_t,y_t,z_t)^T和旋转参数(roll,pitch,yaw)^T得到输入的4D毫米波雷达点云与激光雷达的目标点云点云之间的变化关系；对于输入点云(4D毫米波雷达点云)每个点进行三维坐标变换公式p′_i＝Tp_i，其中T为齐次变换矩阵；

2)问题抽象化：将变化矩阵求解转化为NDT迭代寻优问题，目标点云(激光雷达点云)进行正态分布变化，得到目标点云的所有ND体素；

3)粗标定：对激光雷达和4D毫米波雷达的平衡量和旋转量进行粗略估计；

4)精标定：使用如下公式描述激光雷达点云和4D毫米波雷达点云的拟合度，公式如下：

其中θ＝(roll,pitch,yaw,x_t,y_t,z_t)^T，即坐标转化参数；

接着使用如下高斯牛顿法公式迭代寻找输入4D毫米波雷达点云、目标点云与初始值θ₀直Δθ_k至足够小，最终得到拟合度最优解；

θ_k+1＝θ_k+α_kΔθ_k

其中，J(θ)^T为F(θ)关于θ的导数，Δθ_k最优解必要条件，有如下方程组：

Δθ_k＝-(J(θ_k)^TJ(θ_k))^-1J(θ_k)^TF(θ_k)。

优选地，上述技术方案中，步骤(2)中相机-激光雷达外参标定为在线标定，实现相机和激光雷达空间上的同步，方法包括：

1)使用改进的CMRNet网络估计三维的激光雷达和二维的相机之间的外在校准参数方法，求解二者空间上的位姿关系；

2)改进的CMRNet网络以RGB图像与累计稠密激光雷达投影深度图像与反射率图像作为输入，用初始校准参数进行处理，形成灰度图像、累计稠密激光雷达反射率值图像和累计稠密激光雷达投影深度图像；

3)从灰度图像、累计稠密激光雷达反射率值图像和累计稠密激光雷达投影深度图像神经网络进行特征提取与匹配，进行参数校准，预测6DoF刚体变化输出；

4)相机和激光雷达的外参标定：假设物理空间点P在相机像素平面的齐次坐标为P₁＝[u,v,1]，在雷达坐标系下的齐次坐标为P₂＝[x,y,z,1]，则联合标定求得变换矩阵T，数学描述为：

P₁＝K[R,t]P₂＝TP₂

式中，矩阵K为相机的内参矩阵，R,t分别为相机外参矩阵中的旋转矩阵和平移向量，由标定实验求解；通过可上式将激光雷达数据与RGB数据融合形成RGBD数据；

5)激光雷达-4D毫米波雷达与相机-激光雷达外参标定以后，根据其标定结果叠加得到相机-4D毫米波雷达外参变换矩阵：

P_camera＝T_camera,lidarP_lidar

P_lidar＝T_lidar,radarP_radar

P_camera＝T_camera,lidarT_lidar,radarP_radar

式中，P_camera、P_lidar、P_radar分别为物理空间点P在相机坐标系、激光雷达、4D毫米波雷达下的坐标；T_camera,lidar为激光雷达坐标系原点在相机坐标系中的坐标；T_lidar,radar为4D毫米波雷达坐标系原点在激光雷达坐标系中的坐标。

优选地，上述技术方案中，步骤二中对目标进行识别分割的方法包括：

(1)处于光照条件较好的工况环境时，通过改进的Mask R-CNN实例分割网络实现物体运动属性识别以及轮廓的分割，并通过4D毫米波雷达运动特征检测模块结合自车速度信息，求解出各个点云相对于地面的速度信息，对当前时刻图像特征运动特征检测与判断；

(2)处于视觉退化的工况环境时，对点云使用改进的RangeSeg++网络进行语义分割与点云聚类算法，根据语义信息与聚类结果识别隐藏的动态物体，通过4D毫米波雷达运动特征检测模块结合自车速度信息，对当前时刻图像特征运动特征检测与判断。

优选地，上述技术方案中，步骤(1)在TensorFlow环境下使用改进的Mask R-CNN网络进行图像分割；

改进的Mask R-CNN网络利用注意力机制帮助模型过滤特征，注意力机制包括通道注意力模块和空间注意力模块；其中，通道注意力是全局的，而空间注意力是局部的，联合注意力机制两种注意力模块的串行连接可以表示为：

F′＝M_s(M_c(F))

其中，F为特征金字塔融合后的特征，M_c为通道注意力机制，M_s空间注意力机制，F′为串行连接的注意力特征。

优选地，上述技术方案中，步骤(2)改进的RangeSeg++网络包括：

1)将融合强度图像和深度图像作为网络输入，网络通过如下公式，将三维空间点云P_i＝(x,y,z)转换到(u,v)图像坐标系得到深度图像与强度图像：

其中，(h,ω)为生成深度图像与强度图像高度和宽度，f＝f_up+f_down为传感器的垂直俯视图，r为每个点云到雷达坐标系欧式距离；

2)通过特征金字塔提高网络多尺度特征提取能力，将低层特征图的高分辨率信息与高层特征图的高语义信息结合；

3)加入感受野模块插入到特征金字塔可以扩大感受野。

优选地，上述技术方案中，步骤三基于4D毫米波雷达对目标物体进行特征检测与判断包括：

(1)4D毫米波雷达数据接收与解析：4D毫米波雷达可以通过CAN-bus网络与车辆的工控机进行通信，工控机通过CAN接口连接到CAN-bus网络，并使用相应的硬件和驱动程序来接收和解析4D毫米波雷达发送的CAN消息；

(2)4D毫米波雷达运动特征决策：4D毫米波雷达的扫描频率是固定的，对相邻周期的数据进行比较和分析，以判别并滤除空信号目标和无效信号目标；

假设目标在第n个周期的角度信息为α(n)，距离信息为d(n)，相对速度信息为v(n)，若在第i个周期同时满足以下条件：

则可以判定为有效信号目标，否则该目标作为无效目标将被剔除；式中，α₀，d₀，v₀分别表示相邻周期内角度、距离和相对速度允许的最大变化值，i＝0，1，2，3，4……；

当静态目标的相对速度与车辆自身速度之差小于特定阈值时，将视其为无效目标，排除静止信号目标和无效信号目标，即可有效对可移动物体当前运动状态进行判断。

优选地，上述技术方案中，步骤四的外点判断与去除包括：

(1)采用图像分割网络获得物体运动属性类别以及物体的分割轮廓，主要考虑图像中语义标签为可运动的物体；

(2)同时采用4D毫米波雷达测出目标的方位信息和速度信息，进而判断该物体在特征点当前的运动状态，并将该物体上所有特侦点去除；

(3)对于神经网络无法识别的部分，或者语义标签属性非可运动物体时，通过基于4D毫米波雷达的运动特征检测进行检测与去除。

与现有技术相比，本发明具有如下有益效果：

(1)本发明基于4D毫米波雷达的动态物体滤除方法，采用基于深度学习技术的图像分割方法或者激光点云分割方法结合，基于4D毫米波雷达的运动特征判别方法结合进行运动物体去除。运动物体识别主要通过深度学习方法中的图像分割网络改进的Mask R-CNN或者点云语义分割网络改进的RangeSeg++实现，对环境中的物体进行分割并获取物体语义信息作为运动属性判别先验信息。针对高流量遮挡场景、远景问题以及小目标纹理细节缺失且存在大量背景噪声对分割网络特征提取模块增强，提高无人车辆安全性，运动特征判利用4D毫米波雷达直接测出目标的方位信息和速度信息等，行车过程中仅需解析出需要的信息即可，设定4D毫米波雷达检测目标范围以及条件，当超过阈值即判定物体为运动物体，在进行外点判断与去除时需要将该物体上的所有特征点进行去除，通过对前端跟踪线程改进，使系统在动态环境下具有较好的性能。

(2)本发明基于4D毫米波雷达的动态物体滤除方法，主要通过空间同步与时间同步对4D毫米波雷达、相机和雷达进行数据关联，从而实现传感器之间数据融合，不仅通过分割网络实现一种动态环境中运动物体识别分割，而且通过4D毫米波雷达特性建立运动物体的运动特征检测机制，最终实现更加鲁棒性的动态物体检测、分割并滤除数据中的动态物体，消除因运动物体引入的不稳定数据关联，实现SLAM系统的高鲁棒性建图与高精度定位。

附图说明

图1是根据本发明的基于4D毫米波雷达的动态物体滤除方法的流程图；

图2是本发明方法中改进的CMRNet网络框架图；

图3是本发明方法中改进的Mask R-CNN模型框架图；

图4是本发明方法中改进的RangeSeg++模型框架图；

图5是本发明方法中运动外点判断与去除流程图。

图6是现有技术中一般动态环境下的视觉里程计组成框图。

具体实施方式

下面结合附图，对本发明的具体实施方式进行详细描述，但应当理解本发明的保护范围并不受具体实施方式的限制。

除非另有其它明确表示，否则在整个说明书和权利要求书中，术语“包括”或其变换如“包含”或“包括有”等等将被理解为包括所陈述的元件或组成部分，而并未排除其它元件或其它组成部分。

如图1至图5所示，根据本发明具体实施方式的一种基于4D毫米波雷达的动态物体滤除方法，包括以下步骤：

一、运动物体识别与运动特征去除平台的搭建，先设备安装在车辆上，对相机、激光雷达和4D毫米波雷达联合标定，保证传感器设备采集信息数据空间和时间上的同步，确保相机识别目标、激光雷达和4D毫米波雷达识别目标之间的对应关系。对采集到的传感器数据进行预处理，去除无效数据，降低噪点，提高系统运行速度。

二、根据实际场景中可能会存在多个运动物体，且运动状态可能不一致的特点，根据环境工况选择运用图像分割网络或者点云语义分割网络进行目标检测、目标分类以及目标分割，获得有效物体运动属性、先验信息以及目标分割轮廓信息。当光照条件较好时，视觉因素在分割网络中占主要地位，雷达仅仅提供对应像素坐标深度信息，使用实例分割网络改进的Mask R-CNN进行图像分割。当环境视觉退化，如黑暗的夜晚、空气中模糊的条件(灰尘、雾和烟雾)等，激光雷达深度信息将在分割网络中占主导地位，通过改进的RangeSeg++网络分割每一帧的每个点云。

三、由于深度学习方法虽然能够获得可运动物体的语义信息，却不能判断该物体当前的运动状态，而且当环境存在无法识别运动物体时，识别功能将失效。结合4D毫米波雷达抗干扰能力强、分辨率高且测出目标的方位信息和速度信息等特点，进一步对目标物体进行运动特征检测和判断。

四、根据运动物体识别分割与运动特征检测判别结果，外点判断与去除模块中设定了外点判断与去除机制，使系统能够有效处理环境中的运动物体，消除运动物体的影响。

关于基于4D毫米波雷达的动态物体滤除方法的具体步骤包括：

步骤一：即时定位与建图系统运动物体识别与特征去除平台的搭建及数据处理

1、4D毫米波雷达、相机和激光雷达的安装与标定

1.1空间同步

运动物体识别与运动特征去除平台的软件基于Ubuntu18.04操作系统和机器人操作系统ROS，传感器主要为4D毫米波雷达、单目相机和激光雷达。激光雷达利用二轴云台固定在无人车上方，相机与4D毫米波雷达固定安装在平台前方，可获取无人车前方的图像信息和点云信息。

进一步的，需要对相机内参、相机激光雷达外参、激光雷达和4D毫米波雷达外参进行标定。传感器外参标定本质上是获得两个传感器的位移量(x,y,z)和旋转量(roll，pitch，yaw)，也可以用一个齐次变换矩阵T表示。齐次变换矩阵与两坐标系平移量(x_t,y_t,z_t)和欧拉角R(α,β,γ)变换计算公式如下：

其中，α为roll，β为pitch，γ为yaw。

相机内参标定，可以消除相机畸变，采用张正友标定法，通过至少三幅不同姿态平面棋盘格标定板即可求解相机内参数和畸变参数。

复杂动态环境下，传感器外参在线标定能够确保持传感器的准确性和一致性，为传感器数据的准确性和可靠性踢动基础保障。

(1)激光雷达和4D毫米波雷达外参在线标定。

传感器在线标定，可以保持传感器的准确性和一致性，使自系统能够更精确地感知和理解周围环境。该在线标定方案采用NDT(Normal Distribution Transform，正态分布变换)算法实现在线配准激光雷达和4D毫米波雷达点云位姿。通过匹配NDT算法中平移参数(x_t,y_t,z_t)^T和旋转参数(roll,pitch,yaw)^T得到输入的4D毫米波雷达点云与激光雷达的目标点云点云之间的变化关系。对于输入点云每个点进行三维坐标变换公式p′_i＝Tp_i，其中T为齐次变换矩阵。

具体流程如下：1)问题抽象化。将变化矩阵求解转化为NDT迭代寻优问题，目标点云(激光雷达点云)进行正态分布变化，得到目标点云的所有ND体素；2)粗标定。提供一个较好的初值，并不是完全的自动标定，可采用卷尺手工测量的方法，对于二者传感器平移量和旋转量的粗略估计，精度无需太高，通过NDT在线标定算法优化高精度结果。3)精标定。使用如下公式描述两个点云拟合度：

其中θ＝(roll,pitch,yaw,x_t,y_t,z_t)^T，即坐标转化参数。

接着使用如下高斯牛顿法公式迭代寻找输入4D毫米波雷达点云、目标点云与初始值θ₀直Δθ_k至足够小，最终得到拟合度最优解。

θ_k+1＝θ_k+α_kΔθ_k

其中，J(θ)^T为F(θ)关于θ的导数，Δθ_k最优解必要条件，有如下方程组：

Δθ_k＝-(J(θ_k)^TJ(θ_k))^-1J(θ_k)^TF(θ_k)。

(2)相机-激光雷达外参在线标定。

原CMRNet网络采用整个雷达点云地图作为点云投影输入，学习如何将一幅图像与点云地图相匹配，不完全适用于复杂动态环境实时相机与激光雷达标定。根据三维点的激光反射率值和该三维点投影到的图像像素的相应灰度值具有相关性与单帧点云视场覆盖率较小特点，提出改进的CMRNet网络用于估计三维激光雷达和二维相机之间的外在校准参数方法，即手动/自动触发自动在线激光雷达-相机外参校准模块，求解二者空间上的位姿关系。改进的CMRNet网络以RGB图像与累计稠密激光雷达投影深度图像与反射率图像作为输入，用初始校准参数进行处理，形成三幅图像(灰度、反射率和深度，后两者来自激光雷达)。对从这些图像神经网络进行特征提取与匹配，进行参数校准，直到获得一套满意的6DoF刚体变化输出。具体网络结构如图2所示。图2中T_init代表初始外参。

相机内参标定与相机-激光雷达外参标定后，实现了两个传感器在空间上的同步。假设物理空间点P在相机像素平面的齐次坐标为P₁＝[u,v,1]，在雷达坐标系下的齐次坐标为P₂＝[x,y,z,1]，则联合标定求得变换矩阵T，数学描述为：

P₁＝K[R,t]P₂＝TP₂

式中，矩阵K为相机的内参矩阵，R,t分别为相机外参矩阵中的旋转矩阵和平移向量,由标定实验求解，保证了相机目标和雷达目标在同一车辆坐标系下。通过可上式将激光数据与RGB数据融合形成RGBD数据。

根据激光雷达-4D毫米波雷达与相机-激光雷达外参标定以后，根据其标定结果叠加得到相机-4D毫米波雷达外参变换矩阵：

P_camera＝T_camera,lidarP_lidar

P_lidar＝T_lidar,radarP_radar

P_camera＝T_camera,lidarT_lidar,radarP_radar

式中，P_camera、P_lidar、P_radar分别为物理空间点P在相机坐标系、激光雷达、4D毫米波雷达下的坐标；T_camera,lidar为激光雷达坐标系原点在相机坐标系中的坐标；T_lidar,radar为4D毫米波雷达坐标系原点在激光雷达坐标系中的坐标。

1.2时间同步

时间同步是确保在数据融合过程中多个传感器数据帧能够处于相同时刻。由于4D毫米波雷达、相机和激光雷达的采样频率不同，它们提取有效目标所需的时间也不同。这导致进入融合系统的数据可能是来自不同时间点的信息，从而引起融合结果的明显偏差。通过机器人操作系统ROS消息过滤器MessageFilter保证传感器在时间上的同步，确保了融合系统中的数据来自相同的时刻，从而提高了融合结果的准确性。

2、对图像数据和雷达点云数据进行预处理

4D毫米波雷达能较好的适应天气环境，穿透烟雾、灰尘等物质能力强，因此4D毫米波雷达适合在全天候全天时条件下工作。4D毫米波雷达即受到的每一帧信息包括目标与雷达的距离、角度、相对速度、信号的反射强度以及每个目标的ID号等信息。4D毫米波雷达获取的信号目标中有很大一部分是干扰信号目标，包括空信号目标、无效信号目标和静止信号目标，为提高后期数据处理效率需要剔除这些目标。设定雷达检测目标范围(包括：距离、角度以及相对速度)，进一步提取有效动态目标。

对相机采集到的视频数据转换成图像数据，经过去噪、增强等处理方式提高识别准确性，从而利用算法进行后续的融合检测。

在激光雷达获取点云数据的过程中，受生产误差或者环境影响，其获得的点云数据中会不可避免地夹杂着一些噪点(离群点)，需要将其直接剔除或者以平滑的方式进行处理，一般对点云进行滤波处理。另外，激光雷达一定角度扫描车体本身，且距离中心较远的点云具有不均匀性和稀疏，通过点云滤波滤除车身周围干扰/遮挡点云以及远处噪点。

步骤二：基于分割网络的运动物体识别与分割机制

基于分割网络的运动物体识别与分割机制，根据环境工况选择图像分割网络实现或者点云语义分割网络，对目标进行识别分割，获取图像数据或者点云数据中可运动物体的语义标签信息，根据语义标签信息推测出物体的运动属性。

情况一：当环境光照条件较好时，视觉因素在分割网络中占主要地位，雷达仅仅提供对应像素坐标深度信息。运动物体识别与分割机制采用深度学习技术中的图像分割网络实现，选取合适的图像分割网络，对目标进行识别分割，获取图像中可运动物体的语义标签信息，根据语义标签信息推测出物体的运动属性，识别出图像中的潜在的运动物体，例如静坐着的人、停放的车辆等。在场景中可能存在多个动态物体，且当前运动状态可能不一致，需对不同个体进行区分对待。实例分割是在语义分割的基础上将同类对象的不同个体再进行分类，还将对属于不同人的像素进行识别分割，形成不同个体的像素分割边缘，深度学习框架选择在TensorFlow环境下使用改进的Mask R-CNN网络进行图像分割。

针对由于高流量遮挡场景、远景问题以及小目标纹理细节缺失且存在大量背景噪声，随着网络层数的加深，容易逐渐被背景吞没问题，使整个网络的分割效果并不理想，给无人驾驶带来安全隐患。网络利用注意力机制帮助模型过滤特征，通过设计注意力机制让网络模型知道哪些区域需要更多的关注，对背景的关注相应降低，突出目标区域和目标的重点特征，提高模型对细节信息的掌控能力，最终提升模型实例分割能力。改进的Mask R-CNN网络模型框架如图3所示。

其中，通道注意力是全局的，而空间注意力是局部的，联合注意力机制两种注意力模块的串行连接可以表示为：

F′＝M_s(M_c(F))

其中，F为特征金字塔融合后的特征，M_c为通道注意力机制，M_s空间注意力机制，F′为串行连接的注意力特征。

情况二：当环境视觉退化，如黑暗的夜晚、空气中模糊的条件(灰尘、雾和烟雾)等，激光雷达深度信息将在分割网络中占主导地位。通过语义分割网络与点云聚类算法，识别环境中可能的动态物体。因为此时环境视觉退化，只使用RGBD数据中的激光雷达深度数据，通过改进的RangeSeg++网络分割每一帧的每个点云生成一个语义标签以及概率，再通过点云欧式聚类方法优化网络语义标签可能错误问题边界侵蚀。

同样网络存在高流量遮挡场景、远景问题以及小目标纹理细节缺失且存在大量背景噪声，随着网络层数的加深，容易逐渐被背景吞没问题，因此改进的RangeSeg++网络主要针对改进有以下两点：

1.强度信息能够较好的描述目标物体的几何信息和纹理信息，距离信息能够较好描述目标物体的结构信息，这些信息都能够很好的表达目标特征，具有很强的互补性。因此，在改进的RangeSeg++网络提出将融合强度图像和深度图像作为网络输入，而且由于强度图像和深度图像都来自同一激光雷达，因此其特征可以融合的。改进的RangeSeg++网络模型框架如图4所示。

与针孔相机模型不同，网络通过如下公式，将三维空间点云P_i＝(x,y,z)转换到(u,v)图像坐标系得到深度图像与强度图像：

其中，(h,ω)为生成深度图像与强度图像高度和宽度，f＝f_up+f_down为传感器的垂直俯视图，r为每个点云到雷达坐标系欧式距离。

2.当小目标在图像中占比太小是会被当做噪声过滤掉，并且卷积层只能提取到简略的特征信息，但是该网络首先通过特征金字塔(Field Pyramid Network，FPN)提高网络多尺度特征提取能力，将低层特征图的高分辨率信息与高层特征图的高语义信息做了结合，不同尺度下的特征得到了有效利用，解决小区域物体特征会被卷积层中的隐层忽视掉的情况。

3.加入感受野模块(Receptive Field Block，RFB)RFB插入到FPN可以扩大感受野，有效帮助特征图联系目标与背景间的关系，加强网络的特征提取能力，进一步加强对小动态物体特征提取能力。

步骤三：基于4D毫米波雷达的运动特征检测与判别机制

采用分割网络对RGBD数据分割后只能获得神经网络能够识别的可运动物体的语义标签及轮廓分割信息，并不能判断该物体当前的运动状态，需要结合运动特征检测方法进行可移动物体当前运动状态判断。

运动特征检测模块主要用于实现图像特征当前时刻运动状态的检测与判断。针对运动物体的运动特征检测，可以采用如下几种方式进行判别，如运动模型法、多视图几何约束法、运动特征聚类法等。由于4D毫米波雷达能够直接获取出目标的方位和速度等信息，这使得在行车过程中只需解析出所需信息即可，将其与相机数据结合使用时，动态物体滤除的结果将具有更高的准确性和鲁棒性。具体方法为：

1.雷达数据接收与解析。4D毫米波雷达可以通过CAN-bus网络与车辆的工控机进行通信，工控机通过CAN接口连接到CAN-bus网络，并使用相应的硬件和驱动程序来接收和解析4D毫米波雷达发送的CAN消息。CAN消息中包含有关目标的方位、距离、速度信号的反射强度和每个目标的ID号等信息。

2.4D毫米波雷达运动特征决策。4D毫米波雷达有很强的抗干扰能力和高分辨率，它可以准确地探测目标物体。然而，在实际使用中，它所获取的信号中并非都是真实目标，而是存在一些干扰信号。这些干扰信号包括空的、无效的或者是静止的物体，它们会被错误地识别为目标。因此在解析4D毫米波雷达数据时，需要对这些干扰信号进行筛选和排除，以确保得到准确可靠的目标信息。无效信号目标与空信号目标指的是雷达返回的信号，并不代表实际存在的物体，这种信号可能是由于环境中的杂波、反射或干扰引起的。通常不携带实际目标的信息，通过信号处理和算法，辨别和过滤掉这些空信号目标，以确保准确性和可靠性。4D毫米波雷达的扫描频率是固定的，这意味着相邻扫描周期内的数据信息变化较小。通过利用这一特点，可以对相邻周期的数据进行比较和分析，以判别并滤除空信号目标和无效信号目标。这样可以提高目标检测的准确性，确保所获取的数据更加可靠。

假设目标在第n个周期的角度信息为α(n)，距离信息为d(n)，相对速度信息为v(n)，若在第i(假设为5)个周期同时满足以下条件：

则可以判定为有效信号目标。否则该目标作为无效目标将被剔除。式中，α₀，d₀，v₀分别表示相邻周期内角度、距离和相对速度允许的最大变化值，i＝0，1，2，3，4。

车辆行驶时，仍然存在静态目标，如：树木和护栏等。通过引入速度判定条件，更准确的确定有效的运动特征位置和速度。尽管在实际场景中，树木和护栏是静止的，但由于与车辆的相对运动，它们的速度不为零。因此，当静态目标的相对速度与车辆自身速度之差小于特定阈值时，将视其为无效目标。4D毫米波雷达的64个通道中，大部分信号目标是空信号目标，且静止信号目标和无效信号目标也比较容易排除，最终有效进行可移动物体当前运动状态判断。

步骤四：外点判断去除机制

对于环境中的运动特征而言，从连续图像帧的角度进行分析，其实质上是图像帧中的一些运动部分对应的特征点的数据关联出现异常，即出现了所谓的“外点”。在特征点法视觉里程计中，为了获得较准确的相机位姿估计，需要采用静止稳定的特征点进行匹配计算，首先需要将运动外点有效地去除，保留稳定的特征点进行位姿估计。

采用图像分割网络获得物体运动属性类别以及物体的分割轮廓，主要考虑图像中语义标签为可运动的物体(例如：人、交通工具、动物等)。同时采用4D毫米波雷达测出目标的方位信息和速度信息等，进而判断该物体在特征点当前的运动状态，并将该物体上所有特侦点去除。此外，对于神经网络无法识别的部分，或者语义标签属性非可运动物体时，也有可能出现运动的情况，例如人移动书本、推动椅子等，也可以通过基于4D毫米波雷达的运动特征检测进行检测与去除。运动外点判断与去除流程如图5所示。

本发明基于4D毫米波雷达的动态物体滤除方法，首先，通过手动/自动触发自动在线激光雷达-相机外参校准模块和激光雷达-4D毫米波雷达模块，获取激光雷达/相机外参与激光雷达/4D毫米波雷达外参，并可以RGBD输入预处理模块将激光雷达和相机数据转化为RGBD数据。使用联合标定好的4D毫米波雷达、相机和雷达获取平台周围的点云融合点云深度的图像特征的RGB-D图像数据，并对数据预处理。其次，根据工况选择图像分割网络实现或者点云语义分割网络，判断当前环境光照条件，若环境光照较好时，通过改进的MaskR-CNN实例分割网络实现物体运动属性识别以及轮廓的分割，并通过4D毫米波雷达运动特征检测模块结合自车速度信息，求解出各个点云相对于地面的速度信息，对当前时刻图像特征运动特征检测与判断。当环境视觉特征退化光照条件变差，RGBD数据将拒绝不可靠的视觉因素，对点云使用RangeSeg++网络进行语义分割与点云聚类算法，根据语义信息与聚类结果识别隐藏的动态物体，通过4D毫米波雷达运动特征检测模块结合自车速度信息，对当前时刻图像特征运动特征检测与判断。最后，通过外点判断与去除模块通过设定动态外点的判断与去除策略，对融合数据中的运动点进行去除，消除因运动物体引入的不稳定的数据关联，从而提升SLAM系统在动态环境下的鲁棒性。使系统在动态环境下具有较好的性能。

前述对本发明的具体示例性实施方案的描述是为了说明和例证的目的。这些描述并非想将本发明限定为所公开的精确形式，并且很显然，根据上述教导，可以进行很多改变和变化。对示例性实施例进行选择和描述的目的在于解释本发明的特定原理及其实际应用，从而使得本领域的技术人员能够实现并利用本发明的各种不同的示例性实施方案以及各种不同的选择和改变。本发明的范围意在由权利要求书及其等同形式所限定。

Claims (10)

1.一种基于4D毫米波雷达的动态物体滤除方法，其特征在于，包括以下步骤：

步骤一、通过手动/自动触发自动在线激光雷达-相机与激光雷达-4D毫米波外参校准模块，实现多传感器在空间上以及时间上的同步，对相机雷达融合RGBD数据和4D毫米波雷达数据进行预处理；

步骤二、运动物体识别与分割机制根据环境工况选择图像分割网络实现或者点云语义分割网络，对目标进行识别分割，获取图像数据或者点云数据中可运动物体的语义标签信息，根据语义标签信息初步推测出物体的运动属性；

步骤三、基于4D毫米波雷达的运动特征检测与判别机制，利用4D毫米波雷达测出目标物体的方位信息和速度信息，结合相机运动物体识别分割数据，对目标物体进行运动特征检测与判断；

步骤四、根据运动物体识别分割与运动特征检测判别结果，外点判断与去除模块通过设定动态外点的判断与去除策略，对图像中的运动点进行去除，消除因运动物体引入的不稳定的数据关联。

2.根据权利要求1所述的基于4D毫米波雷达的动态物体滤除方法，其特征在于，步骤一中实现多传感器在空间上以及时间上的同步包括以下步骤：

(1)安装4D毫米波雷达、相机和激光雷达；

(2)对相机的内参、激光雷达-4D毫米波雷达外参、相机-激光雷达外参进行标定，实现多传感器空间上的同步；

(3)通过操作系统ROS消息过滤器保证多传感器在时间上的同步。

3.根据权利要求2所述的基于4D毫米波雷达的动态物体滤除方法，其特征在于，相机的内参标定包括，采用张正有标定法，通过对至少三幅不同姿态平面棋盘格标定板，求解相机内参数和畸变参数。

4.根据权利要求2所述的基于4D毫米波雷达的动态物体滤除方法，其特征在于，步骤(2)中激光雷达-4D毫米波雷达外参标定为在线标定，方法包括：

1)采用NDT算法实现在线配准激光雷达和4D毫米波雷达点云位置，通过匹配NDT算法中平移参数(x_t,y_t,z_t)^T和旋转参数(roll,pitch,yaw)^T得到输入的4D毫米波雷达点云与激光雷达的目标点云点云之间的变化关系；对于输入点云每个点进行三维坐标变换公式p′_i＝Tp_i，其中T为齐次变换矩阵；

2)问题抽象化：将变化矩阵求解转化为NDT迭代寻优问题，目标点云进行正态分布变化，得到目标点云的所有ND体素；

3)粗标定：对激光雷达和4D毫米波雷达的平衡量和旋转量进行粗略估计；

4)精标定：使用如下公式描述激光雷达点云和4D毫米波雷达点云的拟合度，公式如下：

其中θ＝(roll,pitch,yaw,x_t,y_t,z_t)^T，即坐标转化参数；

接着使用如下高斯牛顿法公式迭代寻找输入4D毫米波雷达点云、目标点云与初始值θ₀直Δθ_k至足够小，最终得到拟合度最优解；

θ_k+1＝θ_k+α_kΔθ_k

其中，J(θ)^T为F(θ)关于θ的导数，Δθ_k最优解必要条件，有如下方程组：

Δθ_k＝-(J(θ_k)^TJ(θ_k))^-1J(θ_k)^TF(θ_k)。

5.根据权利要求2所述的基于4D毫米波雷达的动态物体滤除方法，其特征在于，步骤(2)中相机-激光雷达外参标定为在线标定，实现相机和激光雷达空间上的同步，方法包括：

1)使用改进的CMRNet网络估计三维的激光雷达和二维的相机之间的外在校准参数方法，求解二者空间上的位姿关系；

2)改进的CMRNet网络以RGB图像与累计稠密激光雷达投影深度图像与反射率图像作为输入，用初始校准参数进行处理，形成灰度图像、累计稠密激光雷达反射率值图像和累计稠密激光雷达投影深度图像；

3)从灰度图像、累计稠密激光雷达反射率值图像和累计稠密激光雷达投影深度图像神经网络进行特征提取与匹配，进行参数校准，预测6DoF刚体变化输出；

4)相机和激光雷达的外参标定：假设物理空间点P在相机像素平面的齐次坐标为P₁＝[u,v,1]，在雷达坐标系下的齐次坐标为P₂＝[x,y,z,1]，则联合标定求得变换矩阵T，数学描述为：

P₁＝K[R,t]P₂＝TP₂

式中，矩阵K为相机的内参矩阵，R,t分别为相机外参矩阵中的旋转矩阵和平移向量，由标定实验求解；通过可上式将激光雷达数据与RGB数据融合形成RGBD数据；

5)激光雷达-4D毫米波雷达与相机-激光雷达外参标定以后，根据其标定结果叠加得到相机-4D毫米波雷达外参变换矩阵：

P_camera＝T_camera,lidarP_lidar

P_lidar＝T_lidar,radarP_radar

P_camera＝T_camera,lidarT_lidar,radarP_radar

式中，P_camera、P_lidar、P_radar分别为物理空间点P在相机坐标系、激光雷达、4D毫米波雷达下的坐标；T_camera,lidar为激光雷达坐标系原点在相机坐标系中的坐标；T_lidar,radar为4D毫米波雷达坐标系原点在激光雷达坐标系中的坐标。

6.根据权利要求1所述的基于4D毫米波雷达的动态物体滤除方法，其特征在于，步骤二中对目标进行识别分割的方法包括：

(1)处于光照条件较好的工况环境时，通过改进的Mask R-CNN实例分割网络实现物体运动属性识别以及轮廓的分割，并通过4D毫米波雷达运动特征检测模块结合自车速度信息，求解出各个点云相对于地面的速度信息，对当前时刻图像特征运动特征检测与判断；

(2)处于视觉退化的工况环境时，对点云使用改进的RangeSeg++网络进行语义分割与点云聚类算法，根据语义信息与聚类结果识别隐藏的动态物体，通过4D毫米波雷达运动特征检测模块结合自车速度信息，对当前时刻图像特征运动特征检测与判断。

7.根据权利要求6所述的基于4D毫米波雷达的动态物体滤除方法，其特征在于，步骤(1)在TensorFlow环境下使用改进的Mask R-CNN网络进行图像分割；

改进的Mask R-CNN网络利用注意力机制帮助模型过滤特征，注意力机制包括通道注意力模块和空间注意力模块；其中，通道注意力是全局的，而空间注意力是局部的，联合注意力机制两种注意力模块的串行连接可以表示为：

F′＝M_s(M_c(F))

其中，F为特征金字塔融合后的特征，M_c为通道注意力机制，M_s空间注意力机制，F′为串行连接的注意力特征。

8.根据权利要求6所述的基于4D毫米波雷达的动态物体滤除方法，其特征在于，步骤(2)改进的RangeSeg++网络包括：

1)将融合强度图像和深度图像作为网络输入，网络通过如下公式，将三维空间点云P_i＝(x,y,z)转换到(u,v)图像坐标系得到深度图像与强度图像：

其中，(h,ω)为生成深度图像与强度图像高度和宽度，f＝f_up+f_down为传感器的垂直俯视图，r为每个点云到雷达坐标系欧式距离；

2)通过特征金字塔提高网络多尺度特征提取能力，将低层特征图的高分辨率信息与高层特征图的高语义信息结合；

3)加入感受野模块插入到特征金字塔可以扩大感受野。

9.根据权利要求1所述的基于4D毫米波雷达的动态物体滤除方法，其特征在于，步骤三基于4D毫米波雷达对目标物体进行特征检测与判断包括：

(1)4D毫米波雷达数据接收与解析：4D毫米波雷达可以通过CAN-bus网络与车辆的工控机进行通信，工控机通过CAN接口连接到CAN-bus网络，并使用相应的硬件和驱动程序来接收和解析4D毫米波雷达发送的CAN消息；

(2)4D毫米波雷达运动特征决策：4D毫米波雷达的扫描频率是固定的，对相邻周期的数据进行比较和分析，以判别并滤除空信号目标和无效信号目标；

假设目标在第n个周期的角度信息为α(n)，距离信息为d(n)，相对速度信息为v(n)，若在第i个周期同时满足以下条件：

则可以判定为有效信号目标，否则该目标作为无效目标将被剔除；式中，α₀，d₀，v₀分别表示相邻周期内角度、距离和相对速度允许的最大变化值，i＝0，1，2，3，4……；

当静态目标的相对速度与车辆自身速度之差小于特定阈值时，将视其为无效目标，排除静止信号目标和无效信号目标，即可有效对可移动物体当前运动状态进行判断。

10.根据权利要求1所述的基于4D毫米波雷达的动态物体滤除方法，其特征在于，步骤四的外点判断与去除包括：

(1)采用图像分割网络获得物体运动属性类别以及物体的分割轮廓，主要考虑图像中语义标签为可运动的物体；

(2)同时采用4D毫米波雷达测出目标的方位信息和速度信息，进而判断该物体在特征点当前的运动状态，并将该物体上所有特侦点去除；

(3)对于神经网络无法识别的部分，或者语义标签属性非可运动物体时，通过基于4D毫米波雷达的运动特征检测进行检测与去除。