Python, RRT

Authors: Allen Liu

GitHub: View This Project on GitHub

Project Description

Implementation of the Rapidly-Exploring Random Tree (RRT) path planning algorithm for collision-free navigation in 2D environments with obstacles. The algorithm efficiently explores the configuration space to find feasible paths from start to goal.

System Architecture

graph TB
    subgraph DataStructure["Tree Data Structure"]
        ROOT[Root Vertex<br/>Start Position]
        VERTEX[Vertex Class<br/>Position + Parent]
        EDGES[Edge Collection<br/>Connections]
    end

    subgraph Algorithm["RRT Algorithm"]
        SAMPLE[Random Sampling<br/>Configuration Space]
        NEAREST[Find Nearest Vertex]
        EXTEND[Extend Tree<br/>Fixed Step Size]
        COLLISION[Collision Detection]
    end

    subgraph Environment["Environment"]
        OBSTACLES[Obstacle Map]
        GOAL[Goal Position]
    end

    ROOT --> VERTEX
    VERTEX --> EDGES
    SAMPLE --> NEAREST
    NEAREST --> EXTEND
    EXTEND --> COLLISION
    COLLISION -->|Valid| VERTEX
    COLLISION -->|Invalid| SAMPLE
    OBSTACLES --> COLLISION
    GOAL --> COLLISION

    style SAMPLE fill:#e1f5ff
    style COLLISION fill:#fff4e1
    style VERTEX fill:#d4edda

Algorithm Workflow

flowchart TD
    START([Initialize Tree at Start]) --> SAMPLE[Randomly Sample Point q_r]

    SAMPLE --> FIND_NEAREST[Find Nearest Vertex v_near]
    FIND_NEAREST --> EXTEND[Extend from v_near toward q_r<br/>Step size = 1]

    EXTEND --> CHECK_COLLISION{Collision<br/>Check}

    CHECK_COLLISION -->|Collides with Obstacle| SAMPLE
    CHECK_COLLISION -->|Intersects Existing Edge| SAMPLE

    CHECK_COLLISION -->|Valid| ADD[Add New Vertex v_new<br/>Connect to v_near]
    ADD --> CHECK_GOAL{Reached<br/>Goal?}

    CHECK_GOAL -->|No| SAMPLE
    CHECK_GOAL -->|Yes| BACKTRACK[Backtrack to Find Path]
    BACKTRACK --> END([Path Found])

    style CHECK_COLLISION fill:#fff4e1
    style ADD fill:#d4edda
    style END fill:#e1f5ff

Data Structure

Tree Structure:

• Tree: Contains a list of vertices and the root vertex (start position)
• Vertex: Stores position coordinates and parent vertex reference
• Edges: Implicit connections between parent and child vertices

Algorithm Steps

1. Random Sampling: Randomly select a point q_r in the configuration space
2. Nearest Neighbor: Find the closest existing vertex to q_r
3. Tree Extension: Create a new vertex by extending a fixed distance (step size = 1) from the nearest vertex toward q_r
4. Collision Detection: Validate the new edge:
   • Check for collisions with obstacles
   • Check for intersections with existing edges
5. Tree Growth: If valid, add the new vertex to the tree
6. Termination: Repeat until the goal region is reached

Amination

Planing Path on Map with Oval Obstacles

Planning Path on Map with Northwestern Logo

Challenges

• Obstacle Avoidance: In this project, I successfully tackled the primary challenge of determining the optimal line segment from a given vertex to a randomly generated point while ensuring avoidance of collisions with obstacles and existing edges. To overcome this obstacle, I leveraged vector calculus to calculate the distance between the line segment and obstacles. Additionally, I adopted a modeling approach, treating all existing vertices as obstacles with a fixed radius of 1. This strategic modeling guarantees the non-collision of newly generated vertices with their existing counterparts, contributing to the overall success of the project.

Possible improvements.

• To address the program’s suboptimal performance, I plan to implement multi-threading in the future. This approach aims to enhance computational efficiency by enabling simultaneous execution of multiple tasks, ultimately reducing the overall processing time for the entire algorithm. The strategic adoption of multi-threading is anticipated to significantly boost the program’s performance, contributing to improved responsiveness and user experience.