Leveraging Stereo-Camera Data for Real-Time Dynamic Obstacle Detection and Tracking

简介

文章使用立体相机产生的带噪点云数据 检测追踪动态障碍物，实现动态避障。

通过识别机器人周围环境中的单个目标，并将其分为静态和动态两类。动态对象被标记为一个人或一个通用的动态对象。然后估计它们的速度，以生成一个适合执行避障的2D占用网格。

评价指标：

we reach a MOTP of 0.07 ± 0.07m, and a MOTA of 85.3% for the detection and
tracking of dynamic objects. We reach a precision of 96.9% for the detection of static objects.
https://zhuanlan.zhihu.com/p/75776828
https://www.cnblogs.com/YiXiaoZhou/p/5937980.html

相关工作分析：

1. 利用高精度激光雷达生成的点云 可以检测动态目标，但是成本高
2. 利用图像数据和特定目标识别网络 检测 特定数量的动态目标，然而不能作为通用动态障碍物识别方法，且无法运行在算力受限的平台

contribution

1. 引入了人体的图像检测器；
2. 实现了 能够短时间内处理遮挡 的算法，从而提升了行人的检测和跟踪效果；
3. 实时通用

方法

1. 点云生成

如果采用深度相机，那么就可以直接获得深度图，也可以向下面两种方法间接获得。在获得深度图之后，就可以结合相机的内外参数来构建点云。

https://blog.csdn.net/weixin_45358184/article/details/111645985?utm_medium=distribute.pc_relevant.none-task-blog-2_defaultCTRLIST_{default-14.searchformbaiduhighlight&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2}default_CTRLISTdefault-14.searchformbaiduhighlight

a. 块匹配 //TODO

半全局块匹配，并对得到的视差映射应用加权最小二乘滤波器。

b. 深度学习MADNet //TODO

MADNet

2. 点云滤波

为了提升后续的处理速度 以及 去噪，去除不需要的点降低点云的规模。

2.1 剪裁

• 将过远、过高、过低 的点云裁剪掉，处理后的点云记为$h^d$

2.2 滤波

• 对crop之后的点云进行滤波，利用leaf 尺寸为$l_l$的 voxel滤波器 对点云滤波，使得规模降低一个数量级，且点云密度均匀；
• 去除所有 半径$l_r$内 近邻少于 $l_n$的点， 用于边缘去噪。处理后的点云记为$h^s$

3. 聚类、3d追踪

在这个模块中，我们通过聚类识别单个对象，并逐帧跟踪它们。

3.1 聚类 // TODO DBSCAN

使用DBSCAN算法对点云聚类， 获得$C=C^1,C^2,...,C^m$个集群；

与欧几里得聚类[13]相比，我们发现DBSCAN能更精确地在杂乱的点云中分离单个对象，而只引入了边际的计算开销。

由于人类不是刚体，不适于用点云来检测追踪。因此为refine聚类， 引入2d人类检测器生成bbx，将任何与一个以上bbx相关（下面解释）的集群分开，以区分人类对象。

通过分离 那些部分位于bbx内且数量低于阈值的点云， 来区分 人类和附近的静态物体。

3.2 3d追踪

在 $t$ 时刻， 计算所有集群的 质心$c_t$，即点云位置的均值。然后和上一帧 的质心 $c_{t-\Delta t}$ 进行最近邻匹配，即可追踪。

在 k 个$\Delta t$ 内，轨迹被定义为 T t , k = { c t − k Δ t , . . . , c t − Δ t , c t } T_{t, k} = \{ c_{t-k \Delta t},..., c_{t-\Delta t}, c_{t}\} Tt,k​={ct−kΔt​,...,ct−Δt​,ct​}

当前帧的聚类质心 无法与 先前帧 匹配时， 认为 该聚类新出现；
先前帧聚类质心在当前无匹配，认为 该聚类 离开视野。

4. 动态 与 静态 分类

为了保证不同尺度集群分类的准确性，本文设置了两个阈值。让集群中的每个点为集群的类别 进行投票， 如果投动态类的票的绝对数量或者相对数量 超过了阈值$l_{dyn}^{abs},l_{dyn}^{rel}$，则判定为动态。

如果在时间 $\tau$ 内，某集群的类别判断产生了矛盾，则判定该类别为不确定类别。

4.1 投票依据：速度

作者提出假设，如果集群是动态的物体，那么在两帧之间，它们的移动速度肯定是要比静态点云大很多的。

将当前帧 稀疏点云（经滤波后点云$h^s$ ）上的 某集群中的 某个点 和 上一帧中的稠密点云（未滤波的点云$h_{t-\Delta t}^d$）进行 最近邻匹配（更加抗噪声、鲁棒）， 计算距离 $d$ 以及速度。如果速度高于阈值，则投票为动态；如果低于阈值，投票为静态。

对于静态物体，这些测量到的最近邻距离d将以噪声的大小表示。
阈值设定为 略高于 点云$h^s$ 的噪声的水平。

4.2 下面的点 没有投票权 // TODO？？？：

a. 仅被观测到一次，没有投票权。
b. 当前帧的点 被 先前帧中 不同轨迹不同聚类的物体 遮挡，则该点无投票权。而如果是被自身集群遮挡，那么可以参与投票。

如何判定当前点云在上一帧被其他集群的点云遮挡：

1. 将 当前帧的 目标点$q_t$（3d） 投影到 先前帧的成像平面来估计深度图。
2. 在 先前帧的深度图 上进行最近邻搜索，如果找到了最近邻$n^{2d}$，且$n^{2d}$对应的3d点 $n$ 的深度小于 $q_t$

那么如何去判断是被自身还是其他集群遮挡呢？
我们对于 $n^{2d}$，可以反投影得到在上一帧下的其空间点，找出其最近邻的集群id，判断是否跟当前p属于的集群一致即可。

5. 2d人像检测

该系统将一个站着的人归类为静态对象，只有当她开始行走时，才会意识到这是一个动态对象。通过在我们的管道中添加Mobilenet-SSD作为一个可视化的2D人检测器，我们实现了对行人的独立运动检测。

使用IoU跟踪 bbx; 在成像平面生成bbx 轨迹$B_{t,k}$;

另外，2D的跟踪结果可以帮助3D集群的跟踪结果的提升，比如可以统计空间集群中在图像上投影落在框内的个数，从而来建立2D-3D之间联系。

6. 2d与3d 检测和追踪的融合

该模块的输入是3D集群轨迹$T_t^i$，分为静态或动态，2D边界框跟踪轨迹$B_t$。

对每一个集群 的轨迹$T_t^i$，计算关联2D到3D检测的频率$f_a^i$；如果$f_a^i$大于阈值 λ，将这个轨迹的所有集群分类 为行人，不管他们的初始分类，都代表一个动态对象。

此外，该模块检查 bbx轨迹$B_t^i$的所有边界框是否始终与同一集群轨迹$T_t^i$的集群相关联，否则重置$f_a^i$

7. 运动估计 // TODO ? ? ?

我们采用保守运动模型来估计动态物体的速度和短期未来路径。假设动态物体在水平面上运动，我们利用一个恒定的二维速度模型，基于卡尔曼滤波(KF)估计它们的速度。

KF的测量输入zi是集群轨迹 $T_t^i$ 的质心 ${\vec{c}}_i=[c_x,c_y,c_z{]}_i^T$ 在世界坐标系的x-y平面上测量的 ${\vec{z}}_i=[c_x,c_y{]}_i^T$ ，定义状态向量 ${\vec{x}}_i=[x,y,\dot{x},\dot{y}{]}_i^{\top }$。

运动方程和测量模型定义为:
$$\begin{array}{c}{\vec{x}}_i[k+1]=A[k]\cdot {\vec{x}}_i[k]+N(0,Q)\\ \\ {\vec{z}}_i[k]=H[k]\cdot {\vec{x}}_i[k]+N(0,R)\end{array}$$
其中Q和R分别对系统噪声和测量噪声进行建模。H提取 ${\vec{x}}_i$ 的前两个维度，A定义为:

其中，Ts表示两次更新之间的时间间隔。利用KF，我们可以捕捉动态物体的短时遮挡。

具体来说，当目标i在跟踪过程中丢失时，保持KF运行，计算所有新出现的目标 j 与丢失目标相同的概率 $p(j=i)$

我们计算这个概率为 $p(j=i)=N\left({\vec{c}}_j\mid {\vec{c}}_i,C_i\left({\vec{x}}_i\right)\right)$ , $C_i\left({\vec{x}}_i\right)$为状态${\vec{x}}_i$的协方差估计。如果p(j = i)超过一个阈值，我们将对象 i 和 j 的集群跟踪连接起来。

8. 2D Occupancy grid

为了实现路径规划和避障，我们利用了占用网格表示[17,20,40]，并特别使用了计算效率高的Costmap-2d实现
我们为静态、动态和不确定的物体创建了三个平行的地图，其中障碍物以其各自位置的正代价表示。

在不确定的地图中，我们只创建了短期成本，没有静态/动态分类。
在静态地图中，我们使用射线追踪 raytracing来清除被错误占用的空闲空间。
在动态地图中，我们将代价按照物体估计速度的方向进行扩展，使机器人在未来不会与之发生碰撞。
然后，可以通过聚合上述三个层的成本来规划和执行安全路径。

实验

A. 参数与设置

我们记录了多个立体视觉数据集，包括行人，具有挑战性的室内和室外场景，闪亮的表面，低纹理的物体，照明变化，以及空旷和拥挤的空间

为了记录我们的数据集，我们使用了一个Clearpath Robotics Jackal机器人，它配备了立体相机(灰度，752×480px)和用于GT测量的16通道的Ouster OS-1激光雷达。

我们的数据集和机器人的示例图像如图6所示。对于所有实验，都在Intel i7-8650U处理器上运行。

对于我们的立体相机设置，我们设置了上面介绍的参数：

1. 点云滤波部分
   
2. 投票部分
   
   

机器人的标称速度为1.2m/s。我们验证了我们的设置足以让对象可靠地追踪到慢跑者的速度。请注意，这些参数是使用独立于评估中使用的数据集确定的。

为了精确定位我们的机器人，我们运行一个类似OKVIS的VIO算法。对于图像处理，我们使用OpenCV。在Caffe中实现了行人检测算法Mobilenet-SSD，在Tensorflow中实现了深度立体网MADNet。

B. 点云的精度和完整性

在本节中，我们简要评估立体相机点云的质量，以识别精度限制lc，我们将在第四节- c中使用它来评估静态目标检测精度。

为了评估III-A节中确定的两种点云生成方法的性能，我们收集了与立体图像同步的地面真三维激光雷达数据。为了比较激光雷达和视觉点云，我们使用最近邻距离d作为度量，对于两个点p1和p2, d = ||p1−p2||^2。
我们的立体相机点云的精度是通过计算从相机云的每个点到它在激光雷达云中最近的邻居d。
与此相反，我们通过从激光雷达云的每个点到相机云中最接近的点进行计算来测量相机云的完整性。

我们在一个数据集上评估相机点云的准确性和完整性，我们在拥挤的公共人行道上行驶。为了实现实时性，我们通过对图像进行因子2的降采样来简化块匹配过程.

• 精度: 图7所示的直方图显示了距离d的归一化分布。块匹配点云表现最好，精度限制lc = 0.8m，因为所有点都在这个值以下，所以深度立体网络无法与块匹配竞争。

• 完整性: 距离d归一化分布直方图如图7所示。比lc = 0.8m大得多的测量结果表明，相机没有捕捉到物体的点。深度立体网络表现最好，如图3所示。总的来说，我们观察到深度立体网络不太准确，但比块匹配更完整。

对于评估的其余部分，由于其实时性和准确性，我们使用下采样图像的块匹配

C. 分类与跟踪精度

为了评估该系统动态目标的分类和跟踪精度，我们将得到的目标轨迹与激光雷达测量的手动标记轨迹进行了比较，这些轨迹作为我们的地面真实值。我们使用MOTP和MOTA指标.

对于在真实环境中导航的机器人系统来说，静态物体的精确映射至关重要。

由于MOTP和MOTA不评估静态目标的检测，我们评估基于立体和地面真实静态云之间的相似性。

1. 动态对象的分类和跟踪:

我们首先生成一个受控的、完全静态的环境D1的地图ml，使用的是3D激光雷达的测量和基于icp的算法。随后，我们将机器人定位在 ml中，并记录第二个数据集D2，这一次有动态对象存在。

我们首先手动排除D2中所有属于静态物体的激光雷达测量值，从而创建了障碍物轨迹的gt。

然后我们在剩余的激光雷达云中通过裁剪高度提取行人的上半身，以消除移动腿的影响，从而获得平滑的轨迹.

随后，我们应用欧几里得聚类，使用最近的质心对$C^{lidar}$进行聚类跟踪，方法与III-C节相同。我们目视检查并调整激光雷达真实轨迹

以确保没有假阳性、假阴性或错配。

D2长度为4分钟，有31次与行人相遇，1267个gt行人位置雷达。图8显示了我们基于激光雷达的真实轨迹$T^{i,lidar}$、和基于相机的动态物体轨迹$T^j$（用我们的方法生成的）。

动态物体的轨迹(按III-D节所述模块分类)用黄色表示，基于LiDAR数据的地面真实轨迹用蓝色表示，静态物体的立体相机点云用红色表示。轨迹是平滑的，因此，表明我们的解决方案的适用于运动跟踪和预测。短轨迹是由于障碍物离开有限的FOV造成的。这个大厅的尺寸是30 × 10米

为了计算MOTP和MOTA，我们将物体（真实雷达轨迹）和假设（相机轨迹）正确匹配的阈值lT设置为0.4m，假设一个普通人的直径为0.4m，因此，如果物体和假设没有任何重叠，则是不正确的匹配。

使用此方法，MOTP为0.07±0.07m, MOTA为85.3%，其中假阴性率fn = 8.3%
(包括未检测到的动态物体和被误分类为静态或不确定的动态物体)，假阳性率fp = 3.0%(包括ghost或被误分类为动态的静态物体)，不匹配率FM = 3.3%

2. 静态物体分类

D1的地图 ml作为静态对象的ground truth。我们将D2中被分类为静态的所有立体相机集群累加起来，得到点云ms。图9显示了ml与ms叠加的图像。

我们的静态激光雷达地面真相显示为蓝色，静态物体的立体相机点云累积为红色。为了更好的可视化，我们还用绿色显示了累积的动态摄像机点云和激光雷达地平面，为了比较云，我们去掉了它。

为了评估基于立体和真实静态云之间的相似性，我们测量从相机云ms的点到激光雷达云ml的最近邻距离d，理想情况下，静态相机云ms与激光雷达地面真实ml一致。

图10显示了这些距离d的归一化分布。理想情况下，对于静态对象d为零。然而，如
上节B所示，在极端情况下，静态点与静态地面真实值的差异可达lc = 0.8m。

由于每个点的分类没有gt可用，我们使用一个阈值估计管道的正确静态分类。我们选择这个阈值le作为lc = 0.8m和0两种极限情况的平均值，因此le = 0.4。
我们声明相机云低于le的所有静态点都是正确分类的，静态物体的分类精度达到96.9%。

D. 运行时评估

表I显示了我们的模型在部署到第A节描述的嵌入式系统时的时间。两个最重要的贡献者是块匹配和人检测网络。总的来说，运行所有功能可达8.5Hz。除去人探测器，我们达到了13.5Hz。我们工作的核心部分，即点云过滤、聚类和3D跟踪、分类和运动估计每帧立体相机需要21ms。

动态障碍检测和跟踪PIPELINE主要模块在标准CPU上的平均运行时间。

https://blog.csdn.net/weixin_45358184/article/details/111645985?utm_medium=distribute.pc_relevant.none-task-blog-2_defaultCTRLIST_{default-14.searchformbaiduhighlight&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2}default_CTRLISTdefault-14.searchformbaiduhighlight

https://www.cnblogs.com/pinard/p/6208966.html
https://blog.csdn.net/huacha__/article/details/81094891
https://zhuanlan.zhihu.com/p/129329613
https://zhuanlan.zhihu.com/p/83411945
https://zhuanlan.zhihu.com/p/353312179