一种相机辅助的激光动态点云剔除方法、设备及存储介质

本发明涉及一种相机辅助的激光动态点云剔除方法、设备及存储介质，属于激光雷达数据处理。

背景技术：

1、同步定位与建图(simultaneous localization and mapping,slam)在无人车自主导航中是一种较好的解决方案，可提供在未知环境下的地图信息和自身位置信息。然而，环境中大量动态因素会违反slam算法的静态假设，造成系统鲁棒性差和定位精度低。现实环境中不可避免的会有移动的行人、车辆等，这些动态物体一旦消失或者转移了位置，便会对后续的定位和姿态的估计造成极大的干扰，甚至造成slam建图失败。所以对环境中动态因素的剔除尤其重要。

2、当今，去除动态物体是激光slam领域中的一个重要问题，目前已经有很多学者对这个问题进行了深入研究。wei等人利用自适应3d分割技术从点云中提取车辆，并根据运动伪影效应估计车辆的运动状态和速度，实现了对动态车辆的检测。然而，该方法构建的模型只能识别动态车辆，并且主要适用于稀疏点云数据集，对于稠密点云数据集效果不佳。chen等通过构建语义地图可靠地移除动态对象，并且增加语义约束优化扫描匹配，大幅提升了激光里程计的精度。但是该方法需要较大的数据存储空间，并且需要获取先验地图信息，具有一定的应用局限性。张磊等人提出了一种基于动态特征剔除点云与图像融合的位姿估计方法，通过自适应加权策略，有效融合了位姿结果。li等人提出一种全卷积神经网络，以激光点云作为输入样本，结合数据增强训练，实现动态目标的检测。以上利用深度学习技术能够很好地识别图像中特定的物体，结合运动物体的先验知识(汽车、行人、动物)，能够很好的去除潜在的动态区域，实验结果证明该方法是最为简单高效的方案，也是目前主流的技术方案。

3、但需要注意的是，现有方法对于一些非主流的运动物体，如停在路边的移动摊位、汽车以及共享自行车等，现有方法往往会做出错误的判断。同时，以上动态点云的剔除方法，也没有考虑到由于外点的存在而产生的误匹配问题。

4、因此，为了保证动态点云剔除的精度，本领域技术人员急需要对现有的激光动态点云剔除方法进行改进。

技术实现思路

1、目的：为了克服现有技术中存在的环境中大量动态因素会违反slam算法的静态假设，造成系统鲁棒性差和定位精度低，本发明提供一种相机辅助的激光动态点云剔除方法、设备及存储介质，不仅消除了外点对于动态点云剔除的干扰，而且使用基于残差和区域生长的动态目标的聚类的方法对动态点云特征进行剔除，避免了“拖影”对slam定位与建图的干扰。

2、技术方案：为解决上述技术问题，本发明采用的技术方案为：

3、第一方面，一种相机辅助的激光动态点云剔除方法，包括如下步骤：

4、步骤1：利用激光雷达对非主流的运动物体进行扫描，获取激光雷达三维点云数据。利用相机对非主流的运动物体进行拍摄，获取相机三维点云数据。

5、步骤2：将激光雷达三维点云数据转换到相机坐标系下，得到转换后的激光雷达三维点云数据，将转换后的激光雷达三维点云数据与相机三维点云数据进行融合，得到融合后的三维点云数据。

6、步骤3：将融合后的三维点云数据根据相机内参投影到图像平面上，得到融合后的二维点云数据。

7、步骤4：在融合后的二维点云数据中随机选择n个数据点，n≥3，根据n个数据点，采用最小二乘法拟合出曲线y(q)。

8、步骤5：计算融合后的二维点云数据中任意点qj到曲线f(q)的距离，当距离小于阈值时，任意点qj属于内点，当距离大于等于阈值时，任意点qj属于外点，并记录所有内点数量。

9、步骤6：重复步骤4至步骤5，直到完成迭代次数，输出内点数量最大时对应的外点。

10、步骤7：将融合后的二维点云数据中的外点去除，得到动态点云区域。

11、步骤8：获取当前帧动态点云区域和上一帧动态点云区域，计算动态点云区域中每个点在当前帧与上一帧的移动距离，将移动距离与移动阈值相比较，在当前帧动态点云区域中去除小于移动阈值的点云，得到优化后的当前帧动态点云区域。

12、步骤9：在优化后的当前帧动态点云区域中使用区域生长的方法进行聚类，得到的聚类区域作为最终的动态点云区域。

13、步骤10：将最终的动态点云区域进行坐标系逆转换，得到最终的激光动态点云数据。

14、作为优选方案，在lidar坐标系下的3d点云表示为：

15、

16、其中，表示lidar坐标系下的第i个点云，yil、分别表示第i个点云在lidar坐标系下x轴、y轴和z轴方向上的坐标值，t表示转置。

17、在camera坐标系下的点云表示为：

18、

19、其中，表示camera坐标系下的第i个点云，yic、分别表示第i个点云在camera坐标系下x轴、y轴和z轴方向上的坐标值，t表示转置。

20、作为优选方案，把激光点云通过如下变换，转化到相机坐标系下：

21、

22、其中，表示转换后的激光雷达的第i个点云，r表示旋转矩阵，t表示平移矩阵。

23、作为优选方案，所述将转换后的激光雷达三维点云数据与相机三维点云数据进行融合，得到融合后的三维点云数据，具体包括：

24、将转换后的激光雷达三维点云数据与相机三维点云数据相同位置处的点云数据保留一个。

25、将转换后的激光雷达三维点云数据与相机三维点云数据不同位置处的点云数据全都保留。

26、将保留下的点云数据作为融合后的三维点云数据。

27、作为优选方案，到将三维空间中的点投影到二维的图像平面上，计算公式如下：

28、

29、其中，表示融合后的三维点云数据，kc表示相机内参函数，qi表示融合后的二维点云数据。

30、作为优选方案，曲线y(q)的计算公式如下：

31、y(q)＝a0+a1x+a2x2+……+akxk

32、其中，ak表示第k个待定系数，x为曲线y(q)的x轴的数值，k＜n。

33、作为优选方案，所述阈值采用不断调整的阈值t＇，所述阈值t＇计算公式如下：

34、t′＝t0+(t1-t0)*kt

35、其中，t0为设定的初始阈值，t1为设定的目标阈值，kt为当前阈值t在调整过程中所占的比例。

36、作为优选方案，所述步骤9，具体包括：

37、步骤9.1：将优化后的当前帧动态点云区域中任一个像素点作为种子点，并设置生长标准。

38、步骤9.2：判断种子点附近的相邻点是否符合生长标准，如果符合就将相邻点加入聚类区域，否则将相邻点标记为已经访问过的点。

39、步骤9.3：重复步骤9.1至步骤9.2，直至所有的相邻点都被加入到聚类区域或者被标记为已经访问过的点，输出聚类区域作为最终的动态点云区域。

40、第二方面，一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时，实现如第一方面中任一所述的一种相机辅助的激光动态点云剔除方法。

41、第三方面，一种计算机设备，包括：

42、存储器，用于存储指令。

43、处理器，用于执行所述指令，使得所述计算机设备执行如第一方面中任一所述的一种相机辅助的激光动态点云剔除方法的操作。

44、有益效果：本发明提供的一种相机辅助的激光动态点云剔除方法、设备及存储介质，首先对激光雷达和深度相机进行空间联合标定，同时获得点云与视觉特征。然后使用改进随机抽样一致(random sample consensus,ransac)算法对外点进行剔除，加入局部迭代最小二乘(locally iterative least squares，lils)循环，更好地改善了内点集合的质量，从而获得动态视觉区域。通过视觉图像与点云的关联，划分出动态点云区域，以下称之为动态背景区域。进一步将动态背景区域内残差较大的点云特征视为动态点云，以提高剔除动态特征的精度，再基于区域生长进行动态目标的聚类，最后将其剔除，进而提高slam的定位精度和建图效果，为后续无人车的导航与路径规划作业提供支持。相较于现有技术其有益效果如下：

45、(1)、采用sensorcalibration toolbox进行激光雷达和相机的空间联合标定，其标定过程是通过调整内外参数对齐道路场景内的激光点云和图像，从而可以同时获得点云与视觉特征。

46、(2)、使用改进ransac算法对外点进行剔除，加入局部迭代最小二乘循环，更好地改善了内点集合的质量，获得动态视觉区域，从而减少特征匹配过程中的误匹配。通过视觉图像与点云的关联，划分出动态点云区域。进一步将区域内残差较大的点云特征视为动态点云，然后使用基于区域生长的方法对动态目标进行聚类，并将其剔除。提高了剔除动态特征的精度，进而提高slam的定位精度和建图效果。