Path Finding using Rapidly-Exploring Random Tree

From Wikipedia,

A Rapidly-exploring random tree (RRT) is a data structure and algorithm designed for efficiently searching nonconvex, high-dimensional search spaces. The tree is constructed in such a way that any sample in the space is added by connecting it to the closest sample already in the tree.

(ns rrt
  (:refer-clojure :exclude [+ - * =])
  (:use (clojure.contrib.generic [arithmetic :only [+ - *]]
				 [comparison :only [=]]))
  (:use vector-2d.core)
  (:require kdtree))

The basic idea of the algorithm is very simple, RRT will search for a path from the start state to the goal state by expanding the search tree, algorithm is as follows,

Initialize the tree with the starting point as root
Pick a random point within the valid parameter space
Search the one vertex in the tree which is nearest to the random point chosen in 2.
Move a certain distance from this vertex in the direction of the chosen point and create there a new leaf
Loop over step 2. to 4. while the break condition is not satisfied

Following demonstrates RRT path finding on a 2D surface. Starting at the goal, search tree will rapidly expand through the space towards the goal. rrt-plan pretty much summarizes all of the above steps, choose-target either picks a random point on the map or returns the goal, nearest returns the closest point on the tree to the chosen target. explore then checks if we can extend the tree towards the point or not. epsilon is the distance we extend the tree by and p-goal is used to guide the search towards the goal. rrt-plan returns when we add a point to the tree that is closer than epsilon to the goal,

(defn search-world [width height & obstacles]
  [width height obstacles])

(defn explore [[_ _ obstacles] u v epsilon]
  (let [explored (+ u (to-cartesian epsilon (:t (to-polar (- v u)))))]
    (when-not (some true? (map #(let [[c r] %]
				  (point-in-circle? explored c r)) obstacles))
      explored)))

(defn nearest [kdtree {x :x y :y}]
  (apply vector-2d (:point (kdtree/nearest-neighbor kdtree [x y]))))

(defn choose-target [[width height] p-goal goal]
  (if (< 0 (rand) p-goal)
    goal
    (vector-2d (rand width) (rand height))))

(defn rrt-plan 
  ([world start goal epsilon p-goal]
     (let [{:keys [x y]} start]
       (rrt-plan world goal epsilon p-goal {start nil} (kdtree/build-tree [[x y]]))))

  ([world goal epsilon p-goal tree kdtree]
     (let [target (choose-target world p-goal goal)
	   nearest (nearest kdtree target)]
       (if (< (dist nearest goal) epsilon)
	 (rrt-plan tree (list nearest) (tree nearest))
	 (if-let [explored (explore world nearest target epsilon)]
	   (let [{:keys [x y]} explored]
	     (recur world goal epsilon p-goal
		    (assoc tree explored nearest) (kdtree/insert kdtree [x y])))
	   (recur world goal epsilon p-goal tree kdtree)))))

  ([tree path node]
     (if (nil? node)
       [tree path]
       (recur tree (conj path node) (tree node)))))

The Rapidly-exploring Random Tree algorithm is extremely simple and cheap to calculate but it is not a silver bullet, you will get a path quick but it is not guaranteed to the be the cheapest plus you will get a different path for every search.

(let [draw-circle (fn [g pt rad color]
		    (let [[x y] (vals pt)
			  offset (int (/ rad 2))
			  x (- x offset)
			  y (- y offset)]
		      (.setColor g color)
		      (.fill g (java.awt.geom.Ellipse2D$Double. x y rad rad))))

      draw-line (fn [g start end color]
		  (let [[x1 y1] (vals start)
			[x2 y2] (vals end)]
		    (.setColor g color)
		    (.drawLine g x1 y1 x2 y2)))]

  (defn view [world start goal epsilon p-goal]
    (let [[width height obstacles] world
	  [tree path iter] (time (rrt-plan world start goal epsilon p-goal))]
      (println iter)
      (doto (javax.swing.JFrame. (str "epsilon " epsilon " p-goal " p-goal))
	(.add (proxy [javax.swing.JLabel] [] 
		(paint
		  [g]
		  (.setRenderingHint g
				     java.awt.RenderingHints/KEY_ANTIALIASING
				     java.awt.RenderingHints/VALUE_ANTIALIAS_ON)

		  (draw-circle g start 10 java.awt.Color/RED)
		  (draw-circle g goal 10 java.awt.Color/RED)

		  (doseq [[center radius] obstacles]
		    (draw-circle g center radius java.awt.Color/BLUE))

		  (doseq [[a b] tree] 
		    (when (not (nil? b))
		      (draw-line g a b java.awt.Color/BLACK)))

		  (doseq [c path] 
		    (draw-circle g c 5 java.awt.Color/RED)))))

	(.setSize width height)
	(.show)))))

Define the world,

(def w (search-world 640 480
		     [(vector-2d 100 100) 30]
		     [(vector-2d 200 200) 30]
		     [(vector-2d 300 200) 30]
		     [(vector-2d 400 300) 30]
		     [(vector-2d 280 350) 30]
		     [(vector-2d 250 200) 30]))

and search,

(view w (vector-2d 10 10) (vector-2d 600 400) 15 0.3)

(view w (vector-2d 10 10) (vector-2d 600 400) 15 0.1)

(view w (vector-2d 10 10) (vector-2d 400 400) 50 0.3)

To give you an idea about its performance following table shows the average time for 1000 path finds with various epsilon and p-goal values,

core=> (print-rrt-test-table)
=======================================
:epsilon | :p-goal | :time
=======================================
15       | 0.1     | 27.412393000000016
15       | 0.2     | 20.784910000000007
15       | 0.3     | 19.029839999999993
15       | 0.4     | 17.95052699999999
15       | 0.5     | 18.672436999999984
30       | 0.1     | 8.090104
30       | 0.2     | 6.6845480000000075
30       | 0.3     | 6.206767000000004
30       | 0.4     | 5.905667
30       | 0.5     | 6.293392000000007
50       | 0.1     | 3.4616730000000002
50       | 0.2     | 2.7482080000000018
50       | 0.3     | 2.4386669999999984
50       | 0.4     | 2.429180000000003
50       | 0.5     | 2.5016600000000064
=======================================

and for comparison A* search times for the same map for various grid sizes,

core=> (print-a-star-test-table)
===============================
:grid-size | :time
===============================
50         | 15.309023999999988
30         | 15.811200999999986
15         | 45.569509
===============================