# 基于激光雷达和惯性测量单元的智能手机传感器室内定位方法 Son Nguyen Trong 信息科学与 工程学院 立命馆大学 大阪，日本 Yanlei Gu 信息与数据 科学学院 广岛大学 广岛，日本 Igor Goncharenko 信息科学与 工程学院 立命馆大学 大阪，日本 摘要：室内定位技术已成为导航、基于位置的服务和资产跟踪等各种应用的重要组成部分。本文提出了一种创新的室内定位方法，该方法结合了智能手机内置传感器和基于激光雷达传感器数据的正态分布变换 (NDT) 算法，激光雷达传感器来自 iPhone。我们的方法结合了智能手机上的加速度计、陀螺仪和磁力计数据来估计用户的初始位置，然后使用正态分布变换 (NDT) 算法动态地改进此位置，该算法将基于激光雷达的点云与预先存在的 2D 地图对齐，从而提高位置估计的准确性。我们评估了我们的方法在真实室内环境中的性能。我们的结果表明，与传统方法相比，我们提出的方法显着提高了定位精度。这项研究说明了利用智能手机中的激光雷达传感器进行室内定位的潜力，以及结合多种传感器模式以提高准确性的优势。我们的方法有助于开发实用且低成本的智能手机室内定位系统。 关键词：室内定位、激光雷达、智能手机传感器、NDT 匹配、2D 地图 ## 1. 引言 追踪个人的位置已成为零售、交通和紧急服务等多个领域中的重要方面 [^1]。 随着智能手机的普及，定位功能在日常生活中变得越来越重要。 例如，人们依赖于诸如 Google 地图或 Apple 地图之类的导航应用程序来确定其当前位置并找到前往下一个目的地的路线。 在户外环境中，这可以通过使用卫星导航系统（如全球导航卫星系统 (GNSS)）来实现，该系统被广泛使用。 但是，在室内环境中，卫星信号可能被屏蔽或不可靠，因此跟踪变得更加困难。 这促使开发了各种室内定位技术，包括依赖于基础设施和不依赖于基础设施的方法 [^2]。 一种依赖于基础设施的方法是安装传感器网络，例如超宽带 (UWB) [^2]、Wi-Fi [^3] 或蓝牙 [^4]。 但是，在无法预先安装的情况下，这些方法可能不可行。 一种不依赖于外部基础设施的独立方法是行人惯性导航 (PDR)。 该技术利用从集成在智能手机设备中的惯性测量单元 (IMU) 传感器获取的运动数据，用于更新用户的估计位置，每次检测到一步。 通过依次从之前的估计计算当前位置，PDR 本身允许小的误差随着时间的推移累积，导致估计轨迹的逐渐漂移。 此外，由于智能手机级 IMU 的精度有限，并且室内环境会产生各种噪声源（例如磁干扰或不平整的地板），因此 PDR 系统倾向于在较短的跟踪距离内保持高精度。 因此，在没有定期校准或集成补充定位方法的情况下，累积的误差会显著降低较长室内路径的性能 [^5]。 为了提高基于智能手机的 PDR 的性能，使用正态分布变换来对齐 iPhone LiDAR 生成的二维点云与二维地图，从而提高行人室内定位的精度。 该研究将使用 iPhone 14 Pro，因为当前型号配备了足够的传感器，包括 LiDAR 传感器，这使得研究更具实用性和可应用于实际情况。 ### II. 室内定位系统 如图 1 所示，该系统由三个主要、连续的阶段组成，每个阶段的输出通过融合为最终的目标，即意图估计。 在初始阶段，通过行人惯性导航 (PDR) 推导行人的二维位置。 第二阶段涉及将原始 LiDAR 测量值转换为二维点云表示。 最后，第三阶段执行地图匹配以优化基于 PDR 的位置，并生成准确的位置估计。 这三个相互连接的过程旨在完全在安装在智能手机上的板载传感单元上运行，以实现集成位置和意图估计。 ![Image](images/3ad756164450e401ca9cfa1b9afeafe0dd0e4968c0b8f28028f74269989b9325.jpg)
Fig. 1. The flowchart of the proposed system for estimating position in indoor environment
--- ### A. 行人惯性导航 行人惯性导航 (PDR) 广泛应用于智能手机辅助室内导航，它通过跟踪步数、确定步长和确定方向来计算个人的二维位置 [^6]。 图 2 所示的行人惯性导航 (PDR) 系统的架构包括数据预处理、步态检测、模式识别和方向估计。 在初始处理阶段，系统从智能手机传感器获取 IMU 数据，包括加速度计、陀螺仪和磁力计。 智能手机中廉价的惯性传感器不仅受到固有漂移的影响，还受到环境干扰的影响。 尤其，磁力计读数可能会受到局部磁场的影响。 因此，必须过滤和校准原始数据。 从此预处理的数据中提取时域和频域特征。 然后，基于处理后的数据计算步态检测、步长估计和方向确定的参数。 随后，使用 PDR 方程更新行人的位置： $$ \left\{\begin{aligned}X_{k}&=X_{k-1}+L_{k-1,k}.\sin\left(\theta_{k-1,k}\right)\\ Y_{k}&=Y_{k-1}+L_{k-1,k}.\cos\left(\theta_{k-1,k}\right)\end{aligned}\right. $$ 其中，X 和 Y 分别是东西方向和南北方向的坐标，L 是步长，$\theta$ 是在单个步长间隔内的 heading 角度，k 表示行人的步数 [^6]。 ![Image](images/0863f1cefefbbe6767db557b592a1665508be4edb7284e3f49ba435031256a07.jpg)
Fig. 2. The flowchart of Pedestrian Dead Reackoning algorithm
### B. 点云投影 来自 LiDAR 的数据被保存为深度图像，其中每个像素代表距离。处理这些数据的第一步是将深度图像转换为点云，这是一个两步过程。首先，使用内参矩阵将深度图像转换为相机坐标系中的三维表示。然后，应用外参矩阵将这个三维模型映射到世界坐标系，从而得到一个点云，该点云是深度相机捕获的场景空间排列的真实表示。内参矩阵描述了相机的内部特征，例如 x 和 y 轴上的焦距以及主点的坐标，而外参矩阵提供了关于相机在世界中的位置和方向的重要信息，对于将相机中心坐标转换为全局坐标系至关重要。 这些矩阵的数学表示如下： $$ P=K.\left[R\;t\right] $$ 其中，K 是内参矩阵，R 和 t 分别是外参矩阵中的旋转和平移向量。 在点云处理领域，尤其是在室内环境中，通常需要消除非必要的结构，例如地板和天花板，以便专注于感兴趣的对象。随机采样共识 (RANSAC) 算法由 Fischler 和 Bolles 于 1981 年提出 [^7]，用于去除点云中的地板和天花板平面，如图 3a 所示。 最后，将过滤后的点云转换为鸟瞰图 (BEV) 表示。这涉及更改点云的视角，通常是通过将点云的 z 轴与观察者的视线对齐。BEV 变换提供了一个俯视的场景视图，这对于需要理解空间关系和布局的全局任务特别有用。在 BEV 中，地标和道路布局等特征会显示为它们在传统地图上的样子，这使得更容易将观察到的数据与现有地图信息对齐。这种俯视视角也有利于配准，因为当两个数据集都以相同的平面视图表示时，将它们置于相同的坐标系中更加容易。在 BEV 表示中，点云通常投影到一个二维平面上，如图 3b 所示。这种投影简化了数据的复杂性，使其更易于由本质上不是三维的算法进行分析，例如 Normal Distribution Transform 匹配。
(a)
(b)
![Image](images/e03d504615f49e982383618796402d5a43ea9b0c20c05e291ade68c8b9c2ed5e.jpg)
Fig. 3. Coversion of depth map to bird eye view point cloud: (a) Point cloud after removing floor and ceiling, (b) Bird eye view point cloud
### C. 正态分布变换匹配 正态分布变换 (NDT) 匹配是一种用于在公共坐标系中对齐两个点集的技术，该技术基于测量给定位置的点集的概率。首先，将空间划分为单元，并且每个单元都分配一个正态分布，其均值和协方差由该单元内的点计算得出，从而得到分段连续的概率密度函数。然后，通过最大化通过第一个点集定义的正态分布对转换后的点的可能性，找到将一个点集映射到另一个的最佳刚体变换（平移和旋转）[^8]。 该研究中的矢量地图是使用AutoCAD软件开发的，它源自于一个二维图像地图，并被转换为电子矢量格式。应用了一个比例因子，以确保地图准确地表示实际距离。估计的行人位置周围的墙壁被提取出来，并转换到本地的东-北-向上坐标系中。 --- 该过程首先将地图的墙壁转换为一个结构化的点云。点沿墙壁段均匀分布，保持相等的距离，最大间距为10厘米，如图4所示。这种均匀性对于有效地将地图直接用于基于正态分布变换算法至关重要。经过优化后，LiDAR导出的三维点集被转换到东-北-向上坐标系中，以与映射参考相对应，基于提供的初始位置估计 [^9]. 在匹配过程中，走廊的点云被用作参考集，标记为Y，并转换为高斯分布。用于对齐的LiDAR点云表示为X。将转换后的点云X与Y对齐，记为X' [^10]: $$ x_{i}^{\prime}=R x_{i}+T $$ 其中R,T是两个点云之间的旋转和平移，$x_i \in X$ [^10] [^11]. 在NDT的第一步中，空间被分割成一个网格，如图3b所示。单元格的大小对影响收敛区域和最终的定位精度至关重要。经过一系列的试验和错误，选择一个1米的单元格尺寸。当一个单元格包含足够多的建筑物点时，会建立一个二维高斯分布（如图3c所示）。然后，从位于该单元格内的点集Y（表示为$\mu_k$）中计算出该单元格的均值 $P_k$ 和协方差 $Y_k$： $$ \begin{array}{r}{\mu_{k}=\frac{1}{N_{k}}\sum_{y\in Y_{k}}(y)}\end{array} $$ $$ \begin{array}{r}{P_{k}=\frac{1}{N_{k}-1}\sum_{y\in Y_{k}}(y-\mu_{k})(y-\mu_{k})^{T}}\end{array} $$ 当点与 $N_{k}$ 对齐时，点的数量为 $Y_{k}$。 在这种情况下，协方差矩阵 $P_{k}$ 将会存在奇异性，并且不可逆 [^10] [^11]。 在构建每个单元格的二维高斯分布后（如图3c所示），注意力转移到NDT匹配过程。在此阶段，我们寻求最佳的旋转和平移，以将行人的点云Y与LiDAR导出的点云X对齐。通过最小化对数似然函数来实现这种对齐，该函数是每个转换点的概率的负对数和。优化此函数可确保转换后的行人数据尽可能地与环境模型匹配，从而实现行人位置与智能手机LiDAR捕获的周围环境之间的精确注册。对数似然函数使用以下公式计算： $$ -l n L(R,T)=-\sum_{i=1}^{N_{x}}\ln(p(R x_{i}+T)) $$ 其中R, T是两个点云之间的旋转和平移，$x_i \in X$ [^10] [^11]. ![Image](images/962832724b3a8b6e4548bff24306aa4fff72dc27ed352e2c3f1367f7a262333b.jpg)
Fig. 4. Coversion of 2D building vector map to a Normal Distribution map: (a) 2D vector map (b) Division into a grid of cells, (c) Normal Distribution map with ellipsoids
最后，通过融合正态分布变换（NDT）算法的输出和行人惯性导航系统的数据，利用扩展卡尔曼滤波器（EKF） [^12] 进行定位。 ### III. 实验 在本次研究中，我们展示了为了评估我们提出的室内定位方法的有效性而进行的实验。对于系统测试，iPhone 14 Pro 智能手机被用作主要测试平台。定位实验是在室内环境中进行的，具体是在立命馆大学 BKC 校区信息科学与工程学院的 Creation Core 建筑的二楼。图 5 描绘了实验场地的布局，覆盖了约 130 米的面积。该测试环境的特点是典型的磁场干扰，由各种因素引起，例如 GNSS 接收器、计算机、蓝牙和 WiFi 发射器模块以及建筑物的混凝土结构。在实验过程中，测试对象从指定的起点（在图 5 中用红色标记）开始步行，首先到达拐角 1，然后左转到达拐角 2，然后再次左转到达拐角 3，最后左转到达拐角 4，然后返回起点。行人所遵循的轨迹在图 5 中清晰地标出。 ![Image](images/ce23180ecd747469f158997de45998585ee180e840340a34872b7bfe7b8f808a.jpg) > #### 图 5. 室内定位实验场地 > 实验结果以轨迹图的形式呈现，如图 5 所示。该研究从原点开始，坐标为 $(0,0)$，在图中的红色点表示。实验过程中实际走过的路径由红色线条表示，它为我们的分析提供了参考。蓝色线条表示仅使用行人死reckoning (PDR) 系统的轨迹估计。相比之下，绿色线条表示从 PDR 系统获得的轨迹，该系统与正态分布转换 (NDT) 匹配相结合，该方法结合惯性测量单元 (IMU) 数据和 LiDAR 信息以实现更精确的定位。 --- > 仅依赖 IMU 数据的 PDR 系统，在路径的推进过程中，会与实际路径产生明显的偏差。这种偏差表明了与 PDR 系统常见的固有漂移问题，这是由于步数检测和方向估计中的误差累积造成的。可以观察到 PDR 轨迹在图的东北象限中错误地形成一个环路，然后再调整其路线以到达最终目的地。 > 另一方面，通过 NDT 匹配方法（绿色线条）校正的轨迹更接近实际路径。这种提高的准确性突出了 NDT 匹配在校正 PDR 漂移方面的有效性。值得注意的是，在 PDR 轨迹与实际路径出现明显偏差的区域，集成的 NDT 解决方案成功地将估计路径重新对齐，使其更准确地跟踪实际路径。绿色线条与实际路径之间可见的细微差异可能归因于 NDT 算法在某些室内环境中的性能限制，或者归因于 IMU 和 LiDAR 数据的固有噪声。 ![Image](images/3e0bba84c597358ee9d1043de75533000b8030075f94de9ee203c16d31968838.jpg)
Fig. 6. Comparison of the trajectories of PDR only and NDT integrated PDR
TABLE I. COMPARISON OF RMSE VALUES FOR ONLY PDR AND INTEGRATED PDR WITH GROUND TRUTH
MethodRMSE (meters)
PDR only16.37
Integrated PDR1.71
> 通过均方根误差 (RMSE) 进行的轨迹分析比较表明，当将 NDT 匹配应用于 PDR 数据时，定位精度得到了显着的提高。RMSE 从仅使用 PDR 方法的约 16.37 米降低到使用 NDT 集成方法的约 1.71 米，有力地证明了所提出的解决方案的有效性。 NDT匹配与PDR的集成已被证明可以有效地减轻室内定位中的漂移问题。这种集成利用了惯性测量单元（IMU）和激光雷达（LiDAR）技术的优势，利用了IMU数据的更新频率高，以及LiDAR扫描提供的空间感知能力。结果表明，这种混合方法是实现可靠且准确的室内导航的一个有前景的解决方案，这对于各种应用至关重要，例如自主机器人、增强现实以及室内环境中的应急响应。 ### IV. 结论 在本研究中，我们提出了一种通过结合行人定位（PDR）和正态分布变换（NDT）匹配来提高室内定位准确性的方法。该方法集成了来自智能手机的传感器数据，特别是利用惯性测量单元（IMU）和激光雷达（LiDAR）传感器。我们的测试表明，这种组合方法显著减少了传统PDR系统中通常出现的累积漂移。通过定期通过NDT匹配进行校正，我们成功提高了室内定位结果的精度和稳定性。 我们方法的显著优势在于它依赖于智能手机，智能手机是日常生活中常见的设备。这使得我们的解决方案既实用又易于访问，为室内导航任务提供了一种经济实惠的替代方案。所提出的方法在室内导航、应急响应场景、增强现实体验以及资产跟踪等领域具有巨大的应用潜力。 在未来，我们计划通过尝试其他传感器组合并在不同的室内环境中进行广泛的测试来进一步完善我们的方法。未来的工作还将旨在开发自适应校准技术，以减少传感器偏差并提高实时处理能力，从而更顺畅地集成到实际的、现实世界的场景中。 ## 参考文献 [^1]: Shum, L.C.; Faieghi, R.; Borsook, T.; Faruk, T.; Kassam, S.; Nabavi, H.; Spasojevic, S.; Tung, J.; Khan, S.S.; Iaboni, A. Indoor Location Data for Tracking Human Behaviours: A Scoping Review. Sensors 2022, 22, 1220. [^2]: A. Alari, A. Al-Salman, M. Alsaleh, A. Alnafessah, S. Al-Hadhrami, M. A. Al-Ammar, and H.S. Al-Khalifa, Ultra wideband indoor positioning technologies: Analysis and recent advances, Sensors, vol. 16, no. 5, pp. 707743, May 2016 [^3]: Yang, C.C.; Shao, H.R. WiFi-Based Indoor Positioning. IEEE Commun. Mag. 2015, 53, 150–157. [^4]: Subhan, F.; Hasbullah, H.; Rozyyev, A.; Bakhsh, S.T. Indoor positioning in Bluetooth networks using fingerprinting and lateration approach. In Proceedings of the 2011 International Conference on Information Science and Applications, Jeju Island, Republic of Korea, 26–29 April 2011; pp. 1–9. [^5]: C. Lin and Y. Shin, "Deep Learning-Based Multifloor Indoor Tracking Scheme Using Smartphone Sensors," in IEEE Access, vol. 10, pp. 63049-63062, 2022 [^6]: Wang, B.; Liu, X.; Yu, B.; Jia, R.; Gan, X. Pedestrian Dead Reckoning Based on Motion Mode Recognition Using a Smartphone. Sensors 2018, 18, 1811. [^7]: Martin A. Fischler and Robert C. Bolles. 1981. Random sample consensus: a paradigm for model fitting with applications to image analysis and automated cartography. Commun. ACM 24, 6 (June 1981), 381–395. ## [^8]: P. Biber and W. Strasser, "The normal distributions transform: a new approach to laser scan matching," Proceedings 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2003) (Cat. No.03CH37453), Las Vegas, NV, USA, 2003, pp. 2743-2748 --- [^9]: E. Javanmardi, M. Javanmardi, Y. Gu and S. Kamijo, "Adaptive Resolution Refinement of NDT Map Based on Localization Error Modeled by Map Factors," 2018 21st International Conference on Intelligent Transportation Systems (ITSC), Maui, HI, USA, 2018, pp. 2237-2243. [^10]: M. Escourrou, J. Al Hage and P. Bonnifait, "NDT Localization with 2D Vector Maps and Filtered LiDAR Scans," 2021 European Conference on Mobile Robots (ECMR), Bonn, Germany, 2021, pp.1-6 [^11]: M. Magnusson, “The three-dimensional normal-distributions transform: An efficient representation for registration, surface analysis, and loop detection,” Ph.D. dissertation, “Orebro University, 2009. [^12]: Welch, Greg and Gary Bishop. “An Introduction to Kalman Filter.” International Conference on Computer Graphics and Interactive Techniques (1995).