Any-time randomized kinodynamic path planning algorithm in dynamic environments with application to quadrotor

Document Type : Original Article


Electrical Engineering Department, Malek Ashtar University of Technology, Tehran, Iran


Kinodynamic path planning is an open challenge in unmanned autonomous vehicles and is considered an NP-Hard problem. Planning a feasible path for vertical take-off and landing quadrotor (VTOL-Q) from an initial state to a target state in 3D space by considering the environmental constraints such as moving obstacles avoidance and non-holonomic constraints such as hard bounds of VTOL-Q is the key motivation of this study. To this end, let us propose the any-time randomized kinodynamic (ATRK) path-planning algorithm applicable in the VTOL-Q. ATRK path-planning algorithm is based on the Rapidly-exploring random trees (RRT) and consists of three main components: high-level, mid-level, and low-level controller. The high-level controller utilizes a randomized sampling-based approach to generate offspring vertices for rapid exploring and expanding in the configuration space. The mid-level controller uses the any-time method to avoid collision with moving obstacles. The low-level controller with a six-DOF dynamic model accounts for the kinodynamic constraints of VTOL-Q in the randomized offspring vertices to plan a feasible path. Simulation results on three different test-scenario demonstrate the kinodynamic constraints of the VTOL-Q are integrated into the randomized offspring vertices. Also, in presence of moving obstacles, the ATRK re-plans the path in the local area as through an any-time approach.


  1. Campana, S., "Drones in archaeology. State-of-the-art and future perspectives", Archaeological Prospection, Vol. 24, No. 4, (2017), 275-296,
  2. Yang, K., Keat Gan, S. and Sukkarieh, S., "A gaussian process-based rrt planner for the exploration of an unknown and cluttered environment with a uav", Advanced Robotics, Vol. 27, No. 6, (2013), 431-443,
  3. Pizetta, I.H.B., Brandão, A.S. and Sarcinelli-Filho, M., "Avoiding obstacles in cooperative load transportation", ISA Transactions, Vol. 91, (2019), 253-261,
  4. da Silva, M.F., Honorio, L.M., Marcato, A.L.M., Vidal, V.F. and Santos, M.F., "Unmanned aerial vehicle for transmission line inspection using an extended kalman filter with colored electromagnetic interference", ISA Transactions, Vol. 100, (2020), 322-333,
  5. Shobeiry, P., Xin, M., Hu, X. and Chao, H., "Uav path planning for wildfire tracking using partially observable markov decision process", in AIAA Scitech 2021 Forum. (2021), 1677,
  6. Mohammed, H., Romdhane, L. and Jaradat, M.A., "Rrt*n: An efficient approach to path planning in 3d for static and dynamic environments", Advanced Robotics, Vol. 35, No. 3-4, (2021), 168-180,
  7. Floreano, D. and Wood, R.J., "Science, technology and the future of small autonomous drones", Nature, Vol. 521, No. 7553, (2015), 460-466,
  8. Villasenor, J., "“Drones” and the future of domestic aviation [point of view]", Proceedings of the IEEE, Vol. 102, No. 3, (2014), 235-238, DOI: 10.1109/JPROC.2014.2302875.
  9. Taheri, E., Ferdowsi, M.H. and Danesh, M., "Closed-loop randomized kinodynamic path planning for an autonomous underwater vehicle", Applied Ocean Research, Vol. 83, (2019), 48-64,
  10. Barraquand, J., Langlois, B. and Latombe, J.-C., "Numerical potential field techniques for robot path planning", IEEE Transactions on systems, Man, and Cybernetics, Vol. 22, No. 2, (1992), 224-241, DOI: 10.1109/21.148426.
  11. Cabreira, T., Brisolara, L. and Ferreira Jr, P.R., "Survey on coverage path planning with unmanned aerial vehicles", Drones, Vol. 3, No. 1, (2019),
  12. Rubí, B., Pérez, R. & Morcego, B., "A survey of path following control strategies for uavs focused on quadrotors", Journal of Intelligent & Robotic Systems, Vol. 98, No. 2, (2020), 241–265,
  13. Heidari, H. and Saska, M., "Trajectory planning of quadrotor systems for various objective functions", Robotica, Vol. 39, No. 1, (2021), 137-152,
  14. Khosravian, E. and Maghsoudi, H., "Design of an intelligent controller for station keeping, attitude control, and path tracking of a quadrotor using recursive neural networks", International Journal of Engineering, Transactions B: Applications, Vol. 32, No. 5, (2019), 747-758. doi: 10.5829/ije.2019.32.05b.17
  15. Manouchehri, P., Ghasemi, R. and Toloei, A., "Distributed fuzzy adaptive sliding mode formation for nonlinear multi-quadrotor systems", International Journal of Engineering, Transactions B: Applications, Vol. 33, No. 5, (2020), 798-804, DOI: 10.5829/IJE.2020.33.05B.11.
  16. Sangdani, M. and Tavakolpour-Saleh, A., "Particle swarm optimization based parameter identification applied to a target tracker robot with flexible joint", International Journal of Engineering, Transactions C: Aspects, Vol. 33, No. 9, (2020), 1797-1802, DOI: 10.5829/IJE.2020.33.09C.14.
  17. Liu, B., Feng, W., Li, T., Hu, C. and Zhang, J., "A variable-step rrt* path planning algorithm for quadrotors in below-canopy", IEEE Access, Vol. 8, (2020), 62980-62989, DOI: 10.1109/ACCESS.2020.2983177.
  18. Allen, R.E. and Pavone, M., "A real-time framework for kinodynamic planning in dynamic environments with application to quadrotor obstacle avoidance", Robotics and Autonomous Systems, Vol. 115, (2019), 174-193,
  19. Shao, S., Peng, Y., He, C. and Du, Y., "Efficient path planning for uav formation via comprehensively improved particle swarm optimization", ISA Transactions, Vol. 97, (2020), 415-430,
  20. Mashadi, B., Mahmoodi-K, M., Kakaee, A.H. and Hosseini, R., "Vehicle path following control in the presence of driver inputs", Proceedings of the Institution of Mechanical Engineers, Part K: Journal of Multi-body Dynamics, Vol. 227, No. 2, (2013), 115-132,
  21. Mashadi, B., Mahmoudi-Kaleybar, M., Ahmadizadeh, P. and Oveisi, A., "A path-following driver/vehicle model with optimized lateral dynamic controller", Latin American Journal of Solids and Structures, Vol. 11, No. 4, (2014), 613-630,
  22. Gao, F., Lin, Y. and Shen, S., "Gradient-based online safe trajectory generation for quadrotor flight in complex environments", in 2017 IEEE/RSJ international conference on intelligent robots and systems (IROS), IEEE. (2017), 3681-3688, DOI: 10.1109/IROS.2017.8206214.
  23. Heidari, H. and Saska, M., "Trajectory planning of quadrotor systems for various objective functions", Robotica, Vol. 39, No. 1, (2020), 137-152,
  24. Chen, Y.-b., Luo, G.-c., Mei, Y.-s., Yu, J.-q. and Su, X.-l., "Uav path planning using artificial potential field method updated by optimal control theory", International Journal of Systems Science, Vol. 47, No. 6, (2016), 1407-1420,
  25. Frazzoli, E., Dahleh, M.A. and Feron, E., "Real-time motion planning for agile autonomous vehicles", Journal of Guidance, Control, and Dynamics, Vol. 25, No. 1, (2002), 116-129,
  26. Gong, W., "Probabilistic model based path planning", Physica A: Statistical Mechanics and its Applications, (2021), 125718,
  27. Le, A.V., Arunmozhi, M., Veerajagadheswar, P., Ku, P.-C., Minh, T.H.Q., Sivanantham, V. and Mohan, R.E., "Complete path planning for a tetris-inspired self-reconfigurable robot by the genetic algorithm of the traveling salesman problem", Electronics, Vol. 7, No. 12, (2018), 344,
  28. Zheng, S. and Liu, H., "Improved multi-agent deep deterministic policy gradient for path planning-based crowd simulation", IEEE Access, Vol. 7, (2019), 147755-147770, DOI: 10.1109/ACCESS.2019.2946659.
  29. Kimmel, A., Shome, R. and Bekris, K., "Anytime motion planning for prehensile manipulation in dense clutter", Advanced Robotics, Vol. 33, No. 22, (2019), 1175-1193,
  30. Wang, W., Xu, X., Li, Y., Song, J. and He, H., "Triple rrts: An effective method for path planning in narrow passages", Advanced Robotics, Vol. 24, No. 7, (2010), 943-962,
  31. Taheri, E., Ferdowsi, M.H. and Danesh, M., "Fuzzy greedy rrt path planning algorithm in a complex configuration space", International Journal of Control, Automation and Systems, Vol. 16, No. 6, (2018), 3026-3035,
  32. Bouabdallah, S., Noth, A. and Siegwart, R., "Pid vs lq control techniques applied to an indoor micro quadrotor", in 2004 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (IEEE Cat. No.04CH37566). Vol. 3, (2004), 2451-2456 vol.2453, DOI: 10.1109/IROS.2004.1389776.
  33. Bresciani, T., "Modelling, identification and control of a quadrotor helicopter", M. Sc. Thesis, Vol., No. 0280-5316, (2008),
  34. Sabatino, F., "Quadrotor control: Modeling, nonlinearcontrol design, and simulation", KTH, School of Electrical Engineering (EES), Automatic Control., (2015),
  35. Das, A., Lewis, F. and Subbarao, K., "Dynamic inversion with zero-dynamics stabilisation for quadrotor control", IET Control Theory & Applications, Vol. 3, No. 3, (2009), 303-314, DOI:  10.1049/iet-cta:20080002.
  36. Kuffner, J.J.a.L., Steven M, "Rrt-connect: An efficient approach to single-query path planning", Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No. 00CH37065), Vol. 2, (2000), 995-1001, DOI: 10.1109/ROBOT.2000.844730.
  37. Wang, J., Chi, W., Li, C., Wang, C. and Meng, M.Q.H., "Neural rrt*: Learning-based optimal path planning", IEEE Transactions on Automation Science and Engineering, Vol. 17, No. 4, (2020), 1748-1758, DOI: 10.1109/TASE.2020.2976560.
  38. Richter, C., Bry, A. and Roy, N., Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments, in Robotics Research, 2016.649-666,