Atajos para mover eficientemente un quadrotor a través del Grupo Especial Euclídeo SE(3)

(Artículo traducido de colaboración en MappingIgnorance.org: partes 1 y 2 )

La parte más llamativa de los coches sin conductor, drones autónomos y quadrotors haciendo piruetas es, sin duda, lo chulos que quedan los vídeos de las máquinas en plena acción, para qué negarlo. Pero detrás de cada “demo” con robots hay mucho trabajo, teórico y práctico, y hoy vamos a arrojar un poco de luz a uno de los mayores desconocidos para el público en general: las complicaciones matemáticas que tiene el hecho de que tengamos que planificar caminos en un maldito mundo tridimensional, y algunos de los últimos avances en esa rama. Voy a intentar dar una introducción entrando en detalles técnicos, pero a quien de verdad le interese el tema le recomendo que lea los artículos que voy enlazando.

Todos estamos familiarizados con lo que llamamos espacio Euclídeo bi o tridimensional, llamados matemáticamente \(\mathbb{R}^2\) o \(\mathbb{R}^3\). Para definir la posición de un punto en él, nos basta con dar sus dos o tres coordenadas cartesianas, por ejemplo. Un ejemplo que todos conocemos de planificación de caminos para puntos en espacios bidimensionales es el clásico pasatiempo de encontrar la salida de un laberinto. Si vamos marcando con un lápiz el camino hasta la salida, obtenemos una trayectoria que es la secuencia de posiciones que un punto debería recorrer para superar el laberinto.

 

En el mundo de la inteligencia artificial y la robótica, hace años que se conocen métodos bastante eficientes para encontrar caminos libres de obstáculos para un punto que se quiera llevar desde un punto A hasta otro punto B. Por ejemplo, el Rapidly exploring random tree (RRT) ((LaValle, Steven M.; Kuffner Jr., James J. (2001). «Randomized Kinodynamic Planning» (PDF). The International Journal of Robotics Research (IJRR) 20 (5). doi:10.1177/02783640122067453)) propuesto en 2001 es ya todo un clásico a pesar de su relativa corta edad. Consiste en ir haciendo crecer un árbol de trayectorias a base de elegir puntos al azar y unirlos con el tronco más cercano.

 

El problema viene cuando no tratamos de mover “puntos matemáticos”, abstracciones muy elegantes, pero que nada tienen que ver con el cuerpo sólido real de una máquina voladora o un robot con extensión tridimensional. Entonces nos damos cuenta de que además de las coordenadas cartesianas, para conocer la posición exacta de un sólido y saber si entra en colisión con los obstáculos o no, debemos también saber su orientación. Y si ya además introducimos restricciones cinemáticas (p.ej. que un coche sólo puede moverse en la dirección que indiquen las ruedas directrices), y dinámicas (p.ej. un quadrotor volando a 1 m/s no puede hacer un quiebro de 90º) la cosa se pone aún más compleja… ¡piensa si no en la última vez que tuviste que aparcar tu coche en línea con el espacio justito!

En la entrada de hoy solamente hablaremos de las complicaciones causadas por la existencia de la orientación, o ángulos, del vehículo. Como ejemplo de maniobras teniendo en cuenta todo esto, la siguiente figura muestra el camino encontrado por una variación de RRT ((J.L. Blanco, M. Bellone, A. Gimenez-Fernandez, «TP-Space RRT: Kinematic path planning of non-holonomic any-shape vehicles», International Journal of Advanced Robotic Systems, vol. 12, no. 55, 2015. DOI: 10.5772/60463 )) para un laberinto y un vehículo con forma casi rectangular y restricciones reales (dirección tipo Ackermann):

 Respecto al algoritmo RRT, habrás notado que dijimos en su definición que escogíamos puntos “al azar”, lo que lo convierte en un algoritmo no determinista: cada vez que lo ejecutas te devuelve un nuevo camino que lleva hasta la solución, casi nunca dará el mismo y, lo más importante, casi nunca será el camino más óptimo en términos de tiempo o distancia recorrida.

Por tanto, recientemente se usa bastante más frecuentemente una variación conocida como RRT* (se lee “RRT estrella”). Propuesto en 2011 en ((S. Karaman and E. Frazzoli, Sampling-based Algorithms for Optimal Motion Planning, International Journal of Robotics Research, Vol 30, No 7, 2011. http://arxiv.org/abs/1105.1186)), en esta mejora de RRT también se escogen puntos al azar, pero se usan para ir refinando poco a poco las trayectorias que pasan cerca de dicha zona (quien esté interesado, aquí se puede encontrar un simulador que compara RRT con RRT*). Cuanto más tiempo se le deje para calcular, más corta será la trayectoria encontrada, demostrándose que se trata de un planificador asintóticamente óptimo. Es lo que se llama un algoritmo anytime.

 Para ilustrar la diferencia a nivel matemático (realmente, topológico) entre los problemas reales de planificación con vehículos y el clásico pasatiempo del laberinto, imaginemos el siguiente escenario: conducimos un vehículo que tiene forma de “L” por una carretera estrecha. Sí, una pieza de Tetris con ruedas. Las normas son que, en cada instante, sólo podemos elegir entre movernos hacia adelante (la derecha) o girar la pieza en sentido horario o antihorario, de forma que llevemos la pieza sana y salva desde “A” hasta “B” evitando las colisiones con los obstáculos cercanos.

Una posible solución sería la siguiente secuencia de movimientos:

Pues bien: la trayectoria a lo largo del eje horizontal (eje “x”) se puede modelar con una única coordenada \(x \in \mathbb{R}^1\), que pertenece al espacio Euclídeo de una dimensión, la recta. ¿Y la orientación? Pues se podría modelar con un ángulo \(\alpha \), que dado en grados iría desde -180º hasta +180º para cubrir todas las orientaciones posibles. Pero ojo: si llamamos orientación 0º a la inicial en la solución de la figura y vamos siguiendo sus giros, veríamos que la orientación final se corresponde con un valor de 360º, ya que ha girado en sentido antihorario exactamente esa cantidad de grados. Llegamos al problema topológico: 0º es igual que 360º. Luego aunque lo parezca a primera vista, no nos vale decir que la pieza está en las coordenadas \((x, \alpha)\), sin especificar la topología de cada una de esas coordenadas. En este caso, \((x, \alpha) \in \mathbb{R}^1 \times \mathbb{S}^1\), siendo \(\mathbb{S}^1\) la topología del círculo unitario, en el que los dos extremos (0º y 360º) se tocan y realmente son uno mismo.

Si piensas que esto es una tontería sin importancia, piensa en que para girar desde 5º hasta 355º hay que saber que el camino más corto pasa por la discontinuidad que ocurre en el 0º, por ejemplo. En el caso de los algoritmos RRT/RRT*, cuando se escogen puntos al azar y se debe encontrar el punto del árbol más cercano, debemos tener en cuenta no sólo la distancia en el espacio Euclídeo, sino la “distancia de giro” entre dos orientaciones, que deberá tratar de manera correcta esta singularidad en torno a la orientación 0º.

Aun así, con “chapuzas” a nivel de programación se podría sobrevivir decentemente en el mundo de los giros en el plano bidimensional. Pero si nos vamos al mundo 3D, la cosa se complica y si queremos ser rigurosos hay que echar mano de formalismos matemáticos.

Dentro de la planificación de trayectorias con RRT/RRT*, uno de los pasos más críticos y costosos computacionalmente es la búsqueda del punto más cercano (nearest neighbor) dentro del árbol a cada uno de los puntos elegidos aleatoriamente. En la planificación para puntos que se mueven en espacios planos (\(\mathbb{R}^2\), \(\mathbb{R}^3\)), una de las técnicas más usadas son los árboles KD (kd-trees), que ahora describiremos. Sin embargo, estos árboles no funcionan en  los espacios curvos que son las orientaciones 2D o 3D. Por tanto, primero explicaré de qué tipo de espacios curvos estamos hablando, luego veremos qué son los kd-trees y finalmente veremos qué propuestas recientes se han hecho para adaptarlos a búsquedas en espacios curvos.

Matemáticamente, un punto tridimensional x se puede ver como un elemento del grupo \(\mathbb{R}^3\), el espacio Euclídeo tridimensional:

\( {\bf{x}} = \left( \begin{array}{c} x\\ y\\ z \end{array} \right) \in \mathbb{R}^3 \)
Si multiplicamos este punto x, visto como un vector de 3×1, por la izquierda por una matriz R 3×3, obtendremos otro punto transformado x’:

\( {\mathbf{x}}’ = \left( {\begin{array}{*{20}{c}}
{{a_1}}&{{b_1}}&{{c_1}} \\
{{a_2}}&{{b_2}}&{{c_2}} \\
{{a_3}}&{{b_3}}&{{c_3}}
\end{array}} \right){\mathbf{x}} = {\mathbf{R}}\,{\mathbf{x}} \)

Podemos usar infinitas matrices R correspondientes a cualquier cambio de base algebraica que se pueda imaginar, pero sólo unas cuantas de esas matrices representarían posibles rotaciones “rígidas”. Se puede demostrar que para que un objeto se transforme punto por punto a través de una matriz R, y se mantengan las distancias y los ángulos, se debe cumplir que las columnas de R sean ortonormales y su determinante debe ser de +1.

En teoría de grupos, se llama grupo general lineal GL(3,\(\mathbb{R}\)) al conjunto de todas las posibles matrices cuadradas de tamaño 3×3, así que las rotaciones son sólo un subconjunto de éstas. Es tan importante, que hasta tiene un nombre propio: el grupo especial ortogonal, o SO(3), para el caso de puntos 3D. Aunque una matriz de SO(3) tiene 3×3=9 elementos, no podemos elegir arbitrariamente todos sus valores, ya que realmente sólo existen 3 grados de libertad en la orientación de cualquier cuerpo en 3D. Matemáticamente, se dice que SO(3) es una variedad de 3 grados de libertad, isomorfo al grupo de rotaciones del espacio Euclídeo tridimensional y embebido en el espacio superior de 9 dimensiones GL(3,\(\mathbb{R}\)). Una forma de visualizar las variedades es imaginando el ejemplo de la variedad 2-dimensional de la superficie de la Tierra, embebida en el espacio tridimensional.

Hasta ahora sólo hemos hablado de rotaciones, y no hemos mencionado aún las translaciones. Realmente, existe una notación que permite, añadiendo una fila extra con un “1” constante a las coordenadas de todos los puntos, representar la translación y la rotación en una sola multiplicación por una matriz de 4×4:

\(\left( {\begin{array}{*{20}{c}}
{{\mathbf{x}}’} \\
1
\end{array}} \right) = \left( {\begin{array}{*{20}{l}}
{\mathbf{R}}&\vline & {\begin{array}{*{20}{c}}
{{t_x}} \\
{{t_y}} \\
{{t_z}}
\end{array}} \\
\hline
0&\vline & 1
\end{array}} \right)\left( {\begin{array}{*{20}{c}}
{\mathbf{x}} \\
1
\end{array}} \right) = {\mathbf{R}}\,{\mathbf{x}} + {\mathbf{t}}\)

Esta multiplicación es lo que mejor saben hacer las tarjetas gráficas que nos permiten alucinantes gráficos en los videojuegos 3D de alta resolución en tiempo real. El conjunto de todas las matrices de 4×4 que representan translaciones y rotaciones válidas en el espacio 3D se denominan con el nombre de grupo especial Euclídeo SE(3). Topológicamente, es el producto semidirecto de los grupos SO(3) y \(\mathbb{R}^3\), y como variedad es un difeomorfismo de su producto \(\mathbf{SO}(3) \times \mathbb{R}^3\). Por cierto, tanto SO(3) como SE(3) son grupos de Lie, pero las aplicaciones prácticas de este hecho ya las dejaremos fuera de este artículo.

Como habrás observado, si queremos volar un quadrotor o mover un coche autónomo por carreteras reales, el problema consiste en encontrar caminos a través de un espacio cuya estructura topológica es la de SE(3), de ahí la importancia de dicho grupo, que hemos visto se puede descomponer en una parte Euclídea (la translación, en \(\mathbb{R}^3\)) y otra de un espacio curvo (la rotación, en SO(3)).

Veamos primero cómo encontrar los vecinos más cercanos a un punto dado para el caso puramente Euclídeo de \(\mathbb{R}^3\). En principio, si tenemos una serie de N puntos 3D, y nos preguntan por cuál es el más cercano a ciertas coordenadas, sería tan sencillo como calcular la distancia Euclídea de todos los N puntos hasta el punto de interés. Evidentemente, el problema con esta solución obvia de fuerza bruta se revela cuando el número de puntos N empieza a crecer, haciendo la búsqueda cada vez más lenta, con orden de complejidad O(N). Otro ejemplo muy conocido de este mismo problema se tiene al construir mapas de puntos 3D obtenidos con escáneres LIDAR y tratar de encajarlos unos con otros para reconstruir el escenario real.

Seguro que piensas que debe existir un método más eficiente de buscar puntos cercanos que la fuerza bruta, y efectivamente así es. Una de las optimizaciones más usadas es la partición del espacio en volúmenes de manera jerárquica, en forma de árbol, de manera que para realizar una búsqueda iríamos eligiendo en qué “trozo” del espacio estamos buscando, fijándonos en un volumen del espacio más reducido con cada elección. Dentro de estas técnicas, tenemos los k-d trees y los octrees (en que cada nodo se divide en las ocho partes que corresponden a cortes en los planos X, Y y Z).

 

Respecto a los árboles kd, son un tipo especial de árbol de búsqueda binario, y son muy usados en los problemas de que hablamos (y muchos otros, como en reconocimiento de patrones en visión artificial) por adaptar eficientemente las subdivisiones del espacio a los lugares donde realmente hay puntos de interés. Por cada nodo del árbol que se crea, se elige un plano que divide al “espacio padre” en dos mitades. En los árboles kd esta dirección suele coincidir con los ejes cartesianos (X,Y,Z). De este nodo surgirán dos nodos hijos, cada uno representando la mitad del volumen. Existen dos variaciones del algoritmo: en una el punto de corte se toma pasando por un punto existente y otra se toma el lugar geométrico de la media aritmética de todos los puntos. La siguiente figura muestra un ejemplo de cómo se construye un árbol kd para una serie de puntos.

 

Tanta complicación para ordenar un montón de puntos tiene su recompensa: buscar un vecino más cercano en un árbol kd suele conseguirse en tiempo logarítmico, O(log N), una mejora importantísima respecto al O(N) de la fuerza bruta. Por ejemplo, estamos hablando de pasar de hacer unas 1 000 o 10 000 comparaciones de fuerza bruta, a algo del orden de 3 o 4 sobre el árbol, respectivamente.

A pesar de sus ventajas, estos árboles tienen la limitación de asumir que el espacio de trabajo es Euclídeo, luego no se pueden aplicar directamente a los espacios curvos SO(3) ni SE(3), el espacio en que ocurren las planificaciones de trayectorias para vehículos y robots. Así llegamos a las últimas propuestas aparecidas en la literatura técnica para extender los árboles kd a la parte complicada de SE(3), es decir, a SO(3), y así poder optimizar la aplicación de RRT/RRT*.

La primera propuesta vino de mano de Yershova y LaValle (2007) ((Yershova, A., LaValle, S.M.: Improving motion-planning algorithms by efficient nearest-neighbor searching. IEEE Trans. Robotics 23(1) (2007) )). Su propuesta para usar árboles kd en SO(3) es relativamente simple. Para conseguirlo, parten de la representación de rotaciones mediante cuaterniones, una extensión de los números reales parecida a la de los números complejos pero añadiendo 3 nuevas “dimensiones” en lugar de sólo la dirección “imaginaria” i de los complejos. Al igual que multiplicar por números complejos se puede ver como una rotación en el plano, se puede demostrar (Hamilton) que una rotación de \(\theta \) radianes en el espacio tridimensional alrededor de un vector (vx,vy,vz) se pueden representar mediante un cuaternión:

\({\mathbf{q}} = \underbrace {\cos \frac{\theta }{2}}_{qr} + {\mathbf{i}}\,\,\underbrace {{v_x}\,\sin \frac{\theta }{2}}_{qx} + {\mathbf{j}}\,\underbrace {\,{v_y}\,\sin \frac{\theta }{2}}_{qy} + {\mathbf{k}}\underbrace {\,{v_k}\,\sin \frac{\theta }{2}}_{qz}\)

donde las cuatro componentes forman un vector unitario en un espacio de cuatro dimensiones, es decir, \(q_r^2 + q_x^2 + q_y^2 + q_z^2 = 1\). Topológicamente, el espacio de los posibles cuaterniones unitarios es isomorfo a SO(3) y forma una 3-esfera, es decir, una esfera de radio unidad en un espacio 4-dimensional Euclideo, con la peculiaridad de que los extremos opuestos de dicha esfera están en contacto unos con otros (puntos antipodales “identificados”), de igual manera que los extremos 0º y 360º de una circunferencia se identifican. Son estos “atajos” de punta a punta de la esfera lo que obliga a tener cuidado al medir distancias. La propuesta de Yershova y LaValle (2007) es sencilla: construir los árboles kd como si los puntos estuvieran realmente en un espacio Euclídeo, y sólo tener en cuenta la curvatura a la hora de realizar búsquedas, mediante el uso de la métrica adecuada para medir distancias. De esa forma, la distancia geodésica (sobre la variedad) d_M(p,q) entre una orientación p y otra q, se toma como el mínimo de las dos distancias sobre la variedad (digamos, “sobre su” superficie) desde q hasta p y hasta –p, que recordemos que representa la misma orientación. Es más, si sólo nos interesa determinar el punto más cercano, no hará falta calcular la distancia geodésica real, nos basta con minimizar la distancia Euclídea (“atravesando” la superficie de la variedad), como se ilustra en la siguiente figura que habría que entender como un corte bidimensional de la 3-esfera en el plano que pasa por su centro en las otras dos dimensiones:

 

Mediante este sencillo “truco”, los autores demostraron que un árbol kd era más eficiente que otras técnicas de búsqueda genéricas basadas en la definición por el usuario de funciones de distancia métrica a modo de “caja negra”, como son los cover trees ((A. Beygelzimer, S. Kakade, and J. Langford. Cover trees for nearest neighbor. 2004)) o el sb(S) ((K. L. Clarkson. Nearest neighbor searching in metric spaces: Experimental results for sb(s). 2003.)). De esta forma, un ordenador puede resolver puzles como el de la siguiente figura en un tiempo razonable. Sí, ese es el estado de la Inteligencia Artificial (en 2006), así que puedes estar tranquilo: los robots aún están lejos de ser más “hábiles” que un humano torpe… 😉

Más recientemente, en 2014, se ha propuesto una nueva mejora por parte de Ichnowski y Alterovitz en ((Ichnowski, Jeffrey, and Ron Alterovitz. «Fast Nearest Neighbor Search in SE (3) for Sampling-Based Motion Planning.» Algorithmic Foundations of Robotics XI. Springer International Publishing, 2015. 197-214)). Los autores proponen una nueva manera de particionar el espacio SO(3) en 4 “volúmenes”, creando un árbol kd para cada una de ellas. Las 4 caras de la 3-esfera corresponden a la proyección sobre ella de un cubo unidad en 4 dimensiones (teseracto). Como es bastante duro imaginárselo, es conveniente visualizarlo en un símil de menor dimensión: imaginemos un cubo normal (en 3D) proyectado sobre la superficie de una esfera tridimensional de las de toda la vida. La idea es construir un árbol con los puntos de la esfera en cada uno de las 6 “caras” de la esfera, con la particularidad de que en la 3-esfera de los cuaterniones, realmente las antípodas se identifican, siendo suficiente con mapear la mitad de caras: 3 en el caso del cubo en \(\mathbb{R}^3\), 4 en el del teseracto en \(\mathbb{R}^4\).

Además, los hiperplanos que dividen los volúmenes de cada nodo del árbol kd se toman “inclinados”, pasando por el origen en lugar de ser paralelos a los 4 ejes de coordenadas Euclídeas de los cuaterniones como se proponía en Yershova y LaValle (2007) ((Yershova, A., LaValle, S.M.: Improving motion-planning algorithms by efficient nearest-neighbor searching. IEEE Trans. Robotics 23(1) (2007) )). El resultado es una mejora bastante importante en los tiempos de cómputo para problemas de planificación reales, como se ve en la siguiente figura.

Espero que con esta introducción el lector pueda hacerse una idea del tipo de matemáticas y algoritmos que están detrás de los robots en desarrollo hoy día. Como premio por haber llegado hasta el final, dejo este vídeo espectacular de planificación de trayectorias con CC-RRT ((Luders, B., Kothari, M., & How, J. P. (2010, August). Chance constrained RRT for probabilistic robustness to environmental uncertainty. In AIAA guidance, navigation, and control conference (GNC), Toronto, Canada.)) para un robot terrestre por parte del equipo Aerospace Controls Lab (ACL) del MIT: