基于栅格划分的动态点云去除方法

本发明属于高精地图构建、导航与规划，具体涉及一种基于栅格划分的动态点云去除方法。

背景技术：

1、随着科技的发展，人们对定位与建图的需求也日益强烈，越来越精准的地图构建以及定位成为了如今发展的趋势。随着slam(simultaneous localization and mapping)技术的发展，slam技术成为了如今自动驾驶技术中较为基础的技术之一。但是，在当今的真实环境中，由于一些动态点云的存在，比如行人、自行车、汽车，这就会导致构建地图的精度下降，进而会影响到定位的精度。

2、目前主要的动态点云滤除方法包括以下几种：1、基于语义先验信息：利用语义先验标记出可能为动态点云的聚类簇，并计算每个聚类簇中边缘像素的重投影误差进行优化函数的构建，但是该算法需要利用先验信息，对一些没有出现过的动态点云不能进行较好的识别与剔除。并且其依赖于外部的光照条件，对无光的环境无法进行动态点云的滤除。2、基于点云聚类：利用点云场景流估计算法逐点估计移动情况，结合点云聚类、主成分分析对动态点云进行剔除。该类方法优点是可以不依赖于视觉系统，可以在无光条件下运行。但是算法在场景流推理中，速度较慢，导致算法整体的耗时不能保证在0.1s以内，难以保证实时性，并且其针对于动态点云的边界框缺少更为灵活的匹配机制。3、基于地面检测：采用欧几里得聚类方法在非地面区域检测动态点云，并去除动态点云数据。但是易对动态点云进行误判，导致对静态点云进行滤除，从而对建图精度产生影响。

3、综上所述，在无光照以及无先验信息的条件下，现有方法难以实现高精度、实时性的动态点云数据去除，因此，提出一种新的动态点云去除方法是十分必要的。

技术实现思路

1、本发明的目的是为解决在无光照以及无先验信息的条件下，现有方法难以实现高精度且实时性的动态点云数据去除的问题，而提出了基于栅格划分的动态点云去除方法。

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

3、基于栅格划分的动态点云去除方法，所述方法具体包括以下步骤：

4、步骤一、设定时间阈值t和空间阈值s，根据时间阈值t和空间阈值s为当前关键帧从关键帧数据库中选择历史关键帧；利用当前关键帧和选择出的历史关键帧组成当前的局部地图；

5、步骤二、根据当前关键帧下激光雷达坐标系相对于世界坐标系的位姿，将当前的局部地图转换到激光雷达坐标系中；

6、步骤三、以激光雷达为圆柱体中心、以感兴趣区域高度差作为圆柱体高度、以感兴趣区域半径作为圆柱体半径来构建空间圆柱体结构，构建的空间圆柱体结构所包含的区域即为感兴趣区域；

7、步骤四、对感兴趣区域内的当前关键帧点云数据进行径向和轴向的分块，得到当前关键帧的各栅格的点云；对感兴趣区域内的局部地图点云数据进行径向和轴向的分块，得到局部地图的各栅格的点云；

8、步骤五、分别计算当前关键帧的各栅格的描述子以及当前局部地图的各栅格的描述子；

9、步骤六、根据步骤五中计算出的描述子对当前关键帧的各栅格进行分类，确定出动态点云所在的栅格；

10、步骤七、对动态点云所在栅格进行地面拟合，去除栅格中的动态点云数据；

11、步骤八、将步骤七的栅格中剩余点云、其它栅格内的点云以及位于感兴趣区域外部的当前关键帧点云一起写入关键帧数据库，进行全局地图的构建。

12、进一步地，所述步骤一中，根据时间阈值t和空间阈值s为当前关键帧从关键帧数据库中选择历史关键帧；其具体过程为：

13、选取出与当前关键帧时间距离小于时间阈值t的历史关键帧，再分别为选取出的每个历史关键帧构建位姿kd树，再根据构建的位姿kd树分别计算当前关键帧与每个历史关键帧的空间距离，再从选取出的历史关键帧中选择出与当前关键帧空间距离小于空间阈值s的历史关键帧。

14、进一步地，所述当前关键帧下激光雷达坐标系相对于世界坐标系的位姿通过激光雷达惯性里程计系统获得。

15、进一步地，所述将当前的局部地图转换到激光雷达坐标系中；具体为：

16、

17、其中，pworld为局部地图的点云坐标；为当前关键帧下激光雷达坐标系相对于世界坐标系的位姿；上角标-1代表矩阵的逆；plidar为转换到激光雷达坐标系下的点云坐标。

18、进一步地，所述步骤四中，对感兴趣区域内的当前关键帧点云数据和局部地图点云数据进行分块的方法一致；具体的分块方法为：

19、步骤四一、以空间圆柱体结构上表面的圆心为圆心，在上表面内做nr个同心圆，并将最大圆的半径设置为空间圆柱体结构的半径；

20、对最大圆进行扇形划分，将最大圆划分为面积相等的nθ个扇形，nθ个扇形的半径与nr个同心圆均相交，即对于每个圆来说，均将圆内部的区域划分为面积相等的nθ个扇形；

21、则在空间圆柱体结构的上表面，共将空间圆柱体结构的上表面划分为nrnθ个区域，将划分得到的第(i,j)个区域表示为g′(i,j)，i＝1,2,…,nr，i是同心圆的个数，j＝1,2,…,nθ，nθ为每个圆被划分的扇形个数；

22、步骤四二、对于划分所得到的任意一个区域，将该区域向空间圆柱体结构的下表面投影，得到投影区域；再将该区域与投影区域之间的部分作为该区域对应的栅格，将第(i,j)个区域对应的栅格记为第(i,j)个栅格g(i,j)；

23、同理，对划分所得到的每一个区域分别进行处理，分别得到各个区域对应的栅格。

24、进一步地，所述栅格g(i,j)中的点pk＝{xk,yk,zk}满足：

25、

26、其中，pt为落在栅格g(i,j)内的点云集合；rk为在极坐标系下，pk在xy平面的投影点的极径，θk＝arctan(yk,xk)，θk为在极坐标系下，pk在xy平面的投影点的极角；nr为同心区域模型中的环数；nθ为同心区域模型中的扇区数；lmax为栅格g(i,j)的最大极径；lmin为栅格g(i,j)的最小极径，δlm为栅格g(i,j)的极径差，δlm＝lmax-lmin。

27、进一步地，所述扇形个数nθ的取值为24，同心圆个数nr的取值为3。

28、进一步地，所述步骤五的具体过程为：

29、

30、

31、其中，表示在当前关键帧下的第(i,j)个栅格内点云的高度差，即当前关键帧下的第(i,j)个栅格的描述子，表示在局部地图下的第(i,j)个栅格内点云的高度差，即局部地图下的第(i,j)个栅格的描述子，表示在当前关键帧下的第(i,j)个栅格内所有点云的z轴坐标集合，表示在局部地图下的第(i,j)个栅格内所有点云的z轴坐标集合。

32、进一步地，所述步骤六的具体过程为：

33、对于当前关键帧下的第(i,j)个栅格：

34、

35、其中，是当前关键帧下的第(i,j)个栅格内点云的高度差，是局部地图下的第(i,j)个栅格内点云的高度差，sr是中间变量；

36、根据sr确定动态点云所在的栅格，确定方法为：若第(i,j)个栅格内包含点云的个数大于等于10，且满足条件(1)或条件(2)，则第(i,j)个栅格是动态点云所在的栅格；

37、条件(1)：0＜sr＜0.2且

38、条件(2)：sr＞0.2且在第(i,j)个栅格的周围有动态点云所在的栅格。

39、更进一步地，所述步骤七的具体过程为：

40、步骤七一、对于动态点云所在的任一栅格，从该栅格中选取出高度最小的m个点云作为种子点，得到种子点集；

41、步骤七二、初始化迭代次数l＝1；

42、步骤七三、计算种子点集中全部种子点的平均高度值hmean，再选取出高度低于hmean+hground的点作为地面候选点，其中，hground为地面阈值高度；

43、对全部种子点和地面候选点进行地面拟合：

44、ax+by+cz+d＝0

45、其中，a,b,c,d为平面模型的系数，x,y,z为平面模型中的点的三维坐标；即

46、

47、其中，上角标t代表转置；

48、步骤七四、计算的值：

49、计算种子点集的协方差矩阵c：

50、

51、其中，为种子点集中全部种子点三维坐标的均值，s1,s2,…,sm分别为种子点集中第1个，第2个，…，第m个种子点的三维坐标；

52、对协方差矩阵c进行特征分解后，等于最小特征值所对应的特征向量；

53、步骤七五、将代入步骤七三中的拟合方程后，得到拟合平面π0；

54、步骤七六、遍历该栅格中的所有点，分别计算每一个点到拟合平面π0的距离：

55、

56、其中，d是栅格中的点(x0,y0,z0)到拟合平面π0的距离；

57、若d小于阈值，则点(x0,y0,z0)是地面点，并将点(x0,y0,z0)加入种子点集中；否则，d大于等于阈值，则点(x0,y0,z0)不是地面点；

58、步骤七七、判断是否达到设置的最大迭代次数；

59、若达到设置的最大迭代次数，则将最后一次迭代更新后的种子点集中包含的全部点云作为地面点，即去除了动态的点云数据；

60、若未达到设置的最大迭代次数，则令l＝l+1，利用步骤七六中更新后的种子点集，返回执行步骤七二。

61、本发明的有益效果是：

62、本发明提出了一种实时动态点云滤除算法，通过计算并比较当前关键帧以及局部地图在感兴趣区域中对应栅格的点云描述子，识别出存在动态点云的栅格，从而有效去除激光雷达当前扫描帧中的动态点云，提高了动态点云数据去除的精度，进而最终能够构建出滤除动态点云后精准的全局点云地图。而且本发明方法的实现不受限于光照和先验条件，在无光照以及无先验信息的条件下仍可实现，可以全天候工作。