Documentos de Académico
Documentos de Profesional
Documentos de Cultura
Micro-Robot Trazador de Trayectorias PDF
Micro-Robot Trazador de Trayectorias PDF
1. Introducción 1
1.1. Situación actual . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.2. Objetivos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.3. Fases de desarrollo . . . . . . . . . . . . . . . . . . . . . . . . 3
1.4. Material de trabajo . . . . . . . . . . . . . . . . . . . . . . . . 3
3. Micro-robótica móvil 23
3.1. Robótica móvil . . . . . . . . . . . . . . . . . . . . . . . . . . 23
3.2. Mecatrónica para micro-robots . . . . . . . . . . . . . . . . . 26
3.2.1. Mecánica . . . . . . . . . . . . . . . . . . . . . . . . . 26
3.2.2. Eléctrica . . . . . . . . . . . . . . . . . . . . . . . . . . 30
3.2.3. Electrónica . . . . . . . . . . . . . . . . . . . . . . . . 31
3.3. Percepción en micro-robots . . . . . . . . . . . . . . . . . . . 36
3.3.1. Sensores y sistemas de percepción de entorno . . . . . 36
3.3.2. Sensores y sistemas de percepción del propio movi-
miento del robot . . . . . . . . . . . . . . . . . . . . . 41
3.4. Resumen de los sistemas presentes en un robot móvil . . . . . 42
iii
Micro-Robot Trazador de Trayectorias iv
Bibliografía 84
Anexo A 87
Anexo B 91
Anexo C 104
Índice de figuras
v
Micro-Robot Trazador de Trayectorias vi
Índice de cuadros
vii
Micro-Robot Trazador de Trayectorias viii
Palabras clave
ix
Micro-Robot Trazador de Trayectorias x
Resum
xi
Micro-Robot Trazador de Trayectorias xii
Resumen
xiii
Micro-Robot Trazador de Trayectorias xiv
Abstract
In this master thesis, has been done a detailed study of the mechanical
architecture, electrical, electronic and software to build a mobile robot that
possesses the ability to reconstruct the path followed by taking data from an
inertial sensor and applying tools is software for processing that data.
Throughout this report, has been analyzed some theoretical aspects for
the design and construction of a mobile robot as well as theoretical aspects
of inertial navigation, base for the reconstruction of the paths followed by
the robot using for that inertial measurement data taken by the system.
Later, has been detailed the process of creating prototypes and informa-
tics applications that may be used in the future development of a mobile au-
tonomous system capable of reconstructing trajectories and the subsequent
testing and validation of these tools.
Finally, on this paper, has been done a reflect on the validity or not of all
the developments that are detailed along it, and how all thing here presented
can be implemented for future creation of mobile robots with capacity for
reconstruction of trajectories and even develop inertial navigation applica-
tions.
xv
Micro-Robot Trazador de Trayectorias xvi
Capítulo 1
Introducción
1
Micro-Robot Trazador de Trayectorias 2
1.2. Objetivos
El principal objetivo de este proyecto es el de diseñar, desarrollar y testear
posibles soluciones que puedan aplicarse para la construcción de un robot
móvil completamente operativo y funcional que sea capaz de realizar movi-
mientos por sí mismo sin necesidad de ser teleoperado y que este, sea capaz
de de obtener información de su movimiento, para luego, mediante una serie
de herramientas de software, poder calcular cual ha sido la ruta seguida por
el robot.
A continuación se detallan y enumeran de forma detallada cuales son los
objetivos que se persiguen con esta tesis de máster.
Hacer un estudio detallado de qué elementos serian necesarios para
la creación de una plataforma robótica móvil que sea capaz de ejecu-
tar movimientos de forma completamente autónoma, sin necesidad de
estar conectado a fuentes de alimentación externas (por ejemplo, es-
tar conectado a la red eléctrica) ni de ser guiado remotamente por un
operador.
Analizar y desarrollar un sistema que pueda ser integrado en el robot
móvil que permita la adquisición de datos inerciales relacionados con
su movimiento así como de elementos para que el robot móvil pueda
almacenar dicha información para que pueda ser tratada y con ello
reconstruir las trayectorias que ha seguido.
Desarrollo y creación de herramientas de software que permitan, con
los datos obtenidos por el robot durante su movimiento, reconstruir de
una manera lo más fiel posible el movimiento del robot, es decir, crear
alguna aplicación que permita recrear las trayectorias que ha seguido
el robot.
Poner a prueba los elementos que se han desarrollado, para un aná-
lisis de las posibilidades reales de, a partir de estos elementos, crear
un sistema que pueda hacer de manera autónoma una navegación sin
intervención externa y a la vez tomar datos para una posterior recons-
trucción de la trayectoria seguida.
Hacer una reflexión de cuáles son las principales problemáticas que se
pueden presentar a la hora de crear un robot móvil con capacidad para
reconstruir trayectorias.
En suma, se pretende analizar y crear herramientas para que sea viable la
construcción de un robot capaz de moverse de forma autónoma y que al
tiempo sea capaz de obtener suficiente información, para posteriormente,
reconstruir los movimientos ha realizado. A lo largo de los diferentes capí-
tulos que componen esta tesis, se irán desgranando los diferentes objetivos
detallados anteriormente.
Micro-Robot Trazador de Trayectorias 3
5
Micro-Robot Trazador de Trayectorias 6
IMU’s de bajo coste, que básicamente intentan corregir los errores de medida
que estas IMU’s de bajo coste llevan intrínsecos [1, 2].
En la figura 2.1 podemos observar como es el proceso que se utiliza para la
navegación inercial. En un primer momento se obtienen los datos de la IMU,
concretamente información acerca de la velocidad angular (giroscopio) y de la
aceleración (acelerómetro) a la que está sometido el cuerpo a estudio. Estos
datos, junto con información inicial de velocidad, posición y orientación,
permiten mediante un proceso de transformación de coordenadas obtener la
información de posición, velocidad y orientación en el momento en el que ha
recogido el dato.
Como se puede observar este proceso de estimación está basado en la inte-
gración, por lo que los errores que aparecen en la estimación de la orientación
son propagados y acumulados a lo largo del tiempo. Estos errores acumu-
lativos son los que al final determinan el grado de precisión del sistema de
navegación inercial. El estudio de estos errores se basa en métodos estadís-
ticos y son posteriormente usados en los algoritmos de fusión de datos para
minimizar los efectos de estos errores.
El primer tipo de sistema de navegación inercial desarrollado era el deno-
minado “gimballed system”. En este sistema los acelerómetros son montados
sobre una plataforma cardan motorizada que se mantiene siempre alineada
con el marco de navegación. Los sensores se encuentran sobre los cardans
externos e internos que mantienen la pista de la actitud de la plataforma
estabilizada con respecto al vehículo en el que está montado el sistema de
navegación inercial. Esta configuración tiene varios problemas que hacen des-
aconsejable su uso, entre los que se encuentran un mayor consumo por los
motores, mala alineación de la plataforma cardan, los motores han de ser
muy precisos o los altos constes de mantenimiento que tiene.
Con el desarrollo de los MEMS (Microelectromechanical systems) el “gim-
Micro-Robot Trazador de Trayectorias 7
Roll o α es el ángulo girado por los ejes ortogonales móviles con respeto
al eje X del sistema fijo.
Pitch o β es el ángulo girado por los ejes ortogonales móviles con res-
peto al eje Y del sistema fijo.
Yaw o γ es el ángulo girado por los ejes ortogonales móviles con respeto
al eje Z del sistema fijo.
sabiendo que:
Micro-Robot Trazador de Trayectorias 9
cos(γ) −sen(γ) 0
Rz (γ) = sen(γ) cos(γ) 0 (2.2)
0 0 1
cos (β) 0 sen (β)
Ry (β) = 0 1 0 (2.3)
−sen (β) 0 cos (β)
1 0 0
Rx (α) = 0 cos (α) −sen (α) (2.4)
0 sen (α) cos (α)
Si sustituimos las ecuaciones (2.2), (2.3) y (2.4) en (2.1) y operamos,
obtenemos la matriz que sigue:
cβcγ cγsαsβ − cαsγ cαcγsβ + sαsγ
Rfm = cβsγ cαcγ + sαsβsγ −cγsα + cαsβsγ (2.5)
−sβ cβsα cαcβ
siendo cθ y sθ el coseno y el seno del ángulo θ respectivamente.
Esta matriz, es la denominada matriz de rotación que define el giro de
un sistema de referencia móvil (m) con respecto a un sistema de referencia
fijo (f). Así, para rotar un vector vm (xm , ym , zm ) definido en un sistema
de referencia móvil a un vector definido en un sistema de referencia fijo
vf (xf , yf , zf )no hay más que multiplicar la matriz de rotación de los ejes
móviles en ejes fijos siguiendo la siguiente ecuación.
vf = Rfm vm (2.6)
Otro método de cambio de sistemas de referencia es el basado en el uso
de cuaterniones. Un cuaternión es un número complejo de cuatro dimen-
siones que se puede utilizar para representar la orientación de un cuerpo o
sistema de referencia en el espacio tridimensional. Una orientación arbitra-
ria del sistema de referencia móvil respecto al sistema fijo puede lograrse a
través de una rotación de ángulo θ alrededor de un eje rf definido para un
sistema de referencia fijo. En la figura 2.4 se muestra una representación grá-
fica de un cuaternión donde los vectores unitarios mutuamente ortogonales
XA , YA , ZA y XB , YB , ZB definen los ejes de los sistemas de referencia fijo y
móvil respectivamente.
f
El cuaternión que describe esta orientación, qm se define como
θ θ θ θ
f
qm = [q1 q2 q3 q4 ] = [cos − rx sen − ry sen − rz sen ] (2.7)
2 2 2 2
f
Como se puede apreciar en la Ec. (2.7), qm describe la orientación del
eje móvil con respecto al eje fijo, siendo rf el vector descrito con respecto al
Micro-Robot Trazador de Trayectorias 10
eje fijo. Para describir el eje fijo con respecto al eje móvil (qfm )se utiliza la
siguiente expresión conjugada:
f∗
qm = qfm = [q1 − q2 − q3 − q4 ] (2.8)
Para rotar un vector vm (xm , ym , zm ) definido en un sistema de referencia
móvil a un vector definido en un sistema de referencia fijo vf (xf , yf , zf ), hay
que multiplicar por el cuaternión que define la rotación del sistema de ejes
móviles en el sistema de ejes fijos de la manera que se muestra:
f∗
f
v f = qm ⊗ v m ⊗ qm (2.9)
A partir de los cuaterniones, también se puede definir la matriz de rotación
utilizando la siguiente expresión:
a = (ax , ay , az ) (2.11)
d
a= v (2.12)
dt
d
v= x (2.13)
dt
d2
d d
a= x = x (2.14)
dt dt dt2
w = (wx , xy , xz ) (2.18)
d
wi = θi , ∀i(x, y, z) (2.19)
dt
Teniendo esto en cuenta, se puede obtener el ángulo girado de cualquiera
de los tres ejes integrando la expresión anterior, con lo que quedaría de la
siguiente manera:
Z
θ= w.dt (2.20)
Puesto que ya tenemos los tres ángulos de Euler (Roll, Pitch y Yaw), se
puede crear una matriz de rotación. Con ella es posible rotar la aceleración
dada por el acelerómetro, con lo que se obtiene la medida referida al sis-
tema de referecia a los ejes fijos (af ). Para ello es necesario multiplicar el
vector proporcionado por el acelerómetro (am ) por la matriz de rotación de
la siquiente manera:
Este método ofrece una gran simplicidad debido a que es muy sencillo de
implemetar algoritmicamente en un computador. Sin embargo, los girosco-
pios inducen un error acumulativo importante debido a que son integrados
Micro-Robot Trazador de Trayectorias 13
sin un punto de referencia fijo. Así los datos obtenidos a partir de este mé-
todo son poco fiables por el error creciente tras cada integración, originando
desviaciones en la medida de los ángulos girados.
2.6. Errores
Los acelerómetros y giroscopios que son los encargados de tomar los datos
de aceleración y velocidad angular a la que está sometido el cuerpo a estudio
están sujetos a varios parámetros de rendimiento especificados por los fabri-
cantes de sensores para describir la precisión y el rendimiento que el usuario
puede esperar alcanzar [6].
Los errores en las mediciones realizadas por los giroscopios se propagan
en las estimaciones de la posición, velocidad y orientación a través de la inte-
gración de mediciones de velocidad angular inexactas. Debido a este hecho,
también habrá una inexactitud creciente al rotar el vector de la aceleración.
Asimismo, los errores en las mediciones del acelerómetro redundarán en la
propagación de errores a través de un cálculo inexacto de posición. De esta
manera, se puede concluir que los giroscopios y los acelerómetros experi-
mentan el mismo tipo de errores, caracterizados como errores sistemáticos o
aleatorios.
Los errores sistemáticos causan la deriva que se experimenta en un INS,
incluso cuando la unidad está parada. Los principales tipos de errores sis-
temáticos son el sesgo, error de factor de escala y el error de acoplamiento
cruzado. De los tres errores mencionados, es el sesgo (bias) el más grande de
los tres, por lo que consideraremos únicamente este error despreciando los
otros dos por ser muy pequeños en comparación del sesgo.
El sesgo es una desviación sistemática de la medida proporcionada por los
sensores con el tiempo. Todos los sensores presentan algún grado de sesgo. Es
un error independiente lo que significa que se produce independientemente
de la aceleración o la señal del giroscopio. Esto significa que incluso cuando
un INS se deja estacionario, su posición va a experimentar una acumulación
de error. El término sesgo se puede separar en dos componentes: sesgo de
estabilidad y sesgo de repetición. Se puede calcular el sesgo utilizando la
siguiente expresión:
El filtro de Kalman.
yt = Hxt + vt (2.25)
siendo:
xt Estado.
p(w) ∼
= N (0, Q) (2.26)
p(v) ∼
= N (0, R) (2.27)
siendo Q y R las matrices de covarianza de la perturbación del proceso y
de la perturbación de la medida respectivamente.
La matriz A se asume de una dimensión nxn y relaciona el estado en el
periodo previo t-1 con el estado actual t. La matriz H, de dimensiones mxn,
relaciona el estado con la medición yt . Estas matrices pueden cambiar con el
tiempo, pero también se asumen constantes por simplicidad.
Una vez definido el proceso a ser estimado, se deberá aplicar el algorit-
mo del filtro de Kalman. La primera etapa, de predicción, pronostica las
estimaciones del estado y la covarianza mediante:
ˆ +ˆBut−1
xˆ∗t = Ax∗t−1 (2.28)
x̂t = xˆ∗t + Kt yt − H xˆ∗t (2.31)
Pt = (I − Kt H) Pt∗ (2.32)
Después de cada par de actualizaciones, tanto del tiempo como de la
medida, se repite el proceso tomando como punto de partida las nuevas
estimaciones del estado y de la covarianza del error. Esta naturaleza recursiva
es una de las características llamativas del filtro de Kalman.
Micro-Robot Trazador de Trayectorias 16
Π
β =θ+ (2.33)
2
Por otro lado, por trigonometría, sabemos que el ángulo θ se puede cal-
cular de la siguiente manera:
aceleración
aceleración = cos(θ) ∗ g ⇒ θ = arccos (2.34)
g
Si sobre la ecuación (2.33) aplicamos la ecuación (2.34) y tenemos en
cuenta que la aceleración se mide en los 3 ejes obtenemos que podemos
calcular el ángulo Pitch de la siguiente manera:
!
ay
pitch = −arctan p (2.35)
a2x + a2z
Con este sistema somos capaces de filtrar los datos y estimar el error acu-
mulado y eliminarlo. El problema surge cuando se intenta calcular el valor del
ángulo girado por el eje Z, el Yaw. Con este sistema es imposible calcularlo
a partir de las aceleraciones, por lo que se necesitaría otro dispositivo adi-
cional al acelerómetro y el giroscopio para poder filtrar esos datos y estimar
su error, como podría ser una brújula magnética.
Micro-Robot Trazador de Trayectorias 18
wm = [0wx wy wz ] (2.37)
La derivada del cuaternión con respecto al tiempo que describe el cambio
de los ejes fijos con respecto a los ejes móviles se puede calcular utilizando
la siguiente ecuación:
d f 1 f
q = qm,est,t−1 ⊗ wm (2.38)
dt m,w 2
Para calcular la orientación relativa de los ejes fijos con respecto a los
ejes móviles usando la velocidad angular se puede calcular mediante la in-
tegración de la derivada de este con respecto al tiempo (2.38). Si tenemos
definido el periodo de adquisición de datos ∆t y la posición estimada anterior
f
qm,est,t−1 podemos calcular el cuaternión que define la orientación de los ejes
fijos con respecto a los móviles con la siguiente ecuación:
f f d f
qm,w,t = qm,est,t−1 +q ∆t (2.39)
dt m,w
El acelerómetro nos entrega información de la aceleración de los tres ejes
cartesianos que componen el sistema móvil. Se puede representar como un
cuaternión de la siguiente manera:
am = [0ax ay az ] (2.40)
Por otro lado, es perfectamente válido asumir el campo gravitacional de
la tierra como paralelo al eje Z con respecto al sistema de referencia de la
tierra. Se puede representar de la siguiente manera:
gf = [0001] (2.41)
Micro-Robot Trazador de Trayectorias 19
Donde:
f∗
f
f qm , df , sm = qm f
⊗ df ⊗ qm − sm (2.43)
f
qm = [q1 q2 q3 q4 ] (2.44)
2dy q4 − 2dz q3 2dy q3 + 2dz q4
f
J T qm,k , df = −2dx q4 + 2dz q2 2dx q3 − 24dq2 + 2dz q1 (2.50)
2dx q3 − 2dy q2 2dx q41 − 2dy q1 − 4dz q2
−4dx q3 + 2dy q2 − 2dz q1 −4dx q4 + 2dy q1 + 2dz q2
2dx q2 + 2dz q4 −2dx q1 − 4dy q4 + 2dz q3
2dx q1 + 2dy q4 − 4dz q3 2dx q2 + 2dy q3
d f
q
µt = α|| ||∆t, α > 1 (2.53)
dt m,w,t
En la figura 2.7 se presenta un esquema general del algoritmo y su imple-
mentación.
f∗
f
af = qm ⊗ a m ⊗ qm (2.54)
Micro-Robot Trazador de Trayectorias 22
Capítulo 3
Micro-robótica móvil
23
Micro-Robot Trazador de Trayectorias 24
Triciclo. El robot tipo triciclo está formado por dos ruedas convencio-
nales fijas sobre un mismo eje y una rueda convencional centrada orien-
table que concentra las funciones de tracción-dirección. Al igual que en
el caso anterior, la estructura mecánica y la electrónica de control son
Micro-Robot Trazador de Trayectorias 26
3.2.1. Mecánica
En cuanto a la parte mecánica del robot móvil, hay que mencionar los
motores. Si bien es cierto que habitualmente los motores que se utilizan para
los robots móviles son motores eléctricos, es la parte mecánica la que nos
interesa [14, 16, 17].
Los motores en los robots móviles se utilizan principalmente para la loco-
moción. Suelen ser parte de los sistemas de tracción, pero también podemos
encontrar otros motores desempeñando otras funciones, como por ejemplo,
en los sistemas de dirección de cuatriciclos con ruedas delanteras de dirección
o en pinzas y manipuladores, si el robot cuenta con alguna de ellas.
Habitualmente los motores eléctricos se caracterizan por trabajar a unas
revoluciones muy altas, pero a costa de tener un par motor muy bajo. Es
siempre recomendable que el par motor de los sistemas de tracción de los
robots móviles sea lo más alto posible, ya que habitualmente las bases son
pesadas debido a todos los sistemas de control y principalmente a la fuente
de energía que debe ser portátil para que un robot se considere autónomo.
Antes de continuar con la parte mecánica, definiremos algunos de los tipos
de motores más utilizados para aplicaciones robóticas móviles.
Micro-Robot Trazador de Trayectorias 27
Motores paso a paso. Los motores paso a paso se pueden ver como
motores eléctricos sin escobillas. Es típico que todos los bobinados del
motor sean parte del estátor, y el rotor puede ser un imán permanente
o, en el caso de los motores de reluctancia variable, un cilindro sóli-
do con un mecanizado en forma de dientes (similar a un engranaje),
construido con un material magnéticamente "blando" (como el hierro
dulce). La conmutación se debe manejar de manera externa con un
controlador electrónico y, típicamente, los motores y sus controladores
se diseñan de manera que el motor se pueda mantener en una posición
fija y también para que se lo pueda hacer girar en un sentido y en
el otro. La mayoría de los motores paso a paso conocidos se pueden
hacer avanzar a frecuencias de audio, lo que les permite girar muy ve-
lozmente. Con un controlador apropiado, se los puede hacer arrancar
y detenerse en un instante en posiciones controladas. Un motor paso
a paso se define por estos parámetros básicos: voltaje, resistencia eléc-
trica y grados por paso. Los motores paso a paso tienen una tensión
eléctrica de trabajo. Este valor viene impreso en su carcasa o por lo
menos se especifica en su hoja de datos. Algunas veces puede ser nece-
sario aplicar un voltaje superior para lograr que un determinado motor
cumpla con el par motor deseado, pero esto producirá un calentamiento
Micro-Robot Trazador de Trayectorias 28
que puedan rotar libremente los 360 grados. En la figura 3.2 podemos
observar distintas posiciones del eje en función de la señal que recibe.
mentos del robot móvil. Habitualmente para pequeños robots está hecho de
plástico u otros materiales ligeros ya que deben soportar un peso reducido.
Para aquellas aplicaciones que necesiten más robustez o cargar con más peso,
los chasis suelen estar construidos en materiales más resistentes y pesados
como puede ser metal.
3.2.2. Eléctrica
En esta sección hablaremos de los componentes eléctricos que suelen for-
mar parte de los robots móviles. Evidentemente las resistencias, condensado-
res y bobinas estarían incluidas dentro de esta sección por ser componentes
eléctricos, pero dado que son elementos secundarios y que también se usan
en la parte electrónica, serán omitidos en esta parte.
Cuando hablamos de la parte eléctrica de un robot, se suele hacer refe-
rencia a la parte de alimentación y potencia que compone todo robot móvil.
Concretamente de las fuentes de alimentación [15, 16].
Habitualmente cuando nos referimos a robots móviles, hablamos de sis-
temas que se alimentan de energía eléctrica. Por ello es necesario dotar a
estos de algún tipo de sistema que proporcione esta energía de manera que
pueda ser transportada con propio robot móvil, para no depender de fuentes
externas.
Entre los sistemas más comunes para abastecer de energía eléctrica a estos
robots cabe destacar dos, las baterías y los paneles solares fotovoltaicos.
Los paneles solares fotovoltaicos, son sistemas que están formados por
celdas fotovoltaicas capaces de transformar la radiación solar en energía eléc-
trica. Estos sistemas tienen la ventaja de que la engría solar es abundante
y supone una fuente de energía ilimitada y gratuita. Pero tiene numerosos
inconvenientes. El más importante es que es una fuente de energía que sólo
está disponible un número de horas diarias, por lo que hay que añadir al sis-
tema algún tipo de acumulador de energía para las horas en las que no esté
disponible la luz solar. Esto hace que los sistemas solares sean muy pesados y
restringidos a aplicaciones donde los robots móviles se utilicen en ambientes
exteriores.
Por otro lado, tenemos las baterías que son el sistema más extendido
y generalizado para alimentar eléctricamente a los robots móviles. Existen
numeroso tipos de baterías en función del material con el que están hechas,
de si son o no recargables tamaños, etc.
Las baterías pueden caracterizarse a partir de sus parámetros fundamen-
tales, que son:
3.2.3. Electrónica
La electrónica de un robot móvil es todo aquel circuito electrónico o con-
junto de estos, que son necesarios para el correcto funcionamiento de un
robot. En el caso de los robots móviles, los principales sistemas electrónicos
son aquellos relacionados con la administración y conversión de energía, el
control y manejo de los motores y el sistema de control.
A continuación, se presenta un estudio de cada una de las diferentes partes
anteriormente citadas.
Existen dos tipos de reguladores lineales. Los reguladores lineales fijos y los
reguladores lineales ajustables. Ambos son reguladores de tensión lineales,
pero la diferencia radica en que los fijos, proporcionan una tensión fija y
constante determinada por el modelo del regulador. En el caso de los regu-
ladores variables, la tensión es también fija y constante, pero esta no viene
determinada por el modelo, se determina a partir de la tensión que se le
aplica a la patilla de control del regulador. En la figura 3.3 se pueden obser-
var un ejemplo de dos reguladores, un LM1117, que es un regulador lineal
ajustable y de un 7805, que es un regulador lineal fijo [16, 17].
Los drivers de motor aíslan los circuitos lógicos de los picos y ruidos
eléctricos generados por los motores durante el uso de los mismos.
Los drivers de motor por lo general suministran los motores con una
potencia no regulados directamente desde el baterías, en lugar de po-
tencia regulada desde el circuito de fuente de alimentación.
Control
Cuando nos referimos al control de un robot móvil, nos referimos al siste-
ma electrónico encargado de tomar las decisiones de movimiento o actuación
del robot móvil. Este sistema se encargará de recopilar datos de los sensores
de los que disponga el robot, procesar esa información y por último tomar
la decisión de qué hacer.
El sistema de control varía en complejidad de forma paralela a la comple-
jidad de las tareas a las que esté destinado el robot. Por ejemplo, un sistema
de control de un robot móvil que debe seguir una línea negra sobre un fondo
blanco, puede ser dos transistores cuya base esté conectada a unos sensores
de infrarrojos que darán una señal en función de si esta en fondo blanco o
negro, y el colector y emisor de los transistores se conectan a la batería y
Micro-Robot Trazador de Trayectorias 36
Sensores de distancia
Este tipo de sensores son aquellos que se usan para la detección de una
pista marcada en el suelo mediante el uso de un color (habitualmente negro)
sobre un fondo de color distinto (habitualmente blanco) con índices de re-
flexión de luz diferentes. Así cuando está sobre la pista tendremos un valor
diferente del sensor que cuando está sobre el fondo.
Los más usados y extendidos son los de luz infrarroja. Su funcionamien-
to es el mismo que los sensores de presencia infrarrojos. Pero en este caso
la distancia es fija, habitualmente anclados en el chasis de robot móvil. La
diferencia se produce debido al diferente índice de refracción de la superficie
donde rebote el haz de luz infrarroja (el blanco refleja toda la luz y el negro
absorbe toda la luz). Mediante la imposición de un umbral que se calcula
experimentalmente para cada modelo de sensor, es capaz de detectar la pre-
sencia de pista o fondo. Son sensores muy baratos y sencillos de utilizar, pero
se ven afectados por numerosos elementos ambientales: la luz solar, manchas
en la pista o fondo, etc. Estos elementos pueden hacer que se obtengan me-
didas falsas. En la figura 3.6 se puede observar cómo funciona la detección
de la superficie.
Micro-Robot Trazador de Trayectorias 40
Este conjunto de sensores abarca todos los tipos de sensores que dan
información de cualquier tipo de condición meteorológica o de cambios de
luz o de cualquier otra variable que pueda afectar al funcionamiento del robot
móvil.
Al igual que cualquier otro sensor, existen infinidad de tipos de sensores
en función de que se quiera medir y del principio físico o tecnología que se
usen para medirlos.
A continuación de hace un listado de sensores y algunas tecnologías.
Por supuesto estas son características genéricas y comunes a todos los robots
móviles, y en función de la aplicación a la que se destine, habrá que tener en
cuenta más o menos aspectos.
Micro-Robot Trazador de Trayectorias 44
Capítulo 4
Desarrollo de elementos de un
robot móvil trazador
45
Micro-Robot Trazador de Trayectorias 46
Las dimensiones del robot han de ser lo más reducidas posibles, para
que cuando el robot se mueva, desplace el menor peso posible y por
tanto que su consumo sea lo más reducido posible.
Figura 4.2: Robot Pololu 3pi (izquierda) y robot Mark III (derecha)
tras ser estudiadas y analizadas, pueden formar parte formar parte en el futu-
ro desarrollo de un μTrace. Para esta descripción se agruparan los diferentes
elementos seleccionados según su pertenencia a los siguientes subsistemas:
mecánica, alimentación, control de motores, almacenamiento, comunicación,
sensorización y sistema de control.
Mecánica
Alimentación
Control de motores
Almacenamiento
Comunicación
Sensorización
El μTrace recogerá dos tipos de datos: por una lado los datos relacionados
con la navegación o seguimiento de líneas y por otro los datos inerciales
necesarios para el posicionamiento y el trazado de las trayectorias seguidas.
• Giroscopio de 3 ejes.
• Acelerómetro de 3 ejes.
• Sensibilidad de giroscopio ajustable con una escala de: ±250,
±500, ±1000, ±2000 [ grados
s ].
Micro-Robot Trazador de Trayectorias 54
Sistema de Control
Manejo de LCD integrado para poder crear una interfaz con el usuario.
Salida PWM.
Test de funcionamiento del USB. Se trata del código que se utilizó para
hacer los test de la comunicación entre el prototipo de pruebas con la
computadora. El firmware está diseñado para habilitar la conectividad
del microcontrolador con el ordenador mediante un protocolo de puer-
to serie virtual, lo que hace que el PC interprete dicha comunicación
como si de un puerto serie se tratase y así establecer una comunica-
ción mediante consola, en la que el usuario puede enviar comandos y
peticiones al sistema y este responderle en función de lo que se le haya
enviado.
Reprogramable SI SI SI
Almacenamiento NO NO SI
muestreo con lo que obtenemos los ángulos girados por el μTrace en cada
periodo de muestreo por cada uno de los ejes del sistema. Estos ángulos son
directamente los ángulos de Euler. Una vez obtenidos estos ángulos se proce-
de al cálculo de la matriz de rotación R. Una vez obtenida se multiplica por
la aceleración obtenida del acelerómetro y con ello se consigue la aceleración
del sistema referida a los tres ejes cartesianos fijos del punto de origen; esta
operación se repite para todos y cada uno de los datos obtenidos.
Una vez tenemos todas las aceleraciones referidas al sistema fijo de re-
ferencia, convertimos el valor de G a sm2 multiplicando el valor obtenido de
girar la aceleración por el valor de la gravedad, 9.81 sm2 , y compensamos la
aceleración de la gravedad restando el valor de 9.81 a la gravedad medida
en el eje Z. Tras esto, ya podemos integrar estos datos con el valor de la
frecuencia de muestreo para obtener la velocidad, teniendo en cuenta que
la velocidad inicial es nula. Hay que tener en cuenta que en los periodos
estacionarios calculados anteriormente, se considera la velocidad nula, por lo
que tenemos en cuenta esta situación. También intentamos corregir la posible
deriva de velocidad teniendo en cuenta que, tras los periodos estacionarios
donde la velocidad es nula, la velocidad en los siguientes periodos no puede
ser demasiado elevada. Se calcula una tasa de reducción de la velocidad con
lo que corregimos la velocidad. Por último, se vuelve a integrar la velocidad
con lo que tenemos el incremento de la posición en cada uno de los tres ejes
del sistema fijo. También aquí hay que tener en cuenta que la posición inicial
es cero. Los datos de la posición se pasan a una gráfica en tres dimensiones
con lo que obtenemos la trayectoria seguida por el μTrace.
De este algoritmo de trazado no se espera que sea preciso. Como se ha
explicado anteriormente, los giroscopios introducen un gran error y como se
puede observar, en este algoritmo no se corrige de ninguna manera, por lo
que al integrar los datos con error, este error es acumulativo y es de esperar
que la gráfica trazada sea muy imprecisa.
Este algoritmo se basa en los mismos principios que los explicados para
el algoritmo de cálculo directo de los ángulos de Euler, pero en este caso,
se aplica un filtro de Kalman que reduce y compensa el error en dos de los
ángulos del sistema. Este algoritmo está basado en [9].
En primer lugar vamos a definir las ecuaciones y valores de las constantes
y valores iniciales de este algoritmo.
Nos interesa calcular el ángulo girado por cada eje y el error que se comete
cuando se calcula dicho ángulo. Si tenemos en cuenta esto, podemos definir
el ángulo y el error como:
Micro-Robot Trazador de Trayectorias 64
yt = Hxt + vt (4.5)
Si adaptamos las ecuaciones de estado, al modelo de Kalman, obtenemos
los siguientes parámetros:
ángulo
xt =
error
R = 0,3
0,004 0
Q=
0 0,05
1 −dt
A=
0 1
dt
B=
0
H= 1 0
Tras el análisis de cada una de las variables que intervienen en el proceso del
filtro de Kalman, se pasa a la metodología que sigue el algoritmo.
Se inicia de igual manera que el algoritmo anterior. Se extraen los datos de
la memoria micro SD y se tratan hasta obtener los datos en la forma deseada.
También aplicamos los mismos filtros para detectar periodos estacionarios.
Una vez tenemos los datos comienza en cálculo de los ángulos. Este método
sólo se aplica a los ángulos Roll y Pitch, por lo que el cálculo del ángulo Yaw
se calcula exactamente igual que en el método anterior.
En primer lugar se calculan los ángulos mediante el método del giroscopio,
es decir integrando el valor de los datos obtenidos de giroscopio con el valor
de la frecuencia de muestreo. A continuación se calcula el ángulo a partir del
método de la aceleración, es decir, se aplican las ecuaciones 2.35 y 2.36 para
el Pitch y el Roll respectivamente.
Posteriormente, se actualiza la matriz de covarianza del sistema y se pro-
cede al calculo del error entre el ángulo calculado por el método del acele-
rómetro y el giroscopio. Se calcula la ganancia de Kalman K y volvemos a
actualizar la matriz de covarianza. Con la ganancia y el error calculamos el
ángulo a partir del ángulo calculado con los giroscopios y se calcula el sesgo
Micro-Robot Trazador de Trayectorias 66
que se usará para las siguientes estimaciones. Tras esto, se rota la aceleración
para referenciarlos al sistema de referencia fijo y se sigue el mismo proceso
de cálculo de la velocidad y de la posición que en el algoritmo anterior.
Con este sistema compensamos los errores en los ángulos Pitch y Roll
aunque persiste el error en al ángulo Yaw. De esta manera se espera una
mejora con respecto al anterior, si bien es cierto, que los cambios no serán
significativos.
bas de firmware.
A continuación, en las imágenes 4.3 y 4.4, se puede observar el resultado
de la creación del prototipo. En la imagen 4.3 se aprecia la vista superior
de μTrace donde se ubican los motores con sus correspondientes soportes, el
socket para la tarjeta SD, el conector USB, la IMU MPU6050, los botones y
leds y las baterías. En la imagen 4.4 se muestra la vista inferior, lugar donde
se ubican el microcontrolador, el regulador de tensión el puente en H para el
control de los motores, los sensores de infrarrojos y el apoyo mecánico para
que la plataforma no pivote sobre las ruedas.
Prueba de Navegación
El objetivo de estas pruebas es la de ver si esta configuración es factible
para utilizar como robot móvil y por otro lado, comparar el funcionamiento
de los algoritmos de navegación. Para dichas pruebas, se ha decidido que se
compare con otro robot comercial, en este caso, con el Pololu 3pi.
Para realizar las pruebas, se ha programado el microcontrolador del μTrace
con el firmware diseñado específicamente para ello. Previo paso a hacer las
pruebas comparativas se han hecho diferentes pruebas con el prototipo de
pruebas con el fin de sintonizar los parámetros del PID que lleva implemen-
tado. Tras hacer este ajuste, se ha creado un pequeño circuito o pista de
pruebas que servirá para comprobar el correcto funcionamiento del sistema.
Este circuito es un circuito rectangular cuyas esquinas han sido redondea-
das con diferentes radios de curvatura para comprobar cómo se comporta el
sistema ante diferentes radios de giro.
Tras hacer las pruebas con ambos robots, se ha concluido que el μTrace
pasa satisfactoriamente este test. La velocidad y tiempo que necesita para
superar el circuito el μTrace es similar al que necesita el sistema comercial
Pololu 3pi. Por ello se puede decir que es factible utilizar este sistema de
navegación para la creación de un robot móvil. También queda patente, que
la elección de motores, el driver de los mismos y los sensores infrarrojos
pueden ser usados para dicha creación.
Con estos datos, se ha procedido a hacer una primera prueba de los tres
algoritmos. En un primer intento, dado que los parámetros de los scripts
están sin ajustar, los resultados son poco realistas, pero tras una serie de
pruebas, y una puesta a punto, los algoritmos proporcionan una reconstruc-
ción bastante buena de los trazados seguidos.
almacena en ella los datos y una vez finalizado en proceso de toma de estos
datos, se vuelcan de la memoria volátil a la tarjeta SD se eliminan todos
procesos que ralentizan e impiden un correcto funcionamiento del sistema.
Para terminar con el análisis de las pruebas, se hace una serie de reco-
mendaciones que pueden utilizarse para la construcción de un robot seguidor
de líneas con capacidad para reconstruir trayectorias:
Dado que durante las pruebas, el mayor cuello de botella del sistema es
el almacenamiento de datos en una memoria extraíble SD, es necesario
buscar alternativas para el almacenamiento de datos a alta velocidad
de forma transitoria para luego, cuando no sea necesario el uso del
procesador, pasar estos datos a la memoria externa.
Conclusiones y líneas de
trabajo futuras
81
Micro-Robot Trazador de Trayectorias 82
Durante esta tesis de máster se han completado todos los objetivos principa-
les. Pero dado que se trata de un estudio de la problemática que se presenta
a la hora de desarrollar un sistema móvil con capacidad para reconstruir
trayectorias y las posibles soluciones que pueden presentarse, existen nume-
rosas líneas de aplicación futura. La primera y más inmediata es la aplicación
de estos conceptos para la construcción de un robot seguidor de líneas con
capacidad de tomar datos para una posterior reconstrucción de trayectorias.
Pero no es la única. A continuación se hace un listado de las posibles líneas
de desarrollo futuras.
[2] Xiaoying Kong. Inertial Navigation System Algorithms for Low Cost
IMU. Department of Mechanical and Mechatronic Engineering, Univer-
sity of Sydney. August 27, 2000.
[4] Joona Pusa. Strapdown inertial navigation system aiding with nonholo-
nomic constraints using indirect Kalman filtering. Tampere University
of Technology. April 8, 2009.
[5] Eduard Angelats, M. Eulàlia Parés and Ismael Colomina. Methods, Al-
gorithms and Tools for Precise Terrestril Navigation. Institute of Geo-
matics.
85
Micro-Robot Trazador de Trayectorias 86
[15] Fei-Yue Wang, Yanqing Gao. Advanced Studies of Flexible Robotic Ma-
nipulators - Modeling, Design, Control, and Applications. Singapore,
World Scientific Publishing Co. Pte. Ltd., 2003.
[17] Newton C. Braga. Meachatronics for the Evil Genius. MacGraw Hill.
2005
87
Micro-Robot Trazador de Trayectorias 88
Micro-Robot Trazador de Trayectorias 89
Micro-Robot Trazador de Trayectorias 90
ANEXO B Firmware de
μTrace: diagramas de flujo y
código en C
91
Micro-Robot Trazador de Trayectorias 92
Diagramas de Flujo
Micro-Robot Trazador de Trayectorias 93
Micro-Robot Trazador de Trayectorias 94
Micro-Robot Trazador de Trayectorias 95
Micro-Robot Trazador de Trayectorias 96
Micro-Robot Trazador de Trayectorias 97
Micro-Robot Trazador de Trayectorias 98
Micro-Robot Trazador de Trayectorias 99
Codigo en C
#i n c l u d e <18F67J94 . h>
#d e v i c e PASS_STRINGS = IN_RAM
#d e v i c e ADC=12
// L i b r e r i a con l a s d e f i n i c i o n e s de V a r i a b l e s y e t i q u e t a s
#i n c l u d e "/ l i b s /GLOBAL/ global . h"
// D e f i n i c i o n de l o s p u e r t o s de s a l i d a
#u s e FIXED_IO( E_outputs=rueda_derecha_1 )
#u s e FIXED_IO( G_outputs=PWM_DERECHA,PWM_IZQUIERDA, rueda_izquierda_2 ,
rueda_izquierda_1 , rueda_derecha_2 )
// D e f i n i c i o n de l o s p i n e s de S a l i d a d e l PWM
#PIN_SELECT P4A=PWM_DERECHA
#PIN_SELECT P5A=PWM_IZQUIERDA
// D e f i n i c i o n de l o s r e t a r d o s
#u s e d e l a y ( clock =64000000 , c r y s t a l =12000000 ,USB_FULL)
// D e f i n i c i o n de l o s p i n e s d e l p u e r t o SPI
#PIN_SELECT SDI2=MMCSD_PIN_SDI
#PIN_SELECT SDO2=MMCSD_PIN_SDO
#PIN_SELECT SCK2=MMCSD_PIN_SCL
#u s e s p i (MASTER, DI=MMCSD_PIN_SDI, DO=MMCSD_PIN_SDO, CLK=MMCSD_PIN_SCL
, BITS=8 , MSB_FIRST, MODE=0 , baud =400000 , stream=mmcsd_spi )
#d e f i n e MMCSD_SPI_XFER( x ) s p i _ x f e r ( mmcsd_spi , x )
// D e f i n i c i o n de l a l i b r e r i a USB
#i n c l u d e "/ l i b s /USB/mod_usb_cdc . h"
Micro-Robot Trazador de Trayectorias 100
// D e f i n i c i o n de l a l i b r e r i a d e l A c e l e r o m e t r o / G i r o s c o p i o
#i n c l u d e "/ l i b s /MPU6050/mpu6050 . c "
// D e f i n i c i o n de l a l i b r e r i a SD
#i n c l u d e "/ l i b s /SD/ sd . c "
#i n c l u d e "/ l i b s /USB/ rut_usb . c "
// D e f i n i c i o n de l a l i b r e r i a d e l Navegador
#i n c l u d e "/ l i b s /NAVEGADOR/ navegador . c "
#INT_TIMER1
v o i d TIMER1_isr ( v o i d ) {
c l e a r _ i n t e r r u p t (INT_TIMER1) ; // Borramos Fl a g
set_timer1 (0) ; // Volvemos a c a r g a r T e m p o r i z a c i ó n
c o n t a d o r ++;
Get_Accel_Values ( ) ; //Tomamos d a t o s d e l a c e l e r a r o m e t r o
Get_Gyro_Values ( ) ; //Tomamos d a t o s d e l g i r o s c o p i o
Appendcsv ( c o n t a d o r ) ; // Pasamos d a t o s a SD
}
#INT_EXT
v o i d EXT_isr ( v o i d ) {
d i s a b l e _ i n t e r r u p t s (INT_EXT) ; // D e s h a b i l i t a m o s i n t e r r u p c i o n e s
c l e a r _ i n t e r r u p t (INT_EXT) ; // Borramos Fl a g
fin_nav = 1 ;
delay_ms ( 2 0 0 ) ; // Retardo para r e b o t e s
}
v o i d parpadeo1 ( ) {
output_high (LED) ;
delay_ms ( 2 0 0 ) ;
output_low (LED) ;
delay_ms ( 2 0 0 ) ;
}
v o i d parpadeo2 ( ) {
output_high (LED) ;
delay_ms ( 1 0 0 0 ) ;
output_low (LED) ;
delay_ms ( 1 0 0 0 ) ;
}
void i n i c i o ( ) {
// I n i c i a l i z a c i ó n de v a r i a b l e s con v a l o r e s p r e d e f i n i d o s
KP = def_kp ∗ 0 . 1 ;
KD = def_kd ∗ 0 . 0 1 ;
KI = d e f _ k i ∗ 0 . 0 1 ;
GYRO_XOUT_OFFSET = OFF_ACX_DEF;
GYRO_YOUT_OFFSET = OFF_ACX_DEF;
GYRO_ZOUT_OFFSET = OFF_ACX_DEF;
ACCEL_XOUT_OFFSET = OFF_ACX_DEF;
ACCEL_YOUT_OFFSET = OFF_ACX_DEF;
ACCEL_ZOUT_OFFSET = OFF_ACX_DEF;
umbralA=umbralA_def ;
umbralB=umbralB_def ;
umbralC=umbralC_def ;
umbralD=umbralD_def ;
umbralE=umbralE_def ;
// C o n f i g u r a c i o n e l Timer1
setup_timer_1 (T1_INTERNAL | T1_DIV_BY_1) ; //F Timer 1 => Fosc /4 =
64000000 / 4 = 16000000
Micro-Robot Trazador de Trayectorias 101
// C o n f i g u r a c i o n ADC
setup_adc_ports ( sAN0 | sAN1 | sAN2 | sAN3 | sAN6 ) ;
setup_adc (ADC_CLOCK_DIV_64|ADC_TAD_MUL_16) ;
// C o n f i g u r a c i o n l a s s a l i d a s PWM
setup_timer_2 (T2_DIV_BY_16, 2 5 5 , 1 ) ;
setup_ccp4 (CCP_PWM) ;
setup_ccp5 (CCP_PWM) ;
set_pwm4_duty ( 0 ) ;
set_pwm5_duty ( 0 ) ;
// C o n f i g u r a c i o n de l a s i n t e r r u p c i o n e s
e n a b l e _ i n t e r r u p t s (GLOBAL) ;
ext_int_edge ( H_TO_L ) ;
//MPU6050 s e t u p
error = 0 ;
i = 0;
do {
aux_mpu = init_MPU6050 ( ) ;
i f (aux_mpu != 0 ) {
i f (aux_mpu == 1 ) { e r r o r _ 2 = 1 ; } e l s e { e r r o r _ 3 = 1 ; }
error_mpu = 1 ;
i ++;
i f ( i > 5) {
error_mpu = 0 ;
error = 1 ;
}
} else {
error_mpu = 0 ;
error = 0 ;
}
} while ( error_mpu != 0 ) ;
i =0;
i f ( error == 1 ) {
while (TRUE) {
parpadeo2 ( ) ;
}
}
i f ( f a t _ i n i t ( ) != GOODEC) {
error =1;
} else {
error =0;
}
i f ( error == 1 ) {
while (TRUE) {
parpadeo2 ( ) ;
}
}
i =0;
mode = 1 ;
}
v o i d track_navega ( ) {
MakeFile ( 1 ) ;
delay_ms ( 1 0 0 ) ;
MakeFile ( 2 ) ;
delay_ms ( 1 0 0 ) ;
Micro-Robot Trazador de Trayectorias 102
Appendlog ( ) ;
delay_ms ( 1 0 0 ) ;
e n a b l e _ i n t e r r u p t s (INT_EXT) ;
e n a b l e _ i n t e r r u p t s (INT_TIMER1) ;
set_timer1 (0) ;
contador = 0 ;
fin_track = 0;
do {
navegacion ( ) ;
i f ( ! input (START) ) {
fin_nav = 1 ;
}
} while ( fin_nav == 0 ) ;
d i s a b l e _ i n t e r r u p t s (INT_EXT) ;
d i s a b l e _ i n t e r r u p t s (INT_TIMER1) ;
}
void track ( ) {
contador = 0 ;
MakeFile ( 1 ) ;
delay_ms ( 1 0 0 ) ;
MakeFile ( 2 ) ;
delay_ms ( 1 0 0 ) ;
Appendlog ( ) ;
delay_ms ( 1 0 0 ) ;
e n a b l e _ i n t e r r u p t s (INT_TIMER1) ;
output_low (LED) ;
set_timer1 (0) ;
fin_nav = 0 ;
while ( fin_nav == 0 ) {
i f ( ! input (START) ) {
fin_nav = 1 ;
}
};
fin_nav = 0 ;
d i s a b l e _ i n t e r r u p t s (INT_TIMER1) ;
}
v o i d navega ( ) {
e n a b l e _ i n t e r r u p t s (INT_EXT) ;
fin_nav = 0 ;
while ( fin_nav==0){
navegacion ( ) ;
i f ( ! input (START) ) {
fin_nav = 1 ;
}
}
fin_nav = 0 ;
set_pwm4_duty ( 0 ) ;
set_pwm5_duty ( 0 ) ;
d i s a b l e _ i n t e r r u p t s (INT_EXT) ;
delay_ms ( 2 0 0 0 ) ;
}
void a j u s t e ( ) {
Calibrate_Gyros ( ) ;
Calibrate_Accel () ;
calibrado_ir () ;
}
v o i d main ( ) {
Micro-Robot Trazador de Trayectorias 103
output_high (LED) ;
usb_inicio () ;
inicio () ;
while (TRUE) {
output_low (LED) ;
i f ( ! input (SELECT) ) {
fin_usb = 0 ;
output_high (LED) ;
do {
i f ( usb_cdc_connected ( ) ) {
rut_usb ( ) ;
}
} while ( f i n _ u s b==0) ;
f i n _ u s b =0;
}
i f ( ! input (START) ) {
output_high (LED) ;
s w i t c h ( mode ) {
case 0:
track_navega ( ) ;
delay_ms ( 1 0 0 0 ) ;
break ;
case 1:
track () ;
delay_ms ( 1 0 0 0 ) ;
break ;
case 2:
delay_ms ( 2 0 0 0 ) ;
navega ( ) ;
break ;
case 3:
ajuste () ;
delay_ms ( 1 0 0 0 ) ;
break ;
default :
break ;
}
}
}
}
Micro-Robot Trazador de Trayectorias 104
ANEXO C Algoritmos de
trazado: diagramas de flujo y
código en Matlab
105
Micro-Robot Trazador de Trayectorias 106
Diagramas de Flujo
Micro-Robot Trazador de Trayectorias 107
Micro-Robot Trazador de Trayectorias 108
Micro-Robot Trazador de Trayectorias 109
Micro-Robot Trazador de Trayectorias 110
close a l l
clear a l l
p a t h d a t a = ’E : \ P r o y e c t o s ␣ Matlab \ s p i r a l S t a i r s . c s v ’ ;
dato_raw = csvread ( pathdata , 1 , 0 ) ;
g = 9.81;
periodo = 1/256;
%D e t e c c i ó n de p e r d i o d o s e s t a c i o n a r i o s
M a g n i t u d A c e l e r a c i o n = sqrt ( a c e l e r a c i o n _ x . ∗ a c e l e r a c i o n _ x +
aceleracion_y .∗ aceleracion_y + aceleracion_z .∗ aceleracion_z ) ;
CorteFiltro = 0.001;
[ b , a ] = butter (1 , (2∗ C o r t e F i l t r o ) /(1/ periodo ) , ’ high ’ ) ;
MagnitudAceleracionFiltada = f i l t f i l t (b , a , MagnitudAceleracion ) ;
M a g n i t u d A c e l e r a c i o n F i l t a d a = abs ( M a g n i t u d A c e l e r a c i o n F i l t a d a ) ;
CorteFiltro = 5;
[ b , a ] = b u t t e r ( 1 , ( 2 ∗ C o r t e F i l t r o ) / ( 1 / p e r i o d o ) , ’ low ’ ) ;
MagnitudAceleracionFiltada = f i l t f i l t (b , a , MagnitudAceleracionFiltada
);
PeriodoEstacionario = MagnitudAceleracionFiltada < 0 . 0 5 ;
R_euler = zeros ( 3 , 3 ) ;
angulo_x = zeros ( s i z e ( g i r o s c o p i o _ x ) ) ;
angulo_y = zeros ( s i z e ( g i r o s c o p i o _ y ) ) ;
angulo_z = zeros ( s i z e ( g i r o s c o p i o _ z ) ) ;
f o r i = 2 : length ( g i r o s c o p i o _ x )
angulo_x ( i ) = angulo_x ( i −1) + ( d e g 2 r a d ( g i r o s c o p i o _ x ( i ) ) ∗ p e r i o d o ) ;
end
f o r i = 2 : length ( g i r o s c o p i o _ y )
angulo_y ( i ) = angulo_y ( i −1) + ( d e g 2 r a d ( g i r o s c o p i o _ y ( i ) ) ∗ p e r i o d o ) ;
end
f o r i = 2 : length ( g i r o s c o p i o _ z )
angulo_z ( i ) = angulo_z ( i −1) + ( d e g 2 r a d ( g i r o s c o p i o _ z ( i ) ) ∗ p e r i o d o ) ;
end
f o r i = 1 : length ( g i r o s c o p i o _ z )
R_euler ( 1 , 1 ) = cos ( angulo_y ( i ) ) ∗ cos ( angulo_z ( i ) ) ;
R_euler ( 1 , 2 ) = ( cos ( angulo_z ( i ) ) ∗ s i n ( angulo_x ( i ) ) ∗ s i n ( angulo_y ( i ) )
) −(cos ( angulo_x ( i ) ) ∗ s i n ( angulo_z ( i ) ) ) ;
R_euler ( 1 , 3 ) = ( cos ( angulo_x ( i ) ) ∗ cos ( angulo_z ( i ) ) ∗ s i n ( angulo_y ( i ) )
) +( s i n ( angulo_x ( i ) ) ∗ s i n ( angulo_z ( i ) ) ) ;
R_euler ( 2 , 1 ) = cos ( angulo_y ( i ) ) ∗ s i n ( angulo_z ( i ) ) ;
R_euler ( 2 , 2 ) = ( cos ( angulo_z ( i ) ) ∗ cos ( angulo_z ( i ) ) ) −( s i n ( angulo_x ( i
Micro-Robot Trazador de Trayectorias 111
) ) ∗ s i n ( angulo_y ( i ) ) ∗ s i n ( angulo_z ( i ) ) ) ;
R_euler ( 2 , 3 ) = −(cos ( angulo_z ( i ) ) ∗ s i n ( angulo_x ( i ) ) ) +(cos ( angulo_x (
i ) ) ∗ s i n ( angulo_y ( i ) ) ∗ s i n ( angulo_z ( i ) ) ) ;
R_euler ( 3 , 1 ) = −s i n ( angulo_y ( i ) ) ;
R_euler ( 3 , 2 ) = cos ( angulo_y ( i ) ) ∗ s i n ( angulo_x ( i ) ) ;
R_euler ( 3 , 3 ) = cos ( angulo_x ( i ) ) ∗ cos ( angulo_y ( i ) ) ;
a c e l e r a c i o n ( i , 1 ) = ( R_euler ( 1 , 1 ) ∗ a c e l e r a c i o n _ x ( i ) ) +(R_euler ( 1 , 2 ) ∗
a c e l e r a c i o n _ y ( i ) ) +(R_euler ( 1 , 3 ) ∗ a c e l e r a c i o n _ z ( i ) ) ;
a c e l e r a c i o n ( i , 2 ) = ( R_euler ( 2 , 1 ) ∗ a c e l e r a c i o n _ x ( i ) ) +(R_euler ( 2 , 2 ) ∗
a c e l e r a c i o n _ y ( i ) ) +(R_euler ( 2 , 3 ) ∗ a c e l e r a c i o n _ z ( i ) ) ;
a c e l e r a c i o n ( i , 3 ) = ( R_euler ( 3 , 1 ) ∗ a c e l e r a c i o n _ x ( i ) ) +(R_euler ( 3 , 2 ) ∗
a c e l e r a c i o n _ y ( i ) ) +(R_euler ( 3 , 3 ) ∗ a c e l e r a c i o n _ z ( i ) ) ;
end
aceleracion = aceleracion ∗ g ;
aceleracion (: ,3) = aceleracion (: ,3) − g ;
v e l o c i d a d = zeros ( s i z e ( a c e l e r a c i o n ) ) ;
f o r i = 2 : length ( v e l o c i d a d )
velocidad ( i , : ) = velocidad ( i −1 ,:) + ( a c e l e r a c i o n ( i , : ) ∗ periodo ) ;
i f ( P e r i o d o E s t a c i o n a r i o ( i ) == 1 )
velocidad ( i , : ) = [0 0 0 ] ;
end
end
D e r i v a V e l o c i d a d = zeros ( s i z e ( v e l o c i d a d ) ) ;
I n i c i o E s t a c i o n a r i o = find ( [ 0 ; d i f f ( P e r i o d o E s t a c i o n a r i o ) ] == −1) ;
F i n E s t a c i o n a r i o = find ( [ 0 ; d i f f ( P e r i o d o E s t a c i o n a r i o ) ] == 1 ) ;
f o r i = 1 : numel ( F i n E s t a c i o n a r i o )
TasaDeriva = v e l o c i d a d ( F i n E s t a c i o n a r i o ( i ) −1, : ) / ( F i n E s t a c i o n a r i o
( i ) − InicioEstacionario ( i ) ) ;
enum = 1 : ( F i n E s t a c i o n a r i o ( i ) − I n i c i o E s t a c i o n a r i o ( i ) ) ;
D e r i v a = [ enum ’ ∗ TasaDeriva ( 1 ) enum ’ ∗ TasaDeriva ( 2 ) enum ’ ∗ TasaDeriva
(3) ] ;
D e r i v a V e l o c i d a d ( I n i c i o E s t a c i o n a r i o ( i ) : F i n E s t a c i o n a r i o ( i ) −1, : ) =
Deriva ;
end
p o s i c i o n = zeros ( s i z e ( v e l o c i d a d ) ) ;
f o r i = 2 : length ( p o s i c i o n )
posicion ( i , : ) = posicion ( i −1 ,:) + ( velocidad ( i , : ) ∗ periodo ) ;
end
close a l l ;
figure (1) ;
plot3 ( p o s i c i o n ( : , 1 ) , p o s i c i o n ( : , 2 ) , p o s i c i o n ( : , 3 ) ) ;
figure (2) ;
plot ( p o s i c i o n ( : , 1 ) , p o s i c i o n ( : , 2 ) ) ;
Micro-Robot Trazador de Trayectorias 112
close a l l
clear a l l
p a t h d a t a = ’E : \ P r o y e c t o s ␣ Matlab \ s p i r a l S t a i r s . c s v ’ ;
dato_raw = csvread ( pathdata , 1 , 0 ) ;
g = 9.81;
periodo = 1/256;
%D e t e c c i ó n de p e r d i o d o e s t a c i o n a r i o s
M a g n i t u d A c e l e r a c i o n = sqrt ( a c e l e r a c i o n _ x . ∗ a c e l e r a c i o n _ x +
aceleracion_y .∗ aceleracion_y + aceleracion_z .∗ aceleracion_z ) ;
CorteFiltro = 0.001;
[ b , a ] = butter (1 , (2∗ C o r t e F i l t r o ) /(1/ periodo ) , ’ high ’ ) ;
MagnitudAceleracionFiltada = f i l t f i l t (b , a , MagnitudAceleracion ) ;
M a g n i t u d A c e l e r a c i o n F i l t a d a = abs ( M a g n i t u d A c e l e r a c i o n F i l t a d a ) ;
CorteFiltro = 5;
[ b , a ] = b u t t e r ( 1 , ( 2 ∗ C o r t e F i l t r o ) / ( 1 / p e r i o d o ) , ’ low ’ ) ;
MagnitudAceleracionFiltada = f i l t f i l t (b , a , MagnitudAceleracionFiltada
);
PeriodoEstacionario = MagnitudAceleracionFiltada < 0 . 0 5 ;
aceleracion_x = aceleracion_x ∗g ;
aceleracion_y = aceleracion_x ∗g ;
aceleracion_z = aceleracion_x ∗g ;
P_x = [ 1 , 0 ; 0 , 1 ] ; %M a t r i z de c o v a r i a n z a
P_y = [ 1 , 0 ; 0 , 1 ] ; %M a t r i z de c o v a r i a n z a
R_angle = 0 . 3 ;
Q_angle = 0 . 0 0 1 ;
Q_gyro = 0 . 0 0 3 ;
%V a l o r e s i n i c i a l e s
w_bias_giro_x = 0 ;
w_bias_giro_y = 0 ;
angulo_x_k = zeros ( s i z e ( g i r o s c o p i o _ x ) ) ;
angulo_y_k = zeros ( s i z e ( g i r o s c o p i o _ y ) ) ;
angulo_z_k = zeros ( s i z e ( g i r o s c o p i o _ z ) ) ;
angulo_x_a = zeros ( s i z e ( a c e l e r a c i o n _ x ) ) ;
angulo_y_a = zeros ( s i z e ( a c e l e r a c i o n _ y ) ) ;
angulo_x_w = zeros ( s i z e ( g i r o s c o p i o _ x ) ) ;
angulo_y_w = zeros ( s i z e ( g i r o s c o p i o _ y ) ) ;
angulo_z_w = zeros ( s i z e ( g i r o s c o p i o _ z ) ) ;
R_euler=zeros ( 3 , 3 ) ;
f o r i =2: length ( g i r o s c o p i o _ x ) ,
%Sin f i l t r o de Kalman
angulo_z_w ( i )= angulo_z_k ( i −1)+( g i r o s c o p i o _ z ( i ) ∗ p e r i o d o ) ;
periodo ;
angulo_y_k ( i )=angulo_y_k ( i −1)+( g i r o s c o p i o _ y ( i )−w_bias_giro_y ) ∗
periodo ;
%A c t u a l i z a m o s m a t r i z de c o v a r i a n z a
P_x( 1 , 1 ) = P_x( 1 , 1 ) + Pdot_x ( 1 ) ∗ p e r i o d o ;
P_x( 1 , 2 ) = P_x( 1 , 2 ) + Pdot_x ( 2 ) ∗ p e r i o d o ;
P_x( 2 , 1 ) = P_x( 2 , 1 ) + Pdot_x ( 3 ) ∗ p e r i o d o ;
P_x( 2 , 2 ) = P_x( 2 , 2 ) + Pdot_x ( 4 ) ∗ p e r i o d o ;
P_y( 1 , 1 ) = P_y( 1 , 1 ) + Pdot_y ( 1 ) ∗ p e r i o d o ;
P_y( 1 , 2 ) = P_y( 1 , 2 ) + Pdot_y ( 2 ) ∗ p e r i o d o ;
P_y( 2 , 1 ) = P_y( 2 , 1 ) + Pdot_y ( 3 ) ∗ p e r i o d o ;
P_y( 2 , 2 ) = P_y( 2 , 2 ) + Pdot_y ( 4 ) ∗ p e r i o d o ;
%Angulo e x t r a i d o de l o s a c e l e r o m e t r o s
i f ( sqrt ( ( a c e l e r a c i o n _ y ( i ) ) ^2 + ( a c e l e r a c i o n _ z ( i ) ) ^2 ) ) == 0
angulo_x_a ( i ) = pi / 2 ;
else
angulo_x_a ( i ) = −(atan ( a c e l e r a c i o n _ x ( i ) / sqrt ( ( a c e l e r a c i o n _ y (
i ) ) ^2 + ( a c e l e r a c i o n _ z ( i ) ) ^2 ) ) ) ;
end
i f ( sqrt ( ( a c e l e r a c i o n _ x ( i ) ) ^2 + ( a c e l e r a c i o n _ z ( i ) ) ^2 ) ) == 0
angulo_y_a ( i ) = pi / 2 ;
else
angulo_y_a ( i ) = −(atan ( a c e l e r a c i o n _ y ( i ) / sqrt ( ( a c e l e r a c i o n _ x (
i ) ) ^2 + ( a c e l e r a c i o n _ z ( i ) ) ^2 ) ) ) ;
end
error_angulo_x=angulo_x_a ( i )−angulo_x_k ( i ) ;
error_angulo_y=angulo_y_a ( i )−angulo_y_k ( i ) ;
K0_x=P_x( 1 , 1 ) / ( R_angle+P_x( 1 , 1 ) ) ;
K1_x=P_x( 2 , 1 ) / ( R_angle+P_x( 1 , 1 ) ) ;
K0_y=P_y( 1 , 1 ) / ( R_angle+P_y( 1 , 1 ) ) ;
K1_y=P_y( 2 , 1 ) / ( R_angle+P_y( 1 , 1 ) ) ;
v e l o c i d a d = zeros ( s i z e ( a c e l e r a c i o n ) ) ;
f o r i = 2 : length ( v e l o c i d a d )
velocidad ( i , : ) = velocidad ( i −1 ,:) + ( a c e l e r a c i o n ( i , : ) ∗ periodo ) ;
i f ( P e r i o d o E s t a c i o n a r i o ( i ) == 1 )
velocidad ( i , : ) = [0 0 0 ] ;
end
end
D e r i v a V e l o c i d a d = zeros ( s i z e ( v e l o c i d a d ) ) ;
I n i c i o E s t a c i o n a r i o = find ( [ 0 ; d i f f ( P e r i o d o E s t a c i o n a r i o ) ] == −1) ;
F i n E s t a c i o n a r i o = find ( [ 0 ; d i f f ( P e r i o d o E s t a c i o n a r i o ) ] == 1 ) ;
f o r i = 1 : numel ( F i n E s t a c i o n a r i o )
TasaDeriva = v e l o c i d a d ( F i n E s t a c i o n a r i o ( i ) −1, : ) / ( F i n E s t a c i o n a r i o
( i ) − InicioEstacionario ( i ) ) ;
enum = 1 : ( F i n E s t a c i o n a r i o ( i ) − I n i c i o E s t a c i o n a r i o ( i ) ) ;
D e r i v a = [ enum ’ ∗ TasaDeriva ( 1 ) enum ’ ∗ TasaDeriva ( 2 ) enum ’ ∗ TasaDeriva
(3) ] ;
D e r i v a V e l o c i d a d ( I n i c i o E s t a c i o n a r i o ( i ) : F i n E s t a c i o n a r i o ( i ) −1, : ) =
Deriva ;
end
p o s i c i o n = zeros ( s i z e ( v e l o c i d a d ) ) ;
f o r i = 2 : length ( p o s i c i o n )
posicion ( i , : ) = posicion ( i −1 ,:) + ( velocidad ( i , : ) ∗ periodo ) ;
end
close a l l ;
plot3 ( p o s i c i o n ( : , 1 ) , p o s i c i o n ( : , 2 ) , p o s i c i o n ( : , 3 ) ) ;
figure (2) ;
plot ( p o s i c i o n ( : , 1 ) , p o s i c i o n ( : , 2 ) ) ;
Micro-Robot Trazador de Trayectorias 115
close a l l
clear a l l
addpath ( ’ F u n c i o n e s ’ ) ;
%
−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
%I n i c i a l i z a c i ó n de v a r i a b l e s y d a t o s
f i l e P a t h = ’E : \ P r o y e c t o s ␣ Matlab \ s p i r a l S t a i r s . c s v ’ ;
Periodo = 1/256;
g = 9.81;
%
−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
%Recogida de d a t o s
data_raw = csvread ( f i l e P a t h , 1 , 0 ) ;
%
−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
%C a l c u l o de l o s p e r i o d o s e s t a c i o n a r i o s ( a ~0)
% C a l c u l o de l a magnitud de l a a c e l e r a c i ó n
M a g n i t u d A c e l e r a c i o n = sqrt ( a c e l e r a c i o n _ x . ∗ a c e l e r a c i o n _ x +
aceleracion_y .∗ aceleracion_y + aceleracion_z .∗ aceleracion_z ) ;
% F i l t r o paso a l t o para l o s d a t o s de l a a c e l e r a c i ó n
CorteFiltro = 0.001;
[ b , a ] = butter (1 , (2∗ C o r t e F i l t r o ) /(1/ Periodo ) , ’ high ’ ) ;
MagnitudAceleracionFiltada = f i l t f i l t (b , a , MagnitudAceleracion ) ;
% F i l t r o paso b a j o para l o s d a t o s de l a a c e l e r a c i ó n
CorteFiltro = 5;
[ b , a ] = b u t t e r ( 1 , ( 2 ∗ C o r t e F i l t r o ) / ( 1 / P e r i o d o ) , ’ low ’ ) ;
MagnitudAceleracionFiltada = f i l t f i l t (b , a ,
MagnitudAceleracionFiltada ) ;
% L í m i t e de d e t e c c i ó n
PeriodoEstacionario = MagnitudAceleracionFiltada < 0 . 0 5 ;
Micro-Robot Trazador de Trayectorias 116
%
−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
% G r a f i c a de l o s d a t o s d e l s e n s o r , l o s p e r i o d o s e s t a c i o n a r i o s y l o s
datos
% filtrados
f i g u r e ( ’ P o s i t i o n ’ , [ 9 39 900 6 0 0 ] , ’ Number ’ , ’ o f f ’ , ’Name ’ , ’
S e n s o r ␣ Data ’ ) ;
ax ( 1 ) = subplot ( 2 , 1 , 1 ) ;
hold on ;
plot ( tiempo , g i r o s c o p i o _ x , ’ r ’ ) ;
plot ( tiempo , g i r o s c o p i o _ y , ’ g ’ ) ;
plot ( tiempo , g i r o s c o p i o _ z , ’ b ’ ) ;
title ( ’ Giroscopio ’ ) ;
xlabel ( ’ Time␣ ( s ) ’ ) ;
ylabel ( ’ V e l o c i d a d ␣ Angular ␣ (^\ c i r c / s ) ’ ) ;
legend ( ’X ’ , ’Y ’ , ’ Z ’ ) ;
hold o f f ;
ax ( 2 ) = subplot ( 2 , 1 , 2 ) ;
hold on ;
plot ( tiempo , a c e l e r a c i o n _ x , ’ r ’ ) ;
plot ( tiempo , a c e l e r a c i o n _ y , ’ g ’ ) ;
plot ( tiempo , a c e l e r a c i o n _ z , ’ b ’ ) ;
plot ( tiempo , M a g n i t u d A c e l e r a c i o n F i l t a d a , ’ : k ’ ) ;
plot ( tiempo , P e r i o d o E s t a c i o n a r i o , ’ k ’ , ’ LineWidth ’ , 2 ) ;
t i t l e ( ’ Acelerometro ’ ) ;
xlabel ( ’ Time␣ ( s ) ’ ) ;
ylabel ( ’ A c e l e r a c i ó n ␣ ( g ) ’ ) ;
legend ( ’X ’ , ’Y ’ , ’ Z ’ , ’ F i l t e r e d ’ , ’ S t a t i o n a r y ’ ) ;
hold o f f ;
l i n k a x e s ( ax , ’ x ’ ) ;
%
−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
% C a l c u l o de l a c o n v e r g e n c i a i n i c i a l d e l c u a t e r n i ó n
% Esto s e r e a l i a z a para t e n e r una v a l o r i n i c i a l d e l c u a t e r n i ó n de
% referencia .
K p I n i t =1;
Kp=1;
Ki =0;
f o r i =1:2000
i f (norm( media_acc ) == 0 )
warning ( 0 , ’ V a l o r ␣ de ␣ l a ␣ a c e l e r a c i ó n ␣ e s ␣ n u l o . ␣ A l g o r i t m o ␣
Micro-Robot Trazador de Trayectorias 117
abortado . ’ ) ;
return ;
else
media_acc = media_acc / norm( media_acc ) ; %Valor
n o r m a l i z a d o de l a a c e l e r a c i ó n
end
v = [ 2 ∗ ( q ( 2 ) ∗q ( 4 ) − q ( 1 ) ∗q ( 3 ) )
2 ∗ ( q ( 1 ) ∗q ( 2 ) + q ( 3 ) ∗q ( 4 ) )
q ( 1 ) ^2 − q ( 2 ) ^2 − q ( 3 ) ^2 + q ( 4 ) ^ 2 ] ; %
E s t i m a c i ó n de l a d i r e c c i ó n de l a g r a v e d a d
error = cross ( v , media_acc ’ ) ; %
Producto c r u z a d o de l a a c e l e r a c i ó n y l a g r a v e d a d e s t i m a d a
E r r o r I n t e g r a l = E r r o r I n t e g r a l + error ;
%Acumulación d e l e r r o r
Ref = media_gyr − (Kp∗ error + Ki ∗ E r r o r I n t e g r a l ) ’ ; %
Valor d e l g i r o s c ó p i o c o r r e g i d o
pDot = 0 . 5 ∗ ProductoCuat ( q , [ 0 Ref ( 1 ) Ref ( 2 ) Ref ( 3 ) ] ) ; %
C a l c u l o de l a t a s a de cambio d e l c u a t e r n i o n
q = q + pDot ∗ P e r i o d o ; %
I n t e g r a c i ó n de l a t a s a de cambio d e l c u a t e r n i o n
q = q / norm( q ) ; %
Normalización d e l cuaternion
inv_q = ConjugadoCuat ( q ) ; %
I n v e r s a d e l c u a t e r n i o n para cambiar de e j e s
inv_q = inv_q /norm( inv_q ) ; %
N o r m a l i z a c i ó n de l a i n v e r s a d e l c u a t e r n i o n
c u a t = inv_q ; %
Acumulamos e l v a l o r d e l c u a t e r n i ó n
q = ConjugadoCuat ( inv_q ) ; %
Volvemos a i n v e r t i r e l c u a t e r n i ó n para v o l v e r a l o s e j e s
del sistema
end
%
−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
% C a l c u l o de l a o r i e n t a c i ó n d e l c u a t e r n i ó n en cada i n s t a n t e
% Se e s t i m a l a o r i e n t a c i ó n d e l s i s t e m a en cada i n s t a n t e r e g i s t r a d o por
los
% sensore
f o r t = 1 : length ( tiempo )
i f ( PeriodoEstacionario ( t ) )
%S i s e
t r a t a de un p e r i o d o e s t a c i o n a r i o s e l e da mas g a n a c i a a l
sistema
Kp = 0 . 5 ;
else
Kp = 0 ;
%En c a s o de no s e r p e r i o d o e s t a c i o n a r i o s e a n u l a l a
ganancia
end
g i r o s c o p i o = deg2rad ( [ giroscopio_x ( t ) giroscopio_y ( t )
giroscopio_z ( t ) ] ) ; %Convertimos e l v a l o r d e l g i r o s c ó p i o
de Grados a Racianes y creamos un v e c t o r con e s o s d a t o s
aceleracion = [ aceleracion_x ( t ) aceleracion_y ( t ) aceleracion_z
(t) ]; %Creamos un v e c t o r con l o s d a t o s de l a
a c e l e r a c i o n en e l i n s t a n t e t
i f (norm( a c e l e r a c i o n ) == 0 )
warning ( 0 , ’ V a l o r ␣ de ␣ l a ␣ a c e l e r a c i ó n ␣ e s ␣ n u l o . ␣ A l g o r i t m o ␣
Micro-Robot Trazador de Trayectorias 118
abortado . ’ ) ;
return ;
else
a c e l e r a c i o n = a c e l e r a c i o n / norm( a c e l e r a c i o n ) ;
%Valor n o r m a l i z a d o de l a
aceleració
end
v = [ 2 ∗ ( q ( 2 ) ∗q ( 4 ) − q ( 1 ) ∗q ( 3 ) )
2 ∗ ( q ( 1 ) ∗q ( 2 ) + q ( 3 ) ∗q ( 4 ) )
q ( 1 ) ^2 − q ( 2 ) ^2 − q ( 3 ) ^2 + q ( 4 ) ^ 2 ] ;
%E s t i m a c i ó n de l a
d i r e c c i ó n de l a g r a v e d a d
error = cross ( v , a c e l e r a c i o n ’ ) ;
%Producto
c r u z a d o de l a a c e l e r a c i ó n y l a g r a v e d a d e s t i m a d a
E r r o r I n t e g r a l = E r r o r I n t e g r a l + error ;
%
Acumulación d e l e r r o r
Ref = g i r o s c o p i o − (Kp∗ error + Ki ∗ E r r o r I n t e g r a l ) ’ ;
%Valor d e l g i r o s c ó p i o
corregido
pDot = 0 . 5 ∗ ProductoCuat ( q , [ 0 Ref ( 1 ) Ref ( 2 ) Ref ( 3 ) ] ) ;
%C a l c u l o de l a t a s a de cambio d e l
cuaternion
q = q + pDot ∗ P e r i o d o ;
%
I n t e g r a c i ó n de l a t a s a de cambio d e l c u a t e r n i o n
q = q / norm( q ) ;
%N o r m a l i z a c i ó n d e l c u a t e r n i o n
inv_q = ConjugadoCuat ( q ) ;
%I n v e r s a
d e l c u a t e r n i o n para cambiar de e j e s
inv_q = inv_q /norm( inv_q ) ;
%
N o r m a l i z a c i ó n de l a i n v e r s a d e l c u a t e r n i o n
c u a t = inv_q ;
%Acumulamos e l v a l o r d e l c u a t e r n i ó n
q = ConjugadoCuat ( inv_q ) ;
%
Volvemos a i n v e r t i r e l c u a t e r n i ó n para v o l v e r a l o s e j e s
del sistema
Cuaternion ( t , : ) = cuat ;
%
Almacenamos e l v a l o r d e l c u a t e r n i ó n
end
%
−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
% C a l c u l o de l a d i r e c c i ó n de l a a c e l e r a c i ó n
a c e l e r a c i o n = RotacionCuat ( [ a c e l e r a c i o n _ x a c e l e r a c i o n _ y
a c e l e r a c i o n _ z ] , ConjugadoCuat ( C u a t e r n i o n ) ) ; %Posicionamos l a
aceleración respecto a los ejes f i j o s
aceleracion = aceleracion ∗ g ;
%Convertimos de G’ s a m/ s ∗ s
%
Micro-Robot Trazador de Trayectorias 119
−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
% G r a f i c a de l a a c e l e r a c i ó n
%
−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
% C a l c u l o de l a v e l o c i d a d d e l s i s t e m a
v e l o c i d a d = zeros ( s i z e ( a c e l e r a c i o n ) ) ;
f o r t = 2 : length ( v e l o c i d a d )
velocidad ( t , : ) = velocidad ( t −1 ,:) + a c e l e r a c i o n ( t , : ) ∗ Periodo
; %I n t e g r a m o s l a a c e l e r a c i ó n para
obtener la velocidad
i f ( P e r i o d o E s t a c i o n a r i o ( t ) == 1 )
velocidad ( t , : ) = [0 0 0 ] ;
%S i s e t r a t a de un p e r i o d o e s t a c i o n a r i o f o r z a m o s una
v e l o c i d a d nula
end
end
D e r i v a V e l o c i d a d = zeros ( s i z e ( v e l o c i d a d ) ) ;
I n i c i o E s t a c i o n a r i o = find ( [ 0 ; d i f f ( P e r i o d o E s t a c i o n a r i o ) ] == −1) ;
F i n E s t a c i o n a r i o = find ( [ 0 ; d i f f ( P e r i o d o E s t a c i o n a r i o ) ] == 1 ) ;
f o r i = 1 : numel ( F i n E s t a c i o n a r i o )
TasaDeriva = v e l o c i d a d ( F i n E s t a c i o n a r i o ( i ) −1, : ) / (
FinEstacionario ( i ) − InicioEstacionario ( i ) ) ;
enum = 1 : ( F i n E s t a c i o n a r i o ( i ) − I n i c i o E s t a c i o n a r i o ( i ) ) ;
D e r i v a = [ enum ’ ∗ TasaDeriva ( 1 ) enum ’ ∗ TasaDeriva ( 2 ) enum ’ ∗
TasaDeriva ( 3 ) ] ;
D e r i v a V e l o c i d a d ( I n i c i o E s t a c i o n a r i o ( i ) : F i n E s t a c i o n a r i o ( i ) −1, : )
= Deriva ;
end
%
−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
% G r a f i c a de l a v e l o c i d a d
plot ( tiempo , v e l o c i d a d ( : , 1 ) , ’ r ’ ) ;
plot ( tiempo , v e l o c i d a d ( : , 2 ) , ’ g ’ ) ;
plot ( tiempo , v e l o c i d a d ( : , 3 ) , ’ b ’ ) ;
t i t l e ( ’ Velocidad ’ ) ;
xlabel ( ’ Time␣ ( s ) ’ ) ;
ylabel ( ’ V e l o c i d a d ␣ (m/ s ) ’ ) ;
legend ( ’X ’ , ’Y ’ , ’ Z ’ ) ;
hold o f f ;
%
−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
% C a l c u l o de l a p o s i c i ó n d e l s i s t e m a
p o s i c i o n = zeros ( s i z e ( v e l o c i d a d ) ) ;
f o r t = 2 : length ( p o s i c i o n )
p o s i c i o n ( t , : ) = p o s i c i o n ( t −1 ,:) + velocidad ( t , : ) ∗ Periodo ;
%i n t e g r a m o s l a v e l o c i d a d para o b t e n e r l a p o s i c i ó n
end
%
−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
% G r a f i c a de l a p o s i c i ó n
close a l l ;
plot3 ( p o s i c i o n ( : , 1 ) , p o s i c i o n ( : , 2 ) , p o s i c i o n ( : , 3 ) ) ;
figure (2) ;
plot ( p o s i c i o n ( : , 1 ) , p o s i c i o n ( : , 2 ) ) ;