Abstract Tasks increasingly complex in robotics require an interfiera con tems existentes en el espacio de trabajo. En la
increasing effort for path planning in dynamic enviroments. This literatura se han reportado varios algoritmos eficientes para
paper present a novel method for detect collisions between robot resolver este problema. En [2] presentan una clasificacion de
and its enviroment in order to prevent dangerous maneuvers. Our
method is based on transforming each robot link and enviroment los algoritmos reportados en seis grupos: (i) colisiones basadas
object in a set of bounding boxes. The aim for this kind of en politopos convexos, (ii) colisiones basadas en jerarquas
prismatic aproximation is detect collision between objects in de volumenes limitadoras, (iii) deteccion de colision continua
work space by testing collision between box faces from diferent (iv) deteccion de colision de fase extensa, (v) deteccion de
objects. The computacional cost of this aproach has been tested in colisiones por nubes de puntos y (vi) colision en paralelo y
simulation, we set up our enviroment with a HP20D robot and
an obstacle, both of them represented by their corresponding calculo de proximidad. Colision basada en politopos convexos
chain of bounding booxes, the expiriment consisted on moving (Convex Polytope). Se fundamenta en el calculo de distancias
the robot end effector from initial position, on the right of the Euclidianas entre dos politopos convexos. Dos algoritmos re-
obstacle, to the final position, on the left of the obstacle, along portados son, algoritmo de Gilbert?Johnson?Keerthi (GJK) [3]
a straight line trajectory. The probe let us check the correct y algoritmo de Lin-Canny [4]. Colision basada en jerarquas de
behavior of collision detector in a real task. This method was
compared against another method based on retrieving for each volumenes limitadoras (Bounding Volume Hierarchy-BVH).
face of box a point cloud representation and then to establish Se fundamenta en la definicion de estructuras geometricas
collision by detecting if the minimal distance between points from para representar los objetos en el espacio de trabajo del
different boxes is lower than some specific threshold. The result robot. Existen diversas aplicaciones. En [5] se realiza la
shows a difference of some magnitude orders in computacional deteccion de colisiones utilizando a rboles de AABB (Axis-
cycles for method based on point cloud, higher than our method
based on intersecting planes. aligned bounding box-AABB), en [6] se aplica OBB (oriented
bounding box - OBB), en [7] aplican k-DOP (discrete oriented
Index Terms Forward Kinematic model, coillision detector, polytope k-DOP), en [8] utilizan SSVs (swept sphere volume
plane intersect.
- SSV). Otras tecnicas reportadas son las estructuras de datos
espaciales kd-trees y octrees, sin embargo, se prefieren las
I. I NTRODUCTION tecnicas BVH por su rapida respuesta [2]. Deteccion de
como avance conservativo (conservative advancement CA) II. M ODELAMIENTO DEL ENTORNO
se fundamenta en el avance incremental de los objetos en
Los objetos que conforman el entorno de trabajo: robot ma-
cada intervalo de tiempo, mientras se evitan los obstaculos
nipulador Motoman HP20D , caja y suelo, se construyen como
[13]. Deteccion de colisiones de fase extensa (Broad-Phase
conjuntos de rectangulos relacionados entre si por matrices
Collision Detection - BPCD): Es el nombre que reciben las
de transformacion homogeneas, tal y como se presenta en la
tecnicas de deteccion de colisiones que se preocupan por
Fig. 1. All, Cada eslabon se representa a traves de un prisma
resolver la pregunta que objetos tienen una alta posibilidad
hueco de 4 caras (amarilla, roja, azul y verde) referidas al
de colision?. Se diferencia de la alternativa conocida como
sistema de referencia de su articulacion correspondiente. En
narrow-phase, en donde se resuelve la pregunta que parte
posicion home el robot exhibe alineacion de colores por cara,
del objeto A colisiono con el objeto B?. Con el proposito
lo cual permite visualmente detectar rotaciones articulares de
de evitar chequeos de colision con complejidad O(n2), la
una manera simple, des-alineacion de colores entre caras de
tecnica BPCD se encarga de descartar pares de objetos que
eslabones consecutivos. La rotacion de cada eslabon se efectua
estan alejados entre ellos [2]. Algunos algoritmos reportados
alrededor del eje Z para el sistema de referencia asociado.
son, sweep and prune (SaP) [14] y spatial subdivision [15].
Deteccion de colisiones por nubes de puntos (Point Cloud
Collision Detection). Esta alternativa surge por causa de la
baja disponibilidad de representaciones geometricas exactas
en calculos de proximidad y deteccion de colisiones dentro de
ambientes industriales. Por el contrario, las representaciones
disponibles en ambientes industriales son desordenadas y poco
precisas por causa de los sensores utilizados: camaras RGB-
D, sensores LIDAR - Laser Imaging Detection and Ranging.
Adicionalmente, existe una alta frecuencia de actualizacion, lo
que provoca grandes cantidades de nubes de puntos para ser
procesadas. Dos tecnicas reportadas son [16]: (i) AABB tree
representation, (ii) octree representation. Colision en paralelo
y calculo de proximidad. La disponibilidad de CPUs y GPUs
de multiples nucleos permite la implementacion de algoritmos
de proximidad y colisiones en paralelo [17]. Son varios los
algoritmos reportados, entre los que se destacan: en [18]
presentan la solucion a colision simple, en [19] resuelven el
problema de multiples colisiones, en [20] abordan el problema
de colision de N-cuerpos.
c(i ) s(i )c(i ) s(i )s(i ) ai c(i )
s(i ) c(i )c(i ) c(i )s(i ) ai s(i )
Tii1 = 0
s(i ) c(i ) di
0 0 0 1
(1)
A continuacion se construye un esqueleto del robot, uniendo
hilos entre los sistemas de articulacion Ti0 y Ti+1
0
, como se
observa en la Fig. 2.
si P0x = 0
correspondiente para la recta tratada sobre el rectangulo. Los Algorithm 1 Detection algorithm
valores de SiA y SiB se hayan verificando los valores s Precondition:
de cada punto de corte, de ellos se filtran los que esten 1: CollisionF inish F alse
comprendidos entre 0 y 1, luego se obtiene SiA = min (pcij ) 2: F CollisionLink F alse
0s1
y SiB = max (pcij ). 3: F CollisionP lane F alse
0s1 Result: CollisionF inish
Dado que existe una correspondencia entre s y t, cada seg-
4: if (CollisionArm) then
mento [SiA , SiB ] le corresponde un segmento [siA , siB ] que es
5: CollisionLink T rue
un rango medido sobre la linea Lnj del rectangulo y [tiA , tiB ]
6: Break;
que es un rango medido sobre la lnea de interseccion Lni , por
7: else if (CollisionEnviroment) then
lo tanto, para estimar si hay solapamiento entre rectangulos
8: CollisionF inish T rue
basta con determinar si dos segmentos correspondientes a
9: else
dos planos distintos [t1A , t1B ] y [t2A , t2B ] estan separados o
10: CollisionF inish F alse
no. Si no hay interseccion de segmentos en t, tampoco hay
11: end if
interseccion entre rectangulos segun la condicion 2.
Function 1: CollisionArm
12: for (linki kinematicChain) do
III-B. Algoritmo para la Interseccion de cuerpos prismaticos 13: for (linkj=i+2 kinematicChain) do
El algoritmo tiene como condiciones iniciales tres variables 14: F CollisionLink F CollisionLink or
booleanas que permite identificar las colisiones. Se tiene un ColissionLink(Linki , Linkj )
algoritmo principal, el cual esta compuesto por estructuras 15: end for
de decisiones, las cuales se encargan de hacer llamado a las 16: end for
funciones. La funcion uno se encarga de evaluar las restriccio- 17: Return F CollisionLink
nes cinematica por articulacion, con el fin de garantizar el no Function 2: CollisionEnviroment
contacto entre eslabones consecutivos. En caso de identificar 18: for (linki kinematicChainn ) do
una colision la variable FCollisionLink se vuelve verdadera. 19: for (planej Linki ) do
Por lo que dentro de la estructura de decision, CollisionFinish 20: for (P lanek Environment) do
toma el valor verdadero identificado la colision; es decir, 21: F CollisionP lane F CollisionP lane or
evalua si existe colision entre eslabones que disten mas de CollisionP lane(Linki .P lanej , P lanek )
una posicion en la cadena cinematica, comparando todas las 22: end for
4 caras del eslabon i-esimo con las caras de los eslabones i- 23: end for
e simo+2, hasta llegar al final de la cadena. La funcion dos 24: end for
por su parte, tiene como objetivo evaluar la interseccion entre 25: Return F CollisionP lane
todos los rectangulos que conforman la cadena cinematica,
con todos los rectangulos que conforman los demas objetos
del ambiente del robot HP20D. Uno de los aspectos centrales a considerar para la correcta
adecuacion del metodo basado en interseccion de planos en
IV. P RUEBAS Y R ESULTADOS el espacio, es el encapsulamiento de los eslabones de una
Para probar el funcionamiento del detector de colisiones, cadena cinematica en el prisma mas pequeno que pueda
se realizo una interpolacion lineal entre dos posiciones en el contenerlos. En este proceso, si bien, se asumen como parte
espacio del trabajo del robot, posicion inicial a la derecha de la del robot espacios vacos, estos no son significativos y en
caja y posicion final a la izquierda de la caja. En cada posicion cambio de ello, se obtiene un ahorro en costo computacional
intermedia se hizo un llamado al detector de colisiones y el del algoritmo.
resultado fue almacenado a lo largo de toda la trayectoria con El metodo utilizado se comparo con otra alternativa basada
el a nimo de dibujar en color verde las posiciones libres de en el mallado de la superficie de los objetos, donde cada
colision y en color rojo, las posiciones en las cuales el robot malla es un muestreo del objeto que produce una nube
estuvo en contacto consigo mismo o con la caja. El resultado de puntos tridimensionales. La discretizacion de los objetos
de la interaccion del Robot HP20D con el ambiente de trabajo permite estimar la distancia entre ellos, a partir la medicion
a lo largo de la trayectoria definida se puede apreciar en la de distancia entre puntos con alguna metrica, para as concluir
Fig. 7. All se aprecia al interior de la caja un cilindro de color sobre la presencia de alguna condicion de colision. Este tipo
verde en las posiciones libres de colision y rojo en los puntos de alternativa requiere un tiempo de ejecucion, varios ordenes
de interferencia con la caja. de magnitud, por encima del detector de colisiones basado en
interseccion de rectangulos.
V. C ONCLUSIONES Como trabajo futuro, se espera implementar el algoritmo
El detector de colisiones presentado es una propuesta rapida propuesto en plataformas mucho mas rapidas como Visual Cpp
que da solucion a un problema de gran interes en el campo de y acelerar su funcionamiento con la utilizacion de software
la planificacion de tareas y especficamente, en la planificacion paralelo usando CUDA. Adicionalmente, este modulo sera
de trayectorias. integrado en el desarrollo de planeadores de trayectorias
6
R EFERENCES
[1] International federation of robotics ifr, http://www.ifr.org/industrial-
robots/statistics/, 2013, accessed: 21-Jul-2016.
[2] J. Pan, S. Chitta, and D. Manocha, Fcl: A general purpose library for
collision and proximity queries, in Robotics and Automation (ICRA),
2012 IEEE International Conference on. IEEE, 2012, pp. 38593866.
[3] E. G. Gilbert, D. W. Johnson, and S. S. Keerthi, A fast procedure for
computing the distance between complex objects in three-dimensional
space, IEEE Journal on Robotics and Automation, vol. 4, no. 2, pp.
193203, 1988.
[4] M. C. Lin and J. F. Canny, A fast algorithm for incremental distance
calculation, in Robotics and Automation, 1991. Proceedings., 1991
IEEE International Conference on. IEEE, 1991, pp. 10081014.
[5] Software library for interference detection -version 2.1.0,
http://solid.sourceforge.net/, 2002, accessed: 21-Jul-2016.
[6] S. Gottschalk, M. C. Lin, and D. Manocha, Obbtree: A hierarchical
structure for rapid interference detection, in Proceedings of the 23rd
annual conference on Computer graphics and interactive techniques.
ACM, 1996, pp. 171180.
[7] J. T. Klosowski, M. Held, J. S. Mitchell, H. Sowizral, and K. Zikan,
Efficient collision detection using bounding volume hierarchies of k-
dops, IEEE transactions on Visualization and Computer Graphics,
vol. 4, no. 1, pp. 2136, 1998.
[8] E. Larsen, S. Gottschalk, M. C. Lin, and D. Manocha, Fast proxi-
mity queries with swept sphere volumes, Technical Report TR99-018,
Department of Computer Science, University of North Carolina, Tech.
Rep., 1999.