Skip to main content
. 2016 Jan 15;16(1):102. doi: 10.3390/s16010102
Algorithm 1. DV-RRT Algorithm

  1. Get near point qNG and far point qFG from perception module.

  2. Ta .init (qstart), Tb .init (qNG);

  3. Repeat

  4. qrand1 ← Get Random Sample 1 (Ta); qrand2 ← Get Random Sample 2 (Tb).

  5. qnear1 ← Effective_Nearest (Ta, qrand1); qnear2 ← Effective_Nearest (Tb, qrand2);

  6. qnew1 ← Extend (Ta, qrand1, qnear1); xnew2← Extend (Tb, qrand2, qnear2);

  7. if Collision Free (qnew1, qnear1) then

  8. Ta .Add_Node (qnew1); Ta .Add_Edge (qnew1, qnear1);

  9. end if

  10. if Collision Free (qnew2, qnear2) then

  11. Tb .Add_Node (qnew2); Tb .Add_Edge (qnew2, qnear2);

  12. end if

  13. Until find a collision-free path from qstart to qNG, and from qNG to qFG;

  14. T ← Connect (Ta, Tb);

  15. Return S ← Post-processing (T);

Fuction Effective_Nearest (T, qrand)

  1. Cmax ← −∞

  2. for all qi in T

  3. C ← C (qi, qrand) = w1 · dis (qi , qrand) + w2 · head (qi, qrand);

  4.   if C > Cmax

  5.      Cmax ← C; qnear ← qi;

  6.   end if

  7. end for

  8. Return qnear