[发明专利]一种3D激光slam建图中动态障碍物的后处理方法

权利要求书

1.一种3D激光slam建图中动态障碍物的后处理方法，其特征在于，步骤如下：

1)利用3D激光slam方法得到先验地图和建图过程中激光雷达的行驶位姿；

2)对所述得到的先验地图中的点云信息进行重定义；

3)根据所述步骤1)中得到的激光雷达的行驶位姿和步骤2)中重定义后的点云信息对步骤1)中得到的先验地图进行网格划分，得到可能存在动态点的区域；

4)根据所述步骤3)中网格划分得到的可能存在动态点的区域筛选出动态障碍物点云；

5)根据所述步骤4)中的动态障碍物点云，区分动态点和地面点，将动态点移除同时对地面点进行拟合；

6)重复所述步骤5)逐步完成对整个地图的动态障碍物剔除，最终得到不含动态障碍物的静态地图。

2.根据权利要求1所述的3D激光slam建图中动态障碍物的后处理方法，其特征在于，所述步骤1)中先验地图为激光雷达扫描的点云数据逐帧累积构建而成的地图；所述激光雷达的行驶位姿为行驶过程中激光雷达的位置和姿态。

3.根据权利要求1所述的3D激光slam建图中动态障碍物的后处理方法，其特征在于，所述步骤2)中的先验地图中的点云信息为激光雷达扫描得到的周围环境的三维位置信息的集合；

所述步骤2)中的点云信息重定义方法为：设P_t是一组在时间t处包含n个点的点云，P_t^M是先验地图中时间t处的点云，即全局坐标系下时间t处包含的点云信息，P_t^Q是时间t处的查询帧上的查询点云，即激光雷达坐标下时间t处包含的点云信息，T_t^Q是与P_t^Q关联的SE(3)姿态变换矩阵；因此将先验地图中的点云信息进行重定义如下：

式中，T为总时间步长，[T]等于{1,2，...，T-1，T}，M为在世界框架上先验地图中所有被测量的点云信息；同时，P_t^M为全局坐标系下时间t处包含的点云信息，意味着P_t^M可通过T_t^Q的逆变换得到P_t^Q，即P_t^Q和P_t^M存在联系，P_t^Q是查询，P_t^M是映射。

4.根据权利要求3所述的3D激光slam建图中动态障碍物的后处理方法，其特征在于，所述步骤3)中的先验地图网格划分的方法为：以激光雷达为中心，ρ为半径，角度为θ划分先验地图，将其划分为多个网格，将这些网格作为筛选动态障碍物的可能存在动态点的区域V_t；

V_t＝{P_k|P_k∈P_t,ρ_k-1＜ρ(P_k)＜ρ_k,θ_k-1＜θ(P_k)＜θ_k,ρ_k＜ρ_max}

式中，ρ_max为最大半径，ρk-1，ρ_k分别为k-1和k处的半径，θ_k-1，θ_k分别为k-1和k处的角度，P_k是k处的一组点云，ρ(P_k)为P_k的半径，θ(P_k)为P_k的角度，P_t是一组在时间t处包含n个点的点云。