Int ern at i onal  Journ al of  R obot ic s  and  Autom ati on (I JRA)   Vo l. 8 ,   N o. 4 ,   D ece m ber   201 9 ,   pp.  277 ~ 292   IS S N:  20 89 - 4856 ,   DOI: 10 .11 591/ i jra . v 8 i 4 . pp 277 - 292     277       Journ al h om e page :  http ://i ae score. c om/j ourn als/i ndex. ph p/IJRA   Envir onment  m ap pin g ,   m ap  c onstructin g ,   and  p ath   p lanni ng  for  u nd erw ater  n av i gation  of  a  l ow - cost   µAUV  in  a  c lutte red  nu clear storag e p ond       Yibin Pe ng ,   P eter  N  Gree n   School  of El ec tr i ca l   and   Elec t roni cs  Engi n ee rin g ,   t he  Univer si t y   of   Manc heste r ,   UK       Art ic le  In f o     ABSTR A CT    Art ic le  history:   Re cei ved   A pr   26 ,   201 9   Re vised  A ug   27 ,   2019   Accepte d  Oct  6 ,   201 9       Thi s  pap er   pre sents  a   nove l  appr oac h   th at  ena bl es  a   low - cost  µA UV     (m ic ro  au tonomous   under wat er   vehicl e)  n avi g at e   and   work  s afe l y   in   an   enc losed   c lut t er e d  und erwa t er  e nvironment.  In   orde r  to  ac h ie ve   a   au tonom y   col li sion - f ree  na viga ti on   in   unde rwate r ,   i t  r equi r es  a   reliab l e  ap proa ch  that   would  a ll ow  a   µA UV   which  e quippe d   with   sparse  and   inacc ura te  sonar   sensors   to  m ap  the   und erwa t er  envi ronm ent ,   a   m ap  const ruc ti n g  al gor it hm   tha t  conve r ts  the  obta in ed  d at a   to   useful  information  to  esta bl ish  a  m ap ,   and a   pat h   pl anni ng   a lgori thm  tha t   p l ans  a   collision - fre e   pa th   from   the  star t  to     the   d esire d  go al.   The  proposed  a pproa ch  wi ll   integra t e  env ironment  surve y ,   envi ron m ent   re c onstruct ion ,   and   pat h  pl anni ng  as  a  comple te d  ta sk ,   which   invol ves   m ap  a cqui siti on   and  pat h   pla nn ing.  A  complete   sim ula ti on   o f     the   envi ronm en t  topo log y   ac q uisit ion   and   r oute   p la nn ing  proc ess  is   documente d  in t his pa per .   Ke yw or d s :   A* p at h plan nin g   al gorithm   Aer ia l m app in g gr i d  m ap  reconstr uctio n and i nter pola ti on   Au t onom ou s  unde r water  veh ic le   Ma p gr id ding   Po int cl oud  m ap   Copyright   ©   201 9   Instit ut e  o f Ad vanc ed   Engi n ee r ing  and  S cienc e .    Al l   rights re serv ed .   Corres pond in g  Aut h or :   Yib in  Pe ng ,   Sch oo l  of Elec tric al  an d El ect ronics E nginee rin g ,   The U niv er sit y o f  Ma nc hester ,     Oxfor d  Rd ,   Ma nch e ste r  M 13  9P L ,   G reat M a nch e ste r ,   UK .   Em a il :  yib in.peng@m anch est er.ac. uk       1.   INTROD U CTION   Investi gatio ns  on  the   a utonom ou s  ve hicle   hav e   been  m ade  sig nificant   pro gr e ss   ov e r   the  past  few   ye ars.   Ba se d  on  it s  w orki ng  env i ronm ent ,   auto no m ou s   ve hicle s  can   be   c at egorised  i nto  three   ty pes ,   t he   one  work i ng  i n  t he   ai r   is  cal le d  unm ann ed   ae rial   ve hicle s  ( UAVs )  s uc h  as  dron e s ,   w hi ch  is   us e d  to   film ,   env i ronm ental  s urveil la nce ,   g eo grap hy  st udy ,   a nd   eve n  par cel   delive ry .  T he   one   w or king  i n  the   gr ound  is  cal le d  un m anned  gro und  vehi cl e  (UG V)  s uc h  as  sel f - dr i vi ng  car s ,   w hic h  is   us e d  t o  r eplace   hum an  pilot.   More ov e r ,   t he  on e  w orkin g  unde r water  is  ca ll ed  auto no m ous  un derwate r  veh ic le   ( A UV) .  This  pap e r  int erests   at  A U V u nd e r water a ppli cat i on.   A  di ver sit y  of   AUVs  has  al r eady  bee n  devel op e d  in  dif fe ren t  pur po ses   and   de plo ye d  in  dif fer e nt  unde rw at er   sit uations ,   w hich   inclu ding   in dustria l  process es ,   m arine  res earch ,   a nd  ai r   cras h  in vestig at io ns .   Th os e  in dustri al   processes   in cl ud e  but  are   not  lim it ed  to  unde rsea  oil  pi pe li ne  le aka ge  de te ct ion   [ 1]  a nd   s pe nt  nu cl ea r  f uel  st or a ge  pond  m on it ori ng   [2] .  The  ocea n  res earches   inclu de   unde rsea  oil  ex plo it at ion ,   m arine   stud y   an d  in form ation   c ollec ti on ,   o cea n  sea rch  a nd  r escue   [ 3] .  As   f or  ai r  c ras h  i nv e sti gations ,   A U Vs   ha ve   been u sed  to find w rec kag es   of m issi ng  aer op la nes ,   su c h  a s Mal ay sia  A ir li nes  Fli ght 3 70 acci de nt   [4] .   This  work  is  m ot ivate d  by  t he  nu cl ea r  sto r age  pond  de co m m iss ion in g  pro blem .  Stor ag e   ponds  a re  us e d  t o  kee p  sp e nt  fu el   ass e m blies  an d  ot her  ra dio act iv e  m at erial   unde rw at er   unti l  they   a re   suffici ently   ben i gn  to   be   s ubj ect ed   to   processin g  f or  lo ng  te rm   safe  st or a ge.  I n  te rm s  of  siz e ,   Olym pic  swi m m ing   pools   pro vid e  a   c onve nient b e nch m ark ,   al th ough  s tora ge  ponds  a re  ty pical ly   m u ch  dee per .  W a ste   obj ect s   are  p la ce d  Evaluation Warning : The document was created with Spire.PDF for Python.
                IS S N : 2089 - 4856   I nt  J   R ob  &  A uto m ,   Vo l.   8 ,   No.  4 ,   Decem ber   2019  :   277   –   292   278   on  t he  bott om   of  the   pond   wi th  a   si gn ific a nt  de pt h  of  cl ea r   wate r   ab ove  f or  s hieldin g  pu rposes.  T he   m at erial   rem ai ns   within   a  pond  for  a   num ber   of  ye a rs .  T he  s pe nt  nucl ear  f uel  st or a g e  pond  c onta ins  highly   ra dioa ct ive   m et al s  su ch   as   U ra niu m   an d  Th or ium   [ 5 ] ,   wh ic h  are   haz ardo us  to   hum an   healt h.  Plac es  s uc h  as   t he se  a re   near ly   im po ssible  for  hu m ans  to  wor k  in  e ve n  wea rin g  a  protect ive  s uit.  I n  su c h  ci rc um s ta nces ,   A U Vs  cou l d  po te ntial ly   b e   us e d  t o  re place   hum ans  t o  m on it or  for   le akin g  co ntainer s ,   to  fin d  the   disposit ion  of  m at e rial   i n  old e r pon ds ,   a nd to  explo re t he   unkn own u nder water e nvir on m ent.   Ba sed  on  t he  e ra  of  c onstructi on ,   e xisti ng  st orage   ponds  ca n  be   cl assifi e d  a s  le gacy   po nd s   or   m od e rn   ponds.  The   la tt er  te nd  to   be   equ i pp e d  with   regular   sto rage  rac ks   desig ne d  to   hold   can ist ers  co ntaini ng  the   rad i oacti ve  m a te rial .  He nce  t he  distrib ution  of   cl utter  (the  so li d  obj ect s  w it hin   the  pond )   is  re gu la r   or  a t  le ast  well - orga nise d  [ 2 ] .  Fi gure   1  is  an  e xam ple  of   a  m odern   pond.  Le gacy  ponds  a r e  a  diff e ren t  m at te r.   Or i gin at in g  f rom   tim es  wh en   the  haza rds  of  storing  nu cl ea r   m at erial   wer e  no t  as  cl ea rly   unde rstood  a s  they   are  now ,   an d  ha nd li ng  te ch no log y  was  le ss  well   dev el op e d ,   le gacy  ponds  con ta i n  m at erial   that  was  or i gin al ly   store d  in  a   ha ph aza r d  m anner  in  s kips  ( du m ps te rs) .  S ome   m a te rial   in  le gacy  po nd s   ha s  bee n  im m ersed   for  m any ye ars  an d has  par ti al ly  d isi nteg rated  [ 2 ] . F i gure  2  is  an  e xam ple o f t he  le gacy  pond.           Figure  1. Mo de rn nucle ar  s to rag e  po nd [6]           Figure  2. Lega cy  n uclea r  sto r age  pond  [7 ]       It  is  e vid e ntly   desi rab le   that   co ndit ion s   in   both   cl asses   of  po nds  be   m on it or ed ,   al thou gh  this   is    a  pote ntial ly   hazardo us   e nd e avou r.   In   t he  case  of  m od er n  ponds ,   it   is  help fu l  to   che ck  f or  l eaka ge   from   canist ers  s o  th at   rem edial   work  can   be   car ri ed  ou t   in  a   ti m el y  m ann er.  Fo r   le gacy  po nd s ,   it   is  neces sary  to   determ ine  the  disp e rsal  of  ob j ect s  withi n  a  pond  an d  the  di stribu ti ons  of  rad i oacti vity ,   tem per at ure  an d  ot her   par am et ers  thr oughout t he   po nd ,   s o  t hat p la ns f or  m at erial  r e m ov al  and l ong - te rm  stor ag e can be  f or m ulate d.   The   resea rch  r eported   in   this   pap e r   buil ds  on  w ork  on  sm a ll - scal e ,   low - c os t  AUVs   ( µ A UV s )   that  has   been  unde rw ay   in   Ma nc hester   f or  se ver al   ye ars  [ 8 ,   9 ] ,   res ul ti ng   i n  the   A V EX IS   ve hicle   [ 8 ,   9 ].  AV E X IS   ha s   4  degrees  of  fr e edo m .  Sup port ing   c omm un ic at ion   a nd  local isa ti on   syst em s  are   cu rr e ntly   unde r  de velo pm ent.  Evaluation Warning : The document was created with Spire.PDF for Python.
I nt  J   R ob  &  A uto m   IS S N:  20 89 - 4856       Enviro nme nt  m apping  m ap constructi ng ,   an d p ath   plan ning  for... (Yi bin Pe ng )   279   On e   ai m   of  th e  A VE XIS   w ork   is  t o  de velo p  a   s war m   (‘s ho al ’)  of   A U V s  a ble  to   e xp l ore ,   m ap  an d  m on it or   stora ge  ponds.   Su c h  a   syst em   requires   m app ing  a nd  path   pl ann i ng  ca pab il it ie s  and  this   pa per  disc us ses   init ia l  effor ts t o pro vi de  this  k i nd of  functi onal it y.   The  pro blem   of   the   µ AUV   na vig at io n  i n  a   stora ge  pond  i s  ho w  t o  e nab l es  a  µ A UV  t ra ver se   f r om    the  sta rt  posit ion   t o  the  g oa l  po sit io n  (t he   destinat io n  w her e  t he  r obot   hea ding  to )  wh il e  a vo i ding  any  ob sta cl es  in  it s  env i ronm ent  without  the  help  of   GP S  (G P S  w orks  ba dly  in  unde r water)  [ 10 ].   [ 11 - 13 ]   introd uce  se ve ral  na vig at io n  strat e gies  f or   m ob il e  r obots .  3D  path   plan ning  in   a   cl uttered  pond  is     a  par ti cula rly   interest in g  c halle ng e  si nce  it   r el ie s  on  3D  m a ps  w hose  acc ur acy   m ay   no t  be   high ,   a nd  c olli sion s   betwee n   r obots   an d  cl utter  m ay   ha ve  se rio us  conseq ue nces  f or  the   r obot ,   th e  en vir onm ent ,   bysta nde rs ,   or   al l   three.   M or e ove r ,   t he   possi ble  appr oach es   are   co ns trai ne d  by  siz e  an d  co st  issues.   Cl early ,   the  µ A UVs  ne ed  to   be   sm all  en ough  t o  na vig at e   t hro ugh  co nfi ne d  sp ace s ,   an d  a   ke y  obj ect ive   of  this   wor k  ha s  been  t o  kee p  costs  low  t o  e nab le  the syste m  to  be  u se d  i n  a  var i et y of   wat er - ba sed processe s.   Howe ver ,   so   fa r ,   m os t  appro a ches  s uch   a s  F orward - Lo okin g  S on a r  [ 14] ,   Mult i - Be a m   So na r  [ 15]   has   been   c onduct e d  in  e xp e ns i ve  big - siz ed  AUV   for  m arine  res earch .  This p a per  p r opose s  a n  ap proac h  to ad dress  the  pond  m appi ng  an d  pat h  pl ann i n g  pro blem   fo r  a   se nsor - lim it ed  low - c ost   µA UV ,   w hich  c onsist s  of  3  ste ps .   Step  1 ,   a   lo w - cost  AUV   wit h  s pa rse  a nd  i naccurat e  s ona r  se nsors   to   m ap  t he  unde rwat er  e nv ir onm e nt  a nd   gathe r  the   dept h  m easur em e nt  data.  Ste p  2 ,   a  m ap  co nst ru ct io n  that   c onve rts  the   ob t ai ned   data  t o  us ef ul   inf or m at ion   to   est ablish  a n  oc cup a ncy  gr id .   Step  3 ,   A*   pa th - plan ning  a lgorit hm   that  plans  a  c olli sion - f ree   path fr om  the s ta rt to  the  d e sir ed go al .     The  e nv i ronm ent  m app ing   s trat egy  is  bas ed  on  t he  ide a  of   a n  ‘aeria l  su r vey’.   I n  this  c onte xt ,     an  aerial   s urve y  is  si m ply  a  set   of   de pth   m easur em ents  ta ke n  on   a   re gu la r   m easur e m ent  gr i d  at   a  fi xed  dep t h   within   the   cl ear  water   re gion  of  the   po nds.  See  Fi g ure   3 .   The  res ult  of   a   sur vey  is  a   m at rix  of  tu ples ,   w hos e   com po ne nts  a r e  m easur em en t  po sit ion   a nd  corres pondin g  dep t h.   This   da ta   is  the  2D  pro j ect io n  of  th e  3D   cl utter  on  to  th e  m easur em ent  plane  a nd   ca n  be  us e d  to  c on struct  a  hei gh t  m ap  of   t he  ob je ct s  at   the  bo tt om   of   the  pond.  T hi s  in  it sel f   pro vid es   us e f ul  da ta   ab ou t  t he  disposit ion  of   m a te rial   within  a   po nd.  Howev e r ,     a  sig nifica nt  a m ou nt  of   inf or m at ion   is  l os t  and  heig ht  m aps   do   not  pro vid e   ade quat e  su pp or t   f or  th e  pat h  plan ning  nee de d for  detai le d  e xp l or at io n.  What  is reall y nee ded is a  3D r e presentat io n of t he pon d  cl utter .           Fig ure   3.   A  com p l e te  aerial  m app in g  a nd pat h  pla nn i ng in  a clutt ere d  e nc losure       In   this  pa per ,   the  ad van ta ge  is  ta ken   of  the   fact  that  m os t   cl utter  in  stora ge  po nd s  is  pri sm a ti c  in  sh a pe  i.e.  obj e ct s  hav e   a  pol ygonal   base   a nd   a   cr os s - sec ti on   that  do e s   no t  var y  with   heig ht   ( c uboi d  an d  cy li nd rical   co nt ai ner s  that  ar e  ty pical ly   us ed  in   m os t  ponds ) .  T he  a ppr oa ch  ou tl ine d  i n  this   pa per  locat es   cl utter  f r om   a  heig htm ap  and  us e s  t his  in f orm at ion   to   ge ne rate  3D  po i nt  c loud  m ap  of  pri s m at ic   ob j ect s   f ro m   wh ic h  a  f ull 3 D  g ri d  m ap  of  the po nd can  be co ns tr ucted  t hat is s uitable  for  A *  path  p la nn i ng alg or it hm s.   In   orde r  to   ens ur e  t hat  the  A *  path  plan ning  al gorit hm   wo r ks   with  th e  ob ta ine d  3D  gri d  m ap ,   both   processes  m us t  be  perf or m e d  in  the  sam e  coord i nate  s yst e m   and   the  dep t h  m easur em ent  at   ref le ct ion  po sit io ns   m us t  li e  on  the   use r - s pecified   s urvey  gr i d.   The   survey  gri d  i s  the  l ocati on  of  m esh  poin ts  i n    the  m easur em e nt  plane  w her e   de pth  m easur e m ents  are  ta ke n  (as  i nd ic at e d  in  Fig ur e   1)  a nd  the   A *  m eth od   is  app li ed   to   a   w orks pace  base d  on   a  discreti z ed  re pr ese ntati o n  of  the   e nv ir on m ent’s  ge om et ry .  In  this   syst e m ,   the  A *  sea rc hin g  gri d  is   a  3D  gr id   ba sed   on  the  2D  s urvey  gri d  but  ha s  the  sam e  interval  in   the  z - axis.   Evaluation Warning : The document was created with Spire.PDF for Python.
                IS S N : 2089 - 4856   I nt  J   R ob  &  A uto m ,   Vo l.   8 ,   No.  4 ,   Decem ber   2019  :   277   –   292   280   Be cause  the re  is  of te n  unli kely   that  the  sa m pled  locat io ns   wil l  coincide  with  the  s urve y  gr id ,   su c h  as  the  case   that  incli ne d  obj ect s   cau se  th e  de pth  m easur em ent  at   po si ti on s  do  no t  li e  on  t he  s urve y  gr i d.   The refor e ,   it  requires   f urt he r  data  processi ng  to   m ake  the m   con sist ency.   T his   pap e r  e xpla ins   how   to   est ablish  a   3D  dig it al   gr i d  m ap  base d o n raw   de pth  m easur em ent   da ta  an d p r ov i de s so m e si m ulati on   resu lt s.       2.   A ERI AL   MAPPIN G   The  te rm   of   “ Aer ia l  m app in g”  or  “aeria l  s urvey”  ori gin a l ly   co m e  fr om  an  ai rbo rn e  l and   s urvey ,   wh ic h  is  a n  ef f ic ie nt  m et ho d  of   m app in g  ge ogra ph ic   featu r e  of  la rg e   areas .  It  is  al so  kn own  a s  Ec ho - s oundin g  so na r  in   the  under sea  im age  and   i nvest igati on  resea rc h.   Si de - scan  us es   sonar  de vices  tha t  produce  a  c onic al   or   fan - s hap e d  beam   do w n  to   the  seaflo or  with  a  wi de  be a m   that  per pe nd ic ular  to  t he   path  of   t he  sens or  thr ough  the   wa te r  to   gat her  th e  ge ogra ph ic   inf or m at ion   of  the  sea be d  [ 16 ].  Ech o - sou nd i ng  us es   s on a r  de vices   that  pro du ce   a  con ic al   beam   do w nwar d  to  t he   pond  bo tt om   with  a  wide  be a m   ang le   pe r pe nd ic ular  to   th e  path  of the se nsor  th rou gh the  wate r   to  m easur e th e d e pth   of the   bo tt om   [17] .   This  m app i ng  al gorithm   is  sim pl y  a  set   of  de pth  m easur em ents  ta ken  on  a   re gula r   m easur em ent  gr i d  at   a  fixed   de pth   withi n  the  cl ear  water   re gio n  of  the  pond s.  I n  pract ic e ,   µAUV  will   proj ect   a  ver ti cal   beam   from   a  pl ane   with  a  s pecifie d  heig ht  t ow a r d  a n  area   by  hi gh - f reque ncy  acoust ic   pulse   e m issi on   in   order  to   captu re  de pth   i nfor m at ion   ab ou t  the  c orres pondin g  bo tt om   su r face.  T he  m ov e m ent  plane  is  set   ab ov e   cl utters  and  sam pled  in  a  unifo rm ly   sp ace d  2d   gri d  (see  Fig ur e   1) .  Th e  µA U V  will   m ov e  to   e ach  gr i d  a nd  m easur e    the  de pth   of   t he   current  gr i d  by  the  recei ve d  pulse   that  re f le ct ed  off  f ro m   the  obj ect ’s  to p  surfa ce.  A  r outi ne   of   t his  s urvey  is  show n  i n  Fi g ure   4 .   The   res ul t  of   a n  aerial   s urvey  is  an   ar r ay   of   t up le s ,   wh ose   com pone nts  are   m easur em ent posit ion s  and its  corres pondin g heig ht. See  Fi g ure   5   for  t he  i nput a nd outp ut  o f  this m et hod .           Fig ure   4.   Sca n i ti ner ary           Figure  5 .  Dia gra m  o f  i nput a nd  ou t pu t       The  Ec ho - s ound i ng   is  ca rr ie d  out  by  ac ousti c  sensors  w hi ch  rely   on   m od ulati ng   s urface  ac ousti c   wav e s  to   se nse   a  ph ysi cal   obj ect .   Th ese  dev ic es   tra ns duce  el ect rical   sign al s   to  ac ousti c  wa ves ,   r ecei ve   ref le ct ed  wa ve s ,   an d  c onver t   receive d  wa ve s  bac k  to  el e ct rical   sign al   [ 18 ] .  D ur in g  th e  wav e  pro pa ga ti on ,   acoust ic   wa ve s  will   be  ref l ect ed ,   refract e d ,   or  at te nuat ed  by  th e  m e diu m .  For  t he   de pth  m easurem ent ,     the  r eflect ion  i s  the   m ai n  co ncern.   I n  te rm s  of  analy sis ,   the  f orm   of   a  wav e   in - dep t h  m easur em ent  is  not  conve nient ,   bu t  the  cha racteri sti c  of   t he  ac ousti c  wa ve  c a n  be  a ppr oxim a t ed  to   a  ray  an d  sim ply  neg le ct ed  it s  wav e  nat ur e  si nce  the  wa veleng t h  of  the   ty pical   aco us ti c  wav e  is  sm al ler   com par e d   to  the  siz e  of  ph ysi cal   obj ect s und e r  t he  water [ 19 ] .  A s  a   co ns e que nce ,   t he  ray  tra ci ng  alg or it hm   is  intr oduce d  t o  m od el   a nd  si m ula t e   the  s ound   pro pa gation [ 20 ] .   T he  pro pa gation  of   the   aco us ti c   wa ve   is re gard ed  a s r ay s ,   an d  the   c on ic al  b e a m   is  const it uted  of  a  finite   num ber   of  rays .  The   dep t h  ca n  be  c al culat ed  by  knowin g  th e  ti m e  interval  be tween     the tra ns m itted sig nal and  rec ei ved   sig nal and the  pr op a gation sp eed  of the  aco us ti c ray.   Evaluation Warning : The document was created with Spire.PDF for Python.
I nt  J   R ob  &  A uto m   IS S N:  20 89 - 4856       Enviro nme nt  m apping  m ap constructi ng ,   an d p ath   plan ning  for... (Yi bin Pe ng )   281   The  m ai n  pr oble m   is  find in g  the  c orrect  sign al   ( ray)  f r om   the  receive d  sig nal  sp ect r um .  Be cause  there   are   unw anted   sig nals   and  backg rou nd  noise s  i n  the   recei ved  si gnal   sp ect ru m .  T he  bac kgr ound   noise   include s  ref le c ti on s  from   sm a ll   obj ect s ,   re fle ct ion s  of  si delo bes  or  par t   of  r everberati on  (e sp eci al ly   in  s ha ll ow   waters ) ,   the  unwa nted  si gn a l  includes  la te r   ref le ct ions  ( a  sign al   that  ha s   m ulti ple  tim e s  of   ref le ct io ns)  an d  diffuse   re flect ion s   ( a  si g nal  t hat  scat te rs   in   al l  directi on  w hen  hits  an   obje ct ).   T he refor e ,   the   use f ul  si gnal   is  the  no rm al   ref le ct ion   of  t he   em it te d  ray ,   because   a  rec ei ver   a nd  a  tr ansm itter  com bin e d  in   one  dev ic e   (transce iver ) ,   so   t he  on ly   first - order  s pe cular  ref le ct io n  ca n  be   rec ei ved  is  the   norm al   ref le ct ion .   The or et ic al ly ,   it  is ref le ct ed  of f  the nea rest  obsta cl es an d  ha s the h ig hest re m ai nin g  ene r gy  [ 18 ] .  Ba se d o n  this   pro per ty ,   2  ke y  factor s  ca n  be  us e d  to  est i m at e  the  nor m al   ref le ct ion :  tim e   of   flig ht  (TOF)  a nd   i m pu lse  re spo ns e  e nerg y  le vel.  In   t he  i m pu lse   res pons e  in  t he  tim e   do m ai n ,   the  norm al   ref le ct ion   sig nal  is  the  pu lse   with  t he   hi gh e st  ene r gy  re spon s e  a nd  t he   s hortest   T OF.  On ce   t he  TOF  of   the   desire d  sig nal  is   known ,   t he   dep t h  is  giv e n by the e quat io n:       ℎ = ∗ 2   (1)     w he re  v   is t he spee d of   sou nd in wat er a nd t  is t he  ti m e o f  fi gh t  of the  norm al  r eflect ion   sign al .  Th e  f l owcha rt  of the im ple m e nted ray - traci ng  base d  aerial   m app in g  M AT LAB   sim ulator  is sh own  in Fi gure  6.           Figure  6 .   Ra y t racin g  fl ow cha rt     Evaluation Warning : The document was created with Spire.PDF for Python.
                IS S N : 2089 - 4856   I nt  J   R ob  &  A uto m ,   Vo l.   8 ,   No.  4 ,   Decem ber   2019  :   277   –   292   282   A   ray - tr aci ng   si m ulator  is  bu il t  on  MATL AB.  A ss um e  the  po nd   is  a  50 m *25m   swi m m ing     pool - siz e d  e nc losure  with  a  dep t h  of   10m .   Assum e  the  side - sca n  sam pling  res olu ti on  is  0.5m *0 .5m ,   the  ob ta ine d  de pth   m easur em ent  data  an d  it s’  co rr es pondin g  surf ace  pl ot  are  s ho wn   i n  Fig ure  7  a nd   8  resp ect ively .           Figure  7. Ge om et ry  m od el  of a s t or a ge p ond           Figure  8.   Ra w   dep t h  m easur e m ent p oi nts  obta ined by aeria l  m app in g       3.   OCCUP A NCY  G RI D MA P  C O NS T RUC TING  A ND P ATH PL A NNING   The   ra w  data  obta ined   by  the   aerial   m app ing  is  a   to po l og ic al   de pth  m easur em ent  data.  It  is  require d  to  be  processe d  with   bo unda ry  traci ng   (t o  e xtract  obj ect s ’  bounda ry  po i nt s ,   est im at e  ob je ct s’  occupied   reg i on   and  ve rtic es ,   a nd  cal culat e  th e  top  pla ne  e quat ion ) ,   Pla nar  dep t h  mea s ur e men t  data  re ge ner atin g   (to   ge ner at e   dep t h  m easur e m ent  points   th at   li e  on  t he  s urvey  gri d  base d  on  plane   e quat ion   e stim at ed  by   extra ct ed   ve rtic es   po i nt) ,   3D  stru ct ur e  co ns tr ucting   (to  c onstruc t  the  rest  bo dy   by  interp olati ng   po i nts  un de r  the  ass um ption   of   the  ca nister  sha pe) ,   a nd  gr id   meth od   (to   init ia li ze  the  e nv ir on m ent’s  point   cl oud  m at rix  by   bin a ry  var ia bl e)   in   orders  to  est ab li sh   the  3D  digi ta l  gr id  m ap  f or   A *  path  pla nn i ng  al gorith m .  The  flo w  c har t  of  the  pro cessi ng   proce dure is s how n  i n  Fi gure  9.           Evaluation Warning : The document was created with Spire.PDF for Python.
I nt  J   R ob  &  A uto m   IS S N:  20 89 - 4856       Enviro nme nt  m apping  m ap constructi ng ,   an d p ath   plan ning  for... (Yi bin Pe ng )   283   Figure  9. The   proces si ng f lo w  c har t   3.1.    Bo und ar y  t racin g   Boun dar y  traci ng  al gorithm   is  a  te ch nique   that  is  a pp li e d  t o  dig it al   im ag es  in  orde r  t o  extract  thei r   bounda ry  [21].   A   di gital   im ag e  is  a   gro up  of  pix el s  on  a   s quare  te ssell at ion  each   hav i ng  a  certai n  value .  Sinc e   poi nt  cl oud  da ta   has  sim i la r  featur e s  so  bo unda ry  traci ng   al gorithm   can  be  us ed  he re.  Ther e   are  4  c om m on   ways  to  trace  t he  bo unda ry.  Th ey   are  Squa re  Traci ng   Algorithm ,   Moore - Nei ghbor  T ra ci ng ,   Ra dial  S weep ,   and  T he o  Pavli dis'   Algorithm   [22]  se parat el y.  T hey  t r acki ng  bo unda ry  in   a   diff e re nt  m anner.   This   pap e r   us es  the  ra dial  swe ep  al gorithm   t o  extr act   boun dar y  points  fro m   po int  cl oud  data  in  ord er  t o  fin d  ver ti ces   po i nts  because   this al gorithm  is e asy  to  im ple m ent.  The b oundary t racin g  incl ud e s  2  ste ps :   ‐   Extract  boun da ry points.   ‐   Rem ov al  point s of the  trace d object.   Ther e   is  a  dis ti ng uis he d  hei gh t  differe nce   in  the  to polo gical   point  cl oud  data ,   wh i ch  se gm ents  gro und  point   s et s  an d  ob j ect s ’  point set s.  He nce ,   he ig ht  c he ckin g  will  p r oc eed  i n  seq ue nc e  in  o r der  to  f ind   the   first  ob j ect   point  w hich  is  cl assifi ed  as  the  or i gin   of   the  boun dar y.  Af te r   the  first  ob j ec t  po int  is  f ound ,   the   bounda ry traci ng   proce dure i s invok e d.   Thi s u ses the c onc ept of ch ai n  co de  [ 23 ,   24] ,   whose  ch ecki ng  e ncode r   m ov es  al ong  the  bo unda ry  of   t h e  reg i on  to  e xtract  bounda ry  points   (in  a   cl oc kwise   or  antic lo ckw is e   directi on). T his conti nues  un t il  the ch ec king  encode r  ret urn s to  t he  sta rtin g p os it ion.   In  orde r  t o  fin d  the   nex t  bounda ry  po i nt ,   th e  heig ht  of  8  di recti on s   point s  that  s urrou nd ing   t he  st a rt  po i nt  will   be  c heck e d  i n  cl oc kw ise   orde r  f r om   the  so ut h  di recti on   unti l  find i ng  the  first  nex t  boun dar y  po i nt.   Diag ram   of   t he   8  directi ons  re sp ect   t o  t he  sta rt  po i nt  a nd  c he ck  directi on   is   sho wn  in   Fi gure  10  (i , j)   is  use d  t o  represe nt 8 d ir ect ion s  of a dja cent points  r el a ti ve  to th e start  point.           Figure  10.  8 Di recti on s   res pec t t o  the c urre nt  po i nt       On ce  t he  ne xt  bounda ry  point   has  bee n  f ound ,   it   is  save d  and   reg a r ded   as   the  ne w  sta rt  po i nt  an d  a   si m il ar  procedure   to   th e  a bo ve  is   rep eat e d ,   with   one   im po rta nt  diff e re nc e.  T he   diff e re nce   is  rathe r  t ha n  sta rt  check i ng  at   t he   sout h  directi on  it   c omm ences  f ro m   the   dir ect ion   t hat  t he  pr e vious   sta rt  po i nt  with  res pe ct   t o  the curre nt sta r t po i nt. A c omplet e boun dar y  traci ng is  dem on st rated i n  Fi gure  11.   As  s how n  in  F igure  11 ,   red   dots  re pr ese nt  the  f ound  boun dar y  po i nts  an d  gree n  dots  r epr ese nt  th e   nex t  s uccessi ve   boun dar y  points.  T he  sam ple  data  is  scan ned  at   blac k  dot  f ro m   bo tt om   to  top   searc hing  f o r   the  sta rt   point.   A fter  fin ding   the  sta rtin g  poi nt ,   sta rt  the   ini ti al   bounda ry  c heck  in   the   cl oc kw ise   di recti on  in   the  s outh   di rec ti on   of  t he  sta rt ing  point   un ti l  the  nex t   bounda ry  po i nt  is   f ound.  A fter  t hat ,   reg a rd i ng  t he  f ound  nex t  boun dar y   po i nt  as  the  ne w  sta rt ,   a nd  r epeat  the  boun dar y  c heck  at   the  s ou t h - west   directi on  of  th e  ne w   sta rting   po i nt. The  bounda ry check i ng   will  term inate  w hen the initi al  starti ng   po i nt ( the  f irst red   dot) is  fou nd.  Figure  11  de m on strat es  an   exam ple  of  t he  boun dar y  t racin g  process   on  a   sam ple  point  set .  T he   m os t   i m po rtant  t hing  in  t h e  bounda ry  traci ng  pr oc edure  is  the   " sense  of  direct ion ".   The   rig ht   directi ons  a re   with  resp ect   t o  t he  pr e vious  posit ion i ng.  T he refo re ,   it ' s  i m po rta nt  to   kee p  t rac king  of  the  cu rr e nt  an d  prev iou s   or ie ntati on  i n o rd e r  to  m ake the  r ig ht m ov es.     Le gend   H   :  Hei gh t     NN   :  N or th     NE   :  N or th - E ast    NW   :  N or th - Wes t    WW   :  Wes t    EE   :   Eas t    SW   :   Sout h - We st    SS   :  S outh   SE   :  S outh - West     Evaluation Warning : The document was created with Spire.PDF for Python.
                IS S N : 2089 - 4856   I nt  J   R ob  &  A uto m ,   Vo l.   8 ,   No.  4 ,   Decem ber   2019  :   277   –   292   284       Figure  11.   B ound a ry T racin g       In   pract ic e ,   the re  will   be  m ore  than  one  obje ct   in  the  e nv i ronm ent  so   the   obta ined  poin t  cl oud  will   con ta in   a  di ff e ren t  set   of   poin ts  (one  set   of   points  c orres pond s   to  on e   ob je ct ).   H oweve r ,   the  pr ob le m   is   it   will   rep eat e dly  trac e  the   fi rst   ob j e ct   an d  ca nnot   br ea k  out  t he  l oop .   I n  or der  to  esc ape   f ro m   the  lo op ,   it   is  r equ i red  to  rem ov e  eac h  trace d  ob j ec t  fr om   the  po i nt  cl oud  s o  th at   it   do es  not  pr e ve nt  tracki ng   of   t he  rem ai nin g  obj ect s.   On ce   t he  rem ov al   pro cess  has   bee n  done ,   restart  t o  f i nd  the   nex t   obj ect   poi nt  a nd  trace   the   s ucc essive  bounda ry  po i nts point a gain . See  the e xam ple in F i gure  12.           Figure  12. Re m ov al  o f  a  trac ed object   Evaluation Warning : The document was created with Spire.PDF for Python.
I nt  J   R ob  &  A uto m   IS S N:  20 89 - 4856       Enviro nme nt  m apping  m ap constructi ng ,   an d p ath   plan ning  for... (Yi bin Pe ng )   285   Ther e   are   2  se ts  of  po i nts  in   the  point  cl oud ,   so  the re  a re   2  ob j ect s.  T he   re d  po i nts  a r e  boun dar y   po i n ts ,   t he  blue   points   are   re m ind er  obj e ct   po i nts  a nd  the   heig hts  of  the   black   points   w her e   sca nn i ng  sta rts.  Figure  9  dem on strat es  how  t o  e xtract  bo undar y  po i nts  w hen  there   are  m ul ti ple  obj ec ts  in  the  po i nt  cl oud.   Howe ver ,   it   a ppare ntly   le ft  on e  pr ob le m :  When   2  ob j ect s  ov erlap   or  t ou c h  each  oth e r ,   the   point   cl oud  of  their   tou c hed  e dge  will   be   m erg e d  t oget her.   As   a   res ult ,   t his  al gorithm   will   treat   them   as  one   obj ect .   F or  this   par ti cula r  case ,   segm entat ion   of point cl oud wil l be  dep l oy ed  to  classi fy t his  2 obje ct .     3.2.    En vironm ent  rec onstr uctio n by p oin t  cl oud   Usu al ly ,   t he  ra w  de pth   m easu rem ent  po i nts  do  no t  c oinc id e  with   the   use r ’s  s pecifie d  s urvey  gr id ,   s o  it   is  necessary  to  cal culat e  the   plane  e quat io n  to  c reate  ne w  m easur em ent  po ints  t hat  co incide  wit h  the   us er ’s  sp eci fied  s urve y gr i d.   A  plane   in   3D  sp a ce  is   de fin ed  by  t hr ee   points ,   wh ic h  do   no t   al l  li e  on   the  s am e  li ne  or  by  a   poi nt   and  a   norm al   vecto r  t o  the   pl ane ,   w hich   is   cal le d  the   vect or  e qu at io n  of  a  pla ne.  In  othe r  w ords ,   in   order  to   cal culat e  the  v ect or   e quat io n  of  a  plane ,   it   is   re qu i red  to   kn ow  the   co ordi na te   of  at   le ast   3  points  t hat  lo cat ed   in  the  pla ne  an d  not  in  the  sa m e  strai gh t  li ne.   H ow e ver ,   th e  sel ect ion   of  po i nts  nee ds   to   be  care fu l ,   be cause  it   cannot  be  ass um ed  that any  3 arbitra ry point s taken f ro m  the p la ne  are  a bl e to r e present t he plane.  Th e  c ho s en   po i nts  s hould  be  a ble  to   best   descr i be  t he  plane  par am et er.  Ve rtic es  point s  are  t he  best  opti on  to   cal cul at e  the  plane.  Since   th e  bo unda ry  points  i n  t he   point   cl ou d  a re   ext r act ed  by  boun dar y   traci ng ,   ve rtic es  ca n  be   fou nd  by  com par in g  their  x  value  a nd   y  va lue.  Once  the  co ordin at e  of   3  ve rtic esp oin ts  is  known ,   the  vector  pla ne   equ at io n  c an  be cal culat ed. T he gene ral f or m  o f  the  vecto r  equati on of a   plane  is:     ⃗   ∙ ( − 1 ) = 0   (2)     ⃗     is  the  norm al   vecto r  of  the  pl ane.  T he  nor m al   vector   ⃗   = (a ,   b ,   c)  c an  be  ca lc ulate d  by  Cr os s  Pr oduct   of  any  2  vect or s   t hat  c onsti tuted   by   the   known   3  ver ti ces.   P1= (x 1 ,   y 1 ,   z 1 )   is   one   of  the   known   ve rtic es  points.   P=(x ,   y ,   z )  is   any  po i nt  th at   locat ed  on  t his   pla ne.  T his  e quat ion  ca n  be   t ran s f or m ed  int o  t he  ge ner al   plana r   equ at io n ,   an d t he heig ht of a ny  p oi nt on  t he plane is  g i ven  by:     [ , , ] ∙ [ − 1 , − 1 , − 1 ] = 0      +  +  − (  1 +  1 +  1 ) = 0       = (  1 +  1 +  1 ) −  −    (3)     Fo r   t he   case   th at   hei gh t   m eas ur em ents  at   re f le ct ion   posit io ns  that   do   not   li e  on   the   s urve y  gri d  s uch  as ti lt ed  obj ect s ,   this  vecto r plane e qu at io n ( 3 )  ca n be  us e d t o  cal culat e the  h ei ght  of  t he p oin ts  who se  po sit ion  li e  on  t he  s urv ey   gri d  an d  the   ob j ect ’s   to p  pl ane  ( x  a nd  y   va lue  is   the   co ordi nate  of  the   c orres pondin g  s urve y   gr i d) .   T hus ,   ac qu i red  a   ne w   point  cl oud  that  denotes   the   co r respo nd i ng  obje ct ’  to p  pla ne  by  po i nts  t hat  l ie   on   the s urvey g rid . S ince   the re a r e num ber s of  obj ect s ,   the  abo ve pr ocesses  wi ll  b e und e rtak en fo r  eac h obj ect .   Af te r  re gen e rati ng   the  ne w  m easur em ent  po i nts   of  the  obj e ct ’  uppe r   pla ne ,   the  ne xt  ste p  is  to  furthe r   process  it   to  create   a  com plete d  3D   m odel   of   the  e nviro nm ent.  This   proce dure  is   cal le d  en vironm ent  reconstr uctio n.  As  ob j ect s  ar e  assum ed  to  be  pr ism at ic ,   so   thei r   side   s urfaces   are  pe rpend ic ular  t o  it s   uppe r   su r face.  This  a   cru ci al   ass umpti on   f or   reconstr ucting  the  obj ect   becau se   the  key  idea  of   reconstr ucti ng   t he   env i ro nm ent  by   Po int  Cl ou d  is  to  ad d  ne w  po i nts  to  t he  m od ifie d  m eas ur em ent  points   in  the   pe rp e ndic ular   directi on o f  t he   uppe r  plane  t o  create   the r em ai nin g  pa rt  bet ween  the  to p  pl ane  de pth  m ea su rem ent  data o f  t he  ob sta cl e  a nd   t he  gr ound  m e asur em ent  data .  A dd e d  po i nts   m us t  sat isfy  the  co ndit ion   t hat  li es  on   t he   us er ’s  sp eci fied   s urve y  gr i d.   Th us ,   it   ge ner at es   the  3D  point  cl oud  m od el   of  obst acl es.  T his  te c hn i qu e   is  known   as   li near   heig ht  in te rpolat ion   [25 ] .  The  sc hem atic  diag ram   of   the  perpe nd ic ular  he ig ht  inter p olati on  is  s ho wn  i n  Figure  13.     Evaluation Warning : The document was created with Spire.PDF for Python.
                IS S N : 2089 - 4856   I nt  J   R ob  &  A uto m ,   Vo l.   8 ,   No.  4 ,   Decem ber   2019  :   277   –   292   286       Figure  13. A s chem at ic  o f  th e li near   pe rp e ndic ular  h ei gh t i nter po la ti on       An  exam ple   of   us i ng  li near  pe rp e ndic ular   he igh t  i nterpola ti on   to   gen e rate  a  poi nt  cl ou d  of  the  3D   m od el  o f dete c te d  ob j ect s is s how n belo w.   It  i s explai ne d w it h  the ai d of   F igure  14.           Figure  14. A s chem at ic  o f  th e li near   pe rp e ndic ular  h ei gh t i nter po la ti on       Assum e  the  reso luti on  of   the   su r vey  gri d  i s  α *α ,   an d  the   po sit io n  of  m easur em ent  res ult  li es  on    the  s urvey  gri d.   Po i nt  A ,   B ,   C ,   D   are   4  v erti ces  points  of  t he  m easur em ent  po i nt  cl ou d.  Assum e    the  co ordinate  of   A  is  (x a ,   y a ,   z a ) ,   B  is  (x b ,   y b ,   z b ) ,   C  is  (x c ,   y c ,   z c ) ,   D  is  (x d ,   y d ,   z d )  res pecti vely .  It  ne eds  to   cal culat e  the   x ,   y  c oor din at e of  a dded  p oi nts e ,   f ,   g  a nd  m ake  s ur e   they   a re  ly ing   on  the   s urvey   gri d.  P oint   e ,   f ,   g  exist  on   t he  vecto r  that  pe r pendicula r  to  t he  pla ne  a nd   pa ss  thr ough  B ,   so   it   nee ds   to  c al culat e  the  eq uation  of the  vecto r.   The  e quat ion o f  the  v ect or   an d  the  coo rd i nate of the  point i n  the  v ect or is  giv e n by:      ⃗ ⃗ ⃗ ⃗ ⃗   = [ − , − , − ]      ⃗ ⃗ ⃗ ⃗ ⃗   = [ − , − , − ]     ⃗   =  ⃗ ⃗ ⃗ ⃗ ⃗   ×  ⃗ ⃗ ⃗ ⃗ ⃗   = [ , , ]     (4)       = ⃗   + ∙ ⃗   = [ , , ] + ∙ [ , , ]     (5 )     [ , , ] = [ + ∙ , + ∙ , + ∙ ]   (6)     = + ∙   (7)     = + ∙   (8 )     = + ∙                                                                  (9 )     Gr oup poin t se t P oin ts in  de t ec t ed  objec t's  t op pla ne Gr oup poin t se t P oin ts in  de t ec t ed  objec t's  t op pla ne P oin ts ad ded  b y  perpe ndicu lar h eig h t  in t erpola tion A B C D x z y e f g Gr oun d Remain in g   p art   Evaluation Warning : The document was created with Spire.PDF for Python.