Observability-Guided Autonomous Path Planning for Quadrotor Navigation in GNSS-Denied Environments

Amiri Atashgah, Mohammad-Ali; Elahian, Samaneh; Tarvirdizadeh, Bahram

doi:10.22034/jast.2025.470258.1201

Observability-Guided Autonomous Path Planning for Quadrotor Navigation in GNSS-Denied Environments

Document Type : Original Article

Authors

Mohammad-Ali Amiri Atashgah ¹

Samaneh Elahian ¹

Bahram Tarvirdizadeh ²

¹ Department of College of Interdisciplinary Science and Technology, University of Tehran, Tehran, Iran

² College of Interdisciplinary Science and Technology, School of Intelligent Systems Engineering, University of Tehran, Tehran, Iran

10.22034/jast.2025.470258.1201

Abstract

Localization of a quadrotor in an unknown environment without access to Global Navigation Satellite System (GNSS) data, utilizing Simultaneous Localization and Mapping (SLAM), has shown promising results. The integration of SLAM and the Extended Kalman Filter (EKF) provides precise estimations of both the quadrotor’s position and the terrestrial landmark locations. This paper reviews the Observability-Based Path Planning (OBPP) and evaluates its performance in comparison to Monte Carlo Path Planning (MCPP). Furthermore, due to the importance of the initialization process in path planning for pre-planned methods, the performance of OBPP is evaluated for various initial positions. Simulation results demonstrate that the proposed method offers superior accuracy and greater robustness for different initial conditions, validating its effectiveness in diverse scenarios. The robustness and adaptability of this approach make it a valuable contribution to the field of autonomous navigation, especially in environments where GNSS is unavailable. This research advances the precision and reliability of quadrotor localization and path planning.

Keywords

Real-Time Autonomous Path Planning

Observability Degree

Mont Carlo Path Planning

Gram Matrix

Subjects

Flight Mechanics Stability & Contorl

[1] N. Crasta, D. Moreno-Salinas, A. M. Pascoal, and J. Aranda, “Multiple autonomous surface vehicle motion planning for cooperative range-based underwater target localization,” Annu. Rev. Control, vol. 46, pp. 326–342, Jan. 2018, https://doi.org/10.1016/j.arcontrol.2018.10.004.

[2] O. M. Cliff, R. Fitch, S. Sukkarieh, D. L. Saunders, and R. Heinsohn, “Online localization of radio-tagged wildlife with an autonomous aerial robot system,” Robot. Sci. Syst., vol. 11, Jan. 2015, https://doi.org/10.15607/RSS.2015.XI.042

[3] S. Park and S. Hashimoto, “Autonomous mobile robot navigation using passive RFID in an indoor environment,” IEEE Trans. Ind. Electron., vol. 56, no. 7, pp. 2366–2373, 2009, https://doi.org/10.1109/TIE.2009.2013690.

[4] N. Goyal, R. Aryan, N. Sharma, and V. Chhabra, “Line Follower Cargo-Bot for Warehouse Automation,” Int. Res. J. Eng. Technol., Vol. 8 issue 2, 2021, [Online]. Available: https://www.irjet.net/archives/V8/i2/IRJET-V8I2179.pdf.

[5] T. Machado, T. Malheiro, S. Monteiro, W. Erlhagen, and E. Bicho, “Multi-constrained joint transportation tasks by teams of autonomous mobile robots using a dynamical systems approach,” Proc. - IEEE Int. Conf. Robot. Autom., pp. 3111–3117, 2016, https://doi.org/10.1109/icra.2016.7487477

[6] J. Fayyad, M. A. Jaradat, D. Gruyer, and H. Najjaran, “Deep learning sensor fusion for autonomous vehicle perception and localization: A review,” Sensors (Switzerland), vol. 20, no. 15, pp. 1–34, 2020, https://doi.org/10.3390/s20154220.

[7] K. D. Sowjanya, R. Sindhu, M. Parijatham, K. Srikanth, and P. Bhargav, “Multipurpose autonomous agricultural robot,” Proc. Int. Conf. Electron. Commun. Aerosp. Technol. ICECA 2017, pp. 696–699, 2017, https://doi.org/ 10.1109/ICECA.2017.8212756.

[8] M. Polic, A. Ivanovic, B. Maric, B. Arbanas, J. Tabak, and M. Orsag, “Structured Ecological Cultivation with Autonomous Robots in Indoor Agriculture,” Proc. 16th Int. Conf. Telecommun. ConTEL 2021, pp. 189–195, Jun. 2021, https://doi.org/10.23919/CONTEL52528.2021.9495963.

[9] H. Durmus, E. O. Gunes, M. Kirci, and B. B. Ustundag, “The design of general-purpose autonomous agricultural mobile-robot: ‘AGROBOT,’” 2015 4th Int. Conf. Agro-Geoinformatics, Agro-Geoinformatics 2015, pp. 49–53, https://doi.org/10.1109/Agro-Geoinformatics.2015.7248088 .

[10] B. Fu et al., “An improved A* algorithm for the industrial robot path planning with high success rate and short length,” Rob. Auton. Syst., vol. 106, pp. 26–37, Aug. 2018, https://doi.org/10.1016/j.robot.2018.04.007.

[11] Y. Hasegawa and Y. Fujimoto, “Experimental verification of path planning with SLAM,” IEEJ J. Ind. Appl., vol. 5, no. 3, pp. 253–260, 2016, https://doi.org/10.1541/ieejjia.5.253.

[12] Z. Wang and J. Cai, “Probabilistic roadmap method for path-planning in radioactive environment of nuclear facilities,” Prog. Nucl. Energy, vol. 109, pp. 113–120, 2018, https://doi.org/10.1016/j.pnucene.2018.08.006.

[13] K. Wei and B. Ren, “A method on dynamic path planning for robotic manipulator autonomous obstacle avoidance based on an improved RRT algorithm,” Sensors (Switzerland), vol. 18, no. 2, p. 571, Feb. 2018, https://doi.org/10.3390/s18020571.

[14] A. Gasparetto, P. Boscariol, A. Lanzutti, and R. Vidoni, “Path planning and trajectory planning algorithms: A general overview,” Mech. Mach. Sci., vol. 29, pp. 3–27, 2015, https://doi.org/10.1007/978-3-319-14705-5_1.

[15] Y. Li, W. Wei, Y. Gao, D. Wang, and Z. Fan, “PQ-RRT*: An improved path planning algorithm for mobile robots,” Expert Syst. Appl., vol. 152, p. 113425, Aug. 2020, https://doi.org/10.1016/j.eswa.2020.113425

[16] S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimal motion planning,” Int. J. Rob. Res., vol. 30, no. 7, pp. 846–894, Jun. 2011, https://doi.org/10.1177/0278364911406761.

[17] O. Arslan and P. Tsiotras, “Use of relaxation methods in sampling-based algorithms for optimal motion planning,” 2013 IEEE International Conference on Robotics and Automation 2013. https://di.org/10.1109/ICRA.2013.6630906.

[18] Y. Rasekhipour, A. Khajepour, S. K. Chen, and B. Litkouhi, “A Potential Field-Based Model Predictive Path-Planning Controller for Autonomous Road Vehicles,” IEEE Trans. Intell. Transp. Syst., vol. 18, no. 5, pp. 1255–1267, May 2017, https://doi.org/ 10.1109/TITS.2016.2604240.

[19] Y. B. Chen, G. C. Luo, Y. S. Mei, J. Q. Yu, and X. L. Su, “UAV path planning using artificial potential field method updated by optimal control theory,” Int. J. Syst. Sci., vol. 47, no. 6, pp. 1407–1420, Apr. 2016, https://doi.org/10.1080/00207721.2014.929191.

[20] H. Kiaee and H.Reza Heidari, "Cooperative path planning for leader – follower formation of Multi UAV based on the minimum energy consumption for load transportation," Amirkabir Journal of Mechanical Engineering, vol. 52, issue 12, 2021, pp. 3327-3340, (in Prsian) https://doi.rg/10.22060/mej.2019.15994.6245.

[21] L. Carlone, J. Du, M. Kaouk Ng, B. Bona, and M. Indri, “Active SLAM and exploration with particle filters using Kullback-Leibler divergence,” J. Intell. Robot. Syst. Theory Appl., vol. 75, no. 2, pp. 291–311, Oct. 2014, https://doi.org/10.1007/s10846-013-9981-9

[22] V. S. Kalogeiton, K. Ioannidis, G. C. Sirakoulis, and E. B. Kosmatopoulos, “Real-Time Active SLAM and Obstacle Avoidance for an Autonomous Robot Based on Stereo Vision,” Cybern. Syst., vol. 50, no. 3, pp. 239–260, Apr. 2019, https://doi.org/10.1080/01969722.2018.1541599

[23] R. Sharma, R. W. Beard, C. N. Taylor, and S. Quebe, “Graph-based observability analysis of bearing-only cooperative localization,” IEEE Trans. Robot., vol. 28, no. 2, pp. 522–529, Apr. 2012, https://doi.org/10.1109/TRO.2011.2172699.

[24] M. Bryson and S. Sukkarieh, “Observability analysis and active control for airborne SLAM,” IEEE Trans. Aerosp. Electron. Syst., vol. 44, no. 1, pp. 261–280, Jan. 2008, https://doi.org/10.1109/TAES.2008.4517003

[25] C. Leung, S. Huang, and G. Dissanayake, “Active SLAM using model predictive control and attractor-based exploration,” IEEE Int. Conf. Intell. Robot. Syst., pp. 5026–5031, 2006, https://doi.org/10.1109/IROS.2006.282530.

[26] R. Hermann and A. J. Krener, “Nonlinear Controllability and Observability,” IEEE Trans. Automat. Contr., vol. 22, no. 5, pp. 728–740, 1977, https://doi.org/ 10.1109/TAC.1977.1101601.

[27] Y. Liang, L. Han, X. Dong, Q. Li, and Z. Ren, “A quantitative method for observability analysis and its application in SINS calibration,” Aerosp. Sci. Technol., vol. 103, p. 105881, Aug. 2020, https://doi.org/10.1016/j.ast.2020.105881.

[28] Z. Hu, T. Chen, Q. Ge, and H. Wang, “Observable degree analysis for multi-sensor fusion system,” Sensors (Switzerland), vol. 18, no. 12, 2018, https://doi.org/10.3390/s18124197.

[29] F. M. Ham and R. Grover Brown, “Observability, Eigenvalues, and Kalman Filtering,” IEEE Trans. Aerosp. Electron. Syst., vol. AES-19, no. 2, pp. 269–273, 1983, https://doi.org/ 10.1109/TAES.1983.309446.

[30] R. Sharma, “Observability-based control for cooperative localization,” 2014 Int. Conf. Unmanned Aircr. Syst. ICUAS 2014 - Conf. Proc., pp. 134–139, 2014, https://doi.org/ 10.1109/ICUAS.2014.6842248.

[31] V. Lystianingrum, B. Hredzak, V. G. Agelidis, and V. S. Djanali, “Observability degree criteria evaluation for temperature observability in a battery string towards optimal thermal sensors placement,” 2014, https://doi.og//10.1109/ISSNIP.2014.6827641

[32] F. W. J. Van Den Berg, H. C. J. Hoefsloot, H. F. M. Boelens, and A. K. Smilde, “Selection of optimal sensor position in a tubular reactor using robust degree of observability criteria,” Chem. Eng. Sci., vol. 55, no. 4, pp. 827–837, Feb. 2000, https://doi.org/10.1016/S0009-2509(99)00360-7. https://doi.org/10.1016/S0009-2509(99)00360-7.

[33] P. Batista, C. Silvestre, and P. Oliveira, “Single range aided navigation and source localization: Observability and filter design,” Syst. Control Lett., vol. 60, no. 8, pp. 665–673, 2011, https://doi.org/10.1016/j.sysconle.2011.05.00.

[34] P. Zhuo, Q. Ge, T. Shao, Y. Wang, and L. Bai, “Observable degree analysis using unscented information filter for nonlinear estimation systems,” 2015 10th Int. Conf. Information, Commun. Signal Process. ICICS 2015, 2016, https://doi.org/10.1109/ICICS.2015.7459932.

[35] Y. Han, C. Wei, R. Li, J. Wang, and H. Yu, “A novel cooperative localization method based on IMU and UWB,” Sensors (Switzerland), vol. 20, no. 2, 2020, https://doi.org/10.3390/s20020467.

[36] W. Xu, D. He, Y. Cai, and F. Zhang, “Robots’ State Estimation and Observability Analysis Based on Statistical Motion Models,” IEEE Trans. Control Syst. Technol., vol. 30, no. 5, pp. 2030–2045, Oct. 2022, https://doi.og/10.1109/TCST.2021.3133080.

[37] K. W. Lee, W. S. Wijesoma, and I. G. Javier, “On the observability and observability analysis of SLAM,” IEEE Int. Conf. Intell. Robot. Syst., pp. 3569–3574, 2006, https://doi.org/10.1109/IROS.2006.281646.

[38] J. A. Hesch, D. G. Kottas, S. L. Bowman, and S. I. Roumeliotis, “Camera-IMU-based localization: Observability analysis and consistency improvement,” Int. J. Rob. Res., vol. 33, no. 1, pp. 182–201, Nov. 2014, https://doi.org/10.1177/0278364913509675.

[39] S. Elahian, M. A. Amiri Atashgah, and B. Tarverdizadeh, “Collaborative Autonomous Navigation of Quadrotors in Unknown Outdoor Environments: An Active Visual SLAM Approach,” IEEE Access, 2024, https://doi.org/10.1109/ACCESS.2024.3473792.

[40] T. Dam, G. Chalvatzaki, J. Peters, and J. Pajarinen, “Monte-Carlo Robot Path Planning,” IEEE Robot. Autom. Lett., vol. 7, no. 4, pp. 11213–11220, Oct. 2022, https://doi.org/10.1109/LRA.2022.3199674.

[41] H. Mwenegoha, T. Moore, J. Pinchin, and M. Jabbal, “Model-based autonomous navigation with moment of inertia estimation for unmanned aerial vehicles,” Sensors (Switzerland), vol. 19, no. 11, Jun. 2019, https://doi.org/10.3390/s19112467.

[42] Y. Yang and G. Huang, “Aided Inertial Navigation with Geometric Features: Observability Analysis,” in Proceedings - IEEE International Conference on Robotics and Automation, Sep. 2018, pp. 2334–2340, https://doi.org/10.1109/ICRA.2018.8460670.

[43] J. Kim, … S. S. R. and A. (Cat. N., and undefined 2003, “Airborne simultaneous localisation and map building,” 2003 IEEE International Conference on Robotics and Automation (Cat. No.03CH37422) 2003. https://doi.org/10.1109/ROBOT.2003.1241629.

[44] M. W. M. Gamini Dissanayake, P. Newman, S. Clark, H. F. Durrant-Whyte, and M. Csorba, “A solution to the simultaneous localization and map building (SLAM) problem,” IEEE Trans. Robot. Autom., vol. 17, no. 3, pp. 229–241, Jun. 2001, https://doi.org/10.1109/70.938381.

[45] S. Huang, N. M. Kwok, G. Dissanayake, Q. P. Ha, and G. Fang, “Multi-step look-ahead trajectory planning in SLAM: Possibility and necessity,” Proc. - IEEE Int. Conf. Robot. Autom., vol. 2005, pp. 1091–1096, 2005, https://doi.org/10.1109/ROBOT.2005.1570261.

[46] M. Serpas, G. Hackebeil, C. Laird, and J. Hahn, “Sensor location for nonlinear dynamic systems via observability analysis and MAX-DET optimization,” Comput. Chem. Eng., vol. 48, pp. 105–112, 2013, https://doi.org/10.1016/j.compchemeng.2012.07.014.

[47] A. Nemra and N. Aouf, “Robust airborne 3D visual simultaneous localization and mapping with observability and consistency analysis,” J. Intell. Robot. Syst. Theory Appl., vol. 55, no. 4–5, pp. 345–376, Aug. 2009, https://doi.org/10.1007/s10846-008-9306-6.

[48] R. Aguilar-López, J. Mata-Machuca, and R. Martinez-Guerra, “On the observability for a class of nonlinear (Bio)chemical systems,” Int. J. Chem. React. Eng., vol. 8, 2010, https://doi.org/10.2202/1542-6580.2052.

[49] N. Powel and K. A. Morgansen, “Empirical Observability Gramian for Stochastic Observability of Nonlinear Systems,” 2020. Accessed: Dec. 24, 2020. http://doi.org/ 10.48550/arXiv.2006.07451.

[50] L. Huang, J. Song, and C. Zhang, “Observability analysis and filter design for a vision inertial absolute navigation system for UAV using landmarks,” Optik (Stuttg), vol. 149, pp. 455–468, 2017, https://doi.org/10.1016/j.ijleo.2017.09.060.

[51] D. Goshen-Meskin and I. Y. Bar-Itzhack, “Observability Analysis of Piecewise Constant Systems Part I: Theory,” IEEE Trans. Aerosp. Electron. Syst., vol. 28, no. 4, pp. 1056–1067, 1992, https://doi.og/10.1109/7.165367.

[52] S. Brunton and J. Kutz, "Data-driven science and engineering: Machine learning," dynamical systems, and control. 2022, https://doi.org/10.1017/9781108380690.

[53] M. H. Kazma and A. F. Taha, “Observability for Nonlinear Systems: Connecting Variational Dynamics, Lyapunov Exponents, and Empirical Gramians,” Feb. 2024, Accessed: Dec. 20, 2024. [Online]. Available: http://arxiv.org/abs/2402.14711.

[54] T. Tao, Y. Huang, F. Sun, and T. Wang, “Motion planning for SLAM based on frontier exploration,” Proc. 2007 IEEE Int. Conf. Mechatronics Autom. ICMA 2007, pp. 2120–2125, 2007, https://doi.org/10.1109/ICMA.2007.4303879.

[55] C. Yang, L. Kaplan, and E. Blasch, “Performance measures of covariance and information matrices in resource management for target state estimation,” IEEE Trans. Aerosp. Electron. Syst., vol. 48, no. 3, pp. 2594–2613, 2012, https://doi.org/10.1109/TAES.2012.6237611.