Motion Planning HW #4

Due : Oct/8/2007

Problem

Either, for a three-dimensional SE(2) configuration space, implement the A* search -OR- on a two-dimensional space, implement D*

On your web page, show at least two movies - one that illustrates a successful planning episode, and one that fails. Include on your web page a brief description (pdf file or html ONLY) of your approach. This report should include all relevant equations and algorithm descriptions, along with a description of any parameters used by your algorithm.

For all movies, graphically show the order in which A* visits nodes.

In addition to the required explanations, explain why you chose a particular heurstic for the A*, D*, etc.

Solution

I implemented the D* algorithm using GUI of MATLAB.

Workspace Description : Workspace has 2-Dimension and bug has 2 DOF which is position. In this case, workspace is discretized by user defined resolution. Each unit cell has 8 neighbors and the cost between two center of unit cell is defined following.

Following examples apply resolution is set to 10x10 or 30x30 and sensor range is set to a neighbor or infinite.

- Algorithm details

The main purpose of D* algorithm is avoid dynamical obstacle which moves slower than robot. In order to do so, the arc cost parameters can change in real time. This algorithm basically applies initially dynamic backward search from goal to start using Dijkstra's algorithm which uses function only dependent on heuristic distance. After reaching start point, the algorithm starts to replan path whenever it faces unexpected obstacles (dynamic obstacles) using function of old heuristc and updated heuristic distance.

- Variable definition

X : current state of robot

Y : current state of one of neighbors of robot

b(X) = Y : backpointer of a state X to a next state Y

c(X, Y) : arc cost of a path from Y to X

r(X, Y) : arc cost of a path from Y to X based on sensor

t(X) : tag of state X (0=NEW, 1 =OPEN, 2=CLOSED)

h(X) : path cost

k(X) : smallest value of h(X) since X was placed on open list

- Main Function

1. FOR all state of X in the graph
   1. t(x) = NEW
2. ENDFOR
3. Insert(GOAL,0)
4. val = 0
5. WHILE t(START) != CLOSED AND val ~= -1
   1. val = ProcessState
6. ENDWHILE
7. IF t(START) = NEW
   1. RETURN "MISSION FAILED"
8. ENDIF
9. X = START
10. WHILE X ~= GOAL
    1. FOR each (X,Y) such that r(X,Y) ~= c(X,Y)
       1. val = ModifyCost(Y,X,r(X,Y))
    2. ENDFOR
    3. WHILE val < h(X) AND val ~= -1
       1. val = ProcessState
    4. ENDWHILE
    5. X = b(X)
11. ENDWHILE
12. RETURN "MISSION COMPLETED"

Firstly, main function initializes tag of every cell to NEW and puts GOAL to open list which means the process will start from GOAL and propagates cell searching until it reaches to START (line 1 ~ 6). Then it starts to replan from START till getting to GOAL (line 9 ~ 11). When robot moves, if it discovers discrepancy between sensor measurement of an arc cost and the stored arc cost(e.g. due to a detected obstacle). Note that these discrepancies may occur anywhere, not just in the sequence of X [2]. If discrepancies are detected, the robot modifies cost in range of sensor. For example, if the sensor range is infinite robot can dectect whole obstacle but if it has just one neighbor range, it assumes there may be the same as before about obstacles behind detected cell. This process goes recuresively along the backpointer of X.

- ModifyCost(X, Y, cval)

1. c(X,Y) = cval
2. IF t(X) = CLOSED
   1. Insert(X, h(X))
3. ENDIF
4. [X, k_min] = Pop
5. Push(X);
6. RETURN k_min

- ModifyCost function reset the cost of X, Y and insert X into open list. This means even though there might be changing in cost, heuristic distance is not changed yet. When X is expanded at ProcessState, it resets new heuristic function.

- ProcessState

1. [X, k_old] = Pop
2. IF X == -1
   1. k_min = -1
   2. RETURN
3. ENDIF
4. IF k_old < h(X)
   1. FOR each neighbor Y of X
      1. IF t(Y) ~= NEW AND h(Y) <= k_old AND h(X) > h(Y) + c(Y,X)
         1. b(X) = Y
         2. h(X) = h(Y) + c(Y,X)
      2. ENDIF
   2. ENDFOR
5. ENDIF
6. IF k_old == h(X)
   1. FOR each neighbor Y of X
      1. IF t(X) == NEW OR (b(Y) == X AND h(Y) ~= h(X) + c(X,Y)) OR (b(Y) ~= X AND h(Y) > h(X) + c(X,Y))
         1. b(Y) = X
         2. Insert(Y, h(X) + c(Y,X))
      2. ENDIF
   2. ENDFOR
7. ELSE
   1. FOR each neighbor Y of X
      1. IF t(Y) == NEW OR (b(Y) == X AND h(Y) ~= h(X) + c(X,Y))
         1. b(Y) = X
         2. Insert(Y, h(X) + c(Y,X))
      2. ELSEIF b(Y) ~= X AND h(Y) > h(X) + c(X,Y) AND t(X) == CLOSED
         1. Insert(X, h(X))
      3. ELSEIF b(Y) ~= X AND h(X) > h(Y) + c(Y,X) AND t(Y) == CLOSED AND h(Y) == k_old
         1. Insert(Y, h(Y))
      4. ENDIF
   2. ENDFOR
8. ENDIF
9. [X, k_min] = Pop
10. Push(X);
11. RETURN k_min
    

- ProcessState pops state X with minimum value of k in open list. In general if b(A) == B, then h(A) == h(B) + c(A,B) and if b(A) ~= B, then h(A) <= h(B) + c(A,B). This relation can be broken when the cost of arc is changed because of introduction of new obstacles. This is reflected here using raised state and lower state.

Lower State (k(X) == h(X)) : This propagates information about path cost reduction to its neighbor due to a reduced arc cost or new path to the goal.

Raised Stat (k(X) < h(X)) : This propagates information about path cost increases to its neighbors due to an increased arc cost.

At lines 6, each neighbor Y of X is examined to see if its path cost can be lowered. Additionally neighbor states that are NEW receive an initial path cost value, and cost changes are propagated to each neighbor Y that has a backpointer to X, regardless of whether the new cost is greater than or less than the old. Since these states are descendants of X, any change to the path cost of X affects their path costs as well. the backpointer of Y is redirected, if needed, so that the monotonic sequence Y is constructed. All neighbors that receive a new path cost are placed on the open list, so that they will propage the cost changes to their neighbor[2].

If X is raised state, its path cost may not be optimal. Before X propagates cost changes to its neighbors, its optimal neighbors are examined at lines 4 to see if h(X) can be reduced. At line 7, cost changes are propagated to NEW states and immediate descendants in the same way as for lower states[2].

- Insert(X, h_new)

1. IF t(X) == NEW
   1. k(X) = h_new
2. ELSEIF t(X) == OPEN
   1. k(X) = min(k(X), h_new)
3. ELSEIF t(X) == CLOSED
   1. k(X) = min(h(X), h_new)
4. ENDIF
5. h(X) = h_new
6. t(X) = OPEN
7. Push(X)

Insert function inserts state X into open list but before inserting it checks k value is smallest value of h.

- Blief procedure

1. Firstly, the algorithm generates Dijkstra algorithm from goal to start. the red cross means searched cell and red lines stands for the backpointer. In addition, blue circles means open list elements.

2. New obstacle is introduced in cells of (8,5), (9,5), (8,6), (9,6) with respect to the origin located left bottom. Robot moved until (8,7) and it meets new obstacle (8,6). Then (8,6) - orange cell- is inserted in open list as well as all available neighbors which are (8,5), (9,5), (9,6), (9,7), (8,7) - yellow cell. Then, it updates corresponding costs.

3. Note blue circle in (8,6) is disapeared because it was removed from open list. Cells are popped in order of (8,5), (9,5), (9,6), (9,7), (8,7) but after raising h value of (8,6) and (8,7), every cell does nothing to do but (8,7). Actually h of (8,7) is affected, or raised and then it propagates to (7,8), (8,8), (9,8).

4. (9,6) is popped and again inserts (10, 7), (10, 6), (10,5) in the same way. Then (9,6) can have high value of h and raises (10, 7).

5. (10, 6) ==> (9,7) ==> ... Cells are poped. Also (9,5) is popped and inserts (10,5), (10,4), (9,4). This procedures are repeated.

6. Finally it gets out of from obstacles area and it is free to follow backpointer.

The demonstration shows how the unit searching is propagated and how open list is updated in real time. First demo shows how it avoids and Second demo proves what if there is no path to the way of robot first move. If robot cannot find any path in direction of its move, then it also updates open list and tries to find any other optimal path. Last demo shows when the all path are blocked which means the mission failed. This demo performed in 10x10 grid and sensor range is a neighbor length.

D* SUCCESS (EASY)

D* SUCCESS (TRICKY)

D* FAILED

Other demostration is also available. First and second demonstrates when the sensor range is infinite which robot can know all configuration of obstacles so the algorithm is more efficient so that it is more faster than length of a neighbor. Final demo shows more fancy move because the resolution is much higher than before (30x30). However, it takes a lot of time to compute and search, only animation is shown in here.

D* SUCCESS (EASY - INFINITE)

D* SUCCESS (TRICKY - INFINITE)

D* HIGH RESOLUTION

- The reason why I chose heuristic function as just distance from goal was it was quite intuitive and it worked very well in condition of cost function is bidirectional and relation with its backpointer is quite clear. As I mentioned in ProcessState, in general, if b(A) == B, then h(A) == h(B) + c(A,B) and if b(A) ~= B, then h(A) <= h(B) + c(A,B). This might not really work if heuristic as sum of distance from goal and a certain function like distance from start. Then it would be neccessary different function to detect raised/lower state. However, if the algorithm should concern about others such as energy/time consumption, in order to satisfy these condition as well as shortest distance, optimal path can be changed.

- Future works

I tried to find some other algorithm related in D* and I found a bunch of algorithm such as D* Lite, DD* Lite, Focussed D* algorithm etc. These works are intended to improve more efficient than original D*. As I implemented, I found this algorithm took a lot of time if resolution becomes higher maybe in order of 3rd (I am not pretty sure about it). Anyway others should be investigated later if I have time. ^^*

- Reference

[1] H. Choset, K. M. Lynch, S. Hutchinson, G. Kantor, W. Burgard, L. E. Kavraki and S. Thrun, Principles of Robot Motion: Theory, Algorithms, and Implementations,
MIT Press, Boston, 2005.

[2] Anthony Stentz, The D* Algorithm for Real-Time Planning of Optimal Traverses, Carnegie Mellon, 1994