30/10/2013

1
Unidad5:PlaneacindeCaminos
Dr. Jos Armando Segovia De Los Ros
Instituto Tecnolgico de Toluca
armando.segovia@inin.gob.mx
U4:ModeladodelEspacio
5 PlaneacindeCaminos

5.1Introduccin
5.2Bsquedaengrafos
5.3Programacindinmica
5.4Grficasdevisibilidad
5.5CamposPotenciales
5.6Lecturasenplaneacin
Introduccin
Larepresentacindelentornodelrobotpuedevariar
desdeunadescripcingeomtricacontinuaaunmapa
geomtricobasadoendescomposicinoinclusohasta
unmapatopolgico,comosevioanteriormente.

Elprimerpasodecualquiersistemadeplanificacinde
rutasestransformarestemodeloambiental
posiblementecontinuoenunmapadiscretoadecuado
paraelalgoritmodeplanificacinderutaselegido.

Losplanificadoresdecaminodifierenencuantoa
cmorequierenestadescomposicindiscreta.
Sepuedenidentificartresestrategias
generalesparaladescomposicin:

1.Mapaderuta:identificaunconjuntode
rutasdentrodelespaciolibre.
2.Descomposicincelular:discriminarentre
clulaslibresyocupadas.
3.CamposPotenciales:imponenunafuncin
matemticasobreelespacio.
30/10/2013
2
PlanificacindeCaminosempleando
Mapas
Losmtodosdemapaderutascapturanlaconectividaddel
espaciolibredelrobotenunareddecurvasolneasde1D,
llamadasmapaderutas.

Unavezqueunmapaderutasseconstruye,seutilizacomo
unared(deruta)desegmentosdecarreteraparala
planificacindemovimientodelrobot.

Laplanificacindelatrayectoriasereduceasaconectarla
posicininicialyelobjetivodelrobotconlaredde
caminos,ysecontinuaconlabsquedadeunaseriede
caminosdesdelaposicininicialdelrobothastasuposicin
final.
Paraestefin,unabasecomputacionalimportanteenlabsqueda
decaminos(PathPlanning)eslatcnicallamadaBsquedaen
Grafos,lacualesdirectamenteaplicableenproblemasque
involucranvariostiposdemapas,especialmentelostopolgicosy
algunosotrosqueresultanengrafoscomolosdiagramasde
Voronoi.

Elmapadecaminosesunadescomposicindelespaciode
configuracindelrobotbasadaespecficamenteenlageometra
obstculo.

Elretoconsisteenconstruirunconjuntodecaminosqueen
conjuntopermitenalrobotiracualquierpartedesuespaciolibre,
minimizandoelnmerodeltotaldecaminos.

Engeneral,laintegridadseconservaentalesdescomposiciones,
siempreycuandolosverdaderosgradosdelibertaddelrobothayan
sidocapturadosconlafidelidadapropiada.
AcontinuacinsedescribelatcnicadeBsquedaenGrafosy
elempleodedosmapasdecaminosquepermitenplanificar
uncaminoperoquedanresultadosquesondrsticamente
diferentes.

EnelcasodelasGrficasdeVisibilidad,lascarreterasse
acerquelomsposiblealosobstculosyloscaminos
resultantessonsolucionesdelongitudmnima.

EnelcasodelosDiagramasdeVoronoi,loscaminos
permanecenlomslejoscomoseaposibledelosobstculos.
BsquedaenGrafos
Introduccin
Ungrafoesunconjuntodeelementosllamadosnodos
(vrtices)queseconectanunosconotrosmediante
aristas(arcos).
Unaaristapuedeconectardosnodos,o,comoen
algunasaplicaciones,unnodoconsigomismo.
Unaaristaestancladaensusdosextremosanodos,o
posiblementealmismonodoenlosdosextremos.
Enalgnproblemadado,losnodospueden
representarciudades,mientrasquelosarcospueden
serlascarreterasquelasunen.
30/10/2013
3
Formalmente:
Ungrafoes(N,A,P)dondeNesunconjuntode
nodos,Aesunconjuntodearistas,yPesuna
funcindelasaristastalquecadaP(a)={p,q}
dondep,qsonnodos(posiblementeconp=q,as
quepodemosdecirqueP(a)esunconjuntode1o
2elementos).
CuandoGesungrafo,GNdenotasusnodos,GA
susaristas,yGPsufuncindearistas.

ElgrafoGdelafigurasepuededescribircomo
G
N
={1,2,3,4},
G
A
={a,b,c,d,e},
P(a)={1,2},
P(b)={2,3},
P(c)={1,4},
P(d)={1,3},
P(e)={3}.
UnaaristaaconP(a)={p},comolaaristaedel
ejemplo,sellamarulo.
Losrulossonirrelevantesenlasaplicaciones
queconsideraremosaqu.
Unaconexindelnodopconlaaristaase
representacomopP(a),aunqueesms
amigabledecirelnodopestconectadoala
aristaa.
Caminosyconectividad
Unanocinfundamentalesladeuncamino(path).

Uncaminoempiezaenalgnnodo,ysigueporaristas
visitandovariosnodos.

Formalmente:
UncaminoenungrafoG=(N,A,P)esunasucesin
(posiblementevaca)a=a
1
...a
n
dearistastalqueexisteuna
sucesinp=p
0
...p
n
denodostalqueparacadaa
i
,P(a
i
)={p
i1
,
p
i
}.
Lasucesinpsellamaunrecorridodelcaminoa.
Sedicequeaempiezaenp
0
yterminaenp
n
.
30/10/2013
4
MatrizdeAdyacencia
Paradescribirlaconectividaddelosnodosenungrafodosmtodosson
comnmenteempleados:
MatrizdeAdyacenciaylaListadeAdyacencia

Unamatrizdeadyacenciaesunarreglobidimensionalenelcuallos
elementosindicansiunarcoestpresentaentredosvrtices(onodos).

SiungrafotieneNvrtices,lamatrizdeadyacenciaesdeNN.

A B C D
A 0 1 1 1
B 1 0 0 1
C 1 0 0 0
D 1 1 0 0
BsquedaenGrafos
Unadelasmsfundamentalesoperacionesa
efectuarenungrafoesencontrarcualesvrtices
puedenseralcanzadosdesdeunvrtice
especificado.
Haybsicamentedosmtodosdebsquedaen
grafos:
Primeroenprofundidad
PrimeroenAnchura
Ambosalcanzarneventualmentetodoslos
vrticesconectados.
Ladiferenciaentreestosdosmtodosesqueelde
bsquedaprimeroenprofundidad(depthfirstsearch)es
implementadoconunapila,mientrasqueeldebsqueda
enanchura(depthfirstsearch)esimpementadoconuna
cola.
AlgoritmoDFS
PararealizarelprocesodeDFSsetomaunpuntodeinicio
(digmosA).
Luegosehacentrescosas
Sevisitaestevrtice
Secolocaenlapila
Semarcacomovisitado

EnseguidasevaacualquiervrticeadyacenteaAque
nosehayavisitado(supongamosB).
SevisitaB,semarcaysecolocaenlapila.
Despussecontinuadelamismamanera.
Estosepuededefinircomolaregla1.
Regla1:Siesposible,sevisitaunvrticeadyacenteno
visitado,semarcaysecolocaenlapila.
Aplicandolaregla1denuevo,sellegaaH.
Enestepuntoyanoesposiblecontinuar,aquesdonde
entralaregla2.
Regla2:Sinosepuedeseguirlaregla1,entonces,siesposibletomar
unvrticedelapila.

30/10/2013
5
AlgoritmoDFS
PararealizarelprocesodeDFSsetoma
unpuntodeinicio(digamosA).
Luegosehacentrescosas
Sevisitaestevrtice
Secolocaenlapila
Semarcacomovisitado
Enseguidasevaacualquiervrtice
adyacenteaAquenosehayavisitado
(supongamosB).
SevisitaB,semarcaysecolocaenla
pila.
Despussecontinuadelamisma
manera.

Estosepuededefinircomolaregla1.
Regla1:Siesposible,sevisitaunvrticeadyacenteno
visitado,semarcaysecolocaenlapila.
Aplicandolaregla1denuevo,sellegaaH.
Enestepuntoyanoesposiblecontinuar,aques
dondeentralaregla2.
Regla2:Sinosepuedeseguirlaregla1,entonces,sies
posibletomarunvrticedelapila.
SeeliminaelvrticeHdelapila,ysetomael
vrticeF.
Fnotienevrticesadyacentesnovisitados,porlo
cualsetomaotrovrticedelapila.
3.

Assepuedecontinuarhastaencontrarunvrticeenla
pilaconunnodoadyacentenovisitado(A).

Asitienevrticesadyacentesnovisitados,sevisita
entonceselsiguiente,C.

Cestalfinaldelalneatambin,porloqueseelimina
delapilaydenuevoestamosconA.

SecontinuaconesteprocesohastaqueAnotengams
vrticesadyacentesporvisitar,conlocualseaplicala
regla

Regla3:Sinosepuedeseguirlaregla1olaregla2,se
terminaelproceso.

Evento Pila
VisitaA A
VisitaB AB
VisitaF ABF
VisitaH ABFH
SacaH ABF
SacaF AB
SacaB A
VisitaC AC
SacaC A
VisitaD AD
Evento Pila
VisitaG ADG
VisitaI ADGI
SacaI ADG
SacaG AD
SacaD A
VisitaE AE
SacaE A
SacaA
Fin
Elcontenidodelapilaeslarutaquesetomdesdeelvrticeinicialparallegarhasta
dondeseest.

SepuedepensardelDFSquesellegatanlejostanrpidocomosepuede,yseretorna
solocuandosealcanzaunfin.
30/10/2013
6
GrficasdeVisibilidad
LaGrficadeVisibilidadparaunespacioCdeconfiguracin
poligonalconsistedearcosdeuninentretodoslospares
devrticesquesepuedenverentres(incluyendotantola
posicininicialyelobjetivo,ascomolosvrtices).

Laslneasrectasdespejadas(caminos)queunenlos
vrticesson,obviamente,lasdistanciasmscortasentre
ellos.

Latareadelplanificadordecaminoses,portanto,
encontrarelcaminomscortodesdelaposicininicialala
posicinmetaporloscaminosdefinidosporelgrficode
visibilidad.
Laplanificacindecaminosempleandogrficasdevisibilidades
bastante popular en robtica mvil, en parte porque
su implementacin es bastante simple.

Particularmente cuando la representacin ambiental

describe objetos en el medio ambiente como polgonos, ya sea
enelespaciocontinuoodiscreto,
labsquedaengrficas
de visibilidad puede
emplear las
descripciones de los
obstculos poligonales
confacilidad.
Hay,sinembargo,dosadvertenciasimportantes
cuandoseemplealabsquedaengrficasde
visibilidad.

Enprimerlugar,eltamaodelarepresentaciny
elnmerodearistasynodosaumentaconel
nmerodeobstculospoligonales.

Porlotantoelmtodoesextremadamente
rpidoyeficienteenambientesdeescasos
objetos,peropuedeserlentoeineficazen
comparacinconotrastcnicascuandoseutiliza
enentornosdensamentepoblados.
Lasegundaadvertenciaesundefectopotencialmucho
msgrave:lasrutasdesolucinqueseencuentrancon
laplanificacinengrficasdevisibilidadtiendena
colocarelrobotlomscercaposiblealosobstculos
durantesurecorridohaciaelobjetivo.

Msformalmente,sepuededemostrarquela
planificacinconlasgrficasdevisibilidadesptimaen
trminosdelalongituddelarutadesolucin.

Esteresultadopoderosotambinsignificaquetodoel
sentidodelaseguridad,entrminosdemantenersea
unadistanciarazonabledelosobstculos,sesacrifica
paraesteptimo.

30/10/2013
7
Lasolucincomnesagrandarlosobstculos
significativamentemsqueelradiodelrobot,o,
alternativamente,modificarlarutadesolucindespusdela
planificacinderutaparaalejarelcaminodelosobstculos
cuandoseaposible.

Porsupuesto,estetipodeaccionessacrificanlosresultados
deptimalongituddelaplanificacindecaminosempleando
lasgrficasdevisibilidad.
DiagramadeVoronoi
EncontrasteconelmtododeGrficasde
Visibilidad,unDiagramadeVoronoiesun
mtododemapadecarreterascompletoque
tiendeamaximizarladistanciaentreelroboty
losobstculosenelmapa.
Paracadapuntoenelespaciolibre,calculala
distanciaalmscercanoobstculo.
Enlospuntosquesonequidistantesadosoms
obstculos,lagrficadeesadistanciatiene
aristasafiladas.
EldiagramadeVoronoiconsisteenregionesdefinidaspor
esospuntosequidistantesdenominadosesqueletodel
espaciolibre.

Cuandoelespaciodeconfiguracindelosobstculosson
polgonos,eldiagramadeVoronoisedefineporunesqueleto
desegmentosderectasyparablicos.

Algoritmosqueencuentrancaminosenelmapa
decaminosdeVoronoisoncompletosaligual
quelosmtodosdelasGrficasdeVisibilidad,ya
quelaexistenciadeuncaminoenelespaciolibre
implicalaexistenciadeunoeneldiagramade
Voronoitambin(esdecir,ambosmtodos
garantizanlaintegridad).
Sinembargo,elcaminoeneldiagramade
Voronoiestporlogenerallejosdeserptimoen
elsentidodelalongitudtotaldeltrayecto.
30/10/2013
8
EldiagramadeVoronoitieneunadebilidadimportanteen
elcasodesensoresdelocalizacindealcancelimitado.

Puestoqueestealgoritmodeplanificacindecaminos
maximizaladistanciaentreelrobotylosobjetosdel
entorno,cualquiersensordecortoalcanceenelrobot
estarenpeligrodefracasarparapercibirsuentorno.

Siseutilizanestossensoresdecortoalcanceparala
localizacin,elcaminoelegidoserbastantepobredesde
unpuntodevistadelalocalizacin.

Porotrolado,elmtododelagrficadevisibilidadpuede
serdiseadoparamantenerelrobottancercacomose
deseedelosobjetosenelmapa.
Hay,sinembargo,unaimportantesutilventajaqueel
mtododeldiagramadeVoronoitienesobrelamayorade
lastcnicasdelaevitacindelobstculo:ejecutabilidad.}

Dadoundeterminadocaminoplanificadomediantela
planificacincondiagramadeVoronoi,unrobotcon
sensoresdedistancia,talcomountelmetrodelsero
ultrasonidos,puedenseguirunbordeVoronoienelmundo
fsicomediantereglassimplesdecontrolquecoincidancon
lasqueseutilizanparacreareldiagramadeVoronoi:el
robotmaximizalaslecturasdemnimoslocalesensus
valoresdelossensores.

Estesistemadecontrolmantendrnaturalmenteel
robotenbordesdeVoronoi(elesqueletodelespacio
libre),porloquelosmovimientosbasadosenVoronoi
puedenmitigarlainexactituddelcodificador.

Estainteresantepropiedadfsicadeldiagramade
Voronoisehautilizadoparallevaracabolarealizacin
automticademapeodeunmedioambientemediante
labsquedaymovimientoenlosbordesdeVoronoi
desconocidos,yentoncesconstruyendounmapade
Voronoiconsistentedelmedioambiente.
PlanificacindeCaminosconCamposPotenciales
Laplanificacindecaminosmediantecampospotenciales
creauncampooungradiente,atravsdelmapadelrobot
quelodirigealaposicindeseadadesdemltiples
posicionesanteriores.
Esteenfoquefueoriginalmenteinventadopara
planificacindecaminosderobotsmanipuladoresyse
utilizaamenudoyconmuchasvariantesenlacomunidad
delarobticamvil.
Elmtododecampospotencialestrataelrobotcomoun
puntobajolainfluenciadeuncampopotencialartificial
U(q).
Elrobotsemuevesiguiendoelcampo,aligualqueunabola
rodaracuestaabajo.
30/10/2013
9
Elobjetivo(unmnimoenesteespacio)actacomo
unafuerzadeatraccinsobreelrobotylosobstculos
actancomopicosofuerzasrepulsivas.
Lasuperposicindetodaslasfuerzasseaplicaalrobot,
que,enlamayoradeloscasos,sesuponequeesun
puntoenelespaciodeconfiguracin(verfigura6.5).
Talcampodepotencialartificialguasuavementeel
robothacialameta,evitandoalmismotiempolos
obstculosconocidos.

Esimportantetenerencuenta,sinembargo,que
estoesmsqueunaplanificacindecamino.
Elcamporesultanteestambinunaleyde
controlparaelrobot.
Suponiendoqueelrobotpuedalocalizarsu
posicinconrespectoalmapayelcampo
potencial,siemprepodrdeterminarsuprxima
accinrequeridabasndoseenelcampo.
Laideabsicadetrsdetodoslosenfoques
posiblesdelcampoesqueelrobotesatrado
hacialameta,mientrasqueesrechazadopor
losobstculosqueseconocendeantemano.
Sinuevosobstculosaparecieranduranteel
movimientodelrobot,sepuedeactualizarel
campodepotencialconelfindeintegraresta
nuevainformacin.

30/10/2013
10
Enelcasomssencillo,sesuponequeelrobotes
unpunto,porlotantolaorientacindelrobotno
seconsiderayelcampopotencialresultantees
slo2D(x,y).
Siseasumeunafuncindecampopotencial
diferenciable,sepuedeencontrarlafuerza
artificialrelacionadaF(q)actuandoenlaposicin
q=(x,y).
F q = -7u q
donde7u q elvectorgradientedeUenla
posicinq.
7u =
0
x
0

Elcampopotencialactuandosobreelrobotes
entoncescalculadocomolasumadeuncampo
atractivodepartedelobjetivoycamposrepulsivosde
partedelosobstculos:
u q = u
utt
q + u
cp
(q)
Similarmente,lasfuerzaspuedensertambinseparadas
enunaparteatractivayunaparterepulsiva:
F q = F
utt
q -F
cp
q
= -7u
utt
q -7u
cp(q)

PotencialAtractivo
Unpotencialatractivo,puede,porejemplo,ser
definidocomounafuncinparablica.
u
utt
q =
1
2
k
utt
p
gouI
2
q
Dondek
utt
esunafactordeescalapositivoy
p
gouI
q denotaladistanciaEuclidiana
q -q
gouI

30/10/2013
11

Potencialrepulsivo
Laideadetrsdelpotencialrepulsivoesgenerarunafuerza
quealejealrobotdetodoslosobstculosconocidos.
Estepotencialrepulsivodebesermuyfuertecuandoelrobot
estcercadelobjeto,peronodebeinfluirensumovimiento
cuandoestlejosdel.
Unejemplodecamporepulsivoes:
u
cp
q
= _
1
2
k
cp
1
p(q)
-
1
p
0
2
si p(q) p
u > p
0
u si p q
> p
0

dondek
cp
esdenuevounfactordeescala,p(q)esladistancia
mnimadesdeqalobjetoy
p
0
ladistanciadeinfluenciadelobjeto.

Lafuncinpotencialderepulsinu
cp
espositivaoceroytiende
ainfinitocuandoqseacercamsalobjeto.

Hayalgunaslimitacionesconesteenfoque.

Setratademnimoslocalesqueaparecendependiendo
delaformaytamaodelosobstculos.

Otroproblemapuedeaparecersilosobjetosson
cncavos.
Estopodrallevaraunasituacinparalaqueexisten
variasdistanciasmnimas,loqueresultaenlaoscilacin
entrelosdospuntosmscercanosalobjeto,loque
podraobviamentesacrificarlaintegridad.

30/10/2013
12

TAREAS
24/09/13
SimulacindelLIDAR
08/10/13
Desplazamientodeunrobotusandounlser
15/10/13
PuestaenmarchadeunROMICi2

SeguimientodeunrobotROMICi2porotrorobot
DesplazamientodelKomalBot
CeilingCamforKomalbot
NavegacinempleandoGPS