Planificación de Trayectorias Libres de Colisión para Múltiples UAVs usando el Perfil de Velocidad

http://www.revista-riai.org Planificaci´ on de Trayectorias Libres de Colisi´ on para M´ ultiples UAVs usando el Perfil de Velocidad Juan Jos´ e Rebollo ∗Iv´ an Maza ∗An´ ıbal Ollero ∗,∗∗ ∗Grupo de Rob´ otica, Visi´ on y Control Universidad de Sevilla Edif. Escuela Superior de Ingenieros Camino de los Descubrimientos, s/n, 41092 Sevilla, Espa˜ na e-mail: juanjor[email protected]; imaza,aoller[email protected] ∗∗ Centro Avanzado de Tecnolog´ ıas Aeroespaciales, FADA-CATEC Aer´ opolis, Parque Tecnol´ ogico Aeroespacial de Andaluc´ ıa N-IV, Km. 529, C/ Wilbur y Orville Wright, 17 41309, La Rinconada - Sevilla, Espa˜ na e-mail: aoller[email protected]o Resumen: Este art´ ıculo presenta un m´ etodo de resoluci´ on de colisiones entre m´ ultiples UAVs que comparten el espacio a´ ereo con aeronaves no cooperativas. El m´ etodo propuesto encuentra trayectorias libres de colisi´ on modificando el perfil de velocidad de los diferentes veh´ ıculos involucrados en la colisi´ on y teniendo en cuenta la existencia de obst´ aculos m´ oviles o veh´ ıculos no cooperativos. El objetivo es encontrar la soluci´ on m´ as cercana a las trayectorias que ten´ ıan planificadas los UAVs inicialmente. La resoluci´ on de colisiones se divide en dos pasos: inicializaci´ on usando una b´ usqueda en ´ arbol, y c´ alculo de la soluci´ on final empleando b´ usqueda Tab´ u. En el art´ ıculo se presentan diferentes simulaciones que ponen de manifiesto la eficiencia del m´ etodo propuesto para la resoluci´ on de colisiones en tiempo real. Copyright c 2009 CEA. Palabras Clave: M´ ultiples UAVs, resoluci´ on de colisiones, b´ usqueda Tab´ u, b´ usqueda en ´ arbol, discretizaci´ on del espacio. 1. INTRODUCCI ´ ON El empleo de m´ ultiples veh´ ıculos a´ ereos no tripulados (UAVs) ha sido propuesto como una alternativa eficiente en misiones de vigilancia, detecci´ on, seguimiento y monitorizaci´ on con aplicaciones en seguridad, protecci´ on del medio ambiente, intervenci´ on en casos de cat´ astrofes y otras. Dicho empleo, cuando se compara con el uso de un ´ unico UAV, permite incrementar la cobertura (Maza and Ollero, 2007), disminuir retrasos de detecci´ on de eventos, incrementar la fiabilidad al no depender la misi´ on de una ´ unica aeronave, as´ ı como disminuir la incertidumbre al disponerse de informaciones y medidas tomadas desde distintos puntos de vista (Ollero and Maza, 2007). El empleo de m´ ultiples UAVs requiere m´ etodos apropiados de coordinaci´ on ya que los UAVs tendr´ an que compartir recursos (espacio a´ ereo, espacio radioel´ ectrico, etc.). En particular, cuando se emplean m´ ultiples UAVs, pueden producirse colisiones tanto entre los UAVs que se utilizan en la misi´ on como con otras aeronaves con las que se comparte el espacio a´ ereo. Este es el problema que se aborda en este art´ ıculo, llevando a cabo primero una detecci´ on de tales colisiones y posteriormente su resoluci´ on modificando las trayectorias. El problema de la resoluci´ on de colisiones puede tratarse de dos formas distintas. La primera consiste en calcular trayectorias libres de colisi´ on, antes de que los veh´ ıculos empiecen a moverse. Este m´ etodo no tiene restricciones temporales en el c´ alculo de la soluci´ on. En la segunda, las colisiones se resuelven en tiempo real una vez que son detectadas. En este caso, el tiempo de c´ alculo juega un papel muy importante. Este segundo problema es el que se resuelve en este art´ ıculo. El escenario en el que se encuentran los UAVs es din´ amico, por ello se producir´ an cambios en las misiones y no ser´ a posible fijar las trayectorias desde el inicio. El problema de planificaci´ on de movimientos para m´ ultiples robots ha recibido gran atenci´ on en los ´ ultimos a˜ nos. En (Richards and How, 2002), se plantea un problema de programaci´ on lineal entera-mixta (MILP) que se resuelve mediante paquetes de programas bien conocidos. La resoluci´ on de este problema posee una gran complejidad debido al gran n´ umero de restricciones y adem´ as no considera la existencia de obst´ aculos m´ oviles. El m´ etodo MILP se ha aplicado tambi´ en a la optimizaci´ on de trayectorias en presencia de obst´ aculos y amenazas (Ruz et al., 2006). En (Tsubouchi and Arimoto, 1994) se propone un m´ etodo para construir geom´ etricamente unas trayectorias libres de colisi´ on en el espacio (x, y, t). Primero, los autores eval´ uan la posici´ on y velocidad de los obst´ aculos m´ oviles. Teniendo en cuenta que la velocidad de los obst´ aculos se supone constante, se calculan una serie de cilindros oblicuos en el espacio (x, y, t)que deben ser evitados. El problema consiste en encontrar una trayectoria que conecte la posici´ on inicial con una l´ ınea vertical que representa el objetivo. Este m´ etodo no resuelve el problema de planificaci´ on de movimientos multirobot, en el cual la trayectoria de m´ as de un robot puede cambiar para resolver las colisiones. El m´ etodo presentado en (Owen and Montano, 2005) tiene la misma caracter´ ıstica. En este ´ ultimo caso, trabajando en el espacio de las velocidades, se ISSN: 1697-7912. Vol. 6, Núm. 4, Octubre 2009, pp. 51-60 obtienen buenos resultados, considerando obst´ aculos m´ oviles y fijos, pero no se plantea la modificaci´ on de m´ as de una trayectoria de forma coordinada. En (Ferrari et al., 1997) se adopta un enfoque distinto del problema, obteni´ endose caminos alternativos mediante peque˜ nas variaciones del movimiento del robot en el tiempo y en el espacio, emple´ andose las estrategias “Stop & Go” y “Shape Changing” para evitar las colisiones, una vez que se ha encontrado el camino. Se supone que los veh´ ıculos poseen una din´ amica muy simple, y no se consideran obst´ aculos m´ oviles. En (Pallottino et al., 2007), se propone una estrategia para guiar m´ ultiples veh´ ıculos entre un punto de inicio y un punto objetivo independientes, y asegur´ andose trayectorias libres de colisi´ on. En estos m´ etodos todos los veh´ ıculos siguen las mismas reglas de tr´ afico. Se mueven con velocidad constante, aunque se define una zona de seguridad y la velocidad en esta zona puede ser cero. Sin embargo, este m´ etodo normalmente lleva a variaciones de las trayectorias para evitar las colisiones, que no ser´ ıan necesarias si se evitaran simplemente modificando el perfil de velocidad de los veh´ ıculos. En (Fujimori and Teramoto, 2000) se presenta un enfoque geom´ etrico. El ´ angulo de direcci´ on y la velocidad de los robots m´ oviles se usan como variables de control para la navegaci´ on y la resoluci´ on de colisiones. Este m´ etodo solo asegura resoluci´ on de colisiones para dos veh´ ıculos. En (Cruz et al., 1998) se desarrolla un m´ etodo basado en la planificaci´ on de velocidades con obst´ aculos m´ oviles. Los obst´ aculos m´ oviles se incluyen como restricciones del movimiento. Este m´ etodo no considera la posibilidad de modificar la trayectoria de todos los veh´ ıculos involucrados en la colisi´ on. En (Kant and Zucker, 1986) se propone la descomposici´ on del problema de resoluci´ on de colisiones en dos: planificaci´ on de las trayectorias (PPP) y planificaci´ on de velocidades (VPP). Una vez que se ha obtenido una trayectoria (PPP), se deber´ a encontrar un perfil de velocidades que evite las colisiones para tales trayectorias (VPP). Este segundo problema es el que se trata en nuestro trabajo. En este art´ ıculo se resuelve un problema de planificaci´ on de movimientos en 3D para m´ ultiples UAVs, compartiendo el espacio con veh´ ıculos a´ ereos no cooperativos (obst´ aculos m´ oviles). Se obtendr´ a un nuevo perfil de velocidad para los diferentes UAVs involucrados en la colisi´ on. Este art´ ıculo presenta un nuevo enfoque heur´ ıstico que va a permitir encontrar soluciones sub´ optimas en menos tiempo que m´ etodos ´ optimos, gracias a la b´ usqueda de soluciones en un espacio discreto. Para aplicaciones en tiempo real, como la abordada en este art´ ıculo, es primordial encontrar una soluci´ on en un tiempo acotado y del orden de los segundos, aunque se tenga que sacrificar levemente la optimalidad de la soluci´ on. El objetivo es calcular una soluci´ on que cambie la trayectoria inicial lo m´ ınimo posible, modificando solamente el perfil de velocidades de los diferentes veh´ ıculos. Los UAVs que deben modificar su trayectoria para evitar la colisi´ on, puede que est´ en ejecutando una misi´ on para la que sea cr´ ıtico mantener cierta trayectoria, tal como por ejemplo la b´ usqueda o seguimiento de un objeto. Por ello se ha optado por modificar el perfil de velocidades lo menos posible, sin tener en cuenta otras estrategias que impliquen un mayor cambio en la trayectoria. La modificaci´ on de la altitud, aun siendo la estrategia m´ as simple, en general no va a ser posible, debido a que el espacio a´ ereo se estructura en capas (Bichi and Pallottino, 2000). La mayor´ ıa de los veh´ ıculos tienen grandes limitaciones din´ amicas, que no les permiten parar o modificar sus trayectorias lo suficientemente r´ apido. Como se pone de manifiesto en (Munoz et al., 1994), la consideraci´ on de las restricciones cinem´ aticas y din´ amicas tiene una importante influencia en la evitaci´ on de colisiones y planificaci´ on de trayectorias. En este art´ ıculo se considera tambi´ en un modelo del UAV que permite tener en cuenta dichas restricciones en el movimiento de los diferentes veh´ ıculos involucrados. Modificando tan solo las velocidades no es posible resolver cualquier colisi´ on, por ejemplo un choque frontal, por lo que se deber´ ıa modificar el camino a seguir y posteriormente aplicar el algoritmo presentado en este trabajo para optimizar el perfil de velocidades. Este tipo de conflictos ser´ an detectados por el algoritmo desarrollado. El resto de este art´ ıculo est´ a organizado como se describe a continuaci´ on. En la siguiente secci´ on se plantea el problema que se va a resolver. En la Secci´ on 3 se describen los algoritmos con los que se resuelve tal problema, en primer lugar la B´ usqueda en ´ Arbol y posteriormente la B´ usqueda Tab´ u. A continuaci´ on, en la Secci´ on 4, se presentan dos simulaciones, la primera de ellas con 3 UAVs y la segunda con 6 UAVs. Aunque en dichas simulaciones se emplean trayectorias compuestas por segmentos rectil´ ıneos, los m´ etodos presentados permiten resolver el problema para cualquier tipo de trayectoria. Finalmente, se presentan las conclusiones y las l´ ıneas de trabajo futuras. 2. PLANTEAMIENTO DEL PROBLEMA El problema de resoluci´ on de colisiones puede ser dif´ ıcil debido al gran n´ umero de soluciones posibles. Se puede simplificar el problema dividi´ endose el espacio en celdas. Las celdas no van a permitir encontrar una soluci´ on ´ optima, pero permiten el uso de algoritmos de b´ usqueda muy r´ apidos que permitir´ an encontrar la soluci´ on deseada. El primer paso para encontrar una soluci´ on al problema de resoluci´ on de colisiones es la detecci´ on de colisiones potenciales. El espacio cartesiano se divide en celdas c´ ubicas. Una trayectoria se puede describir como una secuencia de celdas a las que se les asocia un tiempo de entrada y un tiempo de salida. Por lo tanto, para asegurar trayectorias libres de colisi´ on, mientras un veh´ ıculo ocupa una celda, no puede entrar otro en ella. Cada UAV conoce todas las listas de celdas por las cuales pasar´ an aquellos UAVs que est´ an dentro de un cierto radio Ren un cierto horizonte temporal ΔT. Los UAVs s´ olo conocer´ an las celdas de paso de los UAVs de su entorno, y solo transmitir´ an sus propias celdas a tales UAVs. El problema se resuelve localmente y la cantidad de informaci´ on considerada depende del radio R. Esto facilita detectar si una colisi´ on se va a producir, porque cada UAV simplemente tiene que detectar solapamiento temporal entre una celda de su trayectoria y una celda que pertenece a la trayectoria de otro UAV. La rejilla tridimensional propuesta en este art´ ıculo disminuye la transmisi´ on de informaci´ on entre los veh´ ıculos, no siendo necesario transmitir las trayectorias completas. Esta estrategia tambi´ en reduce el tiempo de detecci´ on de colisiones con respecto a los m´ etodos que usan la trayectoria completa. El objetivo es encontrar las condiciones temporales para cada celda, que nos den unas trayectorias libres de colisi´ on. En el m´ etodo propuesto es posible modificar las trayectorias de todos los veh´ ıculos involucrados en la colisi´ on, para poder encontrar una soluci´ on mejor. En el caso de que cierto veh´ ıculo 52 Planificación de Tra y ectorias Libres de Colisión Para Múltiples UAVs Usando el Perfil de Velocida d no pueda modificar su trayectoria para alcanzar su objetivo adecuadamente, se considerar´ ıa como obst´ aculo m´ ovil, y mantendr´ ıa su trayectoria original tras resolver el problema. En la resoluci´ on de colisiones se considerar´ an los veh´ ıculos que las han detectado en su trayectoria, y todos aquellos veh´ ıculos cuyas trayectorias intersectan con la de alguno de estos veh´ ıculos. El algoritmo propuesto considera tres tipos de veh´ ıculos implicados en la resoluci´ on del problema: Implicados directos: son todos aquellos veh´ ıculos involucrados en una determinada colisi´ on potencial detectada. Implicados indirectos: son los veh´ ıculos cuyas trayectorias se cruzan con las de los implicados directos y cuyos perfiles de velocidades pueden ser modificados por el sistema de resoluci´ on de conflictos. Obst´ aculos m´ oviles: veh´ ıculos involucrados en los conflictos, pero que no modificar´ an su perfil de velocidades (son veh´ ıculos no cooperativos). Los veh´ ıculos implicados indirectos yobst´ aculos m´ oviles, son aquellos veh´ ıculos que podr´ ıan estar implicados en los nuevos conflictos que pudieran surgir al resolver una determinada colisi´ on entre los implicados directos modificando sus perfiles de velocidades. Por otro lado, a veces puede ser beneficioso considerar algunos veh´ ıculos cooperativos como obst´ aculos m´ oviles, porque se disminuye el intercambio de informaci´ on entre los diferentes UAVs y el tiempo de c´ omputo de los algoritmos. Esto se debe a que no se establecen comunicaciones con los obst´ aculos m´ oviles al resolver la colisi´ on. Esta estrategia tambi´ en se ha tener en cuenta cuando la tarea que est´ e llevando a cabo cierto veh´ ıculo no permita variaciones en las velocidades. 2.1 Formulaci´ on Sea jel identificador de una celda en el espacio. Consideremos una variable Cij que valdr´ a 1 si el veh´ ıculo ipasa a trav´ es de la celda j,´ o 0 en caso contrario. Sea tin ij el instante en el que el veh´ ıculo ientra en la celda j,ytout ij el instante en el que el veh´ ıculo iabandona la celda j. De esta manera, el intervalo de tiempo que el UAV ipasa en la celda jvendr´ a dado por Tij =[tin ij ,t out ij ], y podemos decir que habr´ a una colisi´ on potencial en la celda jsi se verifica:  i:Cij =1 Tij =∅(1) siendo ∅el conjunto vac´ ıo. Dado un n´ umero total de veh´ ıculos N, habr´ a un conflicto entre mveh´ ıculos en la celda de identificador jsi N  i=1 Cij =m(2) Sea Δtij el tiempo que el veh´ ıculo ipermanece en la celda j Δtij =tout ij −tin ij ∈[tmin ij ,t max ij ](3) donde tmin ij ytmax ij son los tiempos m´ ınimo y m´ aximo para recorrer la celda de identificador jde acuerdo con el modelo del UAV ien la trayectoria considerada. El problema consiste en el c´ alculo para cada UAV del perfil de velocidades que da lugar a unos intervalos de paso Tij,de forma que en cada celda con conflicto se verifique:  i:Cij =1 Tij =∅,(4) y adem´ as se minimice para cada UAV el siguiente ´ ındice: J= Si−1  p=0 (Δtp−Δtref p)2(5) donde Sies el n´ umero de celdas en la trayectoria del veh´ ıculo i,Δtref pes el tiempo que se tarda en la trayectoria de referencia en pasar a trav´ es de la celda de identificador pyΔtpes el tiempo que se tarda en la trayectoria soluci´ on en pasar por dicha celda. El objetivo del coste (5) es encontrar una trayectoria soluci´ on cercana a la trayectoria de referencia, la cual es la trayectoria de los veh´ ıculos antes de que se detectara la colisi´ on potencial. Minimizando este coste el cambio en las trayectorias es m´ ınimo. La ecuaci´ on (3) se establece teniendo el cuenta el modelo del UAV que se presentar´ a a continuaci´ on. As´ ı, el tiempo que cada veh´ ıculo puede permanecer en cierta celda depender´ a de sus velocidades m´ axima y m´ ınima, as´ ı como de la distancia que recorre en su interior. 2.2 Modelo del UAV Los UAVs se mueven a lo largo de su trayectoria de acuerdo con el modelo (McLain and Beard, 2005): ˙xi=vicos(ψi) ˙yi=visin(ψi) ˙ ψi=αψi(ψc i−ψi) ˙vi=αvi(vc i−vi) ¨ hi=−α˙ hihi+αhi(hc i−hi) (6) donde αψi,αvi,αhi yα˙ hi son constantes conocidas que dependen de la implementaci´ on del UAV. Las variables de control ser´ an ψc iyvc i.Las coordenadas xi,y i,h iindican la posici´ on en el espacio del UAV y ψila orientaci´ on en el plano xy. Por simplicidad solo se considera una variable de orientaci´ on. La velocidad del UAV viene dada por vi. Respeto a ˙ ψiyvi,seva a considerar: −ci<˙ ψi<c i vmin i<v i<v max i (7) donde ci,vmin iyvmax ison constantes positivas que dependen de la din´ amica del UAV. Los tiempos m´ aximos y m´ ınimos que un UAV puede permanecer en cierta celda se calculan teniendo en cuenta este modelo. Para tal c´ alculo, se necesita la distancia que cada veh´ ıculo viaja en la celda correspondiente como se ha comentado anteriormente. Los dos algoritmos presentados en este art´ ıculo tienen en cuenta el modelo din´ amico de los UAVs. La B´ usqueda en ´ Arbol calcula el tiempo asociado a cada rama considerando el modelo J. J. Rebollo, I. Maza, A. Ollero 53 din´ amico. Posteriormente, en la B´ usqueda Tab´ u cada nuevo vecino ser´ a una soluci´ on que se atiene al modelo din´ amico presentado en esta secci´ on. 3. M ´ ETODO DE RESOLUCI ´ ON DE COLISIONES El objetivo es encontrar cuanto tiempo tiene que permanecer cada veh´ ıculo en las diferentes celdas de su trayectoria para asegurar que no existen conflictos. Incluso para este problema, no es posible encontrar una soluci´ on ´ optima en tiempo polinomial, y es importante encontrar una soluci´ on r´ apidamente, porque de otro modo puede que no sea posible evitar la colisi´ on. Por estas razones, en este art´ ıculo se desarrolla un m´ etodo heur´ ıstico basado en la combinaci´ on de la B´ usqueda en ´ Arbol y la B´ usqueda Tab´ u. Estos dos algoritmos heur´ ısticos obtendr´ an un nuevo perfil de velocidad para los diferentes veh´ ıculos que evitan las colisiones. Los algoritmos se basan en la idea de que la forma m´ as r´ apida de encontrar una soluci´ on, es tener en cuenta una serie de reglas l´ ogicas. El Algoritmo de B´ usqueda en ´ Arbol encuentra una soluci´ on libre de colisiones, pero sin considerar el coste (5). Esta soluci´ on ser´ a la soluci´ on inicial que el algoritmo de B´ usqueda Tab´ u necesita para obtener la soluci´ on que estamos buscando en este art´ ıculo. Todos los algoritmos tienen en cuenta el modelo de los UAVs y la distancia recorrida en cada celda. 3.1 Algoritmo de Inicializaci´ on: B´ usqueda en ´ Arbol El algoritmo descrito en este apartado tiene como objetivo encontrar una primera soluci´ on libre de colisiones, aunque no ser´ a la que minimiza la funci´ on de coste presentada anteriormente. Esta soluci´ on servir´ a al algoritmo de B´ usqueda Tab ´ u como punto de partida. La B´ usqueda en ´ Arbol nos proporcionar´ a una soluci´ on en la que los veh´ ıculos viajan a la velocidad m´ axima que les asegura una trayectoria libre de colisi´ on. Esta elecci´ on se justifica en misiones tales como las de exploraci´ on en las que se pretende minimizar el tiempo de ejecuci´ on. La idea b´ asica del algoritmo es que una vez definido el orden de paso de los UAVs que intervienen en cierto conflicto, existir´ a soluci´ on al conflicto si pasando el primero de los veh´ ıculos por la zona de conflicto a la velocidad m´ axima, el resto pueden pasar a velocidades que les permitan cruzar en el orden previamente establecido y sin que se provoque ninguna colisi´ on entre ellos. Si alguno de los veh´ ıculo aun yendo a la velocidad m´ ınima chocase con el anterior, entonces no habr´ ıa soluci´ on para el orden de paso definido. Todos los c´ alculos que llevar´ a a cabo el algoritmo de Inicializaci´ on, se realizan teniendo en cuenta el modelo del UAV presentado en (6). Se define el orden de paso, como el orden en el que los veh´ ıculos cruzan a trav´ es de una potencial colisi´ on. Un conflicto con mveh´ ıculos implicados, tiene m!ordenes de paso distintos. El algoritmo explora los diferentes ordenes de paso en cada conflicto hasta que se encuentra una soluci´ on. Dependiendo de la topolog´ ıa del conflicto, puede que no se encuentre soluci´ on, y en tal caso se tendr´ ıa que modificar el camino a seguir con algunos de los m´ etodos existentes (Wollkind, 2004; Massink and Francesco, 2001) y posteriormente las velocidades. Esto ocurrir´ a cuando se produzca un choque frontal, trasero o la din´ amica del veh´ ıculo no permita la modificaci´ on de las velocidades necesaria para evitar la colisi´ on. En el algoritmo presentado, en primer lugar se busca una soluci´ on explorando el orden de paso m´ as l´ ogico, para el cual el veh´ ıculo que tiene que recorrer menos distancia para llegar al conflicto pasar´ ıa primero. Para cada orden, es posible comprobar si existe soluci´ on en un tiempo computacional reducido (del orden de cent´ esimas de segundo para las simulaciones que presentaremos, aunque depender´ a de la complejidad del problema que se est´ e tratando). Si no hay soluci´ on para cierto orden de paso, el algoritmo permuta el orden de aquellos veh´ ıculos que tienen que recorrer m´ as distancia para llegar al conflicto. Finalmente se llevar´ aa cabo una nueva b´ usqueda para tal orden de paso. Si el problema tiene un total de Mconflictos con mkveh´ ıculos implicados en el conflicto k-´ esimo, habr´ am0!m1!...mM−1! ´ ordenes que comprobar. Cuando ya se ha explorado cierto orden de paso y no hay soluci´ on, el algoritmo permuta el orden de uno de los conflictos. Primero se permuta el conflicto k-´ esimo cuyo coste Rkdefinido como Rk=μk−σk(8) es mayor, donde μkyσkson la media y la desviaci´ on t´ ıpica de la distancia que cada veh´ ıculo implicado tiene que recorrer para llegar al conflicto k-´ esimo. El algoritmo permuta primero el orden de los veh´ ıculos involucrados en los conflictos que est´ an m´ as lejos del inicio de la trayectoria. Esto se debe a que cuando un conflicto est´ a cerca del inicio de la trayectoria, los veh´ ıculos que tienen que recorrer menos distancia para llegar a ´ el, tienen muchas probabilidades de pasar primero en la soluci´ on del problema. Sin embargo, en un conflicto que est´ am ´ as lejos del inicio de la trayectoria, otro conflicto m´ as cercano al inicio podr´ ıa afectar al criterio de b´ usqueda definido arriba (el veh´ ıculo que recorre menos distancia para llegar a un conflicto pasa primero). Las diferencias en la distancia que los veh´ ıculos recorren para llegar a cierto conflicto hacen al orden de paso inicial m´ as apropiado. El t´ ermino σken (8) tiene en cuenta esta idea, disminuyendo el valor de Rkdel conflicto. El n´ umero de ordenes de paso existentes es elevado, pero se ha de tener en cuenta que muchos se descartar´ an muy r´ apidamente por la B´ usqueda en ´ Arbol. A cada una de las trayectorias de los UAVs se le asociar´ aun ´ arbol. Cuando se va desde un nodo al siguiente se pasa por una celda. Entonces, si se va desde el nodo ra´ ız al nodo m´ as lejano, se recorrer´ a la trayectoria completa del UAV asociado a ese ´ arbol. Se encontrar´ a una soluci´ on cuando todos los ´ arboles se construyan totalmente. La distancia entre dos nodos consecutivos est´ a directamente relacionada con el tiempo que el UAV est´ a en la celda asociada. El Algoritmo 1 muestra como se construyen los ´ arboles y como se encuentra la soluci´ on del problema. B´ asicamente, los ´ arboles crecen a medida que se calcula el tiempo que cada veh´ ıculo pasa en las celdas de su trayectoria yendo a la velocidad m´ axima, hasta que se alcance una celda con conflicto. Cuando se detecta el conflicto, una rama asociada a´ el se crea o no dependiendo de si es el turno de tal ´ arbol. Ese turno corresponde con el orden de paso que el ´ arbol est´ a comprobando en la iteraci´ on correspondiente. Por lo tanto, es el turno de un determinado veh´ ıculo si las ramas de los otros ´ arboles asociadas a la misma celda con conflicto, y la asociada al UAV que tiene que pasar antes a trav´ es del conflicto, ya se han creado. Si es el turno, se comprueba si hay colisi´ on en el conflicto comprob´ andose si hay solapamiento temporal entre los tiempos asociados a otras ramas asociadas 54 Planificación de Tra y ectorias Libres de Colisión Para Múltiples UAVs Usando el Perfil de Velocida d Algoritmo 1 B´ usqueda en ´ Arbol. mientras no se haya encontrado una soluci´ on o no se hayan explorado todos los ordenes de paso hacer mientras no se haya llegado al final de cada ´ arbol, y haya soluci´ on hacer para cada ´ arbol, si no se ha llegado al final de este hacer Se avanza a velocidad Vmax hasta el siguiente conflicto, creando las ramas pertinentes si no se ha llegado al final del ´ arbol entonces si es el turno del ´ arbol tratado en el conflicto entonces Se pasa por el conflicto, cre´ andose la rama asociada a este si hay colisi´ on entonces Se ha de ir hacia atr´ as en el ´ arbol y crear nuevas ramificaciones, que resuelvan la colisi´ on. Esto puede implicar que se retroceda tambi´ en en los otros ´ arboles para asegurar trayectorias libres de colisi´ on. Si se llega al inicio del ´ arbol actual al retroceder, es que no hay soluci´ on para el orden de paso considerado actualmente. fin si fin si fin si fin para fin mientras fin mientras al mismo conflicto. Si una colisi´ on aparece, el algoritmo tiene que cambiar la velocidad en las celdas previas del veh´ ıculo que est´ a generando el ´ arbol y ha provocado el solapamiento temporal, y ahora no se podr´ a viajar a la velocidad m´ axima en las celdas previas al conflicto. En un ´ arbol aparece una bifurcaci´ on cada vez que se tiene que crear una nueva rama porque se detect´ o una colisi´ on. La Figura 1 muestra c´ omo el algoritmo resuelve las colisiones que aparecen. El algoritmo va hacia atr´ as y crea nuevas ramas. La rama a,noesv ´ alida porque se necesitan ramas de mayor longitud para evitar la colisi´ on, y debido al modelo del veh´ ıculo, no puede ser m´ as larga, ya que se est´ a yendo a la velocidad m´ ınima. La velocidad m´ ınima conlleva un mayor tiempo de estancia y por ello mayor longitud de la rama. Las ramas by cson v´ alidas, y permiten llegar m´ as tarde a la celda (8,10,10). La Figura 2 muestra que hab´ ıa solapamiento temporal entre los veh´ ıculos 1 y 2 en la celda (8,10,10), lo cual indica que hay colisi´ on. Despu´ es de crear las nuevas ramas la colisi´ on desaparece, tal como se puede apreciar en la Fig. 2. Los cambios llevados a cabo por el algoritmo de B´ usqueda en ´ Arbol para evitar el solapamiento temporal, podr´ ıan afectar a otros ´ arboles y provocar nuevas colisiones. La Figura 1 muestra que el ´ arbol asociado al UAV3 tiene que generar nuevas ramas (rama d), porque el UAV1 pasa antes por la celda (5,8,8) y el UAV3 colisionar´ ıa con el UAV1 si el ´ arbol asociado al UAV3 no reconstruyera sus ramas previas al nodo m. Se tiene que recalcular el tiempo que el UAV3 permanece en las celdas previas a la (5,8,0), porque la condici´ on que se tiene que satisfacer en el conflicto ha cambiado. Si el algoritmo de b´ usqueda en ´ arbol no encuentra una soluci´ on para cierto orden de paso, todos los ´ arboles se volver´ an a Figura 1. El algoritmo vuelve atr´ as y crea nuevas ramas (b,c yd) que permiten evitar la colisi´ on. Los 3 n´ umeros que aparecen entre par´ entesis son el identificador de la celda tridimensional. Figura 2. En este diagrama temporal se puede observar el solapamiento temporal inicial entre los UAVs 1 y 2 en la celda de identificador (8,10,10). Dicho solape corresponde a una colisi´ on potencial y se ha indicado en el diagrama mediante el ´ area rectangular gris entre las l´ ıneas temporales de los UAVs 1 y 2. construir desde el inicio consider´ andose el siguiente orden de paso determinado por el criterio de b´ usqueda que se define en (8). Los UAVs implicados directos eindirectos construyen el ´ arbol de la misma forma. Sin embargo, el ´ arbol asociado a un obst´ aculo m´ ovil est´ a terminado desde el inicio, y no se podr´ an modificar porque sus trayectorias y velocidades son fijas. El algoritmo de B´ usqueda en ´ Arbol permite encontrar la soluci´ on al problema en un tiempo reducido, del orden de milisegundos, compar´ andolo con otros m´ etodos que resuelven el problema sin considerar un espacio dividido en celdas c´ ubicas (Richards and How, 2002), los cuales tardan unos pocos minutos ya que algunos modifican toda la trayectoria. 3.2 B´ usqueda Tab´ u El algoritmo de B´ usqueda en ´ Arbol encuentra una soluci´ on al problema pero no considera la funci´ on de coste (5). El algoritmo de B´ usqueda Tab´ u (Glover and Laguna, 1997) modifica la soluci´ on que la B´ usqueda en ´ Arbol encontr´ o, minimizando el coste. La B´ usqueda Tab´ u mejora el resultado de un m´ etodo de b´ usqueda local, usando estructuras de memoria para evitar m´ ınimos locales. Los elementos que es necesario definir para el correcto funcionamiento de la B´ usqueda Tab´ u son los siguientes: J. J. Rebollo, I. Maza, A. Ollero 55 Figura 3. Representaci´ on del espacio de las soluciones a las cuales se podr´ a llegar desde la soluci´ on inicial obtenida mediante la B´ usqueda en ´ Arbol. Los puntos negros representan soluciones seleccionadas por la B´ usqueda Tab´ u. Soluci´ on inicial x: Ser´ a la soluci´ on obtenida en la B´ usqueda en ´ Arbol. Esta soluci´ on est´ a formada por una serie de variables temporales que determinan el tiempo que los veh´ ıculos pasan en cierta celda. Cada variable temporal es el tiempo que cierto veh´ ıculo est´ a en un grupo de celdas. Gracias a la agrupaci´ on de celdas, el n´ umero de variables se reduce, lo cual nos permite encontrar la soluci´ on deseada m´ as r´ apido. Vecindario N(x): En cada iteraci´ on el algoritmo de B´ usqueda Tab´ u explora el vecindario N(x), y elige la mejor soluci´ on. Lista tab´ u:LaB ´ usqueda Tab´ u necesita una memoria para recordar las ´ ultimas soluciones visitadas y evitar m´ ınimos locales. En (Hertz et al., 1995), se demuestra que un valor del tama˜ no de la lista para el que se obtienen buenos resultados es 7. En la Fig. 3) se puede ver c´ omo la B´ usqueda Tab ´ u evoluciona desde la soluci´ on inicial hasta la final, actualizando en cada iteraci´ on la lista tab´ u incluyendo las soluciones internas al radio indicado, y que por lo tanto no podr´ an ser soluci´ on hasta que pase cierto n´ umero de iteraciones y se borren de la lista tab´ u. Criterio de aspiraci´ on: Una soluci´ on que pertenece al vecindario es v´ alida si no es una soluci´ on tab´ u. Pero algunas soluciones se han de considerar incluso si son soluciones tab´ u, por ejemplo soluciones que son mejores que la mejor soluci´ on encontrada hasta el momento (Gengreau, 2002). Criterio de finalizaci´ on: Es una condici´ on de finalizaci´ on de la B´ usqueda Tab´ u. Se considera un n´ umero m´ aximo de iteraciones sin mejorar la soluci´ on ´ optima encontrada hasta el momento. El Algoritmo 2 muestra el pseudoc´ odigo de la B´ usqueda Tab´ u. En nuestro problema, el vecindario consistir´ a en todas las soluciones obtenidas incrementando en Δt, una de las variables de tiempo de x. Esta nueva soluci´ on no tiene colisiones y satisface las restricciones impuestas por la din´ amica del modelo. Las soluciones tab´ u ser´ an todas aquellas cuya distancia a las soluciones de la lista es menor que Δt/2(ver Fig. 3). En dicha figura se representa el espacio de las soluciones a las cuales se podr´ a llegar desde la soluci´ on inicial obtenida mediante la Algoritmo 2 Pseudoc´ odigo B´ usqueda Tab´ u. Elegir x∈Xpara empezar el proceso de b´ usqueda y hacer xop =x mientras no se cumpla el criterio de finalizaci´ on hacer Se busca x∈N(x)que minimice f(x)y que no est´ aen la lista tab´ u,osiloest ´ a, cumpla los criterios de aspiraci´ on si f(x)<f(xop)entonces xop =x fin si Se incluye xen la lista tab´ u fin mientras B´ usqueda en ´ Arbol. Los puntos negros representan soluciones seleccionadas por la B´ usqueda Tab´ u. Esas soluciones se corresponden con la iteraci´ on que se indica en cada punto (ser´ ıan las x). El c´ ırculo que rodea cada punto negro contiene las soluciones que se van a a˜ nadir a la lista tab´ u en la iteraci´ on tratada (por lo que no ser´ an v´ alidas durante cierto numero de iteraciones que viene determinado por el n´ umero de elementos de la lista tab´ u), y en nuestro caso son las que est´ an a una distancia inferior a Δt/2. Estos c´ ırculos son tangentes ya que se opt´ o por considerar como vecindario en cada iteraci´ on las soluciones a una distancia Δtde la ´ ultima soluci´ on encontrada. La B´ usqueda Tab´ u modifica el tiempo que los implicados directos eindirectos pasan en cada celda. La Figura 3 muestra c´ omo la soluci´ on se mueve desde la soluci´ on inicial que encontr´ oel algoritmo de B´ usqueda en ´ Arbol, hacia otra soluci´ on en la cual el valor del coste es menor, y por lo tanto ser´ a una soluci´ on mejor para el problema que se pretende resolver. En la iteraci´ on final (la octava), se satisface el criterio de finalizaci´ on y por eso ya no se continua con la b´ usqueda. 4. RESULTADOS DE SIMULACI ´ ON En esta secci´ on se presentan dos simulaciones. Se ver´ a como para los dos problemas que se van a proponer con 3 y 6 UAVs, se han encontrado soluciones de gran calidad en tiempos muy reducidos. 4.1 Simulaci´ on con 3 UAVs En esta secci´ on se presenta una simulaci´ on con 3 UAVs implicados. El UAV1 y el UAV2 est´ an barriendo una regi´ on, y el UAV3 es un veh´ ıculo no cooperativo teleoperado, el cual se considera como obst´ aculo m´ ovil. La Figura 4 muestra la proyecci´ on x−yde las trayectorias completas de los tres UAVs. Las colisiones se detectan y resuelven en el interior del ´ area cuadrada rayada en la Fig. 4. En el espacio xyz de la Fig. 5 se muestra en tres dimensiones y ampliada dicha ´ area de resoluci´ on de la Fig. 4. El UAV1 y el UAV2 se consideran veh´ ıculos implicados directos yelUAV3obst´ aculo m´ ovil. La Figura 6 es un diagrama temporal de las trayectorias originales, en el que cada punto indica una transici´ on hacia la siguiente celda de la trayectoria. En tal figura se puede ver que hay solapamiento temporal en la celda (13,13,10), y por lo tanto colisi´ on. Hay dos conflictos, uno en la celda (13,13,10) entre el UAV1 y el UAV2 y otro en la celda (16,13,10) entre el UAV1 y el UAV3. El objetivo es encontrar una soluci´ on libre de colisiones, que difiera de las trayectorias iniciales lo m´ ınimo posible. La Figura 7 muestra los resultados obtenidos por el algoritmo de B´ usqueda en ´ Arbol. Se puede ver que la trayectoria del 56 Planificación de Tra y ectorias Libres de Colisión Para Múltiples UAVs Usando el Perfil de Velocida d Figura 4. Proyecci´ on x−yde las trayectorias de los 3 UAVs. El UAV1yelUAV2est ´ an barriendo una regi´ on,yelUAV3 es un veh´ ıculo no cooperativo teleoperado. Figura 5. Detalle de la zona rallada de la Fig. 4 en la que se resuelve el problema para los tres UAVs. Figura 6. Diagrama temporal de las trayectorias iniciales, en el que cada punto indica una transici´ on hacia la siguiente celda de la trayectoria. Figura 7. Resultados obtenidos por el algoritmo de B´ usqueda en ´ Arbol. Se puede ver que la trayectoria del UAV3 no cambi´ o porque se consider´ o como obst´ aculo m´ ovil. Figura 8. Soluci´ on obtenida al ejecutar la B´ usqueda Tab´ u sobre la soluci´ on calculada por la B´ usqueda en ´ Arbol. UAV3 no cambi´ o porque se consider´ ounobst´ aculo m´ ovil. Sin embargo, en la soluci´ on para el UAV1 y el UAV2, estos viajan a la velocidad m´ as elevada que asegura que no se produce ninguna colisi´ on. El UAV1 va m´ as lento que el UAV2, ya que el conflicto entre el UAV1 y el UAV3 fija una restricci´ on temporal en la celda (16,13,10) que se debe satisfacer. El UAV3 est´ am ´ as cerca del conflicto que el UAV1, por ello el algoritmo de B´ usqueda en ´ Arbol comprueba si hay soluci´ on pasando el UAV3 primero. Por lo tanto, dado que hay soluci´ on si pasa el UAV3 delante del UAV1, este ser´ aelorden de paso definitivo. Esto hace que el UAV1 tenga una restricci´ on temporal en la celda (16,13,10). Ahora si ejecutamos la B´ usqueda Tab´ u para la soluci´ on encontrada, se obtiene la soluci´ on que puede verse en la Fig. 8. Esta soluci´ on est´ a compuesta de unas trayectorias pr´ acticamente iguales a las que se ten´ ıan inicialmente, las cuales se observan en la Fig. 6, pero ahora no existen colisiones. Un problema con 3 UAVs donde cada uno de ellos tiene una trayectoria compuesta por 30 celdas, con Δt=0,1segundos se ha resuelto en menos de un segundo, lo cual se ajusta a las restricciones temporales de aplicaciones de navegaci´ on de aeronaves en tiempo real. El tiempo de c´ omputo no depende de la forma de las trayectorias (no tienen por qu´ e ser rectil´ ıneas), porque cada una de ellas ser´ a una secuencia de celdas y los algoritmos las tratan siempre de la misma forma. J. J. Rebollo, I. Maza, A. Ollero 57 Figura 9. Trayectorias originales en la simulaci´ on con 6 UAVs, donde los veh´ ıculos UAV1, UAV2, UAV3 y UAV4 (implicados directos) colisionan en la celda (20,20,10), y a su vez tenemos el UAV5 y el UAV6 (obst´ aculos m´ oviles) pasando cerca de la colisi´ on. 4.2 Simulaci´ on con 6 UAVs En este apartado se presentan los resultados obtenidos al resolver un problema con 6 UAVs. De estos 6 UAVs, 4 son los que tendr´ an que evitar la potencial colisi´ on. Los otros dos UAVs se deber´ an tener en cuenta para hallar las nuevas trayectorias, debido a que pasan muy cerca de la colisi´ on a resolver. En la Fig. 9 pueden verse las trayectorias originales, donde los veh´ ıculos UAV1, UAV2, UAV3 y UAV4 (implicados directos) colisionan en la celda (20,20,10),yasuveztenemos el UAV5 y el UAV6 (obst´ aculos m´ oviles) pasando cerca de la colisi´ on. En la resoluci´ on de este problema no se modificar´ an las trayectorias del UAV5 y el UAV6, simplemente se impondr´ an unas condiciones temporales en ciertas casillas, que deber´ an respetar el resto de UAVs en la resoluci´ on del problema. Cada una de las trayectorias de los UAVs est´ a compuesta por 60 celdas, se ha tomado Δt=0,1segundos, y se tienen las colisiones y conflictos registrados en la Tabla 1. En las Figs. 10 y 11, se tiene un diagrama temporal de las trayectorias antes y despu´ es de la resoluci´ on. En este diagrama, el tiempo existente entre un nodo y el siguiente representa el tiempo de estancia en la celda asociada. Es de destacar que se ha resuelto un problema entre 6 UAVs modificando s´ olo las velocidades y haciendo que el perfil de velocidades inicial y el encontrado al resolver el problema difieran muy poco. Se puede ver claramente que las Figs. 10 y 11 son muy similares. De los nuevos tiempos de estancia en las celdas, lo m´ as relevante es c´ omo se ha resuelto la colisi´ on potencial en la celda (20,20,10). En la Fig. 12 se puede ver el solapamiento espacial y temporal que exist´ ıa, y en la Fig. 13 se observa c´ omo despu´ es de ejecutar los algoritmos de resoluci´ on de colisiones, desaparece tal solapamiento. En la Fig. 14 se tiene una representaci´ on espacial de tal informaci´ on para distintos instantes de tiempo. Se muestra la celda en el plano x−yen la que se encuentra cada uno de los UAVs para cuatro instantes distintos. Finalmente se muestra c´ omo evoluciona el tiempo de c´ omputo en funci´ on del n´ umero de UAVs. Se ha calculado el tiempo de c´ omputo considerando primero s´ olo el UAV1 y el UAV2, Figura 10. Diagrama temporal con las trayectorias iniciales. Figura 11. Diagrama temporal tras aplicar el algoritmo de resoluci´ on de conflictos. Figura 12. Solapamiento espacial y temporal inicial entre los UAVsdel1al4. Figura 13. Soluci´ on obtenida para el conflicto presentado en la Fig. 12. 58 Planificación de Tra y ectorias Libres de Colisión Para Múltiples UAVs Usando el Perfil de Velocida d Figura 14. Detalle de la soluci´ on al conflicto presentado en la Fig. 12. Posiciones de los UAVs en los instantes: a) t=14s b) t=16s c) t=18s d) t=20s. Cuadro 1. Conflictos y colisiones entre los distintos UAVs en la simulaci´ on con 6 veh´ ıculos. Colisiones Conflictos (20,20,10) UAV1,UAV2, (24,20,10) UAV1,UAV6 UAV3,UAV4 (20,24,10) UAV2,UAV6 (20,10,10) UAV2,UAV5 (22,22,10) UAV3,UAV6 (10,10,10) UAV3,UAV5 (30,10,10) UAV4,UAV5 (29,10,10) UAV4,UAV5 (34,10,10) UAV5,UAV6 Figura 15. Tiempos de c´ omputo en funci´ on del n´ umero de UAVs. Se ha calculado el tiempo de c´ alculo considerando primero s´ olo el UAV1 y el UAV2, posteriormente se ha incluido el UAV3, y as´ ı sucesivamente hasta llegar a seis UAVs. posteriormente se ha incluido el UAV3, y as´ ı sucesivamente hasta llegar a seis UAVs. Las simulaciones se han llevado a cabo con una CPU de 1.7GHz y 1GB de RAM. Los resultados obtenidos se pueden ver en la Fig. 15. El tiempo crece de forma aproximadamente lineal, si no se considera el caso de 6 UAVs. Esto se debe a que al incluir el UAV6, aparecen 4 nuevas celdas con conflicto, y adem´ as 3 de ellos est´ an muy cerca de la celda (20,20,10) donde se produce la colisi´ on cu´ adruple, con lo que se dificulta el c´ alculo. A pesar de ello siguen siendo tiempos v´ alidos, ya que resuelve el problema en unos pocos segundos. Adem´ as, la B´ usqueda Tab´ u consume la mayor parte de los tiempos mostrados en la Fig. 15. Si solamente se ejecutara la B´ usqueda en ´ Arbol, los tiempos estar´ ıan comprendidos entre 10 y 20ms. Si el n´ umero total de veh´ ıculos del sistema fuese a´ un m´ as elevado, dado que las colisiones se resuelven localmente, s´ olo aquellos veh´ ıculos que est´ an lo suficientemente cerca de la colisi´ on deber´ an tenerse en cuenta en su resoluci´ on. Esto har´ a que el tiempo de c´ alculo siga siendo aceptable a pesar de que se tenga un gran n´ umero de UAVs en el sistema, ya que no se resolver´ ıan todos simult´ aneamente. 5. CONCLUSIONES Y TRABAJO FUTURO En este art´ ıculo se ha presentado una nueva estrategia para resolver el problema de resoluci´ on de colisiones entre m´ ultiples UAVs cooperativos y no cooperativos (obst´ aculos m´ oviles), que comparten el espacio a´ ereo. El objetivo era encontrar una soluci´ on en tiempo real modificando las trayectorias de los veh´ ıcuJ. J. Rebollo, I. Maza, A. Ollero 59 Tabla