D* Lite: Can you change the start node based off actual robot location?

Question

I'm reading through this Koening and Likhachev paper on D* Lite and each iteration it updates the starting node by traversing to a connecting node on the graph. I'm wondering about the use in real world robotics where the robot might overshoot a connecting node and end up at a different point in the graph. Does D* Lite still work if you keep the rest of the algorithm the same while setting the starting node yourself based on the robot's actual location. In particular can these two lines of pseudo code from the paper

{26’}  s_start = argmin s'∈Succ(s_start)(c(s_start, s') + g(s'));
{27’}  Move to s_start;

Become

       s_start = actual robot location on graph

Here is the full pseudo code from the paper:

procedure CalculateKey(s)
{01’} return [min(g(s), rhs(s)) + h(sstart, s) + km; min(g(s), rhs(s))];

procedure Initialize()
{02’} U = ∅;
{03’} km = 0;
{04’} for all s ∈ S rhs(s) = g(s) = ∞;
{05’} rhs(sgoal) = 0;
{06’} U.Insert(sgoal, CalculateKey(sgoal));

procedure UpdateVertex(u)
{07’} if (u ≠ sgoal) rhs(u) = min s'∈Succ(u)(c(u, s') + g(s'));
{08’} if (u ∈ U) U.Remove(u);
{09’} if (g(u) ≠ rhs(u)) U.Insert(u, CalculateKey(u));

procedure ComputeShortestPath()
{10’} while (U.TopKey() < CalculateKey(sstart) OR rhs(sstart) ≠ g(sstart))
{11’}   kold = U.TopKey();
{12’}   u = U.Pop();
{13’}   if (kold ˙<CalculateKey(u))
{14’}     U.Insert(u, CalculateKey(u));
{15’}   else if (g(u) > rhs(u))
{16’}     g(u) = rhs(u);
{17’}     for all s ∈ Pred(u) UpdateVertex(s);
{18’}   else
{19’}     g(u) = ∞;
{20’}     for all s ∈ Pred(u) ∪ {u} UpdateVertex(s);

procedure Main()
{21’} slast = sstart;
{22’} Initialize();
{23’} ComputeShortestPath();
{24’} while (sstart ≠ sgoal)
{25’}   /* if (g(sstart) = ∞) then there is no known path */
{26’}   sstart = argmin s'∈Succ(sstart)(c(sstart, s') + g(s'));
{27’}   Move to sstart;
{28’}   Scan graph for changed edge costs;
{29’}   if any edge costs changed
{30’}     km = km + h(slast, sstart);
{31’}     slast = sstart;
{32’}     for all directed edges (u, v) with changed edge costs
{33’}       Update the edge cost c(u, v);
{34’}       UpdateVertex(u);
{35’}     ComputeShortestPath();

score 1 · Answer 1 · answered Apr 28 '20 at 10:17

Yes you can do it that way. Everytime you move the robot from one vertex/grid cell to another, you call upon the procedur Main() (only call initialize the first time you run procedure Main). Set sstart = robot position, then slast = sstart. Now the algorithm will compute the shortest path from the robots current position to the goal vertex. You have to call upon procedure Main() each time the robot is within the circle of acceptance of a new grid cell. I have implemented the code like this myself. Here is my stack overflow question related to my implementation of the algorithm.

D* Lite: Can you change the start node based off actual robot location?

1 Answers1