Generacion-de-mapa-de-entorno-para-navegacion-de

UNIVERSIDAD DE CHILE
FACULTAD DE CIENCIAS FÍSICAS Y MATEMÁTICAS
DEPARTAMENTO DE INGENIERÍA ELÉCTRICA
GENERACIÓN DE MAPA DE ENTORNO PARA
NAVEGACIÓN DE VEHÍCULO TERRESTRE
AUTÓNOMO
TESIS PARA OPTAR AL GRADO DE DOCTOR EN INGENIERÍA ELÉCTRICA
SEBASTIÁN ISAO PARRA TSUNEKAWA
PROFESOR GUÍA:
JAVIER RUIZ DEL SOLAR SAN MARTÍN
MIEMBROS DE LA COMISIÓN:
MARCOS ORCHARD CONCHA
MIGUEL TORRES TORRITI
PEDRO NÚÑEZ TRUJILLO
SANTIAGO DE CHILE
2014
RESUMEN DE LA TESIS
PARA OPTAR AL GRADO DE
DOCTOR EN INGENIERÍA ELÉCTRICA
POR: SEBASTIÁN ISAO PARRA TSUNEKAWA.
FECHA:
PROF. GUÍA: SR. JAVIER RUIZ DEL SOLAR
GENERACIÓN DE MAPA DE ENTORNO PARA NAVEGACIÓN DE VEHÍCULO
TERRESTRE AUTÓNOMO
En la actualidad los sistemas de navegación autónoma de vehı́culos terrestres ya son una realidad.
Estos sistemas son capaces de navegar de manera autónoma por caminos tanto estructurados como
no estructurados. Para la adecuada interacción de estos sistemas autónomos con su entorno, la
información perceptual debe ser integrada en un mapa para su posterior utilización. Esta integración
puede tener distintos enfoques y representaciones dependiendo del origen de la información y del
uso dado.
Los sistemas autónomos de navegación han tenido éxito desde el punto de vista técnico gracias,
entre otras cosas, a los sensores empleados; los cuales frecuentemente incluyen sensores LIDAR
tridimensionales. Sin embargo, el uso de estos mismos sensores impiden su masificación debido a
su alto costo. Este costo elevado se debe a que la gran mayorı́a de ellos son dispositivos de uso
industrial y no tienen aplicaciones automotrices o de consumo masivo.
Dentro de este ámbito, el Centro Avanzado de Tecnologı́a para la Minerı́a (AMTC) está desarrollando un vehı́culo autónomo terrestre para ser utilizado como plataforma de desarrollo para
tecnologı́a en la industria minera. Se busca generar un sistema que se pueda portar a la industria.
Con este fin, este proyecto plantea el uso de un número limitado de sensores y unidades de computo.
Este trabajo de tesis se enmarca dentro del proyecto de vehı́culo autónomo del AMTC.
Esta tesis propone que un conjunto reducido de sensores de rango bidimensionales es suficiente
para la generación de mapas de entorno en tiempo real, que puedan ser utilizados para la navegación
autónoma de un vehı́culo terrestre. Se postula que las variaciones en la pose de los sensores de rango
utilizados para construir los mapas de entorno, causadas por las vibraciones del vehı́culo debido a
su naturaleza no-rı́gida y a las irregularidades del terreno, pueden ser estimadas y compensadas
utilizando una unidad inercial y un sistema de estimación del estado del vehı́culo basado en el
uso de un Filtro de Kalman Extendido. Este estimador de estado puede enriquecer su estimación
mediante el uso de sensores internos del vehı́culo y de estimaciones de la morfologı́a del terreno
realizadas en instantes anteriores. Se postula asimismo que la fusión temporal de las mediciones de
los sensores de rango, compensadas por el sistema de estimación de pose, puede ser implementada
usando Filtros de Kalman por celda.
También es propuesta una metodologı́a de evaluación que asume la carencia de un ground truth
contra el cual comparar. Debido a lo ligado de la estimación del vehı́culo con el mapa de entorno,
una observación LIDAR inmediata al vehı́culo es comparada con la morfologı́a acumulada en el
mapa permitiendo evaluar la estimación del trayecto recorrido y la integración de las observaciones
simultáneamente. El sistema propuesto es evaluado en caminos no pavimentados a diferentes velocidades, mostrando un mejor desempeño tanto cuantitativo como cualitativo al ser comparado con
el caso en que no se usa el estimador de estado propuesto.
Además es analizado el desempeño del estimador de estado y sus variantes a nivel de trayectorias,
comparándolo con las trayectorias de las observaciones GPS.
Para todo lo anterior se implementa sobre una arquitectura de software versátil y adaptable a
diferentes fuentes de información.
Agradecimientos
A mi familia por su apoyo constante e incondicional, que hizo posible llegar a estas
instancias. Sin ellos esto no hubiese sido posible. A Carmen por acompañarme desde la parte
final de este proceso, alegrándome cada dı́a y dándome ánimos para seguir adelante. A Tera
y Atto por acompañarme y alegrarme en los momentos más difı́ciles.
Al profesor Javier Ruiz del Solar por guiarme en esta tesis y todo su apoyo durante este
tiempo. A los profesores Marcos Orchard, Miguel Torres y Pedro Núñez por sus correcciones
que permitieron terminar este documento.
A Rodrigo Asenjo por todos estos años llenos discusiones sobre temas tecnológicos, y en
especial por sus constructivas correcciones sobre el idioma español. A Paul Vallejos por sus
constructivas conversaciones que siempre deben llegar a un veredicto final, y ası́ aprender
cosas nuevas. A Eliana Monardes y Renée Kellinghusen por toda la ayuda y preocupación
durante todos estos años, en especial a sus constantes recordatorios.
A Rafael Rodrı́guez como amigo, compañero de carrera y doctorado, con quien nunca una
conversación es considerada demasiado nerd. A Pepe Inostroza y Patricio Escobedo por sus
conversaciones completamente no relacionadas con la Universidad.
A Mauricio Correa, Mauricio Mascaró, Fernando Bernuy y Daniel Herrmann que también
estuvieron involucrados en partes del proyecto del vehı́culo, y también como miembros de los
laboratorios de Robótica y de Robótica de Campo.
Agradecido de haber podido participar por tantos años de los ramos SD20A y EI2001,
gracias a sus alumnos logré cultivar la paciencia necesaria para la vida y me permitieron
recobrar la capacidad de asombro.
A muchas otras personas que también influyeron en este trabajo pero no alcanzan a ser
nombradas.
A la administración de la Laguna Carén y del Parque O’Higgins por permitir realizar
pruebas del vehı́culo en sus instalaciones.
Finalmente, agradezco a CONICYT que a través de su programa Becas para Estudios de
Doctorado en Chile y al Centro Avanzado de Tecnologı́a para la Minerı́a (Proyecto CONICYT
FB009) por el financiamiento para la realización de esta tesis. En especial al AMTC que
gracias a su infraestructura fue posible realizar este trabajo.
Tabla de contenido
1. Introducción
1.1. Antecedentes . . . . . . . . . . . . . . .
1.1.1. Fundamentación general . . . . .
1.1.2. Definición del problema a abordar
1.2. Objetivos . . . . . . . . . . . . . . . . .
1.3. Hipótesis . . . . . . . . . . . . . . . . . .
1.4. Metodologı́a . . . . . . . . . . . . . . . .
1.5. Aportes del trabajo de tesis . . . . . . .
1.6. Estructura de este documento . . . . . .
.
.
.
.
.
.
.
.
1
2
2
2
3
3
3
4
4
2. Revisión bibliográfica
2.1. Vehı́culos autónomos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.2. Software en Robótica . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.3. Sistemas de estimación de mapas de entorno . . . . . . . . . . . . . . . . . .
5
6
8
9
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
3. Marco teórico
3.1. Arquitecturas de software y modalidades de comunicaciones
3.1.1. Medios de comunicación entre procesos . . . . . . . .
3.1.2. Arquitecturas de IPC . . . . . . . . . . . . . . . . . .
3.2. Middlewares . . . . . . . . . . . . . . . . . . . . . . . . . . .
3.3. ROS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3.4. Mapas para la robótica . . . . . . . . . . . . . . . . . . . . .
3.4.1. Tipos de mapas . . . . . . . . . . . . . . . . . . . . .
3.5. Principio de funcionamiento de un LIDAR . . . . . . . . . .
4. Vehı́culo Autónomo AMTC
4.1. Vehı́culo . . . . . . . . . . . . . . . . . . . . . . .
4.1.1. Descripción de modificaciones del vehı́culo
4.2. Hardware y Software . . . . . . . . . . . . . . . .
4.2.1. Computador . . . . . . . . . . . . . . . . .
4.2.2. Sensores . . . . . . . . . . . . . . . . . . .
iii
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
12
13
14
16
18
19
20
21
22
.
.
.
.
.
25
26
27
30
30
31
4.3. Discusión . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5. Metodologı́a de mapeo del terreno en tiempo
5.1. Observaciones . . . . . . . . . . . . . . . . . .
5.1.1. Pipeline de las observaciones . . . . . .
5.1.2. Extracción de caracterı́sticas . . . . . .
5.2. Modelo cinemático del vehı́culo . . . . . . . .
5.3. Estimación del estado del vehı́culo . . . . . . .
5.3.1. Estimación directa . . . . . . . . . . .
5.3.2. Estimación mediante EKF . . . . . . .
5.4. Proyección espacial de observaciones . . . . .
5.5. Fusión de Observaciones en mapa del terreno .
5.5.1. Nubes de puntos . . . . . . . . . . . .
5.5.2. Mapas de grilla . . . . . . . . . . . . .
6. Resultados
6.1. Datos de evaluación . . . . . . . . . . . . .
6.2. Evaluación de métodos SLAM . . . . . . .
6.3. Trayectorias . . . . . . . . . . . . . . . . .
6.3.1. Trayectoria GPS . . . . . . . . . .
6.3.2. Trayectoria de Odometrı́a . . . . .
6.3.3. Trayectoria de Estimación Directa .
6.3.4. Trayectoria de Estimación EKF . .
6.4. Estado y Superficie . . . . . . . . . . . . .
6.4.1. Configuración del montaje . . . . .
6.4.2. RMSE como métrica de evaluación
6.4.3. Experimentos . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
real
. . .
. . .
. . .
. . .
. . .
. . .
. . .
. . .
. . .
. . .
. . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
7. Conclusiones
7.1. Comunicaciones y Middleware . . . . . . . . . . .
7.2. Caracterı́sticas de los sensores . . . . . . . . . . .
7.3. Diseño e implementación del estimador de estado
7.4. Mapa de entorno . . . . . . . . . . . . . . . . . .
7.5. Aportes del trabajo de tesis . . . . . . . . . . . .
7.6. Trabajo futuro . . . . . . . . . . . . . . . . . . . .
Bibliografı́a
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
35
.
.
.
.
.
.
.
.
.
.
.
36
37
37
39
43
46
46
48
52
53
54
54
.
.
.
.
.
.
.
.
.
.
.
57
58
60
62
62
62
70
77
87
87
87
88
.
.
.
.
.
.
95
96
96
96
97
97
98
99
A. Cálculo de la esperanza del mı́nimo RMSE
104
B. ROSBAGs
106
iv
C. Paper
116
v
Lista de figuras
2.1.
2.2.
2.3.
2.4.
2.5.
Sandstorm de Carnegie Mellon University .
Stanley de Stanford Racing Team . . . . . .
Boss de Carnegie Mellon University . . . . .
Google driverless car . . . . . . . . . . . . .
Diagrama de flujo del sistema de software de
. . . . .
. . . . .
. . . . .
. . . . .
Stanley
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
6
6
7
7
8
3.1.
3.2.
3.3.
3.4.
Arquitectura de servidor central . . . . .
Arquitectura peer-to-peer . . . . . . . . .
Arquitectura Broadcast . . . . . . . . . .
Descripción de ecos en una emisión láser
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
17
17
18
23
4.1.
4.2.
4.3.
4.4.
Vehı́culo Autónomo del AMTC . . . . . . . . . .
Intervención en el maletero . . . . . . . . . . . . .
Intervención en la ventana del maletero . . . . . .
Botón de emergencia montado en el parachoques .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
26
28
29
29
5.1. Flujo de datos de lo observaciones . . . . . . . . . . . . . . . . . . .
5.2. Ejemplo de calibración una cámara mediante un chessboard . . . . .
5.3. Visualización gráfica de la condición de Ackermann . . . . . . . . .
5.4. Modelo equivalente de bicicleta . . . . . . . . . . . . . . . . . . . .
5.5. Datos experimentales para la estimación de γ . . . . . . . . . . . .
5.6. Regresión y linealización para cálculo de γ . . . . . . . . . . . . . .
5.7. Aproximación de trayectoria en caso del estado directo . . . . . . .
5.8. Aproximación de trayectoria por arcos de cı́rculos . . . . . . . . . .
5.9. Estimación de Pitch y Roll proveniente de las alturas de las ruedas
5.10. Ejemplo de nube de puntos de sensor láser . . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
39
40
43
44
45
46
47
50
53
54
6.1.
6.2.
6.3.
6.4.
. . . . .
. . . . .
. . . . .
caminos
. . . . .
59
60
60
.
.
.
.
.
.
.
.
.
.
.
.
Ejemplo de observaciones en plano XY de los sensores láser . . . .
Recorrido de set de datos sobre camino de tierra . . . . . . . . . . .
Ejemplo de imágenes del recorrido del camino de tierra . . . . . . .
Histograma de velocidades por porcentaje de distancia recorrida en
de tierra . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
vi
61
6.5.
6.6.
6.7.
6.8.
6.9.
Comparativa de la trayectoria GPS y Odometrı́a en grabación 7 . . . . . . .
Comparativa de la trayectoria GPS y Odometrı́a en grabación 8 . . . . . . .
Comparativa de la trayectoria GPS y Odometrı́a en grabación 9 . . . . . . .
Comparativa de la trayectoria GPS y Odometrı́a en grabación 5 . . . . . . .
Diferencia de posición y rumbo entre trayectorias GPS y Odometrı́a en grabación 7. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6.10. Diferencia de posición y rumbo entre trayectorias GPS y Odometrı́a en grabación 8. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6.11. Diferencia de posición y rumbo entre trayectorias GPS y Odometrı́a en grabación 9. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6.12. Diferencia de posición y rumbo entre trayectorias GPS y Odometrı́a en grabación 5. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6.13. Comparativa de la trayectoria GPS y Odometrı́a en grabación 1 . . . . . . .
6.14. Comparativa de la trayectoria GPS y Estimación Directa en grabación 7 . .
6.15. Comparativa de la trayectoria GPS y Estimación Directa en grabación 8 . .
6.16. Comparativa de la trayectoria GPS y Estimación Directa en grabación 9 . .
6.17. Comparativa de la trayectoria GPS y Estimación Directa en grabación 5 . .
6.18. Diferencia de posición y rumbo entre trayectorias GPS y Estimación Directa
en grabación 7. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6.19. Diferencia de posición y rumbo entre trayectorias GPS y Estimación Directa
en grabación 8. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6.20. Diferencia de posición y rumbo entre trayectorias GPS y Estimación Directa
en grabación 9. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6.21. Diferencia de posición y rumbo entre trayectorias GPS y Estimación Directa
en grabación 5. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6.22. Comparativa de la trayectoria GPS y Estimación EKF con variación de parámetro γ en grabación 7 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6.23. Comparativa de la trayectoria GPS y Estimación EKF con variación de parámetro γ en grabación 8 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6.24. Comparativa de la trayectoria GPS y Estimación EKF con variación de parámetro γ en grabación 9 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6.25. Comparativa de la trayectoria GPS y Estimación EKF con variación de parámetro γ en grabación 5 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6.26. Detalle del punto final de la comparativa de la trayectoria GPS y Estimación
EKF con variación de parámetro γ en grabación 7 . . . . . . . . . . . . . . .
6.27. Detalle del punto final de la comparativa de la trayectoria GPS y Estimación
EKF con variación de parámetro γ en grabación 8 . . . . . . . . . . . . . . .
6.28. Detalle del punto final de la comparativa de la trayectoria GPS y Estimación
EKF con variación de parámetro γ en grabación 9 . . . . . . . . . . . . . . .
vii
63
64
65
66
66
67
67
68
69
71
72
73
74
74
75
75
76
78
79
80
81
82
83
84
6.29. Detalle del punto final de la comparativa de la trayectoria GPS y Estimación
EKF con variación de parámetro γ en grabación 5 . . . . . . . . . . . . . . .
6.30. Diferencia de posición y rumbo entre trayectorias GPS y Estimación EKF con
variación de parámetro γ en grabación 7. . . . . . . . . . . . . . . . . . . . .
6.31. Diferencia de posición y rumbo entre trayectorias GPS y Estimación EKF con
variación de parámetro γ en grabación 8. . . . . . . . . . . . . . . . . . . . .
6.32. Diferencia de posición y rumbo entre trayectorias GPS y Estimación EKF con
variación de parámetro γ en grabación 9. . . . . . . . . . . . . . . . . . . . .
6.33. Diferencia de posición y rumbo entre trayectorias GPS y Estimación EKF con
variación de parámetro γ en grabación 5. . . . . . . . . . . . . . . . . . . . .
6.34. Comparación de la modificación de la descripción del robot durante las tomas
de datos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6.35. Diagrama del cálculo del RMSE . . . . . . . . . . . . . . . . . . . . . . . . .
6.36. Histograma de rapidez del vehı́culo en grabaciones 1-2 . . . . . . . . . . . . .
6.37. Medida ROC de grabaciones 1-2 . . . . . . . . . . . . . . . . . . . . . . . . .
6.38. Histograma de rapidez del vehı́culo en grabaciones 3-6 . . . . . . . . . . . . .
6.39. Medida ROC de grabaciones 3-6 . . . . . . . . . . . . . . . . . . . . . . . . .
6.40. Diagrama de secciones del RMSE . . . . . . . . . . . . . . . . . . . . . . . .
6.41. Gráfico RMSE por secciones y RMSE mı́nimo esperable . . . . . . . . . . . .
6.42. Imagen frontal y vista superior mapa de elevación . . . . . . . . . . . . . . .
6.43. Imagen frontal y vista superior mapa de elevación con coloración RGB . . .
6.44. Imagen frontal y vista superior mapa de elevación con coloración RGB de una
superficie no observada . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
viii
84
85
85
86
86
87
88
90
90
90
91
92
92
93
93
94
Lista de tablas
3.1. Valores de reflectividad
4.1.
4.2.
4.3.
4.4.
4.5.
4.6.
4.7.
4.8.
. . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Especificaciones fı́sica del vehı́culo . . . . . . . . . .
Especificaciones técnicas del computador ARK3440
Especificaciones técnicas SICK LMS291-S05 . . . .
Especificaciones técnicas SICK LMS151-10100 . . .
Especificaciones técnicas SICK LD-MRS HD . . . .
Especificaciones técnicas Crossbow NAV440 . . . .
Especificaciones técnicas Delphi ESR . . . . . . . .
Comparativa entre cámaras . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
27
31
31
32
32
33
34
34
6.1. Caracterı́sticas de grabaciones en camino de tierra y en pavimento . . . . . .
6.2. Resultados de medidas de grabaciones 1-2 . . . . . . . . . . . . . . . . . . .
6.3. Resultados de medidas de grabaciones 3-6 . . . . . . . . . . . . . . . . . . .
60
89
91
ix
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
24
Capı́tulo 1
Introducción
Índice
1.1. Antecedentes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2
1.1.1. Fundamentación general . . . . . . . . . . . . . . . . . . . . . . . .
2
1.1.2. Definición del problema a abordar . . . . . . . . . . . . . . . . . .
2
1.2. Objetivos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3
1.3. Hipótesis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3
1.4. Metodologı́a . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3
1.5. Aportes del trabajo de tesis . . . . . . . . . . . . . . . . . . . . . .
4
1.6. Estructura de este documento . . . . . . . . . . . . . . . . . . . . .
4
1.1.
1.1.1.
Antecedentes
Fundamentación general
En la actualidad, los sistemas de navegación para vehı́culos terrestres son una realidad
[43] [47] [5] [54]. Estos sistemas son capaces de navegar de manera autónoma caminos tanto
estructurados como no estructurados. Estos avances han permitido que la investigación de
sistemas autónomos terrestres obtenga relevancia y estos se han vuelvo conocidos por la
sociedad.
Los sistemas autónomos de navegación actuales han tenido éxito desde el punto de vista
técnico gracias entre otras cosas a los sensores empleados. Pero la gran mayorı́a de los sistemas
autónomos tienen una dependencia alta del sistema de localización GPS. Esto acota en gran
medida el desempeño en zonas donde la calidad del servicio es baja o simplemente no esta
disponible. Además estos sensores utilizados impiden su masificación debido a su alto costo.
Este costo elevado viene de que la gran mayorı́a de ellos son dispositivos industriales y no
de enfoque automotriz o de consumo masivo. Mientras la fabricación de estos dispositivos
no se masifique y no bajen sus precios, es necesario explorar otros enfoques de navegación
autónoma.
Este trabajo tesis plantea el diseño e implementación de un sistema de percepción para
ser usado en un vehı́culo autónomo terrestre. Este vehı́culo es parte de un proyecto de investigación y desarrollo del Centro Avanzado de Tecnologı́a para la Minerı́a (AMTC) de la
Universidad de Chile. El proyecto vehı́culo autónomo AMTC busca la implementación de
un vehı́culo autónomo terrestre como plataforma de desarrollo de tecnologı́as aplicables a la
industria minera. Como se busca desarrollar un vehı́culo acorde a los ambientes presentes en
la industria minera, una de las restricciones planteadas es la no dependencia del un sistema
GPS. Otra restricción planteada es el uso de recursos de computo y sensoriales limitados, de
manera de generar un sistema utilizable en la industria.
1.1.2.
Definición del problema a abordar
En los actuales sistemas de navegación de vehı́culos terrestres autónomos, la información
perceptual utilizada en estos suele cubrir todo el entorno del vehı́culo a cada momento. Para
esto, un gran número de sensores, o bien sensores de elevado costo, son requeridos, ası́ como
una gran capacidad de cómputo para el elevado nivel de información. Esto eleva el costo de
un sistema autónomo de manera considerable y no permite la comercialización de proyectos
como el vehı́culo de Google [54]. Mientras no se desarrollen sistemas autónomos de menor
costo estos no podrán masificarse.w
Una manera de disminuir el costo de un sistema autónomo es la utilización de un número
reducido de sensores y aprovechar al máximo la información entregada de estos. El problema
que se aborda en el trabajo de tesis es proponer un sistema de percepción y de fusión capaz
de generar una representación del entorno que permita la navegación de un vehı́culo terrestre,
teniendo en consideración un número reducido de sensores y campo perceptual acotado.
2
1.2.
Objetivos
El objetivo general de esta tesis es la generación de un mapa de entorno local que le permita a un vehı́culo terrestre navegar de manera segura. Se propone para ello un estimador de
estado basado en un filtro de Kalman extendido, que en conjunto con una estructura de mapa
de grilla que fusiona observaciones usando un enfoque de filtrado tipo Kalman unidimensional generan un mapa de entorno local utilizable para planificación de trayectorias. Debido a
la necesidad de funcionamiento en tiempo real, la complejidad del sistema debe mantenerse
acotada. Debido al reducido número de sensores utilizados, las fuentes de información perceptual son analizadas para extraer la máxima información aprovechable, transformándolas
en información útil. Para ello se propone un esquema en que la información sensorial es “mejorada” al extraer diversas caracterı́sticas que son asociadas a su posición espacial, que en
una etapa posterior podrı́a no ser extraı́ble directamente. Todo esto en el contexto de una
arquitectura de software que sea versátil y adaptable a diversas configuraciones.
1.3.
Hipótesis
Como hipótesis general de este trabajo se postula que un conjunto reducido de sensores
de rango bidimensionales es suficiente para la generación de mapas de entorno en tiempo
real, que puedan ser utilizados para la navegación autónoma de un vehı́culo terrestre.
Como hipótesis especı́fica se postula que las variaciones en la pose de los sensores de rango
utilizados para construir los mapas de entorno, causadas por las vibraciones del vehı́culo
debido a su naturaleza no-rı́gida y a las irregularidades del terreno, pueden ser estimadas
y compensadas utilizando una unidad inercial y un sistema de estimación del estado del
vehı́culo basado en el uso de un Filtro de Kalman Extendido. Este estimador de estado puede
enriquecer su estimación mediante el uso de sensores internos del vehı́culo y de estimaciones
de la morfologı́a del terreno realizadas en instantes anteriores. Se postula asimismo que la
fusión temporal de las mediciones de los sensores de rango, compensadas por el sistema de
estimación de pose, puede ser implementada usando Filtros de Kalman por celda.
1.4.
Metodologı́a
Se propone la siguiente metodologı́a de trabajo para la generación de un mapa de entorno
para navegación de un vehı́culo terrestre:
1. Analizar las caracterı́sticas de los sensores disponibles y definir su utilización.
2. Determinar los requerimientos de comunicación entre los procesos y definir el uso de
middleware.
3. Implementación y puesta en marcha de los sistemas de adquisición de sensores.
4. Diseño e implementación de un estimador del estado del vehı́culo, que permita realizar
la integración de las observaciones en el tiempo.
5. Diseñar e implementar un mapa de entorno apto para la integración de las observaciones
de los sensores de distancia.
3
El resultado de esta metodologı́a llevará a la generación en tiempo real de un mapa
de entorno apto para navegación de vehı́culos dentro las restricciones planteadas. Además
este enfoque es replicable y escalable a los recursos disponibles en otras aplicaciones, como
tipo de plataforma móvil, conjunto de sensores disponibles y recursos de computacionales
disponibles.
1.5.
Aportes del trabajo de tesis
Se diseña la estructura de un sistema perceptual que posee un enfoque escalable en número
de sensores y que permite el cómputo distribuido, que es una caracterı́stica muy deseable en
un sistema de tiempo real para navegación de vehı́culos.
Para la estimación del estado del vehı́culo, la metodologı́a propuesta asume una pose no
rı́gida para el sensor ası́ como del mecanismo de suspensión del vehı́culo. Por lo tanto el ruido
asociado a la vibración del vehı́culo y a la pose del sensor es considerada explı́citamente. El
nivel de vibraciones es estimado en tiempo real usando mediciones del sensor inercial. Un filtro
extendido de Kalman es utilizado para la estimación del estado. El estado es relevante en una
vecindad al vehı́culo y no de manera global. Esto difiere con otras aplicaciones tradicionales
en las cuales existe una alta disponibilidad de sistema GPS.
Se diseña un sistema de estimación del entorno donde el ruido asociado a las vibraciones
son consideradas al momento de integrar las observaciones al mapa. Esto permite fusionar
observaciones provenientes de distintos sensores en diferentes instantes de tiempo, diferenciándose del enfoque tradicional donde solo se fusiona basado en las caracterı́sticas del sensor.
Esto también es utilizado en la estimación de la pose del vehı́culo.
1.6.
Estructura de este documento
La tesis está estructurada de la siguiente manera: En el Capı́tulo 2 se presenta una revisión
bibliográfica sobre vehı́culos autónomos, software en proyectos de robótica y sistemas similares al propuesto por esta tesis. El Capı́tulo 3 describe el marco teórico sobre el cual se basa
la tesis. El Capı́tulo 4 describe al vehı́culo autónomo del AMTC, ası́ como las modificaciones
realizadas y dispositivos instalados. El Capı́tulo 5 presenta la metodologı́a propuesta para la
generación de un mapa de entorno. El Capı́tulo 6 presenta las pruebas y evaluación realizadas
a los sistemas propuestos en el Capı́tulo 5. Finalmente el Capı́tulo 7 presenta conclusiones,
discusiones y el trabajo futuro.
4
Capı́tulo 2
Revisión bibliográfica
Índice
2.1. Vehı́culos autónomos . . . . . . . . . . . . . . . . . . . . . . . . . .
6
2.2. Software en Robótica . . . . . . . . . . . . . . . . . . . . . . . . . .
8
2.3. Sistemas de estimación de mapas de entorno . . . . . . . . . . . .
9
2.1.
Vehı́culos autónomos
Figura 2.1: Vehı́culo con la participación en DARPA Grand Challenge 2004, Sandstorm de
Carnegie Mellon University, 2004
Desde los inicios de la robótica, los robots móviles terrestres han estado presentes por ser
una de las formas más naturales y simples de desplazamiento. Estos pueden usar variados
principios para desplazarse por el terreno, siendo las ruedas uno de los medios más comunes. Tradicionalmente estos robots terrestres son máquinas creadas especı́ficamente para ser
robots y no para otros fines.
Históricamente, se ha tratado de convertir máquinas de manufactura masiva en robots.
Para el caso de los automóviles, el primer intento se remontan al año 1925 con el primer
automóvil tele-operado [44]. Otro de los intentos más conocidos, y uno de los proyectos
más grandes corresponde al proyecto Eureka Prometheus a [55] en el año 1987 que buscaba
implementar vehı́culos autónomos, logrando en 1995 un viaje de 1600[Km] por la Autobahn
alemana.
Figura 2.2: Ganador DARPA Grand Challenge,Stanley de Stanford Racing Team, 2005
Un gran auge en el interés de este tipo de robots se observa desde la competencia impulsada por DARPAb en el año 2004. Esta competencia, DARPA Grand Challenge [7], buscaba
que los participantes siguiesen una ruta de 240[Km] por el desierto de Mojave en Estados Unidos de manera autónoma por caminos de tierra. En esa ocasión ningún concursante
a PROgraMme
b Defense
for a European Traffic of Highest Efficiency and Unprecedented Safety, 1987-1995
Advanced Research Projects Agency
6
logró llegar a la meta, siendo Sandstorm de Carnegie Mellon University [48](ver Figura 2.1),
el vehı́culo que logró recorrer la mayor distancia, completando 11,78[km] del recorrido(4,9 %
del recorrido total) antes de chocar contra una roca. Al año siguiente, el DARPA Grand
Challenge [4] se llevó acabo nuevamente y en esta ocasión cinco equipos completaron el recorrido de 228[Km], siendo Stanley [43] de Stanford Racing Team (ver Figura 2.2) el ganador
completando la ruta en 6 horas 53 minutos 58 segundos. En el año 2007, la competencia
cambió de enfoque, buscando un reto mayor, y pasó a llamarse DARPA Urban Challenge [5].
Esta nueva competencia buscaba la interacción directa entre los competidores en un ambiente
urbano en el que tendrı́an que obedecer las reglas de tránsito en un recorrido de 96[Km],
siendo el ganador el vehı́culo Boss de Carnegie Mellon University [47](ver Figura 2.3).
Figura 2.3: Ganador del DARPA Urban Challenge,Boss de Carnegie Mellon University, 2007
Después de estas competencias, muchas iniciativas han surgido buscando solucionar el
problema de automóviles autónomos que no requieran de infraestructura especial y que puedan interactuar con vehı́culos no autónomos entre las cuales se destaca la iniciativa Google
Chauffeur de GoogleX, que es el sistema de software de navegación de sus Google driverless
cars [54] (ver Figura 2.4). Estos son vehı́culos estándares con equipamiento instalado avaluado en U SD150, 000, donde el mas visible corresponde a un sensor Velodyne HDL-64E [49]
montado en el techo del vehı́culo. En el equipo de trabajo de este proyecto se encuentran
Sebastian Thurn y Christopher Urmson, lı́deres de los equipos de Stanford y Carnegie Mellon
University respectivamente.
Figura 2.4: Google driverless car [54]
Una de las principales dificultades actuales para la masificación de vehı́culos autónomos es
el costo del sistema sensorial y computacional, los vehı́culos que participaron en las iniciativas
7
DARPA tenı́an centenas de miles de dólares en equipo, volviéndolo no comercializable. Para
los próximos años, los fabricantes de vehı́culos proyectan ofrecer comercialmente automóviles
autónomos con las certificaciones que les permitan funcionar libremente. Un asunto pendiente
es el cambio en las leyes que cada paı́s deberá realizar para permitir su uso.
2.2.
Software en Robótica
En un comienzo, antes del siglo XVII, los robots eran verdaderas “artesanı́as”, muy complejas desde el punto de vista mecánico, pero que no cumplı́an casi ninguna funcionalidad
práctica [11, 35]. Su funcionamiento estaba basado meramente en sistemas mecánicos sin
componentes eléctricos, pero pese a esto, podı́an ser configurados para cambiar su comportamiento. A medida que la tecnologı́a fue evolucionando, el uso de componentes eléctricos y
electrónicos se hizo común, pero inicialmente sólo como sistemas radio controlados, en especial
para aplicaciones militares. Pero una vez que los computadores se masificaron para el control
de robots, esto gracias a la miniaturización de componentes, todo esto cambió radicalmente.
Al poder modificar de manera sencilla el comportamiento y las estrategias de control sobre
los robots, la programación tomó incluso más importancia que la implementación mecánica.
En la actualidad cualquier proyecto robótico posee una estructura de software compleja
basada en múltiples etapas interconectadas. Para esto muchos de ellos utilizan softwares
de apoyo ya implementados y de esta manera no tener que implementarlas nuevamente,
acelerando el desarrollo, abstrayendo la programación y evitando posibles errores.
Figura 2.5: Diagrama de flujo del sistema de software de Stanley [43]
Un ejemplo de la complejidad de un sistema de software es el proyecto Stanley [43]. En
la Figura 2.5 se pueden apreciar las distintas etapas presentes y como se interconectan datos
8
de cada etapa. Este sistema esta basado en three layer architecture [9] que define tres capas:
capa de control, capa de secuenciamiento y capa de deliberación. Para la comunicación entre
procesos se utilizo un enfoque centralizado con comunicaciones sı́ncronas bidireccionales, pero
que se utilizaron en un solo sentido para evitar deadlocks y poseen un servidor central que
gestiona las comunicaciones y la transferencia de datos.
En el proyecto Boss [47], para la comunicación e interpretación de sus comunicaciones
entre procesos se utilizó LCM [25], lo que les da un enfoque de comunicación ası́ncrona y no
coordinada, pero de baja latencia.
Luego de las competencias DARPA, surgieron nuevas iniciativas que buscaban generar sus
propios vehı́culos autónomos, pero que a su vez permitiesen el intercambio de los desarrollos
con otros grupos de trabajo y de esta manera acelerar el desarrollo de estas tecnologı́as.
Algunos grupos de trabajo desarrollaron drivers y estrategias de control que actualmente
están disponible para su uso (libre o con restricciones, dependiendo de su licenciamiento)
en otros desarrollos. Esto permite directamente que grupos pequeños realicen desarrollos
especı́ficos en un área de investigación gracias a que disponen de una base consolidada sobre
la cual trabajar. Pero este enfoque tiene la desventaja que dificulta la monetización de un
desarrollo. Dependiendo del licenciamiento que tengan los desarrollos base, estos pueden
obligar a la liberación del desarrollo o impedir su monetización, diferenciándose de las licencias
comerciales que si lo permiten.
Bajo esta filosofı́a de desarrollo comunitario y libre, surge por ejemplo el robot PR2
de Willow Garage que es exclusivamente para investigación. Este robot promueve el uso de
ROSa [31,35], permitiendo que personas no tenı́an acceso al robot participaran en el desarrollo
de su software. Esto permite que el tiempo involucrado en el desarrollo de un proyecto tenga
un desarrollo sea menor y que permita enfocarse en las caracterı́sticas relevantes.
Estas desarrollos comunitarios suelen tener un enfoque general a los problemas, por lo
que al momento de enfrentarlos a problemas con restricciones especificas, no suelen tener
rendimiento alto al no poder adaptarse correctamente. Esto lleva a requerir implementaciones
especificas en caso de que un requerimiento sea muy particular o tenga restricciones técnicas
especificas.
2.3.
Sistemas de estimación de mapas de entorno
La estimación del mapa del terreno es un componente importante en la navegación para
robots en ambientes exteriores. Diferentes tipos de representaciones han sido usadas para
mapear terrenos (por ejemplo, grilla 2D de ocupancia, grilla 3D de ocupancia basada en voxels b ), siendo los mapas elevación o mapas de alturas 2.5D uno de los preferidos como opción
en aplicaciones de navegacion en robots terrestres ( [22], [33], [46], por nombrar algunos),
debido a su compactibilidad, bajo requerimiento computacional e insensibilidad al error de
discretización de altura [46]. Los mapas de elevación “almacenan en cada celda de una grilla
discreta la altura de la superficie, la cual corresponde a su ubicación en el ambiente [32]”. En
algunos sistemas de mapeo, la varianza de la altura es también almacenada para cada una
de las celdas [22, 33]. Hay que mencionar que este tipo de enfoque es posible para superficies
terrestres por ser representable como un manto, la limitación de esta es que para cada posición (x, y) solo se representa una altura, por lo que objetos verticales no son representables
a Robot
Operating System
pixel
b volumetric
9
correctamente.
La estimación de la altura del terreno para las celda en un mapa 2.5D requiere de una
correcta fusión de múltiples mediciones obtenidas en diferentes instantes de tiempo durante
el desplazamiento. La incerteza de las mediciones y la incerteza de la posea del sensor en
cada instante de tiempo debe ser considerada en el proceso de fusión. Esto es abordado
en [22], donde ser propone un sistema de estimación de terreno basado en mixture-model que
incorpora ambas incertezas, junto con el mapa de mediciones y las mediciones de la celda en el
mapa. El mapeo del terreno y el algoritmo de estimación usan un análisis probabilı́stico de los
errores, asumiendo que tienen distribución Gaussiana. El sistema proyecta las detecciones de
los sensores en el sistema de coordenadas globales usando un modelo acoplado de la posición
de sensores, haciendo la propagación formal de los errores. La localización del vehı́culo es
obtenida de un GPS, sin el uso de ningún algoritmo de estimación de estado.
Tres mejoras fueron propuestas en [33] para mejorar el desempeño en tiempo real del
sistema propuesto en [22]: una ventana de selección es usada para limitar el área de la
distribución de probabilidades de cada medición, un algoritmo de clustering es usado para
simplificar la tarea de detección de objetos y un vector virtual es introducido para reducir el
costo computacional del algoritmo. A pesar de estas mejoras, no hay cambios fundamentales
al sistema de estimación de terreno propuesto en [22].
En [43] un modelo de Markov de primer orden es propuesto para modelar el drift del error
de la estimación de la pose en el tiempo. La altura del mapa de puntos es asumida Gaussiana
con una varianza que escala linealmente con la diferencia de tiempos de los puntos. Entonces
un test probabilı́stico es usado para detectar la presencia de un obstáculo. Ası́, este trabajo
se enfoca en la detección de obstáculos, más que en proporcionar un mapa que represente
exactamente el entorno.
En [32] se presenta un enfoque que permite a un robot móvil lidiar con objetos verticales
y sobresalientes en los mapas de elevación presentados. El enfoque usa cuatro clases (celdas
transitables, brecha vertical, estructura vertical y posiciones detectadas desde arriba) para
clasificar ubicaciones en el ambiente y lidiar con estructuras sobresalientes. El mapa de elevaciones es actualizado usando una fusión tipo filtro de Kalman (en cada celda, la información
nueva es fusionada con las estimación existente considerando sus varianzas). El cierre del
loop del mapa es resuelto de manera offline y el vehı́culo se condujo a velocidades bajas en
caminos pavimentados.
Por otro lado existe el enfoque del SLAM b [42] [6] el cual busca solucionar el problema
doble, generar un mapa de entorno y localizarse de manera simultanea. Los métodos SLAM
se dividen principalmente en dos tipos: online y offline. Mientras el offline busca reconstruir
a posterior el mapa y la ruta que recorrió el robot, el online busca hacerlo inmediatamente
de manera de poder planificar su recorrido utilizando el mapa recién construido. SLAM es
una de las metodologı́as más estudiadas en robótica debido a su importancia para robots
móviles. La gran mayorı́a de los métodos de SLAM son en dos dimensiones. El disponer
de observaciones de los mismos objetos pero desde posiciones diferentes permiten estimar el
desplazamiento en el mapa e incorporar con coherencia las observaciones nuevas al mapa.
Otro gran incoveniente de los metodos SLAM en general es su alto nivel de computo que
requiere, problema que no es crı́tico en el caso de SLAM offline pero si lo es para el caso
online. Por este motivo, es tı́pico observar que para el caso online los robots deben detenerse
a ubicación
y orientación espacial
Localization And Mapping, Localización Y Mapeado Simultáneos
b Simultaneous
10
para procesar las observaciones antes de tomar una decisión de ruta.
Muchos métodos de SLAM están disponibles para la comunidad en OpenSLAM.org [6].
Uno de lo métodos ampliamente utilizados es GMapping [12] [13] el cual asume un “mundo”
plano en dos dimensiones y utiliza mapas de grilla como mapa. Al ser plana su representación, solo es capaz de estimar guiñada en el mapa. En [18] se presenta HectorSlam, una
implementación de SLAM que pese a representar el mapa en dos dimensiones realizan una
estimación de la localización en seis dimensiones. Esto permite realizar generar un mapa
aún cuando la superficie del suelo tenga inclinaciones irregulares. Este método ha tenido
un gran desempeño en la competencia RoboCup Rescue. En [8] se presenta RGB-D SLAM,
una implementación de SLAM que utiliza una representación del mapa en tres dimensiones,
para ello utiliza un sensor Kinect. Debido al principio de funcionamiento de este sensor, le
permite de manera directa estimar los calces entre dos observaciones sucesivas y de esta
manera obtener el desplazamiento del sensor, esto es la tarea compleja de solucionar en el
SLAM en tres dimensiones, esto le permite funcionar de manera online gracias a la nube
de puntos “organizada”. En [27] se presenta SLAM6D, esta implementación esta incorporada
en el toolkit 3DTK [26]. Este toolkit incluye funcionalidades para el manejo optimizado de
nubes de puntos, generando mapas complejos en tres dimensiones y estimaciones de pose en
seis dimensiones. Lamentablemente su funcionamiento no esta pensado para ser utilizado de
manera online y sus estructuras de datos no son directamente compatibles con otros sistemas.
11
Capı́tulo 3
Marco teórico
Índice
3.1. Arquitecturas de software y modalidades de comunicaciones . . 13
3.1.1. Medios de comunicación entre procesos
. . . . . . . . . . . . . . .
14
3.1.2. Arquitecturas de IPC . . . . . . . . . . . . . . . . . . . . . . . . .
16
3.2. Middlewares
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
3.3. ROS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
3.4. Mapas para la robótica . . . . . . . . . . . . . . . . . . . . . . . . . 20
3.4.1. Tipos de mapas . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
21
3.5. Principio de funcionamiento de un LIDAR . . . . . . . . . . . . . 22
En este capı́tulo se presentará una breve introducción a temas relacionados con esta Tesis
Doctoral, conocimiento que fue relevante al momento de tomar decisiones durante el desarrollo. En la primera sección se describirá sobre los diferentes medios de comunicaciones entre
procesos en un sistema computacional. En una segunda sección se comentarán las implementaciones que fueron evaluadas. En una tercera parte se expone una pequeña descripción
de ROS. En una cuarta parte se describen las caracterı́sticas de tipos tipos de mapas para
navegación y mapeo. Y para finalizar una descripción del principio de funcionamiento de los
sensores utilizados en el vehı́culo autónomo.
3.1.
Arquitecturas de software y modalidades de comunicaciones
Dentro de un sistema autónomo afloran diversas dificultades. A diferencia de los sistemas
que no interactúan fı́sicamente con su entorno, un sistema autónomo que lo hace debe tener
en consideración los tiempos de respuesta y latenciasa . Hay tareas que son necesarias de
que se ejecuten antes de un determinado plazo, después del cual ya no es posible realizar
la acción de control. También está la problemática que surge al momento en que muchos
procesos interactúan simultáneamente, esto genera caminos crı́ticos del procesamiento de la
información en conjunto con eventos ası́ncronos a procesar.
En un sistema de alta complejidad, el procesamiento es altamente no secuencial y la
necesidad de realizar muchas tareas simultáneamente se convierte en una necesidad. Procesadores multi-cores o el uso de un cluster de computadores permite realizar computo en
paralelo, pero agrega la necesidad de comunicar estos procesos de manera eficiente. Para
lograr la interacción adecuada entre los procesos se debe contar con un medio de comunicaciones (ICP, Inter-Process Communication). Dependiendo del uso o las caracterı́sticas de la
información que se comunique, diferentes caracterı́sticas pueden ser necesarias, por ejemplo:
Comunicaciones sı́ncronas o ası́ncronas, latencias fijas o variables, prioridad o relevancia de
cada mensaje, comunicaciones centralizadas o distribuidas.
La “salud” de un proceso en un sistema complejo es de vital importancia. ¿Qué hacer
en caso que un proceso falle?. Responder a esta pregunta es complicado cuando un sistema
interactúa con el mundo real, donde puede llevar a un accidente. En el caso de una respuesta
errónea o inesperada podrı́a provocar un fallo generalizado o decisiones peligrosas. Para prever
estos casos, la supervisión de las salidas crı́ticas o poseer redundancia sobre ciertos procesos
podrı́a ser una solución en este ámbito. En el caso de una falta de respuesta, como en el caso
de un proceso caı́do o detenido, revisar si se sigue ejecutando o si la frecuencia de su salida
está dentro de rangos normales es necesario para generar una alerta. Dependiendo del tipo
de proceso y cuan relevante sea para el sistema, la acción de contingencia puede ir desde
reiniciar el módulo a una alerta completa que detenga el sistema y accione los sistemas de
emergencia.
Otra importante caracterı́stica, ya no desde el punto de vista del desempeño técnico,
corresponde a las herramientas disponibles durante el desarrollo del software, en especifico
para debuggear b un sistema. Al momento de programar cualquier sistema, las posibilidades de
tener errores siempre existe, por lo que es necesario disponer de herramientas que permitan de
a suma
de los retardos dentro de una red
de debug, de la palabra bug(bicho), corresponde a la acción de eliminar errores
b españolización
13
manera lo menos invasiva localizar la razón y la ubicación de un error. En un sistema de alta
complejidad con comunicaciones entre múltiples procesos y en tiempo real esta herramienta
debe permitir observar las comunicaciones de la manera menos invasiva posible para el proceso
o la comunicación de manera de no alterar el comportamiento y desempeño del proceso.
3.1.1.
Medios de comunicación entre procesos
Existen diferentes medios para comunicar información entre dos o mas procesos ejecutándose en un mismo computador. Cada uno tiene caracterı́sticas propias que le dan valor
frente a los otros. Estos medios dependen de la implementación en el sistema operativo, por
lo que podrı́an no estar disponibles en algún sistema operativo en especı́fico. Utilizando estos
medios de comunicación básicos, se implementan librerı́as de comunicaciones que son usadas
mediante sus APIs a con lo cual es posible abstraerse de la implementación. Para el caso de
LINUX tenemos que los principales medios de comunicación son [23]:
Memoria compartida
La manera mas simple e intuitiva de compartir información entre procesos, consiste en
un espacio de memoria común a la que distintos procesos tienen acceso. En esta memoria
los procesos pueden leer y escribir generando una comunicación bidireccional. Es la manera mas rápida de comunicación al no tener que copiar la información compartida, haciendo
muy eficiente en el caso de tener que comunicar grandes volúmenes de información. La falta de arbitraje sobre este espacio de memoria por parte del sistema es su mayor defecto.
No poder controlar cuando un proceso escribe, lee, o quien la accede, pone en peligro la
integridad de la comunicación. Esta memoria puede ser accedida por cualquier numero de
procesos simultáneamente, por lo que una coordinación debe ser implementada para asegurar
la integridad de la información.
Semáforo
Los semáforos son herramientas provistas por el kernel b para coordinar el acceso a un
recurso compartido por parte de uno o más procesos. Su principal caracterı́stica es que son
operaciones atómicasc , de manera que es seguro usarlas para la tarea de compartir recursos
y ası́ evitar los problemas generados por las carreras crı́ticas.
Son implementados como una variable compartida que es usada como contador, en caso
de solicitar el uso del recurso, el contador es decrementado en uno. Una vez decrementado, en
caso que el resultado sea negativo, la solicitud será puesta en espera hasta que ese resultado
se mayor o igual a cero. Al terminar el uso del recurso, el valor del semáforo es incrementado
en uno. Un caso particular de los semáforos es el “semáforo binario” o mutex d el cual es usado
para evitar un doble acceso a un recurso.
Los semáforos son usados para proteger recursos como las memorias compartidas e implementar un sistema de comunicación, pero por sı́ mismas son un medio de comunicación entre
a Application
programming interface, conjunto de funciones y procedimientos disponibles de un software
del sistema operativo, software encargado de coordinar los recursos del computador
c operación indivisible para asegurar su integridad
d MUTual EXclusion o exclusión mutua
b núcleo
14
dos o más procesos, todo depende de como se le interprete, siendo más una herramienta de
sincronización.
Archivo proyectado en memoria
Este medio de comunicación se basa en el uso de un “archivo”, al cual uno o más procesos acceden de manera simultanea, pudiendo leer o escribir en él. En su implementación
el “archivo” es copiado en memoria, y de esta manera que los diferentes procesos accedan
directamente a la memoria y no al sistema de archivos, ya que el acceso a disco es más lento.
La implementación se encarga de gestionar el acceso a este recurso, y que cuando se libere,
volverlo al sistema de archivos. Este “archivo” no necesariamente es un archivo del sistema
de disco, sino que puede ser por ejemplo: un dispositivo fı́sico, una interfaz de usuario, un
recurso de red.
Su principal caracterı́stica es el carácter permanente del medio de comunicación, en el
caso que el recurso se desocupe y otro proceso requiera usarlo nuevamente, la información
anterior seguirá disponible, siendo esto transparente para el proceso.
Pipe y FIFO
Las Pipes a son un dispositivo de comunicación unidireccional que permite a dos procesos
comunicarse. Mientras uno escribe por un extremo del pipe, por el otro lado se leen estos
datos. Por la manera en que están implementados, tienen una capacidad máxima de datos
que pueden retener. Si el extremo que lee del pipe no lo hace y este se llena, el extremo que
escribe esperará hasta que haya espacio. Ası́ mismo, si el pipe se vacı́a, el proceso que lee
esperará hasta que haya información que leer.
Las FIFOs b son un tipo especial de pipe que están mapeadas en el sistema de archivos, por
este motivo también son llamados named pipes c . A diferencia de los pipes, múltiples procesos
pueden escribir y leer simultáneamente de una FIFO.
Socket
Los sockets son un medio de comunicación bidireccional entre dos procesos. Estos está en
una capa de abstracción mas arriba que los analizados anteriormente. La principal caracterı́stica de los sockets es que pueden comunicar de manera bidireccional a dos procesos, los
cuales no necesariamente están en un mismo computador, pero sı́ en la misma red. Internamente el enlace es manejado de manera diferente, si la comunicación es entre procesos dentro
del mismo computador o si es entre computadores distintos, pero la implementación permite
abstraerse de esto.
La información enviada es dividida en packets d de menor tamaño, que son los que finalmente se envı́an. Al definir un socket, un estilo de control para la comunicación debe ser
definido, y determinará como son manejados los packets y como se hacen llegar al socket de
destino. Hay dos principales estilos de control para los sockets:
a tuberı́a
b First
In First Out, primero en entrar primero en salir
nombrada
d paquetes
c tuberı́a
15
Orientado a la conexión: En este enfoque se garantiza la entrega de todos los packets
en el orden en que fueron enviados. En el caso de pérdida o desorden de los packets
por problemas en la red, el receptor solicita el reenvı́o de la información, que solo es
entregada una vez que se recibe el mensaje completo. El protocolo de este tipo más
usado es el TCP a .
No orientado a la conexión: En este enfoque no se garantiza la entrega de los packets
ası́ como tampoco se garantiza el orden en que lleguen. Pero esto mismo hace que la
latencia de la comunicación sea mucho menor. El protocolo de este tipo más usado es
el UDP b
Desde el punto de vista de los procesos, se definen dos roles. El de cliente que es el
encargado de iniciar el enlace al solicitar una conexión al servidor, y el de servidor que esta
siempre esperando una solicitud de conexión. Una vez iniciada la conexión, el comportamiento
del enlace en ambos procesos es el mismo, es similar a tener dos pipes, uno de escritura y uno
de lectura.
3.1.2.
Arquitecturas de IPC
Dependiendo de los requerimientos del sistema robótico que se estén tratando de cumplir,
diferentes enfoques y arquitecturas de como interactuaran los procesos pueden ser necesarios.
A continuación se exponen algunos de estos tipos de arquitectura,que en su gran mayorı́a son
implementados mediante sockets para poder comunicarse con procesos en otros computadores.
Central server: En este enfoque de comunicaciones un proceso central es el encargado
de manejar las comunicaciones. Si dos procesos quieren comunicarse entre sı́, primero
deben definir que es lo que se comunicarán. Cada proceso se identificará con el servidor
central y permanecerán conectados a este mientras dure la comunicación. El servidor
central recibirá los mensajes de cada proceso y los hará llegar a donde corresponda.
Este enfoque disminuye el número total de conexiones, ya que los procesos generan una
única conexión al servidor central y a través de este se realizan todas las comunicaciones.
Esto genera un máximo de conexiones igual al numero total de procesos. La Figura 3.1
muestra un diagrama de ejemplo de este esquema.
En su contra, todo el tráfico que se genere pasará por el servidor central, que debe
recibirlo y reenviarlo, aumentando la latencia de la comunicación entre los procesos. En
caso que un proceso no este corriendo en el mismo computador que el servidor central,
el enlace hasta el servidor pasara por la red externa, lo que aumentará la latencia que
incluso puede propagarse a los procesos que si se ejecutan en el mismo computador.
El servidor central también puede almacenar información como parámetros que los procesos puedan necesitar. Dentro de esta categorı́a caen los sistemas que tienen un proceso
central, que aparte de manejar las comunicaciones también maneja los dispositivos.
Peer-to-Peer: En el esquema “punto a punto” si un procesos necesita comunicarse con
otro, lo hace de manera directa sin que los datos pasen por ningún otro proceso. Esto
no quiere decir que no pueda existir un proceso o tabla que coordine la comunicación
a Transmission
b User
Control Protocol
Datagram Protocol
16
Figura 3.1: Arquitectura de servidor central
entre los procesos, solo que la información transferida no pasará por este. La Figura
3.2 muestra un diagrama de ejemplo de este esquema.
Este enfoque trata de minimizar el uso de los recursos de comunicación al sólo existir
un enlace para cada canal de información. La latencia del enlace es reducida al mı́nimo
que permita el protocolo de control de transmisión del enlace. En caso de un proceso
este en otro computador, sólo los enlaces con este proceso se verı́an afectados por la
latencia el enlace de red, ya que los enlaces entre procesos de un mismo computador
no se afectan directamente.
Figura 3.2: Arquitectura peer-to-peer
Broadcast En este esquema, cada procesos envı́a su mensaje a todos los procesos presentes,
y no solo a los procesos con los que se busca comunicar. Por este motivo cada proceso
debe estar siempre escuchando todo, y descartar los mensajes que no van dirigidos a él.
El carácter unidireccional y ası́ncrono de la comunicación lo hace perfecto para enviar
comandos y mensajes pequeños. Por lo que los procesos que solo reciban información
no deben mantener una conexión establecida de transmisión de datos. La Figura 3.3
muestra un diagrama de ejemplo del esquema broadcast.
Este esquema trata de minimizar la latencia de la comunicación y el número de conexiones permanentes. Por sus limitaciones de implementación, el tamaño de los mensajes
suele ser reducido, por lo que no es apto para intercambiar cantidades grandes de información. Este enfoque esta reservado para redes pequeñas, por que en el caso que uno o
más procesos este fuera del computador, los mensajes generados deberá salir a la red,
y dependiendo de la topologı́a de la red, podrı́an inundarla de mensajes repetidos.
17
Figura 3.3: Arquitectura Broadcast
3.2.
Middlewares
En un sistema robótico en el que se requiere la interconexión de muchos procesos diferentes, implementar las comunicaciones entre los procesos a medida, vuelve demasiado especifica
la implementación del sistema y puede ser una fuente importante de errores. Si cada sistema
tiene su propio sistema de comunicación, herramientas y estándares, esto impide la reutilización de estos módulos del sistema en otros sistemas y también dificulta el desarrollo
distribuido. Es por esto que el uso de middlewares se adapta a los sistema robóticos. En [10]
se describe un middleware como “... una clase se software que ayuda mantener la complejidad
y heterogeneidad inherente en un sistema distribuido. Esta definida como una capa de software que esta sobre el sistema operativo pero bajo los programas de aplicación y que provee de
una abstracción al programar en un sistema distribuido ... ”. En este mismo trabajo definen
que las razones para utilizar un middleware son la portabilidad, la confiabilidad y manejo de
la complejidad.
Existen muchos middlewares diferentes, cada uno con sus caracterı́sticas propias. A continuación se presentan algunos que son considerados como los más relevantes:
CARMEN: Es el middleware implementado por Carnegie Mellon University, de ahı́ su
nombrea , alrededor del año 2003. Para la comunicación entre procesos se utiliza la
implementación de IPC de Simmons [40], por lo que usa el esquema de servidor central. Provee drivers para algunos sensores y plataformas robóticas, ası́ como algunas
implementaciones de algoritmos de navegación y herramientas afines, como monitores
gráficos y sistema de grabación de mensajes. Pese a ser uno de los primeros proyectos
de middleware especı́ficos para robótica, no logró surgir y solo fue ocupado por algunos
proyectos, pero marcó un precedente para los que vinieron después.
LCM: Es la implementación de middleware [25] realizada por el MIT para la competencia
DARPA Urban Challenge con su vehı́culo TALOS [19]. El nombre viene de Lightweight
Communications and Marshalling b , por que principalmente provee de un sistema de
comunicaciones entre procesos y no otras funcionalidades. Es un sistema de filosofı́a
publicador/subscriptor que define el empaquetado de los datos dentro de estos mensajes. Su implementación está basada en broadcast de mensajes mediante UDP, lo que
implementa un sistema de muy baja latencia.
a CARnegie
MEllon robot Navigation toolkit
ligeras y ordenamiento
b Comunicaciones
18
Al ser una comunicación transversal, también permite el monitoreo y loggeo de los
mensajes permitiendo reproducirlos de manera transparente para los procesos sin alterar
el desempeño de las comunicaciones de manera significativa. Pese a proveer solo el
sistema de comunicaciones, su simplicidad de uso lo ha hecho popular y está presente
en variados proyectos.
RoboComp: Es un middleware [20] implementado por The Robotics and Artificial Vision
Laboratory (ROBOLAB) de la Universidad de Extremadura. Para la comunicación entre
procesos utiliza ICE [57], este es un middleware de comunicaciones orientado a objetos
que implementa múltiples estilos de comunicación. Uno de los principales beneficios
de RoboComp, es que ICE esta implementado en muchos lenguajes de programación y
tiene soporte en la mayorı́a de los sistemas operativos, permitiendole extenderse a ellos.
Implementa una abstracción de hardware(HALa ) que le permite mantener bajo control
la complejidad del código y abstraer de este al programador. Además provee un conjunto
de herramientas visuales para complementar su funcionamiento.
ROS: Es un middleware que se concibe en Stanford University, pero que logra su éxito bajo
Willow Garage b . Su nombre viene de Robot Operating System [31, 34] que aún cuando
no es un sistema operativo propiamente tal, provee de herramientas que lo asemejan a
uno aún cuando corra sobre Linux. Provee un sistema de comunicación de procesos peerto-peer y muchas otras funcionalidades que se describen en la Sección 3.3. Lo que lo ha
vuelto altamente popular es la gran comunidad de desarrolladores que ha logrado crear.
Esto ha posibilitado un alto intercambio de ideas e implementaciones, que van desde
drivers de dispositivos, a algoritmos de control muy especı́ficos. Sus estándares bien
definidos de como programar y su versatilidad, le ha permitido surgir hasta convertirse
en uno de los middleware más activos actualmente.
3.3.
ROS
ROS [31] es un acrónimo para Robot Operating System, y es un middleware especializado para la robótica. Su activa comunidad ha sido una de las principales razones que le
han permitido crecer explosı́vamente desde su lanzamiento. Las principales caracterı́sticas de
este middleware son una robusta y flexible infraestructura de comunicaciones, caracterı́sticas
especı́ficas enfocadas a robots y una extensiva colección de herramientas relacionadas.
Además ROS integra dentro de sus funcionalidad librerı́as bien difundidas, como son:
Gazebo [30] como simulador multi-robot, OpenCV [16] como librerı́a de manipulación de
imágenes, PCL [29, 36] como librerı́a de percepción y procesamiento 3D y MoveIt! [41] como
librerı́a de planeamiento de movimiento.
Otro aspecto importante para su masificación, es que provee soporte para dos lenguajes
de programación: C++ y Python. Sistema de administración de software ha permitido el fácil
intercambio de código permitiendo que comunitariamente haya generado un mayor soporte
para robots, dispositivos y sensores más utilizados.
Su arquitectura de comunicaciones entre procesos es de tipo peer-to-peer, pero posee un
servidor central encargado de coordinar las comunicaciones de los procesos. Este servidor
a Hardware
Abstraction Layer
de proyectos robóticos
b Incubadora
19
también permite monitoriar el estado de los procesos y ha permitido la creación de variadas herramientas de debuggeo y visualización, que invaden lo menos posible a los procesos,
manteniendo el desempeño del sistema.
El sistema de comunicación entre procesos está implementado con dos enfoques. Por
un lado está el enfoque publicador/subscriptor en donde un proceso publica la información
bajo un topic a y otro se subscribe a ese tema. Este esquema permite además la existencias de
múltiples publicadores en un mismo tema, ası́ como múltiples subscriptores. La comunicación
es ası́ncrona ya que el publicador no se entera que se han subscrito a su topic. El otro enfoque
es el de prestador/cliente, donde un proceso prestador ofrece un servicio y el cliente lo utiliza,
entregando parámetros y recibiendo de vuelta un resultado. La información es procesada en
el prestador del servicio, y el cliente espera hasta obtener el resultado de vuelta, siendo esta
una comunicación sı́ncrona.
En ROS, cada proceso es llamado node b , habiendo un nodo especial llamado roscore. Este
nodo es el encargado de coordinar las comunicaciones, supervisar a los otros nodos, proveer
del servidor de parámetros y servir de nodo de loggeo de rosout. Rosout es un topic común
para los nodo para desplegar información del funcionamiento.
Otra de las caracterı́sticas disponibles en ROS, son los rosbag. Un rosbag corresponde a
una grabación de los mensajes que fueron transmitidos por los nodos en un cierto momento.
Este tipo de log de mensajes puede ser reproducido posteriormente para ser utilizado por los
nodos, siendo transparente a los nodos que esos mensajes son grabaciones. Esta caracterı́stica
permite “generar” las entradas de un sistema sin que necesariamente estén se estén generando
“online”.
Dentro de las principales herramientas gráficas disponibles en ROS para complementar el
funcionamiento del sistema se encuentran:
rviz: Es un entorno tridimensional de visualización que permite desplegar desde información
sensorial hasta modelos del robot.
rxplot: Permite, en tiempo real, visualizar variables numéricas en función del tiempo en
forma de gráfico.
rxbag: Permite la visualización de un rosbag en formato de linea temporal y controlar su
reproducción.
rxgraph: Entrega una visualización en forma de grafo, de los nodos y los tópicos con los que
se interconectan. Este grafo se genera en tiempo real, desplegando además información
relevante sobre el estado del nodo y el tópico.
3.4.
Mapas para la robótica
En robótica, al referirse a mapas se habla de una estructura de datos que represente un
lugar fı́sico en el que se encuentra el robot y las cosas presentes en el lugar.
Dentro de los usos de los mapas en la robótica, por ejemplo, son de vital importancia al
momento de afrontar la problemática de navegación robótica, donde es necesario disponer
de una representación del entorno para poder desplazarse espacialmente. Una navegación
a tópico
o tema
b nodo
20
eficiente esta estrechamente relacionada con una representación de mapa adecuada. Los algoritmos de navegación suelen estar optimizados a la estructura de dato de su mapa, por lo
que es habitual encontrar algoritmos con estructuras propias, de manera de generar alguna
caracterı́stica necesaria.
También existen estructuras de mapas generales, que les permite ser versátiles en sus implementaciones y servir para diferentes usos. Esto se observa comúnmente cuando se requiere
algún mapa para múltiples tareas simultáneamente.
3.4.1.
Tipos de mapas
El enfoque del mapa dependerá directamente del uso dado, los usos tı́picos son [50]:
navegación, localización, path planning a , exploración y comunicación. Básicamente los tipos
de mapas existentes para la robótica son dos:
Topológicos: Corresponden a los mapas que registran las relaciones existentes entre ubicaciones en el mapa. Este tipo de mapa son comúnmente almacenado en estructuras
tipo grafos. Dentro de esta categorı́a, los enfoques tı́picos son los grafos de vistas y los
grafos de ruta.
Métricos: Corresponden a los mapas que representan la posición de los objetos registrados
en el mapa. No se almacena explı́citamente la relación entre los objetos, siendo necesario
calcularlo a partir del sus posiciones. Dentro de esta categorı́a, los enfoques tı́picos son
los basados en ocupancia, representación geométrica y los basados en landmarks.
Las propiedades que debe tener un mapa dependiendo de su uso están definidas por tres
caracterı́sticas:
Extractabilidad y Mantenibilidad: Corresponde a la caracterı́stica que debe tener el mapa para poder acceder adecuadamente a la información presente y agregar nueva de
manera dinámica.
Adecuada información: Se refiere a que la información contenida por el mapa es la adecuada para el uso que se le dará. Esta información se puede almacenar de manera implı́cita
o explı́cita, pero lo importante es que la información esté contenida y no exista pérdida
de ella.
Eficiencia y escalabilidad: La eficiencia de un mapa corresponde a la caracterı́stica de
incorporar información nueva y recuperar la almacenada, siendo esto en consideración
al espacio utilizado o al cómputo requerido. Con respecto a la escalabilidad, si el tamaño
máximo del mapa esta acotado a un cierto tamaño fijo o puede crecer dinámicamente.
En caso de ser de tamaño dinámico, es importante si la eficiencia varı́a con respecto al
tamaño que tiene.
Dentro del los mapas métricos, existen varios enfoques de como implementarlos, principalmente separados en cuantas dimensiones se tienen en consideración. En el caso de solo
considerar 2D, el cual es el mapa tı́picamente utilizado al momento de realizar implementaciones robóticas, el eje z suele ser simplificado y solo utilizadas las dimensiones x e y del
entorno. En este caso, un objeto estará representado por una ubicación espacial sea cual fuese
a planeamiento
de trayectoria
21
su posición vertical. Este enfoque presenta una gran simplificación al momento de realizar
cómputos y almacenarlo, y es el más utilizado por robots, en especial en robots terrestres. En
el caso de necesitar el modelado del entorno en 3D, se requiere tener nuevas consideraciones
con respecto a los cálculos y geometrı́a utilizada. Además, el utilizar una nueva dimensión
agrega un requerimiento mayor de almacenamiento para datos, ası́ como la necesidad acceder
a los datos de manera ordenada y eficiente.
La manera intuitiva de afrontar el problema es almacenar todas las observaciones con sus
componentes espaciales, pero tiene en su contra el alto espacio de almacenamiento a utilizar
ya que crece dinámicamente con el número de observaciones. Diversas estructuras de datos
han sido propuestas para este tipo de problemas, entre los que se encuentran: grillas 2D, 2.5D
y 3D [22, 24, 33, 43, 46], quadtree [37], KDtree [27, 29], octree [29, 56], y muchos otros.
3.5.
Principio de funcionamiento de un LIDAR
LIDAR es el acrónimo de LIght Detection And Ranging, pero también de la unión de las
palabras LIght y raDAR. En aplicaciones robóticas, la habilidad de medir distancias a los
objetos de manera remota o sin contacto es indispensable para lograr un desplazamiento por
el medio ambiente. Sensores de ultrasonido son buenas alternativas para sistemas robóticos
pequeños o cuando una alta resolución no es requerida. Las principales cualidades que poseen
este tipo de sensores es buena resolución en ángulo y distancia que pueden lograr, ası́ como
una alta tasa de muestreo, esto lo hace una de las alternativas más usada en proyectos de
robótica móvil. La resolución en distancia tiene la caracterı́stica de permanecer constante en
todo el recorrido, a diferencia de lo que pasa con otros métodos como la triangulación o la
luz estructurada. [52].
Como ya se mencionó, su funcionamiento esta basado luz (no necesariamente visible).
Haces de luz son proyectados sobre el objeto al cual se quiere estimar la distancia y se mide
el tiempo que tarda en volver la luz. Asumiendo conocida y constante la velocidad de la luz
en el aire, el tiempo resultante corresponde al viaje de la luz recorriendo dos veces la distancia
al objeto (3.1).
c · ∆t
(3.1)
2
Este principio de funcionamiento es conocido como Time-of-Flight a [52], por que la distancia se estima directamente del tiempo que demora en ir y volver la luz. Es el mismo principio
de funcionamiento que los sensores de ultrasonido pero utilizando otra tipo de onda.
Pero no siempre se puede implementar directamente de esta manera, dado que la implementación de un mecanismo para medir diferencias de tiempo tan pequeños puede resultar
complejo y caro, otros métodos para estimar la diferencia de tiempo son empleados. Uno
muy usado es la utilización de luz modulada, en este caso la emisión de luz es modulada en
amplitud con una frecuencia ω (ω = 2πf ), ∆t se transforma en la fase φ entre la luz emitida
y la recibida. La medición de la fase de dos señales es más simple de implementar que la
medición precisa del tiempo. Reemplazando términos en la Ecuación (3.1) para el caso de
luz modulada, se puede expresar en función de la fase como (3.2). Este método fija que la
máxima distancia estimable tiene directa relación con la longitud de onda λ de la frecuencia
moduladora.
d=
a tiempo
de vuelo
22
λ·φ
(3.2)
4
Debido a que la emisión no esta perfectamente colimadaa , a medida que se alejan del sensor
los rayos van divergiendo y aumentando el área de proyección. Esta apertura es conocida como
spot diameter b y crece linealmente con la distancia al sensor, haciendo crecer el área que
cubre este punto a medida que se aleja. De esta manera un punto puede cubrir diferentes
objetos a diferentes distancias en una misma observación. En la Figura 3.4 se muestra como
podrı́an generarse estos múltiples reflejos con una sola emisión del láser.
d=
Figura 3.4: Ejemplo de múltiples ecos de una emisión láser, donde al llegar el primer reflejo,
el segundo aún le falta por recorrer distancia
Los sensores que pueden detectar estos múltiples reflejos son denominados Multi Echoc .
Para detectar estos múltiples ecos, se registra la forma de onda de toda la recepción de luz
y es analizada para determinar los posibles reflejos de objetos y sus distancias.
Otro de los parámetros importantes para un sensor LIDAR es el spot spacing d , que es
la distancia a la que están situados entre medidas el punto de medición. Está en directa
relación con la resolución angular, y en caso de que el espaciamiento de los puntos sea menor
que el diámetro del punto, existirá un traslape en el área de observación entre observaciones
sucesivas.
Reflectividad e corresponde a la cantidad de energı́a del láser que es reflejada de vuelta
al sensor. Dependiendo de las caracterı́sticas del sensor, una cierta cantidad de energı́a es
necesaria para determinar una observación válida. Cada objeto tiene una reflectividad propia
que esta determinada por su material. Una relación entre esta reflectividad y de la distancia
de observación determinará si es posible detectar un objeto. Esta reflectividad depende de la
longitud de onda de la luz utilizada, y tiene un valor de 100 % para una “superficie blanca
reflectante perfectamente difusa” [3, 14, 39]. Por como esta definida, el valor puede ser mayor
a un 100 % dependiendo del material y su acabado. En la Tabla 3.1 se muestran algunos
ejemplo de reflectividad.
a luz
cuyos rayos son paralelos entre sı́
del punto
c Múltiples ecos
d espaciamiento de puntos
e Reflectivity, reflectancia
b diámetro
23
Tabla 3.1: Valores de reflectividad relativa (según KODAK standards) para algunos materiales [38, 51]
Material
asfalto negro
agua
cartón negro mate
tablas de maderas
cartón gris
tierra
madera sin procesar
PVC gris
vegetación
aluminio mate
concreto blanco
papel mate blanco
nieve
aluminio anodizado negro
acero sin óxido brillante
acero brillante
reflector
24
Reflectividad [ %]
3%
5%
10 %
17 %
20 %
30 %
40 %
50 %
50 %
74 %
78 %
80 %
80 %
110 % a 150 %
120 % a 150 %
140 % a 200 %
mayor a 2000 %
Capı́tulo 4
Vehı́culo Autónomo AMTC
Índice
4.1. Vehı́culo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
4.1.1. Descripción de modificaciones del vehı́culo . . . . . . . . . . . . . .
27
4.2. Hardware y Software . . . . . . . . . . . . . . . . . . . . . . . . . . 30
4.2.1. Computador . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
30
4.2.2. Sensores . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
31
4.3. Discusión . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
4.1.
Vehı́culo
El proyecto de vehı́culo autónomo desarrollado en el AMTC está implementado en un
Volkswagen Tiguan. El objetivo principal en este caso es la implementación de una plataforma
de desarrollo para tecnologı́as de navegación y automatización de vehı́culos orientado a la
minerı́a. Tener un vehı́culo minero real a la disposición de la investigación es muy difı́cil
debido al alto costo de ellos y por ello la utilización de un vehı́culo convencional se vuelve
una necesidad. Un automóvil es capaz de recorrer largas distancias, tiene de una escala más
adecuada que un robot de laboratorio, y es capaz de recorrer ambientes mineros al igual que
una maquinaria minera.
Una de las caracterı́sticas importantes para la plataforma de desarrollo es que mantenga
las caracterı́sticas de vehı́culo convencional permitiéndole transitar libremente por las calles
sin la necesidad de un vehı́culo extra que lo transporte. Para esto las intervenciones al vehı́culo
tratan de ser lo menos invasivas, de manera que cumpla con la reglamentación vigente en
Chile. En la Figura 4.1 se puede apreciar el vehı́culo durante una de las pruebas en el parque
O’Higgins.
Figura 4.1: Vehı́culo Autónomo del AMTC
Como se mencionó anteriormente corresponde a un Volkswagen Tiguan año 2010, cinco
puertas, patente CRJZ21. Es un vehı́culo todoterreno con tracción a las cuatro ruedas con un
motor bencinero de cuatro cilindros 2.0L 125KW (16V) Turbo FSI. Tiene capacidad para 4
pasajeros más el conductor y un porta maletas amplio. En la Tabla 4.1 se encuentran algunas
de las caracterı́sticas fı́sicas del vehı́culo.
a batalla,
distancia entre eje delantero y trasero
entre ruedas izquierdas y derechas
b distancia
26
Tabla 4.1: Especificaciones fı́sica del vehı́culo
Especificaciones del vehı́culo Valor
Masa
1590[Kg]
Largo
4427[mm]
Ancho
1809[mm]
Alto
1686[mm]
a
Wheelbase
2604[mm]
b
Track
1686[mm]
4.1.1.
Descripción de modificaciones del vehı́culo
Para convertirlo en un vehı́culo autónomo muchas modificaciones se han llevado acabo en
el vehı́culo. A continuación se describen algunas de las más relevantes que se han realizado:
Instalación de parrilla para sensores: Para el montaje de sensores en el vehı́culo se instaló una estructura de perfiles de aluminio extruido MiCRO. Estos perfiles tienen canales donde se pueden montar dispositivos de manera precisa y mucha firmeza, permitiendo cambios de configuración de los dispositivos fácilmente. Son utilizados perfiles
de 45x45 [mm] y 45x90 [mm] de la linea estructural de MiCRO. Para la fijación de esta
estructura al vehı́culo, una parrilla para carga de equipaje es fijada a los soportes del
techo del vehı́culo. Esto permite desmontar toda la estructura en caso de ser necesario.
En la Figura 4.1 se puede ver la parrilla instalada en el vehı́culo con los sensores instalados y en la Figura 4.3 se alcanza a ver la parte posterior de la parrilla y se puede
apreciar la forma de los perfiles de aluminio.
Actuación del volante: Para la intervención del volante un motor brushless DC fue montado junto a la columna de dirección mediante una cadena. El motor brushless DC
es marca Emoteq modelo HT03005-C01-H y tiene incorporado un resolver marca LNT
modelo RE21-1-D64. El controlador del motor es marca Maccon modelo SWM 48-25R-CT es empleado para controlar el motor. La intervención permite un manejo normal
del volante cuando no se esta en modo autónomo [21].
Actuación del freno: Para la actuación del pedal de freno un actuador lineal fue instalado
a los pies del asiento del copiloto. Este servomotor tira de una piola de acero que tira
del pedal de freno. Esta piola esta montada de tal manera que permite accionar el pedal
aún cuando el actuador no se accione. El actuador lineal es un servo-tubo marca Copley
modelo STA2508P-155-S-S-03-F. El controlador del motor es marca Accelnet modelo
Panel ADP-090-36-S [21].
Intervención del acelerador: El pedal del acelerador fue intervenido de manera electrónica. El acelerador en este vehı́culo corresponde a dos referencias de voltaje generadas en
el pedal, y que deben ser coherentes entre ellas. Para intervenirlas, un sistema genera
estos voltajes y emula al comportamiento del pedal [21].
Intervención del freno de mano: La palanca de freno de mano de este vehı́culo es completamente electrónica. Una emulación de la palanca fue agregada al vehı́culo en paralelo a la palanca original. La palanca de freno de mano además tiene la funcionalidad
27
de parada de emergencia en caso de accionarse por sobre 20 [Km/h], lo que permite
accionar este sistema de emergencia en caso de ser necesario [21].
Actuación de la palanca de cambios: Para la intervención, es necesario reemplazar la
palanca por un sistema que sea capaz de desplazar la varilla que mueve originalmente
la palanca. Para ello un actuador con desplazamiento lineal es instalado en su lugar.
Una primera intervención fue implementada por [21], pero ha tenido que ser modificada
posteriormente.
Figura 4.2: Intervención en el maletero
Rack para controladores y computadores: El maletero del vehı́culo fue asignado para
ubicar los controladores y computadores del sistema autónomo. En la Figura 4.2 se puede apreciar estas intervenciones. Para esto un implemento un rack a donde se instalaron
los controladores y computadores abordo. Además un switch ethernet y reguladores de
voltajes están ubicados en esta área. Una baterı́a de plomo ácido también esta en el
maletero. Los interruptores principales del sistema autónomo están ubicados también
acá.
Intervención bus CAN: Es necesario realizar una intervención al bus CAN del vehı́culo
para poder leer de él información relevante. El vehı́culo dispone de tres buses CAN,
y la intervención es sobre el bus PowerTrain. Debido a la gran cantidad de mensajes
que fluyen por este bus, un Gateway es utilizado para filtrar los mensajes relevantes del
bus.
Pasada de cables al exterior: Una gran parte de los dispositivos instalados en el exterior
del vehı́culo deben conectarse por cables que deben llegar al maletero del vehı́culo. Para
esto la ventana lateral del maletero fue retirada y reemplazada por una goma con un
a soporte
metálico para alojar equipamiento
28
Figura 4.3: Intervención en la ventana del maletero
ducto que lleva al exterior. En la Figura 4.3 se puede apreciar esta intervención. Esto
permite llevar los cables al exterior, sin que entre agua o polvo al maletero.
Figura 4.4: Botón de emergencia montado en el parachoques
Sistema de parada de emergencia: Para evitar accionamientos del sistema autónomo de
manera accidental mientras se trabaja cerca del vehı́culo, interruptores de emergencia
fueron instalados en diversas partes. Para habilitar el sistema autónomo todos los pulsadores deben estar no activados, y en el caso que alguno se presione, el sistema autónomo
se desactiva y acciona una parada de emergencia. Estos interruptores están instalados
tanto en el exterior como en el interior del vehı́culo. En la Figura 4.4 se puede apreciar
uno de los interruptores montado en el parachoques trasero del vehı́culo.
29
Baliza de precaución: Al momento de estar habilitado el modo autónomo o el modo telecomandado, una baliza color amarilla es activada para advertir el estado del vehı́culo.
En la Figura 4.3 se puede apreciar la ubicación de la baliza.
Conexión a baterı́a: Una conexión directa desde la baterı́a hasta el maletero fue agregada.
Esta conexión no tiene fusible como el socket de 12 [V] del maletero, lo que permite
alimentar más dispositivos. En reemplazo del fusible un disyuntor magnético de 40 [A]
fue instalado como disyuntor principal, y tres disyuntores térmicos de 10 [A] para las
diferentes redes eléctricas. El alternador del vehı́culo entrega suficiente potencia para
alimentar todos los componentes.
Instalación de radio-modem: El vehı́culo tiene instalado un radio-modem Teledesign Systems Inc. modelo TS4000B (5 [W], 450-470 [MHz]) que le permite supervisar el comportamiento del sistema autónomo durante el funcionamiento. Por este mismo canal
es posible tele-comandar el vehı́culo ası́ como accionar el sistema de emergencia para detenerlo. En la Figura 4.2 se puede los radio-modem instalados a la izquierda del
computador.
4.2.
Hardware y Software
El sistema de automatización esta compuesto de múltiples controladores y procesadores
encargados de controlar el vehı́culo y realizar las tareas asignadas.
El control de los actuadores y supervisión del sistema hardware especializado es empleado.
Como dispositivos básico de control son usados microcontroladores PIC24HJ256GP610A en
un formato de socket diseñado por AMTC. Esta placa denominada AMTC-brain es utilizada
en placas de control especialmente diseñadas para cada tarea, que incorporan los transceiver
y hardware adicional para cada función desempeñada. Para el control de alto nivel, un computador con estándar automotriz es utilizado procesar la información perceptual proveniente
de los sensores.
4.2.1.
Computador
El computador corresponde a un Advantech ARK-3440. Este computador es de estándar
automotriz, no tiene ventilador y un disco duro SSDa lo que le permite estar expuesto a
ambientes con vibraciones. Las caracterı́sticas técnicas están expuestas en la Tabla 4.2.
Con respecto al software utilizado en el vehı́culo, el sistema operativo instalado en el
computador es Ubuntu 12.04. Git [45] es utilizado para el versionamiento del software implementado para ser utilizado en el vehı́culo de manera de coordinar la version que se ejecuta
y supervisar los cambios sobre esta. ROS es utilizado como middleware principal, el cual se
describe en la Sección 3.3.
Mientras está en ejecución el sistema autónomo es posible comunicarse con el computador mediante la interfaz Ethernet desde el interior del vehı́culo. Estando en el laboratorio,
el computador se conecta automáticamente a la red inalámbrica permitiéndole transferir
actualizaciones del código o datos capturados.
a Solid
State Drive, unidad de estado solido
30
Tabla 4.2: Especificaciones técnicas del computador ARK3440
Modelo
Procesador
Chipset
RAM
Disco duro
Ethernet
Wifi
Interfaz serial
Interfaz USB
Watchdog timer
Voltaje
Consumo tı́pico
Consumo máximo
Dimensiones
4.2.2.
ARK-3440
Intel i7 610E 2.53GHz
Intel QM57
4GB
SSD 64GB
2 x GigaE
Intel WiFi Link 5300
2 x RS232, 1 x RS232/422/485
6 x USB 2.0
timer 255 niveles, controlado por software
9[VDC] - 34[VDC]
38[W]
47.94[W]
220 x 102.5 x 200[mm]
Sensores
El vehı́culo tienen instalado diversos sensores que son usados para su navegación autónoma. Para suministrar la energı́a a los sensores se dispone de diferentes bus de poder. Cada
sensor tiene su propia manera de conectarse al sistema. A continuación se presenta una lista
los sensores instalados en el vehı́culo:
SICK LMS291-S05: Este sensor es un LIDAR muy utilizado en sistemas robóticos. En la
Tabla 4.3 se exponen las caracterı́sticas del sensor. Su popularidad esta esta dada por
su frecuencia de barrido, buen rango y robustez frente a la luz solar. También su costo
bajo con respecto a otros sensores le favoreció en su popularidad.
Tabla 4.3: Especificaciones técnicas SICK LMS-291
Modelo
Ángulo de escaneo
Frecuencia de escaneo
Resolución angular
Resolución radial
Error tı́pico
Rango
Temperatura de operación
Interfaz de comunicación
Voltaje
Consumo tı́pico
LMS291-S05
180°
75[Hz]
0.25° ,0.5° ,1.0°
10[mm]
+/-35[mm]
30[m] (10 % de reflectividad)/ 80[m](máximo)
0 a 50 [°C]
RS-422
24[VDC] +/-15 %
30[W]
Entre sus cualidades negativas está su baja precisión en distancia. Otra caracterı́stica
negativa es su tasa de transferencia de datos limitada, lo que le limita a no poder
entregar las distancias y las reflectancias simultáneamente.
31
SICK LMS151: Este sensor LIDAR es de uso industrial de menor rango que el LMS291,
pero con mayor ángulo de escaneo. En la Tabla 4.4 se muestran las caracterı́sticas del
LMS151.
Pese a ser de menor frecuencia de barrido que el LMS291, este LIDAR posee buena
resolución angular y resolución de distancia. Ası́ como su gran ángulo de escaneo le da
una gran versatilidad. Su interfaz ethernet le permite tener un mayor bandwidth comparado a otras interfases, logrando entregar las mediciones de distancia simultáneamente
que las de reflectancia, enriqueciendo las mediciones.
Tabla 4.4: Especificaciones técnicas SICK LMS151-10100
Modelo
Ángulo de escaneo
Frecuencia de escaneo
Resolución angular
Error tı́pico
Error sistemático
Rango
Temperatura de operación
Interfaz de comunicación
Voltaje
Consumo tı́pico
LMS151-10100
270°
50[Hz]
0.25° ,0.5°
+/-12[mm]
+/-30[mm]
18[m] (10 % de reflectividad)/ 50[m](máximo)
-30 a 50 [°C]
Ethernet
10.8 a 30[VDC]
8.4[W]
Tabla 4.5: Especificaciones técnicas SICK LD-MRS HD
Modelo
Ángulo de escaneo
Frecuencia de escaneo
Resolución angular horizontal
Separación entre capas
Resolución radial
Error tı́pico
Rango
Temperatura de operación
Interfaz de comunicación
Voltaje
Consumo tı́pico
LD-MRS-400102
85°
50[Hz]
0.125° ,0.25°, 0.5°
0.8° (3.2° en total)
40[mm]
+/-100[mm]
50[m] (10 % de reflectividad)
-40 a 70 [°C]
Ethernet
9 a 27[VDC]
8[W]
SICK LD-MRS HD: Este LIDAR posee caracterı́sticas muy especiales. La primera que
resalta es su capacidad de no solo disponer de una capa de barrido sino que cuatro
capas de barrido simultáneos. Además implementa tecnologı́a Multi-eco, de hasta 3
ecos. Esto le permite ser mas robusto frente condiciones climáticas adversas. Tiene un
tamaño reducido, y esta diseñado pensando en sistemas móviles. En la Tabla 4.5 se
muestran sus caracterı́sticas relevantes.
32
Este sensor además es muy compacto y liviano. Es capaz de entregar las detecciones
además de algunas banderas con información extra sobre la medición. Pese a todo esto
la practica ha mostrado que no es muy preciso ni constante detectando superficies del
terreno, su habilidad de tener 4 capas le permite discriminar obstáculos directamente.
Otra información relevante que entrega un sensor es la duración del eco recibido, la
cual es entregada en metros. Este valor tiene correlación con el ángulo de observación
de la superficie observada.
Crossbow NAV440: Es una INSa marca Crossbow que además incorpora un receptor GPS.
Esta unidad realiza la integración de las mediciones de los acelerómetros, giróscopos
y magnetómetros, para estimar el desplazamiento realizado. Esta fusión lo realiza internamente mediante un filtro de Filtro de Kalman Extendido (EKF). La utilización
del GPS le permite referenciar globalmente sus estimaciones. Algunas caracterı́sticas
técnicas son expuestas en la Tabla 4.6.
Tabla 4.6: Especificaciones técnicas Crossbow NAV440
Modelo
NAV440
Frecuencia de salida
100[Hz]
Precisión de orientación (pitch,roll) <0.4°
Precisión de orientación (yaw)
<1.0° rms (magnético)
Precisión de posición
<3[m]
Error velocidad angular
<10[°/h]
Temperatura de operación
-40 a 71 [°C]
Interfaz de comunicación
RS232
Voltaje
9 a 42[VDC]
Consumo tı́pico
<4[W]
Kvaser Leaf Light HS: Corresponde a una interfaz CAN instalada para que el computador
pueda recibir y transmitir información en un bus CAN. El bus al que esta conectado es
el bus CAN de automatización, y no es el powertrain CAN bus b del vehı́culo. En este
bus se publica mucha información referente al vehı́culo y hablar en el podrı́a afectar el
funcionamiento de este. El computador para obtener información del powertrain lo hace
mediante el gateway CAN. Este se conecta a ambos buses y retransmite los mensajes
de interés del powertrain al bus de automatización, sirviendo de filtro para no atender
tantos mensajes.
Esta interfaz se conecta al computador mediante USB, y es accedida mediante la librerı́a
canlib.
Radar Delphi ESR: Es un radar de uso automotriz, utilizado en vehı́culos comerciales. La
sigla ESR proviene de Electronically Scanning Radar c , esto implica que no tiene partes
móviles como los radares tradicionales, donde el barrido es un movimiento mecánico
a Inertial
Navigation System, sistema de navegación inercial
CAN principal de más bajo nivel del vehı́culo
c radar de barrido electrónico
b bus
33
de la antena. Posee dos lóbulos de detección, uno de angosto de 20° de apertura que le
permite medir hasta 174 [m] y uno ancho de 90° de apertura hasta 60 [m].
En su funcionamiento normal, es capaz de integrarse al bus CAN del vehı́culo y adquirir los datos necesarios para sus calculo, y controlar el sistema de velocidad del
vehı́culo, permitiéndole disminuir la velocidad o frenar. En la Tabla 4.7 se exponen sus
caracterı́sticas.
Tabla 4.7: Especificaciones técnicas Delphi ESR
Modelo
Delphi ESR
Frecuencia de salida
20[Hz]
Rango de velocidades
-100 a 25 [m/s]
Rango de distancias
60[m] / 174[m]
Apertura de detección
90° / 20°
Interfaz de comunicación CAN
Voltaje
12[VDC]
Consumo tı́pico
aprox 10[W]
Por el principio de funcionamiento del radar, internamente se obtiene información de
velocidad con mejor precisión que de posición. Internamente a estos objetos se les hace
seguimiento y la información entregada finalmente corresponde a detecciones de objetos
con su posición, velocidad y aceleración.
El radar esta conectado el bus CAN de automatización pero solo interactúa con el
computador, el cual le provee la información necesaria para funcionar.
Cámara de espectro visible: En el vehı́culo existen dos cámaras de espectro visible instaladas. La primera de ellas es la AVT Manta G-046C y la segunda corresponde a
una Sony Playstation Eye. En la Tabla 4.8 se muestran las comparativas entre sus
caracterı́sticas.
Tabla 4.8: Comparativa entre cámaras
Modelo
AVT Manta G-046C
Tipo de sensor
CCD
Máximo framerate
67[fps]
Máximo tamaño de imagen 780 x 580
Lente
8mm F=1.4D D=45°
Tipo de interfaz
GigaEthernet
Voltaje
8 a 30[VDC]
Sony Playstation Eye
CMOS
120[fps]
640 x 480
D= 56° o 75°
USB 2.0
USB
Originalmente la cámara Manta G-046C fue elegida para realizar detección de camino,
y su conveniente interfaz de conexión. La Playstation Eye fue agregada posteriormente,
para supervisión pero gracias a sus buenas caracterı́sticas y bajo costo se siguió utilizando. Posee un FOVa mayor y una sensibilidad elevada, con lo que puede obtener
imágenes con menor motion blur.
a Field
Of View, campo de visión. Corresponde ángulo solido sobre el cual el sensor captura información
34
4.3.
Discusión
En este Capı́tulo se han presentado las caracterı́sticas del vehı́culo y sus modificaciones,
los sensores y el sistema computacional. Estos elementos son la base sobre la cual se plantea
la metodologı́a de mapeo propuesta, la cual tiene caracterı́sticas muy particulares dadas
las limitaciones y restricciones impuestas por todo este sistema. En el capı́tulo siguiente se
expone la metodologı́a propuesta por esta Tesis Doctoral.
35
Capı́tulo 5
Metodologı́a de mapeo del terreno en
tiempo real
Índice
5.1. Observaciones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
5.1.1. Pipeline de las observaciones . . . . . . . . . . . . . . . . . . . . .
37
5.1.2. Extracción de caracterı́sticas . . . . . . . . . . . . . . . . . . . . .
39
5.2. Modelo cinemático del vehı́culo . . . . . . . . . . . . . . . . . . . . 43
5.3. Estimación del estado del vehı́culo . . . . . . . . . . . . . . . . . . 46
5.3.1. Estimación directa . . . . . . . . . . . . . . . . . . . . . . . . . . .
46
5.3.2. Estimación mediante EKF . . . . . . . . . . . . . . . . . . . . . . .
48
5.4. Proyección espacial de observaciones . . . . . . . . . . . . . . . . . 52
5.5. Fusión de Observaciones en mapa del terreno . . . . . . . . . . . 53
5.5.1. Nubes de puntos . . . . . . . . . . . . . . . . . . . . . . . . . . . .
54
5.5.2. Mapas de grilla . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
54
El proceso de mapeo del entorno propuesto consta principalmente de dos etapas: Una
primera etapa consiste en obtener una observación del entorno que represente alguna caracterı́stica relevante del entorno. Una segunda etapa consta en determinar espacialmente donde
se encuentra esta caracterı́stica. Dado el enfoque de mapeo de entorno que se está buscando
solucionar, esta segunda etapa consiste en determinar cuanto se ha desplazado el vehı́culo
desde la última observación y como ha cambiado su pose tridimensional. El mapeo de entorno propuesto no busca conseguir una coherencia espacial global como serı́a necesario para
aplicaciones de geomensura. En esta caracterı́stica se diferencia la metodologı́a propuesta del
SLAM. La coherencia local-temporal es la necesaria para una navegación apropiada y segura,
pero que cumpla con los requerimientos temporales presentados.
En este Capı́tulo se describirá el procesamiento realizado a las observaciones provenientes
de los sensores de distancia LIDAR. Se describirá también la metodologı́a para la estimación
del estado del vehı́culo, describiendo su modelo de movimiento y las diferentes opciones
propuestas para la estimación del movimiento de este. También se describirá de qué manera
es realizada la proyección de una observación a un marco de referencia “global”, ası́ como las
diferentes propuestas de medio de almacenaje e integración de estas observaciones.
5.1.
Observaciones
Como observación entenderemos la información relevante ubicada en el espacio que se
puede utilizar para identificar el entorno del robot. La principal fuente de estas observaciones
de este sistema proviene de los sensores LIDAR del vehı́culo (Sección 4.2.2). Estos sensores
contienen información espacial (x, y, z)t de la presencia de un objeto. Pero además de las
información espacial de cada medida, la relación entre ellas aporta información relevante.
Dependiendo del tipo de sensor, estos pueden aportar información extra recopilada por su
mecanismo de funcionamiento.
Dentro de los enfoques usados en mapeo, las observaciones son fusionadas espacialmente
y posteriormente caracterı́sticas son extraı́das de la fusión. Este enfoque funciona bien si el
resultados de esta extracción no es requerida con urgencia inmediata, por ejemplo en el caso
de la construcción de un mapa para ser usado posteriormente. Esta fusión y extracción de
caracterı́sticas de muchos datos suele ser costosa en tiempo, por lo que su utilización en una
aplicación en tiempo real suele quedar descartada, como suele ocurrir con los métodos de
SLAM tradicionales. Este enfoque además tiene en contra que requiere de la integración de
observaciones para la extracción de caracterı́sticas, agregando latencia a la extracción.
El enfoque propuesto aquı́ consiste en la utilización de caracterı́sticas pero solo de ı́ndole
local temporal y espacial. De esta manera la latencia del flujo de información es baja, y puede
ser utilizada en la toma de decisiones. Las caracterı́sticas propuestas sin caracterı́sticas de
baja carga de cómputo, que utilizan la información instantánea disponible y no requieren de
muchas observaciones integradas.
5.1.1.
Pipeline de las observaciones
El esquema de flujo de datos implementado para las observaciones corresponde a un
pipeline. En la Figura 5.1 se muestra un diagrama del flujo de datos y nodos involucrados
en el proceso, partiendo a la izquierda con la adquisición y terminando a la izquierda en la
utilización de las observaciones enriquecidas.
37
Las etapas relevantes del flujo son:
Nodo driver: Es el encargado de comunicarse directamente con el sensor a través de la
interfaz fı́sica. Controla su configuración, adquiere las observaciones, las interpreta para
generar el mensaje y las publica en su tópico correspondiente. Puede recibir mediante
llamadas a servicios u mensajes de control cambios en la configuración del sensor.
Hay que notar que diferentes modelos de sensores pueden entregar sus observaciones
en formatos diferentes y agregar información adicional propia del sensor en el mismo
mensaje.
Nodo proyección y enriquecimiento En este nodo se realiza la proyección espacial de la
observación del sensor LIDAR. Hasta el momento las observaciones están referenciadas al sistema de coordenadas centrado en el sensor, siendo necesario proyectarlas al
sistema de coordenadas local al vehı́culo. Esta proyección es realizada en coordenadas
cartesianas(x, y, z). Para realizar esta proyección es necesario disponer del estado del
vehı́culo para saber su pose con respecto al sistema de coordenadas local al vehı́culo.
Esta pose es obtenida de la estimación realizada que es explicada en la Sección 5.3.
Por sı́ sola esta proyección implica una perdida de información, por lo que además de
las coordenadas espaciales resultantes se agrega canales de datos con la información
necesaria para suplir esta perdida. A este proceso se le llama enriquecimientoa de las
observaciones.
Un ejemplo de la información perdida al realizar la proyección es la pose del sensor al
momento de la observación, lo que se refleja en la perdida de la distancia y ángulos en
que fueron observadas. El instante de tiempo en que fue observada también es relevante
y es incorporada al observación enriquecida.
También hay información proveniente de otras fuentes que es agregada. Un ejemplo de
esto es la información RGBb del punto espacial. Si el punto espacial es posible proyectarlo dentro de los margenes de una imagen observada por una cámara, la información
de color es asociada a la observación espacial. Esta proyección es explicada en la Sección
5.1.2.
Además dependiendo del modelo del sensor, existe información intrı́nseca propia de cada
sensor, como por ejemplo reflectancia de la observación, duración del pulso observado,
capa y eco de la observación, entre otras.
Además otras caracterı́sticas son agregadas a las observaciones. Algunas de estas caracterı́sticas requieren parámetros propios dependiendo del sensor utilizado, como algunas
no pueden ser extraı́das por la naturaleza del sensor. Estas caracterı́sticas son explicadas
con mayor detalle en la Sección 5.1.2.
La salida de este módulo es mensaje en formato de PointCloud multidimensional debido
a que contiene la proyección cartesiana, la información original para cada observación
y los canales con información agregada. Una de las cualidades de la implementación
realizada es su fácil extensión a nuevas caracterı́sticas si grandes modificaciones.
Nodo final: Corresponde al nodo que utilice la información de las caracterı́sticas generadas.
Esto puede ir desde la construcción de un mapa en base a ellas o en la generación de
a enhance
b Red,
Green, Blue
38
alertas, dependiendo del uso que se le de a la caracterı́stica extraı́da. También puede
corresponder a un clasificador que permita clasificar las observaciones enriquecidas bajo
alguna caracterı́sticas en particular utilizando la información agregada.
Figura 5.1: Esquema de flujo de las observaciones, desde el sensor a su utilización
5.1.2.
Extracción de caracterı́sticas
En esta sección se presentarán algunas de las caracterı́sticas o información agregada a las
observaciones.
Fusión láser-imagen
En la Sección 5.1.1, una de las tareas del nodo de proyección y enriquecimiento es el
agregar la información visual a la observación del sensor de distancia LIDAR. Para lograr
esto, se debe encontrar la relación de transformación existente entre el punto espacial de la
observación y el pixel correspondiente en la imagen, siendo posible que no exista. Uno de los
requerimientos para que esta transformación sea valida es que ambos puntos sean observables
desde ambos sensores, ya que un punto espacial puede proyectarse fuera de los margenes de
la imagen y en ese caso no hay información de color.
Para realizar la transformación se utiliza la implementación disponible en OpenCV [16] y
que ROS tiene integrada. Esta transformación esta basada en el modelo de cámara pin hole
más componentes polinomiales para compensar de deformación radial [2].
Las Ecuaciones (5.1) y (5.2) corresponden a la proyección del modelo pin hole sin considerar el modelo de distorsión del lente, donde (u, v) corresponden a la posición en pı́xeles
en la imagen, (X, Y, Z) son los puntos espaciales a proyectar en coordenadas cartesianas, A
es la matriz de parámetros geométricos intrı́nsecos de la cámara y R|t son los parámetros
extrı́nsecos de la cámara que corresponden a la rotación y traslación espacial de esta.
m0 = A · [R|t] · M 0
39
(5.1)
 
u

fx
  
v  =  0
1
0
0
fy
0
X
cx
r1 1 r1 2 r1 3 t1  

 Y 
cy 
 · r2 1 r2 2 r2 3 t2  ·  
Z 
1
r3 1 r3 2 r3 3 t3
1
 



(5.2)
El procedimiento de obtención de los parámetros intrı́nsecos y deformación radial de la
cámara es llamado procedimiento de calibración. En [58,59] es propuesto un procedimiento de
calibración mediante un patrón de geometrı́a conocido (originalmente era utilizado un patrón
de cuadrados, pero evolucionó en un chessboard a ) que es mostrado desde varias perspectivas y
en todo el campo visual de la cámara (Figura 5.2). A partir de estas imágenes son calculados
los parámetros geométricos y el patrón de distorsión.
Figura 5.2: Ejemplo de calibración una cámara mediante un chessboard [2]
Esta proyección no es biyectiva, mientras un punto espacial solo tienen un pı́xel donde proyectarse, por la naturaleza plana de una imagen, un pı́xel proyectado en el espacio
corresponde a un rayob , por lo que un pı́xel corresponde infinitos puntos. El tı́pico uso de
esta proyección es “dibujar” la observación espacial en la imagen y corregir la distorsión de
la imagen, pero en este caso se realiza al revés, el pı́xel es “dibujado” en el punto al agregar su información de color a la ubicación espacial. Como la ubicación en pı́xeles es una
discretización, el color es determinado mediante técnicas de sub-pixel.
Debido a que los sensores, LIDAR y cámara, no tienen la misma área de cobertura,
puede ser que como resultado de la proyección se obtenga un pı́xel no existente en la imagen.
Además, en un esquema en que los mensajes de información llegan de manera ası́ncrona, la
imagen y el láser no tienen el mismo timestamp, por lo que hay que tener en consideración
este desplazamiento extra. Como la ubicación del vehı́culo es conocida en tiempos anteriores,
el desplazamiento de los puntos espaciales del LIDAR son agregados al desplazamiento del
vehı́culo al proyectarla en la imagen. Como la sincronización está marcada por la recepción
del mensaje de distancia del sensor LIDAR, un buffer de la imagen debe ser guarda para
a tablero
b rayo
de ajedrez
o semi-recta, es una recta que tiene un origen y se extiende hasta el infinito
40
extraer el color en un tiempo posterior, la proyección es realizada sobre la imagen en la
ubicación espacial correspondiente a su timestamp.
En la implementación realizada, en caso que el resultado de la proyección localice al
punto fuera de la imagen, se le asignará un color negro (RGB = (0, 0, 0)) pero con canal de
transparencia (alpha) igual a cero, para marcar la diferencia de un punto de color negro, y
además se marca un indicador de que esa observación no fue pintada.
Puntos de quiebres
Los puntos de quiebresa corresponden a puntos donde mediciones LIDAR sucesivas se
pueden clasificar como interrumpidas. Estas interrupciones pueden estar dadas por sombras
de objetos mas cercanos o por cantos del objeto observado. Estos puntos permiten definir
agrupaciones de puntosb , donde una grupo de observaciones contiguas deberı́an ir de punto
de quiebre a otro.
k(r, φ)l − (r, φ)l−1 )k > ri−1 ·
sin∆φ
+ 3σr
sin (λ − ∆φ)
(5.3)
Para la implementación se utilizo lo propuesto en [28], donde se utilizan caracterı́sticas
geométricas e intrı́nsecas al sistema de medición LIDAR rotacional. En la Ecuación 5.3 se
expresa como se discrimina un punto de quiebre. (r, φ)l corresponde a la observación i, ∆φ
es el paso angular entre mediciones, σr es la desviación estándar de la medición de distancia
propia del sensor y λ es un parámetro de ajuste de máximo separación entre medidas. Este
enfoque es debido a que dependiendo de la distancia de la observación depende de cuanto es
necesario considerar como una ruptura de las observaciones.
Puntos de adyacentes y cúmulos
Los puntos adyacentes son obtenidos a partir de los puntos de quiebres explicados anteriormente. Corresponde a cuantos puntos hay hasta el siguiente punto de quiebre, esto hacia
la derecha y la izquierda. Esto permite saber si hay que considerar vecindades de los puntos
sin tener que explorar el arreglo de observaciones.
Los cúmulos es una etiqueta agregada a los datos que identifican agrupaciones de observaciones contiguas. Donde el número de observaciones de un cúmulo corresponden a los
adyacentes hacia ambas direcciones más uno. Estas etiquetas son incrementales desde izquierda a derecha, partiendo de 0. De esta manera si es necesario saber cuantos cúmulos hay solo
basta buscar el extremo derecho.
Índice de curvatura
El ı́ndice de curvatura corresponde a una estimación de la curvatura entre observaciones sucesivas. Esto permite obtener información geométrica o de como están dispersas las
observaciones dentro de un cúmulo. La Ecuación 5.4 expresa el cálculo de este ı́ndice.
indiceCurvaturai =
p
~lef t − p~i × p~right − p~i p
~lef t − p~right a breakpoints
b clusters
41
(5.4)
Este ı́ndice esta basado en el calculo de áreas de triángulos utilizando producto cruz
vectorial. Para cada punto es calculado p~i es buscado un p~lef t como el mı́nimo entre sus
adyacentes hacia la izquierda y un número fijo que representa la máxima diferencia angular
de las medias a tomas. Análogamente se define también p~right .
Un mayor valor de este ı́ndice permite determinar cuando un cúmulo de observaciones
presenta un cambio de dirección como puede ser en una solera o una esquina en una pared por
ejemplo. En caso de ser bajo este ı́ndice, indica una alineación de las observaciones. Hay que
tener en cuenta que debido a la precisión de las mediciones de distancia, a distancias cercanas
este indicador serás más ruidoso por que la relación de distancia entre puntos adyacentes y
distancias observadas al sensor.
Flags
Flags o banderas corresponde a una estructura de datos que permite almacenar información binaria pertinente a una observación. Esta flag permite recuperar de manera directa si
una observación tiene o no registra alguno de las caracterı́sticas.
También en esta estructura se almacenan algunas de las caracterı́sticas entregadas por
algunos sensores en particular. Por ejemplo el LDMRS, entrega si una observación es transparente (existe algún eco detrás de esta observación) o si es ruido meteorológico. También si
un la observación tiene incorporada color es registrado en esta estructura.
Toda mensaje de observaciones tiene agregada este canal, ya que se utiliza como canal de
control.
Índices
Corresponde a un número entero que permite recorrer mediciones validas de manera
contigua. En un sensor LIDAR de una capa no es mucho aporte, pero para un sensor multieco como es el caso del sensor LDMRS, permite recorrer las observaciones por capas, ası́ como
también se le agrega un ı́ndice para recorrer las observaciones de manera vertical.
Básicamente estos ı́ndices están implementando un mapeo para recorrer de manera ordenada las observaciones.
Caracterı́sticas del sensor LDMRS
Las mas relevantes caracterı́sticas que entrega únicamente el sensor LDMRS corresponde
a: DeltaTime, EchoWidth, Layer, Echo y Transparent.
DeltaTime corresponde a cuanto tiempo a transcurrido desde que se empezó a realizar
una medición. Esto permite saber exactamente en que momento se observo la medición. Otros
sensores estos tiempos los consideran constantes y solo dependientes del ángulo, pero como
este sensor tiene varias capas, estos tiempos se entregan explı́citamente.
EchoWidth es una medición de distancia de cuanto tiempo se observo un eco. Indirectamente significa el tiempo de duración del eco, pero se entrega en unidades de metro. Este
permite saber que tan perpendicular fue el rebote del láser sobre la superficie.
Layer y Echo son la información relevante por ser un sensor multi-capa y multi-eco.
Identifica que a que eco corresponde y en que capa fue observado. Una observación no es
Transparent si es que es el ultimo eco percibido.
42
Caracterı́sticas del sensor LM151
La única información extra que entrega este sensor es la intensidad de la reflectancia
percibida de vuelta. Esto permite interpretar en cierta medida el “color” en infrarojo del
material sobre el que reboto la observación.
Existen metodologı́as para con esta información realizar correcciones de distancias. Por
ejemplo, al observar la reflectancia es posible discriminar entre pavimento, pintura de calle,
pasto, vehı́culos.
5.2.
Modelo cinemático del vehı́culo
El modelo de movimiento del vehı́culo planteado corresponde a la de un vehı́culo con
cuatro ruedas, de las cuales dos son orientables para cambiar la dirección del movimiento.
Esta orientación de cada ruedas debe ser coordinado para obtener un movimiento adecuado.
El ángulo de las ruedas deben cumplir con la condición de Ackermann [17]. Esta condición
geométrica representada en forma de ecuación se expresa en (5.5) y corresponde a que todas
las ruedas deben tener el mismo centro de giro como se muestra en la Figura 5.3. track
corresponde a las distancia entre las ruedas frontales, wheelbase es la distancia entre eje
trasero y eje delantero, y O es el centro de rotación.
cot (α)o − cot (α)i =
track
wheelbase
(5.5)
Figura 5.3: Visualización gráfica de la condición de Ackermann
La condición de Ackermann es necesaria para tener un movimiento sin desplazamiento
lateral de las ruedas bajo condiciones estáticas o bajas velocidades. Bajo estas condiciones no
hay fuerzas laterales en las ruedas. En una implementación real de un sistema de dirección
de un vehı́culo no siempre se cumple la condición de Ackermann en todo su recorrido, ya que
condiciones más dinámicas es requerido otro comportamiento intencionalmente. Considerar la
43
existencia de un único radio de rotación del vehı́culo al desplazarse es una buena aproximación
como modelo cinemático.
El movimiento del vehı́culo está definido para este modelo como un movimiento circular
uniforme instantáneo en un plano, del cual el radio de giro es modificado mediante el ángulo
del volante. El desplazamiento del vehı́culo está dado por la velocidad de este. En un esquema
de vehı́culo autónomo, la referencia de entrada a la dirección y a la velocidad es conocida,
pero cuando lo conduce una persona no, por esta razón se decidió no utilizar las referencias
de entradas y solo los valores observados del vehı́culo. El modelo de movimiento se implementó pensando tener un sistema de uso general, sin considerar el comportamiento interno
del vehı́culo y sólo basarse en su movimiento, y no depender del funcionamiento interno que
no siempre esta disponible.
De la condición de Ackermann se puede deducir también que el movimiento de cada rueda
no es simétrico al momento del virar a la izquierda o a la derecha, por lo que el ángulo de
una rueda no es un buen ángulo a considerar para definir el estado del vehı́culo. Si se hace
un modelo de bicicleta equivalente para el vehı́culo, esto es considerar una rueda “virtual”
al centro del eje delantero, que cumpla con la condición de Ackermann, y considerar ese
ángulo como el ángulo de dirección. En la Figura 5.4 se aprecia el modelo equivalente y del
cumplimiento de la condición de Ackermann se obtiene la Ecuación (5.6), que es la relación
existente entre los tres ángulos anteriores.
cot (α) =
cot αi + cot αo
2
(5.6)
Figura 5.4: Modelo equivalente de bicicleta y radio de giro
El ángulo α no es medible en un vehı́culo real por que no existe fı́sicamente como tal. Lo
mismo ocurre con el radio de giro. El ángulo del volante αwheel si existe, sin embargo la relación de este ángulo con α no es conocida a priori pero deberı́a ser cercana a lineal. Geométricamente, a partir de la Figura 5.4 es posible deducir que existe una relación geométrica entre
α, wheelbase y el r, dado que el angulo α se repite entre las ruedas y el centro de rotación.
Esta relación se expresa en la Ecuación (5.7).
sin (α) =
wheelbase
r
44
(5.7)
Pese a que la implementación de la dirección del vehı́culo αwheel no es lineal con respecto
α [17], pero se busca una aproximación lineal a esta relación con la forma de la Ecuación
(5.8), donde β es una constante real.
(5.8)
α ≈ β · αwheel
Mediante datos experimentales de conducción del vehı́culo se puede encontrar una relación entre estos datos, pero α no es posible medirlo directamente, por lo que se busca un
método indirecto para su estimación. De la relación entre r y α, en conjunto con una aproximación para aceleraciones bajas proveniente de un movimiento circular uniforme vt = ωt · rt
que corresponde a la velocidad lineal para movimientos circulares sin aceleraciones. Esta
aproximación suficientemente válida para casos de bajas aceleraciones de vt . De lo anterior
es posible despejar una relación para ω y α (Ecuación (5.9)).
vt · sin (αt )
(5.9)
wheelbase
Si además se hace notar que α esta acotado en un rango pequeño, es posible aproximarlo
como α ≈ sin (α) y de esta manera se llega a la expresión de la Ecuación (5.10). Con γ una
constante real.
ωt =
ωt ≈
vt · β · αtwheel
≈ vt · αtwheel · γ
wheelbase
γ · αtwheel =
(5.10)
ωt
vt
(5.11)
0.15
ωt rad
[
]
νt m
0.1
0.05
0
−0.05
−0.1
−4
−3
−2
−1
0
1
2
3
4
5
6
αtwheel [rad]
Figura 5.5: Discretización de datos para la estimación de γ
En base a la Ecuación (5.10) y mediante datos experimentales de conducción, utilizando
ωzt como ωt que es la velocidad angular en el eje z de la IMU y vt y αtwheel proveniente
directamente del bus CAN del vehı́culo, de esta manera la Ecuación (5.10) es transformada
en la Ecuación (5.11). Como los datos para la estimación son numerosos y no tienen una
distribución uniforme con respecto al ángulo en la representación de αwheel y además tienen
error asociados a la medición por no ser medidos con vt constante, la estimación se realiza
sobre una simplificación de los datos. Los valores de αwheel son discretizados y a los valores
de ωt y vt se le calcula la media para cada celda de discretización de αwheel . El gráfico en la
Figura 5.5 muestra estos datos sobre un recorrido de αwheel .
45
En el gráfico en la Figura 5.5 se expresa la Ecuación (5.11), el gráfico expresa la relación
entre γ · αwheel y αwheel y esto implica que realizando una regresión lineal sobre estos datos
es posible determinar γ. Buscando una linealización de la forma: y(x) = κ · x, se determina
que κ = 0,02359[1/m], con un R2 = 0,9992. En el gráfico en la Figura 5.6 se muestra los datos
utilizados en la regresión y la linealización obtenida.
0.1
angulos positivos
angulos negativos
aproximacion lineal
0.08
0.06
ωt rad
[
]
νt m
0.04
0.02
0
−0.02
−0.04
−0.06
−3
−2
−1
0
1
2
3
4
5
αtwheel [rad]
Figura 5.6: Datos sobre los que fue realizada la regresión y la linealización resultante
Además de poder determinar γ, también se logra determinar β, resultando en una relación
de reducción de αwheel a α de 16,27 : 1. Este valor es cercano del rango tı́pico para este tipo
de vehı́culos, como lo expresa Reza Jazar en su libro [17].
5.3.
Estimación del estado del vehı́culo
Como se explicó anteriormente, la estimación de cuanto se ha desplazado y como ha
cambiado la orientación del vehı́culo de un momento a otro es una de las bases para poder
integrar las observaciones de un terreno.
La primera tarea consiste en definir que es el “estado” o al menos que es lo que se busca que
contenga. En este caso se entenderá como estado a las variables necesarias para poder definir
la posición y orientación del vehı́culo, y las condiciones internas del vehı́culo que permitan
determinar una nueva posición y una nueva orientación en un ∆t en el caso de no cambiar
las condiciones del vehı́culo. De esta manera se define un estado mı́nimo como el estado que
contiene las coordenadas espaciales del vehı́culo p~, la orientación ~q y velocidad lineal v. Siendo
h
iT
h
iT
p~ = x y z la posición tridimensional en el eje de referencia fijo y ~q = qw qx qy qz
el cuaternión con la orientación del vehı́culo.
Se proponen dos estimaciones de estado, las cuales tienen enfoques distintos y se utilizarán
para comparar el rendimiento del sistema.
5.3.1.
Estimación directa
Esta estimación del estado ~x está basada en la utilización directa de la información proveniente del vehı́culo y de la IMU sin ningún filtro extra. El nombre estimación directa o
pseudo-estado viene del hecho que los datos se utilizan directamente y no mediante un filtrado o procesamiento extra, por lo que tampoco tiene etapa predictiva. Solamente el vector p~
es calculado como la integración de la velocidad orientada con ~q proveniente de la IMU.
46
El vector ~x está conformado de la siguiente forma:
h
~xt = p~t ~qt vt
iT
(5.12)
La actualización de la posición p~t es calculada como:
p~t = p~t−1 + ~qt ◦ ∆~pt ◦ ~qt?
(5.13)
Con ∆~pt definido como el desplazamiento del vehı́culo siguiendo una lı́nea recta durante
∆t (Ecuación (5.14)). Este modelo de desplazamiento está basado en que es posible aproximar
la trayectoria por un ∆~pt recto y después una rotación. La frecuencia a la que se calcula este
estado es del orden de 300[Hz]. A esta frecuencia se calculan pequeños desplazamientos por
lo que la aproximación tiene validez para el rango de velocidades que se utiliza. Además
esta frecuencia está sobre las frecuencias de las observaciones de los sensores evitando tener
dos observaciones consecutivas sin cálculo de desplazamiento entre ellas. En la Figura 5.7 se
muestra el concepto de esta aproximación.
h
∆~pt = v · ∆t 0 0
iT
(5.14)
Figura 5.7: Aproximación de trayectoria en caso del estado directo
Además ~qt es calculado directamente a partir de la orientación entregada por la IMU, pero
los valores vienen como pitch a , roll b y yaw c . Por eso es necesario realizar una conversión
a la representación como cuaternión mediante la Ecuación (5.15). Esta ecuación utiliza la
conversión definida por la función definida en la Ecuación (5.16).
a cabeceo
b alabeo
c guiñada
47
U IM U
)
, θt
~qt = qY P R (ψtIM U , φIM
t
(5.15)
cos ( 2θ ) · cos ( φ2 ) · cos ( ψ2 ) + sin ( 2θ ) · sin ( φ2 ) · sin ( ψ2 )


sin ( θ ) · cos ( φ ) · cos ( ψ ) − cos ( θ ) · sin ( φ ) · sin ( ψ )
2
2
2
2
2
2


qY P R (ψ, φ, θ) = 
φ
ψ
φ
ψ 
θ
θ
)
·
sin
(
)
·
cos
(
)
+
sin
(
)
·
cos
(
)
·
sin
(
)
cos
(

2
2
2
2
2
2 
cos ( 2θ ) · cos ( φ2 ) · sin ( ψ2 ) − sin ( 2θ ) · sin ( φ2 ) · cos ( ψ2 )

5.3.2.

(5.16)
Estimación mediante EKF
El otro método de estimación del estado es mediante un filtro de Kalman Extendido [53],
el que fusiona la información proveniente de las diferentes fuentes y genera un estado.
El vector de estado ~xt incluye todas las variables mı́nimas que definen el estado del
vehı́culo, agregando algunas nuevas:
h
~xt = p~t ~qt vt αtwheel ηt
iT
(5.17)
Con p~t siendo un vector 3D representando la posición de relativa de SL relativo a SG , ~qt
es un cuaternión representado la orientación de SL a relativo a SG b , vt es un escalar representando la rapidez del vehı́culo, αwheel es un escalar representando el ángulo del volante y ηt
la varianza de la vibración del vehı́culo en el eje z del vehı́culo. El problema de la estimación
es resuelta en este caso con la implementación de un filtro de kalman extendido o EKF por
su sigla en ingles. El estado αtwheel es agregado para la etapa predictiva y ηt para la etapa
de fusión de datos. El ηt solo puede estar presente en este estimador por que es una estimación generada por la etapa correctiva, por lo que en el caso de la estimación directa no esta
presente.
A continuación se exponen las etapas predictivas y correctivas del filtro.
Etapa predictiva
En la etapa predictiva son usadas las ecuaciones tı́picas de un EKF :
~xt = f (~xt−1 , u~t , 0)
Pt− = At · Pt−1 · ATt + Q
(5.18)
Con At la matriz jacobiana de las derivadas parciales de f () con respecto a ~xt , Q es la
matriz de covarianza del ruido del proceso y u~t es la orden de actuación. El ruido del proceso
es caracterizado y se define Q.
En el modelo, u~t es considerada como desconocida, aún cuando u~t puede ser conocida
para el caso de conducción autónoma. Esto es busca considerar el caso en que otro sistema,
por ejemplo una persona, realice la conducción.
La función f (·) es definida por las Ecuaciones (5.19)
a sistema
b sistema
de referencia local
de referencia global
48
?
p~−
~t−1 + ~qt−1 ◦ ∆~pt ◦ ~qt−1
t =p
~qt− = ~qt−1 ◦ ∆~qt
vt− = vt−1
−
αtwheel
ηt−
(5.19)
wheel
= αt−1
= ηt−1
Con ∆~pt como la variación tridimensional en el eje de referencia SLt con respecto a SLt−1 ,y
∆~qt la variación de la orientación de SLt con respeto a SLt−1 , siendo ∆~qt un cuaternión.
El cambio de la posición del vehı́culo, en el sistema de referencia local SL , es calculado
de dos maneras dependiendo del valor de αtwheel .
Si αtwheel = 0:
vt · ∆t

∆~pt = 
 0 
0


(5.20)
Y si αtwheel , 0:
rt · sin (ωt · ∆t)


∆~pt = rt · (1 − cos (ωt · ∆t))
0


(5.21)
Con rt una estimación del radio de rotación, ωt la velocidad angular en torno al eje z de
SL y ∆t el intervalo de tiempo desde la última actualización del filtro.
La trayectoria del vehı́culo en ∆t es aproximada como circular. En la Figura 5.8 se muestra
un esquema de la aproximación de trayectorias por arcos de cı́rculos. ωt y rt son aproximados
usando la estimación de αwheel y vt en la Ecuación (5.22) y en la Ecuación (5.23).
ωt = vt · αtwheel · γ
rt =
1
vt
= wheel
ωt αt
·γ
(5.22)
(5.23)
Con γ el factor de ajuste que depende del vehı́culo calculado en la sección 5.2. ∆~pt
está definido por dos casos de αwheel , en el caso de αtwheel = 0 provoca que rt = ∞, y en este
caso el limite de la Ecuación (5.21) corresponde a la Ecuación (5.20).
La diferencia de la pose angular ∆~qt es calculada como:
∆~qt = qY P R (ωt · ∆t, 0, 0)
(5.24)
Con qY P R dada como la función expresada en la Ecuación (5.16) que convierte una orientación expresada con ángulos yaw, pitch y roll en su representación como cuaternión.
El resto de los elementos del vector de estado no son actualizados en la etapa predictiva,
por lo que mantienen el valor actual.
49
Figura 5.8: Aproximación de trayectoria en caso de la estimación del estado mediante EKF,
implementada mediante arcos de cı́rculos sucesivos
Etapa correctiva
A continuación se muestran las ecuaciones tı́picas para la etapa correctiva de un EKF.
Kt = Pt− HtT (Ht Pt− HtT + Rt )−1
~xt = ~x−
x−
t + Kt (zt − h(~
t ))
(5.25)
Pt = (I − Kt Ht )Pt−
Con Ht la matriz Jacobiana de derivadas parciales de h(·) con respecto a ~x−
t , Rt la matriz
de covarianzas del ruido de medición y Kt la ganancia de Kalman.
La corrección es realizada de manera secuencial usando información perceptual obtenida
de la IMU y los sensores internos del vehı́culo, ası́ como también el uso de las observaciones
virtuales obtenidas del mapa de elevaciones, el cual es considerado como un soft-sensor.
En el modelo se eligió transformar las diferentes mediciones (por ejemplo, la velocidad
angular entorno al eje z) de los sensores al espacio del estado del filtro, y de esta manera tener
una linealización exacta para la operación de este. Ası́, la función h(·) es la función identidad y
Ht una matriz identidad. Esta transformación mantiene los efectos de las estadı́sticas de alto
orden, mejorando el desempeño del filtro de Kalman [22]. Naturalmente, esto significa que las
matrices Rt correspondiente a cada fuente de información deben ser estimadas exactamente
para tener una fusión basada en filtrado de Kalman adecuada. Las matrices de covariancia del
ruido de las mediciones correspondientes a la IMU y mediciones del vehı́culo son invariantes en
el tiempo, y son denotadas como RIM U y Rvehicle respectivamente. La matriz de la covarianza
del ruido asociada a la medición virtual, proveniente del mapa de elevaciones, es denotado
como Rtmap y es cambiante en el tiempo.
50
La IMU provee observaciones que permiten actualizar los componentes de la pose, ángulo
del volante y varianza de la vibración del vector de observaciones. Las medidas de orientación
U y θ IM U son usadas directamente para actualizar ~
qtIM U mediante qY P R definido
ψtIM U , φIM
t
t
como la función 5.16.
U IM U
~qtIM U = qY P R (ψtIM U , φIM
, θt
)
t
(5.26)
Despejando αwheel de la ecuación (5.22),y siendo la observación del ángulo del volante
obtenido de la velocidad angular entorno al eje z proveniente de la IMU ωtz se obtiene:
αtIM U =
ωtz
vt · γ
(5.27)
La cuantificación de la vibración del vehı́culo en el eje z es estimada como la varianza de
la aceleración en el eje z de la IMU:
Nf −1
ηtIM U
=
X
i=0
(azt−i − āzt )2
Nf
(5.28)
Con Nf el número de muestras sobre las que se calcula, y āzt el valor promedio de la
aceleración sobre las Nf muestras.
~ IM U queda definido como:
Finalmente Z
t

∗

 IM U 

~
q

 t
IM U
~

Zt
= ∗ 

 IM U 
α

 t
(5.29)
ηtIM U
Donde ∗ corresponde a componentes que no se actualizan y se reemplazan por las ya
estimadas en el estado.
El vehı́culo provee mediciones que permiten actualizar la componente de rapidez y el ángu~ vehicle . Dado que los sensores internos del vehı́culo
lo del volante del vector de observaciones Z
t
~ vehicle está compuesto por
proveen la velocidad de las cuatro ruedas y el ángulo del volante, Z
t
los siguientes componentes:
∗


∗



~ tvehicle =  1 (v vehicle,rl + v vehicle,rr )

Z
2 t

t


vehicle


αt
∗


(5.30)
Donde vtvehicle,rl es la velocidad de la rueda trasera izquierda y vtvehicle,rr es la velocidad
de la rueda trasera derecha.
El mapa de elevaciones provee observaciones virtuales que permiten actualizar la pose
fl
fr
rr
~ map . Sean hrl
del vector observacional Z
t , ht , ht y ht las alturas de las ruedas traserat
izquierda, trasera-derecha, frontal-izquierda y frontal-derecha respectivamente. Estas alturas
son obtenidas evaluando el mapa del terreno (mapheight [·] [·], arreglo definido en Sección 5.5.2)
en las coordenadas normalizadas de los puntos de contacto de las ruedas con la superficie
51
estimada. Ası́, los ángulos de rotación entorno al eje x e y del vehı́culo están dados por la
Ecuación (5.31). En la Figura 5.9 se muestran los ángulos reflejados en el vehı́culo.


φmap
= arcsin
t
h̄rear − h̄ft ront 
 t
wheelbase


h̄lef t − h̄right
t

θtmap = arcsin  t
track
(5.31)
lef t
right
rr
Con h̄ft ront = (hft l + hft r )/2, h̄rear
= (hrl
= (hft l + hrl
= (hft r +
t
t + ht )/2, h̄t
t )/2 y h̄t
hrr
t )/2.
~ map estará dado como:
Ası́ Z
t


∗

map map 
qY P R (ψt , φt , θt
)


map
~


∗
Zt
=



∗


∗
(5.32)
Con qY P R dado por la Ecuación (5.16), y donde ψt corresponde al valor estimado actual
de yaw de SL con respecto a SG .
La matriz de la covarianza del ruido asociada a la medición virtual que provee el mapa
de elevaciones Rtmap es calculada como:
0
0

map
0 Rt,q

Rtmap = 
0
0

0
0
0
0

0
0
0
0
0
0
0
0
0
0
0

0

0

0

0

(5.33)
map
Con Rt,q
definido como:
1
0
map
Rt,q
=

0
0

0
1
0
0
0
0
1
0
0
0

fl fr
rr
 · max χt , χt , χrl
,
χ
t
t
0
1

(5.34)
rr
Donde χft l , χft r , χrl
t y χt son las varianzas asociadas a las alturas de las ruedas frontalizquierda, frontal-derecha, trasera-izquierda y trasera-derecha, respectivamente. Estos valores
son obtenidos al evaluar las coordenadas normalizadas x e y de los puntos de contacto de las
ruedas con el piso en el mapa de varianzas (mapvar [·] [·] arreglo definido en la Sección 5.5.2).
Las matrices de la covarianza del ruido RIM U y Rvehicle son estimadas en un proceso de
calibración estándar.
5.4.
Proyección espacial de observaciones
Debido al principio de funciones de un LIDAR, las observaciones son expresadas en coordenadas polares relativas al sensor LIDAR (Sección 3.5), donde cada observación esta compuesta por la distancia dt y el ángulo θt , y cada conjunto de observaciones tiene Nsamples .
52
Figura 5.9: Estimación de Pitch y Roll proveniente de las alturas de las ruedas
Con el fin de fusionar las mediciones del sensor adquiridas en diferentes instantes de tiempo,
de uno o más sensores, las observaciones son primero transformadas a coordenadas cartesianas y después proyectadas en el sistema local de referencia SL , y finalmente sobre el sistema
global SG . Para la primera proyección, es necesario conocer la pose del sensor en SL , la cual
SL
L
es dada por un vector de posición tridimensional p~Slaser
y un cuaternión de orientación ~qlaser
.
La pose del vehı́culo, presente en el estado ~xt , es utilizada para la segunda proyección. Ası́,
las mediciones LIDAR ρ~St,iG ∈ R3 son obtenidas como:
h
ρ~laser
= 0 dt,i · cos (θt,i ) dt,i · sin (θt,i ) 0
t,i
SG
SG
ρ~St,iG = xSt,iG yt,i
zt,i
h
iT
iT
SL
SL ?
L
= ~qt ◦ ~qlaser
◦ ρ~laser
◦ ~qlaser
+ p~Slaser
◦ ~qt? + p~t
t,i
n
(5.35)
(5.36)
o
Donde i ∈ 1, ..., Nsamples . La ecuación (5.35) es la forma “quaterniorizada” del vector
SL
L
de orientación. La posición p~Slaser
y la orientación ~qlaser
corresponden a la pose del sensor en
el sistema de referencia SL , la cual no cambia en el tiempo al ser una montura fija.
Para cada medición LIDAR i, la varianza de la medida χt,i es estimada. Los dos principales
factores considerados para esta estimación son la distancia medida dt,i y la varianza de las
vibraciones en el eje z ηt definida en la Sección 5.3.2. Entonces, la estimación para χt,i
propuesta está definida como:
χt,i = κd · d2t,i + κη · ηt + κbase
(5.37)
Donde κd , κη y κbase (0,04, 0,5 y 0,1, respectivamente) son factores de ajustes calculados
mediante una minimización sobre un set de datos de una superficie lisa a distintas velocidades.
κd corresponde al factor aportado por la distancia de la observación, κη corresponde al aporte
dado por las vibraciones al momento de observación y κbase es el aporte intrı́nseco de la
medición aportado por las caracterı́sticas del sensor involucrado.
5.5.
Fusión de Observaciones en mapa del terreno
Una vez obtenida la observación en el sistema de referencia SG , la siguiente etapa consiste en almacenarla de alguna manera útil. Para almacenarla es necesario disponer de una
53
estructura que sea capaz de guardar la información espacial y otras caracterı́sticas de las
observaciones, y también poder recuperar estas. A continuación se presentan algunas de las
estructuras que se han utilizado para almacenar estas observaciones ası́ como algunas de sus
caracterı́sticas fundamentales.
5.5.1.
Nubes de puntos
Es la manera más intuitiva de almacenar las observaciones, y consiste en almacenar cada
una de las observaciones. Básicamente, son arreglos gigantes que almacenan en la información
de posición cartesiana. En la Figura 5.10 se muestra un ejemplo de una nube de puntos de
observaciones de sensores LIDAR.
Figura 5.10: Ejemplo de nube de puntos parcial de sensor láser LMS-151, Parque O’Higgins
El principal problema con las nubes de puntos es que requieren “grandes espacios de
memoria” para guardar tal volumen de información y normalmente no están organizadas
para búsqueda o procesamiento de los datos. Información relevante sobre su vecindad con
otros puntos se puede utilizar para lograr mejoras a los sistemas de almacenamientos de
nubes de puntos que permiten guardar de manera más eficiente estos datos, sin embargo
son omitidos en primera instancia. Aun ası́, los volúmenes de información siguen siendo muy
elevados, haciendo que en la practica, realizar procesamiento en tiempo real sea una tarea
difı́cil. Es por esta razón que se busca simplificar los datos antes de realizar cualquier tipo de
procesamiento.
5.5.2.
Mapas de grilla
El fundamento para implementar una estructura para mapas basado en grillas es su
versatilidad al momento de almacenar diferentes tipos de información en la celda, ası́ como
las caracterı́sticas geométricas de la estructura de grilla. Es posible acceder de manera directa
a una celda y encontrar relaciones geométricas de esta con sus vecinos.
La estructura propuesta tiene como fundamento que las áreas observadas no son muy
extensas entorno al vehı́culo y el recorrido del vehı́culo no cubre completamente toda la
54
superficie dejando muchos espacios sin observaciones. Un ejemplo claro de esto son las calles
en una ciudad donde el área de las calles es menor que el área de las cuadras. Por eso
la estructura propuesta corresponde a una grilla de grillas que son inicializadas de manera
dinámica y que también permite su eliminación. La abstracción de esta estructura de datos
permite acceder a la información de cada celda en base a su posición cartesiana con respecto
a un eje de coordenadas arbitrario, permitiendo extraer la información de cualquier punto
espacial directamente.
La implementación de la estructura de grilla esta compuesta por tres principales elementos: contenedor de grillas, contenedor de celdas y las celdas.
El contenedor de grillas corresponde a una estructura de datos donde se almacenan grillas,
que son generadas a medida que una celda es accedida. Cada grilla es almacenada indexada
con el valor de una función hash de la posición espacial (x, y). Cada grilla cubre una área
que corresponde un cuadrado de lado Mceldas · ∆celda , donde ∆celda es el tamaño de la celda
y Mceldas es el número de celdas por lado que tienen una grilla.
El contenedor de celdas corresponde a una matriz cuadrada de tamaño Mceldas × Mceldas
donde cada elemento de la matriz es una celda. Cada celda es una estructura de datos que
varia de la implementación de mapa necesaria, definiendo sus métodos de fusión propias.
Como se mencionó antes, las grillas son posibles de eliminar basadas criterios geométricos,
por lo que si el vehı́culo se aleja de cierta zona son eliminadas, esto por que solo es busca
la información local del ambiente. Esto en conjunto con la función hash permite que todo
el área del mapa sea una estructura donde el limite de alcance esta dado por el limite de
resolución del tipo de dato de las posiciones espaciales (en este caso, representación de punto
flotante).
Mapa de elevaciones de un único nivel
En este caso la celda esta representada como un altura y una varianza, esto implica
que la estructura de mapa se comporte como un mapa de elevación 2.5D. Lo que se busca
representar en este tipo de mapa es una superficie donde la altura define el limite entre el
espacio ocupado y el libre, donde el libre esta sobre el ocupado. Esto genera que observar
cosas desde abajo no sea interpretable.
n
o
Las mediciones láser ρ~St,iG ; i ∈ 1, ..., Nsamples son almacenas en el mapa. Dado el hecho
de que más de una medición LIDAR puede ser mapeada en una misma celda, la varianza
de la medición es considerada para el proceso de fusión. Por lo tanto, el mapa almacena dos
valores para cada celda: la altura medida mapheight y la varianza de la estimación mapvar .
Si se asume que las mediciones LIDAR y la estimación de altura en el mapa tienen una
distribución Gaussiana, ellas pueden ser fusionadas de manera
usando un enfoque de
óptima
SG
filtrado tipo Kalman [53]. Ası́, en una celda con posición xSt,iG , yt,i
, un filtro Kalman 1D
estima la variable de estado (altura) y su varianza como:
SG
SG
SG
+ mapvar xSt,iG yt,i
· zt,i
χt,i · mapheight xSt,iG yt,i
h
SG
mapheight xSt,iG yt,i
=
h
ih
i
ih
i
h
SG
SG
mapvar yt,i
yt,i
+ χt,i
h
ih
SG
mapvar xSt,iG yt,i
· χt,i
h
SG
mapvar xSt,iG yt,i
=
h
ih
i
i
ih
55
ih
i
(5.38)
i
SG
mapvar xSt,iG yt,i
+ χt,i
h
ih
i
(5.39)
SG SG
Con i ∈ 1, ..., Nsamples , xSt,iG , yt,i
, zt,i correspondientes a las coordenadas cartesianas
de las observaciones LIDAR.
Este enfoque busca solucionar la representación de las superficies para la navegación
del vehı́culo. Al extender su uso a otras tareas, el principal problema de este enfoque es la
imposibilidad de representar dos observaciones muy dispares sobre una misma celda y de esta
manera generando varianzas excesivas que no es posible reparar como un solo objeto, como
ocurre al observar un elemento vertical(por ejemplo un poste o una pared). Por esta razón
una variación a este sistema es presentado al implementar mapa de elevaciones de multi-nivel.
n
o Mapa de elevaciones de multi-nivel
En este implementación la celda corresponde a un arreglo de intervalos que son agregadas
dinámicamente. Cada intervalo almacena su altura mı́nima (Z min ), altura máxima (Z max ) y
número de observaciones (N hit ).
Al momento de agregar una nueva observación, esta es convertida en un intervalo como
lo expresa la Ecuación (5.40):
√
χt,i
√
S
max
Zt,i
= zt,iG + χt,i
SG
min
Zt,i
= zt,i
−
(5.40)
hit
Nt,i
=1
SG SG
Con i ∈ 1, ..., Nsamples , xSt,iG , yt,i
, zt,i , χt,i correspondientes a las coordenadas cartesianas de las observaciones LIDAR y a la varianza de la observación.
Al momento de fusionarlas, se recorre el arreglo de y se revisan los calces de los intervalos
almacenados con el intervalo nuevo. Un calce ocurre cuando un limite del intervalo queda
contenido en otro intervalo. En caso de existir un calce, la celda es actualizada como lo
expresa la Ecuación (5.41):
n
o min
Z min = min Z min , Zt,i
max
Z max = max Z max , Zt,i
(5.41)
hit
N hit = N hit + Nt,i
En caso de no existir calce entre los intervalos, este intervalo es agregado al arreglo.
Además regularmente es posible fusionar intervalos para reducir el número de estos basado
en algún criterio dados por arbitrario. En las pruebas se fusionaban los intervalos basado
en que no existiera una distancia mı́nima de ∆celda , correspondiendo al tamaño de la celda.
Esto se realiza de la misma manera a lo expresado en la Ecuación (5.41), pero agregando a
los limites ∆celda .
Esta implementación permite representar estructuras tridimensionales, volúmenes, basándose en un mapa de grilla, logrando representar túneles y objetos en el ambiente a costa de un
mayor computo y complejidad.
56
Capı́tulo 6
Resultados
Índice
6.1. Datos de evaluación . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
6.2. Evaluación de métodos SLAM
. . . . . . . . . . . . . . . . . . . . 60
6.3. Trayectorias . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62
6.3.1. Trayectoria GPS . . . . . . . . . . . . . . . . . . . . . . . . . . . .
62
6.3.2. Trayectoria de Odometrı́a . . . . . . . . . . . . . . . . . . . . . . .
62
6.3.3. Trayectoria de Estimación Directa . . . . . . . . . . . . . . . . . .
70
6.3.4. Trayectoria de Estimación EKF . . . . . . . . . . . . . . . . . . . .
77
6.4. Estado y Superficie . . . . . . . . . . . . . . . . . . . . . . . . . . . 87
6.4.1. Configuración del montaje . . . . . . . . . . . . . . . . . . . . . . .
87
6.4.2. RMSE como métrica de evaluación . . . . . . . . . . . . . . . . . .
87
6.4.3. Experimentos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
88
En este capı́tulo se presentan los resultados obtenidos de la metodologı́a propuesta. En
una primera parte se presentan los resultados obtenidos analizando la trayectoria obtenida
por los estimadores de estado. Estos estimadores son odometrı́a, estimación directa (pseudoestado) y estimación mediante EKF. Estos son comparados contra la trayectoria obtenida
mediante GPS.
En una segunda parte se analiza el desempeño de la estimación directa y la estimación
mediante EKF al generar las superficies. Para esto realizaron diversos experimentos variando
las configuraciones del sistema. La necesidad de tener un método de evaluación para los
resultados fue la principal razón de los cambios en las configuración de los experimentos.
Al no disponer de un ground truth de superficies o del estado del vehı́culo para realizar
comparación y evaluar la validez del modelo, modificaciones a la configuración original son
hechas para la utilización de uno de los sensores como ground truth.
En la Sección 6.1 se describen los set de datos utilizados para la evaluación del método.
En la Sección 6.2 se describe el intento de adaptación de algún método de SLAM disponible
al sistema robótico.
En la Sección 6.3.1 se describe la trayectoria basada en observaciones GPS utilizada para
compararla con las trayectorias propuestas. En la Sección 6.3.2 se exponen las comparativas
de las trayectorias generadas en base a la odometrı́a. En la Sección 6.3.3 se exponen las
comparativas de las trayectorias generadas en base a la estimación directa. En la Sección
6.3.4 se exponen las comparativas de las trayectorias generadas en base a la estimación
mediante EKF y el análisis de la sensibilidad del parámetro γ.
En la Sección 6.4.1 se describe el montaje de sensores y las modificaciones a este para la
metodologı́a de la evaluación. En la Sección 6.4.2 se describe el uso del RMSE como métrica
de evaluación. En la Sección 6.4.3 los resultados y observaciones obtenidas del sistema. Finalmente en la Sección 6.4.3 se presentan ejemplos de la superficie estimada y ejemplos de
malas estimaciones.
6.1.
Datos de evaluación
Los principales sets de datos de evaluación fueron grabado al interior del parque O’Higgins,
el cual está ubicado en la comuna de Santiago, Chile. Estos sets de datos fueron capturados
mientras un conductor manejaba el vehı́culo por un camino de tierra rugoso a velocidades
bajas (< 10[m/s]) y a velocidades medias (< 20[m/s]) y en camino pavimentado a velocidades
bajas (< 10[m/s]).
Cada grabación tiene mediciones de tres sensores láser, una IMU, un RADAR, dos cámaras, un GPS y la información interna del bus CAN del vehı́culo. (ver dispositivos en la Sección
4.2.2 y detalle de los sets en el Anexo B).
Como se explicó antes, el mapa del terreno es construido con las mediciones de los sensores
LIDAR. El sensor SICK LMS291 y LMS151 tienen un plano de barrido, mientras que el SICK
LD-MRS HD tiene cuatro. Los tres sensores LIDAR están montados en diferentes ángulos de
inclinación para lograr diferentes distancias de observación. Considerando el vehı́culo sobre
una superficie plana, para las observaciones del sensor SICK LMS291, el plano de escaneo
impacta en el suelo a 16[m] en frente del vehı́culo. Para el caso del SICK LD-MRS, el plano
de escaneo inferior llega a 19[m], mientras que los superiores no alcanzan a tocar el suelo.
El plano de escaneo del sensor SICK LMS151 está en el plano YZ de SL , justo enfrente
del vehı́culo (notar montaje modificado en Figura 6.34). Las observaciones de este sensor
58
permiten tener una representación con menor error de la superficie por la que se desplaza el
vehı́culo, debido a que se disminuye la distancia a la que se observa. Las observaciones son
menos distorsionadas por los movimientos verticales del vehı́culo y además posee una mayor
densidad de observaciones. En la Figura 6.1 se puede observar un ejemplo de observaciones
del conjunto de sensores.
Figura 6.1: Ejemplo de observaciones en plano XY de los sensores LIDAR, LMS151(verde),
LMS291(rojo) y LD-MRS(azul)
La principal zona sin pavimentar utilizada está formada por un camino muy irregular de
unos 800[m] de largo con pendientes ascendentes y descendentes. El camino es un recorrido
hecho de tierra rodeado de pasto y algunos árboles. La diferencia de altura entre el punto
mı́nimo y el máximo del recorrido es de unos 3[m] aproximadamente. En la Figura 6.2 se
puede apreciar el recorrido proyectado en una imagen satélital y un ejemplo del recorrido
observado mediante el sensor LM151.
Para realizar la evaluación de las trayectorias generadas se utilizaron 3 recorridos en
superficies pavimentadas del parque y se contrastaron con una trayectoria de un camino de
tierra. Estos recorridos tienen un largo promedio de 1000[m].
Para realizar la evaluación del estimador de estado y superficies se utilizaron 6 recorridos
a diferentes rangos de velocidad y sentidos del recorrido, en la Tabla 6.1 se describen las
condiciones de estos recorridos.
En la Figura 6.4 se presentan los histogramas de velocidad de las grabaciones en camino
de tierra, normalizado por la distancia recorrida.
a Imagen
de GoogleMaps
59
(a) vista satelitala
(b) nube de puntos
Figura 6.2: Recorrido de set de datos sobre camino de tierra
Figura 6.3: Ejemplo de imágenes del camino de tierra
Tabla 6.1: Caracterı́sticas de grabaciones en camino de tierra y en pavimento
ID de la grabación
1-2
3-6
7-9
6.2.
Tipo de suelo
sin pavimento
sin pavimento
con pavimento
Rango de velocidad Condición
bajo
seco y soleado
media
seco y soleado
bajo
seco y soleado
Evaluación de métodos SLAM
Se analizó la posibilidad de utilizar alguno de los métodos de SLAM cuyas implementaciones estuviesen disponibles y fuesen compatibles con el middleware utilizado y datos
60
Figura 6.4: Histograma de velocidades por porcentaje de distancia recorrida en caminos de
tierra
disponibles, para realizar comparativas con la metodologı́a propuesta. Los métodos analizados están descritos en la Sección 2.3.
El primer enfoque analizado es GMapping [12,13], el que debido a su enfoque planar no es
posible adecuarlo al sistema perceptual. Los sensores de distancias utilizados en el vehı́culo,
como está descrito en la sección anterior, están instalados inclinados con respecto al vehı́culo.
El segundo enfoque analizado es RGB-D SLAM [8]. En este caso pese a que realiza una
estimación de localización en seis dimensiones e integra el ambiente en tres dimensiones, la
información sensorial no es compatible. Esto debido a que este método requiere de información
de distancia organizada y bidimensional (como una imagen) para poder realizar los calces
para la estimación de desplazamiento. La información proveniente de los sensores LIDAR,
aún cuando contengan la información de color al igual que la medición del sensor Kinect, los
puntos repetidos observados entre diferentes observación son insuficientes.
El ultimo enfoque analizado es HectorSlam [18]. El sistema se logró adaptar de manera
correcta al sistema disponible en el vehı́culo. La información sensorial es del mismo tipo a
la utilizada y se generó la información odométrica en el formato necesario. Los resultados
obtenidos no fueron satisfactorios, debido a que las observaciones sucesivas eran muy similares
entre ellas. Gran parte de las observaciones son de la superficie del terreno y al realizar un
desplazamiento no existe mayor cambio en la observación. Aún cuando se observen objetos
verticales y se observe el desplazamiento en observaciones sucesiva, la observación de la
superficie frente al vehı́culo es considerada como un objeto frontal, y aún cuando la odometrı́a
indique desplazamiento, la corrección basada en la observación frena al vehı́culo.
61
6.3.
6.3.1.
Trayectorias
Trayectoria GPS
Debido a la falta de un ground truth para el estado real del vehı́culo, el uso de la trayectoria
generada por la mediciones reportadas por un receptor GPS son una buena alternativa en
este caso. Basado en la caracterı́sticas de las estimación de posición mediante GPS [1], una
vez obtenida una estimación, su error estará acotado. El receptor que se ha utilizado no posee
una gran exactitud pero suficiente para comparar una trayectoria a gran escala, en el plano
de la superficie terrestre, ya que la estimación de altura tiene muy baja precisión.
No es directo realizar la comparación de la trayectoria GPS y las estimaciones de la
trayectoria. Las estimaciones GPS son entregadas en latitud a y longitud b mientras que las
estimaciones están en coordenadas cartesianas locales en metros. Para solucionar esto, las
coordenadas GPS son convertidas a coordenadas UTMc [15] y se le resta la posición inicial,
de esta manera quedan localmente referenciadas.
Para realizar estas evaluaciones se emplearon set de datos de conducción en zonas pavimentadas dentro del parque (set 7-9) y un recorrido de camino de tierra (set 5).
Debido a que las mediciones de GPS son entregadas a una frecuencia de 1[Hz], esta
trayectoria tendrá un retraso con las estimaciones. La estimación del rumbo a partir de
estas observaciones también tienen un retraso que se puede apreciar en las comparaciones
por ser estimada a partir de dos mediciones sucesivas. Además los recorridos terminan con
el vehı́culo detenido, por lo que las estimaciones de rumbo para estas trayectorias al ser
calculadas en base a la diferencia de las posiciones, se vuelven muy ruidosas y malas, por lo
que las diferencias de rumbo tenderán a no muy fidedignas.
6.3.2.
Trayectoria de Odometrı́a
En una primera evaluación se analiza la estimación de la trayectoria solo mediante la
odometrı́a. Esta odometrı́a corresponde a la generación de la trayectoria basada en la Ecuación (5.10) y en la Ecuación (5.14) solo en dos dimensiones. Como la trayectoria de basada
en Odometrı́a no tiene una referencia global para el ángulo inicial, esta fue alineada a la
trayectoria del GPS en el tiempo inicial.
En la Figuras 6.5, 6.6, 6.7 y 6.8 se muestran las comparativas entre las trayectorias. En las
trayectorias están marcadas posiciones de manera de tener una apreciación de la comparación
a nivel temporal, estas marcas ocurren cada 5[s].
Es claramente apreciable y esperable que la posición final no coincida con la trayectoria
del GPS. En las Figuras 6.9, 6.10, 6.11 y 6.12 se muestran las diferencias de las posiciones y
rumbos durante el recorrido. En estos gráficos se puede apreciar que las distancias entre las
trayectorias comparadas tiende a ser creciente, lo que, esto es esperable por que la trayectoria
de odometrı́a es completamente en lazo abierto con respecto a la orientación, por lo que la
diferencia de rumbo no puede ser reducida. También es apreciable como el retardo en la
estimación del rumbo por parte del GPS genera que las diferencias de rumbo sean muy poco
estables, que sumado a la baja frecuencia de muestreo del GPS empeora la comparación. La
a distancia
angular desde un paralelo a la linea del Ecuador
angular desde un meridiano al meridiano de referencia(Greenwich)
c Universal Transverse Mercator
b distancia
62
350
300
250
Norte [m]
200
150
100
50
0
GPS
odometria
−200
−150
−100
−50
Este [m]
0
50
100
150
Figura 6.5: Comparativa de la trayectoria GPS y Odometrı́a en grabación 7 (camino pavimentado).
trayectoria de odometrı́a es calculada a una frecuencia muy superior a la del GPS, por lo que
solo es comparada en los puntos en los instantes dados por el GPS.
Del análisis realizado a estas trayectorias, este error viene principalmente asociado a
velocidades medias y altas, y situaciones con virajes cerrados, donde el vehı́culo experimenta
un comportamiento más dinámico en su trayectoria. Esto se puede corroborar al observar el
desempeño de la trayectoria de odometrı́a en un recorrido a bajas velocidades. En la Figura
6.13 se puede ver como las trayectorias coinciden de manera muy cercanas durante todo el
recorrido.
De este análisis, el modelo de movimiento del vehı́culo basado en odometrı́a tiene un
desempeño que le permite ser utilizado al momento de planificar trayectorias en distancia
cortas, a nivel de un mapa local al vehı́culo.
63
200
100
0
Norte [m]
−100
−200
−300
−400
GPS
odometria
−500
−50
0
50
100
150
Este [m]
200
250
300
Figura 6.6: Comparativa de la trayectoria GPS y Odometrı́a en grabación 8 (camino pavimentado).
64
GPS
odometria
50
0
−50
Norte [m]
−100
−150
−200
−250
−300
−50
0
50
100
Este [m]
150
200
Figura 6.7: Comparativa de la trayectoria GPS y Odometrı́a en grabación 9 (camino pavimentado).
65
400
350
300
Norte [m]
250
200
150
100
50
GPS
odometria
0
−300
−250
−200
−150
−100
−50
Este [m]
0
50
100
Figura 6.8: Comparativa de la trayectoria GPS y Odometrı́a en grabación 5 (camino de
tierra).
70
80
distancia entre GPS y odometria
angulo entre GPS y odometria
60
60
50
40
grados [°]
distancia [m]
40
20
30
0
20
−20
10
0
−40
0
50
100
150
200
250
0
tiempo [s]
50
100
150
200
250
tiempo [s]
Figura 6.9: Diferencia de posición y rumbo entre trayectorias GPS y Odometrı́a en grabación
7.
66
50
60
distancia entre GPS y odometria
angulo entre GPS y odometria
45
40
40
20
35
30
grados [°]
distancia [m]
0
25
−20
20
15
−40
10
−60
5
0
−80
0
20
40
60
80
100
tiempo [s]
120
140
160
180
0
20
40
60
80
100
tiempo [s]
120
140
160
180
Figura 6.10: Diferencia de posición y rumbo entre trayectorias GPS y Odometrı́a en grabación
8.
120
60
distancia entre GPS y odometria
angulo entre GPS y odometria
50
100
40
80
grados [°]
distancia [m]
30
60
20
40
10
20
0
0
−10
0
50
100
150
200
250
0
tiempo [s]
50
100
150
200
250
tiempo [s]
Figura 6.11: Diferencia de posición y rumbo entre trayectorias GPS y Odometrı́a en grabación
9.
67
90
20
distancia entre GPS y odometria
angulo entre GPS y odometria
80
10
70
0
60
grados [°]
distancia [m]
−10
50
−20
40
−30
30
−40
20
−50
10
0
−60
0
10
20
30
40
50
tiempo [s]
60
70
80
90
100
0
10
20
30
40
50
tiempo [s]
60
70
80
90
100
Figura 6.12: Diferencia de posición y rumbo entre trayectorias GPS y Odometrı́a en grabación
5.
68
GPS
odometria
−50
−100
−150
Norte [m]
−200
−250
−300
−350
−400
−450
−300
−250
−200
−150
−100
−50
Este [m]
0
50
100
Figura 6.13: Comparativa de la trayectoria GPS y Odometrı́a en grabación 1 (camino de
tierra).
69
6.3.3.
Trayectoria de Estimación Directa
La estimación directa del estado, tal como se presenta en la Sección 5.3.1, esta basada
en la utilización de la orientación entregada por la IMU y la velocidad del vehı́culo. En esta
evaluación se presenta la comparación de la trayectoria generada por esta estimación y la
generada de las mediciones GPS. Al igual que en el caso de la trayectoria de la odometrı́a las
trayectorias GPS presentan inconvenientes por su tasa de muestreo y poca exactitud local,
por lo que en distancias largas esos inconvenientes dejarı́an de importar.
En las Figuras 6.14, 6.15, 6.16 y 6.17 se presentan las comparativas de las trayectorias
GPS y las trayectorias de estimación directa. En las trayectorias están marcadas posiciones
de manera de tener una apreciación de la comparación a nivel temporal, estas marcas ocurren
cada 5[s]. Al observar estas comparaciones, inmediatamente se puede apreciar una mejora
para los caso en que el vehı́culo se desplaza en superficies pavimentadas con respecto a las
trayectorias basadas solo en odometrı́a. Esta mejora principalmente proviene del hecho que
la estimación de orientación de la IMU provee una referencia global al rumbo gracias a su
magnetómetro. El tener esta referencia externa le permite no acumular error permanente con
respecto al rumbo del vehı́culo y de esta manera no acumular error en la trayectoria.
En las Figuras 6.18, 6.19, 6.20 y 6.21 se muestran las diferencias de las posiciones y rumbos
durante el recorrido. En estos gráficos es posible apreciar que en los casos de pavimento, los
error finales de las posiciones están dentro de un radio de 10[m] aún después de recorrer más
de 1000[m] en promedio.
Un caso a aparte es el caso de comparación con el camino de tierra. Esta trayectoria al
ser mucho más dinámica que los casos pavimentados, la IMU no logra entregar un rumbo
muy similar al desplazamiento observado por el GPS. En la Figura 6.17 es apreciable como
la densidad de marcadores en la trayectoria es muy baja en un comienzo mostrando que la
velocidad elevada, y una curva muy dinámica pueden generar malas estimaciones de orientación a la IMU. Esto provoca que al extraviar el rumbo en un comienzo la distancia entre
ambas trayectorias no logra recuperase, siendo apreciable en la Figura 6.21. En esta figura se
muestra como la distancia entre las trayectorias crece desde un comienzo y sigue creciendo a
lo largo del recorrido, aún cuando la diferencia de rumbo si disminuye.
70
350
300
250
Norte [m]
200
150
100
50
GPS
pseudo−estado
0
−200
−150
−100
−50
Este [m]
0
50
100
150
Figura 6.14: Comparativa de la trayectoria GPS y Estimación Directa en grabación 7 (camino
pavimentado).
71
200
100
0
Norte [m]
−100
−200
−300
−400
GPS
pseudo−estado
−500
−50
0
50
100
150
Este [m]
200
250
300
Figura 6.15: Comparativa de la trayectoria GPS y Estimación Directa en grabación 8 (camino
pavimentado).
72
GPS
pseudo−estado
50
0
−50
Norte [m]
−100
−150
−200
−250
−300
−100
−50
0
50
100
Este [m]
150
200
250
Figura 6.16: Comparativa de la trayectoria GPS y Estimación Directa en grabación 9 (camino
pavimentado).
73
GPS
pseudo−estado
400
350
300
Norte [m]
250
200
150
100
50
0
−300
−250
−200
−150
−100
Este [m]
−50
0
50
100
Figura 6.17: Comparativa de la trayectoria GPS y Estimación Directa en grabación 5 (camino
de tierra).
20
50
distancia entre GPS y pseudo−estado
angulo entre GPS y pseudo−estado
18
40
16
30
14
20
10
grados [°]
distancia [m]
12
10
0
8
−10
6
−20
4
−30
2
0
−40
0
50
100
150
200
250
0
tiempo [s]
50
100
150
200
250
tiempo [s]
Figura 6.18: Diferencia de posición y rumbo entre trayectorias GPS y Estimación Directa en
grabación 7.
74
18
60
distancia entre GPS y pseudo−estado
angulo entre GPS y pseudo−estado
16
40
14
20
10
grados [°]
distancia [m]
12
0
8
6
−20
4
−40
2
0
−60
0
20
40
60
80
100
tiempo [s]
120
140
160
180
0
20
40
60
80
100
tiempo [s]
120
140
160
180
Figura 6.19: Diferencia de posición y rumbo entre trayectorias GPS y Estimación Directa en
grabación 8.
25
35
distancia entre GPS y pseudo−estado
angulo entre GPS y pseudo−estado
30
20
25
20
15
grados [°]
distancia [m]
15
10
10
5
0
5
−5
0
−10
0
50
100
150
200
250
0
tiempo [s]
50
100
150
200
250
tiempo [s]
Figura 6.20: Diferencia de posición y rumbo entre trayectorias GPS y Estimación Directa en
grabación 9.
75
40
30
angulo entre GPS y pseudo−estado
distancia entre GPS y pseudo−estado
35
20
30
10
grados [°]
distancia [m]
25
20
0
15
−10
10
−20
5
0
−30
0
10
20
30
40
50
tiempo [s]
60
70
80
90
100
0
10
20
30
40
50
tiempo [s]
60
70
80
90
100
Figura 6.21: Diferencia de posición y rumbo entre trayectorias GPS y Estimación Directa en
grabación 5.
76
6.3.4.
Trayectoria de Estimación EKF
La trayectoria basada en la Estimación EKF es la presentada en la Sección 5.3.2. En
esta evaluación se presenta la comparación de la trayectoria generada por esta estimación
y la generada de las mediciones GPS. A diferencia de las anteriores comparaciones, se le
agrega el análisis de la sensibilidad de la trayectoria frente a la variación del parámetro
γ. Este parámetro esta presente en la etapa predictiva (Ecuación (5.22), Sección 5.3.2), y
corresponde al factor utilizado en el modelo cinemático del vehı́culo (Sección 5.2).
Para analizar la incidencia del modelo cinemático en la trayectoria basada en EKF,
se exponen las trayectorias generadas sobre un mismo recorrido pero con variaciones del
parámetroγ. El parámetro γ es modificado desde 0,5 · γ hasta 1,5 · γ en incrementos de 0,1 · γ,
lo que genera 11 trayectorias.
En las Figuras 6.22, 6.23, 6.24 y 6.25 se presentan las comparativas de las trayectorias
GPS y las trayectorias de estimación basada en EKF. En las trayectorias están marcadas
posiciones de manera de tener una apreciación de la comparación a nivel temporal, estas
marcas ocurren cada 5[s]. En las Figuras 6.26, 6.27, 6.28 y 6.29 se presenta una sección de las
figuras anteriores donde se puede apreciar con mayor detalle el punto final de las trayectorias
y como difieren entre ellas.
Visualmente a una gran escala amplia (Figuras 6.22, 6.23, 6.24 y 6.25) las diferentes
trayectorias no se aprecian distintas entre si y tienen un comportamiento similar al observado
en la Sección 6.3.3. Pero al observarlas a una escala menor, del orden de metros, (Figuras
6.26, 6.27, 6.28 y 6.29) se logran observar como para cada valor del parámetro γ se tiene un
punto final de la trayectoria diferente. Esto es debido por que el parámetro γ es utilizado para
calcular ∆~pt durante la etapa predictiva (Ecuación (5.21)), generando pequeñas variaciones
en la trayectoria, pese a que durante la etapa correctiva se corrija la orientación.
Estas variaciones de trayectorias pueden parecer ı́nfimas y generar trayectorias similares
a las generadas por la estimación directa, pero como se analiza en la Sección 6.4, tiene una
incidencia importante al momento de integrar las observaciones de los sensores. En la Figura
6.26 se puede apreciar estas diferencias debido a que el segmento final de las trayectorias
quedan ubicadas paralelas una tras otra y como la correspondiente al valor original de γ
queda ubicada cerca de la estimación de la trayectoria del GPS.
En las Figuras 6.30, 6.31, 6.32 y 6.33 se muestran las diferencias de las posiciones y rumbos durante el recorrido, para cada variación del parámetro γ. Es posible observar como se
dispersan la diferencias de las trayectorias al momento de realizar virajes generando diferencias de rumbo por momentos que se transforman en diferencias en la trayectoria a posterior.
Un caso particular es el presentado en la Figura 6.33, en esta figura se puede apreciar una
ocurrencia similar a la observada con la estimación directa para este mismo recorrido. Una
vez que se genera una diferencia el rumbo, que es amplificada con la variación γ, las trayectorias se separan y las diferencias no se reducen debido a la forma de la trayectoria. En la
Figura 6.29 se puede observar como los puntos finales de la trayectoria terminan alineados
sobre la mismo rumbo de la trayectoria, caso opuesto a lo ocurrido en la Figura 6.26.
77
GPS
estimaciones
350
300
250
Norte [m]
200
150
100
0.5 gamma
0.6 gamma
0.7 gamma
0.8 gamma
0.9 gamma
1.0 gamma
1.1 gamma
1.2 gamma
1.3 gamma
1.4 gamma
1.5 gamma
50
0
−200
−150
−100
−50
Este [m]
0
50
100
150
Figura 6.22: Comparativa de la trayectoria GPS y Estimación EKF con variación de parámetro γ en grabación 7 (camino pavimentado).
78
200
100
0
GPS
estimaciones
Norte [m]
−100
0.5 gamma
0.6 gamma
0.7 gamma
0.8 gamma
0.9 gamma
1.0 gamma
1.1 gamma
1.2 gamma
1.3 gamma
1.4 gamma
1.5 gamma
−200
−300
−400
−500
−50
0
50
100 150
Este [m]
200
250
300
Figura 6.23: Comparativa de la trayectoria GPS y Estimación EKF con variación de parámetro γ en grabación 8 (camino pavimentado).
79
50
0
GPS
estimaciones
−50
0.5 gamma
0.6 gamma
0.7 gamma
0.8 gamma
0.9 gamma
1.0 gamma
1.1 gamma
1.2 gamma
1.3 gamma
1.4 gamma
1.5 gamma
Norte [m]
−100
−150
−200
−250
−300
0
50
100
Este [m]
150
200
Figura 6.24: Comparativa de la trayectoria GPS y Estimación EKF con variación de parámetro γ en grabación 9 (camino pavimentado).
80
400
GPS
estimaciones
350
0.5 gamma
0.6 gamma
0.7 gamma
0.8 gamma
0.9 gamma
1.0 gamma
1.1 gamma
1.2 gamma
1.3 gamma
1.4 gamma
1.5 gamma
300
Norte [m]
250
200
150
100
50
0
−250
−200
−150
−100
Este [m]
−50
0
Figura 6.25: Comparativa de la trayectoria GPS y Estimación EKF con variación de parámetro γ en grabación 5 (camino de tierra).
81
148
GPS
estimaciones
146
144
142
Norte [m]
140
0.5 gamma
0.6 gamma
0.7 gamma
0.8 gamma
0.9 gamma
1.0 gamma
1.1 gamma
1.2 gamma
1.3 gamma
1.4 gamma
1.5 gamma
138
136
134
132
130
128
−185
−180
−175
−170
Este [m]
−165
−160
Figura 6.26: Detalle del punto final de la comparativa de la trayectoria GPS y Estimación
EKF con variación de parámetro γ en grabación 7 (camino pavimentado).
82
GPS
estimaciones
−470
0.5 gamma
0.6 gamma
0.7 gamma
0.8 gamma
0.9 gamma
1.0 gamma
1.1 gamma
1.2 gamma
1.3 gamma
1.4 gamma
1.5 gamma
−480
Norte [m]
−490
−500
−510
−520
−530
145
150
155
160
165
Este [m]
170
175
180
Figura 6.27: Detalle del punto final de la comparativa de la trayectoria GPS y Estimación
EKF con variación de parámetro γ en grabación 8 (camino pavimentado).
83
85
0.5 gamma
0.6 gamma
0.7 gamma
0.8 gamma
0.9 gamma
1.0 gamma
1.1 gamma
1.2 gamma
1.3 gamma
1.4 gamma
1.5 gamma
Norte [m]
80
75
GPS
estimaciones
70
30
40
50
60
Este [m]
70
80
90
Figura 6.28: Detalle del punto final de la comparativa de la trayectoria GPS y Estimación
EKF con variación de parámetro γ en grabación 9 (camino pavimentado).
450
440
Norte [m]
430
420
0.5 gamma
0.6 gamma
0.7 gamma
0.8 gamma
0.9 gamma
1.0 gamma
1.1 gamma
1.2 gamma
1.3 gamma
1.4 gamma
1.5 gamma
410
GPS
estimaciones
400
−120
−100
−80
−60
−40
−20
Este [m]
Figura 6.29: Detalle del punto final de la comparativa de la trayectoria GPS y Estimación
EKF con variación de parámetro γ en grabación 5 (camino de tierra).
84
60
25
0.5 gamma
0.6 gamma
0.7 gamma
0.8 gamma
0.9 gamma
1.0 gamma
1.1 gamma
1.2 gamma
1.3 gamma
1.4 gamma
1.5 gamma
20
40
20
grados [°]
distancia [m]
15
0
10
0.5 gamma
0.6 gamma
0.7 gamma
0.8 gamma
0.9 gamma
1.0 gamma
1.1 gamma
1.2 gamma
1.3 gamma
1.4 gamma
1.5 gamma
20
5
40
0
0
50
100
150
200
60
0
250
50
100
150
200
250
tiempo [s]
tiempo [s]
Figura 6.30: Diferencia de posición y rumbo entre trayectorias GPS y Estimación EKF con
variación de parámetro γ en grabación 7.
30
60
0.5
0.5 gamma
gamma
0.6
0.6 gamma
gamma
0.7
0.7 gamma
gamma
0.8
0.8 gamma
gamma
0.9
0.9 gamma
gamma
1.0
1.0 gamma
gamma
1.1
1.1 gamma
gamma
1.2
1.2 gamma
gamma
1.3
1.3 gamma
gamma
1.4
1.4 gamma
gamma
1.5
1.5 gamma
gamma
25
20
0.5 gamma
0.5 gamma
0.6 gamma
0.6 gamma
0.7
0.7gamma
gamma
0.8
0.8gamma
gamma
0.9
0.9gamma
gamma
1.0gamma
gamma
1.0
1.1
gamma
1.1 gamma
1.2
gamma
1.2 gamma
1.3 gamma
1.3 gamma
1.4 gamma
1.4
1.5gamma
gamma
1.5 gamma
40
20
grados [°]
distancia [m]
0
15
−20
10
−40
5
−60
0
0
20
40
60
80
100
tiempo [s]
120
140
160
−80
180
0
20
40
60
80
100
tiempo [s]
120
140
160
180
Figura 6.31: Diferencia de posición y rumbo entre trayectorias GPS y Estimación EKF con
variación de parámetro γ en grabación 8.
85
25
60
50
0.5
0.5gamma
gamma
0.6
0.6gamma
gamma
0.7
0.7gamma
gamma
0.8
0.8gamma
gamma
0.9
0.9gamma
gamma
1.0
gamma
1.0 gamma
1.1
1.1gamma
gamma
1.2
1.2gamma
gamma
1.3
1.3gamma
gamma
1.4
1.4gamma
gamma
1.5
1.5gamma
gamma
distancia [m]
15
40
0.5
gamma
0.5
gamma
0.6
gamma
0.6
gamma
0.7
gamma
0.7
gamma
0.8
gamma
0.8 gamma
0.9
gamma
0.9
gamma
1.0
gamma
1.0
gamma
1.1
gamma
1.1
gamma
1.2
gamma
1.2
gamma
1.3
gamma
1.3
gamma
1.4
gamma
1.4 gamma
1.5
gamma
1.5 gamma
30
grados [°]
20
20
10
10
5
0
0
0
50
100
150
200
−10
250
0
50
100
tiempo [s]
150
200
250
tiempo [s]
Figura 6.32: Diferencia de posición y rumbo entre trayectorias GPS y Estimación EKF con
variación de parámetro γ en grabación 9.
50
40
0.5
0.5gamma
gamma
0.6
0.6gamma
gamma
0.7
0.7gamma
gamma
0.8
0.8gamma
gamma
0.9
gamma
0.9 gamma
1.0
gamma
1.0 gamma
1.1
1.1gamma
gamma
1.2
1.2gamma
gamma
1.3
1.3gamma
gamma
1.4
1.4gamma
gamma
1.5
1.5gamma
gamma
45
40
35
0.5gamma
gamma
0.5
0.6gamma
gamma
0.6
0.7gamma
gamma
0.7
0.8gamma
gamma
0.8
0.9
gamma
0.9 gamma
1.0gamma
gamma
1.0
1.1gamma
gamma
1.1
1.2gamma
gamma
1.2
1.3gamma
gamma
1.3
1.4gamma
gamma
1.4
1.5gamma
gamma
1.5
30
20
10
grados [°]
distancia [m]
30
25
0
−10
20
−20
15
−30
10
−40
5
0
0
10
20
30
40
50
tiempo [s]
60
70
80
90
−50
100
0
10
20
30
40
50
tiempo [s]
60
70
80
90
100
Figura 6.33: Diferencia de posición y rumbo entre trayectorias GPS y Estimación EKF con
variación de parámetro γ en grabación 5.
86
6.4.
6.4.1.
Estado y Superficie
Configuración del montaje
Como fue explicado anteriormente, el montaje original de los sensores debió ser modificado
para la nueva necesidad. En este nuevo esquema el sensor SICK LMS-151 fue trasladado de
su ubicación original sobre la parrilla (donde apuntaba al suelo a varios metros delante del
vehı́culo) a estar montado justo delante del parachoques (montado del soporte del radiador)
apuntando al suelo en la misma lı́nea del parachoques.
Las principales dificultades de esta configuración corresponden a la extensión y recorrido
del cable de comunicaciones y alimentación del sensor. Debido a su limitada extensión no
pueden quedar tendidos de manera permanente. Una segunda dificultad es que el sensor al
quedar sobresaliente de la lı́nea original del vehı́culo le impide poder circular de manera
legal por caminos públicos. Además el sensor ocluye la placa patente del vehı́culo. Por estas
razones, el sensor debı́a ser desmontado y montado nuevamente cada vez que el vehı́culo es
trasladado al lugar de pruebas.
Figura 6.34: Comparación de la modificación de la descripción del robot durante las tomas
de datos, a la derecha es el montaje original, a la izquierda es la modificación
El resto de los sensores siguen en su ubicación original, salvo pequeños cambios de orientación. En la Figura 6.34 se muestran una comparativa de los sensores, en ambas configuraciones, siendo el más notorio el cambio de ubicación del sensor SICK LMS-151.
6.4.2.
RMSE como métrica de evaluación
Para realizar la evaluación de la fusión de las observaciones y la estimación del estado se
utilizó como medida el RMSEa . La Ecuación (6.1) es el cálculo del RMSE donde a y b son los
ı́ndices de inicio y de fin del subconjunto de mediciones ocupadas para la evaluación. Estas
cubren 120◦ delante del vehı́culo. Para ellos las medidas del sensor SICK LMS-151 montado
como esta descrito en Sección 6.4.1 son comparadas con las altura almacenadas en el mapa
de observaciones ya fusionado.
a Root
Mean Square Error
87
RM SE =
v
u
2
u Pb u i=a mapheight (Xsensor,i , Ysensor,i ) − Zsensor,i
t
b−a
(6.1)
La idea detrás de esta medida es que una buena estimación del estado del vehı́culo proveerá una buena estimación del desplazamiento desde donde se capturaron las observaciones
e integraron hasta cuando se posiciona el vehı́culo sobre ellas. En caso de que esta sea una
mala estimación, una diferencia existirá entre la superficie observada integrada en el mapa
y la medición instantánea capturada por el sensor SICK LMS-151. Para cada medición del
sensor SICK LMS-151 dentro del área de comparación se proyecta en la grilla se calcula el
RMSE. En la Figura 6.35 se muestra a que corresponden las mediciones del sensor SICK
LMS-151.
Figura 6.35: Diagrama del cálculo del RMSE
Una de las caracterı́sticas del RMSE, es que considera el error medio y la varianza simultáneamente. Esto conlleva a que aún teniendo una correcta estimación, el RMSE no
podrá ser cero por que esta acotado por la varianza proveniente del sensor. Dentro de las
caracterı́sticas de un sensor la precisión del es cuanto puede variar su medición, aún cuando
tenga una perfecta exactitud.
Para este sensor σ 2 es igual a 0,0122 . Realizando un desarrollo matemático de la esperanza
del RMSE para el caso de una medición de una superficie perfectamente plana y perfectamente estimada pero considerando la varianza del sensor se puede determinar el valor mı́nimo
esperable para esta medida. El valor resultante mı́nimo esperable por un escaneo del sensor
del RMSE es 0,0101. El desarrollo de este cálculo esta incorporado en el Anexo A.
6.4.3.
Experimentos
Camino de tierra
El sistema de estimación de terreno basado en Kalman (Sección 5.3.2) es comparado
contra el sistema base (Sección 5.3.1), el cual usa directamente la información de la IMU
como entrada de orientación y la rapidez del vehı́culo. Además el uso de las observaciones
virtuales provenientes del mapa del terreno es analizado en conjunto con estimador de estado
basado en Kalman. Ası́ las siguientes configuraciones del sistema fueron analizados:
S1 “sistema base”: La información de la IMU y de la rapidez del vehı́culo son utilizadas
para obtener el desplazamiento y el cambio de orientación. Ningún filtro de Kalman es
88
utilizado para la estimación de la pose o para la fusión del terreno, por lo que todas la
mediciones de distancia tienen el mismo peso.
S2 “Estimador de estado”: El sistema S2 corresponde al estimador propuesto usando
todas las fuentes de información descritas.
S3 “Estimador de estado sin feedback del mapa”: El sistema S3 corresponde al estimador propuesto usando todas las fuentes de información descritas, excepto el feedback
de la altura del mapa.
Como ya se ha mencionado, las mediciones obtenidas del sensor montado en el plano YZ
del vehı́culo son usadas para la construcción del ground truth. Estas medidas no son incorporadas al mapa, y son comparadas con los valores de altura correspondientes de mapheight .
Al no haber una manera directa de poder evaluar el error de la estimación de estadom
por no poseer un ground truth de este, se debe realizar de una manera indirecta. La manera
propuesta para realizar esta evaluación consiste en comparar el error de la integración del
mapa con la medición frontal de la superficie del terreno sin integrarla. De esta manera, en
caso de tener una buena estimación del estado conllevará a una coincidencia de los perfiles
del terreno y en caso de disparidad entre ellas podrı́a significar un error de la estimación del
estado. Para cuantificar esta discrepancia, tres tipos de medidas son propuestas:
1. El RMSE (Root Mean Square Error) de cada perfil laser y luego integrado sobre todo
el recorrido (Ecuación (6.1)). Este valor posteriormente es normalizado por el numero
total de muestras capturadas, de esta manera es comparable en diferentes recorridos,
el cual es denominado RMSEsample .
2. El porcentaje del recorrido total para el cual el RMSE de S2 y S3 es mejor que el caso
S1.
3. El porcentaje del recorrido para el cual el RMSE se encuentra debajo de un umbral
variable. Esti genera una curva similar a una curva ROC (Receiver Operating Curve).
En un primer experimento el sistema fue comparado usando las grabaciones 1 y 2 (baja
velocidad). La Figura 6.36 muestra el histograma de esta velocidades sobre el recorrido total
que corresponde a 1,6[Km]. La Tabla 6.2 muestra RMSE de cada sistema (1) y su comparativa
(2), y la Figura 6.37 muestra la medida (3). Se puede observar que para los sistema S2 y S3
los resultados del RMSE total son considerablemente menores que el caso de S1, reduciéndose
aproximadamente a un tercio de S1. También es apreciable que el rendimiento de S2 y S3 es
al menos un 80 % del tiempo menor que S1. Y en el caso S2 en que se usa toda la información
es ligeramente superior al caso S3 cuando se omite la información del mapa.
Tabla 6.2: Resultados de medidas (1) y (2) de grabaciones 1-2
RMSEsample
% mejor que S1
S1
S2
S3
−3
−3
41,5877 · 10 [m] 14,7799 · 10 [m] 14,8113 · 10−3 [m]
—
81,21 %
80,26 %
89
Figura 6.36: Histograma de rapidez del vehı́culo en grabaciones 1-2
Figura 6.37: Medida (3) de grabaciones 1-2. % de celdas para las cuales RMSE es menor que
un umbral.
En el segundo experimento los sistemas son comparados usando las grabaciones 3-6 (velocidad media). La Figura 6.38 muestra el histograma de velocidades, sobre el recorrido total
que corresponde a 3,2[Km]. La Tabla 6.3 se muestra RMSEsample de cada sistema (1) y su
comparativa (2), y la Figura 6.39 muestra la medida (3). Ası́ como en el caso de las grabaciones 1-2 se puede observar que los sistemas que usan el estimador (S2 y S3) obtiene un
menor RMSEsample en comparación con el sistema base, de hecho es aproximadamente dos
tercios del sistema base. Los casos en que S2 y S3 son mejor que S1 y comparativamente con
el caso de las grabaciones 1-2 son menores. En esta ocasión al comparar S2 y S3, S3 marca
un desempeño menor que S2, siendo aproximadamente un 10 % menor que el S2.
Evaluación por secciones
En la sección anterior se evaluó mediante el RMSE de la superficie durante todo el recorrido, donde el área de evaluación corresponde la zona frontal del vehı́culo. Otro análisis
Figura 6.38: Histograma de rapidez del vehı́culo en grabaciones 3-6
90
Tabla 6.3: Resultados de medidas (1) y (2) de grabaciones 3-6
RMSEsample
% mejor que S1
S1
S2
S3
78,990 · 10−3 [m] 52,4298 · 10−3 [m] 57,8229 · 10−3 [m]
—
63,18 %
59,95 %
Figura 6.39: Medida (3) de grabaciones 3-6. % de celdas para las cuales RMSE es menor que
un umbral.
a esta evaluación corresponde al RMSE seccionado por áreas frente al vehı́culo. En la Figura 6.40 se muestra como se divide el área de evaluación. Las evaluaciones se realizaron
dividiendo las mediciones del sensor frontal en siete segmentos de igual cobertura angular,
y estos segmentos no tienen traslape entre ellos. Por tener una cobertura angular constante,
la superficie que cubre no son iguales, pero el calculo del RMSE se realiza sobre el mismo
numero de observaciones.
Para cada segmento se calcula el RMSE, en base a la Ecuación 6.1 cada segmento tiene
diferentes ı́ndices a y b, pero b − a es entre 34 a 35 mediciones.
Algo particular surge de la existencia de un RMSE mı́nimo esperable explicado en la
Sección 6.4.2 y desarrollado en el Anexo A. Cada segmento tiene un propio RMSE mı́nimo
esperable, siendo mayor hacia la parte central de las observaciones y declinando en medida
que la observación del sensor es más oblicua a la superficie.
En la Figura 6.41 se muestra un gráfico del RMSE por secciones y el RMSE mı́nimo para
esa sección. Las varianzas de las muestras también esta representado. Los datos mostrados en
esta figura corresponden a datos capturados al interior del parque O’higgins por los caminos
pavimentados al costado de una laguna bajo condiciones de conducción normales.
De la Figura 6.41 se pueden realizar variadas observaciones. Por naturaleza del RMSE, la
principal diferencia entre el RMSE medido y el RMSE mı́nimo esperable es la provocada por
el error medio de la proyección de las observaciones. Son mediciones observadas e integradas
a más de 15 [m] hasta que llegan al vehı́culo un error de menos de 0,05 [m].
Con respecto a la curvatura opuesta observada del RMSE con respecto al RMSE mı́nimo
esperable, esto es esperable ya que una pequeña variación en la estimación ángulo roll del
vehı́culo generarı́a un comportamiento de ese estilo por la diferencia de ángulo entre las dos
superficies. Este error de ángulo puede estar dado por una mala estimación o por un pequeño
error en el ángulo de montaje de los sensores.
91
Figura 6.40: Diagrama de secciones del RMSE
0.05
RMSE
minimo RMSE
0.045
RMSE y varianza de RMSE
0.04
0.035
0.03
0.025
0.02
0.015
0.01
0.005
0
−60
−40
−20
0
angulo [grados]
20
40
60
Figura 6.41: Gráfico RMSE por secciones, con varianza y RMSE mı́nimo esperable
Ejemplos de superficies
En esta sección se mostraran algunos ejemplos de las superficies estimadas para realizar
una análisis cualitativo. La información existen en el mapa de elevación es principalmente la
existencia de observaciones sobre una celda y su altura. Además la información de color si
esta disponible puede ser desplegada sobre la celda, permitiendo su uso posterior para una
etapa posterior de procesamiento o solo visualización.
La visualización del mapa de elevaciones puede ser un poco incomoda en un documento
impreso, los ejemplos que se mostraran incluyen una imagen correspondiente al mapa y una
imagen observada por la cámara visible.
En la Figura 6.42 se puede observar la cobertura y el alcance de sistema de mapeo. En el
centro se observa la representación del vehı́culo en conjunto con la trayectoria que ha recorrido
hasta el momento, como una linea roja dejada tras su paso. Este ejemplo esta extraı́do del
comienzo de la grabación número cuatro.
En la escena que se observa se aprecia una superficie sin mayores cambios de alturas, dadas
por las tonalidades de la superficie, hacia la trayectoria del vehı́culo. Hacia los costados del
camino, por el lado derecho se observa una bajada que lleva hasta el borde de una laguna,
donde en el mapa se puede apreciar una brecha sin observaciones que viene dada por la
sombra del borde y por la presencia de agua. En este caso es posible observar sobre el agua
por que esta zona tiene muchas plantas de loto sobre el agua. Esta brecha es claramente
observable y demarca una zona que no debe ser transitada.
92
Figura 6.42: Imagen frontal y vista superior mapa de elevación, color como altura
Hacia el lado izquierdo de la marcha del vehı́culo se puede apreciar en la imagen visible
una subida de altura hacia el pasto, la cual se puede apreciar como un cambio de tonalidad en
la vista superior del mapa. Además se pueden apreciar los troncos de arboles y sus sombras
que generan sobre el mapa. Este tipo de sombras de observación son caracterı́sticas de tener
pocos sensores y no se pueden solucionar teniendo esta limitación.
Figura 6.43: Imagen frontal y vista superior mapa de elevación con coloración
En la Figura 6.43 se puede apreciar otro ejemplo de mapa de altura. En este ejemplo a
diferencia del anterior, los colores representan los colores provenientes de la imagen visible .
Al momento de realizar la proyección de los sensores LIDAR, es buscan su ubicación en la
ultima imagen disponible. Esto es más eficiente y más efectivo que buscar la proyección de
la imagen sobre la superficie ya integrada.
El observar el mapa de esta manera provoca que la percepción de altura se pierde por
completo. Sólo es posible distinguir los puntos que son que no han sido observados en el
mapa. Este ejemplo esta extraı́do del comienzo de la grabación número tres.
En este escena se puede observar como la senda del camino es apreciable fácilmente, pero
las sombras de los arboles dan mucho contraste al piso impidiendo su reconocimiento. En
la información espacial están definidos los bordes y las superficies, como los estaban en la
Figura 6.42.
93
Al lado izquierdo de la marcha del vehı́culo hay una pared que no permite a las observaciones del sensor LIDAR, por eso el corte abrupto que se observa.
Por el lado derecho de la marcha del vehı́culo hay un canal de regadı́o el cual es apreciable
nuevamente por la falta de observaciones que genera la sobra del borde sobre los sensores
LIDAR. Más allá del canal, hay una planicie que esta un poco mas bajo que el camino y
tienen muchos arboles. Estos arboles generan sombras sobre las superficie y por eso que lo
observado tiene sectores sin observaciones.
Uno de los problemas observados en el sistema de percepción es la disposición de los
sensores. Existen situaciones en la limitante de disponer pocos sensores impiden observar de
manera adecuada la superficie próxima al vehı́culo. Observar a mayor distancia permite tener
una mayor distancia de reacción y permite una velocidad mayor de manejo. Pero el observar
a una mayor distancia con el sensor genera que en situaciones de caminos con pendientes,
zonas no sean observadas. Tradicionalmente, esto es solucionado mediante sensores situados
a diferentes distancias. Esto podrı́a ser solucionado mediante un sistema actuado de sensores
permitiendo un ángulo configurable dependiendo de la situación.
Figura 6.44: Imagen frontal y vista superior mapa de elevación con coloración de una superficie
no observada
En la Figura 6.44 se muestra una situación como la descrita anteriormente. Corresponde a
la parte más alta del recorrido de tierra, en donde al terminar el vehı́culo de subir la loma, se
encuentra con una zona plana. Debido a lo empinado de la subida y el ángulo de los sensores,
estos no son capaces de observar el primer trayecto sobre la loma. La extensión de la zona no
observada coincide con la distancia a la cual tocan la superficie los sensores LIDAR. Y esto
podrı́a ser solucionado mediante un sensor LIDAR con pitch variable. En la configuración
original de sensores estas situaciones son cubiertas con el sensor LMS151, mejorando en parte
estas situaciones.
94
Capı́tulo 7
Conclusiones
Índice
7.1. Comunicaciones y Middleware . . . . . . . . . . . . . . . . . . . . . 96
7.2. Caracterı́sticas de los sensores . . . . . . . . . . . . . . . . . . . . . 96
7.3. Diseño e implementación del estimador de estado . . . . . . . . . 96
7.4. Mapa de entorno . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 97
7.5. Aportes del trabajo de tesis . . . . . . . . . . . . . . . . . . . . . . 97
7.6. Trabajo futuro . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 98
7.1.
Comunicaciones y Middleware
Pese a que toda la primera etapa de la tesis alcanzó a estar implementada utilizado LCM,
el cambio a la utilización de ROS fue acertada. ROS en su momento no estaba tan consolidado
ni estable a como es actualmente, por lo que LCM fue elegido en un comienzo por su buenas
prestaciones. Pero con el lanzamiento de ROS Fuerte, el proyecto dejó LCM y se mudó a
la utilización de ROS. Esto permitió principalmente la utilización de las herramientas de
visualización y debuggeo existentes en ROS. Los drivers de sensores que estaban disponibles
en ROS fueron utilizados y los que no, fueron portados de las implementaciones previamente
creadas con LCM.
7.2.
Caracterı́sticas de los sensores
La elección de los sensores utilizados en el vehı́culo no fueron parte de este trabajo de
tesis. Probablemente de haber sido ası́, no se hubiesen seleccionado los mismos sensores.
En base a las propiedades intrı́nsecas de cada sensor, se definió que función debe cumplir
cada sensor dentro del sistema perceptual de manera de aprovechar al máximo la información del conjunto. Como resultado de este análisis, el sensor LD-MRS mostró poseer mejor
desempeño para detección de obstáculos y el LMS291 posee buen desempeño para observar
superficies. El sensor LMS151 se dejó sin una tarea fija de manera de utilizarlo tanto para
detecciones como para evaluaciones dependiendo del objetivo.
De las caracterı́sticas analizadas de los sensores disponibles, se definieron caracterı́sticas
comunes extraı́bles de todos los sensores, ası́ como caracterı́sticas propias para cada tipo de
sensor. Estas caracterı́sticas fueron utilizadas para generar reglas heurı́sticas que permitiesen
en cierta medida clasificar las observaciones de manera sencilla.
7.3.
Diseño e implementación del estimador de estado
El estimador de estado diseñado e implementado mediante un EKF demostró ser una
mejora efectiva con respecto a la estimación entregada por la INS más los datos del vehı́culo
para la estimación local y la coherencia del mapa integrado. El diseño interconectado de la
estimación del ambiente y la estimación del estado entrega resultados satisfactorios, obteniéndose mejores resultados que considerarlos por separado. El considerar las vibraciones del
vehı́culo como parte del error de la estimación permite una mejor fusión de las observaciones.
El método propuesto y utilizado en la evaluación del estimador de estado es una alternativa ad-hoc para el caso de no disponer de un ground truth contra que comparar los
resultados.
Al momento de comparar la trayectoria generada por el estimador de estado mediante
EKF y el estimador de mediante estimación directa no existen grandes diferencias dejando
en evidencia que la principal diferencia esta dada por estimación de la orientación instante a instante más que a largo plazo. Y esto es una de las limitaciones del sistema al no
poder mantener la coherencia global de la trayectoria, lo cual es esperable al no tener una
retroalimentación de esto.
Además de la comparación de las trayectorias se puede concluir que la utilización de una
trayectoria solo basada en el modelo cinemático del vehı́culo serı́a suficiente para realizar una
96
planificación de trayectoria en un corto plazo, como podrı́a ser el alcance de los mapas de
entorno generados.
Del análisis de la sensibilidad del estimador basado en EKF para el parámetro γ se puede
concluir que pese a no a afectar a gran escala la trayectoria, si afecta a una escala menor. Se
generan trayectorias levemente diferentes, las cuales podrı́an ser un aporte a la diferencia de
desempeño entre el estimador EKF y el estimador directo.
7.4.
Mapa de entorno
El diseño de mapa de entorno presentado permite cumplir con la restricción de desempeño
en tiempo real exigido. Además la estructura de mapa implementada permite de manera
versátil incorporar nuevas capas de manera de almacenar nueva información o metodologı́as
de fusión. La utilización de grillas de dos dimensiones para almacenar el entorno tuvo un
buen desempeño para el enfoque planteado.
Su fuerte integración con la estimación del estado del vehı́culo, debido a la utilización
cruzada en las correcciones mantiene coherentes ambas estimaciones.
Pese a no ser una representación realista del ambiente, la información presente es consistente para que un sistema de navegación pueda desplazar el vehı́culo de manera apropiada
bajo las restricciones planteadas en un comienzo.
7.5.
Aportes del trabajo de tesis
Se a estudiado y definido la utilización de una arquitectura de software y la utilización
de un Middleware de apoyo para el sistema de navegación del vehı́culo. Esto permite un
desarrollo de software amigable y escalable.
Se ha generado un procedimiento para aprovechar la información presente en observaciones de sensores láser. La estructura de pipeline presentada para las observaciones permite
implementar de manera sencilla nuevas estrategias de segmentación manteniendo la simpleza
y baja complejidad del software.
Se diseño un procedimiento para la estimación de la relación de viraje de un vehı́culo
y mediante una unidad inercial, sin necesariamente disponer de la información mecánica
correspondiente al vehı́culo. Este procedimiento se puede extender a otro tipo de maquinaria
, incluso si no es similar a la de un vehı́culo utilizado en esta tesis.
El estimador de estado propuesto realiza una estimación adecuada para ser utilizada en
la navegación de un vehı́culo, sin depender de un sistema de localización global. El método
utilizado para retro-alimentar a la orientación de la estimación del estado con la estimación
de la superficie es un aporte no visto anteriormente. Este estimador al estar acoplado con
estimación del entorno, no es posible evaluar su desempeño por si solo. Dado a lo anterior, se
propuso una metodologı́a para la evaluación del estimador del estado basado en mediciones
espaciales en diferentes instantes de tiempo. Esta metodologı́a de evaluación pese a no poder
medir directamente la estimación del estado o la fusión de las medidas, evalúa de manera
adecuada al ser dependiente principalmente de la orientación y el desplazamiento del vehı́culo
durante el proceso de fusión.
Se ha implementado una estructura de representación de entorno de baja complejidad pero
adecuada para ser utilizada en la implementación de un sistema de navegación autónomo.
97
Extra a lo anterior, se ha implementado un conjunto de software anexo que permite implementar de manera estructurada software para el sistema autónomo. También se realizaron
implementaciones de interfaces y sensores que no estaban disponibles anteriormente.
7.6.
Trabajo futuro
Actualmente, la etapa de navegación autónoma del vehı́culo es la etapa faltante para
poder cerrar el lazo de control completamente. Esto permitirı́a la validación de este sistema
perceptual en un sistema autónomo. Probablemente, una vez completado el lazo de control
las etapas requieran de ajustes para un correcto funcionamiento.
Una vez que este sistema esté funcionando, los lı́mites de funcionamiento podrán ser
medidos y se podrá evaluar adecuadamente los sistemas en su conjunto. Por otro lado, nuevas
estrategias de mapeo del entorno podrı́an ser probadas y evaluadas al disponer del sistema
con funcionamiento modular y tener un punto de partida contra el cual poder evaluar estas
nuevas estrategias.
Además una estrategia diferentes podrı́a ser utilizada con respecto a que información es
integrada en el mapa. Si se cambia a solo incorporar caracterı́sticas que puedan ser consideradas peligrosas en vez de tratar de estimar la superficie, esto podrı́a generar mapas más
livianos y algoritmos para genera trayectorias eficientes.
98
Bibliografı́a
[1] Borre, K., Akos, D. M., Bertelsen, N., Rinder, P., and Jensen, S. H. A
software-defined GPS and Galileo receiver: a single-frequency approach. Springer, 2007.
[2] Bradski, G., and Kaehler, A. Learning OpenCV: Computer vision with the OpenCV
library. O’Reilly Media, Inc., 2008.
[3] Budde, W. Calibration of reflectance standards. Standardization in Spectrophotometry
and Luminescence Measurements (1976), 75–85.
[4] Buehler, M., Iagnemma, K., and Singh, S. The 2005 darpa grand challenge.
Springer Tracts in Advanced Robotics 36, 5 (2007), 1–43.
[5] Buehler, M., Iagnemma, K., and Singh, S. The DARPA urban challenge: autonomous vehicles in city traffic, vol. 56. springer, 2009.
[6] Cyrill Stachniss, Udo Frese, G. G. Openslam.org. http://openslam.org/.
[7] DARPA. Darpa grand challenge autonomous ground vehicles. https://web.archive.
org/web/20131205081657/http://archive.darpa.mil/grandchallenge04/. Accessed: 2014-04-14.
[8] Engelhard, N., Endres, F., Hess, J., Sturm, J., and Burgard, W. Real-time
3d visual slam with a hand-held rgb-d camera. In Proc. of the RGB-D Workshop on
3D Perception in Robotics at the European Robotics Forum, Vasteras, Sweden (2011),
vol. 180.
[9] Gat, E., et al. On three-layer architectures, 1998.
[10] Gill, C. D., and Smart, W. D. Middleware for robots. In Proceedings of the AAAI
Spring Symposium on Intelligent Distributed and Embedded Systems (2002).
[11] Graefe, V., and Bischoff, R. From ancient machines to intelligent robots - a
technical evolution -. In Electronic Measurement and Instruments, 2009. ICEMI09. 9th
International Conference on (2009), IEEE, pp. 3 – 418.
[12] Grisetti, G., Stachniss, C., and Burgard, W. Improving grid-based slam with
rao-blackwellized particle filters by adaptive proposals and selective resampling. In Robotics and Automation, 2005. ICRA 2005. Proceedings of the 2005 IEEE International
Conference on (2005), IEEE, pp. 2432–2437.
99
BIBLIOGRAFÍA
[13] Grisetti, G., Stachniss, C., and Burgard, W. Improved techniques for grid
mapping with rao-blackwellized particle filters. Robotics, IEEE Transactions on 23, 1
(2007), 34–46.
[14] Grum, F., and Wightman, T. Absolute reflectance of eastman white reflectance
standard. Appl. Opt 16, 11 (1977), 2775–2776.
[15] Hager, J. W., Behensky, J. F., and Drew, B. W. The universal grids: Universal
transverse mercator (utm) and universal polar stereographic (ups). edition 1. Tech. rep.,
DTIC Document, 1989.
[16] Itseez. Opencv. http://opencv.org/. Accessed: 2014-04-20.
[17] Jazar, R. N. Vehicle dynamics: theory and application. Springer, 2008.
[18] Kohlbrecher, S., Von Stryk, O., Meyer, J., and Klingauf, U. A flexible and
scalable slam system with full 3d motion estimation. In Safety, Security, and Rescue
Robotics (SSRR), 2011 IEEE International Symposium on (2011), IEEE, pp. 155–160.
[19] Leonard, J., How, J., Teller, S., Berger, M., Campbell, S., Fiore, G.,
Fletcher, L., Frazzoli, E., Huang, A., Karaman, S., et al. A perceptiondriven autonomous urban vehicle. Journal of Field Robotics 25, 10 (2008), 727–774.
[20] Manso, L., Bachiller, P., Bustos, P., Núñez, P., Cintas, R., and Calderita, L. Robocomp: a tool-based robotics framework. In Simulation, Modeling, and
Programming for Autonomous Robots. Springer, 2010, pp. 251–262.
[21] Mascaró, M. Diseño e implementación de sistemas de actuación electro-mecánica
para vehı́culos terrestres autónomos. Memoria Ingenierı́a Civil Eléctrica, Universidad
de Chile, 2011.
[22] Miller, I., and Campbell, M. A mixture-model based algorithm for real-time terrain
estimation. Journal of Field Robotics 23, 9 (2006), 755–775.
[23] Mitchell, M., Oldham, J., and Samuel, A. Advanced linux programming. Sams
Publishing, 2001.
[24] Montemerlo, D., Roy, N., and Thrun, S. Perspectives on standardization in
mobile robot programming: The carnegie mellon navigation (carmen) toolkit. In Intelligent Robots and Systems, 2003.(IROS 2003). Proceedings. 2003 IEEE/RSJ International
Conference on (2003), vol. 3, IEEE, pp. 2436–2441.
[25] Moore, D., Olson, E., and Huang, A. Lightweight communications and marshalling for low-latency interprocess communication. DSpace@MIT (2009).
[26] Nuchter, A. 3dtk - the 3d toolkit. http://slam6d.sourceforge.net/.
[27] Nuchter, A., Lingemann, K., Hertzberg, J., and Surmann, H. 6d slam-3d
mapping outdoor environments. Journal of Field Robotics 24, 8-9 (2007), 699–722.
100
BIBLIOGRAFÍA
[28] Nunez, P., Vazquez-Martin, R., del Toro, J. C., Bandera, A., and Sandoval, F. Feature extraction from laser scan data based on curvature estimation for
mobile robotics. In Robotics and Automation, 2006. ICRA 2006. Proceedings 2006 IEEE
International Conference on (2006), IEEE, pp. 1167–1172.
[29] OpenPerception. Pcl: Point cloud library. http://pointclouds.org/. Accessed:
2014-04-20.
[30] OpenSourceRoboticsFoundation. Gazebo. http://gazebosim.org/. Accessed:
2014-04-20.
[31] OpenSourceRoboticsFoundation. Ros: Robot operating system. http://ros.
org/. Accessed: 2014-04-20.
[32] Pfaff, P., Triebel, R., and Burgard, W. An efficient extension to elevation maps
for outdoor terrain mapping and loop closing. The International Journal of Robotics
Research 26 (2007), 217–230.
[33] Qiu, Q., Yang, T., and Han, J. A new real-time algorithm for off-road terrain
estimation using laser data. Science in China Series F: Information Sciences 52, 9
(2009), 1658–1667.
[34] Quigley, M., Conley, K., Gerkey, B., Faust, J., Foote, T., Leibs, J., Wheeler, R., and Ng, A. Y. Ros: an open-source robot operating system. In ICRA
workshop on open source software (2009), vol. 3.
[35] Rossi, C., Russo, F., and Russo, F. Ancient Engineers and Inventions. Springer,
2009.
[36] Rusu, R. B., and Cousins, S. 3D is here: Point Cloud Library (PCL). In IEEE
International Conference on Robotics and Automation (ICRA) (Shanghai, China, May
9-13 2011).
[37] Samet, H. The quadtree and related hierarchical data structures. ACM Computing
Surveys (CSUR) 16, 2 (1984), 187–260.
[38] Sick. LMS200/211/221/291 Laser Measurement Systems : TECHNICAL DESCRIPTION. Sick AG, 12 2006.
[39] Sick. LD-MRS Laser Measurement Sensor : Operating Instructions. Sick AG, 04 2011.
12.
[40] Simmons, R., and Dale, J. Inter-process communication: A reference manual. ipc
version 6.0. Robotics Institute, Carnegie Mellon University (1997).
[41] Sucan, I. A., and Chitta, S. Moveit! http://moveit.ros.org/. Accessed: 201404-20.
[42] Thrun, S., Burgard, W., and Fox, D. Probabilistic robotics. MIT press, 2005.
101
BIBLIOGRAFÍA
[43] Thrun, S., Montemerlo, M., Dahlkamp, H., Stavens, D., Aron, A., Diebel,
J., Fong, P., Gale, J., Halpenny, M., Hoffmann, G., et al. Stanley: The robot
that won the darpa grand challenge. Journal of field Robotics 23, 9 (2006), 661–692.
[44] TimeMagazine. Time magazine. science: Radio auto, houdina., 1925.
[45] Torvalds, L. Git. http://git-scm.com/. Accessed: 2014-04-20.
[46] Tse, R., Ahmed, N., and Campbell, M. Unified mixture-model based terrain estimation with markov random fields. In Multisensor Fusion and Integration for Intelligent
Systems (MFI), 2012 IEEE Conference on (2012), IEEE, pp. 238–243.
[47] Urmson, C., Anhalt, J., Bagnell, D., Baker, C., Bittner, R., Clark, M.,
Dolan, J., Duggins, D., Galatali, T., Geyer, C., et al. Autonomous driving
in urban environments: Boss and the urban challenge. Journal of Field Robotics 25, 8
(2008), 425–466.
[48] Urmson, C., Anhalt, J., Clark, M., Galatali, T., Gonzalez, J. P., Gowdy,
J., Gutierrez, A., Harbaugh, S., Johnson-Roberson, M., Kato, H., et al.
High speed navigation of unrehearsed terrain: Red team technology for grand challenge
2004. Robotics Institute, Carnegie Mellon University, Pittsburgh, PA, Tech. Rep. CMURI-04-37 (2004).
[49] Velodyne. Hdl-64e. https://web.archive.org/web/20140411015142/http://
velodynelidar.com/lidar/hdlproducts/hdl64e.aspx. Accessed: 2014-04-17.
[50] Wallgrun, J. O. Hierarchical Voronoi Graphs: Spatial Representation and Reasoning
for Mobile Robots. Springer, 2009.
[51] wavelength, N. Identifying materials by their reflectivity. Electronically, 2009.
[52] Webster, J. G. The Measurement, Instrumentation, and Sensors: Handbook. Springer,
1999, ch. 9, p. 1.
[53] Welch, G., and Bishop, G. An introduction to the kalman filter, 1995.
[54] Wikipedia. Google driverless car. https://web.archive.org/web/20140402022101/
http://en.wikipedia.org/wiki/Google_driverless_car. Accessed: 2014-04-17.
[55] Williams, M. Prometheus-the european research programme for optimising the road
transport system in europe. In Driver Information, IEE Colloquium on (1988), IET,
pp. 1–1.
[56] Wurm, K. M., Hornung, A., Bennewitz, M., Stachniss, C., and Burgard,
W. Octomap: A probabilistic, flexible, and compact 3d map representation for robotic
systems. In Proc. of the ICRA 2010 workshop on best practice in 3D perception and
modeling for mobile manipulation (2010), vol. 2.
[57] ZeroC. Ice - internet communications engine. http://www.zeroc.com/.
102
BIBLIOGRAFÍA
[58] Zhang, Z. Flexible camera calibration by viewing a plane from unknown orientations. In
Computer Vision, 1999. The Proceedings of the Seventh IEEE International Conference
on (1999), vol. 1, IEEE, pp. 666–673.
[59] Zhang, Z. A flexible new technique for camera calibration. Pattern Analysis and
Machine Intelligence, IEEE Transactions on 22, 11 (2000), 1330–1334.
103
Apéndice A
Cálculo de la esperanza del mı́nimo
RMSE
A. Cálculo de la esperanza del mı́nimo RMSE
Asumiendo una superficie plana, y una estimación de la superficie perfecta, pero un sensor
2
distinto de 0. Bajo esta premisa se obtiene que altura medida obtenida de la
con un σsensor
medición del sensor serı́a:
Zlaser (αi ) = di · cos (αi ) − hsensor
(A.1)
2
.
Donde E[di ] = hsensor /cos (αi ) y V ar[di ] = σsensor
Bajo estas condiciones, se llega a que las E[Zlaser ] y la V ar[Zlaser ] es:
E[Zlaser ] =
V ar[Zlaser ] =
hsensor
· cos (αi ) − hsensor = 0
cos (αi )
2
Zlaser
di
(A.2)
2
2
· σsensor
= cos(αi )2 · σsensor
(A.3)
El RMSE se calcula como:
s
RM SE =
2
i=1 (di · cos (αi ) − hsensor )
PN
(A.4)
N
Por lo que calculando la E[RM SE]:
 sP

2
N
i=1 (di · cos (αi ) − hsensor ) 
E [RM SE] = E 
N
(A.5)
Desarrollando un poco la ecuación se llega a :
 sP
E [RM SE] = E 
2

N
i=1 Zlaser (αi ) 
=
v
h
i
u PN
2
u
E
Z
(α
)
i
laser
t i=1
N
N
(A.6)
2
Como E Zlaser
= V ar [Zlaser ] + |E [Zlaser ]|2 :
h
i
s
E [RM SE] = σsensor ·
2
i=1 cos (αi )
PN
(A.7)
N
2
Para el caso particular del sensor utilizado en esta evaluación, σsensor
= 0,0122 , además
los ángulos que se utilizan van entre −60◦ y 60◦ con un paso de 0,5◦ ,por esto:
−pi −pi
αi = −
,
3
3
pi
∆αi =
→ N = 241
360
De esta manera el calculo de la sumatória:
N
X
cos (αi )2 = 169,8683
(A.8)
(A.9)
(A.10)
i=1
Por lo que el RMSE minimo para ese sensor:
s
E [RM SE] = σsensor ·
169,8683
= 0,8396 · σsensor = 0,0101
241
(A.11)
105
Apéndice B
ROSBAGs
B. ROSBAGs
Archivo
Fecha
Hora
Duración
Distancia
Información
Observaciónes
2014-01-16-10-15-09.bag
16 de Enero de 2014
10:15:09
239 [s]
832 [m]
GPS, Gateway, PS3-Eye, LDMRS, LMS151, LMS291
IMU Nav440, PS3-Move, Radar ESR
LMS151 montado frontal
6
velocidad [m/s]
5
4
3
2
1
0
0
50
100
150
200
250
segundos [s]
107
B. ROSBAGs
Archivo
Fecha
Hora
Duración
Distancia
Información
Observaciónes
2014-01-16-10-19-17.bag
16 de Enero de 2014
10:19:17
144 [s]
802 [m]
GPS, Gateway, PS3-Eye, LDMRS, LMS151, LMS291
IMU Nav440, PS3-Move, Radar ESR
LMS151 montado frontal
14
12
velocidad [m/s]
10
8
6
4
2
0
0
50
100
150
segundos [s]
108
B. ROSBAGs
Archivo
Fecha
Hora
Duración
Distancia
Información
2014-01-16-10-23-54.bag
16 de Enero de 2014
10:23:54
150 [s]
778 [m]
GPS, Gateway, PS3-Eye, LDMRS, LMS151, LMS291
IMU Nav440, PS3-Move, Radar ESR
LMS151 montado frontal
Observaciónes
14
12
velocidad [m/s]
10
8
6
4
2
0
0
20
40
60
80
segundos [s]
100
120
140
160
109
B. ROSBAGs
Archivo
Fecha
Hora
Duración
Distancia
Información
2014-01-16-10-27-42.bag
16 de Enero de 2014
10:27:42
100 [s]
780 [m]
GPS, Gateway, PS3-Eye, LDMRS, LMS151, LMS291
IMU Nav440, PS3-Move, Radar ESR
LMS151 montado frontal
Observaciónes
16
14
velocidad [m/s]
12
10
8
6
4
2
0
0
20
40
60
segundos [s]
80
100
120
110
B. ROSBAGs
Archivo
Fecha
Hora
Duración
Distancia
Información
2013-12-26-12-25-57.bag
26 de Diciembre de 2013
12:25:57
313 [s]
833 [m]
GPS, Gateway, PS3-Eye, LDMRS, LMS151, LMS291
IMU Nav440, PS3-Move, Radar ESR
LMS151 montado frontal
Observaciónes
5
velocidad [m/s]
4
3
2
1
0
0
50
100
150
200
250
300
350
segundos [s]
111
B. ROSBAGs
Archivo
Fecha
Hora
Duración
Distancia
Información
2013-12-26-12-31-43.bag
26 de Diciembre de 2013
12:31:47
260 [s]
791 [m]
GPS, Gateway, PS3-Eye, LDMRS, LMS151, LMS291
IMU Nav440, PS3-Move, Radar ESR
LMS151 montado frontal
Observaciónes
6
velocidad [m/s]
5
4
3
2
1
0
0
50
100
150
segundos [s]
200
250
300
112
B. ROSBAGs
Archivo
Fecha
Hora
Duración
Distancia
Información
Observaciónes
2013-11-07-11-32-06.bag
07 de Noviembre de 2013
11:32:06
202 [s]
1028.2 [m]
GPS, Gateway, PS3-Eye, LDMRS, LMS151, LMS291
IMU Nav440, PS3-Move, Radar ESR
LMS151 montado en parrilla
8
velocidad [m/s]
6
4
2
0
0
50
100
150
200
250
segundos [s]
113
B. ROSBAGs
Archivo
Fecha
Hora
Duración
Distancia
Información
2013-11-07-11-35-46.bag
07 de Noviembre de 2013
11:35:46
169 [s]
1233.8 [m]
GPS, Gateway, PS3-Eye, LDMRS, LMS151, LMS291
IMU Nav440, PS3-Move, Radar ESR
LMS151 montado en parrilla
Observaciónes
12
velocidad [m/s]
10
8
6
4
2
0
0
20
40
60
80
100
120
140
160
180
segundos [s]
114
B. ROSBAGs
Archivo
Fecha
Hora
Duración
Distancia
Información
Observaciónes
2013-11-07-11-42-30.bag
07 de Noviembre de 2013
11:42:30
245 [s]
953.8 [m]
GPS, Gateway, PS3-Eye, LDMRS, LMS151, LMS291
IMU Nav440, PS3-Move, Radar ESR
LMS151 montado en parrilla
7
velocidad [m/s]
6
5
4
3
2
1
0
0
50
100
150
200
250
segundos [s]
115
Apéndice C
Paper
J Intell Robot Syst
DOI 10.1007/s10846-014-0087-9
A Kalman-filtering-based Approach for Improving Terrain
Mapping in off-road Autonomous Vehicles
Isao Parra-Tsunekawa · Javier Ruiz-del-Solar ·
Paul Vallejos
Received: 4 February 2014 / Accepted: 15 July 2014
© Springer Science+Business Media Dordrecht 2014
Abstract The generation of accurate terrain maps
while navigating over off-road, irregular terrains is a
complex challenge, due to the difficulties in the estimation of the pose of the laser rangefinders, which is
required for the proper registration of the range measurements. This paper addresses this problem. The
proposed methodology uses an Extended Kalman filter to estimate in real-time the instantaneous pose of
the vehicle and the laser rangefinders by considering
measurements acquired from an inertial measurement
unit, internal sensorial data of the vehicle and the estimated heights of the four wheels, which are obtained
from the terrain map and allow determination of the
vehicle’s inclination. The estimated 6D pose of the
laser rangefinders is used to correctly project the laser
measurements onto the terrain map. The terrain map is
a 2.5D map that stores in each cell the mean value and
variance of the terrain height. In each map’s cell position, new laser observations are fused with existing
I. Parra-Tsunekawa · J. Ruiz-del-Solar ()
Department of Electrical Engineering,
Universidad de Chile, Santiago, Chile
e-mail: [email protected]
I. Parra-Tsunekawa
e-mail: [email protected]
I. Parra-Tsunekawa · J. Ruiz-del-Solar · P. Vallejos
Advanced Mining Technology Center,
Universidad de Chile, Santiago, Chile
e-mail: [email protected]
height estimations using a Kalman filter. The proposed
methodology is validated in the real world using an
autonomous vehicle. Field trials show that the use of
the proposed state estimation methodology produces
maps with much higher accuracy than the standard
approaches.
Keywords Terrain mapping · Off-road autonomous
vehicles · Kalman Filter · Information fusion · Laser
rangefinder pose estimation
1 Introduction
Terrain mapping is a relevant problem in autonomous
robot navigation; the accurate modeling of the terrain
is required for the robot in order to decide a safe path
to follow. In off-road applications, the generation of
accurate terrain maps while navigating is a difficult
challenge due to the difficulties in estimating the pose
of the laser rangefinders, which is required for the
proper registration of the range measurements. Even
small pose errors could be magnified onto large errors
in the projected positions of laser points, because the
laser rangefinders are aimed at the road up to several
meters in front of the vehicle [13].
We address the problem of real-time terrain mapping for autonomous vehicles such as cars and trucks
that move at low (<10[m/s]) and medium speeds
(<20[m/s]) for off-road terrain applications, as in the
case of the autonomous truck operation in open pit
J Intell Robot Syst
mines, where typical speeds are in the range of 5-14
[m/s]. We address the specific problem of building
coherent semi-local terrain maps with laser range finders whose 6D pose changes due to the irregular surface
that the vehicle is traversing and the non-rigid characteristics of the vehicle (vehicle suspension system).
We use the term semi-local map to denote a map that:
(1) is built without using GPS information, (2) uses a
global reference system that is defined at the beginning of the map estimation, (3) has an extension of
several hundred meters, and (4) can be integrated onto
the global map using the GPS and inertial data. We use
semi-local maps because we are interested in achieving a very accurate 6D pose estimation of the laser
range finders; the use of standard GPS (no differential)
in this estimation process would produce inaccuracies that would be reflected in the generated map.
The semi-local map can be used for taking local decision (obstacle avoidance, decision about which path to
follow) and/or for building the global map.
The proposed methodology uses an Extended
Kalman filter for estimating in real-time the instantaneous pose of the vehicle and the laser rangefinders
by considering measurements (observations in the
Kalman filtering terminology) acquired from an inertial measurement unit (3D angular pose and speed, and
3D linear acceleration), internal sensorial data of the
vehicle (vehicle speed and angle of the steering wheel)
and the estimated heights of each wheel, which are
obtained from the terrain map and allow determining
the vehicle’s inclination. The estimated 6D pose of the
laser rangefinders is used to correctly project the laser
measurements onto the terrain map. The terrain map
is a 2.5D map that stores the mean value and variance of the terrain height. In each map’s cell position,
new laser observations are fused with existing height
estimations using a Kalman filter.
The main contribution of this work is the proposal
of an integrated sensor-map estimation methodology
in which: (i) Kalman filters estimate the laser range
finder 6D pose and fuse past and new observations in
each cell of the terrain map and (ii) the estimated terrain map is fed back and used for improving the 6D
pose estimation of the laser range finders (past estimations of the terrain heights are used for determining
the current heights of the wheels and therefore the
inclination of the vehicle).
The methodology is validated in the real world
(O’Higgins Public Park, located near downtown area
in Santiago de Chile, Chile), using the AMTC’s
autonomous vehicle. Field trials show that the use of
the proposed state estimation methodology produces
maps with higher accuracy than approaches that use
the inertial data directly (without any state estimator)
for correcting the pose of the laser range finders.
The paper is organized as follows: Related work is
described in Section 2; The proposed Kalman-filtering
based methodology for terrain mapping is explained in
Section 3; Descriptions of experiments and results are
presented in Section 4 and Conclusions of this work
are given in Section 5.
2 Related Work
Terrain map estimation is an important component in
outdoor mobile robot navigation, and several authors
have addressed it ([2–13]). Some of them have concentrated their effort on the fusion problem [12] while
others have addressed the representation of the terrain
data within the map [5] or the use of elevation functions over a 2D domain to represent the map [3]. Other
authors have tackled the filtering of the laser range
finder data to improve map quality [8], the assessment
of the traversability of the terrain [9], the accurate registration of the laser range finder data [10–13] or the
loop-closing problem when building a global map [5].
Some of these works use small rovers in the experiments or cars moving at low speeds (<10 [m/s]) while
in some cases the map is built offline [5].
Different types of representations have been used to
map terrains (e.g., 2D occupancy grid, 3D occupancy
voxel), being the elevation or 2.5D height map one of
the preferred options in ground robot mapping applications (for example [10–12]) due to their compactness, lower computation and insensitivity to height
discretization errors [12]. Elevation maps “store in
each cell of a discrete grid the height of the surface
at the corresponding place in the environment” [5]. In
some mapping systems the height’s variance is also
stored in each cell [10, 11].
The estimation of the terrain height in each cell
in a 2.5D map requires the correct fusion of measurements obtained at multiple times while moving.
The uncertainty of the measurements and the uncertainty of the sensor pose must be considered during the
fusion process. This is addressed in [10] by proposing a mixture-model terrain estimation framework
J Intell Robot Syst
that incorporates both uncertainties, together with the
measurement-map association and the in-cell measurement fusion into the map. The terrain mapping
and estimation algorithm uses a probabilistic analysis of the errors, assuming their Gaussian distribution.
The system projects the sensors’s detections onto the
global coordinate frame using a tightly coupled model
of the sensors position, making the respective formal
error propagation. The vehicle localization is obtained
from a GPS, without the use of any state estimation
algorithm
In [11], three enhancements were proposed for
improving the real-time performance of the system
proposed in [10]: a selection window was used to
limit the probability distribution area of each measurement, a clustering algorithm was used to simplify
the object detection tasks and a virtual point vector
was introduced to reduce the computational cost of
the algorithm. Despite these enhancements, no fundamental changes were made to the terrain estimation
framework proposed in [10].
In [12], a Markov Random Field (MRF) representation of a sensor and terrain fusion model was
proposed. The MRF represents the model of 2.5D map
sensor uncertainties, enabling a probabilistic fusion
between the sensor and terrain information. This
approach is also based on [10], but it does not make the
assumption of independency among cells in the map.
In Thrun [13], a first-order Markov model is proposed for modeling the drift of the pose estimation
error over time. The heights of the map points are
assumed Gaussian with a variance that scales linearly with the time difference of the points. Then,
a probabilistic test is used to detect the presence of
an obstacle. Thus, that work focuses on the detection of obstacles rather than on providing an accurate
map. The methodology proposed in this publication
addresses both problems; it is able to compute an accurate semi-local map in real-time, which can be then
used for obstacle avoidance.
The main difference of the current methodology
compared with [10–13] is that in the methodology proposed in this paper, a non-rigid sensor location and
vehicle suspension is assumed. Therefore, the noise in
the sensor pose associated with the sensor and vehicle vibration is explicitly considered. This vibration is
estimated in real-time using the Inertial Measurement
Unit (IMU) measurements. Furthermore, the vehicle
pose is estimated using an Extended Kalman filter,
avoiding the extreme dependency upon the GPS that
most terrain map estimation systems have.
In [5], an approach that allows a mobile robot to
deal with vertical and overhanging objects in elevation maps is presented. The approach uses four classes
(traversable cells, vertical gaps, vertical structures and
locations sensed from above) for classifying locations in the environment and dealing with overhanging
structures. The map of elevations is updated using a
Kalman filtering fusion approach (in each cell new
data is fused with existing estimations by considering
their variances). In that work the loop closing in the
map is solved offline and the vehicle moves on paved
roads at low speeds.
In [6], a near real-time ground segmentation system based on Gaussian Process for autonomous land
vehicle is proposed. Terrain is segmented into ground
and non-ground, and the average processing time is
74.93 [ms], being near real-time. Terrain mapping systems built using the methodology proposed in this
paper are able to process the data in real-time, having
a processing time of ∼3 [ms] (see Section 4.3).
In summary, one of the main contributions of this
work is the proposal of a real-time terrain map estimation system enhanced by the estimation of the noise
produced by the sensors and vehicle vibration, its use
in the measurement fusion process, and the estimation of the vehicle and sensors pose without the GPS
dependency.
3 A Kalman-Filtering Based Methodology
for Terrain Mapping
3.1 System Overview
The proposed methodology for terrain mapping
has three main stages: Vehicle State Estimation,
Laser Projection and Terrain Map Update (see block
diagram in Fig. 1). The first stage estimates the full 6D
pose of the vehicle using an Extended Kalman filter.
For correcting the predicted pose, it uses: (i) observations acquired from the IMU, (ii) vehicle internal
sensors whose measurements are available in the CAN
bus (Controller Area Network), and (iii) virtual observations obtained from the terrain map. The second
stage generates a projection of the laser rangefinders measurements onto a global reference system
using the 6D pose of the laser rangefinders, which is
J Intell Robot Syst
Fig. 1 Block diagram of
the proposed estimation
system
computed from the vehicle’s pose. Finally, the Terrain
Map Update stage accumulates the laser observations
in the terrain map. The terrain map stores, in each cell,
the mean value and variance of the terrain height. A
Kalman filter is used in each cell for fusing new laser
observations with existing height estimations.
3.2 Vehicle Model
A simplified diagram of the vehicle model is shown
in Fig. 2. The vehicle model corresponds to a standard
four-wheel front-steering vehicle with laser rangefinders and an IMU mounted on a roof rack. The distance between the front and rear axes is called the
wheelbase, while the distance between the steerable
wheels is known as the track. The vehicle kinematics
is given by the following variables (see reference sys
tems in Fig. 2): global position p t , 3D angular pose
q t , speed vt , instantaneous rotation radius rt , angular
speed ωt , and steer angle αt . The angle αt is defined
as the equivalent steer angle of a bicycle with both
the same wheelbase and rt as the actual vehicle. This
angle is related to the steer angles of the inner and
outer wheels, αti and αto respectively, as shown in
Eq. (1) [1],
cot αt =
cot αto + cot αti
2
(1)
sensors data (e.g., speed) that is measured by the
vehicle itself and that is available in the CAN bus.
The IMU provides the following data: angular
imu
pose ϑ t
The variables p t and q t are measured in the global
reference system SG , with the x axis pointing towards
the east, the y axis point towards the north and the
z axis pointing upwards. θ, φ, and ψ denote rotation
angles around the x, y and z axes, respectively.
3.3 Sensor Models and Variables
The following information sources are used in the
vehicle: an IMU, laser rangefinders and internal
imu
∈ R3 and linear
acceleration a t ∈ R3
By considering a laser reference system Slaser (see
Fig. 2b), a laser rangefinder provides a sequence
laser
=
of measurements in polar coordinates ρ t,i
T
dt,i θt,i ; i ∈ 1, ..., Nsamples , where: dt,i is
the measured distance, θt,i the measured angle and
Nsamples the number of samples at time step t. In case
that more than one laser rangefinder is used, the index
j will be used to identify laser rangefinder number j,
j
as in dt,i
The data provided by the internal sensors of the
vehicle through the CAN bus interface are the angle of
the steering wheel αtvehicle and the linear speed of the
vehicle
∈ R4 . This vector contains the speed
wheels v t
of the front-left, front-right, rear-left and rear-right
wheels.
3.4 Vehicle State Estimation
The state vector x t includes all relevant variables that
define the state of the vehicle:
T
x t = p t q t vt αtwheel ηt
imu
∈ R3 , angular speed ωt
(2)
with p t a 3D-vector representing the position of SL
relative to SG , q t a quaternion representing the orientation of SL relative to SG , vt a scalar representing
the speed, αtwheel a scalar representing the angle of
the steering wheel, and ηt the variance of the vehicle’s
vibration in the z axis. The problem of state estimation
is solved in this case through the implementation of a
Kalman filter.
J Intell Robot Syst
Fig. 2 Geometry, reference
systems, and relevant
variables (see main text for
details): a Top and b lateral
views
(a)
(b)
In our model u t is supposed unknown1 and f is
given by:
3.4.1 Extended Kalman Filter: Prediction Step
For the prediction step of the Extended Kalman filter,
the following standard equations are used:
x t = f x t−1 , u t , 0
−
Pt− = At · Pt−1 · ATt + Q
(3)
with At the Jacobian matrix of partial derivatives of
f with respect to x t , Q the process noise covariance
and u t the actuation order. The process noise is characterized and Q is estimated by means of field trial
experiments.
−
−
∗
p t = p t−1 + q t−1 · p t · q t−1
q t = q t−1 ◦ q t
vt− = vt−1
wheel
αtwheel − = αt−1
−
ηt = ηt−1
1 Even
(4)
though, u t is known when the vehicle operates
autonomously, we decided to estimate the pose change in order
to have a more general model and a more robust system; in
many situations, such as, vehicle slippage, there is a significant
difference between the given and executed orders.
J Intell Robot Syst
with p t and q t the 3D position and orientation
change, respectively; q t is a quaternion.
The change of the vehicle’s position, in the
local/vehicle reference system SL , is computed as:
⎞
⎛
sin(ω
·
t)
r
t
t
⎝ rt (1 − cos(ωt · t)) ⎠ if αtwheel = 0
⎛ 0 ⎞
p t = vt · t
⎝
0 ⎠
if αtwheel = 0
0
−1
− T
Kt = Pt− HtT H
P
H
+
R
t
t
t
t
−
−
x t = x t + Kt zt − h( x t )
with rt and ωt the estimated rotation radius and speed
and t the time interval.
The trajectory of the vehicle in t is approximated
as circular, and ωt and rt are approximated using the
estimated steering wheel angle and linear speed (see
derivation in Appendix 1) as:
αt
(6)
·γ
with γ an adjusting factor that depends of the vehicle. As mentioned, in case when αtwheel = 0, ωt = 0
and rt = ∞, and the trajectory is approximated as a
straight line.
The angular pose difference is computed as:
q t = qY P R (ωt · t, 0, 0)
3.4.2 Extended Kalman Filter: Correction Step
For the correction step of the Extended Kalman filter,
the following standard equations are used:
(5)
ωt vt · αtwheel · γ
1
rt = ωvtt wheel
(iii) a proper error projection of the angular errors to
the quaternion space.
(7)
with qY P R given by:
qY P R (ψ, ϕ, θ )
⎡ ⎤
cos θ2 ·cos ϕ2 ·cos ψ2 +sin θ2 ·sin ϕ2 ·sin ψ2
⎢ ⎥
ϕ ϕ θ ⎢
ψ
ψ ⎥
θ
⎢sin 2 ·cos 2 ·cos 2 −cos 2 ·sin 2 ·sin 2 ⎥
⎢
⎥
=⎢ θ ϕ ϕ ψ
ψ ⎥
⎢cos 2 ·sin 2 ·cos 2 +sin θ2 ·cos 2 ·sin 2 ⎥
⎣ ⎦
cos θ2 ·cos ϕ2 ·sin ψ2 −sin θ2 ·sin ϕ2 ·cos ψ2
(8)
Pt = (I
(9)
− Kt Ht )Pt−
with Ht the Jacobian matrix of partial derivatives
−
of h with respect to x t , Rt the measurement noise
covariance matrix and Kt the Kalman gain.
The corrections are done sequentially using the perceptual data obtained from the IMU and the vehicle
internal sensors as well as using virtual observations
obtained from the map of elevations, which is considered as a soft sensor.
In our modeling, we choose to transform the different measured variables (e.g., angular speed around the
zaxis) from the sensor measurement space onto the filter state space by means of its exact linearization. In
this way, the resultant h is the identity function and Ht
is an identity matrix. This transformation preserves the
higher order statistical effects, improving the Kalman
filter performance [10]. Naturally, this means that
the Rt matrices corresponding to each information
sources must be estimated very accurately in order
to have a proper Kalman-filtering-based fusion. The
measurement noise covariance matrices corresponding to the IMU and the vehicle measurements are
time invariant and are denoted as R I MU and R vehicle ,
respectively. The noise covariance matrix associated
with the virtual measurement, provided by the map of
map
elevations, is denoted as Rt
and is time variant.
The IMU provides observations that allow updating the pose, steer angle and vibration’s variance
I MU
The estimation of the noise covariance matrix Q
considers the following elements: (i) it is assumed that
the noise components are not correlated and therefore
that Q is diagonal, (ii) the variance of the different
components is estimated using the technical specifications of the sensors (IMU’s accelerometers and
gyroscopes, internal sensors of the vehicle) and the
vehicle’s speed range given by the application, and
. The
components of the observational vector z t
orientation measurements θtI MU , φtI MU , and ψtI MU
I MU
are used directly to update q t
I MU
qt
:
= qYP R ψtI MU , φtI MU , θtI MU
with qYP R given by Eq. (8)
(10)
J Intell Robot Syst
Fig. 3 Rotation angles
around the x and y axes
By using Eq. (6), the observation of the steering
wheel angle is obtained from the IMU angular speed
around the z axis, ωtz , as:
ωtz
(11)
vt · γ
The quantification of the vehicle vibration in the
z axis is estimated as the variance of the IMU’s
acceleration in the z axis:
αtI MU =
N
f −1 ηtI MU =
i=0
z
at−i
− ātz
2
(12)
Nf
with Nf the number of samples and ātz the mean value
of the acceleration in the z axis in the same period.
I MU
Finally, z t
⎛
I MU
zt
is given by2 :
⎞
∗
⎜ I MU
⎜ qt
⎜
=⎜ ∗
⎜ I MU
⎝ αt
ηtI MU
⎟
⎟
⎟
⎟
⎟
⎠
(13)
The vehicle provides measurements that allow
updating the speed and steering wheel angle com-
with vtvehicle,rl /vtvehicle,rr the speed of the rear-left
/rear-right wheel.
The map of elevations provides virtual observations
that allow updating the vehicle pose components of
map
fl
rr
the observational vector z t . Let hrl
t , ht , ht and
fr
ht be the heights of the rear-left, rear-right, front-left
and front-right wheels, respectively. These heights are
obtained by evaluating the terrain map (mapheight [·][·]
array defined in Eq. (22)), with the normalized x and
y coordinates of the contact point of the wheels on the
ground (see details in Section 3.6). Then, the rotation
angles around the x and y axes are given by (see Fig. 3
for details):
f ront
h̄rear
−
h̄
map
t
t
φt
= asin
wheelbase
lef t
(15)
right
h̄t − h̄t
map
= asin
θt
track
rl
f ront
rear
ht + hrr
t
with
h̄
=
=
t 2, h̄t fl
fr
f
l
lef t
ht + hrl
ht + ht
t
, h̄t
=
2
2 and
fr
rr
right
h̄t
= ht + ht
2.
vehicle
ponents of the observational vector z t
. Given
that the vehicle’s internal sensors directly provide the
speed of the four wheels and the angle of the steering
vehicle
wheel z t
vehicle
zt
has the following form:
⎞
∗
⎟
⎜
⎜ vehicle,rl∗ vehicle,rr ⎟
1
⎜
= ⎜ 2(vt
+ vt
)⎟
⎟
vehicle
⎠
⎝
αt
∗
⎛
(14)
2 We
use “*” to indicate that this component of the observation vector does not exists and, therefore, it is not used in the
corrective stage.
Fig. 4 AMTC’s Autonomous Vehicle
J Intell Robot Syst
Table 1 Vehicle model
parameters
(Volkswagen®Tiguan)
E
M
Izz
lf
lr
Cf
Cr
ρ
ε
A
Petrol Engine 4-cylinders 2.0L 125KW (16V) Turbo FSI
Mass
Moment of Inertia with respect to z-axis
Distance from front axle to CoG position (wrt. x-axis)
Distance from rear axle to CoG position (wrt. x-axis)
Characteristic curves of the front tires
Characteristic curves of the rear tires
Atmospheric density
Aerodynamics coefficient
Frontal area
Spin rate steering wheel–tire
Acceleration 0-100 [km/h]
Dry pavement friction coefficient
µ
map
Finally, z t
⎛
map
zt
⎡
is given by:
map
with Rt,q
⎞
∗
⎜ qY P R ψt , φtmap , θtmap ⎟
⎜
⎟
⎟
∗
=⎜
⎜
⎟
⎝
⎠
∗
∗
map
Rt
0 0
map
⎢ 0 Rt,q
⎢
=⎢
⎢0 0
⎣0 0
0 0
0
0
0
0
0
0
0
0
0
0
⎤
0
0⎥
⎥
0⎥
⎥
0⎦
0
1
0
0
0
0
1
0
⎤
0
0⎥
fl
fr
· max χt , χt , χtrl , χtrr
0⎦
1
(18)
(16)
fl
with qY P R given by Eq. (8), and ψt the current estimation of the rotation angle around z.
The noise covariance matrix associated to the virtual measurement provide by the map of elevations
map
Rt , is computed as:
⎡
1
⎢0
=⎣
0
0
1590[Kg]
2695[Kg·m2 ]
1.302[m]
1.302[m]
7791[Kg· m/s(rad)]
7791[Kg· m/s(rad)]
1.225[Kg/m3 ]
0.33
3.049974[m2 ]
1/14.7
10.4[s]
0.8
fr
where χt , χt , χtrl and χtrr are the variances associated to the heights of the rear-left, rear-right, front-left
and front-right wheels, respectively. These values are
(17)
0
Table 2 AMTC’s Autonomous Vehicle sensors
Sensor Type
Model
LIDAR
LIDAR
LIDAR
IMU
CAMERA
CAMERA
RADAR
CAN Interface
SICK, LMS291-S05
SICK, LMS151
SICK, LD-MRS HD
Crossbow, NAV440
AVT, Manta G046C
Sony, PlayStation®Eye Camera
Delphi, ESR
Kvaser, Leaf Light HS
Fig. 5 Example of measurements obtained by the LMS151
(green), LMS291 (red) and LD-MRS (blue) laser rangefinders
J Intell Robot Syst
(a)
(b)
(c)
Fig. 6 a Satellite view of the unpaved road, b Example of pictures of the road and c laser rangefinder data used as ground truth.
Source: Google Maps satellite image
obtained by evaluating the variances of the terrain map
(mapvar [·][·] array defined in Eq. (23)), with the normalized x and y coordinates of the contact point of the
wheels on the floor (see details in Section 3.6).
The noise covariance matrices R I MU and R vehicle
are estimated in a standard sensor calibration process.
3.5 Laser Projection
The laser measurements are acquired in polar coordinates, relative to the laser rangefinder device (see Subsection 3.3). In order to integrate laser measurements
acquired at different times, by one or more different
laser rangefinders, measurements are first transformed
into Cartesian coordinates, then projected onto the
local reference system SL , and finally onto the global
reference system SG . For the first projection, it is necessary to know the laser pose in SL , whichis given
SL
SL
by the 3D vector p laser and the quaternion q laser .
The vehicle’s pose is used for the second projection.
SG
Thus, the laser measurements ρ t,i ∈ R3 are obtained
as:
T
laser
ρ t,i = dt,i · cos θt,i dt,i · sin θt,i 0 ; (19)
i ∈ 1, ..., Nsamples
S
T
L
SG
laser
SG SG SG
ρ t,i = xt,i
= q t · q laser · ρ t,i
yt,i zt,i
SL
SL
·(q laser )∗ + plaser · (q t )∗
+pt ; i ∈ 1, ..., Nsamples
(20)
For each laser measurement, the variance of the
measurement, χt,i , is computed. The two main factors
affecting this value are: the measured distance dt,i and
the variance of the vehicle’s vibration in the z axis.
Then, χt,i is given by:
2
+ κη · ηt + κbase
χt,i = κd · dt,i
(21)
where: κd , κη , and κbase are adjusting factors empirically set to 0.04, 0.5 and 0.1, respectively.
3.6 Terrain Map Update
Table 3 Recording’s characteristics
Recording ID Ground Class Speed Range Weather Condition
1-2
3-6
Unpaved
Unpaved
Low
Medium
sunny/dry
sunny/dry
The terrain map is organized as a 2.5D height map,
composed by cells of size Mx · My [m2 ]. The laser
SG
measurements ρ t,i ; i ∈ 1, ..., Nsamples are stored
in the map. Since more than one laser measurement
J Intell Robot Syst
can be mapped onto the same cell and that laser measurements corresponding to different time instants can
also be mapped onto the same cell, the variance of the
laser measurement is considered in the fusion process.
Hence, the map stores two values in each cell position: the measured height, mapheight , and the variance
mapheight [Xt,i ][Yt,i ] =
of the measurement, mapvar . By assuming that laser
measurements and map height estimation have Gaussian distributions, they can be optimally fused using
a Kalman filtering approach [14]. Thus, in each map
cell position(Xt,i , Yt,i ), a 1D Kalman filter estimates
the state variable (map height) and its variance as:
SG
χt,i · mapheight [Xt,i ][Yt,i ] + mapvar [Xt,i ][Yt,i ] · zt,i
mapvar [Xt,i ][Yt,i ] + χt,i
; i ∈ 1, ..., Nsamples
(22)
mapvar [Xt,i ][Yt,i ] =
mapvar [Xt,i ][Yt,i ] · χt,i
; i ∈ 1, ..., Nsamples
mapvar [Xt,i ][Yt,i ] + χt,i
SG
SG
where Xt,i = xt,i
/Mx and Yt,i = yt,i
/My are
normalized cell coordinates.
4 Experimental Results
4.1 AMTC Autonomous Vehicle
The Advanced Mining Technology Center (AMTC) of
University of Chile developed an autonomous vehicle
whose final goal is autonomous navigation inside open
pit mines. For this application typical truck speeds are
in the range of 5-14 [m/s] and navigation based on
pure GPS is not reliable, because insome deep sections of the pit GPS signal is not always available.
Therefore, robust terrain mapping systems are of high
interest.
The AMTC autonomous vehicle corresponds to
a standard Volkswagen Tiguan 2010 (see picture in
Fig. 4). The roof rack has aluminum-extruded profiles
installed for mounting sensors. The vehicle parameters are presented in Table 1. A list of installed sensors
is presented in Table 2. The original Tiguan vehicle was mechanically and electronically modified in
order to make it autonomous. The modifications can
be outlined as follow: the steering wheel is actuated
by a brushless motor connected by a chain to the
steering column therefore both rotate synchronously.
The brake pedal is pulled by a steel rope, which is
moved by a linear actuator, placed on the co-pilot’s
footrest. The control hardware of the accelerator pedal
(23)
and handbrake were modified, therefore both devices
are electronically controlled. The vehicle’s internal
sensors data is directly acquired from the CAN bus
interface. A rack with electronic controllers and computers was installed in the vehicle’s trunk. The data
acquisition and control routines run on an automotive standard Intel i7 610E @2.53GHz (4GB RAM)
computer running ROS-Fuerte on Ubuntu 12.04. More
details on the control modules used in this vehicle can
be found in [15, 16].
4.2 Evaluation Dataset
Evaluation data was recorded inside O’Higgins Public Park, located near downtown area in Santiago de
Chile, Chile. The aforementioned data were captured
while the vehicle was driven on unpaved, rough terrain
at low (<10 [m/s]) and medium speeds (<20[m/s]).
Each recording have measurements from three laser
rangefinders, one IMU, one radar, two cameras, one
GPS and the vehicle’s CAN bus data (see device’s
models in Table 2).
As explained previously, the terrain map is built
using the laser measurements. The SICK LMS291 and
LMS151 rangefinders have one scanning plane while
the SICK LD-MRS HD has four scanning planes. The
three laser rangefinders are placed with different and
fixed pitch angles, thus are capable of taking measurements at different distances in front of the vehicle.
The SICK LMS291’s scanning plane touches the floor
at 16 [m] in front of the vehicle, while the SICK
J Intell Robot Syst
Fig. 7 Percentage of the total traveled distance versus the vehicle’s speed for recordings 1-2 and 3-6
LD-MRS’s inferior scanning plane touches the floor
at 19[m] in front of the vehicle. The SICK LMS151
is mounted vertically pointing to the floor. This sensor is used as ground truth for the map’s integration.
An example of the laser measurements is shown on
Fig. 5.
The unpaved area is formed by a very irregular
road having a length of about 800 [m] with positive and negative slopes (see Fig. 6a). The road is
a track made of ground surrounded with grass and
some trees. The height difference between the lowest and the highest point of the track is about 4
meters. In this area six recordings with different speed
ranges and driving directions were taken as shown in
Table 3. Figure 7 shows the speed distribution of those
recordings. The average speed for recordings 1-2 is
2.91 [m/s] while the average speed for recordings 3-6
is 5.40 [m/s].
4.3 Experiments
The proposed Kalman-based terrain estimation
methodology is compared against a baseline methodology, which does not use any state estimator but the
IMU and speed information. In addition, the use of
the terrain map as virtual observations in the state
estimator is analyzed. Thus, the following systems are
compared:
–
–
–
S1 “baseline”. The IMU measurements and the
vehicle’s speed information sources are used in
order to obtain the displacement of the vehicle and
the pose’s change of the laser rangefinders. No
Kalman filter is used for estimating the pose of the
laser rangefinders or for data fusion in the terrain
map.
S2 “full state estimator”. The S2 system is built
using the here-proposed methodology, using all
information sources.
S3 “full without map feedback”. Same as S2, but
without using the feedback of the map as virtual
observations in the Kalman filter.
As already mentioned, the measurements obtained by
a laser rangefinder mounted vertically on the front of
the vehicle are used to build the ground truth. The
measurements of this sensor are compared with the
corresponding values of mapheight . Naturally these
values are compared using the terrain coordinates.
In order to quantify the map estimation errors, the
root square error is measured in each map cell. Then,
two quality measures are computed: (i) the Root Mean
Table 4 Experiment results for recordings 1-2 (low speed), normalized by the number of samples (11,602)
RMSEsample
% Cases better than baseline system
S1
S2
S3
41.5877 10−3 [m]
–
14.7799 10−3 [m]
81.21%
14.8113 10−3 [m]
80.26%
J Intell Robot Syst
Fig. 8 ROC curves for recording 1-2. % of cells whose root mean square error is lower than a given threshold
Square Error (RMSE) over the whole map, normalized by the number of samples (RMSEsample ) and (ii)
the percentage of map cells which root square error
is lower than a given threshold value. Using several
different threshold values, Receiver Operating Curves
(ROCs) are built. In addition, the percentage of cases
in which the proposed system (S2) or a variant (S3)
has a lower RMSEsample, than the baseline system
(S1) is calculated.
In the first experiment, the systems were compared
using recording 1 and 2 (low speed). Table 4 shows
the obtained RMSEsample and Fig. 8 shows the ROC
curves of the error. It can be observed that the systems
that use the state estimators (S2 and S3) obtained a
much lower error in the map compared to the baseline
system (S1); the RMSEsample of systems S2 and S3
is 35 % of the one of S1. Table 4 also shows that in
approximately 80 % of the map cells (81.21 % for S2
and 80.26 % for S3), the height estimations of the systems that use the state estimators (S2 and S3) have a
lower error than the ones of the baseline system. Furthermore, in Table 4 it can be observed that best results
are obtained when all information sources are used in
the state estimator (S2); a slightly higher RMSEsample
is obtained when the terrain map data is not used in
the state estimation.
In the second experiment, the systems were compared using recordings 3-6 (medium speed). Table 5
shows the obtained RMSEsample and Fig. 9 displays
the ROC curves of the error. As in the first experiment,
it can be observed that the systems that use the state
estimators (S2 and S3) obtained a much lower RMSE
in the map compared to the baseline system (S1). In
fact, the RMSE is reduced by approximately one third
Table 5 shows that in about 60 % of the map cells
(63.18 % for S2 and 59.95 % for S3), the height estimations of the systems that use the state estimators
(S2 and S3) have a lower error that the ones of the
baseline system. As in experiment 1, in Table 5 it can
be observed that the best results are obtained when
all information sources are used in the state estimator
(S2); a 10 % higher RMSEsample is obtained when the
terrain map data is not used in the state estimation.
It is worth to mention that in addition to the
reported experiments in unpaved, rough terrain, several experiments were carried out in paved roads at
different speeds, in order to estimate the minimal error
that the terrain map estimation system can have. The
minimal RMSE is 10.1 10−3 [m], which is of the same
order of the RMSE obtained in recordings 1-2. This
value is also very close to the theoretical minimum
RMSE that can be obtained by such a system, which
is given by the angular projections of the laser range
finder variance:
"
N
!
2
N
(24)
E [RMSE] =
cos(αi )2 · σsensor
i=1
with αi the sweep angle of the sensor and N the
number of measurements.
Table 5 Experiment results for recordings 3-6 (medium speed) normalized by the number of samples (11,904)
RMSEsample
% Cases better than baseline system
S1
S2
S3
78.990 10−3 [m]
–
52.4298 10−3 [m]
63.18%
57.8229 10−3 [m]
59.95%
J Intell Robot Syst
Fig. 9 ROC curves for recording 3-6. % of cells whose root square error is lower than a given threshold
Finally, it is important to stress that the two estimation systems derived from the application of the
proposed methodology (S2 and S3) work on-line and
in real-time. Table 6 shows the processing time of each
stage, when it is executed. These processing times
were measured using the main computer of the AMTC
autonomous vehicle (See Section 4.1).
The prediction step of the Vehicle Estimation Stage
is executed at a framerate of 200 [Hz]. The correction
step is executed asynchronously everytime the IMU
the internal sensors of the vehicle or the map updating
process generate new data. The IMU and the internal sensors generate data at the following rates: IMU
at 100 [Hz], angle of the steering wheel at 50 [Hz],
and linear speed of the wheels at 50 [Hz]. The map
updating process generates data at a framerate of 10
[Hz].
The Laser Projection Stage and the Terrain Map
Update are also executed asynchronously everytime
the laser range finders generate new data. These sensors generate data at the following rates: LMS 291
laser range finder at 75 [Hz] and LD-MRS laser range
finder at 50 [Hz].
Table 6 Processing time of the estimation process
Processing Time Number of times
that the module is
[10−6 s]
called in a second
Vehicle State Estimation
0.5
(Prediction Step)
Vehicle State Estimation 122.6
(Correction Step)
Laser Projection
87.1
Terrain Map Update
104.0
200
210
125
125
5 Conclusions
The accurate and real-time generation of terrain maps
in off-road, irregular terrains is addressed in this work.
The proposed methodology is based on the use of
an Extended Kalman filter for estimating in real-time
the instantaneous pose of the vehicle and the laser
rangefinders by considering measurements taken from
an inertial measurement unit, internal sensorial data
of the vehicle and the estimated heights of the four
wheels, which are obtained from the terrain map and
allow determining the vehicle’s inclination. The estimated 6D pose of the laser rangefinders is used to
correctly project the laser measurements onto the terrain map. The terrain map is a 2.5D map that stores the
mean value and variance of the terrain height. In each
map’s cell position, new laser observations are fused
with existing height estimations using a Kalman filter.
The methodology is validated in the real world
(O’Higgins Public Park, located near downtown area
in Santiago de Chile, Chile), using an autonomous
vehicle. The evaluation dataset was captured while the
vehicle was moving on unpaved, rough terrain at low
and medium speeds, for 4.8 [km].
Experiments show that the use of the proposed
methodology allow to increase the quality of the
obtained maps; the RMSE is reduced to one third in
the case that the vehicle is moving at low speeds, and
reduced to two thirds when the vehicle is moving at
medium speeds. Furthermore, it can be observed that
in most of the map’s cells (80 %/60 % in case the
vehicle is moving at low/medium speeds), the height
estimations are more accurate when the Kalman-filterbased methodology is used.
In addition, experiments show that the use of
the map heights for the estimation of the pose of
the laser rangefinders allow obtaining more accurate
J Intell Robot Syst
maps. When the map heights are not used, the RMSE
increases by approximately 10 %, when the vehicle is
moving at medium speeds.
The following linear approximation between the
steer angle αt and the angle of the steering wheel
αtwheel is used:
Acknowledgments This research was partially supported by
FONDECYT (Chile) under Project Number 1130153.
αt β · αtwheel
Appendix 1:
From Fig. 2a, it can be observed that:
wheelbase
sin (αt ) =
(25)
rt
Then, given that vt = ωt · rt , the following expression for wt is obtained:
vt sin (αt )
(26)
wt =
wheelbase
Fig. 10 Relationship
between the steer angle and
the angle of the steering
wheel, for the Volkswagen
Tiguan vehicle
(27)
with β a real number. Figure 10 shows the linear relationship obtained for the Volkswagen Tiguan used in
this work.
Finally, from Eqs. (26) and (27), and considering
that αt is small, the final expression for wt is defined
as:
wt vt · β · αtwheel
vt · αtwheel · γ
wheelbase
(28)
with γ a real number.
For the Volkswagen Tiguan,
1
γ = 0.02359 m is obtained by a linear regression.
J Intell Robot Syst
References
1. Jazar, R.N.: Vehicle Dynamics: Theory and Application,
p. 1015. Springer (2008)
2. Stavens, D., Thrun, S.: A self-supervised terrain roughness
estimator for off-road autonomous driving. In: Proceedings
of Conference on Uncertainty in AI, pp. 13–16 (2006)
3. Hadsell, R., Bagnell, J.A., Huber, D., Hebert, M.: Accurate rough terrain estimation with space-carving kernels.
In: Proceedings of the 2009 Robotics: Science and Systems
Conference - RSS 2009, Washington (2009)
4. Hebert, M., Caillas C., Krotkov, E., Kweon, I.S., Kanade,
T.: Terrain mapping for a roving planetary explorer. In:
Proceedings of International Conference on Robotics and
Automation (ICRA), vol. 2, pp. 997–1002 (1989)
5. Pfaff, P., Triebel, R., Burgard, W.: An efficient extension
to elevation maps for outdoor terrain mapping and loop
closing. Int. J. Robot. Res. 26(2), 217–230 (2007)
6. Chen, T., Dai, B., Wang, R., Liu, D.: Gaussian-processbased ground segmentation for autonomous land vehicles.
J. Intell. Robot. Syst. (In press)
7. Shin, Y., Jung, Ch., Chung, W.: Drivable road region detection using a single laser range finder for outdoor patrol
robots. In: Proceedings of the 2010 IEEE Intelligent Vehicles Symposium, University of California, San Diego, pp.
21–24 (2010)
8. Ye, C., Borenstein, J.: A new terrain mapping method for
mobile robots obstacle negotiation. In: Proceedings of the
UGV Technology Conference at the 2002 SPIE AeroSense
Symposium. pp. 21–25 (2003)
9. Gu, J., Cao, Q., Huang, Y.: Rapid traversability assessment
in 2.5D grid-based map on rough terrain. Int. J. Adv. Rob.
Syst. 5(4), 389–394 (2008)
10. Miller, I., Campbell, M.: A mixture-model based algorithm
for real-time terrain estimation. J. Field Robot. 23(9), 755–
775 (2006)
11. Qiu, Q., Yang, T., Han, J.: A new real-time algorithm for
off-road terrain estimation using laser data. Sci. China Ser.
F: Inf Sci. 52(9), 1658–1667 (2009)
12. Tse, R., Ahmed, N., Campbell, M.: Unified mixture-model
based terrain estimation with Markov Random Fields. In:
Proceedings of the 2012 IEEE International Conference On
Multisensor Fusion and Integration for Intelligent Systems.
Germany (2012)
13. Thrun, S., Montemerlo, M., Dahlkamp, H., Stavens, D.,
Aron, A., Diebel, J., Fong, P., Gale, J., Halpenny, M.,
Hoffmann, G., Lau, K., Oakley, C., Palatucci, M., Pratt,
V., Stang, P., Strohband, S., Dupont, C., Jendrossek, L.E., Koelen, C., Markey, C., Rummel, C., van Niekerk,
J., Jensen, E., Alessandrini, P., Davies, B., Ettinger, S.,
Kaehler, A., Nefian, A., Mahoney, P.: Stanley: The robot
that won the DARPA grand challenge. J. Field Robot. 23,
661–692 (2006)
14. Welch, G., Bishop, G.: An Introduction to the Kalman
Filter. University of North Carolina at Chapel Hill, NC
(1995)
15. Bernuy, F., Ruiz-del-Solar, J., Parra-Tsunekawa, I., Vallejos, P.A.: Adaptive and real-time unpaved road segmentation using color histograms and RANSAC. In: 9th IEEE
International Conference on Control & Automation - ICCA
2011, pp. 136–141. Santiago (2011)
16. Cabello, F., Acuna, A., Vallejos, P.A., Orchard, M.E., Ruizdel-Solar, J.: Design and validation of a fuzzy longitudinal
controller based on a vehicle dynamic simulator. In: 9th
IEEE International Conference on Control Automation ICCA 2011, pp. 997–1002. Santiago (2011)