path_planner

Hybrid A* Path Planner for the KTH Research Concept Vehicle

This repository contains the implementation of a Hybrid A* Path Planner for autonomous vehicles, specifically developed for the KTH Research Concept Vehicle. The Hybrid A* algorithm is a powerful path planning approach that combines the benefits of A* search in continuous space with a discretized set of headings. It enables the generation of efficient and smooth paths for nonholonomic vehicles navigating complex environments.

Table of Contents

• Introduction
• Characteristics
• Videos
• Images
• Dependencies
• Setup
• Visualization
• Citation

Introduction

The code in this repository is the result of my master's thesis which I have written at the Integrated Research Lab (ITRL) at KTH Royal Institute of Technology (2016). The code is documented here and the associated thesis can be found here.

The goal of the thesis and hence this code is to create a real-time path planning algorithm for the nonholonomic Research Concept Vehicle (RCV). The algorithm uses a binary obstacle map as an input, generated using LIDAR mounted on top of the vehicle. The algorithm is being developed using C++ due to real-time requirements in combination with ROS to ensure modularity and portability as well as using RViz as a visualization/simulation environment.

Key Characteristics

• Sampling in continuous space with 72 different headings per cell (5° discretization)
• Constrained Heuristic - nonholonomic without obstacles
• Unconstrained Heuristic - holonomic with obstacles
• Dubin's Shot
• C++ real-time implementation (~10 Hz)

Large parts of the implementation are closely related to the hybrid A* algorithm developed by Dmitri Dolgov and Sebastian Thrun (Path Planning for Autonomous Vehicles in Unknown Semi-structured Environments DOI: 10.1177/0278364909359210)

Videos

• Path Planning with Search Visualization
• Dubin's Path - Constrained Heuristic
• 2D A* Search - Unconstrained Heuristic
• Open Loop Path Planning using Sensor Fusion

Images

Dependencies

• Open Motion Planning Library (OMPL)
• ros_map_server

Setup

Run the following command to clone, build, and launch the package (requires a sources ROS environment):

sudo apt install libompl-dev \
&& mkdir -p ~/catkin_ws/src \
&& cd ~/catkin_ws/src \
&& git clone https://github.com/karlkurzer/path_planner.git  \
&& cd .. \
&& catkin_make \
&& source devel/setup.bash \
&& rospack profile \
&& roslaunch hybrid_astar manual.launch

Visualization (Rviz)

1. Add -> By Topic -> /map, /path, /pathVehicle, (/visualizeNode2DPoses)
2. Click 2D Pose Estimate to set a start point on the map (p)
3. Click 2D Nav Goal to set a goal point on the map (g)
4. Wait for the path being searched! (this process can be visualized [optional])

Citation

I would appreciate if you cite my work, in case you are using it for your work. Thank you :-)

…

Cited By (not complete)

• N. Adiuku, N. P. Avdelidis, G. Tang, and A. Plastropoulos, ‘Advancements in Learning-Based Navigation Systems for Robotic Applications in MRO Hangar’, Sensors, 2024.
• H. Chi, P. Cai, D. Fu, J. Zhai, Y. Zeng, and B. Shi, ‘Spatiotemporal-restricted A∗ algorithm as a support for lane-free traffic at intersections with mixed flows’, Green Energy and Intelligent Transportation, 2024.
• R. Han et al., ‘NeuPAN: Direct Point Robot Navigation with End-to-End Model-based Learning’, arXiv preprint arXiv:2403. 06828, 2024.
• O. Maass and T. Vallgren, ‘Multi-Agent Trajectory Planning for Nonholonomic UAVs’. 2024.
• A. Prabu, ‘Integrating Data-driven Control Methods with Motion Planning: A Deep Reinforcement Learning-based Approach’, Purdue University Graduate School, 2024.
• Z. Wu, B. Zhao, X. Han, and D. Lu, ‘Berthing Trajectory Tracking of Underactuated Surface Vehicle Based on NMPC and Position Estimation’, IEEE Access, 2024.
• T. Xia and H. Chen, ‘A Survey of Autonomous Vehicle Behaviors: Trajectory Planning Algorithms, Sensed Collision Risks, and User Expectations’, Sensors, 2024.
• W. Yu et al., ‘Ldp: A local diffusion planner for efficient robot navigation and collision avoidance’, arXiv preprint arXiv:2407. 01950, 2024.
• C. Zhang, X. Wu, J. Wang, and M. Song, ‘Efficient Uncertainty-Aware Collision Avoidance for Autonomous Driving Using Convolutions’, IEEE Transactions on Intelligent Transportation Systems, 2024.
• Z. M. Al-Zubaidi, A. Y. Serdar, and A.-K. Mohanned, ‘A comparative study of various path planning algorithms for pick-and-place robots’, 2023.
• O. Angatkina, A. G. Alleyne, and A. Wissa, ‘Robust design and evaluation of a novel modular origami-enabled mobile robot (oscar)’, Journal of Mechanisms and Robotics, 2023.
• N. Boros, G. Kallós, and Á. Ballagi, ‘Implementation of Trajectory Planning Algorithms for Track Serving Mobile Robot in ROS 2 Ecosystem’, Tehnički vjesnik, 2023.
• X. Cai, Z. Xue, Z. Chen, and Y. Liu, ‘An Improved Path Planning Algorithm for USV Based on Voxel Extension’, in 2023 9th International Conference on Computer and Communications (ICCC), 2023.
• T. Guan, Z. He, R. Song, and L. Zhang, ‘TNES: terrain traversability mapping, navigation and excavation system for autonomous excavators on worksite’, Autonomous Robots, 2023.
• L. Han, L. He, X. Sun, Z. Li, and Y. Zhang, ‘An enhanced adaptive 3D path planning algorithm for mobile robots with obstacle buffering and improved Theta* using minimum snap trajectory smoothing’, Journal of King Saud University-Computer and Information Sciences, 2023.
• D. Li and K. Li, ‘Monovision End-to-End Dual-Lane Overtaking Network without Map Assistance’, Applied Sciences, 2023.
• J. Li et al., ‘Improved A-Star Path Planning Algorithm in Obstacle Avoidance for the Fixed-Wing Aircraft. Electronics 2023, 12, 5047. htps’, doi. org/10. 3390/electronics12245047 Academic Editors: Carlos Tavares Calafate and Christos J. Bouras Received, 2023.
• B. Maity, ‘Self-aware Memory Management for Emerging Architectures’, UC Irvine, 2023.
• C. Nantabut and D. Abel, ‘Weighted Hybrid A*-Based Trajectory Planning for Dynamic Environments’, SN Computer Science, 2023.
• C. Nantabut, ‘A*-based trajectory planning in dynamic environments for autonomous vehicles’, Universitätsbibliothek der RWTH Aachen, 2023.
• H. Sheikhi Darani, ‘Developing a planning and a perception module for an autonomous warehouse mobile robot’, 2023.
• H. Sui et al., ‘Multi-UAV Cooperative and Continuous Path Planning for High-Resolution 3D Scene Reconstruction’, Drones, 2023.
• Q. Yang et al., ‘Decoupled real-time trajectory planning for multiple autonomous mining trucks in unloading areas’, IEEE Transactions on Intelligent Vehicles, 2023.
• C. Zhang, Z. Li, J. Wang, and M. Song, ‘A convolution-based motion planning method for autonomous driving with localization uncertainty’, IFAC-PapersOnLine, 2023.
• I. Chichkanov and M. Shawin, ‘Algorithm for Finding the Optimal Obstacle Avoidance Maneuver for Wheeled Robot Moving Along Trajectory’, in 2022 16th International Conference on Stability and Oscillations of Nonlinear Control Systems (Pyatnitskiy’s Conference), 2022.
• T. D’Amico, ‘Developing an autonomous haulage system testbed for 5G mobile connectivity evaluation’, University of British Columbia, 2022.
• J. Huang, Z. Liu, X. Chi, F. Hong, and H. Su, ‘Search-Based Path Planning Algorithm for Autonomous Parking: Multi-Heuristic Hybrid A’, in 2022 34th Chinese Control and Decision Conference (CCDC), 2022.
• T. Miao, E. El Amam, P. Slaets, and D. Pissoort, ‘An improved real-time collision-avoidance algorithm based on Hybrid A* in a multi-object-encountering scenario for autonomous surface vessels’, Ocean Engineering, 2022.
• C. Nantabut and D. Abel, ‘A Robust Modified Hybrid A*-based Closed-loop Local Trajectory Planner for Complex Dynamic Environments’, in VEHITS, 2022.
• J. Pang, S. Zhang, J. Fu, J. Liu, and N. Zheng, ‘Curvature continuous path planning with reverse searching for efficient and precise autonomous parking’, in 2022 IEEE 25th International Conference on Intelligent Transportation Systems (ITSC), 2022.
• M. Parseh, ‘Pre-crash Motion Planning for Autonomous Vehicles in Unavoidable Collision Scenarios’, KTH Royal Institute of Technology, 2022.
• X. Tang, J. Liao, L. Liu, Y. Yan, Y. Xiao, and L. Wang, ‘Towards Inspection and Cleaning on Capacitor Towers: Design and Implementation of Multi-task Operation Robot’, in Journal of Physics: Conference Series, 2022.
• C. Zhang, M. Song, and J. Wang, ‘A convolution-based grid map reconfiguration method for autonomous driving in highly constrained environments’, in 2022 IEEE Intelligent Vehicles Symposium (IV), 2022.
• Y. Zhao, Y. Zhu, P. Zhang, Q. Gao, and X. Han, ‘A hybrid A* path planning algorithm based on multi-objective constraints’, in 2022 Asia Conference on Advanced Robotics, Automation, and Control Engineering (ARACE), 2022.
• H. Zhou, X. Zhang, and X. Peng, ‘A Robust and Efficient Path Planning System Based on the Global Euclidean Distance Fields’, in 2022 IEEE Conference on Telecommunications, Optics and Computer Science (TOCS), 2022.
• O. Angatkina, ‘Design and control of an origami-enabled soft crawling autonomous robot (OSCAR)’, University of Illinois at Urbana-Champaign, 2021.
• Y. Chung and Y.-P. Yang, ‘Hardware-in-the-loop simulation of self-driving electric vehicles by dynamic path planning and model predictive control’, Electronics, 2021.
• T. Guan, Z. He, D. Manocha, and L. Zhang, ‘Ttm: Terrain traversability mapping for autonomous excavator navigation in unstructured environments’, arXiv preprint arXiv:2109. 06250, 2021.
• T. Guan, Z. He, R. Song, D. Manocha, and L. Zhang, ‘Tns: Terrain traversability mapping and navigation system for autonomous excavators’, arXiv preprint arXiv:2109. 06250, 2021.
• G. Huang, L. Yang, Y. Cai, and D. Zhang, ‘Terrain classification-based rover traverse planner with kinematic constraints for Mars exploration’, Planetary and Space Science, 2021.
• T.-W. Kang, J.-G. Kang, and J.-W. Jung, ‘A bidirectional interpolation method for post-processing in sampling-based robot path planning’, Sensors, 2021.
• B. Maity et al., ‘Chauffeur: Benchmark suite for design and end-to-end analysis of self-driving vehicles on embedded systems’, ACM Transactions on Embedded Computing Systems (TECS), 2021.
• J. P. Moura and Others, ‘Investigação do desempenho do planejador de trajetórias Motion Planning Networks’, 2021.
• X. Shi, J. Zhang, C. Liu, H. Chi, and K. Chen, ‘State estimation and reconstruction of non-cooperative targets based on kinematic model and direct visual odometry’, in 2021 IEEE 11th Annual International Conference on CYBER Technology in Automation, Control, and Intelligent Systems (CYBER), 2021.
• B. Tang, K. Hirota, X. Wu, Y. Dai, and Z. Jia, ‘Path planning based on improved hybrid A* algorithm’, Journal of Advanced Computational Intelligence and Intelligent Informatics, 2021.
• L. Wang, Z. Ye, and L. Zhang, ‘Hierarchical planning for autonomous excavator on material loading tasks’, in ISARC. Proceedings of the International Symposium on Automation and Robotics in Construction, 2021.
• B. Zhang and D. Zhu, ‘A new method on motion planning for mobile robots using jump point search and Bezier curves’, International Journal of Advanced Robotic Systems, 2021.
• S. Zhan