Herramienta para la simulacion integradas de subsistemas en un...

Capítulo 3

Información sensorial

En este capítulo veremos los sensores usados, en primer lugar el dispositivo que incorpora unaIMU, posteriormente veremos el sensor óptico, que se trata de una cámara industrial. Tras laintroducción de ambos sensores, veremos que obtenemos de cada uno, la necesidad de usarlos y elacoplamiento entre ambos.

3.1. IMU

Las siglas IMU provienen del inglés Inertial Measurement Unit que en español se traduce porunidad de medida inercial, que en general es un dispositivo con acelerómetros y giróscopos. Eldispositivo concreto que se usará es de la empresa Xsens, el modelo MTi-G y es un dispositivo queintegra un GPS y una IMU con un procesador AHRS (Attitude and Heading Reference System, olo que es lo mismo, sistema de referencia de posición y orientación), en la figura 3.1 podemos verel aspecto exterior de la IMU y de la antena de GPS.

Figura 3.1: IMU y antena GPS

El dispositivo MTi-G está basado en sensores inerciales MEMS (MicroElectricalMechanical

Systems o sistemas microelectromecánicos) y en un receptor GPS en miniatura, además incluyetambién otros sensores, como un magnetómetro 3D, un sensor de presión estática (barómetro) yun termómetro. El procesador interno de bajo consumo ejecuta en tiempo real un filtro de Kalman

19

20 CAPÍTULO 3. INFORMACIÓN SENSORIAL

de Xsens que mejora la estimación de la posición y de la velocidad y con el GPS mejora la derivaproducida.

En la figura 3.2 podemos ver la arquitectura de la IMU y las distintas funcionalidades que posee:

Figura 3.2: Arquitectura de la IMU

Características:

Estimación en tiempo real de la posición y velocidad y tratamiento de los datos del GPS.

Compatibilidad con el sistema GALILEO (requiere actualización de firmware).

Sensibilidad en seguimiento de -160 dBm.

Alta inmunidad a interferencias GPS.

Medición en 360º para la orientación.

Aceleración 3D, tasa de giro 3D y magnetismo terrestre 3D.

Sensor de presión estática (barómetro).

Alta tasa de actualización (120 Hz en el DSP integrado, 512 Hz en los datos inerciales solos).

Diseño compacto.

Poco peso.

Muy bajo consumo.

Varios modos de salida digital.

Sensores inerciales MEMS de estado sólido en el interior.

Detección de fallo de antena.

3.1. IMU 21

Filtro de Kalman extendido

La orientación y posición del MTi-G es estimada usando un filtro de Kalman extendido deno-minado Filtro de Kalman de Xsens 6DOF GPS (XKF-6G, XKF proviene de Xsens Kalman Filter,DOF Degree of Freedom o grados de libertad). Un filtro de Kalman está divido en un paso deestimación y en un paso de corrección. En el paso de estimación, el sensor inercial se integra a lolargo del tiempo para obtener una estimación de la posición y de la orientación. Debido a pequeñasinexactitudes de los giróscopos y acelerómetros, las estimaciones pueden no ser perfectas y el erroren la estimación puede crecer con el tiempo y tendremos pequeñas derivas. En el paso de correc-ción, esta deriva es corregida con la recepción GPS y con el sensor de presión. Dependiendo de laconfiguración en la que se encuentre el dispositivo, se pueden usar otros sensores para ayudar en ladicha corrección.

Dead reckoning usando sensores inerciales

El MTi-G está diseñado para obtener lo máximo de los MEMS, giróscopos y acelerómetros,pero la exactitud de los sensores en miniatura está limitada y pequeños errores pueden crecer hastaconvertirse en errores significativos si integramos durante más de varios segundos. Los órdenes demagnitud son aproximadamente de una deriva de 1-2 grados por medio minuto de orientación.

Los sensores inerciales MEMS son usados para obtener la posición y orientación por integracióna través del tiempo. A este proceso se le suele denominar dead reckoning. Dada una estimación deuna orientación previa, la velocidad angular de los giróscopos es integrada para obtener una nuevaestimación de la orientación. La posición se calcula dada una posición previa, integrando doblementela señal del acelerómetro para obtener el cambio de posición, el cual sumamos a la medida previaque teníamos para obtener la nueva estimación de la posición. Las medidas de los acelerómetros,incluye la aceleración debida a la gravedad y la aceleración libre, es decir, la segunda derivadade la posición. La gravedad debe ser substraída usando una estimación exacta de la orientación.Para muchos movimientos, la gravedad es una parte importante de la señal del acelerómetro. En lapráctica, esto implica que un error pequeño en la orientación puede conllevar errores relativamentegrandes en aceleración, velocidad y posición.

Corrección de la posición y orientación usando el GPS

Para recibir la señal de los satélites debemos conectarle al MTi-G una antena GPS. La posicióny velocidad están basadas en la posición y movimiento de la antena, no del sensor alojado en elMTi-G. Por lo que la relación posicional entre la antena GPS y el origen del MTi-G debe serconocida y fijada.

La exactitud alcanzable de posición y velocidad depende fuertemente de las condiciones talescomo el número de satélites que capta la antena, incertidumbres en las posiciones de los satélites yen la sincronización del reloj, presencia de obstáculos en la línea de visión con el satélite, condicionesatmosféricas (incluidas la ionosfera y troposfera), etc. La funcionabilidad depende también de lahistoria, es decir, la exactitud actual depende del pasado. El receptor GPS necesita un cierto tiempopara iniciarse.

El receptor GPS del MTi-G tiene una banda L1 preparada para GALILEO L1 cuando el sistemaesté disponible, para ello requiere una actualización de firmware, con capacidad SBAS (Satellite-


Based Augmentation System, sistema de aumento basado en satélite) que mejora la exactitud yfiabilidad del sistema GPS.

El sistema GPS complementa al sistema inercial proveyéndole de posición y velocidad estable desalida. Pequeños errores introducidos en la integración en la señal de los acelerómetros y giróscoposson corregidos con posición y velocidad del receptor GPS resultando una salida a la vez estable ycapaz de seguir cambios rápidos en los movimientos. Igualmente importante, el GPS no sólo corrigeposición y velocidad, la orientación también, ya que la dirección del vector de los acelerómetros esconocida en el sistema de coordenadas inercial para calcular la posición, es decir, si la posición puedeser corregida, la orientación con respecto al vector del acelerómetro también puede ser corregida.

El vector del acelerómetro mide tanto la aceleración de la gravedad como la causada por elmovimiento del MTi-G respecto al sistema inercial. Para muchos tipos de movimientos el vectordel acelerómetro será aproximadamente vertical, por lo que al inclinación puede ser estabilizada.Para aplicaciones con relativa dinámica alta, el vector del acelerómetro tendrá dirección diferentepermitiendo una estimación exacta de la orientación 3D.

Corrección de altura usando el sensor de presión estática

Debido a la geometría de la posición de los satélites GPS (denominada constelación), la esti-mación de la altura usando GPS es en la práctica menos precisa que la estimación de la posiciónhorizontal. Usando el sensor de presión estática o barómetro, como un sensor de ayuda para laaltitud, incrementará la precisión vertical. Los errores del barómetro dependen principalmente dela relación entre la altura y la presión. Dicha relación depende de varios factores aunque el más im-portante es el tiempo atmosférico, por ejemplo la presión P0 dada una cierta altitud. Otras fuentesde error pueden ser cambios transitorios en la presión.

La medida de la altitud basada en la presión es una medida relativa, lo que se hace es compararla presión actual con una presión de referencia. Sin embargo, la presión estática atmosférica al niveldel mar P0, que varía según la climatología, es estimada por XKF-6G cuando está disponible laestimación de altura mediante el GPS. Por lo que el barómetro solo será usado como sensor decambio de altura con un paso lento. En entornos en los que no podamos usar el GPS, el barómetrosolo estará disponible como referencia vertical para estabilizar la estimación de la posición verticalbasada en los sensores inerciales.

Si asumimos un gradiente constante dTdH

tendremos:

H = T0

( dTdH ) ·

(

1 −(

PP0

)

(

-dTdH

Rg

)

)

donde:

H es la altura.

T0 es la temperatura en grados Kelvin a nivel del mar.

P0 es la presión en kPa a nivel del mar.

P es la presión en kPa a la altura H.

g es el módulo de la aceleración de la gravedad en m/s2, típicamente g = 9,82m/s2.

R es una constante de valor R = 287,052m2/s2/K.

3.1. IMU 23

A modo de ejemplo, dando un valor típico para el gradiente de −6,5ºC/km , la temperatura presióna nivel del mar, T0 = 288,15ºK P0 = 101,325kPa, tendremos:

H = 44330 ·(

1 −(

PP0

)0,19)

Errores debidos a no modelar correctamente la temperatura cambian alrededor de 1.5 metroscuando vamos desde el nivel del mar hasta los 3000 metros. La linealidad en este rango es sobre0.1 metros. Esto es mucho menor que cambios en la salida debidos a cambios actuales en la presiónatmosférica, gradientes de temperatura local o flujo de aire.

Para grandes valores de altitud (10 km o mayores) el rendimiento del barómetro puede degra-darse debido a ruidos en la señal. Por eso si tenemos valores altos, deberemos usar el escenarionobaro, configuración que veremos más adelante que sensores podemos usar con ella.

Observabilidad de la orientación

En situaciones con relativa dinámica baja, la orientación no puede ser siempre observada desdelas medidas GPS solamente. Sin embargo, ya se vio en la sección de dead reckoning que la orien-tación es bastante importante, ya que inexactitudes pueden causar que el sensor inercial integre laaceleración en una dirección errónea obteniendo una posición errónea.

Para mejorar la observabilidad de la orientación el dispositivo cuenta con otros sensores quepueden ayudar, así como hipótesis del movimiento que tendremos, como se ve en el apartado delos escenarios XKF.

Escenarios XKF

Los métodos para incrementar la observabilidad de la orientación difiere según el escenarioelegido para el XKF.

Determinar la orientación usando solamente el GPS en el caso de tener suficiente dinámica,es el escenario de propósito general.

Usando el campo magnético terrestre con los magnetómetros incorporados, es el escenarioaeroespacial.

En vehículos donde el rumbo es esencialmente como está orientado, sin desplazamientos la-terales, el escenario es automoción.

Usando el MTi-G en aplicaciones marinas, el escenario adecuado es el marino, en él se usanlos magnetómetros ya que es habitual que sea necesario el campo magnético.

La memoria FLASH no volátil del dispositivo permite almacenar sólo 5 escenarios, podemos usarel software suministrado por el fabricante para cargar nuevos escenarios. Las carácteristicas de losdistintos escenarios se encuentran resumidas en la tabla 3.1


Escenario XFK IMU GPS Magnetómetro Barómetro Holonómico

(sin desplazamiento lateral)

Propósito general • • •

General_nobaro • •

Aeroespacial • • • •

Aeroespacial_nobaro • • •

Automoción • • • •

Automoción_nobaro • • •

Marino • • •

Cuadro 3.1: Características de los escenarios

Escenario de Propósito General

Este escenario debe usarse si el campo magnético terrestre en el entorno donde nos encontremosno se usa. La orientación sólo puede observarse si el objeto experimenta suficiente aceleración. Si seencuentra parado o moviendo con velocidad constante más de unos 10 segundos, la observación dela orientación puede degradarse lentamente. Cualquier deriva en la orientación puede recuperarsetan pronto como vuelva a haber aceleración.

Escenario Aeroespacial

Este escenario usa los magnetómetros incluidos en el dispositivo como sensor de ayuda parala observación del norte magnético. La ventaja de este escenario es que el campo magnético sepuede usar si el vehículo está parado o con velocidad constante, la orientación es observable entodo momento.

El requerimiento clave para este escenario se denomina mapeado del campo magnético que seda cuenta de los materiales ferromagnéticos que se encuentran en una posición fija respecto alobjeto que será seguido. Usando el mapeo del campo magnético, la distorsión de dichos materialestiene una posición fija respecto al MTi-G y puede ser calibrada. Si se usa el dispositivo MTi-G

en entornos donde tengamos grandes estructuras de acero próximas al dispositivo será convenienteusar otro tipo de escenario, como el general o el de automoción.

Escenario Automoción

Este escenario se desarrolla para vehículos cuyo movimiento no tenga excesivo desplazamientolateral. El eje x del MTi-G tiene que ser alineado con exactitud en la dirección de desplazamientodel vehículo. Se supone que en promedio, el llamado rumbo sobre tierra será igual que la orientacióndel vehículo, esto es la restricción no holonómica. Dicha restricción no se usa cuando la velocidades menor que 2m/s (en cualquier dirección).

Si el eje x del dispositivo no puede montarse en la dirección del movimiento pero la orientaciónes conocida, se puede alinear con una matriz de orientación.

3.1. IMU 25

Escenario Marino

Este escenario está desarrollado para vehículos de superficie marina con velocidad significativa.El magnetómetro es usado para mejorar la observabilidad.Se recomienda el mapeado del campomagnético para compensar los efectos del hierro duro y/o blando.

Entre los distintos escenarios en los que se puede configurar la IMU, se ha elegido el aerospace

nobaro. Se usa este escenario porque incluye el magnetómetro, sin el cual el ángulo yaw tendría unaderiva continua y se incrementa o decrementa indefinidamente resultando inútil para el control. Elmagnetómetro aunque no marque el norte real servirá para dar un cero estable si la configuraciónmagnética del entorno no varía. Y se ha elegido la opción sin barómetro pues la altura a la que iráel vehículo no sufrirá incrementos suficientemente grandes como para que sea apreciable un cambiode la presión atmosférica.

Limitaciones

El filtro dentro del MTi-G, XFK, es un filtro causal, sólo usamos información actual y pasadapara obtener la posición y orientación. Esto implica que el MTi-G necesita cierto tiempo paraestabilizarse, típicamente unos 30 segundos. La puesta en marcha es de un minuto aunque el filtrocontinua mejorando incluso hasta los 15 minutos, este tiempo es requerido para establecer un buensesgo de la posición, orientación y cambios de los giróscopos.

La antena GPS debe tener visión clara del cielo. En entornos con recepción de señal pobre deGPS la estimación de posición y orientación puede perder exactitud. En particular, saltos repentinosen la señal GPS puede causar comportamientos erráticos en el XKF.

Especificaciones

A continuación se verán las especificaciones de los distintos componentes del dispositivo:

Receptor GPS Posición y orientación del XKF

Tipo recepción 50 canales

GPS L1 Rango Dinámico

GALILEO

Tasa de actualización del GPS 4 Hz Pitch (cabeceo) ±90º

Tasa actualización Posición/Velocidad 120 Hz Roll (balanceo) ±90º

Exactitud Posición SPS 2.5 m CEP Heading ±180º (0...360º)

SBAS 2.0 m CEP Resolución angular 0.05 deg

Tiempo de inicio en frío 29 s Precisión estática

Readquisición <1 s Roll/Pitch <0.5 deg

Sensibilidad de seguimiento -160 dBm Heading <1 deg

Exactitud en temporización 30 ns RMS Precisión dinámica

Límites operacionales Roll/Pitch 1 deg RMS

Altitud máxima 18 km Heading 2 deg RMS

Velocidad máxima 600 m/s Tasa de actualización máxima

(2160 km/h) De forma autónoma 120 Hz

Dinámica máxima GPS 4 g datos PC/raw 512 Hz

Cuadro 3.2: Especificaciones GPS Y XKF


tasa de acele- campo tempe- presión

giro ración magnético ratura estática

Unidades [deg/s] [m/s2

] [mGauss] [ºC] [Pa]

Dimensiones 3 ejes 3 ejes 3 ejes - -

Escala completa (FS) [unidades] +/- 300 +/- 50 +/- 750 -55 - +125 30 - 120·103

Linealidad [ % de FS] 0.1 0.2 0.2 <1 0.5

Sesgo estabilidad [unidades 1σ] 1 0.02 0.1 0.5 100/año

Factor escala estabilidad [ % 1σ] - 0.03 0.5 - -

Densidad de ruido [unidades/

√Hz] 0.05 0.002 0.5 (1σ) - 4

error de alineación [deg] 0.1 0.1 0.1 - -

Ancho de banda [Hz] 40 30 10 - -

resolución A/D [bits] 16 16 16 12 9

Cuadro 3.3: Especificaciones de los sensores

Se pueden disponer de configuraciones personalizadas de los acelerómetros y de la tasa de losgiróscopos:

Acelerómetro Rectificación de las especificaciones

±50 m/s2 (5 g) (por defecto) Ninguna±17 m/s2(1.7 g) Ninguna±180 m/s2(18 g) Densidad de ruido: 0.004 m/s2/

√Hz

Cuadro 3.4: Especificaciones acelerómetro

Tasa del giróscopo Rectificación de las especificaciones

±1200 deg/s Densidad de ruido 0.1º/s/√

Hz± 300 deg/s (por defecto) Ninguna

± 150 deg/s Densidad de ruido 0.04º/s/√

Hz

Cuadro 3.5: Especificaciones giróscopo

Consideraciones

El modo de lectura de la IMU, puede ser free-run o data-request. En el modo free-run la IMUmanda el valor de la lectura cuando lo tiene, sin necesidad de pedírselo y en el modo data-request

3.2. Cámara 27

no lo manda hasta que no se le pide. Hubo que cambiarlo al modo free-run ya que se observó queen el otro modo se obtenían medidas imprecisas y fluctuantes.

Con la antena conectada, hay que esperar entre 3 y 5 minutos aproximadamente para que seestablezca un enlace con el GPS, empiece la corrección de las medidas y por tanto, se estabilicen.Como era de esperar en interiores funciona un poco peor debido a la pobre intensidad que se recibede los satélites.

3.2. Cámara

Lo relacionado con la cámara o sensor óptico, se ha obtenido del proyecto fin de carrera deJesús María González Villagomez, [2]. En este apartado no se verá con el mismo nivel de detalleque se ve en el trabajo recién mencionado, veremos partes muy importantes como son el modeladoy la calibración tanto externa como interna explicando que parámetros se determinan en cada unade ellas.

3.2.1. Modelado

El modelado del sistema de la cámara se basa en el modelo pinhole. En este modelo, la cámararealiza una proyección en perspectiva de un punto espacial P a un punto m en el plano de la imagena través del centro óptico de la cámara {C}.

El punto espacial P con coordenadas (Xc, Yc, Zc) con referencia al sistema de coordenadas dela cámara, queda representado en el plano de la imagen por un punto m con coordenadas (u, v), otambién denominadas coordenadas centrales, con referencia al punto central de la imagen (u0, v0),figura 3.3, donde el punto principal de la cámara se encuentra en el eje óptico de la misma a unadistancia focal f de su centro.

Figura 3.3: Modelo pinhole de la cámara

En el modelo presentado se verifica que:

u

Xc=

v

Yc=

f

Zc


La expresión anterior se denomina transformación en perspectiva, donde se ha supuesto unmodelo libre de distorsión óptica. Ésta puede ser de dos tipos, radial y tangencial. La radial sedebe a la forma de la lente, haciendo que los bordes de la imagen se vean alterados, como con unefecto de doblado, es decir, redondeando las esquinas. La tangencial se produce durante el procesode fabricación de la cámara y se debe a que el plano del sensor no se coloca paralelo al plano de lalente.

3.2.2. Calibración cámara

En este apartado se describe el conjunto de parámetros que se obtiene tras el proceso de cali-bración de la cámara.

Parámetros extrínsecos

La superficie de los puntos de la escena se corresponde con el sistema de coordenadas, podríamosdecir, del mundo real, con un origen {W}. Dado un punto de un objeto, P, éste queda expresadoen el sistema de coordenadas respecto a {W} de la forma (Xw, Yw, Zw).

Supongamos ahora que se establece otro sistema de referencia en el origen {C}, cuyos ejes son{−→xc,

−→yc ,−→zc }, situado en el centro del eje óptico de la cámara, como se muestra en la figura 3.4

Figura 3.4: Relación entre el sistema de coordenadas {C} y el sistema de coordenadas {W}

Gracias a esta relación geométrica se pueden definir cuáles son los parámetros extrínsecos delmodelo (calibración externa), los que determinarán la posición y orientación del sistema de coor-denadas de la cámara respecto del sistema de coordenadas del mundo real o del objeto.

La posición del centro óptico {C} respecto a {W} se da mediante el vector de traslación t:

3.2. Cámara 29

t =

txtytz

La orientación de los ejes del sistema de la cámara respecto al sistema de ejes del objeto dereferencia viene dado por lo que se conoce como matriz de rotación R, una matriz cuadrada deorden 3, como las ya vistas en el capítulo 2.

Ambos sistemas quedan relacionados mediante:

Xc

Yc

Zc

= R

Xw

Yw

Zw

+ t

Por lo que para un punto cualquiera, P, cuyas componentes están respecto al sistema de coor-denadas {W}, se puede obtener su representación respecto al sistema de coordenadas {C}:

CP = CRW · W P + CtW

Parámetros intrínsecos

El punto m de la figura 3.3 expresado en coordenadas normalizdas, m = [u v 1]T , puede referirsecon respecto al borde superior izquierdo de la imagen que se da en coordenadas p = [x y]T enpíxeles, o denominadas coordenadas laterales, cuyas coordenadas normalizadas son p = [x y 1]T :

x = u0 + fxu

y = v0 + fyv

donde fx y fy son las distancias focales en las direcciones ~x e ~y respectivamente.

De forma matricial puede expresarse:

p = K · m

K =

fx 0 u0

0 fy v0

0 0 1

A la matriz K se le denomina matriz de parámetros intrínsecos, es de 3x3 y describe la geometríay la óptica de la cámara.


Figura 3.5: Parámetros intrínsecos

3.3. Fusión cámara IMU

Ya hemos visto una descripción de los dos sensores, del óptico o cámara y de la IMU, ahoraveremos con que nos quedamos de cada uno de ellos en el caso concreto del quadrotor.

3.3.1. Estructura de datos de la IMU

Podemos obtener datos de orientación, de aceleración lineal, de velocidad angular, de nortemagnético, de temperaturas y barométricos.

Datos de orientación

Se puede obtener la orientación de tres formas diferentes que se indican a continuación. Debetenerse en cuenta que los datos de orientación parecen tener una deriva importante en, al menos,uno de los ángulos. Esto se reduce significativamente con la antena GPS en exterior. En interior,la deriva se logra reducir, aunque dista de desaparecer totalmente.

Cuaterno unitario

El tipo apropiado para una medida en forma de cuaterno es la siguiente estructura:

struc CmtQuat { double m_data[4]; };

Definiríamos un dato de este tipo CmtQuat qatData; y podríamos obtener la medida con lafunción de lectura: qatData = packet ->getOriQuat(0);.

3.3. Fusión cámara IMU 31

Todas las funciones del tipo get...() tienen como argumento el número del dispositivo IMU delcual toman las medidas. En nuestro caso, será siempre 0.

Ángulos de Euler XYZ (Roll, Pitch and Yaw)

Se puede obtener la orientación mediante la tripleta de ángulos de Euler respecto a ejes fijos.Es importante notar que las medidas de estos ángulos se dan en grados, no en radianes.

La estructura de datos asociada es la siguiente:

struc CmtEuler { double m_roll; // Rotación alrededor del eje x / línea-atrás-adelantedouble m_pitch; // Rotación alrededor del eje y / línea-derecha-izquierdadouble m_yaw; // Rotación alrededor del eje z / línea-abajo-arriba

Definiríamos un dato de este tipo: CmtEuler eulerData; y podríamos obtener la medida me-diante:

eulerData = packet->getOriEuler (0);

Los ángulos de Euler tienen una indeterminación cuando el segundo ángulo está próximo o esigual a los 90º o -90º. Esta indeterminación no existe en formato cuaterno o matriz de rotación.

Matriz de rotación

Si lo deseamos, podemos pedir que se nos facilite la matriz de rotación completa. La estructuraasociada es la siguiente: struc CmtMatrix {double m_data[3] [3]; };

Podemos definir un dato de este tipo mediante: CmtMatrix matrixData; y obtener la matrizusando la función matrixData = packet->getOriMatrix(0);

Calibrated data

Permite obtener la medida de tres sensores: acelerómetro, giróscopo y brújula.

Acelerómetro: ofrece medida de la aceleración lineal (en m/s2) de la IMU respecto al sis-tema inercial, pero expresado respecto al propio sistema de la IMU. Una inconveniencia deesta medida es que lleva incorporado el efecto de la aceleración de la gravedad. Es como si,estando parada, la IMU se estuviera moviendo en dirección vertical hacia arriba con una ace-leración igual a la de la gravedad. Tras varias pruebas, esa aceleración es aproximadamente[0, 0, 9,723923]T .

Giróscopo (“gyro”): proporciona un vector velocidad angular (en rad/s) de la IMU respectoal sistema inercial, pero expresado respecto al propio sistema de la IMU.

Brújula (“compass”): da la dirección del vector norte magnético respecto al sistema de coor-denadas local de la IMU. Las unidades son en a.u (“arbritrary units”, unidades normalizadascon la fuerza del campo magnético de la Tierra). Dada las unidades, de esta medida proba-blemente sólo tenga sentido usar la dirección y sentido.


Es importante insistir en que estas medidas no están expresadas respecto al sistema de coorde-nadas inercial.

La estructura asociada es la siguiente: struct CmtCalData{ CmtVector m_acc, m_gyr, m_mag;};

Cada uno de los vectores es del tipo: struct CmtVector {double m_data[3]; };

Podemos definir un dato de este tipo: CmtCalData calData; y obtener en él la medida mediante:calData = packet->getCalData(0);

O bien, acceder selectivamente:... = packet->getCalAcc(0) ;... = packet->getCalGyr(0) ;... = packet->getCalMag(0) ;

Hay otra opción para obtener los calibrated data, se trata del formato raw data, en formatoentero, sin convertir y donde va la temperatura ya integrada. En este caso, la estructura es lasiguiente:

struct CmtRawData { CmtShortVector m_acc, m_gyr, m_mag ;uint16_t m_temp };

Cada uno de los vectores es del tipo:CmtShortVector: struct CmtShortVector {uint16_t m_data[3]; };

Podemos definir un dato de este tipo: CmtRawData rawData; y obtener la medida mediante:rawData = packet->getRawData(0) ;

Por otro lado, puede obtenerse la medida de temperatura aisladamente:double temperat = packet->getTemp(0) ;

3.3.2. Datos proporcionados por la IMU

Se asume que la IMU proporciona una medida relativamente precisa de la orientación y dela velocidad angular, así como de la aceleración lineal: η, ω y a respectivamente. Estos datos seobtendrían con las llamadas a las funciones:

a ⇐getCalAcc(0)ω ⇐getCalGyr(0)η ⇐getOriEuler(0) o alternativamente: getOriQuat(0), getOriMatrix(0)

Estas medidas son orientaciones, velocidades etc. del sistema de coordenadas ligado a la IMU (alque denominaremos {G} de gyro), respecto del sistema de coordenadas inercial (al que denominamos{W} de world coordinate system). En particular, la orientación viene expresada respecto al sistemade referencia inercial, mientras que la velocidad angular y aceleración lineal vienen expresadasrespecto al sistema de coordenadas móvil de la IMU. Para evidenciar este hecho, completaremosnuestra nomenclatura de la siguiente forma:

W a(G)G ⇐getCalAcc(0)

W ω(G)G ⇐getCalGyr(0)

W ηG ≡ W η(W )G ⇐getOriEuler(0)


Fijándonos en primer lugar en la parte lineal tenemos que para obtener una medida efectiva dela aceleración lineal, tenemos que eliminar la contribución de la gravedad:

W aG = W RG · W a(G)G

W tG = W aG − W g

siendo W g ≈ [0, 0, 9,713923]T y W RG es la matriz de rotación correspondiente a la tripleta deángulos W ηG = [φ, θ, ψ]T .

Desde el punto de vista del control, resulta más interesante disponer de estimaciones de laposición y velocidad lineales, en lugar de la aceleración lineal. A partir de dicha aceleración lineal,se puede obtener por software una estimación de la velocidad lineal. En principio, la opción másinmediata es hacer una estimación discreta mediante Euler hacia atrás, usando un periodo deintegración fijo T:

W tG(k) ≈ W tG(k − 1) + T W tG(k)

W tG(k) ≈ W tG(k − 1) +T

2( W tG(k) + W tG(k − 1))

Y si el periodo no es suficientemente constante de un ciclo al siguiente:

W tG(k) ≈ W tG(k − 1) +12

(T (k) W tG(k) + T (k − 1) W tG(k − 1))

Donde T (k) se define como el tiempo transcurrido desde que se tomó la salida (k−1) hasta quese tomó la medida k. Lo que no parece viable es aplicar el mismo mecanismo de integración (encualquiera de sus variantes) para obtener una aproximación de la posición, puesto que es previsibleque integrar dos veces una medida dé resultados poco aceptables.

Pasando ahora a los términos angulares obtenidos, tenemos W ω(G)G y W ηG. En lugar de la

velocidad angular W ω(G)G , nos interesa la derivada de los propios ángulos, W ηG, cuya relación entre

ambos vectores viene dada por:

W ηG =

φ

θ

ψ

= W −1( W ηG) · W ω

(G)G

Esta misma relación se puede expresar directamente en términos de los elementos de la matrizde rotación, en lugar de en función de la tripleta de ángulos:

W ηG = W −1(R) · W ω(G)G

Para comprobar que los valores obtenidos mediante esta expresión son correctos, se pasa a com-probar, W ηG , mediante una comparación con la derivación numérica de la tripleta de ángulos,W ˆηG,como se puede ver en las siguientes figuras:

W ˆηG ≈W ηG = W −1( W ηG) · W ω

(G)G


Figura 3.6: Ángulos obtenidos en el experimento

Figura 3.7: Comparación del ángulo roll

Figura 3.8: Comparación del ángulo pitch


Figura 3.9: Comparación del ángulo yaw

Se observa en las figuras que en efecto, la aproximación es correcta y el error cometido aceptable,por lo que las operaciones que se han realizado confirman que se obtiene de la IMU la velocidadangular y los ángulos de Euler.

Con esto, podemos componer un vector de medida preliminar a partir de los datos obtenidosdirectamente de la IMU, de la forma:

xraw =

W tGW ηGW ηG

Recapitulando, con la IMU únicamente podemos llegar a disponer de un vector de medidaampliado respecto al anterior, gracias a la integración numérica, de la forma:

xIMU =

W tGW tGW ηGW ηG

Sin embargo, lo que a nosotros nos interesaría como vector de realimentación para el control es:

x =

W tBW tBW ηBW ηB

Para resolver esta deficiencia es para lo que introducimos un sistema de visión cuyo funciona-miento se explica en el siguiente apartado.

Aunque si se conociera el modelo geométrico del objeto, se podria usar: W xB =

Ot(W )B

O t(W )B

W ηBW ηB


3.3.3. Datos proporcionados por la cámara

Acabamos de ver que para obtener de la IMU la posición es necesario integrar dos veces laaceleración lineal, situación a evitar si es posible, ya que una doble integración podría introducirerrores en la medida. Para subsanar dicha circunstancia se incorpora una cámara al vehículo, paraque aporte una medida de posición respecto al objeto que se seguirá como referencia.

El origen del sistema de referencia inercial lo podemos situar en el origen del objeto, siendoequivalente para la traslación no para la rotación, por tanto la traslación de la cámara respecto alobjeto será como la traslación de la cámara respecto al sistema inercial, resultando entonces quetenemos Ct

(W )O . Pero nos hace falta la traslación del cuerpo no de la cámara, transformación que

veremos en el apartado de la fusión sensorial.

En la figura 3.11 se puede ver un esquema con los distintos sistemas de referencia que intervienen.

Figura 3.10: Esquema sistemas de referencia

3.3.4. Fusión sensorial y obtención vector medida

3.3.4.1. Principios de la fusión

El sistema de visión estará compuesto por una cámara acoplada rígidamente a la IMU y unobjeto apropiado para la tarea a realizar. El sistema de coordenadas ligado a la cámara se denomina{C}. Por su parte, el objeto visualizado por la cámara, en principio, consistirá en una plantilla decalibración de tipo tablero de ajedrez. El sistema de referencia ligado al mismo es {O}. En la figura3.11 aparecen, aparte de los elementos ya mencionados, la IMU con su sistema de coordenadaslocal, {G}, y el sistema de coordenadas inercial {W}, así como las transformaciones necesarias parapasar de uno a otro.


En general, la posición y orientación de un sistema cualquiera {D} respecto a otro {A}, podríaexpresarse mediante una matriz de transformación homogénea AHD:

AHD =

[

ARDAtD

0 1

]

Por ejemplo, la expresión obtenida en el apartado 3.2.2 para la cámara se puede representarcomo una única matriz cuadrada de dimensión 4, expresada en forma de coordenadas homogéneas:

[

CP1

]

= CHW

[

W P1

]

Figura 3.11: Sistemas de coordenadas involucrados y nomenclatura

En el caso particular de la posición y orientación de {G} respecto a {W}:

W HG =

[

W RGW tG

0 1

]

⇐[

W tGW ηG

]

Si bien la matriz de rotación de W RG se puede obtener sin ninguna dificultad a partir delvector W ηG, no podremos disponer de la matriz W HG completa, dado nuestro desconocimientode W tG. Esta es la deficiencia que se intenta paliar con la introducción del sistema de visión, cuyofuncionamiento se explica a continuación.

En el sistema de visión, se pondrá en marcha un método empleado para seguimiento tridi-mensional o, lo que es lo mismo, seguimiento en el espacio cartesiano. Con él se habilitarán unosalgoritmos que estiman la posición y orientación de la cámara respecto al sistema de referencialigado al objeto. Como premisas, es necesario que se haya resuelto fuera de línea el problema dela calibración interna de la cámara. Gracias a esta calibración, se dispondrá de una estimación delos parámetros intrínsecos de la cámara, tales como la distancia focal, la distorsión de la lente,


la dimensión del píxel, etc. Por otro lado, se necesita disponer del modelo geométrico del objetoempleado.

A este proceso de estimación de la posición y orientación de la cámara respecto a un objetoconocido se llama calibración externa. Como conclusión de este proceso, se puede obtener un valorde la matriz OHC en cada instante.

Por otro lado, se supone conocida la transformación existente entre los sistemas de coordenadas{C} y {G}: GHC . Esta matriz se habrá obtenido como parte de un proceso de calibración IMU-cámara que se realiza fuera de línea. Esta transformación será constante, una vez se haya establecidoel acoplamiento mecánico rígido entre la cámara y la IMU. Dada esta matriz, podemos obtener laposición y orientación de la IMU respecto al sistema de coordenadas del objeto:

OHG = OHC · GH−1C

De esta forma, tendríamos posicionada la IMU y, en consecuencia, el helicóptero, no en relaciónal sistema de coordenadas inercial, {W}, sino al sistema de coordenadas del objeto, {O}, que puedeser móvil por sí mismo.

3.3.4.2. Conversión de datos de la cámara e IMU

En estos momentos, tenemos cierta información que puede ser redundante y otra que es comple-mentaria. Por un lado, dado que asumimos que la orientación que proporciona la IMU por sí sola essuficientemente precisa, podemos extraer la parte del vector de medida correspondiente a W ηG(olo que es equivalente W RG). Por otra parte, dada la dificultad de obtener una medida directa deW tG, optamos por reemplazarla por OtG. Asimismo, realizaremos una estimación de la derivadade este último vector: O tG. Esto nos llevaría a un nuevo vector de medida:

O,W xG =

OtGO tG

W ηGW ηG

Que tiene como característica poco deseable que no todos sus elementos están referidos almismo sistema de coordenadas. Pese a que tal vez eso no supusiera ningún problema de facto,resulta más natural tener todos los componentes del vector de medida referidos al mismo sistemade coordenadas, {W}. Para ello, premultiplicamos tanto OtG como O tG por la matriz de rotaciónW RO:

Ot(W )G = W RO · OtG

O t(W )G = W RO · O tG

Resulta, por tanto, necesario disponer de la matriz W RO. Esta matriz puede obtenerse en cadamomento de la siguiente forma:

W RO = W RG · GRC · OR−1C


Por último, para disponer de los valores del centro de masas del cuerpo, se debe sumar ladistancia entre el centro de referencia de la IMU y el centro de pasas del objeto, cuyo valor se veráen la parte de acoplamiento:

Ot(W )B = Ot(W )

G + Gt(W )B

Para obtener el valor de la tripleta de ángulos que describa la rotación del cuerpo respectoal sistema inercial, W ηB, a partir de la rotación de la IMU, W ηG, con carácter general tenemosque obtener la matriz de rotación, W RG, luego calcular la matriz, W RB, y por último calcular latripleta, W ηB:

W ηG → W RG

W RB = W RGGRB

W RB → W ηB

Del apartado 3.3.2 tenemos la derivada de los ángulos en el sistema de referencia de la IMU y sedebe pasar al sistema de referencia del cuerpo, cumpliéndose además que W ω

(G)B = W ω

(G)G puesto

que no hay movimiento relativo entre la IMU y el cuerpo. Por lo que para cambiar de sistema dereferencia tendremos:

W ω(B)B = BRG

W ω(G)B

Y al final, sustituyendo en la expresión vista en 3.3.2, para obtener la derivada de los ángulostenemos:

W ηB = W −1(W ηB) W ω(B)B = W −1(W ηB) BRG

W ω(G)B

Con todo esto, estamos en condiciones de ofrecer el vector de medida definitivo:

W xB =

Ot(W )B

O t(W )B

W ηBW ηB

Desde el punto de vista del control, la única diferencia apreciable respecto al uso del vector demedida comentado al principio del apartado es que, aún manteniendo la referencia constante, si semueve el objeto, se moverá el helicóptero (si bien sin cambiar su orientación), para reubicarse enla misma posición relativa al objeto que fija la referencia.

3.3.4.3. Acoplamiento cámara e IMU

Se han ensayado dos alineaciones, una con la IMU girada 45º según el eje OZ o alineada con losejes del quadrotor. Se desechó la primera opción ya que la alineación con el sistema de referencia delcuerpo se hacía mediante software y los errores de la medición de la IMU se quedaban fluctuandoy acababan acumulando una pequeña deriva en el ángulo pitch.

GRB = Roz,+45º giro en el eje OZ de 45º en sentido positivo. Y GtB = [−28′51, −15′35, 22′6]T

expresada en mm.


Figura 3.12: Acoplamiento mecánico entre IMU y cámara

Caso actual, GRB = I y GtB = [0, 0, 22′6]T expresada en mm.

Figura 3.13: Acoplamiento mecánico

Herramienta para la simulacion integradas de subsistemas en un...

Documents

Transcript of Herramienta para la simulacion integradas de subsistemas en un...