Veloci y-Based Heu is ic E alua ion o Pa h
Planning and Vehicle Rou ing o Vic im
Assis ance in Disas e Scena ios
Manuel Toscano-Mo eno

, An hony Mandow,
Ma ía Alcáza Ma ínez, and Al onso Ga cía-Ce ezo
Uni e sidad de Málaga, Sys ems Enginee ing & Au oma ion Dep ., Málaga, Spain,
[email p o ec ed],
WWW home page: h ps://www.uma.es/ obo ics-and-mecha onics
Abs ac . Na u al and human-made disas e s equi e effec i e ic im
assis ance and las -mile elie supply ope a ions wi h eams o g ound e-
hicles. In hese applica ions, digi al ele a ion models (DEM) can p o ide
accu a e knowledge o sa e ehicle mo ion planning bu g id ep esen a-
ion esul s in e y la ge sea ch g aphs. Fu he mo e, a el ime, which
becomes a c ucial cos op imiza ion c i e ion, may be affec ed by inclina-
ion and o he challenging e ain cha ac e is ics. In his pape , ou goal
is o e alua e a sea ch heu is ic unc ion based on aniso opic ehicle e-
loci y es ic ions o building he cos ma ix equi ed o mul i- ehicle
ou ing on na u al e ain and disas e si es. The heu is ic is applied o
compu e he as es a el imes be ween e e y pai o ma ix elemen s
by means o a pa h planning algo i hm. The analysis is based on a case
s udy on he o opho og aphic-based DEM o na u al e ain wi h di -
e en a ge poin s, whe e he p oposed heu is ic is compa ed agains
an exhaus i e sea ch solu ion.
Keywo ds: mul i- obo eam, heu is ics, sea ch and escue, pa h plan-
ning, ehicle ou ing p oblem
1 In oduc ion
Efficien managemen o pa hs and ou es o eams o au onomous off- oad ehi-
cles is c ucial o challenging obo ic applica ions such as plane a y explo a ion
[11], ag icul u e [4], and sea ch and escue (SAR) in pos -disas e si ua ions [6].
In off- oad en i onmen s, he absence o p e-defined oads esul s in a much
la ge sea ch space, and e ain cha ac e is ics, such as g adien , affec ehicle
mobili y in aspec s such as na igabili y, a el ime o ene gy consump ion. In
his sense, g id ep esen a ions such as digi al ele a ion models (DEM) can cap-
u e e ain knowledge o each cell ha is use ul o conside ing e ain slope
na igabili y [9][14], uel consump ion es ima ion [13], he p esence o ic ims a
isk in escue ope a ions [12], o weed in es a ion in a ming en i onmen s [4].
The e is a g owing in e es in he ehicle ou ing p oblem (VRP) o flee s
o unmanned ae ial ehicles (UAV) [10]. Howe e , ew wo ks ha e add essed
This is a p e-p in o a con ibu ion published in "Robo 2019: Fou h Ibe ian Robo ics Con e ence. Ad ances in In elligen
Sys ems and Compu ing, Vol 1093. Sil a M., Luís Lima J., Reis L., San eliu A., Ta dioli D. (eds)" published by Sp inge ,
Cham. The inal au hen ica ed e sion is a alaible online a : h ps://doi.o g/10.1007.987-3-030-36150-1_10
2 Manuel Toscano-Mo eno e al.
he ehicle ou ing p oblems by conside ing e ain ele a ion. In [3] a digi al
ele a ion map is conside ed o a eam o UAVs in a e ain mapping applica ion.
In he case o g ound ehicles, [11], conside s he unc ionali y cons ain s o an
unmanned g ound ehicle (UGV) whe e he goal is o each a se e al a ge
poin s o a plane a y su ace in combina ion wi h an UAV by minimizing he
a elling dis ance.
E en i he VRP is in insically a spa ial p oblem, some applica ions impose
ele an empo al aspec s [5]. This pape ocuses on SAR ope a ions, whe e
selec ing eliable pa hs is necessa y o ac ually p o ide imely a en ion o ic-
ims. In ac , one majo diffe ence be ween he objec i e unc ion in eme gency
esponse ou ing and o he ou ing p oblems is ha he a i al ime a ic ims’
loca ions is mo e impo an han he o al a el ime [2].
Planning node- o-node pa hs can be he fi s s ep in a VRP solu ion o build
he inpu cos ma ix. A ime-awa e planning can be achie ed by conside ing
he a e sal imes o he edges in opological sea ch g aphs [1][8]. Cells in g id
ep esen a ions can be used as nodes in g aph-sea ch me hods de i ed om he
Dijks a algo i hm [7], bu high esolu ion maps may esul in e y la ge sea ch
g aphs.
In his pape , ou goal is o e alua e a sea ch heu is ic unc ion based on
aniso opic ehicle eloci y es ic ions o building he cos ma ix equi ed
o mul i- ehicle ou ing on na u al e ain and disas e si es. The heu is ic is
applied o compu e he as es a el imes be ween e e y pai o ma ix elemen s
by means o a pa h planning algo i hm. The p oposed analysis is based on a case
s udy on he o opho og aphic-based DEM wi h a se o a ge poin s, whe e
he p oposed heu is ic is compa ed agains an exhaus i e sea ch solu ion. Thus,
he con ibu ion o he pape is no ocused on pa h planning o mul i- ehicle
ou ing, bu on he cons uc ion o he p io cos ma ices equi ed o add ess
hese p oblems. In pa icula , he p oposed algo i hms allow defining a cos
ma ix o each UGV.
The emaining o he pape is o ganized as ollows. Sec ion 2.1 offe s defi-
ni ions and he p oblem o mula ion. Sec ion 2 in oduces he compu a ion o
a el ime and cos ma ices. Sec ion 3 discusses expe imen al esul s om a
case s udy o a dis ibu ion o ic ims on a DEM. Sec ion 4 closes he pape
wi h he conclusions.
2 T a el Time Ma ices and Cos Ma ices
This sec ion p oposes he compu a ion o a se o a el ime ma ices T ha can
be used o build cos ma ices C o a numbe o UGVs (nug ). The ou line o
he app oach p oposed in his wo k is gi en in figu e 1. The cos ma ix o each
ehicle indica es he imes equi ed o a el be ween any wo elemen s o a se
o n ic ims. The pu pose o hese Cma ices is o be sui able o u u e mul i-
ehicle ou ing solu ions. Fi s , we desc ibe an exhaus i e opological sea ch
o find Tby conside ing aniso opic beha io esul ing om e ain elie and
Veloci y-Based Heu is ic E alua ion . . . 3
Exhaus i e sea ch:
Tmxn
nug xn a el ime ma ices:
ime o each ic im om any cell
Heu is ic sea ch:
DEM ma ix
Kinema ic
es ic ions
Pa h planning
nug block eloci y ma ices
8-neigbou aniso opy
nug xn 2 a el ime ma ices:
ime o each ic im om any
CLOSED cell
nug cos ma ices
be ween ic ims
and UGVs
Scope o he pape
C(n+1)xn
EmxnV(mxn)2
Tmxn
Vic ims loca ions
Fig. 1. Ou line o exhaus i e and heu is ic compu a ion p ocesses o cos ma ices
kinema ic es ic ions. Then, we in oduce a heu is ic o efficien ly cope wi h he
la ge sea ch space p o ided by he g id ep esen a ion o he DEM.
2.1 P oblem Fo mula ion
The en i onmen is ep esen ed by an n×mma ix Ewi h ele a ion alues ob-
ained om i s DEM and he esolu ion δis he dis ance be ween wo con iguous
cells.
T a e sal eloci y ma ices Vcan be buil o each ehicle by conside ing
kinema ic es ic ions o each cell in E. Thus, Vis a block ma ix o med by
m×nspa se subma ices o he same o de .
V∈M
m×n(Mm×n(R)) (1)
Consequen ly, he e is a subma ix o each cell in he DEM. Each subma ix
ep esen s he XY a e sal eloci y om he co esponding cell in he map
o i s 8-neighbo cells, so he es o elemen s o he subma ices is always ze o.
Each elemen c1c2 ep esen s he XY a e sal eloci y om he cell c1 o i s 8-
neighbo cell c2and, he e o e, null alue exp esses he kinema ic inadmissibili y
o UGV o a gi en a e sal displacemen .
T a el ime ma ices T o a gi en UGV a e buil as n×mma ices whe e
each elemen ij is he as es (as op imized by he sea ch algo i hm) a el ime
om cell c=(i, j) o he goal cell cg=(ig,j
g).
The cos ma ix C, o a gi en UGV, is an (n +1)×n ma ix whe e he
uppe n ×n squa e subma ix con ains he as es a el imes be ween ic ims
and he las ow con ains he as es a el imes o each ic im om he ini ial
UGV’s loca ion.
2.2 Exhaus i e Sea ch App oach o Compu a ion o Cos Ma ix
Algo i hm 1 desc ibes he p ocedu e o compu e he cos ma ix C o a gi en
UGV using an exhaus i e sea ch. This i e a i e me hod compu es a a el ime
4 Manuel Toscano-Mo eno e al.
Algo i hm 1: Cos ma ix C h ough an exhaus i e sea ch
Da a: δ. . . he esolu ion o en i onmen disc e iza ion
Da a: V. . . he ma ix wi h XY a e sal eloci ies o a gi en UGV
Da a: [cg1...cgn ]. . . he lis o n goal cells
Da a: cs. . . he s a cell o a gi en UGV
Resul : C. . . he cos ma ix o applying o VRP
1 o each cgk2∈[cg1...cgn ]do
2T←Exhaus i eT a elTimesMa ix δ, V,cgk2
3 o each cgk1∈[cg1...cgn ]do Ck1,k
2← cgk1
4Cn +1,k
2← cs
Algo i hm 2: Exhaus i e compu a ion o he a el ime ma ix T
Da a: δ. . . he esolu ion o en i onmen disc e iza ion
Da a: V. . . he ma ix wi h XY a e sal eloci ies o a gi en UGV
Da a: cg. . . he goal cell
Resul : T. . . he a el ime ma ix
Func ion Exhaus i eT a elTimesMa ix (δ,V,cg):
1 o all Vc∈Vdo c←∞
2CLOSED ←∅,OPEN ←cg, cg←0
3 epea
4c←a gmin
c∈OPEN  c
5OPEN ←OPEN c,CLOSED ←CLOSED ∪c
6 o each ckso ha ckc
=0and ck
∈CLOSED do
7 ck←min  ck,
c+δ·L2ck,c
ckc
8OPEN ←OPEN ∪ck
9un il OPEN =∅
ma ix T(algo i hm 2) o e e y goal cell and consul s in T he a el ime asso-
cia ed wi h he loca ions o each o he goal cells o assigning o an app op ia e
elemen o Cma ix.
2.3 Exhaus i e Sea ch o Compu a ion o T a el Time Ma ix
Algo i hm 2, based on he s a egy o well-known me hods like Dijks a, i s
a ia ions (A* o D*) o Fas Ma ching, desc ibes he p ocedu e o compu e
he a el ime ma ix T o a gi en UGV. This algo i hm does no inco po a e
heu is ics, so he sea ch o as es a el ime is unin o med and exhaus i e,
which implies a high compu a ional cos , especially o he case o a opological
sea ch g aph based on a dense g id disc e iza ion.
Veloci y-Based Heu is ic E alua ion . . . 5
In each i e a ion, he cells in he en i onmen can be assigned o wo se s:
OPEN and CLOSED. The se OPEN con ains he cells in he en i onmen
which ha e been e alua ed. On he o he hand, he se CLOSED con ains he
cells c ha ha e al eady explo ed by he algo i hm and, he e o e, whose a el
imes cha e al eady been compu ed.
The algo i hm ini ializes: 1) he Tma ix o infini e alues, 2) he se OPEN
con aining he goal cell cg=(ig,j
g), and 3) he a el ime cgassocia ed wi h
he a o emen ioned goal cell o ze o (lines 1 o 2).
The i e a i e me hod explo es he cell c=(i, j)belonging o se OPEN
wi h he sho es alue o he e alua ion unc ion, i.e. sho es a el ime c
(line 4), es ima ing he a el ime om kinema ic admissible cells ck- i.e.
hose ha ha e a non-null XY a e sal eloci y ckc- which ha e no ye been
explo ed (line 6). In his way, he a el ime is accumula ed in he successi e
i e a ions by upda ing Twi h he a el ime ha he ehicle akes om e e y
cell un il eaching he goal cell cg. Fo compu a ion o cumula i e a el ime
(line 7), he algo i hm conside s he dis ance δbe ween con iguous cells in he
en i onmen , he XY a e sal eloci y ckcassocia ed wi h his displacemen ,
and he Euclidean dis ance L2be ween he kinema ic admissible cell ck=(ik,j
k)
and he cell c=(i, j)ex ac ed om he se OPEN in each i e a ion.
When he e alua ion canno con inue (line 9), he a el ime ma ix Thas
been compu ed o all cells in he en i onmen .
2.4 Heu is ic Sea ch App oach o Compu a ion o Cos Ma ix
Algo i hm 3 desc ibes he p ocedu e o compu e he cos ma ix C o a gi en
UGV using a heu is ic sea ch. This i e a i e me hod compu es a a el ime
ma ix T(algo i hm 4) o each pai o cells whose ep esen each pai o ic im’s
loca ions and consul s in each T he a el ime associa ed wi h he loca ion o
espec i e s a cell o assigning o an app op ia e elemen o Cma ix.
2.5 Heu is ic Sea ch o Compu a ion o T a el Time Ma ix
In o de o inc ease he compu a ional efficiency, a a ian o he ini ially p o-
posed algo i hm 2 is p esen ed, which uses o a heu is ic o es ima e he a el
ime ma ix T. The use o a heu is ic aims o educing he sea ch space by he
en i onmen , di ec ing i owa ds he cell om whe e he na iga ion o UGV
s a s in each case. To do ha , algo i hm 4 p esen s modifica ions o he algo-
i hm 2.
Algo i hm 4 es ablishes wo e e ence cells: 1) loca ion cgo he ic im o
assis , and 2) ini ial loca ion c0o he UGV om whe e he na iga ion s a s.
While he fi s p oposal, p esen ed in he subsec ion 2.3, only equi es cell cg
associa ed wi h he loca ion o each ic im, his second one equi es a new e e -
ence cell c0. The cell c0will be diffe en depending on he elemen Ck1,k
2o
cos ma ix C ha is being es ima ed, ep esen ing he new loca ion om whe e
he UGV na iga ion will s a a e assis ing each ic im.

6 Manuel Toscano-Mo eno e al.
Algo i hm 3: Cos ma ix C h ough a heu is ic sea ch
Da a: δ. . . he esolu ion o en i onmen disc e iza ion
Da a: V. . . he ma ix wi h XY a e sal eloci ies o a gi en UGV
Da a: [cg1...cgn ]. . . he lis o n goal cells
Da a: cs. . . he s a cell o a gi en UGV
Resul : C. . . he VRP cos ma ix
1 o each cgk1∈[cg1...cgn ]do
2 o each cgk2∈[cg1...cgn ]do
3T←Heu is icT a elTimeMa ix δ, V,cgk1,cgk2,Ck1,k
2← cgk1
4T←Heu is icT a elTimeMa ix δ, V,cs,cgk1,Cn +1,k
1← cs
Algo i hm 4: Heu is ic compu a ion o he a el ime ma ix T
Da a: δ. . . he esolu ion o en i onmen disc e iza ion
Da a: V. . . he ma ix wi h XY a e sal eloci ies o a gi en UGV
Da a: c0. . . he cell om whe e he displacemen s a s
Da a: cg. . . he goal cell
Resul : T. . . he a el ime ma ix
Func ion Heu is icT a elTimesMa ix (δ,V,c0,cg):
1 o all Vc∈Vdo c←∞,
c←∞
2CLOSED ←∅,OPEN ←cg,
cg←0,hcg←0
3while OPEN 
=∅and c0
∈CLOSED do
4c←a gmin
c∈OPEN 
c
5 c←
c−hc
6OPEN ←OPEN c,CLOSED ←CLOSED ∪c
7 o each ckso ha ckc
=0and ck
∈CLOSED do
8hck←δ·L2ck,c0
maxV
9
ck←min 
ck,
c+δ·L2ck,c
ckc
+hck
10 OPEN ←OPEN ∪ck
Algo i hm 4 desc ibes he heu is ic p ocedu e o compu e he a el ime
ma ix To a gi en UGV om a ini ial cell c0 o a goal cell cgin he en i onmen .
As in algo i hm 2, in each i e a ion, he cells in he en i onmen can be assigned
o wo se s: OPEN and CLOSED.
Du ing he i e a ions, he me hod upda es alues co T (line 5) using wo
auxilia y ma ices 
Tand H. Fo each cell cbelonging o he se OPEN, he
Veloci y-Based Heu is ic E alua ion . . . 7
1
1
2
345
67
8
9
2
Fig. 2. DEM o a eal en i onmen . The ele a ion o he e ain is ep esen ed wi h
diffe en ed onali ies, whe e da ke one indica es highe ele a ion alue. The i egula
shape in he g aph’s bo de ep esen s unmodeled a ea o he en i onmen .
ma ix 
Tcon ains he es ima ed a el ime 
c om he ini ial cell c0 o he goal
cell cgas i passes h ough he cell c(line 9). On he o he hand, he ma ix H
con ains he es ima ed ime hc om he ini ial cell c0 o he cell c(line 8). The
ma ix His used as a heu is ic o accele a e he con e gence o he algo i hm.
When he sea ch canno con inue o he ini ial cell c0has been explo ed
(line 3), he a el ime ma ix Thas been compu ed o a subse o cells in he
en i onmen (CLOSED se ).
3 Expe imen al Analysis
In his sec ion we compa e he pe o mance o he heu is ic sea ch algo i hm
agains he unin o med exhaus i e sea ch me hod. The compu a ion has been
ca ied ou using Ma lab code on a 6-co e In el i7-8750H CPU 2.21 GHz. In
pa icula , we conside a case s udy wi h wo omnidi ec ional UGVs (nug =2)
and nine ic ims (n =9). Figu e 2 shows a digi al ele a ion model (DEM)
o a eal SAR expe imen a ion en i onmen [6] whe e UGVs and ic ims a e
ep esen ed as numbe ed hexagons and squa es, espec i ely. The dimensions o
he DEM a e 230 ×243 me e s wi h a esolu ion o one me e .
The aniso opic a e sal eloci y ma ices, V1and V2, ha e been defined
be o ehand o each UGV. These eloci y ma ices con ain he XY a e sal
eloci ies o each cell in he en i onmen owa ds each o i s 8-neighbo s. The
algo i hms p oposed in his pape a e independen o he numbe o kinema ic
es ic ions conside ed o build V o a gi en applica ion. On an illus a i e le el,
his case s udy conside s a simple se o es ic ions, which a e gi en in able 1.
8 Manuel Toscano-Mo eno e al.
Table 1. UGVs’ kinema ic es ic ions
UGV-1 UGV-2
Sa e y adius 1.4 m 1.4 m
Nominal speed 0.3 m/s 0.2 m/s
Maximum na igable slope 20◦35◦
Table 2. Resul ing cos ma ices C o he case s udy (in seconds)
Vic im # 1 2 3 4 5 6 7 8 9
1 0 391 147 497 408 686 514 90 541
2 391 0 523 115 110 300 215 343 155
3 147 523 0 629 540 819 646 198 674
4 497 115 629 0 223 259 320 449 147
5 408 110 541 223 0 401 111 361 226
6 686 300 819 259 401 0 443 466 268
7 514 215 646 320 111 443 0 466 268
8 90 343 198 449 361 639 466 0 494
9 541 155 674 147 226 175 268 494 0
UGV-1 222 517 367 623 535 813 640 237 668
a) C1 o UGV-1
Vic im # 1 2 3 4 5 6 7 8 9
1 0 360 217 303 453 678 616 135 511
2 360 0 353 151 162 450 322 267 232
3 217 353 0 207 510 462 673 297 407
4 303 151 207 0 313 388 471 288 213
5 453 162 510 313 0 602 167 340 339
6 678 450 462 388 602 0 665 675 263
7 616 322 673 471 167 665 0 504 402
8 135 267 297 288 340 675 504 0 462
9 511 232 407 213 339 263 402 462 0
UGV-2 433 215 492 364 96 657 248 305 399
b) C2 o UGV-2
3.1 Cos Ma ices
Bo h sea ch me hods each he same alues o he cos ma ices C1and C2
compu ed o bo h UGVs (see able 2). The uppe squa ed subma ices o C1
and C2p o ide he as es a el ime be ween he co esponding pai o ic im
loca ions whe eas he las ow indica es he as es ime be ween he ini ial
posi ion o he UGV and he ic ims.
The cos s be ween ic ims depend on he ela i e posi ions be ween hem,
he e ain elie and he kinema ic es ic ions con ained in he Vma ices.
Fo example, while he cos o UGV-1 be ween ic ims #3 and #4 is 629 s,
o UGV-2 i is 207 s. This diffe ence can be explained by he limi a ions o
UGV-1 o su pass he s eep slope be ween ic im posi ions (see figu e 2), which
p o okes a longe a ound pa h. Con e sely, he as e nominal speed o UGV-1
esul s in a lowe cos when a elling be ween #5 and #7 (i.e., 111 s agains
167 s), whe e bo h ehicles could a el in a s aigh line.
Veloci y-Based Heu is ic E alua ion . . . 9
ic im4 ic im4
a) UGV-1 b) UGV-2
Fig. 3. Examples o g aphical ep esen a ion o a el ime ma ix T o each ic im
#4 wi h exhaus i e sea ch
3.2 Exhaus i e Sea ch App oach
Wi h exhaus i e sea ch, Tma ices need o be compu ed only once o each
ic im/UGV pai (see figu e 1). Fo each UGV, (n +1)×n elemen s o C
ma ix a e assigned looking he co esponding alues up in he se o Tma ices.
The compu a ion imes o ob aining each one o nug ×n Tma ices (18 o
he s udy case) ange be ween 57.5 ms and 84.8 ms. The o al compu a ional
ime o ob ain he cos ma ices o bo h UGVs has been 1249 ms.
Figu e 3 illus a es wo examples o a el ime ma ices Tcompu ed wi h
he exhaus i e sea ch app oach. These examples co espond o he compu a ion
o T o each ic im #4 wi h each UGV. The ic im’s loca ion is ep esen ed
wi h a yellow ci cle. Each cell ep esen s he as es a el ime o each he
ic im s a ing om ha cell. T a el imes a e ep esen ed wi h diffe en shades
o g een. The whi e cells a e hose om whe e he ic im canno be accessed
due o UGV’s kinema ic es ic ions (i.e., he sea ch has no p oduced a solu ion
pa h).
This diffe ence can be explained by he limi a ions o UGV-1 o su pass
he s eep slope be ween ic im posi ions (see figu e 2), which equi es a longe
a ound pa h.
Fu he mo e, he da kes g een cells o UGV-1 (see figu e 3.a) ep esen a
low a ea o he na u al e ain om whe e he ehicle needs o a el a ound
an unsu passable slope o assis ic im #4 (see figu e 2). Howe e , UGV-2 has
no limi a ions o su moun mos e ain slopes in hese en i onmen (wi h he
excep ion o hose in whi e cells) so a el imes a e mo e ela ed o Euclidean
dis ance (see figu e 3.b).