XI JORNADAS DE CIENCIA Y TECNOLOGA

FACULTADES DE INGENIERA DEL NOA

DE
Universidad Nacional de Jujuy

San Salvador de Jujuy, 23 y 24 de Junio de 2016

Facultad de Ingeniera

Sistema de navegacin autnoma por seguimiento de trayectorias

Perez1, Jorge O.; Ferrao1, Hilda N.; Juarez2, Gustavo E. y Sarapura1, Rubn.
(1) Laboratorio de Procesamiento Digital de Informacin, Dpto. de Electricidad, Electrnica y
Computacin, Facultad de Ciencias Exactas y Tecnologa, Universidad Nacional de Tucumn.
lpdi@herrera.unt.edu.ar
(2) Laboratorio de Inteligencia Artificial, Dpto. de Electricidad, Electrnica y Computacin,
Facultad de Ciencias Exactas y Tecnologa, Universidad Nacional de Tucumn.
juarez.gustavo@gmail.com
RESUMEN: Se presenta el desarrollo e implementacin de un sistema para la navegacin autnoma de
vehculos autoguiados. La estrategia de diseo est basada en la utilizacin de algoritmos de seguimiento de
trayectorias con asistencia de tecnologa GPS (Global Positioning System) (El-Rabbany, 2002) en conjunto
con una brjula electrnica para el control del ngulo de desplazamiento de las trayectorias p reviamente
definidas en una base de datos. Se considera su aplicacin en vehculos con desplazamiento en un sistema de
dos coordenadas horizontales. El sistema comprende el hardware asociado para el movimiento de un prototipo
mvil, y el software embebido con la codificacin algortmica. Los ensayos realizados en un vehculo
experimental terrestre, permitieron verificar la operacin del sistema. Se concluye que la utilizacin de esta
metodologa es adecuada para el control de trayectorias en forma autnoma en un plano horizontal. La continuidad
del trabajo est dirigida a contemplar los efectos del ruido de medicin y de control, efectuando correcciones
mediante la utilizacin de un filtro de Kalman (Grewal & Andrews, 2001) para su utilizacin en tres dimensiones
aplicado a un vehculo areo drone en construccin.

ABSTRACT: the development and implementation of a system for autonomous navigation of self-guided vehicles
(AGV) is presented. The design strategy is based on the use of algorithms of tracking trajectories assisted by GPS
(Global Positioning System) technology, in conjunction with an electronic compass for controlling the displacement
angle of the trajectories previously defined in a database. It is considered its application on vehicles with
displacement in a system of two horizontal coordinates. The system comprises the hardware associated for the
movement of a mobile prototype, and embedded software with the algorithmic encoding. Tests performed in a
terrestrial experimental vehicle, allowed verify system operation. It is concluded that the use of this methodology is
suitable for controlling trajectories autonomously in a horizontal plane. Continuity of work is directed to look at the
effects of the noise of measurement and control, making corrections using a Kalman filter for use in three dimensions
applied to an air vehicle 'drone' under construction.
Palabras claves: VAG, Trayectorias, Kalman, GPS
Keywords: VAG, Trajectory, Kalman, GPS

INTRODUCCIN

Los vehculos autoguiados poseen mltiples

aplicaciones tanto en actividades militares como
civiles, motivo por el cual poseen en la actualidad un
alto grado de desarrollo tecnolgico. El inters en los
mismos se vio incrementado en los ltimos aos con
los vehculos areos drones debido fundamentalmente a
su uso en actividades comerciales en desarrollos
agrcolas y de vigilancia, por citar solo algunas dentro
de sus amplias potencialidades. Las prestaciones de
seguimiento de trayectorias ocupan un lugar
preponderante para estos vehculos en aquellas
aplicaciones del tipo repetitivo o planificado, tales
como en sembradoras y cosechadoras que recorren un
campo con trayectorias diagramadas con anterioridad.
Otro ejemplo de inters es la vigilancia de incendios en
areas forestales y las de planimetra. Con el nimo de

contribuir en el campo de los vehculos autoguiados,

se presenta una aplicacin para el seguimiento de
trayectorias en un vehculo terrestre con la idea de
extrapolar los resultados obtenidos en esta primera
aproximacin a un vehculo areo drone en
construccin, analizndose en este informe solo lo
concerniente al control de trayectorias.

La propuesta aqu presentada utiliza tecnologa

GPS y comps magntico para la formulacin del
algoritmo de seguimiento de trayectorias aplicado
al control de direccin de un carro experimental
prototipo. El comportamiento y efectividad del
mismo fue verificado computacional y
experimentalmente en ensayos controlados, como
se describe en este trabajo.

XI JORNADAS DE CIENCIA Y TECNOLOGA

FACULTADES DE INGENIERA DEL NOA

DE
Universidad Nacional de Jujuy

San Salvador de Jujuy, 23 y 24 de Junio de 2016

Facultad de Ingeniera

SEGUIMIENTO AUTNOMO DE
TRAYECTORIAS

La operacin de este sistema se basa en la

definicin previa de las trayectorias a seguir
utilizando coordenadas terrestres de altitud y
latitud considerando un espacio bidimensional
para el desplazamiento del mvil autoguiado. A
tal propsito se diagram una base de datos con el
conjunto de puntos de coordenadas a seguir, la
cual puede ser conformada por ingreso manual o
mediante un registro automtico capturado de un
recorrido previo del mvil a modo de aprendizaje.
La utilizacin de un tarjeta comercial GPS Ublox
NEO-6M (Gonzalez Benavides y Marin Mendez,
2015) con salida serie UART permite contar con
la informacin requerida de las coordenadas
terrestres punto a punto, como se muestra en la
Fig. 1a, refiriendo a las coordenadas locales con
centro en P0, y la Fig. 1b coordenadas geogrficas
medidas por el GPS.

terrestre, estas problemticas son analizadas a

continuacin.
2.1 Distancias y Curvatura de la Tierra
El clculo geomtrico de la distancia entre dos
puntos resulta de una operacin trigonomtrica
sencilla, sin embargo para dos puntos
cualesquiera ubicados sobre la esfera terrestre
adems de calcular la distancia lineal entre ellos
a partir de la informacin de latitud y longitud, el
clculo entre ambas posiciones deber
contemplar la curvatura terrestre. La solucin
correctora consiste en la utilizacin de la frmula
de Haversine (Smart and Green, 1986), la cual
considera el radio terrestre R (medido en el
ecuador) para el clculo de la distancia entre dos
puntos afectados por la curvatura de la tierra, la
misma es dada en la expresin 1, en donde los
variables indicadas se corresponden con: R=
6378.1 km, lat2 latitud punto 2, lat1 latitud
punto1, long2 longitud punto2 y long1 longitud
punto1.

[
(

]
)

]
(1)

Figura 1: Coordenadas de referencias, (a) Punto a

Punto locales, (b) coordenadas geogrficas
terrestres (GPS).

( )

[ ]

2.2 Magnetmetro y Declinacin Magntica

Simultneamente el seguimiento de los puntos de
llegadas y salidas requieren conocer en el mvil
el ngulo de escape para recorrer las trayectorias
indicadas mostradas en la Fig. 1a, dicho ngulo
puede ser calculado geomtricamente en base al
clculo trigonomtrico, contando con la
informacin previa de los puntos P1, P2, Pi, y
utilizando un comps electrnico como
herramienta de medicin y control del ngulo
para su seguimiento, en este caso se trata de una
unidad magnetmetro
HMC5883L (Tutorial
Arduino, 2015), con salida I2C.
Sin embargo en la prctica existen factores
limitantes dados por la imprecisin propia de los
instrumentos utilizados (GPS y magnetmetro) y
los ruidos de medicin, tambin deben ser
considerados factores externos como la curvatura
de la tierra a partir de determinadas distancias, y
el sistema de referencia del ngulo medido en
relacin a los ejes magntico y geogrfico

La declinacin magntica en un punto de la Tierra

es el ngulo comprendido entre el norte
magntico local y el norte verdadero (o norte
geogrfico), es la diferencia entre el norte
geogrfico y el indicado por una brjula (el
denominado tambin norte magntico) que en
este caso es un magnetmetro electrnico por lo
cual debe considerarse esta correccin en la
determinacin del ngulo, como se indica en la
Fig. 2.

XI JORNADAS DE CIENCIA Y TECNOLOGA

FACULTADES DE INGENIERA DEL NOA

DE
Universidad Nacional de Jujuy

San Salvador de Jujuy, 23 y 24 de Junio de 2016

Facultad de Ingeniera

( )

( )
( )

( )
[

( )

(2)

Figura 2: Norte geogrfico y magntico.

2.3 Modelado Cintico

El modelado matemtico de la cintica del mvil
puede ser desarrollado en base a la Fig. 3,
considerando como tal dos puntos consecutivos
de las ensimas trayectorias parciales a recorrer.
A partir de la expresin 1 se posee la distancia
entre los puntos basada en la medicin GPS,
mientras que el ngulo del mvil lo proporciona
el comps magntico, ambos instrumentos
entregan medidas con ruido de medicin, y el
mvil en s mismo posee su accin de empuje con
un comportamiento no lineal que no est
considerado en este modelo. En la expresin 2, se
indican el clculo de los errores angulares y
distancias en el acercamiento a un punto de
trayectoria.

En funcin de la geometra mostrada en la Fig.

3, las ecuaciones indicadas por la expresin 2,
y considerando las leyes del movimiento, se
plantean las ecuaciones odomtricas del mvil.
La informacin disponible sobre las distancias y
ngulos provienen de dos fuentes de referencias
bien
diferenciadas,
por
un
lado
las
correspondientes al conjunto de puntos
almacenados en la base de datos interna con las
trayectorias programadas a recorrer, y por el otro
lado las aportadas por el sistema de lectura GPS y
comps electrnico angular, ambas imprecisas y
con ruido electrnico de medidas. Como se
menciona anteriormente el uso del algoritmo de
Kalman aportara entonces las medidas ptimas
para ambas variables de posicionamiento en cada
punto recorrido, por lo cual en la continuidad de
este trabajo se contempla su utilizacin, mientras
que en la instancia actual se procedi a una
comparacin de resultados considerando la
solucin de Kalman a nivel de modelado en
Matlab a fin de verificar la exactitud relativa del
sistema, planteando el conjunto de expresiones
discretas indicadas en la expresin 3.

)[

]
(

)[
]

(
(3)

2.4 Mediciones, Ruidos, y Filtrado de Kalman

Figura3: Esquema de clculo de ngulos y

distancias.

De los muchos mtodos utilizados para la

estimacin de los valores verdaderos a partir de
mediciones ruidosas,
el filtro de Kalman
presenta la particularidad de aportar una
estimacin ptima a partir de un
clculo
computacional simple, plausible de ser
implementado
con
tecnologa
de
microcontroladores. El filtro de Kalman requiere
para su funcionamiento de las medidas de los
sensores y la ecuacin de estados con la

XI JORNADAS DE CIENCIA Y TECNOLOGA

FACULTADES DE INGENIERA DEL NOA

DE

San Salvador de Jujuy, 23 y 24 de Junio de 2016

Universidad Nacional de Jujuy

Facultad de Ingeniera

evolucin de las variables fsicas a estimar, as

tambin como de condiciones mnimas necesarias
para su aplicacin: ruido de valor medio cero y
distribucin Gausiana normal,
con
un
comportamiento lineal para el sistema tratado. En
este primer desarrollo se trabaj con el modelado
del algoritmo original propuesto por Kalman,
considerando al mvil
en condiciones de
linealidad. Las expresiones mostradas en 4,
indican el conjunto de funciones paramtricas que
rigen el modelado de Kalman. Las mismas
contemplan los estados de las variables y las
mediciones de las mismas.
(

(4)

DESARROLLO DEL SISTEMA

3.1 Mediciones de coordenadas GPS

Las mediciones efectuadas por el GPS y el
magnetmetro, son utilizadas como referencias en
tiempo real de la posicin instantnea y direccin
de avance del mvil hacia un punto dado del
conjunto de puntos que definen la trayectoria
diagramada, tal como se muestra en la figura 4,
para el punto intermedio Pi entre P1 y P2.
Por otro lado se conoce la posicin de cada uno
de los puntos de trayectoria P1, P2, etc., los
cuales estn almacenados en el microcontrolador
en un arreglo de dos dimensiones X, Y referidos a
las posiciones de altitud y latitud terrestres como
se muestra en la Tabla I.

x: Vector de estado de las variables (lat, long, d,

I)

NORTE

u: Ruido de medicin

Ruido del proceso

P3

z: Vector de medidas

Pi=(xk,yk,

v: Vector de ruido de medicin

Los dos procesos del filtro interactivo de Kalman,
estimacin y actualizacin fueron implementados
en Matlab, basados en las ecuaciones que definen
los mismos, para el caso del mvil propuesto se
plantearon las ecuaciones mostradas en las
expresiones 5 y 6.

k)

P4
P1
Figura 4: Posicin de un punto de trayectoria.

(5)
(

(6)

En las que las covarianzas del ruido de medicin

y de proceso fueron consideradas, la ganancia de
K de Kalman con un valor inicial de 0.5, la
expresin 7 indica la estructura del vector de
estado
utilizado
para
la
simulacin
computacional.

(7)

En la tabla I, el formato indicado se corresponde

con el entregado por el GPS a fin de poder operar
trigonomtricamente de acuerdo a las ecuaciones
odomtricas desarrolladas anteriormente. Este
formato debe ser interpretado como Grados,
minutos y segundos, altitud norte y latitud este,
en correspondencia con la figura 5, que muestra
la informacin provista en formato serial por la
tarjeta GPS.
Tabla I: Formato de Puntos de Trayectorias
Punto

Altitud

Latitud

P1

A1

L1

P2

A2

L2

Pi

N-Gi-Mi-Si

E-Gi-Mi-Si

Pn

An

Ln

XI JORNADAS DE CIENCIA Y TECNOLOGA

FACULTADES DE INGENIERA DEL NOA

DE

San Salvador de Jujuy, 23 y 24 de Junio de 2016

Universidad Nacional de Jujuy

Facultad de Ingeniera

Figura 5: Trama del GPS.

En esta trama existe informacin adicional que no

es utilizada en el marco de este desarrollo, a ttulo
informativo se describe a continuacin, siendo las
altitud y latitud terrestre las que son
enmascaradas para su uso como deteccin de
posicionamiento, estas se corresponden con el
protocolo NMEA (National Marine Electronics
Association) manejado por el GPS utilizado.
GGA = Global Positioning System Fix Data

El error e en la expresin 8 es utilizado entonces

como medida para un controlador digital simple
del tipo proporcional, cuya salida es utilizada para
el control del motor de traccin derecho o
izquierdo segn corresponda al signo y valor del
error e. La relacin de control est dada por la
expresin 9, en la cual Ti es el tiempo de
activacin de cada motor, y kp el valor de
ganancia proporcional escogida experimentalmente mediante un procedimiento de ensayo y
error kp=[0.15,0.82].

3.3 Software embebido y hardware desarrollado

111516 = Muestra tomada a 10:25:17 UTC

2649038 = Norte, Latitud 26 grados 49.038 N
6513000 = Este, Longitud 65 grados 13.000 E
1 = Fix quality

La electrnica asociada son

los circuitos
necesarios para el control de potencia de dos
motores de CC, y los sensores de ngulo y
posicin con alimentacin a bateras de litio
polmero.

08 = Numero de satlites siendo rastreados

0.9 = Dilucin horizontal de la posicin
450,M = Altitud, metros, sobre el nivel del mar
46.9,M = altura del geoid (promedio nivel del
mar) sobre WGS84
(empty field) = time tiempo desde la ltima
muestra
(empty field) = estacin DGPS numero ID
*46 = datos checksum, siempre comienza con *
3.2 Mediciones angulares
La lectura del ngulo de avance I en relacin al
norte geogrfico, son aportadas por el
magnetmetro, recordando que esta informacin
se refiere al norte magntico y no terrestre, por lo
cual debe ser afectada por el ngulo de correccin
correspondiente a la declinacin magntica de
6.17 radianes para la ciudad de San Miguel de
Tucumn, este valor es designado como M, en la
expresin 8 se indica el clculo del error
instantneo.
(

(9)

)
(8)

Figura 6: Diagrama de flujo principal.

XI JORNADAS DE CIENCIA Y TECNOLOGA

FACULTADES DE INGENIERA DEL NOA

DE
Universidad Nacional de Jujuy

San Salvador de Jujuy, 23 y 24 de Junio de 2016

Facultad de Ingeniera

En las Figs. 6, 7 y 8 se muestra el diagrama de

flujo del software embebido, y los modelos
fsicos
de
interconexin
del
hardware
desarrollado.

Figura 9: Captura Sofware Ublox center.

Figura 7: Conexin GPS.

Los resultados obtenidos mostraron gran

dispersin en las medidas registradas tanto por el
GPS como el magnetmetro, a modo de caso
testigo se presentan los resultados de un conjunto
de 20 repeticiones sobre dos puntos de referencia,
se muestra las grficas de las Figs. 10 y 11, para
un recorrido repetido en forma consecutiva sobre
la misma geografa entre dos puntos separados 20
Mts, coordenadas 26.8400, 65.2311 y 26.8402,
65.2310, en el cual se incluye el modelado por
correccin con filtrado de Kalman, con niveles
SNR=92.5 db y SNR=115 db,a fin de evaluar su
futura
realizacin
embebida
en
el
microcontrolador.

DISTANCIAS
23

Figura 8: Conexin Magnetmetro.

GPS
Referencia
Kalman

22.5
22

ENSAYOS EXPERIMENTALES

Los ensayos experimentales estuvieron dirigidos

a la verificacin de un conjunto de trayectorias
previamente almacenadas en forma de latitud y
longitud como se indic en la tabla I, en
distancias medias de 20 Mts. Como herramientas
de apoyo se conto con la informacin de
googlemaps, GPS GARMIN Nuvi 2597 y
medidas fsicas con cinta mtrica convencional.
Tambin se utilizo como soporte grafico el
software ublox-center, con el fue posible
contrastar las entradas ledas por el modulo GPS
Ublox NEO-6M (Ublox, 2016) mediante la
entrada USART del ordenador, Fig. 9.

Medidas (Mts)

21.5
21
20.5
20
19.5
19
18.5
18

10
12
Mediciones

14

16

18

20

Figura 10: Medidas y correccin con filtro de kalman,

SNR=92.5 db.

XI JORNADAS DE CIENCIA Y TECNOLOGA

FACULTADES DE INGENIERA DEL NOA

DE

San Salvador de Jujuy, 23 y 24 de Junio de 2016

Universidad Nacional de Jujuy

Facultad de Ingeniera

DISTANCIAS
23
GPS
Referencia
Kalman

22.5
22

Medidas (Mts)

21.5
21

REFERENCIAS

20.5
20

Ahmed El-Rabbany, Introduction to GPS: The

Global Positioning System, Artech House,
ISBN: 1-58053183-1. 2002.

19.5
19
18.5
18

10
12
Mediciones

14

16

18

20

Figura 11: Medidas y correccin con filtro de

kalman, SNR=115 db.
5

CONCLUSIONES

Se concluye que la utilizacin de esta

metodologa es adecuada para el control de
trayectorias en forma autnoma en un plano
horizontal, pero que posee marcados errores en la
determinacin de las medidas por instrumentos.
Estos errores pueden ser adjudicados en parte a la
precisin de estos circuitos electrnicos, para el
caso del GPS la precisin de las lecturas
dependen del nmero de satlites intervinientes,
para el magnetmetro de los campos magnticos
cercanos (presentes en los motores y conductores
elctricos
del
mvil)
que
influencian
notablemente a las lecturas de los ngulos. La
utilizacin del algoritmo de Kalman a nivel de
simulacin computacional (fuera de lnea) indica
que puede ser una alternativa de correccin valida
si el mismo es implementado en forma embebida,
siendo esto lo que se contempla como
continuidad del trabajo a futuro.

Grewal, M. S. and A. P. Andrews. Kalman

Filtering: Theory and Practice Using MATLAB
Wiley-Interscience. 2001.
Gonzalez Benavides, J. F. y C. A. Marin Mendez,
Prototipo de sistema de seguridad en vehculos
de transporte pblico que permite la captura de
imagen fotogrfica, posicionamiento global y
almacenamiento
en
base
de
datos,
http://botscience.net/store/index.php?route
product/product&product_id=73. 2015.
Tutorial Arduino, Practica con Arduino y brjula
hmc5883L.http://besoft.com.es/tutorialarduino/tutorial-arduino-puntos-cardinalesusando-brujula-hmc5883l/. 2015.
Smart, W. M. and R. M. Green. Textbook on
spherical astronomy, W. M. Smart. Cambridge ;
New York : Cambridge University Press, 1986.
Ublox.

U-CenterWindows. GNSS evaluation

software
for
Windows.
https://www.ublox.com/en/product/u-center-windows. 2016.

Este trabajo ha sido realizado con apoyo de la Secretara de

Ciencia y Tcnica de la UNT a travs del Programa PIUNT
26E512.