Figure: SHIFT-planner algorithm architecture diagram.
SHIFT-Planner is a holistic framework for robotic coverage, path planning, and real-time trajectory optimization in dynamic or large-scale environments. It integrates:
- IKD-Tree-based A* Search Algorithms for rapid pathfinding and obstacle avoidance.
- Potential Fields for heuristic-driven obstacle avoidance.
- Segment-Based Trajectory Optimization for local path refinement.
- 3D Coverage Path Planning (3DCCP) for aerial or uneven terrain applications.
- IKD-SWOpt (Incremental KD-Tree with Sliding Window Optimization in C++).
Figure: Coverage path planning using the SHIFT-RFICP algorithm on a semantic map of arid hilly terrain.
This repository accompanies our upcoming IROS paper, aiming to encourage further research, application, and community-driven enhancements.
Paper Citation:
"SHIFT Planner: Speedy Hybrid Iterative Field and Segmented Trajectory Optimization with IKD-tree for Uniform Lightweight Coverage"
Zexuan Fan, Sunchun Zhou, Hengye Yang, Junyi Cai, Ran Cheng, Lige Liu, Tao Sun
Target Conference: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2025)
Preprint Link: https://arxiv.org/abs/2412.10706
- SHIFT-Planner
SHIFTPlanner addresses the critical need for fast, adaptive, and robust path planning in robotic systems deployed in complex and changing environments. By integrating Incremental KD-Trees for dynamic obstacle handling, potential fields for heuristic guidance, and sliding window optimization, SHIFTPlanner ensures smooth, collision-free trajectories in real-time.
A core part of this repository is IKD-SWOpt, a C++ implementation of:
- Incremental KD-Tree for dynamic and high-dimensional obstacle management,
- Sliding Window Optimization for real-time trajectory smoothing and re-planning,
- A* Search enhanced with potential fields for efficient pathfinding.
With both Python and C++ implementations available, SHIFTPlanner caters to a broad range of use cases—from quick prototyping in Python to high-performance C++ modules for production-level robotic systems.
-
KD-Tree-Based A* Search + Potential Fields
Combines speed and accuracy by fusing KD-Trees with A* and potential fields. -
Segment-Based Trajectory Optimization
Locally refines path segments near obstacles for higher precision in cluttered spaces. -
3D Coverage Path Planning (3DCCP)
Allows for uniform coverage of large-scale or uneven 3D terrains (e.g., UAV mapping). -
SciPy Optimization
Integrates L-BFGS-B or other SciPy solvers for efficient trajectory refinement. -
Modular Design
Each module (A* planner, optimization, coverage) can be used independently or together.
-
Incremental KD-Tree
Efficiently updates obstacle information in dynamic or partially known environments. -
Sliding Window Optimization
Real-time local trajectory refinement using a lightweight optimization window. -
High-Performance A* with Potential Fields
Achieves fast pathfinding with advanced heuristics for obstacle avoidance. -
2D/3D Support
Benchmarking in both 2D and 3D makes it applicable to ground robots, UAVs, or other robotic platforms. -
Extensive Visualization
Outputs images and animations that illustrate planning and optimization processes.
-
Dynamic Algorithm Experimentation
Facilitates dynamic obstacle handling and real-time path adjustments using RFICP (Radiant Field-Informed Coverage Planning). -
Static Algorithm Experimentation
Enables evaluation of path planning algorithms in static environments to benchmark performance. -
Comprehensive Simulation
Includes simulation tools for dirt cleaning scenarios, showcasing practical applications of RFICP in 2D environments. -
Visualization Tools
Provides visual feedback on algorithm performance, including dirty area maps and cleaning simulations.
SHIFTPlanner/
├── LICENSE # License file
├── README.md # Main project documentation (this file)
├── requirements.txt # Python dependencies (optional for additional tools)
├── CMakeLists.txt # Top-level CMake configuration for IKD-SWOpt
│
├── docs/
│ ├── IKD-SWOpt-Paper.pdf # Research paper (placeholder or final PDF)
│ └── images/ # Figures, diagrams, and animations
│ └── ...
├── src/
│ ├── IKD-SWOpt/
│ │ ├── CMakeLists.txt # CMake configuration specific to IKD-SWOpt
│ │ ├── docs/
│ │ │ └── images/ # Additional documentation images (if any)
│ │ ├── include/
│ │ │ ├── Astar.h
│ │ │ ├── DistanceField.h
│ │ │ └── ikd_Tree.h # KD-Tree header
│ │ ├── README.md # IKD-SWOpt specific README
│ │ └── src/
│ │ ├── IKD-SWOpt.cpp # Main executable
│ │ ├── ikd_Tree.cpp # KD-Tree implementation
│ │ └── lbfgs.hpp # Optimization library
│ │
│ ├── 2D-RFICP/
│ │ ├── SHIFTPlanner-2D-RFICP-Dynamic Algorithm Experiment.py
│ │ └── SHIFTPlanner-2D-RFICP-Static Algorithm Experiment.py
│ │
│ └── SHIFTPlanner/
│ ├── __init__.py
│ ├── kdtree_astar.py
│ ├── SHIFTplanner-3DCCP.py
│ ├── SHIFTplanner_PLANNER_TRJ_OPTIM.py
│ └── SHIFTplanner-SegmentTrjOptim.py
│
│
└── experiments/ (ongoing) # Contains experiments, configs, results
├── figures/ # Stores figures from experiment logs
└── example_run_*.md # Step-by-step instructions for replicating experiments
-
docs/images/: Contains all figures and animations used in the documentation, organized into subdirectories for clarity.
-
src/SHIFTPlanner/
Contains all the Python modules implementing the planning algorithms.__init__.py: Initializes the SHIFTPlanner as a Python package.kdtree_astar.py: Implements KD-Tree-based A* search with potential fields.SHIFTplanner-3DCCP.py: Handles 3D coverage path planning.SHIFTplanner_PLANNER_TRJ_OPTIM.py: Integrates planning and trajectory optimization.SHIFTplanner-SegmentTrjOptim.py: Manages segment-based trajectory optimization.
-
src/IKD-SWOpt/
Contains the C++ implementation of the IKD-SWOpt algorithms.CMakeLists.txt: CMake configuration specific to IKD-SWOpt.include/: Header files for IKD-SWOpt.Astar.h,DistanceField.h,ikd_Tree.h
src/: Source files for IKD-SWOpt.IKD-SWOpt.cpp: Main executable.ikd_Tree.cpp: KD-Tree implementation.lbfgs.hpp: Optimization library.
docs/: Additional documentation for IKD-SWOpt.images/: (Optional) Additional images if needed.
README.md: IKD-SWOpt specific README.
-
src/2D-RFICP/
Python scripts for dynamic and static algorithm experiments. -
experiments/
Contains experiment configurations, results, and additional documentation.figures/: Stores generated figures from experiments.example_run_*.md: Markdown files detailing example runs and their outcomes.
-
Clone the Repository
git clone https://github.com/YourUserName/SHIFTPlanner.git cd SHIFTPlanner -
Create a Virtual Environment (Optional but Recommended)
Using
venv:python -m venv shiftplanner_env source shiftplanner_env/bin/activate # On Windows: shiftplanner_env\Scripts\activate
Using
conda:conda create -n shiftplanner_env python=3.9 conda activate shiftplanner_env
-
Install Python Dependencies
pip install -r requirements.txt
If using conda, you can set up an environment file.
-
Ensure Required Dependencies
-
Build IKD-SWOpt
cd src/IKD-SWOpt mkdir build cd build cmake .. make
Note: Ensure that the
CMakeLists.txtfile is correctly placed in thesrc/IKD-SWOpt/directory as per the repository structure. -
Verify Installation
- SHIFTPlanner: Ensure that Python scripts run without errors.
- IKD-SWOpt: Check that the
IKD-SWOptexecutable is generated in thebuild/directory.
-
Navigate to SHIFTPlanner Directory
cd src/SHIFTPlanner/ -
Run Modules
python kdtree_astar.py python SHIFTplanner_PLANNER_TRJ_OPTIM.py python SHIFTplanner-3DCCP.py python SHIFTplanner-SegmentTrjOptim.py
Each script performs its designated function, such as path planning, optimization, or visualization, and may generate plots and console outputs.
-
Navigate to the Build Folder
cd src/IKD-SWOpt/build -
Run the Executable
./IKD-SWOpt
The executable performs the following:
- Environment Generation: Creates random environments with specified obstacle densities.
- Pathfinding: Utilizes A* search enhanced with potential fields to determine optimal paths.
- Trajectory Optimization: Applies sliding window optimization using B-splines and L-BFGS for smooth trajectories.
Outputs Include:
- Visualization Windows: Display paths and optimized trajectories.
- Saved Images: Automatically saves visualizations for each test case in the working directory.
-
Navigate to 2D-RFICP Directory
cd src/2D-RFICP/ -
Run Dynamic Algorithm Experiment
python SHIFTPlanner-2D-RFICP-Dynamic Algorithm Experiment.py
-
Run Static Algorithm Experiment
python SHIFTPlanner-2D-RFICP-Static Algorithm Experiment.py
-
Visual Outputs
- Dirt cleaning simulations.
- Dirty area maps visualizing algorithm performance.
Refer to the experiments/ directory for detailed experiment setups and example runs. Each example_run_*.md provides step-by-step instructions and results for specific scenarios.
Implements a KD-Tree-based A* search algorithm enhanced with potential fields to facilitate obstacle avoidance. The module builds a KD-Tree from obstacle coordinates for efficient nearest-neighbor queries, calculates a distance field, and performs A* search considering both path cost and potential field influences.
Serves as the main integrated planner and trajectory optimizer. It combines random obstacle generation, A* pathfinding with potential fields, and trajectory optimization using SciPy's L-BFGS-B algorithm. The script also includes visualization of the initial and optimized paths.
Handles 3D coverage path planning suitable for aerial drones and robots operating in uneven terrains. It generates random 3D terrains, constructs Triangulated Irregular Networks (TIN), and performs A* search in 3D space while considering obstacles. The module visualizes the terrain, obstacles, and planned paths in 3D plots.
Focuses on segment-based trajectory optimization. It identifies path segments near obstacles and optimizes them locally to refine the overall path. The script includes animation functions to visualize the iterative optimization process and integrates optimized segments back into the main path.
The main executable for the IKD-SWOpt component. It initializes the planning framework, generates random environments, performs pathfinding using A* with potential fields, and optimizes trajectories using sliding window optimization.
Implements the Incremental KD-Tree data structure for efficient management and querying of dynamic obstacles in high-dimensional spaces. The KD-Tree supports real-time updates, insertions, and deletions, making it suitable for dynamic environments.
A header-only library implementing the Limited-memory Broyden–Fletcher–Goldfarb–Shanno (L-BFGS) optimization algorithm. It is used for trajectory optimization within the sliding window framework to ensure smooth and feasible robot paths.
Conducts dynamic algorithm experiments using the 2D-RFICP framework. This script simulates obstacle environments and evaluates the algorithm's performance in real-time path adjustment and obstacle avoidance.
Performs static algorithm experiments within the 2D-RFICP framework. This script assesses the path planning and coverage performance in static obstacle environments, providing baseline metrics for comparison.
SHIFTPlanner and IKD-SWOpt provide extensive visualizations to illustrate planning processes and trajectory optimizations. Below are examples of the types of visualizations available. Replace the placeholders with your actual files as needed.
Figure: The coarse A* path generated using the IKD-tree and its corresponding distance field representation.
Figure: Optimization process of trajectory segments within the sliding window of the IKD-SWOpt framework.
Figure: Velocity Planning Algorithm Based on 2D/3D-RFICP and Semantic Maps.
Figure: Trajectory planning visualization using the SHIFT Planner framework.
Figure: Optimization process based on the IKD-Tree.
Figure: Demonstration of the IKD-SWOpt algorithm in 3D environments for obstacle avoidance and path optimization.
Figure: Visualization and optimization process of the distance field map based on the IKD-SWOpt algorithm.
Figure: Visualization of the trajectory optimization process for segments detected by the sliding window in the SHIFT algorithm.
Figure: Optimization process for 2D obstacle-avoidance trajectory segments.
Figure: 2D-SHIFTPlanner Coverage Planning Process, 2D-RFICP Dirty Removal Cleaning Simulation.
The animations highlight the Incremental IKD-tree Sliding Window Optimization (IKD-SWOpt) approach, designed for real-time obstacle avoidance and trajectory refinement in dynamic environments. Here's an overview of the process:
-
Dynamic Obstacle Detection:
When the planned path encounters obstacles, the Circular Judgment and Assessment Module continuously scans within a defined circular detection window. Regions of the path that violate the cost function (e.g., collision risks) are flagged. -
Adaptive Sliding Window Optimization:
Identified problematic path segments are incorporated into an Adaptive Sliding Window. Within this localized window, the path is iteratively optimized using IKD-SWOpt, ensuring smooth and collision-free trajectories. -
Iterative Refinement:
The framework continues to refine the trajectory until a feasible path is achieved, avoiding unnecessary computational overhead by only activating optimization when required. -
Superior Performance:
Extensive experiments with drones and robotic vacuum cleaners in real-world scenarios validate the method's effectiveness. The approach achieves:- Efficient Coverage: Uniform and thorough area traversal.
- Dynamic Adaptability: Real-time obstacle avoidance and re-planning.
- Computational Efficiency: Optimization is performed selectively, reducing system load.
This repository currently includes:
- Python implementations of core functionalities, focusing on pathfinding, trajectory optimization, and experimental visualization.
- Experimental results showcasing the proposed framework's performance across diverse environments.
Planned Updates:
- Incremental release of additional Python modules, including detailed scripts for trajectory optimization and real-time obstacle avoidance.
- Further visualization assets and experimental results.
This phased release strategy ensures accessibility to the community while maintaining high-quality documentation and reproducibility.
Note: Some advanced features, including full IKD-SWOpt integration for 3D environments, will be made available in future updates.
-
Pathfinding Performance
Demonstrates low collision rates, short paths, and fast computations in cluttered 2D and 3D environments. -
3D Coverage
Shows uniform coverage on random 3D terrains, validated with metrics like coverage percentage vs. flight path length. -
Segment-Based Optimization
Illustrates local path refinements near obstacles through animated optimizations, significantly reducing collisions in complex corridors. -
IKD-SWOpt
Achieves real-time re-planning in dynamic environments with moving obstacles, ensuring smooth, collision-free trajectories. -
2D-RFICP Demonstrated robust performance in environments with moving obstacles.Provided baseline metrics for path planning efficiency and coverage.
Extensive experiments conducted with drones and robotic vacuum cleaners in complex real-world environments validate the effectiveness of our approach. The proposed framework demonstrates superior coverage performance and adaptability compared to existing methods, achieving efficient, uniform, and obstacle-aware trajectories suitable for autonomous systems operating in dynamic environments.
Refer to the experiments/example_run_*.md files for reproducible experiments and performance graphs.
If you use SHIFTPlanner or IKD-SWOpt in your academic work, please consider citing:
@inproceedings{fan2025shiftplanner,
title={SHIFT Planner: Speedy Hybrid Iterative Field and Segmented Trajectory Optimization with IKD-tree for Uniform Lightweight Coverage},
author={Fan, Zexuan and Zhou, Sunchun and Yang, Hengye and Cai, Junyi and Cheng, Ran and Liu, Lige and Sun, Tao},
booktitle={Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
year={2025}
}This project is licensed under the MIT License. You are free to use, modify, and distribute it as long as you retain the original license and attribution notices.
Contributions, bug reports, and feature requests are highly welcome! Please:
- Fork this repository.
- Create a new branch (
git checkout -b feature/new-awesome-feature). - Commit your changes.
- Submit a pull request to the main branch.
We appreciate community engagement to refine and extend SHIFTPlanner.
For inquiries, feedback, or support, please reach out via email:
- Zexuan Fan
mail to FanZexuan
Project Link: SHIFTPlanner (demo pages, updates, and documentation)
Notes:
- The most critical Radiant Field-Informed Coverage Planning (RFICP) algorithm, speed planning code based on semantic maps, as discussed in the paper, will be open-sourced in a future update.
Happy Planning!
© 2025 SHIFTPlanner Team. All rights reserved.