）。这些资源可用于在现有人体跟踪系统难以处理的场景中测量和分析人类行为。
本文剩余部分结构安排如下：下一节介绍相关工作，第三节描述所提系统的整体概况，第四节和第五节分别介绍基于三维LIDAR的离线同步定位与建图（SLAM）方法，以及包含传感器定位和人体跟踪在内的在线人类行为测量方法，第六节介绍在医院开展的实地测试并给出初步分析，最后一节总结全文并讨论未来工作方向。
## 相关工作
人类行为测量系统可分为两类：(1) 使用固定在环境中的静态传感器的系统；(2) 使用附着在目标人物身上的可穿戴传感器的系统。
利用相机、激光测距仪等静态传感器进行人体追踪已得到广泛研究。其中，面向监控的基于摄像头的人体追踪是计算机视觉领域的一大核心研究课题。已有大量研究工作提出了基于RGB相机的人体检测$^{9}$与追踪方法$^{10}$。近年来低成本消费级RGB-D相机让我们能够可靠地检测和追踪人体$^{11}$，已有学者提出了基于RGB-D相机的人体追踪相机网络系统$^{12}$。尽管这类工作能够实现可靠的人体追踪，但我们仍需要具备恢复曾一度离开相机视野的行人轨迹的能力。该问题即行人重识别，一直是基于视觉的人体追踪系统的主要研究方向之一。目前已有大量基于行人外观$^{13-16}$和软生物特征$^{17,18}$的重识别方法被提出，这些方法能够实现跨时间、跨相机的可靠行人重识别。
激光测距仪也已被应用于人体追踪系统$^{19,20}$。这类系统能够非常精准地对行人进行定位，且单个传感器的测量范围大于相机。尽管这类基于静态传感器的系统具备出色的可靠性和检测精度，它们仅能在传感器视野覆盖的有限区域内测量行人行为。若要覆盖大范围环境，就需要部署大量静态传感器，这会增加所有传感器安装和校准的时间与成本。
若要在大范围内长时间测量特定行人的行为，另一种方案是为每个目标行人佩戴可穿戴传感器，通过该传感器测量其行为。已有多种传感器被用于该场景，例如惯性导航系统（INS）和全球定位系统（GPS）。近年来小型可穿戴GPS传感器让我们能够在户外
---
环境中对行人进行追踪，该技术已被应用于多项人体行为测量与分析场景中$^{21,22}$。作为一类应用，已有研究提出了基于GPS的可穿戴设备，用于帮助老年人或视障人士$^{23,24}$。GPS与INS结合能够提升GPS信号强度较低场景下的追踪精度$^{25}$。但在建筑周边区域和室内环境中，无法接收GPS信号。
近年来，基于Wi-Fi信号的定位技术得到了广泛研究$^{26-28}$。其中部分方法基于Wi-Fi信号强度三角定位，在理想场景下可达到分米甚至厘米级精度$^{26,27}$。但这类方法需要在环境中部署多个天线才能精准估计设备位置，因此很难应用于大范围环境。另一类方法基于Wi-Fi指纹匹配$^{28}$。尽管这类方法不依赖外部天线，可应用于所有覆盖Wi-Fi信号的大范围环境，但其估计精度非常有限。
目前也已有研究提出基于行人航位推算的室内环境行为测量系统。$^{29,30}$ 这类方法通过对佩戴在被测人员身上的惯性导航系统（INS）采集得到的加速度和角速度进行积分，来估计目标人员的位置。为了抑制估计漂移，Li等人将行人航位推算与基于地图的定位方法相结合。$^{29}$ 只要被测人员携带传感器，这类方法就可以持续跟踪其位置。由于这类方法所使用的智能手机近年来已十分普及且价格低廉，因此具备成本低、易推广的优点。但惯性导航系统属于内置传感器，无法感知周围环境，因此难以准确测量人员相对于环境的位置，以及人员与其他人员之间的相对位置。因此，这类方法无法应用于人员间交互的测量，也无法用于测量受环境影响的人员行为。
## 系统概述
图2展示了本文所提系统的整体架构。在该系统中，观测人员背负搭载了三维激光雷达（velodyne HDL-32e）和个人电脑的背包，跟随被测人员移动。该三维激光雷达以10 Hz的频率输出360°测距数据，系统通过测距数据在跟踪目标人员的同时估计自身位姿。本文所提系统的流程分为两个阶段：(1) 离线环境建图；(2) 在线传感器定位与人员检测/跟踪。
在离线建图阶段，我们构建覆盖整个测量区域的三维环境地图。建图过程采用基于图优化的SLAM方法（即图SLAM $^{31}$）。为了补偿扫描匹配累计产生的旋转误差，我们分别针对室内和室外环境引入了地平面约束和GPS位置约束。
Figure 2. System overview.
在行为测量阶段，系统将扫描匹配算法与基于无迹卡尔曼滤波（UKF）的角速度位姿预测相结合，估计自身在离线构建的地图上的位姿。$^{32}$ 同时，系统完成对目标人员的检测与跟踪。
## 离线环境建图
## 图SLAM
图SLAM是解决SLAM问题最成功的方法之一。该方法通过构建并优化一个图来求解SLAM问题：图的节点表示待优化参数，如传感器位姿和路标点位置，边表示约束，如传感器位姿与路标点之间的相对位姿。对图进行优化可以最小化参数与约束之间的误差。接下来，$^{31,33}$ 设$\mathbf{x}_k$为节点k，设$z_k$和$\mathcal{Q}_k$分别为与$\mathbf{x}_k$相关约束的均值和信息矩阵，目标函数定义为
$$ F(\boldsymbol{x})=\sum\boldsymbol{e}_{k}(\boldsymbol{x}_{k},\boldsymbol{z}_{k})^{T}\boldsymbol{\varOmega}_{k}\boldsymbol{e}_{k}(\boldsymbol{x}_{k},\boldsymbol{z}_{k}), $$
式中$e_k(x_k, z_k)$是参数$x_k$与约束$z_k$之间的误差函数。通常，会对式(1)进行线性化，再通过高斯-牛顿法或列文伯格-马夸尔特算法进行最小化求解。
但如果参数分布在非欧几里得空间（例如位姿参数），上述算法可能会得到次优解或无效解。解决该问题的一种方法是在流形上进行误差优化：流形是参数的最小表示，且在局部满足欧几里得空间的性质。为实现这一操作，本文引入算子$\boxplus$，用于变换流形上的局部变化$\Delta x$。
---
输入：$\mathcal{P} = \{p_0, \ldots, p_N\}$，位姿节点
输入：$\mathcal{R} = \{r_{0,1}, \ldots, r_{N-1,N}\}$，里程计边
输出：$\mathcal{L} = \{l_0, \ldots, l_M\}$，回环边
1: $\mathcal{L} \Leftarrow \{\}$
2: 对于 $i = 0 \ldots N - 1$ 执行
3: $\mathcal{C} \Leftarrow \{\}$ $\triangleright$ 回环候选
4: $accum\_d \Leftarrow 0$ $\triangleright$ 累积距离
5: 对于 $j = i + 1 \ldots N$ 执行
6: $d \Leftarrow \|\mathbf{p}_i.t - \mathbf{p}_j.t\|$
7: $accum\_d \Leftarrow accum\_d + d$
8: 如果 $d < th_d$ 且 $accum\_d > th_a$ 则
9: 将回环候选 $l = \{p_i, p_j\}$ 添加到 $\mathcal{C}$
10: 结束如果
11: 结束循环
12: 对于 $l = \{p_i, p_j\}$ 中的 $\mathcal{C}$ 执行
13: $m \Leftarrow scan\_matching(p_i, p_j)$
14: 如果 $m.score < th_s$ 则
15: $\mathcal{L} \Leftarrow \mathcal{L} \cup \{l\}$
16: 结束如果
17: 结束循环
18: 结束循环
通常，在三维SLAM问题中，节点$x_k$包含k时刻传感器位姿的参数（平移向量$\boldsymbol{t}_k$和四元数$\boldsymbol{q}_k$）。四元数$\boldsymbol{q}_k = [q_w, q_x, q_y, q_z]^T$构成的流形可以表示为$[q_x, q_y, q_z]^T$，算子$\boxplus$可描述为
$$ \boldsymbol{q}_{k}\boxplus\Delta\boldsymbol{q}=\left[\sqrt{1-\|\boldsymbol{q}_{x}^{\prime}+\boldsymbol{q}_{y}^{\prime}+\boldsymbol{q}_{z}^{\prime}\|^{2}},q_{x}^{\prime},q_{y}^{\prime},q_{z}^{\prime}\right] $$
其中 $q_{*}^{\prime}=q_{*}-\Delta q_{*}$
在所提系统中，我们首先通过在连续帧之间迭代应用正态分布变换（normal distributions transform, NDT）扫描匹配$^{34}$来估计传感器轨迹。对于三维激光雷达，NDT在可靠性和处理速度两方面的性能都优于其他扫描匹配算法（如迭代最近点算法$^{35}$）。$^{36}$ 设$p_t$为t时刻的传感器位姿，由平移向量t和四元数$\boldsymbol{q}$组成，$r_{t,t+1}$为扫描匹配估计得到的t时刻与$t+1$之间的相对传感器位姿。我们将它们作为节点$[p_0, \cdots, p_N]$和边$[r_{0,1}, \cdots, r_{N-1,N}]$添加到位姿图中。随后，我们在轨迹中检测回环，并将其作为边（即回环闭合）添加到图中，通过算法1校正扫描匹配的累积误差。
本文的回环检测算法与Nelson的研究思路类似。$^{37}$ 首先，我们基于节点间的平移距离和轨迹长度检测回环候选（第2–11行）。随后，为了验证回环候选，我们对每个候选的两个节点之间应用扫描匹配算法（本文使用NDT）。如果匹配得分低于阈值（例如0.2），我们就将该回环作为节点间的边添加到位姿图中（第12–17行）。每次检测到回环后，位姿图都会
Figure 3. Comparison of the sensor trajectories estimated by the existing method and the proposed method. (a) BLAM. (b) LeGO-LOAM. (c) Ours without plane constraints. (d) Ours with plane constraints.
更新，以使得式(1)最小化。我们采用g2o这一超图优化通用框架$^{33}$进行位姿图优化。
随着生成地图的规模增大，扫描匹配的累积旋转误差会导致地图发生弯曲（参见图3）。为了补偿该误差，我们分别针对室内和室外环境引入了地平面约束和GPS位置约束。图4为本文所提系统的图结构示意图。
## 地平面约束
为了可靠地生成大型室内环境的地图，我们假设环境存在单一平整地面，并引入地平面约束来优化
---
Figure 4. The proposed pose graph structure.
Figure 5. Ground plane detection. Points within a certain height range are extracted by height thresholding (green points), and then RANSAC is applied to them to detect the ground plane (red points). The horizontality of the ground plane is validated by checking the plane normal.
构建位姿图，使得每次观测中检测到的地平面对应同一平面。该假设在学校、医院等诸多室内公共环境中均成立。
我们假设传感器的近似高度已知（例如2米），并提取特定高度范围内的点，地平面点应当包含在该范围内（例如距离地面$(-1.0, +1.0)$米）。随后，我们对提取出的点云应用RANSAC $^{38}$检测地平面。如果检测到平面的法向量接近竖直（法向量与单位竖直向量的夹角小于$10^\circ$），我们则认为地平面检测正确，并向图中添加一条地平面约束边。图5展示了一个地平面检测的示例。绿色点是通过高度阈值提取的点，红色点属于经RANSAC检测得到的地平面。我们每10秒检测一次地平面，并将对应的传感器位姿节点$\boldsymbol{p}_i$与固定地平面节点相连，该节点的平面系数为$\boldsymbol{\pi}_0 = [n_x, n_y, n_z, d]^T = [0, 0, 1, 0]^T$。
为了计算传感器位姿$\boldsymbol{p}_t$与地平面$\boldsymbol{\pi}_0$之间的误差，我们首先将地平面变换到传感器位姿$\boldsymbol{p}_t$的局部坐标系下
$$ [n_{x}^{\prime},n_{y}^{\prime},n_{z}^{\prime}]^{T}=\boldsymbol{R}_{t}\cdot[n_{x},n_{y},n_{z}]^{T} $$
$$ \boldsymbol{d}^{\prime}=\boldsymbol{d}-\boldsymbol{\mathbf{t}}_{t}\cdot[n_{x}^{\prime},n_{y}^{\prime},n_{z}^{\prime}]^{T} $$
其中$\pi'_{0} = [n'_{x}, n'_{y}, n'_{z}, d']$为局部坐标系下的地平面，$[R_{t}|t_{t}]$为时刻t的传感器位姿。
遵循Ma等人$^{39}$的工作，我们采用最小参数化$\tau(\pi)=(\phi,\psi,d)$，其中$\phi,\psi$、d分别为方位角、仰角和截距长度。位姿节点与地平面节点之间的误差定义为
$$ \tau(\pi)=\left[\arctan\left(\frac{n_{y}}{n_{x}}\right),\arctan\left(\frac{n_{z}}{|n|}\right),d\right] $$
$$ \boldsymbol{e}_{i,0}=\boldsymbol{\tau}(\boldsymbol{\pi}_{0}^{\prime})-\boldsymbol{\tau}(\boldsymbol{\pi}_{t}) $$
其中$\pi_{t}$为时刻t检测到的地平面。
## GPS约束
在地面不平坦的室外环境中，我们使用基于GPS的位置约束替代地平面约束。为了便于优化，我们首先将GPS数据转换为通用横轴墨卡托坐标系，在该笛卡尔坐标系中，每个GPS数据包含东向坐标、北向坐标和海拔值。随后，每个GPS数据会与时间戳最接近的位姿节点相关联，作为先验位置信息的一元边。
位姿节点$t_{t}$的平移向量$p_{t}$与GPS位置$T_{t}$之间的误差可简单表示为
$$ \boldsymbol{e}_{i}=\boldsymbol{t}_{t}-\boldsymbol{T}_{t} $$
## SLAM框架评估
为了验证本文提出的SLAM系统，我们在室内环境中记录了一组三维点云序列。图6展示了实验环境与该序列的轨迹。序列时长约为45分钟（2700秒），轨迹长度约为2400米（由本文提出的方法估计得到）。
为了进行对比，我们分别使用本文提出的带平面约束和不带平面约束的方法生成了三维环境地图。我们还将现有的公开SLAM框架BLAM $^{37}$和LeGO-LOAM $^{40}$应用于该数据集。
图3展示了不同SLAM算法估计得到的轨迹。BLAM和LeGO-LOAM在序列中途因估计轨迹失败且无法恢复而中断运行。由于扫描匹配累积的旋转误差，BLAM无法找到回环，生成了扭曲且不准确的轨迹。由于LeGO-LOAM保持了
---
Figure 6. The experimental environment. The duration of the sequence is about 45 min, and the length of the trajectory is about 2400 m.
Figure 7. The created environmental map. The color indicates the height of each point. The height of the floor is consistent thanks to the plane constraint.
在连续帧的地平面假设下，估计得到的轨迹比BLAM估计的轨迹更平缓。但由于缺乏全局地面约束，该方法仍然存在累积旋转误差问题。最终，当观测者在狭窄走廊末端掉头时，该方法无法估计出有效轨迹。
无论是否添加平面约束，得益于NDT的可靠性，本文提出的方法都可以正确构建位姿图，并生成一致的地图。但如果没有平面约束，累积旋转误差会导致最终生成的地图发生扭曲，且平面上的回环很难校正该误差。添加地平面约束后，累积旋转误差得到校正，最终生成的地图完全平整。图7展示了生成的环境地图，颜色表示每个点的高度，得益于平面约束，地板的高度保持一致。结果表明，本文提出的平面约束能够有效补偿大尺寸室内环境中的累积旋转误差。
表1展示了本文提出方法与BLAM的处理时间。LeGOLOAM的处理时间在此无法获取，因为它仅支持实时处理。BLAM生成地图耗时约15327秒，而得益于NDT的计算效率，本文提出方法仅耗时约5392秒。
我们还在室外环境中验证了本文提出的方法。图8(a)展示了实验环境和该序列的轨迹，序列时长约为42分钟（2500秒）。图8(b)展示了本文方法添加GPS约束后生成的地图。尽管环境存在较大起伏，得益于GPS约束，系统仍能正确检测回环并构建合理的位姿图。需要注意的是，如果没有GPS约束，扫描匹配误差会导致系统无法检测回环，最终无法生成环境地图。
## 在线人员行为测量
为了测量人员行为，该系统同时估计三维环境地图中的传感器位姿，并跟踪观测者周围的人员。图9展示了在线传感器定位与人员跟踪系统的整体架构。通过集成LIDAR提供的角速度和测距数据，系统估计传感器位姿。随后，系统检测并跟踪人员，得到人员相对于环境地图的位置。需要说明的是，为了避免全局定位问题，传感器的初始位姿由人工给定。
---
Table I. Processing time of BLAM and our SLAM system.
| Method | | Time (s) |
| Ours | Scan matching | 1542 |
| Floor detection | 231 |
| Loop closing | 3619 |
| Total | 5382 |
| BLAM | Total | 15,327 |
(a)
(b)
Figure 8. The SLAM system validation in an outdoor environment. (a) The outdoor environment. The duration of the sequence is about 42 min, and the length of the trajectory is about 3000 m. (b) The 3-D map of the outdoor environment generated by the proposed method with GPS constraints. The color indicates the height of each point. 3-D: three-dimensional; GPS: global positioning system.
## 传感器定位
我们可以像SLAM部分那样，通过迭代应用扫描匹配算法来估计传感器自身运动。但与SLAM场景不同的是，观测者在测量过程中需要跟随目标人员，有时必须快速移动才能将目标保持在传感器视野内。在这类情况下，帧间传感器运动幅度很大，大位移会导致扫描匹配错误估计传感器自身运动。为了解决该问题，我们通过UKF将NDT扫描匹配与三维LIDAR提供的角速度数据进行融合。 $^{32}$
我们将待估计的传感器状态定义为
$$ \boldsymbol{x}_{t}=[p_{t},\boldsymbol{q}_{t},\boldsymbol{v}_{t},\boldsymbol{b}_{t}^{a}]^{T} $$
其中$p_t$是位置，$q_t$是旋转四元数，$v_t$是速度，$\boldsymbol{b}_t^a$是t时刻传感器角速度的偏差。假设传感器运动模型满足平移速度恒定，角速度传感器偏差恒定，用于状态预测的系统方程定义为
$$ \boldsymbol{x}_{t}=[\boldsymbol{p}_{t-1}+\Delta t\cdot\boldsymbol{v}_{t-1},q_{t-1}\cdot\Delta\boldsymbol{q}_{t},\boldsymbol{v}_{t-1},\boldsymbol{b}_{t-1}^{a}]^{T} $$
其中，$\Delta t$ 是t与$t-1$之间的时间间隔，$\Delta q_t$ 是经偏差补偿的角速度$\Delta t$在$\boldsymbol{a}_t' = \boldsymbol{a}_t - \boldsymbol{b}_{t-1}^a$时间段内产生的旋转量。
$$ \Delta\boldsymbol{q}_{t}=\left[1,\frac{\Delta t}{2}a_{t}^{x^{\prime}},\frac{\Delta t}{2}a_{t}^{y^{\prime}},\frac{\Delta t}{2}a_{t}^{z^{\prime}}\right]^{T} $$
结合式(9)，系统采用UKF预测传感器位姿，随后将估计得到的$\boldsymbol{x}_t$和$\boldsymbol{q}_t$作为传感器位姿的初始猜测，应用NDT将观测点云与全局地图进行匹配。之后，系统利用扫描匹配$\boldsymbol{z}_t = [\boldsymbol{p}_t', \boldsymbol{q}_t']^T$估计得到的传感器位姿对传感器状态进行校正。观测方程定义为
$$ \mathbf{z}_{t}=[\boldsymbol{p}_{t},\boldsymbol{q}_{t}]^{T} $$
我们在每一次预测步骤和校正步骤之后，都会对状态向量中的四元数进行归一化处理，避免其范数因无迹变换和累积计算误差发生改变。值得一提的是，我们也实现了考虑加速度的位姿预测，但由于加速度观测存在强噪声，估计结果反而变差。
## 行人检测与跟踪
我们首先从观测点云中移除背景点，提取出前景点。随后，我们基于环境地图，以特定体素大小（例如0.5 m）构建占据栅格地图。根据UKF估计得到的传感器位姿，将输入点云变换到地图坐标系下，再将落在包含环境地图点的体素中的所有点作为背景移除。之后对前景点采用欧氏聚类，检测出待选人体聚类。但如果多个行人距离较近，他们的聚类可能会被错误合并，检测为单个聚类。为了解决这个问题，我们采用了Haselich的分合聚类算法。$^{41}$
该算法首先通过dp-means $^{42}$将一个聚类不断划分为子聚类，直到每个子聚类的尺寸都小于阈值（例如0.45 m），从而保证每个聚类都不包含
---
Figure 9. The online sensor pose estimation and people detection and tracking system.
Figure 10. Haselich's clustering algorithm. The green bounding box indicates the Euclidean clustering result. Two persons are wrongly detected as a single cluster. The cluster is divided into small subclusters (red bounding boxes) and then remerged if there is no gap between those subclusters. The blue bounding boxes are the final detection result. (a) Top view. (b) Bird's eye view.
不同行人的点。随后，如果这些子聚类之间不存在间隔，则认为它们属于同一个行人，重新合并为一个聚类。图10给出了一个检测结果示例。得益于分合处理流程，即使行人距离非常近，他们的聚类也能被正确分离。
检测得到的聚类中可能包含非人体聚类（即误检）。为了剔除已检测聚类中的非人体聚类，我们采用Kidono等人$^{43}$以及Schapire和Singer$^{44}$提出的、基于切片特征训练的人体分类器，判断聚类是否对应人体。假设行人在地面平面行走，我们在不考虑高度的XY平面上对行人进行跟踪。我们采用恒速模型卡尔曼滤波与全局最近邻数据关联$^{45}$相结合的方法跟踪行人。该跟踪方案只要满足以下条件就能良好工作：
Figure 11. The experimental environment of the sensor localization experiment.
被跟踪的行人处于传感器可视范围内，且被正确检测。
## 传感器定位评估
为了说明位姿预测如何提升传感器定位性能，我们开展了传感器定位实验。图11展示了实验环境：一名测试人员携带系统沿走廊移动，系统通过距离和角速度数据估计自身位姿。我们共进行了两次实验：第一次实验中，测试人员步行移动（约1.5 m/s），避免传感器快速运动；第二次实验中，测试人员跑步移动（约3.0 m/s），传感器受到的震动非常强烈。
图12展示了第一次试验的结果。图12(a)给出了使用和不使用位姿预测时的估计轨迹。由于观测者在第一个序列中移动缓慢，两种方法都得到了相同的正确轨迹。为了评估传感器位姿预测的效果，
---
(a)
(b)
(c)
(d)
Figure 12. The results of the first trial of the sensor localization experiment. The observer walked during the trial (about 1.5 m/s). Both the trajectories with and without the angular velocity-based pose prediction are correctly estimated. With the prediction, the initial guess for NDT significantly gets closer to the correct pose. (a) Estimated trajectories. (b) Difference between the predicted and the corrected positions. (c) Difference between the predicted and the corrected rotations. (d) Processing time. NDT: normal distributions transform.
由于很难获取传感器轨迹的真值，我们假设NDT估计得到的轨迹基本正确，并将预测的传感器位姿与NDT估计的位姿进行对比。图12(b)和(c)展示了预测传感器位姿（初始猜测位姿）与NDT估计位姿之间的差值。在不使用位姿预测的情况下，我们将上一次的匹配结果作为初始猜测。使用位姿预测时，平移和旋转位姿预测误差分别因恒速模型和对角速度的考量而显著降低。
第二次试验的结果如图13所示。由于观测者移动速度极快，帧间传感器位移变大，不使用位姿预测时系统无法估计出传感器位姿（参见图13(a)）。不使用位姿预测时，NDT匹配需要更长时间（每帧约56毫秒），因为帧间大位移使得NDT需要更多
(a)
(b)
(c)
(d)
Figure 13. The results of the second trial of the sensor localization experiment. The observer ran during the trial (about 3.0 m/s). Without the pose prediction, the system could not correctly estimate the pose due to the very quick motion. (a) Estimated trajectories. (b) Difference between the predicted and the corrected positions. (c) Difference between the predicted and the corrected rotations. (d) Processing time.
Table 2. The summary of the sensor localization experiment.
| With prediction | Without prediction |
| Seq. | Error (m) | Error ( $^{\circ}$) | Time (ms) | Error (m) | Error ( $^{\circ}$) | Time (ms) |
| st (walk) | 0.0588 | 1.0913 | 38.88 | 0.1367 | 2.1625 | 40.06 |
| nd (run) | 0.1851 | 4.2845 | 45.14 | 0.3330 | 6.6798 | 56.11 |
次迭代才能收敛到局部解。使用位姿预测时，得益于良好的初始猜测，匹配每帧仅需约45毫秒（参见表2）。结果表明，基于角速度的位姿预测让位姿估计对快速运动具备鲁棒性，且收敛速度更快。
## 行人检测评估
为了分析分拆-合并聚类$^{41}$和人体分类器，$^{43}$我们记录了一段三维距离数据序列，
---
Table 3. The people detection evaluation result.
| $Split-merge_{{41}}$\nclustering | $Human classifier^{{43}}$ | Precision | Recall | F-measure |
| Without | Without | 1.000 | 0.834 | 0.909 |
| Without | With | 1.000 | 0.809 | 0.894 |
| With | Without | 0.902 | 0.995 | 0.946 |
| With | With | 0.961 | 0.961 | 0.961 |
序列中有两人并肩近距离行走。对于常规的欧氏聚类来说，这是一个困难场景，因为两个人的点云簇可能会被合并成一个。该序列共102帧，我们分别将启用和禁用分拆-合并聚类与人体分类器的行人检测方法应用到该序列上。
表3展示了评估结果。不使用这两种技术时，召回率较低（0.834），原因是欧氏聚类有时会将两个人的簇误检测为一个。使用分拆-合并聚类后，错误合并的簇会被拆分为子簇，召回率得到提升（0.995）。同时使用分拆-合并聚类和人体分类器时，过度拆分产生的子簇会被分类器剔除，最终得到最高的F值（0.961）。该结果表明，在行人距离较近的场景下，分拆-合并聚类$^{41}$能够有效提升行人检测的召回率，将其与人体分类器结合后，$^{43}$我们可以得到可靠的行人检测结果。
## 与基于静态传感器的行人追踪系统对比
为了明确本文所提出系统的优缺点，我们将该系统与公开可用的基于静态传感器的行人追踪框架OpenPTrack进行了对比。$^{12}$该框架针对使用静态RGB-D相机的行人追踪设计，可扩展至大型相机网络。此外，它采用的硬件性价比高，易于安装设置，已经被包括计算机视觉非专业人士（如艺术家和心理学家）在内的各类人员使用。
图14展示了实验环境与RGB-D相机网络的配置，地图由本文提出的SLAM方法构建。我们共布置了9台Kinect v2相机，以覆盖约$2 \times 20 \, m^2$的区域。我们按照OpenPTrack提供的流程对相机网络完成标定，随后通过Kinect点云与环境地图之间的ICP配准，估计出了环境地图与相机网络之间的变换关系。
当一名受试者在走廊中行走时，携带本文所提系统的观测人员跟随在他身后。本文系统与OpenPTrack分别测量了两人的运动轨迹。表4统计了本文系统与OpenPTrack测得的人员位置之间的差值。由于检测
Figure 14. The experimental environment and the configuration of RGB-D cameras for OpenPTrack. Nine Kinect v2s are placed in the corridor. While OpenPTrack can measure only the limited area covered by cameras (about $2 \times 20 \, m^2$ area), the proposed system can cover the whole of the floor.
Table 4. The difference of the observer and the subject positions measured by the proposed system and OpenPTrack.
| Difference (m) |
| Min | Max | Mean | Standard deviation |
| Observer | 0.0008 | 0.2126 | 0.0768 | 0.0448 |
| Subject | 0.0035 | 0.2837 | 0.0990 | 0.0445 |
误差出现在OpenPTrack的视场边缘，差值有时会变大（约0.2–0.3 m）。但平均差值低于0.1 m，结果表明本文所提系统的测量精度与基于静态传感器的人员追踪系统相当。
综上，本文所提便携式系统的追踪精度与基于静态传感器的系统相当，且该系统的测量区域可以轻松扩展。例如，该系统可以测量图7所示整个地图区域内的人员行为（$200 \times 50 \, m^2$）。如果在该环境中使用基于静态传感器的系统覆盖整个地图区域，将需要数百台相机。另一方面，基于静态传感器的系统可以同时测量覆盖区域内所有人员的行为，而本文所提系统仅能覆盖周边区域。因此我们可以得出结论：本文所提系统适用于测量大范围内特定人员的行为，而基于静态传感器的系统更适用于测量相对较小环境中所有人员的行为。
## 医院现场测试
## 测量照护老年人群的护理人员行为
为证明本文所提系统可应用于实际人员行为测量，我们在
---
(a)
Figure 15. A snapshot of the field test. The behavior of the care giver attending an elderly is recorded by using the proposed system. (a) Image. (b) Range data.
泽良比开福新村医院（Sawarabikai Fukushimura hospital）完成了现场测试。该医院是一所专门开展老年护理的机构，有数百名老年患者在此住院接受护理和康复治疗。在获得医院许可后，我们记录了专业护理人员照护痴呆老年人群过程中的行为。图15是本次现场测试的快照：护理人员看护老年人，预防绊倒、碰撞、跌倒等意外，有时还会将他们引导回房间。
本次实验共获得33段行为序列，总时长约52分钟。我们还在图8所示的室外环境记录了一段陪护行为序列，该室外序列时长约22分钟。请注意，出于隐私保护考虑，我们仅在获得医院、受试者及其家属的特殊许可后，才对图15所示序列拍摄了图像，其余序列仅记录了距离数据。本文所提系统的一大优点就是能够在测量人员行为的同时避免隐私问题。
图16展示了通过现场测试构建的室内环境地图。老年人会在一楼餐厅休息，之后由护理人员陪同乘坐电梯返回二楼的病房。在他们乘坐电梯后，我们会将使用的地图从一楼地图切换为二楼地图。
测量过程中，场景内还存在其他患者以及轮椅、药架等物体，观测人员有时需要快速移动才能将受试者保持在传感器视野内。但得益于3-D LIDAR的宽测量范围，以及扫描匹配与基于角速度的位姿预测的融合，本文提出的系统在所有序列中都能完成正确的自定位。
在人员跟踪方面，当有患者走到观测人员与待观测受试者之间时，系统会丢失对受试者的跟踪，受试者重新出现后会被分配新的ID。遇到这类情况时，系统会发出丢失目标跟踪的提示，再由我们手动为受试者重新分配正确的ID。由于这类情况仅少数发生，系统在大部分序列中都能持续跟踪受试者，我们只需要花费极小的工作量就能完成所有ID的重新分配。
(a)
(b)
Figure 16. The environments of the field test. (a) Hallway (IF). (b) Ward (2F).
## 照护行为的初步分析
为了验证本文提出的系统用于行为分析的可行性，我们对测量得到的行为序列开展了初步分析。
图17(a)展示了室内环境中照护者与老年人之间距离的分布。该分布为单峰分布，峰值约在0.6米处。在空间关系学中，该距离被归类为“个人距离（0.45–1.2米）”，人们仅允许熟悉的人进入这一距离范围$^{1}$，而在与陌生人见面或互动时会保持更远的距离（即“社交距离（1.2–3.6米）”）。这表明，与见面这类普通人际互动相比，照护过程中双方会保持更近的距离。图17(b)展示了照护者相对于老年人的位置分布。照护者通常位于老年人的侧方，为了引导老年人，他们会稍微走在患者前方。该分布存在一定的各向异性：当照护者跟在老年人后方时，双方之间的距离往往更大，因为照护者需要同时观察老年人和周边环境。从这一初步分析可以发现，照护者会根据“将老年人保持在视野内”和“观察前方环境”的需求来选择自己的照护位置。
图18(a)展示了拐角处照护者和老年人的运动轨迹，该结果也印证了可视性对照护位置选择的重要性。本次共统计了17条轨迹，照护者倾向于
---
(a)
(b)
Figure 17. An analysis of the people attending behavior during the field test in an indoor environment. (a) The distribution of the distance between the elderly person and the caregiver. (b) The distribution of the relative position of the caregiver with respect to the elderly person.
(a)
(b)
(c)
Figure 18. The trajectories of the caregivers (in orange) and the elderly persons (in green) at a corner. The light blue lines indicate that the connected points are measured at the same time. In most of the cases, the caregivers walked on the outer side of the corner (15 of 17). In a few cases, the caregivers walked on the inner side. In such cases, they preceded the elderly persons to ensure outlook of the corridor (2 of 17). (a) All the trajectories of the caregivers and the elderly person. (b) An example of the cases where the caregiver walks on the outer side of the corner. (c) The case where the caregiver walks on the inner side of the corner.
走在拐角的外侧（17条中有15条）。我们认为，照护者走在外侧可以保持对走廊的开阔视野，预防绊倒、碰撞等意外。少数情况下（17条中有2条）照护者会走在内侧，但此时他们会走在老年人前方，以便在老年人进入拐角前提前确认安全。这些结果表明，照护者会时刻留意周围其他人员和轮椅等物体的情况，以预防事故发生。
图19(a)展示了在室外环境中记录的运动轨迹。在这组行程中，老人行走状态良好，陪护人员在引导老人返回医院的过程中，允许老人相对自由地活动。图19(b)展示了陪护人员的步行速度以及其位置在全局地图中的海拔高度。当陪护人员（和老人）上坡时，他们会将速度降至1.0–1.2 m/s，而在下坡时步行速度为1.2–1.4 m/s。坡度不仅会影响步行速度，还会影响两人的位置关系。我们分别提取了他们在上坡和下坡时的行为，计算了陪护人员相对于老人的位置分布（见图20）。可以看出，在下坡时老人走在陪护人员前方，而在上坡时由于步行速度的变化，两人并肩行走。尽管陪护人员的“X轴”位置会随步行速度变化，但他/她几乎始终保持在老人身侧0.6米的位置。这一现象在室内环境中也能观察到（见图17）。上述结果表明，专业陪护人员在陪护过程中，会根据老人的状态和周边环境调整自身位置，同时保持与老人的侧身距离恒定。这一结论可应用于行人跟随
---
机器人的设计中。现有的大多数行人跟随机器人仅保持与目标的距离恒定。然而，这对人来说可能是不自然的行为。我们可以让机器人保持
(a)
Figure 19. The recorded attendant behavior in the outdoor environment. (a) People trajectory. (b) The caregiver's walking speed (green) and altitude (blue).
与目标的侧身距离恒定，这有助于提升机器人跟随行为的自然度。
使用现有测量系统（依靠静态传感器或INS、GPS这类可穿戴设备）很难得到这类分析结果，因为研究需要精准测量人与人以及人与周边环境之间的相对行为。研究结果表明，本文提出的系统能够捕获和分析这类人际行为。
## 行人跟随行为规则
基于对真实陪护人员行为的分析，我们针对陪护机器人的行为设计提出了经验规则。这有助于设计能够陪护人员、帮助人员远离危险场景的机器人。
1. 在条件允许的情况下，机器人应保持与被陪护人员并肩的位置进行陪护。具体而言，机器人应保持在被陪护人员身侧0.6米的位置。
2. 相对位置会随步行速度沿前后方向偏移，但即使在这种情况下，机器人仍应保持与被陪护人员恒定的侧身距离。
3. 在拐角处，机器人应走在拐角的外侧，这样既可以检查走廊的安全性，又不会干扰被陪护人员。
4. 如果由于位置限制或障碍物导致机器人无法走到外侧，那么它应在被陪护人员进入拐角前走到内侧，检查环境是否安全。这种情况会对被陪护人员的行走造成轻微干扰，但安全的优先级高于舒适度。
5. 要陪护行走状态良好的人员，机器人需要能够以约1.4 m/s的速度移动。
(a)
(b)
Figure 20. The distribution of the relative position of the care giver with respect to the elderly person in an outdoor environment. (a) Up slopes. (b) Down slopes.
---
需要注意的是，规则中的数值（例如与被陪护人员的距离）需要根据机器人的配置（如尺寸、形状）进行调整。但我们认为，这些规则可为设计符合社会接受度、舒适易用的陪护机器人提供良好的初始指导。
## 结论与讨论
本文介绍了一套使用3-D LIDAR的便携式人员行为测量系统。该系统可支持长期、广域范围的行为测量。系统会先在测量前使用Graph SLAM方法提前构建环境的三维地图，随后同时完成自身位姿估计、人员检测与人员跟踪。该系统的跟踪精度与基于静态传感器的人员跟踪系统相当。作为现场测试，我们验证了所提系统在测量专业护理人员照护老年人行为方面的有效性。基于对测量得到的行为数据的分析，本文提出了设计陪护机器人行为的经验规则。该测量系统与专业护理人员行为数据集已公开，可用于人员陪护行为的测量与分析研究。
当前系统需要人类观测者背负搭载3-D LIDAR的背包，因此观测人员仍需投入人工工作量。未来可将人类观测者替换为移动机器人，从而自动生成大规模陪护行为数据集，用于优化机器人的陪护行为。
## 致谢
作者谨向O. Kohashi、S. Yamamoto和T. Gomyo致谢，感谢他们允许我们在Swarabikai Fukushimura医院开展现场测试，并在测试过程中提供了极佳的配合。
## 利益冲突声明
作者声明，本文在研究、作者署名和/或发表方面不存在潜在利益冲突。
## 资助
作者披露，本文的研究、作者署名和/或发表获得了以下经费支持：本工作部分得到了JSPS Kakenhi No. 25280093以及MEXT顶尖研究生院计划R03的支持。
## ORCID iD
Kenji Koide 🆔
## 参考文献
1. Hall E. The hidden dimension: Man's use of space in public and private. 英国伦敦：Doubleday anchor books, Bodley Head，1969。ISBN .
[^2]: Helbing D and Molnar P. Social force model for pedestrian dynamics. Phys Rev E 1995; 51(5): 4282. DOI: 10.1103/PhysRevE.51.4282.
[^3]: Ferrer G, Garrell A, and Sanfeliu A. Robot companion: a social-force based approach with human awareness-navigation in crowded environments. In: IEEE/RSJ International Conference on Intelligent Robots and Systems, Tokyo, Japan, 6 January 2013, pp. 1688–1694. IEEE. DOI: 10.1109/IROS.2013.6696576.
[^4]: Ferrer G and Sanfeliu A. Proactive kinodynamic planning using the extended social force model and human motion prediction in urban environments. In: IEEE/RSJ International Conference on Intelligent Robots and Systems, Chicago, USA, 14 September 2014, pp. 1730–1735. DOI: 10.1109/IROS.2014.6942788.
[^5]: Oishi S, Kohari Y, and Miura J. Toward a robotic attendant adaptively behaving according to human state. In: IEEE International Symposium on Robot and Human Interactive Communication, New York, USA, 26 August 2016, pp. 1038–1043. IEEE. DOI: 10.1109/ROMAN.2016.7745236.
[^6]: Brscic D, Kanda T, Ikeda T, et al. Person position and body direction tracking in large public spaces using 3D range sensors. IEEE Trans Human Mach Syst 2013; 43(6): 522–534.
[^7]: Baltieri D, Vezzani R, and Cucchiara R. 3DPEs: 3D people dataset for surveillance and forensics. In: ACM Workshop on Multimedia access to 3D Human Objects, Scottsdale, Arizona, USA, pp. 59–64.
[^8]: Benfold B and Reid I. Stable multi-target tracking in real-time surveillance video. In: IEEE Conference on Computer Vision and Pattern Recognition, Colorado Springs, USA, 20 June 2011, pp. 3457–3464.
[^9]: Zhang S, Benenson R, Omran M, et al. How far are we from solving pedestrian detection? In: IEEE Conference on Computer Vision and Pattern Recognition, Las Vegas, USA, 27 June 2016, pp. 1259–1267. IEEE. DOI: 10.1109/CVPR.2016.141.
[^10]: Fuentes LM and Velastin SA. People tracking in surveillance applications. Image Vision Comput 2006; 24(11): 1165–1171. DOI: 10.1016/j.imavis.2005.06.006. Performance Evaluation of Tracking and Surveillance.
[^11]: Luber M, Spinello L, and Arras KO. People tracking in RGBD data with on-line boosted target models. In: IEEE/RSJ International Conference on Intelligent Robots and Systems, San Francisco, USA, 25 September 2011, pp. 3844–3849. IEEE. DOI: 10.1109/IROS.2011.6095075.
[^12]: Munaro M, Basso F, and Menegatti E. OpenPTrack: Open source multi-camera calibration and people tracking for RGB-d camera networks. Robot Auton Syst 2016; 75:525–538. DOI: 10.1016/j.robot.2015.10.004.
[^13]: Bedagkar-Gala A and Shah SK. A survey of approaches and trends in person re-identification. Image Vision Comput 2014; 32(4): 270–286. DOI: 10.1016/j.imavis.2014.02.001.
## [^14]: Satake J, Chiba M, and Miura J. A SIFT-based person identification using a distance-dependent appearance model for a person following robot. In: IEEE International Conference on Robotics and Biomimetics,
---
中国广州，2012年12月11日，第962–967页。IEEE。DOI: 10.1109/ROBIO.2012.6491093.
[^15]: Koide K and Miura J. Identification of a specific person using color, height, and gait features for a person following robot. Robot Auton Syst 2016; 84: 76–87. DOI: 10.1016/j.robot.2016.07.004.
[^16]: Ristani E and Tomasi C. Features for multi-target multi-camera tracking and re-identification. In: IEEE Conference on Computer Vision and Pattern Recognition, Salt Lake City, USA, 18 June 2018.
[^17]: Munaro M, Fossati A, Basso A, et al. One-shot person re-identification with a consumer depth camera. In: Person Re-Identification, 2014, pp. 161–181. Springer. DOI: 10.1007/978-1-4471-6296-4_8.
[^18]: Semwal VB, Raj M, and Nandi G. Biometric gait identification based on a multilayer perceptron. Robot Auton Syst 2014;65: 65–75. DOI: 10.1016/j.robot.2014.11.010.
[^20]: Nakamura K, Zhao H, Shibasaki R, et al. Human sensing in crowd using laser scanners. London, UK: INTECH Open Access Publisher, 2012. DOI: 10.5772/33276.
[^19]: Song X, Cui J, Zhao H, et al. Laser-based tracking of multiple interacting pedestrians via on-line learning. Neurocomputing 2013; 115: 92–105. DOI: 10.1016/j.neucom.2013.02.001.
[^21]: Sabapathy T, Mustapha MA, Jusoh M, et al. Location tracking system using wearable on-body GPS antenna. In: Engineering Technology International Conference, Ho Chi Minh City, Vietnam, 5 August 2016, vol. 97. EDP. DOI: 10.1051/matecconf/20179701099.
[^22]: Doherty ST, Lemieux CJ, and Canally C. Tracking human activity and well-being in natural environments using wearable sensors and experience sampling. Soc Sci Med 2014;106: 83–92. DOI: 10.1016/j.socscimed.2014.01.048.
[^23]: Escriba C, Roux J, Hajjine B, et al. Smart wearable active patch for elderly health prevention. In: 5th Annual Conference on Computational Science & Computational Intelligence, Las Vegas, United States, 13 December 2018.
[^24]: Ramadhan A. Wearable smart system for visually impaired people. Sensors 2018; 18(3): 843. DOI: 10.3390/s18030843.
[^25]: Zhu X, Li Q, and Chen G. Apt accurate outdoor pedestrian tracking with smartphones. In: Proceedings IEEE INFO-COM, Turin, Italy, 14 April 2013, pp. 2508–2516. IEEE. DOI: 10.1109/INFCOM.2013.6567057.
[^26]: Kotaru M and Katti S. Position tracking for virtual reality using commodity Wi-Fi. In: IEEE Conference on Computer Vision and Pattern Recognition, Honolulu, USA, 21 July 2017.
[^27]: Soltanaghaei E, Kalyanaraman A, and Whitehouse K. Multipath triangulation: decimeter-level Wi-Fi localization and orientation with a single unaided receiver 2018; DOI: 10.1145/3210240.3210347.
[^28]: Edwards A, Silva B, dos Santos R, et al. Wi-Fi based indoor positioning using pattern recognition. In: IEEE 27th International Symposium on Industrial Electronics, Cairns, Australia, 13 June 2018. IEEE. DOI: 10.1109/isie.2018.8433869.
[^29]: Li F, Zhao C, Ding G, et al. A reliable and accurate indoor localization method using phone inertial sensors. In: ACM Conference on Ubiquitous Computing, Pittsburgh, USA, 5 September 2012, pp. 421–430. ACM. DOI: 10.1145/2370216.2370280.
[^30]: Kang W and Han Y. Smartpdr: smartphone-based pedestrian dead reckoning for indoor localization. IEEE Sens J 2015;15(5):2906–2916. DOI: 10.1109/JSEN.2014.2382568.
[^31]: Grisetti G, Kummerle R, Stachniss C, et al. A tutorial on graph-based slam. IEEE Int Trans Syst Magaz 2010; 2(4):31–43. DOI: 10.1109/MITS.2010.939925.
[^32]: Wan E and Merwe RVD. The unscented Kalman filter for nonlinear estimation. In: Adaptive Systems for Signal Processing, Communications, and Control Symposium, Lake Louise, Canada, 4 October 2000. IEEE. DOI: 10.1109/asspcc.2000.882463.
[^33]: Kümmerle R, Grisetti G, Strasdat H, et al. G2o: A general framework for graph optimization. In: IEEE International Conference on Robotics and Automation, Shanghai, China, 9 May 2011, pp. 3607–3613. IEEE. DOI: 10.1109/ICRA.2011.5979949.
[^34]: Magnusson M, Lilienthal A, and Duckett T. Scan registration for autonomous mining vehicles using 3D-NDT. J Field Robot 2007; 24(10): 803–827. DOI: 10.1.1.189.2393.
[^35]: Besl PJ and McKay ND. A method for registration of 3-D shapes. IEEE Trans Pattern Analysis Mach Int 1992; 14(2): 239–256. DOI: 10.1109/34.121791.
[^36]: Magnusson M, Nuchter A, Lorken C, et al. Evaluation of 3D registration reliability and speed - a comparison of ICP and NDT. In: IEEE International Conference on Robotics and Automation, Kobe, Japan, 12 May 2009, pp. 3907–3912. IEEE. DOI: 10.1109/ROBOT.2009.5152538.
37. Nelson E. Blam - Berkeley localization and mapping, 2016. （2019年4月3日访问）.
[^38]: Fischler MA and Bolles RC. Random sample consensus: a paradigm for model fitting with applications to image analysis and automated cartography. Communications 1981; 24(6): 381–395. DOI: 10.1145/358669.358692.
[^39]: Ma L, Kerl C, Stckler J, et al. CPA-SLAM: Consistent plane-model alignment for direct RGB-D slam. In: IEEE International Conference on Robotics and Automation, Stockholm, Sweden, 16 May 2016, pp. 1285–1291. IEEE. DOI: 10.1109/ICRA.2016.7487260.
[^40]: Shan T and Englot B. Lego-loam: lightweight and ground-optimized LIDAR odometry and mapping on variable terrain. In: IEEE/RSJ International Conference on Intelligent Robots and Systems), Madrid, Spain, 1 October 2018, pp. 4758–4765. IEEE. DOI: 10.1109/IROS.2018.8594299.
[^41]: Haselich M, Jobgen B, Wojke N, et al. Confidence-based pedestrian tracking in unstructured environments using 3D laser distance measurements. In: IEEE/RSJ International Conference on Intelligent Robots and Systems, Chicago, USA, 14 September 2014, pp. 4118–4123. IEEE. DOI: 10.1109/iros.2014.6943142.
## [^42]: Kulis B and Jordan MI. Revisiting k-means: new algorithms via Bayesian nonparametrics. CoRR 2011; abs/1111.0352.
---
[^43]: Kidono K, Miyasaka T, Watanabe A, et al. Pedestrian recognition using high-definition LIDAR. In: IEEE Intelligent Vehicles Symp, (IV), Baden-Baden, Germany, 5 June 2011, pp. 405–410. IEEE. DOI: 10.1109/IVS.2011.5940433.
[^44]: Schapire RE and Singer Y. Improved boosting algorithms using confidence-rated predictions. In: Annual Conference
on Computational learning theory, vol. 37, 美国麦迪逊，1998年7月24日，第297–336页。ACM。DOI: 10.1145/279943.279960.
[^45]: Radosavljevic Z. A study of a target tracking method using global nearest neighbor algorithm. Vojnotehnicki glasnik 2006; (2): 160–167. DOI: 10.5937/vojtehg0602160r.