Map-Based Indoor Pedestrian Navigation Using an Auxiliary Particle Filter

摘要
  本文主要使用APF（辅助粒子滤波）将内置MEMS传感器的智能手机和室内地图信息集成，提出一种基于非基础结构的低成本的室内导航方法。设计了一种级联结构的卡尔曼粒子滤波算法，来减少计算负担和提高APF的估算速度（通过减少APF的更新频率和粒子数量）。在底层的滤波即卡尔曼滤波中，用零速更新和非完整约束(non-holonomic constraints)来校正导航器件的误差。该设计的创新之处在于结合了顶层滤波器（粒子滤波）的地图匹配（MM）和地图辅助（MA）方法以进一步约束导航。
关键词：地图信息、MEMS传感器、地图辅助、辅助粒子滤波、级联结构算法、室内行人导航

1、介绍

  地图信息不直接参与位置计算，而是在基本算法的“表示层”绘制位置解和目的地，然而，可以利用数据融合算法在导航算法中进一步利用地图信息，可约束导航误差。地图与INS的结合方法有两种：MM和MA，MM广泛应用于固定轨迹应用，例如室外车辆导航，假定物体在已知且受限的路径内移动，对于室内导航来说，MA更加灵活，不需要任何关于轨迹的假设，但是不如MM强大，所以在本文两种方法互相补充。
  将INS与辅助信息集成在一起常见的方法是使用卡尔曼滤波器(KF)，但需要非线性估计技术，所以使用粒子滤波（PF），但是存在计算量大和样本需求多的问题，所以组合使用。
本文算法：
1、仅使用智能手机内置的IMU和室内地图信息；
2、KF与PF结合，级联结构算法，减少PF粒子数量减少计算负担；
3、组合MM和MA，提高导航精度。

2、算法

2.1 级联结构算法

两层结构：级联KF和APF

  通过改变上层PF的速率运算，将大大降低PF的计算负担。但是，为了利用高频的传感器数据，底层的KF根据高速惯性传感器测量结果执行更新，且使用ZUPT和NHC来校正INS。上层PF使用行人航位推算计算行人位置，使用KF导航结果计算出来的步长变化和航向变化作为上层滤波器的非线性状态模型，先验的已知地图信息作为独立测量量用来校正底层滤波。底层KF用了行人三轴位置、三轴速度和姿态作为状态向量，上层的APF，考虑到计算负担，状态向量中只有二维位置信息。

2.2 底层卡尔曼滤波

  底层滤波器中，捷联惯性导航方法用于求解室内初始位置，根据用户的运动状态，通过KF使用ZUPT和NHC校正初始结果。NHC用来限制行人横向和垂直速度，平台不可能打滑或者跳跃，所以假设横向速度和垂直速度为零，行人处于静态且被正确检测，触发ZUPT用于INS校正。底层KF离散测量模型为：

其中，$\delta z_k^{ins/zupt}$是测量向量，是系统的真实速度减去系统在静态模式下从INS得到的速度向量，$\delta x_k^{ins}$是系统状态，包括位置、速度和姿态，$v_k^{ins/zupt}$是测量噪声矩阵，测量矩阵$H_k^{ins/zupt}$为：

每次更新状态误差$\delta x_k^{ins}$时，将其应用于INS导航，并将$\delta x_k^{ins}$重置。使用文献[45]中的方法，在INS过程中执行上层滤波器的步幅检测，使用底层KF提供的导航解决方案来计算步幅长度。具体来说，将行人静止时每两个坐标之间的距离定义为步幅长度，即

其中，$(x_{step-m},y_{step-m})$是根据底层KF得到的估计位置。每一步的航向差异是当行人静止时KF提供的每两个航向之间的差异，采用以下公式计算

因此，PF的测量模型可以表示为

其中，$v_{sk}$和$v_{\phi k}$分别表示步幅长度的误差和航向变化。也就是说，步幅和航向误差都有一个相应的误差模型，描述PF观测的不确定性，该模型是由$v_{sk}$的分布$v_s$和$v_{\phi k}$的分布$v_{\phi }$决定。为了简化问题，假设$v_{sk}$和$v_{\phi k}$都是正态分布的。

2.3 顶层粒子滤波器

  粒子滤波器的出现是为了从数学上实现贝叶斯估计，如图2示，PF中有三个阶段：system propagation，测量更新和重采样（如果有必要的话）。没有使用贝叶斯估计中的先验概率，而是采用一组具有值和权重的粒子通过蒙特卡洛采样来近似表示$p(x_k|Y_k)$。estimated state的后验密度函数$p(x_{0:k}|Y_k)$可表示为


其中，参数$N$是粒子数量，是计算工作量和估计精度之间的折中值，$Y_k$是在k时刻测量值$y_k$的集合，$w_k^i$表示粒子的权重，和值为1，其表达式为

其中分母是importance density function重要性采样 (IDF)，标准粒子滤波选择先验密度函数作为IDF，简化上式

然而，在IDF中没有考虑新的测量值，重要性采样中得到的样本和真实PDF之间有差异，这会导致样本穷尽的问题，重采样可以解决该问题，但是会增加系统的计算负担。本文的PF作为顶层滤波，与传统的PF算法不同的是，将当前测量的重要性分布考虑，APF可解决粒子穷尽的问题，通过引入以下重要性采样

粒子权重

其中${\mu }_k^i$是在前一时刻状态基础上当前状态的统计特征，与传统PF相比，APF进行了两次加权操作，且权重更加稳定，在实际应用中更准确。
  在本文的研究中，APF的状态模型是DR定位的更新模型。行人航位推算（PDR）通过利用行人步态的运动学特征以及行进距离和航向信息来实现，本质上PDR是使用先前已知位置的知识以及当前行进距离和航向信息来确定新位置。推算方程为

2.4 MM和MA

  本文将MM和MA结合起来，通过充分利用室内地图信息来提高最终位置解的准确性，MA在测量更新过程中将室内地图信息用作测量资源，以修改APF中粒子的权重。MM通过使用地图信息，将估计的解与现有地图匹配来减少预测的位置误差。

2.4.1 MA

  对于室内行人导航，室内地图信息之所以可以用来约束行人轨迹，减少不确定性，提高精度，是通过更新粒子权重。APF中，权重更新是将预测的测量结果与从实际测量过程中获得的概率分布函数进行比较。用到了基于室内地图的穿墙检测方法，穿墙问题也是通过改变粒子权重实现的，如果新生成的粒子与前一个粒子之间的线段与墙边界相交，则该粒子无效，其权重将被指定为零。穿墙检测十分重要，因为错误的检测将会把错误的粒子带到下一步。
  为了检测新的粒子是否穿越墙，设计了新的有效的粒子检测算法。得到数据，转为以米为单位的x和y坐标，以矩阵中向量形式存储。将有效的粒子检测问题转换为粒子与地图信息矢量之间的关系分析问题。首先，假设$A=(x_{k-1}^{(i)},y_{k-1}^{(i)})$和$B=(x_k^{(i)},y_k^{(i)})$分别表示第i个粒子在k-1时刻和k时刻的坐标。假定A和B是水平面步长矢量的起点和终点。如图3，CD是地图中的一条线段，如果AB与CD相交且满足以下条件之一，表示新的粒子无效。
1、A和B位于线段矢量CD的不同侧；C和D位于AB的不同侧，怎么判断？
对于向量$(x_1,y_1)\to (x_2,y_2)$，定义$M=x_1(y_3-y_2)+x_2(y_1-y_3)+x_3(y_2-y_1)$。
如果$M=0$，点3就在点12线段上；
如果$M>0$，点3在线段右侧；
否则在左侧。

2、如图4，线AB和CD应该有一个交点，如果$AB>max(OA,OB)$并且$CD>max(OC,OD)$，则O为交点。

2.4.2 MM

  MM是利用数字道路网络地图数据库在集成过程中通过将估计位置投影到先验数字地图来改善预测位置误差的过程。在本文中，利用室内地图，生成线段构成的可能的行人轨迹，当用户当前估计位置和地图上某段的距离小于一个阈值时，MM将当前用户估计位置投影到其相关的路段上，这个阈值由经验值确定。如果有多个地图上的路段小于阈值，不执行MM。MM有三种：点对点、点对线、线到线，具体选哪个看数据或网络结构。本文采用点到点。

其中，$(x_e,y_e）$是指基于MA的APF算法得到的估计位置，$(x_s,y_s）$和$(x_d,y_d）$数字 地图上匹配路段的起点和终点。第一个等式用于计算估计位置与先前已知的地图路径段之间的距离，并用于检测应该在其上投影估计的位置的线段。第二三式用于计算投影点。

3、实验及其结果

IMU采样率40Hz，参考轨迹生成方法见文献2，基于加速度计和陀螺仪。

手持智能手机，小米3，根据给定的地图信息手动输入初始位置，参考轨迹[2]。粒子数1000.



还有两个实验，不同场景，拐点较多，粒子数不同。实验结果差不多。

4、结论

只用INS，低成本MEMS惯导，结果不好，虽然使用ZUPT和NHC校正系统误差，但是航向不可观察unobservable。使用APF将室内地图信息集成，精度大大提高，因为地图信息可以通过删除无效粒子来强烈限制系统的前进方向。估计误差的RMS（均方根误差）和估计误差的均值可以控制在2 m之内。而且，与传统粒子滤波相比，计算负担降低，因为PF的更新速率已从IMU数据输出速率（40 Hz）更改为步进检测更新速率。
此外还可以看出，在拐角处不执行MM，这是因为在拐角处至少有两个地图上的路径段满足MM距离阈值，这是MM的缺点之一。
粒子数减少，仅MM估计误差的RMS增加，这是因为根据大数定律，PF的精度与粒子数有关，N无穷大时，最近接概率密度函数。

参考文献

1. Woodman, O.; Harle, R. Pedestrian localisation for indoor environments. In Proceedings of the 10th International Conference on Ubiquitous Computing, Seoul, Korea, 21–24 September 2008; ACM: New York, NY, USA; pp. 114–123.
2. Lan, H.; Yu, C.; Zhuang, Y.; Li, Y.; El-Sheimy, N. A novel kalman filter with state constraint approach for the integration of multiple pedestrian navigation systems. Micromachines 2015, 6, 926–952. [CrossRef]
3. Rintamäki, T.; Mitronen, L. Creating information-based customer value with service systems in retailing. In Service Systems Science; Springer: Tokyo, Japan, 2015; pp. 145–162.
4. Khan, M.I. Design and Development of Indoor Positioning System: For Portable Devices; LAP LAMBERT Academic Publishing: Saarbrücken, Germany, 2013.
5. Lan, K.-C.; Shih, W.-Y. Using smart-phones and floor plans for indoor location tracking. IEEE Trans. Hum. Mach. Syst. 2014, 44, 211–221.
6. Emilsson, A. Indoor Navigation Using an iPhone. Available online: http://www.diva-portal.org/smash/ get/diva2:328320/FULLTEXT01.pdf (accessed on 19 July 2017).
7. Lan, H.; Yu, C.; El-Sheimy, N. An integrated PDR/GNSS pedestrian navigation system. In China Satellite Navigation Conference (CSNC) 2015 Proceedings: Volume III; Springer: Berlin/Heidelberg, Germany, 2015; pp. 677–690.
8. Rehman, U.; Cao, S. Augmented reality-based indoor navigation using google glass as a wearable head-mounted display. In Proceedings of the 2015 IEEE International Conference on Systems Man and Cybernetics (SMC), Kowloon, China, 9–12 October 2015; pp. 1452–1457.
9. Fallah, N.; Apostolopoulos, I.; Bekris, K.; Folmer, E. Indoor human navigation systems: A survey. Interact. Comput. 2013. [CrossRef]
10. Woodman, O.; Harle, R. Rf-based initialisation for inertial pedestrian tracking. In Proceedings of the 7th International Conference on Pervasive Computing, Nara, Japan, 11–14 May 2009; pp. 238–255.
11. Chai, W.; Chen, C.; Edwan, E.; Zhang, J.; Loffeld, O. INS/Wi-Fi based indoor navigation using adaptive kalman filtering and vehicle constraints. In Proceedings of the 2012 9th Workshop on Positioning Navigation and Communication (WPNC), Dresden, Germany, 15–16 March 2012; pp. 36–41.
12. Zhuang, Y.; Lan, H.; Li, Y.; El-Sheimy, N. PDR/INS/WiFi integration based on handheld devices for indoor pedestrian navigation. Micromachines 2015, 6, 793–812. [CrossRef]
13. Titterton, D.; Weston, J.L. Strapdown Inertial Navigation Technology; IET: Stevenage, UK, 2004; Volume 17.
14. Karimi, H.A. Indoor Wayfinding and Navigation; CRC Press: Boca Raton, FL, USA, 2015.
15. Del Rosario, M.B.; Redmond, S.J.; Lovell, N.H. Tracking the evolution of smartphone sensing for monitoring human movement. Sensors 2015, 15, 18901–18933. [CrossRef] [PubMed]
16. Alojz, K.; Kajánek, P.; Lipták, I. Systematic error elimination using additive measurements and combination of two low cost IMSs. IEEE Sens. J. 2016, 16, 6239–6248.
17. Du, H.; Bogue, R. Mems sensors: Past, present and future. Sens. Rev. 2007, 27, 7–13.
18. Lan, H.; El-Sheimy, N. A state constraint kalman filter for pedestrian navigation with low cost mems inertial sensors. In Proceedings of the 27th International Technical Meeting of the Satellite Division of the Institute of Navigation (ION GNSS+ 2014), Tampa, FL, USA, 8–12 September 2014; p. 579589.
19. Syed, Z.F.; Aggarwal, P.; Niu, X.; El-Sheimy, N. Civilian vehicle navigation: Required alignment of the inertial sensors for acceptable navigation accuracies. IEEE Trans. Veh. Technol. 2008, 57, 3402–3412. [CrossRef]
20. Enrico, P.; Leccese, F. Improvement of altitude precision in indoor and urban canyon navigation for small flying vehicles. In Proceedings of the 2015 IEEE Metrology for Aerospace (MetroAeroSpace), Benevento, Italy, 4–5 June 2015.
21. Otto, C.; Milenkovic, A.; Sanders, C.; Jovanov, E. System architecture of a wireless body area sensor network for ubiquitous health monitoring. J. Mob. Multimed. 2006, 1, 307–326.
22. Stojanovi´c, D.; Stojanovi´c, N. Indoor localization and tracking: Methods, technologies and research challenges. Facta Univ. Ser. Autom. Control Robot. 2014, 13, 57–72.
23. Li, Y.; Zhuang, Y.; Lan, H.; Zhang, P.; Niu, X.; El-Sheimy, N. WiFi-aided magnetic matching for indoor navigation with consumer portable devices. Micromachines 2015, 6, 747–764. [CrossRef]
24. Google, Indoor Maps Availability. Available online: https://support.google.com/gmm/answer/1685827? hl=en (accessed on 9 March,2017).
25. Von Gioi, R.G.; Jakubowicz, J.; Morel, J.-M.; Randall, G. LSD: A line segment detector. Image Process. Line 2012, 2, 35–55. [CrossRef]
26. Link, J.A.B.; Smith, P.; Viol, N.; Wehrle, K. Footpath: Accurate map-based indoor navigation using smartphones. In Proceedings of the 2011 International Conference on Indoor Positioning and Indoor Navigation (IPIN), Guimaraes, Portugal, 21–23 September 2011; pp. 1–8.
27. Pinto, A.M.; Moreira, A.P.; Costa, P.G. A localization method based on map-matching and particle swarm optimization. J. Intell. Robot. Syst. 2015, 77, 313–326. [CrossRef]
28. Yang, D.; Cai, B.; Yuan, Y. An improved map-matching algorithm used in vehicle navigation system. In Proceedings of the 2003 IEEE Intelligent Transportation Systems, Shanghai, China, 12–15 October 2003; pp. 1246–1250.
29. Vaughn, D. Vehicle Speed Control Based on GPS/MAP Matching of Posted Speeds. U.S. Patent 5,485,161, 16 January 1996.
30. White, C.E.; Bernstein, D.; Kornhauser, A.L. Some map matching algorithms for personal navigation assistants. Transp. Res. Part C Emerg. Technol. 2000, 8, 91–108. [CrossRef]
31. Quddus, M.A.; Ochieng, W.Y.; Noland, R.B. Current map-matching algorithms for transport applications: State-of-the art and future research directions. Transp. Res. Part C Emerg. Technol. 2007, 15, 312–328. [CrossRef]
32. Bierlaire, M.; Chen, J.; Newman, J. A probabilistic map matching method for smartphone GPS data. Transp. Res. Part C Emerg. Technol. 2013, 26, 78–98. [CrossRef]
33. Quddus, M.A.; Ochieng, W.Y.; Zhao, L.; Noland, R.B. A general map matching algorithm for transport telematics applications. GPS Solut. 2003, 7, 157–167. [CrossRef]
34. Bandyopadhyay, A.; Hakim, D.; Funk, B.; Kohn, E.A.; Teolis, C.; Weniger, G.B. System and Method for Locating, Tracking, and/or Monitoring the Status of Personnel and/or Assets Both Indoors and Outdoors. U.S. Patent 9,008,962, 14 April 2015.
35. Tao, L.; Georgy, J.; Wang, D. Method and Apparatus for Using Map Information Aided Enhanced Portable Navigation. U.S. Patent 14/845,903, 4 September 2015.
36. Petritoli, E.; Tonino, G.; Fabio, L. Lightweight GNSS/IRS integrated navigation system for UAV vehicles. In Proceedings of the IEEE Metrology for Aerospace (MetroAeroSpace), Benevento, Italy, 29–30 May 2014.
37. Reimer, C.; Schneider, T.; Stock, M. INS/GNSS Integration for Aerobatic Flight Applications and Aircraft Motion Surveying. Sensors 2017, 17, 941.
38. Fox, D.; Burgard, W.; Dellaert, F.; Thrun, S. Monte carlo localization: Efficient position estimation for mobile robots. In Proceedings of the Sixteenth National Conference on Artificial Intelligence and Eleventh Conference on Innovative Applications of Artificial Intelligence (AAAI/IAAI), Orlando, FL, USA, 18–22 July 1999; pp. 343–349.
39. Georgy, J. Advanced Nonlinear Techniques for Low Cost Land Vehicle Navigation. Ph.D. Thesis, Queen’s University, Kingston, ON, Canada, 2010.
40. Yu, C.; Lan, H.; Liu, Z.; El-Sheimy, N.; Yu, F. Indoor map aiding/map matching smartphone navigation using auxiliary particle filter. In China Satellite Navigation Conference (CSNC) 2016 Proceedings: Volume I; Springer: Singapore, 2016.
41. Krach, B.; Roberston, P. Cascaded estimation architecture for integration of foot-mounted inertial sensors. In Proceedings of the 2008 IEEE/ION Position, Location and Navigation Symposium, Monterey, CA, USA, 5–8 May 2008; pp. 112–119.
42. Yu, C.; Lan, H.; Gu, F.; Yu, F.; El-Sheimy, N. A Map/INS/Wi-Fi Integrated System for Indoor Location-Based Service Applications. Sensors 2017, 17, 1272. [CrossRef] [PubMed]
43. Courbon, J.; Mezouar, Y.; Martinet, P. Indoor navigation of a non-holonomic mobile robot using a visual memory. Auton. Robot. 2008, 25, 253–266. [CrossRef]
44. El-Sheimy, N. Inertial Techniques and INS/DGPS Integration; Engo 623-Course Notes; University of Calgary: Calgary, AB, Canada, 2003; pp. 170–182.
45. Cho, S.Y.; Park, C.G. Mems based pedestrian navigation system. J. Navig. 2006, 59, 135–153. [CrossRef]
46. Klepal, M.; Beauregard, S. A novel backtracking particle filter for pattern matching indoor localization. In Proceedings of the First ACM International Workshop on Mobile Entity Localization and Tracking in GPS-Less Environments, San Francisco, CA, USA, 14–19 September 2008; ACM: New York, NY, USA; pp. 79–84.
47. Simon, D. Optimal State Estimation: Kalman, h Infinity, and Nonlinear Approaches; John Wiley & Sons: Hoboken, NJ, USA, 2006.
48. Arulampalam, M.S.; Maskell, S.; Gordon, N.; Clapp, T. A tutorial on particle filters for online nonlinear/non-gaussian bayesian tracking. IEEE Trans. Signal Process. 2002, 50, 174–188. [CrossRef]
49. Beauregard, S.; Klepal, M. Indoor PDR performance enhancement using minimal map information and particle filters. In Proceedings of the 2008 IEEE/ION Position, Location and Navigation Symposium, Monterey, CA, USA, 5–8 May 2008; pp. 141–147.
50. Cossaboom, M.; Georgy, J.; Karamat, T.; Noureldin, A. Augmented kalman filter and map matching for 3D RISS/GPS integration for land vehicles. Int. J. Navig. Obs. 2012, 2012. [CrossRef]
51. Grisetti, G.; Stachniss, C.; Burgard, W. Improved techniques for grid mapping with rao-blackwellized particle filters. IEEE Trans. Robot. 2007, 23, 34–46. [CrossRef]
52. Zhang, X.; Cheng, Y.; Shi, C. Observability analysis of non-holonomic constraints for land-vehicle navigation systems. J. Glob. Position. Syst. 2012, 11, 80–88.