使用2D激光雷达进行移动物体检测和跟踪

该软件包旨在为配备2D激光雷达传感器并发布'sensor_msgs/LaseScan' ROS消息的机器人平台提供移动物体检测和跟踪功能。 如下图所示的场景就是一个例子,其中黑色缩放的汽车配备了激光雷达传感器,需要通过激光雷达测量来跟踪红色车辆的运动。

该软件包的输出如下图所示,可以看到它估计了红色车辆的位置、速度、方向和尺寸。

它专门用于跟踪矩形物体,因此主要用于车辆跟踪。 该软件包的工作流程受到Kim等人2018年发表的论文[1]的启发。

概述

下面您可以阅读其功能和操作的概要。 关于该软件包内部工作原理的更深入解释可以在这篇论文中找到。

检测

系统的检测部分如下流程图所示:

聚类

在聚类步骤中,原始激光雷达测量被划分为组/簇。通过这种方式,环境中的不同物体被区分开来。 一种简单的方法是根据激光雷达测量之间的欧几里得距离来分离簇。 因此,如果两个连续的激光雷达测量之间的距离大于预定义的阈值距离,这两个点就被划分为两个单独的簇。

然而,由于激光雷达测量随着距离传感器的距离增加而变得稀疏,物体 因此,阈值距离应该以一种与距离成正比的方式进行调整。 在这个系统中,这是通过使用自适应断点检测算法来实现的。 其操作如上图右侧所示,使用的方程如下所示。

矩形拟合和L形提取

在这一步中,提取的簇被拟合成矩形。 这样做是为了提高矩形物体的跟踪精度和形状估计。 用于矩形拟合的算法是Zhang等人2017年[2]开发的基于搜索的矩形拟合算法。

矩形拟合后,从所有矩形中提取L形。 L形代表了靠近传感器的矩形角及其相邻的两条边。 因此,每个L形包含五个测量值,即角点的位置、矩形的方向(theta)以及其边的长度(L1, L2)。

跟踪

系统的跟踪部分如下流程图所示:

数据关联

通过最近邻数据关联方案和欧几里得距离准则在时间帧之间跟踪簇。

外观变化检测器

在被跟踪车辆的最近角点在测量之间发生变化的情况下,通过比较车辆四个角点的马氏距离与新L形的马氏距离来检测这一变化。

运动学和形状跟踪器

检测到的车辆的运动基于两个运动学跟踪器进行跟踪。 一个具有恒定速度模型的卡尔曼滤波器和一个具有协调转弯模型的无迹卡尔曼滤波器。

检测到的车辆的形状和方向由包含两个模型的卡尔曼滤波器跟踪。 第一个模型是恒定形状模型,表示检测到的车辆的形状保持不变。 第二个模型是恒定转弯率模型,表示检测到的车辆的转弯率保持不变,而其方向取决于转弯率。

下面您可以找到一个视频演示,其中解释了该软件包的一些早期功能。

安装和使用

这个ROS软件包可以通过以下方式安装:

从ROS仓库安装

如果您已安装ROS Melodic，可以通过以下方式安装：

sudo apt install ros-melodic-datmo

从源代码安装

1. 首先，导航到您的catkin工作空间的源文件夹。例如 cd ~/catkin_ws/src。
2. 运行

git clone git@github.com:kostaskonkk/datmo.git
cd ..
catkin_make

现在datmo包应该已安装到您的计算机上，在设置工作空间环境后即可使用。

source devel/setup.bash

您可以通过运行以下命令来演示DATMO包：

roslaunch datmo example.launch bag:=overtakes

您可以通过输入以下命令来运行它：

roslaunch datmo datmo.launch

ROS API

订阅的话题

scan(sensor_msgs/LaserScan) - 此话题应由您的激光雷达传感器创建。

发布的话题

此节点可以发布各种话题，但最终配置取决于用户。默认情况下，大多数话题都被禁用，应通过启动文件配置来启用。

datmo/marker_array (visualization_msgs/MarkerArray) - 在此话题中发布各种Rviz标记，有助于理解程序的内部工作原理。
datmo/box_kf (datmo/TrackArray) - 在此话题中发布使用恒速模型的卡尔曼滤波器输出，用于跟踪包围聚类的框的中心。\

注意：如果marker_array话题从机器人发布并在安装了不同ROS版本（Kinetic、Melodic等）的计算机上可视化，消息将无法发布，datmo节点将因md5sum错误而崩溃。要修复此错误，您应在机器人上安装与计算机上运行的ROS安装相同版本的visualization_msgs包。

自定义消息

此包使用两种自定义消息类型datmo/Track和datmo/TrackArray来方便发布结果。据我所知，在开发时没有标准的ROS消息可以完成相同的任务。

datmo/Track消息的结构如下：
int32 id - 对象ID，用于在跟踪过程中区分不同对象
float32 length - 对象的估计长度
float32 width - 对象的估计宽度
nav_msgs/Odometry odom - 对象的估计姿态

datmo/TrackArray消息是包含多个datmo/Track消息的数组，目的是实现高效发布。

Rviz标记

如果pub_markers标志设置为true，此包会发布可在Rviz中显示的可视化消息。发布以下消息：

closest_corner - 周围车辆的最近角点用黑色矩形可视化。
bounding_box_center - 边界框的中心用黄色矩形可视化。
velocities - 被跟踪对象的速度用箭头表示。\

参数

• "lidar_frame" ("string", 默认值: "laser") - LaserScan消息的变换框架（frame_id）名称
• "world_frame" ("string", 默认值: "world") - 世界坐标系的名称，如果不可用，此值可设置为与lidar_frame相同
• "threshold_distance" ("double", 默认值: "0.2") - 聚类算法使用的距离值
• "euclidean_distance" ("double", 默认值: "0.25") - 欧几里得距离数据关联算法使用的距离值
• "pub_markers" ("bool", 默认值: "false") - 是否发布可视化标记

使用者

如果您使用此包或在研究中发现它有用，欢迎将您的项目添加到此列表：

1. Fast and Safe Trajectory Optimization for Autonomous Mobile Robots Using Reachability Analysis. 2021. Vaskov, Sean. PhD Thesis.

参考文献

[1] D. Kim, K. Jo, M. Lee, and M. Sunwoo, "L-shape model switching-based precise motion tracking of moving vehicles using laser scanners," IEEE Transactions on Intelligent Transportation Systems, vol. 19, no. 2, pp. 598–612, 2018.
[2] X. Zhang, W. Xu, C. Dong, and J. M. Dolan, "Efficient l-shape fitting for vehicle detection using laser scanners," in 2017 IEEE Intelligent Vehicles Symposium (IV), pp. 54–59, IEEE, 2017.