A portable three-dimensional LIDAR-based system for long-term and widearea people behavior measurem
https://github.com/koide3/hdl_graph_slam
目录
Abstract
Introduction
Related Work
System Overview
Offlfline Environmental Mapping
Graph SLAM
Ground Plane Constraint
GPS Constraint
SLAM framework evaluation
Online People Behavior Measurement
Sensor Localization
People Detection and Tracking
Sensor Localization Evaluation
行为分析等
Abstract
测量和分析人的行为对于设计与人互动的系统是很重要的。本文介绍了一种使用三维激光雷达的便携式人的行为测量系统。在这个系统中，一个观察者携带着配备了3D激光雷达的系统，并跟踪要被测量的人，同时保持他们在传感器的视图中。该系统在三维环境地图中估计传感器的姿态，并跟踪目标人员。它支持长期和广泛的人员行为测量，这对现有的人员跟踪系统来说是困难的。作为现场测试，我们记录了在一家医院治疗老年痴呆症患者的专业护理人员的行为。对这些行为的初步分析揭示了照顾者如何在检查周围的人和环境的同时决定参与的位置。在分析结果的基础上，提出了辅助机器人行为设计的经验规则。
Introduction
在设计与人互动的系统时，测量和分析人的行为是很重要的。我们必须了解人们如何对周围的人和环境表现行为，以实现与人自然和丰富互动的系统。特别是对于服务机器人，通过分析一个正在帮助另一个人的人的行为，我们可以模拟他们的行为，并创造出一个具有类人行为的机器人。这使得机器人可以与人类进行自然的互动，并使它们在日常服务的情况下更容易被接受。
已经提出了几种描述人与人间社会互动的模型，如社会距离[1]和社会力量模型[2]，许多工作已经将这些模型应用于服务机器人[3,4,5]。但是，由于这些模型是基于对人与人之间距离的简单分析，因此它们不能描述周围环境与其他人的影响。这些限制可能会导致机器人在复杂情况下的非自然行为。为了实现一个具有自然和可接受行为的机器人，有必要测量人在不同情况下的行为，并构建一个复杂的交互行为模型。
有几个数据集提供了在室内[6]和室外环境[7,8]中的人的行为。然而，据我们所知，没有一个数据集提供涉及被跟踪者和被跟踪者之间互动的人们行为，即使这种情况在日常服务中非常常见。大多数现有的机器人只是保持与目标人的距离不变，而这种幼稚的跟随策略可能会让人们感到不舒服。
我们认为，有必要测量和分析服务员的行为，以设计服务员机器人的行为，它触发我们开发一个系统，使长期和广泛的人的行为测量，并创建一个数据集，由真正的专业人员的伴随行为数据组成。
图1展示了所提出的人的行为测量系统。该系统基于一个3D激光雷达，一个人类观察者携带该系统，并跟踪要被观察的人，同时保持他们在传感器的视图中。该系统同时估计三维环境地图中的传感器姿态，并跟踪目标人员。该系统可应用于长期、广域的人的行为测量任务。
图1，该系统被建议使用3D激光雷达来测量人们的行为。观察者用3D激光雷达携带背包，并跟踪要被测量的人。
图1展示了所提出的人的行为测量系统。该系统基于一个3D激光雷达，一个人类观察者携带该系统，并跟踪要被观察的人，同时保持他们在传感器的视图中。该系统同时估计三维环境地图中的传感器姿态，并跟踪目标人员。该系统可应用于长期、广域的人行为测量任务。
本文的贡献有三方面。
首先，我们提出了一种便携式测量系统，它可以实现长期和广域的人的行为测量。我们验证了该系统的跟踪精度与基于静态传感器的人跟踪系统相当。
其次，我们提供了在一家医院中对所提出的系统的现场测试的初步分析。我们记录了治疗老年痴呆症患者的专业护理人员的行为。结果表明，该系统可应用于真实人行为的测量。此外，基于分析结果，我们提出了经验规则来设计伴随机器人的行为。
第三，我们提供了系统的软件和记录的人的行为作为开源和公共数据集*。它们将有助于测量和分析在现有的人跟踪系统的情况下的人的行为。
本文的其余部分组织如下。以下部分解释相关工作。第三部分描述了拟议系统的概述。第四部分和第五部分分别描述了使用三维激光雷达的离线SLAM方法和包括传感器定位和人员跟踪的在线人员行为测量方法。第六部分解释了在医院进行的现场测试，并提供了对现场测试的初步分析。最后一部分总结了论文，并讨论了未来的工作。
Related Work
测量人类行为的系统可以分为两类： 1)使用固定在环境中的静态传感器的系统，以及2)使用附着于目标人身上的可穿戴传感器的系统。
人们使用静态传感器跟踪，如相机和激光瞄准仪，已经得到了广泛的研究。特别是，人们使用摄像机进行监控进行跟踪，是计算机视觉社区的一个主要研究课题。许多研究提出了使用RGB相机的[10]检测[9]和跟踪方法。最近廉价的消费者RGB-D相机使我们能够可靠地检测和跟踪人的[11]，而使用RGB-D相机跟踪人的相机网络系统已经被提出为[12]。虽然这样的工作提供了可靠的人跟踪，但恢复一个人的跟踪，谁离开相机视野，一次的能力是必要的。这个问题（即人的再识别）一直是基于视觉的人跟踪系统的主要研究课题之一。提出了许多基于人外观特征和软生物特征特征的再识别方法。它们使可靠的人能够随着时间的推移和通过摄像头重新识别身份。
激光测距仪也被用于人跟踪系统[19,20]。这样的系统可以非常准确地定位人，而且每个传感器的测量面积都比摄像机要大。虽然这些基于静态传感器的系统的可靠性和检测精度非常好，但它们只能在受传感器视图限制的区域内测量人的行为。为了覆盖一个大的环境，它们需要放置大量的静态传感器，因此增加了安装和校准所有传感器的时间和成本。
另一种在广阔区域内长期测量特定人行为的方法是在每个目标人身上安装一个可穿戴传感器，然后用该传感器测量他们的行为。几种传感器，如INS（惯性导航系统）和GPS（全球定位系统），已被用于这一目的。最近的小型可穿戴GPS传感器允许我们在户外环境中跟踪一个人，它们已经被应用于人的行为测量和分析[21,22]的一些应用。作为一个应用程序，基于GPS的可穿戴设备，以帮助老年人或视力受损者，已被提出为[23,24]。GPS和INS的结合提高了在低水平GPS无线电功率[25]下的跟踪精度。然而，在靠近建筑物和室内环境的地方，却没有GPS信号。
近年来，基于WiFi信号的[26,27,28]定位得到了广泛的研究。其中一些是基于WiFi信号强度的三角测量，在理想情况下显示分米或厘米精度。但是，它们需要在环境中放置多个天线来准确地估计设备的位置，因此很难应用于较大的环境。其他的是基于WiFi指纹匹配匹配[28]。虽然它们不依赖于外部天线，可以应用于WiFi信号可用的大型环境，但估计精度非常有限。
基于行人航迹推算的室内环境行为测量系统也被提出了[29,30]。这些方法通过积分INS（附于人）获得的加速度和角速度来估计目标人的位置。为了防止估计漂移，Li等人结合了行人航迹推算和基于地图的定位[29]。这些方法可以保持跟踪的人的位置，只要他们持有传感器。由于它们使用的智能手机近年来非常普遍和便宜，这些方法具有成本效益，易于采用。但是，由于INS是一个内部传感器，它不能感知周围的环境，所以很难准确地测量人的位置相对于环境和其他人的位置。因此，它们不能应用于衡量人与人之间的相互作用和受环境影响的人的行为的相互作用。
System Overview
图2显示了本文提出的系统的概述。在这个系统中，观察者携带配备了3D激光雷达（HDL-32e）和PC的背包，并跟踪要被测量的人。3D激光雷达提供10 Hz的360度范围数据，从范围数据中，系统在跟踪目标人员时估计其姿态。该系统的开发过程包括两个阶段：1）离线环境测绘 2）在线传感器定位和人员检测/跟踪。
在离线建图阶段，我们创建了一个覆盖整个测量区域的三维环境地图。对于建图，我们采用了一种基于图优化的SLAM方法（即图的SLAM [31 A tutorial on graph-based slam]）。为了补偿scan 匹配的累积旋转误差，我们分别引入了室内和室外环境下的地平面位置约束和GPS位置约束。在行为测量阶段，系统通过将扫描匹配算法与使用无迹的卡尔曼滤波器UKF[32]的基于角速度的姿态预测相结合，来估计其在离线创建的地图上的姿态(定位)。同时，该系统可以检测和跟踪目标人员。
Offlfline Environmental Mapping
Graph SLAM
图SLAM是解决SLAM问题最成功的方法之一。在该方法中，通过构造和优化一个图来解决该问题，其节点表示要优化的参数(状态)，如传感器姿态和地标位置，边表示约束，如传感器姿态和地标之间的相对姿态。对图进行了优化，使参数和约束之间的误差最小化。在[31,33]之后，设
为节点k的状态。设
和
为与相关的约束条件的均值和信息矩阵。目标函数的定义为：
其中，ek（，）是参数和约束之间的误差函数。通常等式(1)用高斯-牛顿或LM算法进行线性化和最小化。然而，如果参数跨越非欧几里得空间（如姿态参数），这些算法可能会导致次优或无效的解。处理这个问题的一种方法是在一个流形上进行误差优化，这是参数和的最小表示作为一个局部的欧几里得空间。为了实现它，引入了一个算子
，它变换了流形上的局部变化∆x,通常，在三维SLAM问题中，节点在k处有传感器姿态的参数（一个平移向量
和一个四元数
）。一个四元数![q_k=[q_w, q_x, q_y, q_z]^T](http://img.e-com-net.com/image/info8/c3f7c507a9514f748cf77f53210c57e8.gif){kind=small}
的流形可以被表示为![[q_x, q_y, q_z]^T](http://img.e-com-net.com/image/info8/571f6fb6384d4f1fb5f8049dc1efc255.gif){kind=small}
，算子被描述为：
其中
在该系统中，我们首先通过在连续帧间迭代应用NDT（正态分布变换）扫描匹配[34, Scan registration for autonomous mining vehicles using 3d-ndt]来估计传感器的轨迹。对于3D激光雷达，NDT在可靠性和处理速度[36]方面都优于其他扫描匹配算法，如迭代最接近点ICP[35]。设
为t处的传感器姿态，由一个平移向量t和一个四元数q组成，
为通过扫描匹配估计的t和t+1之间的相对传感器姿态。我们将它们作为节点[
、··、
]和边[
、··、
]添加到姿态图中。然后，我们在轨迹中找到闭环，并将它们作为边（即循环闭合）添加到图中，用算法1以校正的扫描匹配的累积误差。
闭环检测算法类似于[37 Blam - berkeley localization and mapping]。首先，我们根据节点之间的平移距离和轨迹长度来检测候选闭环（Line 2∼11）。然后，为了验证候选循环，在每个候选循环的节点之间应用了一个扫描匹配算法（在我们的示例中，是用NDT）。如果fifitness score得分低于一个阈值（例如，0.2），我们将该闭环作为节点之间的一条边添加到图中（Line 12∼17）。每次找到一个闭环时，位姿图都会被更新，使等式(1)是最小化的。我们利用g2o，一个超图优化[33]的通用框架，来进行位姿图优化。
随着生成的地图变大，由于帧匹配的累积旋转误差，它倾向于弯曲（见图7）。为了补偿误差，我们分别为室内和室外环境引入了地平面和GPS位置约束。图3显示了该系统的图示结构。
图3
图7
Ground Plane Constraint
为了可靠地生成一个大型室内环境的地图，我们假设环境有一个单一的平面，并引入地面约束来优化姿态图，使每次观测中检测到的地平面成为同一平面。这一假设在许多室内公共环境中都是有效的，如学校和医院。(PS：只能在实际场景也是水平面的地方适用，若出现斜坡则会出现问题）。
我们假设(雷达)传感器的近似高度是已知的（例如2m)，并且在一定高度范围内提取的点应该包含地面点（例如，[-1.0，+1.0]米）。然后，我们将RANSAC [38]应用于提取的点云，并检测基平面。如果检测平面的法线几乎是垂直（法线与单位垂直向量之间的夹角小于10度），我们认为地平面被正确检测，并在图中添加一个地平面约束边。图4显示了所检测到的地平面的一个示例。绿点是通过高度阈值提取的点，红点属于用RANSAC提取的地平面。
我们每10秒检测一次地平面，并将相应的传感器姿态节点
与固定的地平面节点连接起来，其中平面系数![\pi _0=[n_x,n_y,n_z,d]^T=[0,0,1,0]^T](http://img.e-com-net.com/image/info8/21a748f92e3149748e6d070729e44112.gif){kind=small}
为了计算传感器姿态与地平面
之间的误差，我们首先将地平面转换为传感器姿态的局部坐标：
为t时刻传感器的位姿，(3）式左边定义的是地平面在局部坐标系的表达。
在Ma的工作[39 Cpa-slam: Consistent plane model alignment for direct rgb-d slam]之后，我们采用最小参数化
，其中
分别是方位角、仰角角和截距的长度。姿态节点与地平面节点之间的误差定义为：
其中
为t时刻检测的地面。
GPS Constraint
在地面不平坦的室外环境中，我们使用基于GPS的位置约束，而不是地面平面约束。为了便于优化，我们首先将GPS数据转换为UTM（通用横向墨卡托）坐标，其中GPS数据在笛卡尔坐标中具有东、北和高。然后，每个GPS数据与姿态节点相关联，该姿态节点具有与GPS数据最接近的时间戳，作为先验位置信息的一元边。
姿态节点的平移向量
与GPS位置
之间的误差简单地表示为：
SLAM framework evaluation
为了验证所提出的SLAM系统，我们在室内环境中记录了一个三维点云序列。图5为实验环境和序列的运动轨迹。该序列的持续时间约为45分钟（2700秒），而轨迹的长度约为2400米（由所提出的方法估计）。为了进行比较，我们使用有和没有平面约束的方法生成了三维环境图。我们还将现有的公开使用的SLAM框架，BLAM [37]和LeGO-LOAM [40]，应用于这个数据集。
图7为不同SLAM算法估计的轨迹。BLAM和LeGO-LOAM在序列中间未能估计轨迹且没有恢复时在序列中间中止。由于扫描匹配的累积旋转误差，BLAM未能找到回路，产生了一个扭曲和不准确的轨迹。由于LeGO-LOAM保持了连续帧之间地平面的局部一致性，因此估计的轨迹比BLAM估计的轨迹更平坦。然而，由于缺乏全局地面约束，因此它仍然是存在累积的旋转误差。最终，当观察者在一条狭窄走廊的尽头进行u型转弯时，它未能估计出轨迹。
在有平面约束和没有平面约束的情况下，NDT的可靠性使该方法可以正确地构造姿态图，并生成一致的地图。然而，在没有平面约束的情况下，合成的地图由于累积的旋转误差而弯曲，这很难通过平面上的闭环来修正。在地平面约束下，修正了累积的旋转误差，得到的坐标图完全平坦。图6为生成的环境图。该颜色表示每个点的高度。由于平面约束，地面具有一致的高度。结果表明，所提出的平面约束能有效地补偿室内大环境下的累积旋转误差。
表1显示了该方法和BLAM的处理时间。LeGO-LOAM的处理时间在这里不可用，因为它只提供实时处理。BLAM生成地图大约需要15327秒，而由于NDT的计算效率，所提出的方法大约需要5392秒。
我们还在户外环境中验证了所提出的方法。图8 (a)显示了序列的环境和运动轨迹。该序列的持续时间约为42分钟（2500秒）。图8 (b)为该方法在GPS约束下生成的地图。虽然存在较大的波动，但由于GPS约束，系统正确地找到了闭环并构造了合适的姿态图。请注意，如果没有GPS约束，由于扫描匹配错误，系统无法找到该闭环，并且无法创建环境地图。
Online People Behavior Measurement
为了测量人的行为，该系统同时估计传感器在三维环境地图上的姿态，并跟踪观察者周围的人。图9为在线传感器定位和人跟踪系统的概述。通过整合激光雷达提供的角速度和距离数据，该系统估计了传感器的姿态。然后，它检测和跟踪人们，以了解人们对环境地图的位置。请注意，传感器的初始姿态是手工给出的，以避免全局定位问题。
Sensor Localization
我们可以通过迭代应用如在SLAM部分的扫描匹配算法来估计传感器的自我运动。然而，与SLAM场景相反，观察者在测量过程中必须跟踪目标人员，有时必须快速移动以将他们保持在传感器中的视野。
在这种情况下，帧间的传感器运动非常大，扫描匹配可能会由于大位移而错误地估计传感器的自我运动。为了解决这一问题，我们利用无迹卡尔曼滤波器[32]将NDT扫描匹配与三维激光雷达提供的角速度数据进行积分。
我们将要估计的传感器状态定义为：
![x_t=[p_t,q_t,v_t,b^a_t]^T](http://img.e-com-net.com/image/info8/2d50a4220f434a8ab7a6e75c15dc805b.gif){kind=small}
分别表示位置、姿态、速度、角速度bias。
假设传感器运动模型的平动速度恒定(即认为加速度为0)，而角速度传感器的bias恒定，则预测状态的系统方程定义为：
其中，
为两帧时间差，
为经过bias补偿的角速度值在内的旋转，它可以表示为：
,在时间比较短的时候
与通过等式(9)，系统利用UKF对传感器姿态进行预测，然后应用NDT将观测到的点云与估计的状态
和
作为传感器姿态的初始估计进行匹配。然后，系统利用扫描匹配![z_t=[x_t,q_t]^T](http://img.e-com-net.com/image/info8/f711c3d320e54e389493ae9388598b1b.gif){kind=small}
估计的传感器姿态对传感器状态进行校正。观测方程的定义为：
在每个预测和修正步骤后，我们对状态向量中的四元数进行归一化，以防止其范数因无迹变换累计计算误差而发生变化。
People Detection and Tracking
我们首先从观测点云中去除背景点，提取前景点。然后，我们从环境地图中创建一个具有一定体素大小（例如0.5m）的占用网格地图。根据UKF估计的传感器姿态，将输入点云转换为地图坐标，然后去除包含环境地图点的体素上的每个点作为背景。然后将欧氏聚类应用于前景点来检测人类候选聚类。但是，如果人靠得很近，他们的集群可能被错误地合并并被检测为单个集群。为了解决这个问题，我们采用了Haselich的分裂-合并聚类算法[41 Confifidence-based pedestrian tracking in unstructured environments using 3D laser distance measurements]。
该算法首先使用dp-means [42]将一个集群划分为子簇，直到每个集群小于一个阈值（例如，0.45m），这样每个集群就不会有不同的人的点。然后，如果这些子集群之间没有差距，则认为这些集群属于单个人，并重新合并到一个集群中。图10为检测结果为例。由于分离和重新合并过程，即使它们非常接近，人集群也会被正确地分离。
检测到的集群可能包含非人类集群（即假阳性）。为了消除检测到的聚类中的非人类聚类，我们通过使用Kidono等人[43]和成人st[44]的切片特征训练的人类分类器来判断聚类是否属于人类。假设人在地面上行走，我们在XY平面上跟踪没有高度的人。我们采用卡尔曼滤波器结合恒速模型和全局最近邻数据关联[45]来跟踪人。只要从传感器可见被跟踪人员并被正确检测，跟踪方案工作良好。
Sensor Localization Evaluation
为了展示姿态预测如何改进传感器定位，我们进行了一个传感器定位实验。图11为实验环境。一个观测者携带系统并沿着走廊移动，系统根据范围和角速度数据估计其姿态。我们做了两次实验。在第一次试验中，观察者会行走（约1.5 m/sec），以避免传感器被快速移动。在第二次试验中，观察者跑（约3.0 m/sec），传感器非常摇晃。
图12为第一次试验的结果。图12 (a)显示了有和没有位姿预测的估计轨迹。由于观察者在第一个序列中移动缓慢，这两个结果都显示了相同的正确轨迹。为了评估传感器姿态预测的效果，我们假设NDT估计的轨迹大多是正确的，并且我们将预测的传感器姿态与NDT估计的姿态进行比较，因为测量传感器轨迹的地面真值是困难的。
图12 (b)和(c)显示了预测的传感器姿态（初始估计姿态）与NDT估计的姿态之间的差异。在没有姿态预测的情况下，使用之前的匹配结果作为初始估计。通过预测，由于恒速模型和对角速度的考虑，平移和旋转位姿预测误差显著降低。第二次试验的结果如图13所示。系统在没有姿态预测的情况下无法估计传感器的姿态(见图13 (a))，因为观察者移动非常快，帧间的传感器位移越来越大。NDT匹配在没有进行姿态预测的情况下需要更长的时间（约每帧56毫秒），因为帧间的大位移使得NDT需要更多的迭代来收敛到局部解。通过预测，由于良好的初始估计（见表2），匹配每帧大约需要45毫秒。结果表明，基于角速度的姿态预测使姿态估计对快速运动具有鲁棒性和快速收敛能力。
下图为速度约1.5 m/sec移动
下图为速度约3.0 m/sec移动
表2：
行为分析等
略