近年來隨著更高的自動化駕駛層級發展，這考驗了自駕車中導航系統是否能提供更精確的定位、導航與定時服務。然而考量到傳統的導航系統(整合慣性導航及全球導航衛星系統)精確度易受限於環境透空度等因素，本研究在傳統導航系統的基礎上加入低單價的光學雷達(簡稱光達)作為輔助感測器，並透過鬆耦合的架構，藉由擴展式卡曼濾波器將多感測器進行整合。其中，光達點雲的應用是藉由常態分佈轉換的方式與高精地圖進行掃描匹配，以達成同步定位與地圖構建。導航過程中，使用誤差偵測與排除機制作為觀測量更新的約制條件，嘗試降低潛在誤差對於導航系統影響。
然而，考量到高精地圖的高製作成本無法即時應用於實際情境，因此本研究提出的多感測器整合系統適用於有或無高精地圖的場域進行導航測試。與此同時，嘗試模擬供自駕車使用的開源軟體—Autoware 作為精度分析的對照組。由於場域中包含多樣態的場景，因此將針對各場域中的開放式、半遮蔽式以及衛星挑戰性等不同透空度環境深入分析與探討。
藉由與高規格的導航系統所提供的參考軌跡進行精確度分析，在具備高精地圖的場域，由於常態分佈轉換的掃描匹配方法對於點雲分布以及初始值的給定較敏感，尤其是在衛星訊號被屏蔽或干擾的場景，可能會造成錯誤的掃描匹配結果。在此情境下，演算法中的誤差偵測與排除機制針對誤差進行初步剔除，使系統可以更加穩定地估算導航解，尤其對於高程以及航向角的誤差約束能力有顯著的提升，並可以達到車道等級(1.5 公尺)的導航精度。
在尚未建構高精地圖且衛星定位品質良好的場域，本研究提出的演算法搭配不同規格的光達，皆可以達車道內等級(0.5 公尺)的導航精度。雖然不同規格的光達在此場景提供相似的輔助能力，然而點雲的密度高低仍反映於誤差震盪的幅度。基於此特性，在衛星訊號屏蔽而僅能仰賴點雲的環境中，採用高規格的光達可以體現較高的導航能力提升。
綜觀來說，相較於對照組，本研究提出之演算法在多樣態的實驗場景皆有較佳的導航能力展現，低規格的光達亦可提供穩定的導航估算，以符合在提升導航精度的同時，滿足自動駕駛系統在現代發展的需求。

With the development of higher automation level for autonomous driving (AD),this has tested whether the navigation system in self-driving cars can provide more precise positioning, navigation and timing (PNT) services. However, considering that the accuracy of traditional navigation system, the inertial navigation system (INS) and global navigation satellite system (GNSS) integrated system, is easily limited in areas such as GNSS-denied or multipath environments. Therefore, this study adds light detection and ranging (LiDAR) as an auxiliary sensor. To conduct the multi-sensor fusion, it’s based on loosely coupled (LC) architecture with extended Kalman filter (EKF). Among them, the application of LiDAR point cloud is registered to high-definition map (HD map) by means of normal distribution transform (NDT), in order to fulfill simultaneous localization and mapping (SLAM). During the navigation process, the fault detection and exclusion (FDE) schemes are applied as the constraint conditions for the measurement update, which can minimize the impact of uncertainty on the system.
However, the construction of HD map requires a lot of manpower and time, it’s unable to prevail all over the world in short time. Based on this consideration, the algorithm raised in this study is suitable for navigation tests in fields with or without HD maps. At the same time, to show the navigation ability of the proposed method, a control group is established based on the simulation of the Autoware (which is an open-source software for autonomous driving). Due to the variety of scenarios, the experiment extracts the open sky, semi-open sky, and GNSS challenging environment, respectively, as the test fields.
The navigation accuracy analysis is conducted by comparing with the reference trajectory from the high-grade navigation system. During the experiments in the scenario with HD map, owing to the NDT is more sensitive to the point cloud distribution and the given initial guess, especially in the fields where the satellite signal is blocked or interfered, it may cause false scan matching results. Under the circumstances, the FDE can preliminarily exclude the errors, so that the system can estimate the navigation solution more robust. Especially the error constraint ability of height and heading angle is significantly improved, which can reach the lane level (1.5m) navigation accuracy.
In the field where HD maps have not been constructed and the satellite positioning quality is stable, the algorithm proposed in this study can achieve where-in-lane level (0.5 meters) accuracy with different grades of LiDAR. Although both low and high-cost LiDAR provide similar auxiliary capabilities in this scene, the density of the point cloud is still reflected in the magnitude of the error oscillation. Based on this feature, in the field with GNSS outages and can only rely on point cloud, the use of high-cost LiDAR can show better navigation improvement.
To sum up, compared with the control group, the algorithm proposed in this study has better navigation ability in various experimental scenarios. Meanwhile, the low-cost LiDAR can provide stable navigation estimation and meet the demands as the auxiliary attachment for AD.

Abdelkrim, N., Aouf, N., Tsourdos, A., & White, B. (2008). Robust nonlinear filtering for INS/GPS UAV localization. In 2008 16th Mediterranean Conference on Control and Automation (IEEE) (pp. 695-702).
Aggarwal, P. (2010). MEMS-based integrated navigation. Artech House.
Aghili, F., & Su, C. Y. (2016). Robust relative navigation by integration of ICP and adaptive Kalman filter using laser scanner and IMU. IEEE/ASME Transactions on Mechatronics, 21(4), 2015-2026.
Akai, N., Morales, L. Y., Takeuchi, E., Yoshihara, Y., & Ninomiya, Y. (2017). Robust localization using 3D NDT scan matching with experimentally determined uncertainty and road marker matching. In 2017 IEEE Intelligent Vehicles Symposium (IV) (pp. 1356-1363).
Akai, N., Morales, L. Y., Yamaguchi, T., Takeuchi, E., Yoshihara, Y., Okuda, H., ... & Ninomiya, Y. (2017, October). Autonomous driving based on accurate localization using multilayer LiDAR and dead reckoning. In 2017 IEEE 20th International Conference on Intelligent Transportation Systems (ITSC) (pp. 1-6). IEEE.
Allen, J. J. (2009). Micro-system inertial sensing technology overview.
Angrisano, A. (2010). GNSS/INS integration methods. Dottorato di ricerca (PhD) in Scienze Geodetiche e Topografiche Thesis, Universita’degli Studi di Napoli PARTHENOPE, Naple, 21.
Biber, P., & Straßer, W. (2003, October). The normal distributions transform: A new approach to laser scan matching. In Proceedings 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2003) (Cat. No. 03CH37453) (Vol. 3, pp. 2743-2748). IEEE.
Cai, H., Hu, Z., Huang, G., Zhu, D., & Su, X. (2018). Integration of GPS, monocular vision, and high definition (HD) map for accurate vehicle localization. Sensors, 18(10), 3270.
CBINSIGHTS (2018). “Unbundling The Autonomous Vehicle”. https://www.cbinsights.com/research/startups-drive-auto-industry-disruption/.
Chen, S., Ma, H., Jiang, C., Zhou, B., Xue, W., Xiao, Z., & Li, Q. (2021). NDT-LOAM: A Real-time Lidar odometry and mapping with weighted NDT and LFA. IEEE Sensors Journal.
Chiang, K. W., Lin, Y. C., Huang, Y. W., & Chang, H. W. (2009). An ANN–RTS smoother scheme for accurate INS/GPS integrated attitude determination. GPS solutions, 13(3), 199-208.
Chiang, K. W., Tsai, G. J., Chu, H. J., & El-Sheimy, N. (2020). Performance enhancement of INS/GNSS/Refreshed-SLAM integration for acceptable lane-level navigation accuracy. IEEE Transactions on Vehicular Technology, 69(3), 2463-2476.
Chiu, Y. T., Srinara, S., Tsai, M. L., & Chiang, K. W. (2022). IMPROVEMENT OF LiDAR-SLAM-BASED 3D NDT LOCALIZATION USING FAULT DETECTION AND EXCLUSION ALGORITHM. The International Archives of Photogrammetry, Remote Sensing and Spatial Information Sciences, 43, 189-195.
Deng, Q., Sun, H., Chen, F., Shu, Y., Wang, H., & Ha, Y. (2021). An Optimized FPGA-Based Real-Time NDT for 3D-LiDAR Localization in Smart Vehicles. IEEE Transactions on Circuits and Systems II: Express Briefs, 68(9), 3167-3171.
Dubé, R., Gollub, M. G., Sommer, H., Gilitschenski, I., Siegwart, R., Cadena, C., & Nieto, J. (2018). Incremental-segment-based localization in 3-d point clouds. IEEE Robotics and Automation Letters, 3(3), 1832-1839.
El-Sheimy, N., Hou, H., & Niu, X. (2007). Analysis and modeling of inertial sensors using Allan variance. IEEE Transactions on instrumentation and measurement, 57(1), 140-149.
El-Sheimy, N., Nassar, S., & Noureldin, A. (2004). Wavelet de-noising for IMU alignment. IEEE Aerospace and Electronic Systems Magazine, 19(10), 32-39.
Falco, G., Pini, M., & Marucco, G. (2017). Loose and tight GNSS/INS integrations: Comparison of performance assessed in real urban scenarios. Sensors, 17(2), 255.
Farrell, J., 2008. Aided navigation: GPS with high rate sensors. McGraw-Hill, Inc.
Gao, Y., Liu, S., Atia, M. M., & Noureldin, A. (2015). INS/GPS/LiDAR integrated navigation system for urban and indoor environments using hybrid scan matching algorithm. Sensors, 15(9), 23286-23302.
Ge, M., Gendt, G., Dick, G., & Zhang, F. P. (2005). Improving carrier-phase ambiguity resolution in global GPS network solutions. Journal of Geodesy, 79(1), 103-110.
Gebre-Egziabher, D., & Gleason, S. (2009). GNSS applications and methods. Artech House.
Ghallabi, F., Nashashibi, F., El-Haj-Shhade, G., & Mittet, M. A. (2018, November). Lidar-based lane marking detection for vehicle positioning in an HD map. In 2018 21st International Conference on Intelligent Transportation Systems (ITSC) (pp. 2209-2214). IEEE.
Grewal, M. S., Weill, L. R., & Andrews, A. P. (2007). Global positioning systems, inertial navigation, and integration.
Han, S., Gong, Z., Meng, W., Li, C., & Gu, X. (2016). Future alternative positioning, navigation, and timing techniques: A survey. IEEE wireless communications, 23(6), 154-160.
Hayal, A. G. (2010). Static calibration of the tactical grade inertial measurement units (Doctoral dissertation, The Ohio State University).
Hobert, L., Festag, A., Llatser, I., Altomare, L., Visintainer, F., & Kovacs, A. (2015). Enhancements of V2X communication in support of cooperative autonomous driving. IEEE communications magazine, 53(12), 64-70.
Hong, H., & Lee, B. H. (2017, September). Probabilistic normal distributions transform representation for accurate 3D point cloud registration. In 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (pp. 3333-3338). IEEE.
IDTechEx (2020). “Autonomous Cars and Robotaxis 2020-2040: Players, Technologies and Market Forecast”. https://www.idtechex.com/tw/research-report/autonomous-cars-and-robotaxis-2020-2040-players-technologies-and-market-forecast/701.
IEEE standard specification format guide and test procedure for single-axis interferometric fiber optic gyros. In IEEE Std 952–1997; IEEE: New York, NY, USA, 1998; pp. 1–95.
Javanmardi, E., Javanmardi, M., Gu, Y., & Kamijo, S. (2018). Factors to evaluate capability of map for vehicle localization. IEEE Access, 6, 49850-49867.
Javanmardi, E., Javanmardi, M., Gu, Y., & Kamijo, S. (2020). Pre-estimating self-localization error of NDT-based map-matching from map only. IEEE Transactions on Intelligent Transportation Systems, 22(12), 7652-7666.
Jennifer Shuttleworth (2019). “SAE Standards News: J3016 automated-driving graphic update.” https://www.sae.org/news/2019/01/sae-updates-j3016-automated-driving-graphic.
Jiang, Y. F. (1998). Error analysis of analytic coarse alignment methods. IEEE Transactions on Aerospace and Electronic Systems, 34(1), 334-337.
Jun, L., Wei, L., Donglai, D., & Qiang, S. (2015, July). Point cloud registration algorithm based on NDT with variable size voxel. In 2015 34th Chinese Control Conference (CCC) (pp. 3707-3712). IEEE.
K.W. Chiang, G.J. Tsai, H.W. Chang, C. Joly, N. EI-Sheimy, Seamless navigation and mapping using an INS/GNSS/grid-based SLAM semi-tightly coupled integration scheme, Information Fusion, Volume 50, 2019, Pages 181-196, ISSN 1566-2535
Kenji, F. (2018). The Autoware Foundation. https://github.com/autowarefoundation/autoware/issues/1789.
Kim, T., & Park, T. H. (2020). Extended Kalman filter (EKF) design for vehicle position tracking using reliability function of radar and lidar. Sensors, 20(15), 4126.
Kim, Y., & Bang, H. (2018). Introduction to Kalman filter and its applications. Introduction and Implementations of the Kalman Filter, 1, 1-16.
Kim, Y., Jeong, J., & Kim, A. (2018, October). Stereo camera localization in 3d lidar maps. In 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (pp. 1-9). IEEE.
Koopman, P., & Wagner, M. (2016). Challenges in autonomous vehicle testing and validation. SAE International Journal of Transportation Safety, 4(1), 15-24.
Levinson, J., Montemerlo, M., & Thrun, S. (2007, June). Map-based precision vehicle localization in urban environments. In Robotics: science and systems (Vol. 4, No. Citeseer, p. 1).
Li, X., Du, S., Li, G., & Li, H. (2019). Integrate point-cloud segmentation with 3D LiDAR scan-matching for mobile robot localization and mapping. Sensors, 20(1), 237.
Li, Z., Cheng, Y., & Cui, X. (2019, November). Precise Fault isolation method of INS/GPS/ADS integrated navigation system. In 2019 Chinese Automation Congress (CAC) (pp. 4171-4175). IEEE.
Madyastha, V., Ravindra, V., Mallikarjunan, S., & Goyal, A. (2011, August). Extended Kalman filter vs. error state Kalman filter for aircraft attitude estimation. In AIAA Guidance, Navigation, and Control Conference (p. 6615).
Magnusson, M. (2009). The three-dimensional normal-distributions transform: an efficient representation for registration, surface analysis, and loop detection (Doctoral dissertation, Örebro universitet).
Magnusson, M., & Duckett, T. (2005, September). A comparison of 3D registration algorithms for autonomous underground mining vehicles. In Proceedings of the European Conference on Mobile Robotics (ECMR 2005) (pp. 86-91).
Magnusson, M., Lilienthal, A., & Duckett, T. (2007). Scan registration for autonomous mining vehicles using 3D‐NDT. Journal of Field Robotics, 24(10), 803-827.
Magnusson, M., Nuchter, A., Lorken, C., Lilienthal, A. J., & Hertzberg, J. (2009, May). Evaluation of 3D registration reliability and speed-A comparison of ICP and NDT. In 2009 IEEE International Conference on Robotics and Automation (pp. 3907-3912). IEEE.
Mohamed, A. H., & Schwarz, K. P. (1999). Adaptive Kalman filtering for INS/GPS. Journal of geodesy, 73(4), 193-203.
Nassar, S. (2003). Improving the inertial navigation system (INS) error model for INS and INS/DGPS applications (pp. 10-32). Calgary, AB, Canada: University of Calgary, Department of Geomatics Engineering.
Noureldin, A., Karamat, T. B., & Georgy, J. (2013). Fundamentals of inertial navigation, satellite-based positioning and their integration.
Okunsky, M. V., & Nesterova, N. V. (2019, April). Velodyne LIDAR method for sensor data decoding. In IOP Conference Series: Materials Science and Engineering (Vol. 516, No. 1, p. 012018). IOP Publishing.
Pang, S., Kent, D., Cai, X., Al-Qassab, H., Morris, D., & Radha, H. (2018, August). 3d scan registration based localization for autonomous vehicles-a comparison of ndt and icp under realistic conditions. In 2018 IEEE 88th vehicular technology conference (VTC-Fall) (pp. 1-5). IEEE.
Petovello, M.G. (2003). Real-time integration of a tactical-grade IMU and GPS for high-accuracy positioning and navigation.
Prikhodko, I. P., Bearss, B., Merritt, C., Bergeron, J., & Blackmer, C. (2018, March). Towards self-navigating cars using MEMS IMU: Challenges and opportunities. In 2018 IEEE International Symposium on Inertial Sensors and Systems (INERTIAL) (pp. 1-4). IEEE.
Randell, C., Djiallis, C., & Muller, H. (2003, October). Personal position measurement using dead reckoning. In Seventh IEEE International Symposium on Wearable Computers, 2003. Proceedings. (pp. 166-166). IEEE Computer Society.
Reshetyuk, Y. (2009). Self-calibration and direct georeferencing in terrestrial laser scanning (Doctoral dissertation, KTH).
Särkkä, S. (2008). Unscented Ra—h--T—g--Striebel Smoother. IEEE transactions on automatic control, 53(3), 845-849.
Seif, H. G., & Hu, X. (2016). Autonomous driving in the iCity—HD maps as a key challenge of the automotive industry. Engineering, 2(2), 159-162.
Shan, T., & Englot, B. (2018, October). Lego-loam: Lightweight and ground-optimized lidar odometry and mapping on variable terrain. In 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (pp. 4758-4765). IEEE.
Sharp, G. C., Lee, S. W., & Wehe, D. K. (2002). ICP registration using invariant features. IEEE Transactions on Pattern Analysis and Machine Intelligence, 24(1), 90-102.
Shin, E. H. (2005). Estimation techniques for low-cost inertial navigation. UCGE report, 20219.
Shin, E. H., & El-Sheimy, N. (2002, January). Accuracy improvement of low cost INS/GPS for land applications. In Proceedings of the 2002 national technical meeting of the institute of navigation (pp. 146-157).
Shin, E. H., & El-Sheimy, N. (2004, April). An unscented Kalman filter for in-motion alignment of low-cost IMUs. In PLANS 2004. Position Location and Navigation Symposium (IEEE Cat. No. 04CH37556) (pp. 273-279). IEEE.
Sobreira, H., Costa, C. M., Sousa, I., Rocha, L., Lima, J., Farias, P. C. M. A., ... & Moreira, A. P. (2019). Map-matching algorithms for robot self-localization: a comparison between perfect match, iterative closest point and normal distributions transform. Journal of Intelligent & Robotic Systems, 93(3), 533-546.
Stephenson, S., Meng, X., Moore, T., Baxendale, A., & Ford, T. (2011). Accuracy requirements and benchmarking position solutions for intelligent transportation location-based services. In Proceedings of the 8th international symposium on location-based services.
Tahsin, M., Sultana, S., Reza, T., & Hossam-E-Haider, M. (2015, May). Analysis of DOP and its preciseness in GNSS position estimation. In 2015 International conference on electrical engineering and information communication technology (ICEEICT) (pp. 1-6). IEEE.
Titterton, D., Weston, J. L., & Weston, J. (2004). Strapdown inertial navigation technology (Vol. 17). IET.
Turner, D., Lucieer, A., & Wallace, L. (2013). Direct georeferencing of ultrahigh-resolution UAV imagery. IEEE Transactions on Geoscience and Remote Sensing, 52(5), 2738-2745.
Wan, G., Yang, X., Cai, R., Li, H., Zhou, Y., Wang, H., & Song, S. (2018, May). Robust and precise vehicle localization based on multi-sensor fusion in diverse city scenes. In 2018 IEEE international conference on robotics and automation (ICRA) (pp. 4670-4677). IEEE.
Wong, H., Zhang, X., Wen, T., Yang, M., Jiang, K., & Yang, D. (2021, September). HD-Map Aided LiDAR-INS Extrinsic Calibration. In 2021 IEEE International Intelligent Transportation Systems Conference (ITSC) (pp. 3136-3143). IEEE.
Wang, Y., Chernyshoff, A., & Shkel, A. M. (2019). Study on estimation errors in ZUPT-aided pedestrian inertial navigation due to IMU noises. IEEE Transactions on Aerospace and Electronic Systems, 56(3), 2280-2291.
Welch, G. F. (2020). Kalman filter. Computer Vision: A Reference Guide, 1-3.
Wen, W., Hsu, L. T., & Zhang, G. (2018). Performance analysis of NDT-based graph SLAM for autonomous vehicle in diverse typical driving scenarios of Hong Kong. Sensors, 18(11), 3928.
Yuanxi, Y. A. N. G. (2016). Concepts of comprehensive PNT and related key technologies. Acta Geodaetica et Cartographica Sinica, 45(5), 505.
Zabalegui, P., De Miguel, G., Pérez, A., Mendizabal, J., Goya, J., & Adin, I. (2020). A review of the evolution of the integrity methods applied in GNSS. IEEE Access, 8, 45813-45824.
Zaganidis, A., Magnusson, M., Duckett, T., & Cielniak, G. (2017, September). Semantic-assisted 3D normal distributions transform for scan registration in environments with limited structure. In 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (pp. 4064-4069). IEEE.
Zhang, G., & Hsu, L. T. (2018). Intelligent GNSS/INS integrated navigation system for a commercial UAV flight control system. Aerospace Science and Technology, 80, 368-380.
Zhang, J., & Singh, S. (2014, July). LOAM: Lidar Odometry and Mapping in Real-time. In Robotics: Science and Systems (Vol. 2, No. 9).
Zhang, J., Zhang, K., Grenfell, R., & Deakin, R. (2006). Short note: On the relativistic Doppler effect for precise velocity determination using GPS. Journal of Geodesy, 80(2), 104-110.
Zhang, Y., Yu, F., Gao, W., & Wang, Y. (2018). An improved strapdown inertial navigation system initial alignment algorithm for unmanned vehicles. Sensors, 18(10), 3297.
Zhao, L., Qiu, H., & Feng, Y. (2016). Analysis of a robust Kalman filter in loosely coupled GPS/INS navigation system. Measurement, 80, 138-147.
Zhong, Z., Liu, S., Mathew, M., & Dubey, A. (2018). Camera radar fusion for increased reliability in ADAS applications. Electronic Imaging, 2018(17), 258-1.
Zhou, B., Tang, Z., Qian, K., Fang, F., & Ma, X. (2017, July). A LiDAR odometry for outdoor mobile robots using NDT based scan matching in GPS-denied environments. In 2017 IEEE 7th Annual International Conference on CYBER Technology in Automation, Control, and Intelligent Systems (CYBER) (pp.1230-1235). IEEE.
Zhou, H., Xu, W., Chen, J., & Wang, W. (2020). Evolutionary V2X technologies toward the Internet of vehicles: Challenges and opportunities. Proceedings of the IEEE, 108(2), 308-323.
Zhu, J., Gehrung, J., Huang, R., Borgmann, B., Sun, Z., Hoegner, L., ... & Stilla, U. (2020). TUM-MLS-2016: An annotated mobile LiDAR dataset of the TUM city campus for semantic point cloud interpretation in urban areas. Remote Sensing, 12(11), 1875.
Ziebinski, A., Cupek, R., Erdogan, H., & Waechter, S. (2016, September). A survey of ADAS technologies for the future perspective of sensor fusion. In International Conference on Computational Collective Intelligence (pp. 135-146). Springer, Cham.
Zou, Q., Sun, Q., Chen, L., Nie, B., & Li, Q. (2021). A comparative analysis of LiDAR SLAM-based indoor navigation for autonomous vehicles. IEEE Transactions on Intelligent Transportation Systems.