占据栅格地图如何解决动态障碍物


占据栅格地图

在3D点云中，占据概率栅格地图比较常用。一般的概率更新形式有贝叶斯更新，Crartographer中用的就是这种。在别的论文中也看到了其他的更新形式。
对于SLAM而言，动态障碍物的影响体现在匹配和建图两个部分，采用占据栅格地图，可以减少动态障碍物在建图上的体现，但是在匹配时，当前帧的动态障碍物仍然存在，对匹配影响很大。

更改概率更新形式（减少对匹配影响）

IMU-Assisted 2D SLAM Method for Low-Texture and Dynamic Environments

2018 sci 北京交通大学

下图为2D占据栅格地图，-1代表empty,1代表occupied,0代表unknown area

在这里插入图片描述

栅格概率更新公式
在这里插入图片描述

两种情况下更新栅格：

  1. 激光击中栅格

Lomas=0.3

在这里插入图片描述

如果此栅格上一时刻之前都是free，这一时刻突然击中，代表是动态栅格，限制更新。

问题：如果最初时刻是free(-1),那么以后就不会改变。如果最初是unknown(0),之后被检测为free，再被检测为occupied，就会变。

  1. 激光通过栅格

在这里插入图片描述

如果此栅格上一时刻之前都是occupied，这一时刻检测被free。代表动态障碍物离开，立即更新为静态
在这里插入图片描述
两种方法比较：
前三幅图中，上述方法。
传统方法（下面三幅图）中看出：从左到右，连续三帧，移动物体会在地图上留下trajectory，影响帧与帧间的匹配。

cartography中remove moving object（减少对建图影响）

cartographer中在建图后比较每个栅格被击中和被路过的次数来判断动静态栅格
在assets_writer_backpack_3d.lua文件中voxel_filter_and_remove_moving_objects选项，可以用来配置Voxel过滤数据并仅传递我们认为在非运动对象上的点。

筛选出hit>0的voxel
计算路过这些voxel的rays次数
比较rays和hits数目关系

先获取miss_per_hit_limit的值，默认3

算法

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
for each
point in PointBatch

       Find the voxel

++voxel.hits

for each
point in Point Batch

       For each voxel from orign to point

              If voxel.hits>0:

++voxel.rays

for each
point in Point Batch

       For each voxel from orign to point

              If voxel.rays>=miss_per_hit_limit*voxel.hits

                     remove voxel

三个函数（cartographer/cartographer/io/OutlierRemivingPointsProcessor.cpp

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
1 void ProcessInPhaseOne(const PointsBatch& batch);
//计算每个voxel的hit数目

void
OutlierRemovingPointsProcessor::ProcessInPhaseOne(

    const PointsBatch& batch) {

  for (size_t i = 0; i <
batch.points.size(); ++i) {//遍历每个点，更新点对应的voxel的hit.几个点落入一个voxel,hit就加上

   
++voxels_.mutable_value(voxels_.GetCellIndex(batch.points[i].position))

          ->hits;

  }
}

2 void ProcessInPhaseTwo(const PointsBatch& batch);

//计算pass through这些voxel中rays数目（只对于hit>0的voxel计算）

void OutlierRemovingPointsProcessor::ProcessInPhaseTwo(

    const PointsBatch& batch) {

  for (size_t i = 0; i <
batch.points.size(); ++i) {//遍历每个点

    const Eigen::Vector3f delta =
batch.points[i].position - batch.origin;

    const float length = delta.norm();

    for (float x = 0; x < length; x +=
voxel_size_) {//遍历起点到点每个栅格

      const Eigen::Array3i index =

          voxels_.GetCellIndex(batch.origin +
(x / length) * delta);

      if (voxels_.value(index).hits > 0) {//如果栅格之前的hit>0，那么就更新ray

       
++voxels_.mutable_value(index)->rays;
      }
    }
  }
}

3 void ProcessInPhaseThree((std::unique_ptr<PointsBatch> batch);

//保留inliers,过滤outliers。如果hit所在的voxel的hit-to-ray值很高，那么就是inlier

 

void
OutlierRemovingPointsProcessor::ProcessInPhaseThree(

    std::unique_ptr<PointsBatch> batch) {

  absl::flat_hash_set<int> to_remove;

  for (size_t i = 0; i <
batch->points.size(); ++i) {//遍历每个点

    const VoxelData voxel =

       
voxels_.value(voxels_.GetCellIndex(batch->points[i].position));

    if (!(voxel.rays < miss_per_hit_limit_ *
voxel.hits)) {//如果点的rays>3*hits那么就认为是动态障碍物

      to_remove.insert(i);

    }

  }

  RemovePoints(to_remove, batch.get());

  next_->Process(std::move(batch));

}

（用于建图完成之后）

去除当前帧动态障碍物(减少对匹配和建图影响)

Online localization and mapping with moving object tracking in dynamic outdoor environments
2007 IEEE

  1. 总结：
    这篇文章中提出了一种SLAM算法，该算法可从装有激光传感器和里程计的移动车辆在动态室外环境中检测和跟踪移动物体（DATMO）。 为了从里程计信息中中校正车辆位置，引入了增量扫描匹配方法，该方法可以在动态室外环境中可靠地工作。
    在估计好车辆位置之后，周围地图将逐步更新，并且在没有先验目标知识的情况下检测到运动物体。 最后使用GlobalNearestNeighborhood（GNN）方法对检测到的运动对象进行跟踪。

  2. 创建贝叶斯占据栅格地图，并基于此进行scan matching
    直接对点云进行ICP来校正传感器误差并不准确，因为环境有很多动态物体。作者提出可以把匹配问题看成一个最大似然问题。在这种方法下，给定潜在的车辆动力学约束，通过与过去根据所有观察结果构建的局部网格图进行比较，而不是仅与之前的一次扫描进行比较，即可校正当前扫描的位置。
    在这里插入图片描述

zt = {z1 t,…,zK t } k个激光束，xt:t时刻车辆pose,ut:t时刻车辆速度 ，Mt-1是t-1时刻的占据栅格地图。等式（2）中，前者是观测模型，后者是运动模型。如何求解等式作者采用了Hill climbing strategy

  1. 障碍物检测与跟踪

两种方式检测动态障碍物：

（1） 基于局部网格图中观察到的free-cell和occ-cell之间的不一致。 如果在先前被视为free的位置上检测到物体，则该物体为移动物体。如果在先前占据的位置观察到物体，则该物体可能是静态的。
如果某个对象出现在以前未观察到的位置，那么我们就无法对这个对象做出判断

（2） 过去检测到的运动物体的信息。如果有许多移动物体通过某个区域，那么出现在该区域中的任何物体都应被识别为潜在的移动物体。因此，除了前一节所述的SLAM构造的局部静态图M外，还创建了局部动态网格图D以存储有关先前检测到的运动对象的信息。动态图的pose，大小和分辨率与静态图相同。每个动态网格单元存储一个值，该值指示在该单元位置已观察到移动物体的观察次数。

（3） 运动物体检测过程分两个步，第一步是检测可能的动态对象。给定新的激光扫描z，校正后的车辆位置以及由SLAM计算的局部静态图M和包含有关先前检测到的移动物体的信息的动态图D，单个测量zk的状态可分为以下三种类型之一：

在这里插入图片描述

第二步是在确定动态测量之后，然后通过将这些光束的端点聚集到单独的组中来识别移动的对象，每个组代表一个对象。
如果两个点之间的距离小于0.3 m，则认为这两个点属于同一对象。

PS：地图更新过程利用了移动物体检测步骤的结果。检测为动态的测量不会用于更新SLAM中的地图。对于未知的测量，我们将先验地假设它们是静态的，直到后面的证据出现为止。这将有助于消除虚假物体并得到更好的地图。

  1. 总结

本文将SLAM和DATMO结合，建图过程如下:
t-1时刻的占据栅格地图Mt-1,—>位姿估计得到Xt —>将t时刻点云与Mt-1对比，得到动态障碍物栅格，更新D（检测到的移动物体的信息的动态图）—>更新Mt

后话

才疏学浅，有问题请大家即使指出：）