RECONSTRUCCIÓN TRIDIMENSIONAL DE UN AMBIENTE
NO ESTRUCTURADO Y ESTÁTICO USANDO UNA
PLATAFORMA MÓVIL TELE OPERADA MEDIANTE ROS Y
SU REPRESENTACIÓN EN UN ENTORNO VIRTUAL.
BRAYAN STEVEK LEMUS QUIROGA
JAIRO FERLEY CLAVIJO MAYORGA
EDINSON MAURICIO YOSA VELÁSQUEZ
TRABAJO DE GRADO PARA OPTAR AL TÍTULO DE INGENIERO
MECATRÓNICO
DIRECTOR
ING. OSCAR AVILÉS SÁNCHEZ, PH.D.
CO-DIRECTOR
ING. MAURICIO MAULEDOUX MONROY, PH.D.
PROGRAMA DE INGENIERÍA MECATRÓNICA
FACULTAD DE INGENIERÍA
UNIVERSIDAD MILITAR NUEVA GRANADA
BOGOTÁ D.C
2017
1RECONSTRUCCIÓN TRIDIMENSIONAL DE UN
AMBIENTE NO ESTRUCTURADO Y ESTÁTICO
USANDO UNA PLATAFORMA MÓVIL TELE
OPERADA MEDIANTE ROS Y SU
REPRESENTACIÓN EN UN ENTORNO
VIRTUAL
Edinson Yosa Velasquez, 1802267 Ferley Clavijo Mayorga, 1802202, y Brayan Lemus Quiroga, 1802223
Abstract—This works consists in the design, programming and
manufacture of a mobile robotic platform which need to be able
to develop a trhree-dimensional scan of a real environment using
a two-dimensional sensor (SICK TIM 351). The mobile platform
is commanded by remote control that allows the user to control
position and orientation in the scene in order to increase the
information in those areas that could be more important to
scan. The algorithms has been programmed throw the Robot
Operating System (ROS) and the sensor information it’s acquire
by a FITPC settle at the platform and send it to a central
computer by a WIFI connection, where the spatial coordinates
of the scan environment are calculated. Finally the point cloud
created it’s represented in a Matlab Graphical User Interface
(GUI).
I. INTRODUCCIÓN
El avance de las herramientas computacionales permite al
área de robótica plantearse problemas cada vez mas complejos
y que brinden apoyo en tareas en las que el ser humano lo
requiere, tal es el caso de digitalizar espacios que: o bien
pueden contener un ambiente de difícil acceso, o algunos
de sus límites son complicados de alcanzar, o simplemente
por error humano es un trabajo que prefiere encargar a un
sensor con una precisión constante, El presente trabajo de
grado pretende aportar una solución en la tarea de obtener
reconstrucciones tridimensionales de ambientes no conocidos
mediante una plataforma móvil tele-operada, que hace uso
del sistema operativo ROS para emplear SLAM, odometría
y una central inercial, en la determinación integrada de la
ubicación del robot, y así añadir información a una nube de
puntos a partir de la información 3D obtenida rotando el plano
visualizado por un sensor lidar; para luego representar dicha
información en una herramienta computacional.
II. DISEÑO PLATAFORMA MÓVIL
Para el desarrollo del proyecto se plantean las necesidades
surgidas a raíz del problema, para posteriormente proponer
sistemas a utilizar según la necesidad, y continuar con el
diseño mecánico y estructural de la plataforma. El objetivo
primordial del robot es escanear el entorno por donde se
mueve, por lo tanto el sensor de entorno o escáner láser
se encuentra sobre una base rotatoria (base azul figura 1),
accionada por un motor. Toda este estructura, esta dispuesta
sobre una base fija (base amarilla en la figura 1) la cual a su vez
esta montada sobre una plataforma móvil, que se desplaza por
entornos en interiores sobre áreas planas. La plataforma móvil
debe contar con actuadores eléctricos que dan propulsión al
sistema.
Fig. 1. Base Sensor.
Teniendo en cuenta que el proyecto va destinado para
captura de entornos en interiores en donde las áreas por
donde se mueve son planas y duras, se propone implementar
una plataforma móvil con locomoción por RUEDAS CON
TRACCIÓN DIFERENCIAL, por contar con una simplici-
dad en su implementación presentando menor error en los cál-
culos odométricos con respecto a otras locomociones, ademas
de una buena eficiencia energética en el desplazamiento. La
navegabilidad de este tipo de robot esta dispuesta para terrenos
planos, eludiendo obstáculos sin necesidad de sobrepasarlos,
2la estabilidad de este tipo de robot está sujeta al terreno por
el cual se desplaza.
Para estimar el motor tenemos en cuenta los posibles
movimientos de la plataforma en un plano, para ver en cual
de ellos los motores hacen su mayor esfuerzo; el primer
movimiento es de avance o retroceso, en estos movimientos
los dos motores se mueven en sentido de giro contrario,
reduciéndose a la mitad el esfuerzo requerido de cada motor.
El segundo movimiento es de rotación con los dos motores, en
el cual estos se mueven en el mismo sentido de giro reduciendo
el esfuerzo máximo de cada uno al igual que en el avance y
retroceso, y por ultimo se presenta la rotación con un solo
motor en donde el punto de pivote es la llanta del extremo
contrario, es en este movimiento donde el motor requiere del
mayor esfuerzo ya que este debe desplazar toda la masa de la
plataforma.
Asumiendo que el centro de gravedad de la plataforma esta
sobre el eje de rotación de los motores y que el neumático de
la llanta tiene una pequeña flexión, el diagrama que se muestra
en la figura 2 en donde el torque (τ ) depende del radio real (R)
y de la fuerza motriz (Fm), la cual es contraria a la fuerza de
fricción (Fr) pero de igual magnitud, la que a su vez depende
del peso (W ) y del coeficiente de fricción que para el calculo
se asume como 1 pues lo deseado es que la rueda no se deslice,
esta relación queda plasmada en las ecuaciones 1 y 2.
τ = Fm ∗R (1)
Fm = Fr = µWg (2)
τ = µWg ∗R (3)
Fig. 2. Esquema de Cuerpo Libre de la Rueda de la plataforma movil
El robot, ademas del sensor debe llevar una unidad de
procesamiento de datos que para el caso del proyecto es una
FitPC 2 [1], con especificaciones suficientes para ejecutar el
sistema operativo Ubuntu, lo que permite ejecutar ROS sobre
ella.
TABLE I
TABLA DE PESOS DE LOS DISPOSITIVOS
La plataforma cuenta con un Router para la comunicación
entre el sensor la fitPC y el centro de mando a Distancia.
El peso de la plataforma se estima de acuerdo a los pesos
dados en las tablas de características de cada dispositivo
nombrados anteriormente, un resumen de los pesos de cada
dispositivo se muestra en la tabla I, donde se observa que el
peso estimado de la plataforma es de 4572 gramos, peso con
el cual se calculó el torque necesario de acuerdo a la ecuación
3.
τ = (1)(4, 572Kg)(9.8m/s2) · (0, 03335) (4)
τ = 1, 494Nm (5)
El torque máximo estimado necesario en los motores es
de 1.494Nm ≈1.5Nm (Ecuación 5) valor que se obtuvo al
reemplazar los valores asumidos (Ecuación 4) , torque que
en el movimiento de avance se reducirá puesto que cada
motor ejercerá la mitad del torque. Teniendo en cuenta la
gráfica “performance Graph” presentada en la figura 3 se
pueden analizar sus características, según la gráfica la corriente
consumida a un torque máximo de 1.5Nm es de 1.4 Amperios,
y la eficiencia se reduce a un ±70 %, pero la corriente
consumida en un movimiento de avance en que cada motor
genera un torque de 0,75 Nm es de 0,7 Amperios y su
eficiencia es de ±87%.
Fig. 3. Características Técnicas Motor Dynamixel MX 64[2]
3La plataforma móvil consta de una base principal elaborada
de MDF de 5mm que soporta el peso de todos los dispositivos,
posee soportes verticales en donde están montados los motores
y que sirven de separadores entre la base principal y la base
superior, al igual que que los separadores que conforman el
compartimiento de la batería, el cual está diseñado para evitar
el movimiento de la batería mientras el robot se encuentra en
movimiento. La plataforma ademas de las ruedas de tracción
posee dos ruedas tipo castor que generan dos puntos de apoyo
adicionales, lo que proporciona mayor estabilidad, estas ruedas
están sujetas a una estructura con un desnivel respecto a la
base principal para garantizar horizontalidad respecto al suelo
como se muestra en la figura4
(a) Isometrico Plataforma Móvil
(b) Vista lateral Plataforma Móvil
Fig. 4. Plataforma Móvil
Como aspecto importante cabe destacar que la plataforma
se diseña pensando en generar el menor numero posible de
traslaciones al sistema coordenado del sensor, para que los
errores mecánicos o de fabricación tengan poca incidencia
sobre la adquisición de los datos. es por ello que la plataforma
del sensor esta dispuesta en la plataforma móvil de tal manera
que coincidan los ejes verticales del sistema coordenado del
sensor y el de la plataforma como se puede observar en las
figuras 5a y 5b.
Se diseña una PCB (“Printed Circuit Board”) que permita
la distribución de la energía para los diferentes dispositivos
presentes en el robot, ademas de contener la unidad de
medición inercial (IMU) y el arduino que es el encargado de
recibir la información de la IMU y trasmitirla a la FITPC, en
la figura 6 se muestra el diagrama de conexiones de la PCB la
(a) Vista Frontal. (b) Vista lateral del Robot.
Fig. 5. Ensamble Base Sensor y Plataforma Móvil
cual se compone de dos partes: la primera es de distribución de
la energía de la batería y la segunda es la comunicación entre
IMU y Arduino, se tiene especial atención con la ubicación
de la IMU, para que su sistema coordenado o por lo menos el
eje vertical, coincida con el de la plataforma móvil y el sensor
Fig. 6. Circuito Esquemático
Una parte importante en la proyección del robot es la
distribución de conexiones, es necesario planear el flujo de in-
formación y energía de modo que cada uno de los dispositivos
presentes, cumpla sus funciones de la mejor manera posible;
como consecuencia de ser un robot móvil, no deben existir
cables de energía o de comunicación a nada que se encuentre
estático en el espacio ya que puede afectar negativamente en la
ejecución de la tarea objetivo. Por tanto se plantea un diagrama
de conexiones que asegure el buen funcionamiento y flujo de
datos en el robot móvil ( Figura 7).
En la estructura se tiene en cuenta el cableado, agregando
agujeros por los cuales este pasa para darle estética al robot
y para una organización del cableado que permita identificar
fallas provocadas por conexiones. En la figura8 se muestra el
diseño CAD en donde se tiene en cuenta todos los aspectos
antes mencionados, desde la ubicación de los dispositivos
hasta el enrrutamiento del cableado para que no interfiera con
la extracción de otros dispositivos en caso de mantenimiento.
4Fig. 7. Conexiones del Robot
Fig. 8. Diseño Final Robot
.
La fabricación de la base del robot se realiza en corte láser,
por ofrecer una alta precisión en los cortes, lo que permite que
el ensamblaje consista en armar las piezas creadas, ofreciendo
además un error bajo en el posicionamiento de los dispositivos,
como ejemplo se tiene la colinealidad de los ejes de rotación
de los motores ya que de no asegurar esta coincidencia, la
veracidad de los cálculos hechos en la odometría se vería
comprometida. Por otro lado la ubicación del sensor SICK y de
la IMU tienen una gran importancia puesto que la información
que entregan estos dos sistemas debe ser complementaria para
determinar la orientación del robot. En la figura 9, se muestra
una comparación entre el diseño CAD y el resultado final del
ensamblaje del robot con su respectivo cableado para cada uno
de los dispositivos.
III. LECTURA ESTÁTICA TRIDIMENSIONAL
A. Sensor
1) Adquisición de Datos : Con el propósito de interpretar
los datos entregados por el sensor SICK, se ejecuta el launch
de comunicación (“sick_tim551_2050001”), y una vez estable-
cida la comunicación se debe identificar el tipo de mensaje
Fig. 9. Resultado final
que se está enviando por el topic creado, el cual es un mensaje
tipo “sensor_msgs/LaserScan” que tiene la siguiente estructura
(Tabla II).
Tipo Argumento Descripción
Float32 angle_min Angulo inicial de las lecturas.
Float32 angle_max Angulo final de las lecturas.
Float32 angle_increment Desplazamiento angular entre medidas.
Float32 time_increment Tiempo entre mediciones.
Float32 scan_time Tiempo para realizar una lectura completa.
Float32 range_min Distancia mínima de detección.
Float32 range_max Distancia máxima de detección
Float32[] ranges Vector de distancias de cada lectura.
Float32[] intensities Vector de intensidades de cada lectura.
TABLE II
ESTRUCTURA DEL MENSAJE ENTREGADO TRAS LA LECTURA DEL SENSOR
SICK TIM
Cada posición del vector “ranges” corresponde al valor de
distancia detectado para cada angulo de medición; iniciando
desde la lectura tomada en el valor de “angle_min” hasta
completar el vector en la medición en el “angle_max”. Debido
a esta organización es posible interpretar los datos de modo
que la posición “ i ” del vector equivalga a la medición en
el angulo “ i ” que varia de 0 a 270, como resultado se
tendrá un plano de trabajo como el mostrado en la figura 10a
. La disposición en la cual se han interpretado las lecturas
del sensor permite obtener cada distancia en coordenadas
polares (r, θ), que al operarlas obtienen su equivalencia en
coordenadas cartesianas siguiendo las ecuaciones 6 y 7.
Para tener los datos expresados en un sistema de coor-
denadas coherente al diseño de la plataforma móvil que se
pretende usar, hace falta aplicar una rotación fundamental de
90 grados al sistema de coordenadas obtenido para que sus ejes
coordenados coincidan a los propuestos en la sección II, por
esto se le aplica la ecuación 8 a las coordenadas cartesianas,
para finalmente obtener el plano de trabajo óptimo para la
plataforma móvil y cada una de las distancias tomadas respecto
a este nuevo sistema de coordenadas local [3].
rx = r ∗ cos
(
i ∗ pi
180
)
(6)
5(a) Sistema Coordenado Lecturas del Sen-
sor.
(b) Sistema de Coordenadas Local del
Sensor.
Fig. 10. Transformacion de Sistema de Coordenadas para las Lecturas del
Sensor.
ry = r ∗ sen
(
i ∗ pi
180
)
(7)
[
rxl
ryl
]
=
[
cos(β) −sen(β)
sen(β) cos(β)
]
∗
[
rx
ry
]
(8)
2) Barrido del Sensor para Realizar el Escaneo 3D:
A medida que se va cambiando la orientación del sensor
en el eje x (figura 11), implícitamente se esta realizando
una rotación del sistema coordenado desde el cual se estan
adquiriendo todas las coordenadas, esto quiere decir que el
conocer el angulo de inclinación con respecto al eje horizontal
al momento de realizar una lectura, permite el calculo de la
posición global del punto de interés. Las coordenadas que se
obtuvieron en la sección III-A1 se pasan a un sistema de
medición global a través de la ecuación 9, la cual asegura
que cada lectura tendrá su correspondiente coordenada en el
espacio según la orientación a la que se encuentra el sensor
al momento de realizar la lectura .
(a) Plano de Trabajo con Angulo de
Inclinación Positivo
(b) Plano de Trabajo con Angulo de
Inclinación Cero
(c) Plano de Trabajo con Angulo de
Inclinación Negativo
Fig. 11. Rotacion del Sensor en el Eje X.
 r′xgr′yg
r′zg
 =
 1 0 00 cos(α) −sin(α)
0 sin(α) cos(α)
 ∗
 rxlryl
0
 (9)
El resultado de la ecuación 9, es una forma ideal de
obtener la ubicación espacial de cada punto de lectura, esto
se debe a que el punto desde el cual se emite el láser
debe ser exactamente el centro de giro de la rotación que
genera el barrido, solo esto asegura que las coordenadas tridi-
mensionales que se obtienen corresponden a las ubicaciones
reales de los puntos evaluados. Esta corrección puede ser
resuelta matemáticamente conociendo la distancia a la cual
se encuentra el punto de rotación real respecto al punto de
medición. Para realizar esta consideración la matriz expresada
en la ecuación 9 no será simplemente una rotación en el eje X,
sino que sera una combinación entre la rotación y la distancia
a lo largo de los tres ejes entre los puntos de medición. Con
el fin de disminuir errores mecánicos que puedan afectar los
resultados de la obtención de coordenadas, el punto de emisión
y recepción de láser solo cuenta con desplazamiento en el eje
Z respecto al eje de giro del sensor, por lo que las coordenadas
globales de medición quedaran determinadas por la ecuación
10
rxg
ryg
rzg
1
 =

1 0 0 0
0 cos(α) −sin(α) 0
0 sin(α) cos(α) dz
0 0 0 1
∗

rxl
ryl
0
1
 (10)
Para controlar la inclinación del sensor al momento de
realizar el barrido horizontal se utiliza un motor dynamixel
MX28 [4] debido a sus características técnicas y su posible
manejo desde el sistema ROS. La librería para estos motores
en el sistema operativo [5] , permite la comunicación con el
motor mediante un topic en el cual se escribe la posición
angular deseada y otro en el que se recibe la posición en la que
se encuentra, para poder utilizar efectivamente los motores,
es necesario seguir con una serie de pasos que permiten la
configuración inicial de acuerdo a sus funciones. Los motores
dynamixel tienen dos tipos específicos de trabajo, tipo rueda
y tipo articulación para realizar la rotación que se desea;
debe ser configurado como tipo articulación siguiendo las
instrucciones del ’USB2Dynamixel User’s Manual [6].
IV. DETERMINACIÓN DE LA UBICACIÓN ESPACIAL DE LA
PLATAFORMA MÓVIL
Cada lectura que conforma el escaneo tridimensional re-
alizado en la sección III depende del sistema coordenado
local de la plataforma; la eficacia de la reconstrucción de
un espacio depende de la cantidad de información que se
tenga del ambiente real, es por esto que la plataforma de
escaneo debe poder moverse por el entorno causando que la
distancia entre la plataforma y los objetos cambie, aunque
estos se encuentren estáticos. En consecuencia se plantean
tres métodos de ubicación espacial de modo que cada nuevo
barrido se realice conociendo la relación existente entre los
nuevos puntos obtenidos y el desplazamiento recorrido.
6A. Odometría
La odometría consiste en el análisis cinemático de una
plataforma móvil, y a partir de sus movimientos se estima una
ubicación relativa según el tipo de plataforma, su locomoción
y su diseño mecánico. La plataforma propuesta usa una
tracción diferencial donde el desplazamiento esta dado por las
velocidades de las ruedas laterales, como resultado se obtienen
tres valores; los cuales pertenecen a desplazamiento en los ejes
X y Y del sistema coordenado global y la rotación en el eje
Z.
Para obtener la expresión matemática que describe el robot
móvil se supone un estado de movimiento con el fin de
observar el aporte individual que hace cada llanta al adquirir
una velocidad angular y con el método de superposición
adquirir el modelo cinemático del robot [7], la ecuación 11
da como resultado el vector de velocidades con respecto al
sistema coordenado local de la plataforma en función de
las velocidades angulares de cada llanta en donde; rllanta
corresponde al radio de la llanta y la distancia entre las dos
ruedas está establecida como 2l (figura 12).
Fig. 12. Estado de Movimiento y Variables del Modelo
 VxVy
ω
 =
 0 0rllanta
2
−rllanta
2−rllanta
2∗l
−rllanta
2∗l
 ∗ [ θ˙l
θ˙r
]
(11)
Con las velocidades lineal en los ejes X y en Y y la angular
en el eje Z con respecto al sistema coordenado global, es
posible determinar el desplazamiento que tiene la plataforma
al cambiar la velocidad de los motores, para esto es necesario
integrar cada una de las velocidades para obtener la posición
lineal y angular que la plataforma va obteniendo a medida
que el robot se mueve. La integral se realiza teniendo en
cuenta que en términos de frecuencia, una señal se puede
integrar al multiplicarla por un factor s−1, sin embargo la
lectura de los valores corresponde a un tiempo discreto por
lo cual se discretiza ese valor con la transformada de Tustin
y su correspondiente expresión en términos de muestras [8].
Obteniendo la ecuación 12 donde P representa el vector de
posición lineal en X y en Y y posición angular en el eje Z y
V el vector de velocidades de la ecuación 11.
P (k) =
T
2
∗ (V (k) + V (k−1)) + P (k−1) (12)
Debido al constante cambio de orientación de la plataforma
a lo largo del movimiento, es necesario agregar una rotación
fundamental en Z a la ecuación 11 para trasladar correctamente
las velocidades al sistema coordenado global usando como
angulo beta el obtenido en la ecuación 12. Por lo tanto el
vector de velocidades que se debe integrar para obtener la
ubicación del robot será el mostrado en la ecuación 13.
V =
 VxVy
ω
 = rotZ(β)
 0 0rllanta
2
−rllanta
2−rllanta
2∗l
−rllanta
2∗l
 ∗ [ θ˙l
θ˙r
]
(13)
B. Lectura de Orientación a través de una Central Inercial
Una centra inercial (IMU) cuyas siglas representan su
nombre en ingles Inercial Measurement Unit, es un dispositivo
electrónico que combina diferentes tipos de sistemas micro-
electromecánicos (MEMS [9]) y permite medir magnitudes
físicas como aceleración y velocidad angular de acuerdo a su
propio eje coordenado [10]. La central inercial MPU9250 de
InvenSense usada para este proyecto cuenta con dos módulos
integrados, uno de ellos es el MPU6050 que cuenta con un
giroscopio junto a un acelerómetro de 3 ejes, y cada uno
permite la configuración de distintos valores de rango de
trabajo y sensibilidad, el otro modulo es el AK8963 que posee
un magnetómetro de 3 ejes; teniendo en cuenta que cada eje
de medición tiene dedicado un conversor análogo-digital de
16 bits , el rango de trabajo configurado para cada medición
dará como resultado la sensibilidad con la que se estará
trabajando. Esta central inercial cuenta con DMP (Digital
Motion Processor), que esta encargado de leer y organizar
la información digitalizada detectada por los MEMS a una
frecuencia de 200Hz, de esta manera incluso si el dispositivo
que se este comunicando con la IMU necesita los datos a
una menor tasa el DMP tendrá lista la información para ser
enviada. La ventaja de este procesador interno es que el
dispositivo conectado debe ejecutar menos instrucciones para
realizar una lectura exitosa del sensor.
Para adquirir la orientación se implementa la librería de Jeff
Rowberg [11],que utiliza como referencia el vector constante
que provee la aceleración gravitacional con respecto a la
central inercial y halla los ángulos de inclinación que describen
este vector con respecto a su sistema de referencia, debido a
que se pretende montar la IMU a una plataforma móvil, los
datos de aceleración del robot generaran distorsiones en la
medida real de la gravedad adicionando a cada componente
la aceleración instantánea debida al movimiento del robot
causando una variación en la magnitud y orientación del vector
compuesto por cada componente entregada por el sensor;
para suplir este inconveniente se utiliza el procesador de
movimiento digital DMP, el cual permite realizar esa distinción
entre las aceleraciones externas y la aceleración gravitacional
7[12]. Una vez obtenidas las proyecciones reales de la gravedad
en cada eje, este vector resultante se pasa a forma de cuater-
niones los cuales son una representación vectorial que consta
de cuatro constantes, para lograr operar la lectura del sensor y
de esta forma obtener los ángulos de inclinación del dispositivo
[13].
Los ángulos adquiridos por este método tienen un sistema
de referencia único dificultando su manipulación y su actu-
alización respecto a los movimientos reales de la plataforma,
lo que implica que el espacio de trabajo de [−pi, pi] entregado
debe trasladarse a un espacio de trabajo en el cual se logre
modificar el cero (eje de referencia) con el fin de sincronizar
el angulo inicial del robot con el calculado con los datos de
la IMU. La obtención de este nuevo espacio debe tener en
cuenta las condiciones mostradas en las ecuaciones 14 y 15
para lograr que el espacio de trabajo obtenido este completo
como se muestra en la figura 14 y no se obtengan intervalos
de información perdida (figura 13).
(αl > (pi + αi))→ αi > 0 (14)
(αl < (−pi + αi))→ αi < 0 (15)
(a) Angulo Inicial Positivo al Restar
el Valor Inicial
(b) Angulo Inicial Negativo al
Restar el Valor Inicial
Fig. 13. Espacio de Trabajo con el Rango Incompleto
(a) Angulo Inicial Positivo con el
Rango Completo
(b) Angulo Inicial Negativo con el
Rango Completo
Fig. 14. Espacio de Trabajo con el Rango Completo
1) SLAM: Simultaneous Localization And Mapping o
SLAM es el nombre otorgado no a un algoritmo en concreto,
sino al concepto cuya solución puede ser planteada de diversas
formas, dicho concepto busca solucionar el problema de
generar un mapa de un espacio desconocido mientras navega
y se ubica en el mismo[14], [15]. El problema es que para
generar un buen mapa requiere información confiable de la
ubicación pero para conocer su ubicación necesita un mapa
del espacio, es por esto que tanto la localización como la
construcción del mapa deben realizarse de manera simultanea.
Las principales partes presentes en el SLAM son la extracción
de marcas, asociación de datos, estimación de la posición, ac-
tualización de estado y actualización de marcas. En cada parte
hay presencia de diversos métodos de análisis, identificación
y filtrado de información. El filtro mas común y que por lo
general es el centro de desarrollos en SLAM es el filtro de
kalman, cuyo planteamiento incluye un vector de estados que
permite predecir los estados planteados y realizar correcciones
sobre las lecturas realizadas[14].
El package de ROS elegido para esta tarea se llama “Hec-
torSlam”, la ventaja que ofrece es que realiza sus propias
predicciones y no requiere de información externa, ademas
permite generar una ayuda visual a la persona que tele-opera
el robot que es el mapa 2D del espacio explorado[16], [17].
La clave del funcionamiento de “HectorSlam” radica en
el uso del método de Gauss Newton para optimización, la
finalidad es minimizar la distancia entre el plano guardado
y la lectura actual del sensor encontrando la transformación
que haga coincidir los planos descritos, una muestra visual del
proceso puede ser vista en 15[18], [16].
Fig. 15. Ilustración del Método de Gauss-Newton(Tomado de [19])
Así la justificación matemática consiste en encontrar la
transformación ξ = (px, Py, ψ) que minimiza la ecuación 16:
ξ∗ = argminξ
n∑
i=1
[1−M(si(ξ))]2 (16)
8Donde la función M(si(ξ)) regresa el valor en el mapa del
punto si(ξ), que son las coordenadas en el mapa del punto
reconocido por el láser. De acuerdo al método, es asignada
una transformación inicial ξ, y a partir de la optimización del
error se determina un valor 4ξ:
n∑
i=1
[1−M(si(ξ +4ξ))]2 → 0 (17)
Aplicando una expansión de taylor de primer orden a
M(si(ξ+4ξ)) y haciendo la derivada parcial respecto a4ξLa
ecuación de Gauss-Newton para minimizar el problema es:
4ξ = H−1
n∑
i=1
[
∇M(si(ξ))∂si(ξ)
∂ξ
]T
[1−M(si(ξ))] (18)
Donde :
H =
n∑
i=1
[
∇M(si(ξ))∂si(ξ)
∂(ξ)
]T [
∇M(si(ξ))∂si(ξ)
∂ξ
]
(19)
En el código fuente del nodo fue añadido un listener que
permitiera actualizar la posición estimada actual del método
para poder actualizarlo con el valor calculado en el filtro que
será descrito a continuación.
2) Filtro complementario: De acuerdo a desarrollos re-
alizados no solo en SLAM sino en diversas aplicaciones
que requieren de información precisa, se deduce que agregar
información de varios sensores aporta en la determinación
de un valor mas confiable y estable[20][21]. Cada método
implementado para determinar la ubicación espacial contiene
condiciones en las que su valor es o no confiable, la odometría
por ejemplo presenta errores propios de variaciones físicas del
dispositivo o en la determinación de la velocidad, la IMU
puede ser afectada por mala calibración, o el SLAM puede
entregar estimaciones erróneas por bajas frecuencias de esca-
neo [22][21] [23]. Aprovechando las características de cada
método y basado en el análisis descrito, es elegido un filtro
complementario, que ademas de encontrar el valor correcto
de la ubicación basado en toda la información obtenida, le
informa a cada uno de los métodos cuando su error en posición
es evidente.
Un filtro complementario ofrece la ventaja de no tener
atenuación de la salida debido a que la suma de sus constantes
siempre debe ser 1, ademas permite paso de frecuencias altas
o bajas a las entradas según sea la necesidad[21], [20].
El filtro corresponde a:
Pf = A ∗ PS +B ∗ PI + C ∗ PO (20)
donde:
Pf →Posición filtrada
PS →Posición segun SLAM
PI →Posición según IMU
PO →Posición según Odometría
A,ByC → Constantes del filtro
Obteniendo un valor filtrado de la posición, lo ideal es hacer
que cada método inicia a calcular la ubicación tomando como
referencia este valor filtrado para buscar que todos se encuen-
tren en constante equilibrio; como cada dispositivo según fue
descrito funciona a frecuencias distintas no es correcto retornar
el valor filtrado todo el tiempo, ya que puede escribir sobre
información que puede ser importante en el funcionamiento,
la solución planteada es proyectar un movimiento del robot
en la dirección en la que se encuentra cada instrumento, y
si al realizar ese avance los puntos finales se alejan hasta
alcanzar un valor límite estipulado de distancia, entonces se
refresca cada uno de los métodos con el valor de posición
filtrado actual. El funcionamiento de la corrección que realiza
el filtro puede ser entendida mas fácilmente en la figura16, en
la cual se muestran 4 distintas ubicaciones y orientaciones
de cada uno de los métodos, para un tiempo inicial (t=1)
las ubicaciones no están muy cercanas pero su orientación
les permite acercar su medición en el siguiente instante de
tiempo (t=2), como el angulo de ninguno tuvo variación, en
el tercer instante de tiempo (t=3), las proyecciones de la
posición se hicieron muy lejanas entre ellas (las distancias
están representadas por el triangulo amarillo donde ningún
cateto puede sobrepasar un valor dado, para el robot fue
definido en 10 cm), cuando es superado, los tres valores son
actualizados con el filtro como se observa para t=4.
Fig. 16. Ilustración Proyecciones del Filtro
C. Resultados
1) Lectura tridimensional estática: El control de la posi-
ción angular permitió realizar el escaneo mostrado en la figura
17, la cual muestra un escaneo de 135◦ con un total de 100
lecturas, lo que representa una variación angular entre ellas de
1.35◦, en esta figura se hace evidente que el numero de lecturas
por barrido afecta el espacio reconstruido, si este numero es
alto como el usado en este escaneo la legibilidad del ambiente
virtualizado no presentara problemas.
2) Determinación de la ubicación del robot:
Para realizar pruebas a la determinación de la ubicación,
se realizó una trayectoria cerrada registrando la información
real en un tablero para caracterizar la información importante y
comparar con los datos obtenidos por el robot como se muestra
9Fig. 17. Reconstrucción Tridimensional a Partir de un Barrido de 100 Lecturas
Fig. 18. Experimento con Filtro
en la figura 18; la información determinada por cada uno de los
métodos fue registrada en un archivo de texto y posteriormente
graficada en Matlab obteniendo las trayectorias orientación y
trayectoria mostradas en las figuras 19 y 20 respectivamente.
(a) IMU (b) Odometría (c) SLAM
Fig. 19. Orientación
(a) Odometría (b) SLAM
Fig. 20. Desplazamiento
Una vez fue implementado el filtro se realizó de nuevo
el experimento obteniendo los resultados de las figuras 21 y
22 en las que nuevamente se muestra orientación y posición
respectivamente, el error medio en la trayectoria fue menor y
se puede evidenciar en un segmento del tercer recorrido recto
del robot la corrección que ejerce el filtro sobre la odometría.
(a) Orientación Filtrada IMU (b) Orientación Filtrada
Odometría
(c) Orientación Filtrada SLAM (d) Resultado Orientación Filtrada
Fig. 21. Resultados en Orientación
(a) Desplazamiento Filtrado
Odometría
(b) Desplazamiento Filtrado
SLAM
(c) Resultado Desplazamiento Fil-
trado
Fig. 22. Resultados en desplazamiento
3) Nube de puntos de la integración.: Finalmente el re-
sultado de la nube de puntos de la integración incluye el
10
desarrollo de los items anteriores, para ello fue registrado
un espacio en tres lecuras y con la ayuda de la interfaz
desarrollada en matlab se visualiza el resultado de cada captura
aplicando filtros a 0.01m y 0.1m, las gráficas obtenidas se
muestran en las figuras 23 y 24.
(a) Toda la Nube (b) Primer Lectura
(c) Segunda Lectura (d) Tercer Lectura
Fig. 23. Visualización en la Interfaz Gráfica de Matlab filtro 0.01
(a) Toda la Nube (b) Primer Lectura
(c) Segunda Lectura (d) Tercer Lectura
Fig. 24. Visualización en la Interfaz Gráfica de Matlab filtro 0.1
V. CONCLUSIONES
Como primera conclusión nombramos el hecho que el
trabajo de ingeniería es un proceso iterativo que se vio aplicado
en este trabajo, aunque en el documento se muestra como
un proceso lineal; muchos de las soluciones implementadas,
fueron aplicadas después de un revisión o incluso mejoradas al
ir agregando nuevos sistemas, hasta obtener el funcionamiento
deseado.
En cuanto al espacio caracterizado, se obtuvo el resultado
esperado en cuanto al rango de visión del sensor, a la in-
munidad a la luz tanto ambiente como natural presente en
el entorno, así como su resolución en angulo para definir un
contorno a la distancias no mayores a 5m.
Un punto muy importante que se resalta, es la utilización
del sistema operativo ROS en el cual se implemento todo el
software, haciendo eficiente el proceso de implementación de
cada sistema como un paso a paso, permitiendo realizar la
conexión al sensor y posteriormente realizar la adquisición
de los datos relevantes para este proyecto. Por su versatilidad
en la utilización de nodos, permite adicionar la comunicación
de un servo motor que varia el plano de sensado, sin tener
que hacer grandes modificaciones al programa que adquiere la
información del sensor; teniendo un nodo para la adquisición
de información del sensor, el cual trasmite esa información,
y otro, el cual da la posición al motor. La estructura dis-
eñada para la rotación del sensor, funciona correctamente,
produciendo un torque pequeño gracias a la utilización de
rodamiento tanto axial como radial para disminuir al máximo
el rozamiento entre las piezas móviles, ademas de resaltar la
sencillez en la fabricación del mismo, generando una nube
de puntos tridimensional que de acuerdo a la resolución
programada, muestra suficientes detalles como para identificar
la fisionomía de una persona.
No menos importante es la determinación de la posición
del robot; respecto al método para determinar la ubicación es
lección aprendida que un solo método no es de fiar y que en
el planteamiento de las soluciones hay que tener en cuenta las
fortalezas y debilidades de cada una para complementarlas en
busca de un sistema robusto, el filtro implementado mostró
reducir el error acumulativo en las lecturas hechas.
En cuanto al diseño de la plataforma, una ventaja es la
selección de los componentes que gracias a lo compacto de
cada sistema permitió una integración, entre los mismos y dar
ubicación pensando en el centro de masa total de la plataforma
para que sea sobre las llantas y de esta forma darle la máxima
adherencia posible y tratar de reducir el error en la odometría.
Un factor que afecta el buen desempeño de la plataforma,
son las ruedas castor, debido a su movimiento de orientación
al cambiar el sentido de rotación al robot, pues generan un
movimiento no esperado en la plataforma total del robot.
Para finalizar, la interfaz realizada para visualizar los datos
ofrece la opción de mostrar poco a poco las lecturas para
entender un poco mejor el espacio, ademas de permitir aplicar
una reducción en la cantidad de puntos, que aunque representa
perder información permite tener mejor control sobre los datos
además de hacer mas ligero el archivo resultante.
11
A. Recomendaciones
La utilización de ruedas castor como ruedas auxiliares para
aumentar los puntos de apoyo, no se recomienda debido que
al generarse un movimiento de acomodación de la orientación
de estas ruedas, hace que el robot experimente una desviación
involuntaria, provocando errores en la estimación de la posi-
ción por odometria, puesto que es un movimiento que no se
tiene en cuenta.
La implementación de un filtro que tenga en cuenta los
estados anteriores de las variables a evaluar en caso que el
error de los distintos métodos aplicados fallen, permitiendo
retomar con mejor criterio la estimación de los mismos.
REFERENCES
[1] Q. fanless solution. (2017) fit-pc2 models specifications. [Online].
Available: http://www.fit-pc.com/web/products/fit-pc2/
[2] ROBOTIS. (2010) Robotis e-manual v1.31.01. [Online].
Available: http://support.robotis.com/en/product/actuator/dynamixel/
mx-series/mx-64at-ar.htm
[3] P. H. S. J. L., “Técnicas modernas en automática parte iii robotica.”
[4] ROBOTIS. (2010) Robotis e-manual v1.31.01. [Online].
Available: http://support.robotis.com/en/product/actuator/dynamixel/
mx-series/mx-28at-ar.htm
[5] D. Lim. (2017, octubre) Dynamixel workbench. [Online]. Available:
https://github.com/ROBOTIS-GIT/dynamixel-workbench
[6] ROBOTIS, USB2Dynamixel Users Manual, v1.2.
[7] A. Baturone, Robótica: manipuladores y robots móviles, ser. ACCESO
RÁPIDO. Marcombo, 2005. [Online]. Available: https://books.google.
com.co/books?id=TtMfuy6FNCcC
[8] K. Ogata, Ingeniería de control moderna, ser. Pearson educación.
Pearson Educación, 2003. [Online]. Available: https://books.google.
com.co/books?id=QK148EPC_m0C
[9] J. W. Judy, “Microelectromechanical systems (mems): fabrication,
design and applications,” Smart Materials and Structures, vol. 10,
no. 6, p. 1115, 2001. [Online]. Available: http://stacks.iop.org/
0964-1726/10/i=6/a=301
[10] M. Dadafshar, “Accelerometer and gyroscopes sensors: Operation, sens-
ing, and applications,” Maxim Integrated, 2004.
[11] J. Rowberg. (2012). [Online]. Available: https://github.com/jrowberg/
i2cdevlib/tree/master/Arduino/MPU6050
[12] A. Mukherjee. (2016, Feb.) Getting started with imu (6dof)
motion. [Online]. Available: https://create.arduino.cc/projecthub/Aritro/
getting-started-with-imu-6-dof-motion-sensor-96e066
[13] E. B. D. M. K. M. Lillholm, “Quaternions, interpolation and animation,”
University of Copenhagen, Jul. 1998.
[14] S. Riisgaard and M. R. Blas, “Slam for dummies.”
[15] W. Burgard, C. Stachniss, K. Arras, and M. Bennewitz, Eds., Introduc-
tion to Mobile Robotics: SLAM.
[16] K. Kamarudin, S. Muhammad, A. Y. Shakaff, and A. Zakaria, “Per-
formance analysis of the microsoft kinect sensor for 2d simmultaneous
localization and mapping(slam) techniques,” in sensors, 2014.
[17] U. Darmstadt. (2017, 10) Hector slam. [Online]. Available: https:
//github.com/tu-darmstadt-ros-pkg
[18] S. Kohlbrecher, J. M. Oskar von Stryk, and U. Klingauf, “A flexible and
scalable slam system with full 3d motion estimation,” IEEE, 2011.
[19] R. M. (2016, 04) Lecture 3 2 hector mapping. [Online].
Available: https://www.youtube.com/watch?v=Q4qM-Uzj1SI&index=
2&list=PL7rtKJAz_mPdM_6IkYZonG9Gw7yYmm_Bz
[20] D. P. Koch, T. W. McLain, and K. M. Brink, “Multi-sensor robust
relative estimation framework for gps-denied multirotor aircraft,” in
2016 International Conference on Unmanned Aircraft Systems (ICUAS),
June 2016, pp. 589–597.
[21] S. Hajdu, S. T. Brassai, and I. Szekely, “Complementary filter based
sensor fusion on fpga platforms,” in 2017 International Conference on
Optimization of Electrical and Electronic Equipment (OPTIM) 2017
Intl Aegean Conference on Electrical Machines and Power Electronics
(ACEMP), May 2017, pp. 851–856.
[22] T. Abbas, M. Arif, and W. Ahmed, “Measurement and correction
of systematic odometry errors caused by kinematics imperfections in
mobile robots,” in 2006 SICE-ICASE International Joint Conference,
Oct 2006, pp. 2073–2078.
[23] R. Answers. (2016, 06) problems in hector slam. [Online]. Available:
https://answers.ros.org/question/235304/problems-in-hector-slam/