Autor Tema: Sistema de navegacion para UAV's  (Leído 9435 veces)

0 Usuarios y 1 Visitante están viendo este tema.

Desconectado elgarbe

  • Moderador Local
  • PIC24H
  • *****
  • Mensajes: 2178
Sistema de navegacion para UAV's
« en: 05 de Enero de 2014, 11:44:22 »
Buenas, la idea de este hilo es compartir experiencias y resolver cuestiones sobre sistemas de navegacion, autopilotos, estaciones de tierrra, etc. para UAV's.
Tanto PCCM como yo estamos trabajando en esto y estaria bueno contar con un foro en español sobre este tema y mejor todavía, lleno de electronicos!!!!

Bueno, en breve empezaremos a publicar lo que tenemos hasta el momento.
Por otra parte voy a abrir un par de hilos más dedicados particularmente a sensores utilizados para este fin



Saludos!
« Última modificación: 08 de Enero de 2014, 21:27:52 por elgarbe »
-
Leonardo Garberoglio

Desconectado elgarbe

  • Moderador Local
  • PIC24H
  • *****
  • Mensajes: 2178
Re: Sistema de navegacion para UAV's
« Respuesta #1 en: 05 de Enero de 2014, 11:44:48 »
Una de las partes principales de los sistemas de navegacion de cualquier UAV (avion, multicoptero, autos, etc.) el sistema encargado de detectar la horientacion del UAV respecto de una referencia determinada. Generalmente la referencia tomada es La Tierra. Estos sistemas están cmpuestos por uno o varios sensores, entre los cuales se destacan los acelerómetros y gyróscopos. Estos pueden venir en unidades separadas o integrados en una sola placa. A dichas unidades se las conoce con el nombre de IMU (inertial measurement unit, unidad de medicion inercial). En my caso estoy trabajando con una IMU denominada por DealExtreme como GY-80. En concreto es esta:
GY-80

Aparte de la IMU es necesario algun sistema programable (uC) en el cual se realizarán los cálculos necesarios para, interpretando las mediciones de los sensores, obtener la orientacion del UAV. Hay varios "algoritmos" para realizar esto, 2 o 3 de ellos muy estudiados y utilizados. Estos son:
  • La fusion de los datos con filtro Kalman
  • Los ángulos de Euler (Pitch o cabeceo, Roll o giñada y Yaw o banqueo Roll o banqueo y Yaw o guiñada)
  • La matriz de cosenos directores
  • Los quaterniones
El primero y el tercero desconozco por completo, sé que se utilizan como estrategia para obtener la orientacion del UAV, pero nunca los estudie.


Los ángulo de euler son ampliamente conocido: banqueo, cabeceo, guiñada.
La ventaja de este sistema es que es muy intuitivo y la representación de la actitud solo contiene 3 parámetros.
pero su desventaja es que tiene una singularidad por ejemplo para un angulo de cabeceo de +-90 grados, sucede el "bloqueo de cardan" o "gimbal lock"
Donde los ejes de banqueo y guiñada coinciden, provocando que el banqueo y guiñada se comporten igual. en este video se puede apreciar mejor.


Los cuaterniones tampoco presentan singularidad además la representación de la actitud solo contiene 4 parámetros, pero también no es intuitiva.
Gracias PCCM

En mi caso estoy trabajando en la implementacion de la Matriz de Cosenos Directores (DCM) en un uC ARM cortex M3 (LPC1769).

Sobre los sensores de mi IMU iré publicando informacion en los sub foros de cada tipo de sensor. Aquí ire tratando de poner, en un comienzo, todo lo relacionado con la DCM y su implementacion en el ARM.

Bien comencemos con la piedra fundamental del los sistemas de navegacion, la DCM.
Como punto de partida, estos son las dos "Biblias de la DCM" con las que luche durante el último mes (pueden ver mis comentarios en el blog de Starlino...):
DCM por Starlino
DCM por William Premerlani

El primer link tiene mucha álgebra, esta muy bien explicado, con buenos gráficos. En el mismo blog podemos encontrar la implementacion de un sistema de control de un quad, hecho en base a un dsPic.
El segundo es de un groso total, arranco con ese "bosquejo" y hoy tiene un sistema de navegacion opensource super avanzado, MatrixPilot. Esta más pensado para aviones, pero es extensible a otros UAV's. menos álgebra que Starlino y quizás más fácil de leer.

Yo voy a intentar resumir un poco en español lo que de ellos aprendí.

Bueno, en el control de un UAV tenemos que tener presente que existen dos sistemas o marcos de referencia. Uno que podemos decir que es fijo y es con el que estamos acostumbrados a trabajar en física, La Tierra y otro, vinculado al cuerpo (body) del UAV, el cual se está rotando constantemente junto al UAV. Aquí cabe una aclaracion, la DCM está pensada para obtener la ORIENTACION, no la posicion del UAV. Debemos trabajar en como obtener la orientacion del sistema de referencia en el cuerpo del UAV visto desde el sistema de referencia de la tierra.

Notar que ambos marcos de referencia poseen el mismo origen O. En la imagen tambien se definen los vectores unidad (i, j, K) del marco del UAV así como I, J, K los vecotres unidad, codireccionales con los ejes X, Y y Z del marco de la Tierra.
La notacion vectorial en 3 dimensiones de estos versores (vectores con unidad 1) es I=[1 0 0], J=[0 1 0], k=[0 0 1]
El versor i en el marco de referencia global (la tierra) ig = [I.i, J.i, K.i] en este caso I.i es el producto escalar entre el versor I y el versor i. Como el modulo de I y de i es 1, tambien podemos escribirlo como ig=[cos(I,i), cos(J,i), cos(K,i)]
De igual modo podemos decir que el versor j escrito en el marco de referencia de la tierra será jg = [J.i, J.j, J.k] y kg = [K.i, K.j, K.k]
uniendo esto podemos escribir lo siguiente:

Esa matriz (denominada Matriz de Cosenos Directores) posee una gran importancia ya que permite transformar un punto escrito en el marco de referncia del UAV, al marco de referncia global. Aparte es la que describe la rotacion de un marco de referencia relativo al otro. En otras palabras, con dicha matriz podemos conocer la horientacion del UAV respecto de la Tierra.

Un vector arbitrario expresado en el sistema de coordenadas del UAV, B, puede ser escrito en el sistema de referencia de La Tierra, G, (el cual está rotado, girado) de la siguiente forma:


En este punto es conveniente echarle una mirada a Wikipedia - Matriz de rotaciones
ya que es un poco lo que hemos visto.
De ese artículo lo más interesante es esto:

Ahí se define una matriz de 3x3 (al igual que la DCM) con las rotaciones básicas, es decir las rotaciones alrededor de un solo eje.

Si tomamos los marcos de referencia, girados uno de otro, como en la siguiente figura:


la matriz de rotacion completa, combinando las tres rotaciones quedará así:


En teoría la matriz DCM se calcula computando todas esas funciones trigonométricas... por suerte alguien estudio un poco más el tema y buscó algunas simplificaciones.

Si bien la matriz DCM parece tener 9 parámetros, solo 3 son independientes. Esto es debido a que cada vector (fila) tiene longitud 1 y es perpendicular a las otras dos filas. Esto restringe a solo 3 parámetros independientes.
Otro punto importante es que si tenemos una matriz DCM1 que permite ir del punto A al punto B y tenemos otra matriz DCM2 que permite ir del punto B al punto C, entonces la matric DCM3 = DCM1 . DCM2. Esto es, se pueden componer rotaciones multiplicandolas unas a otras.

Lo que nos está faltando visualizar es como intervienen las mediciones de nuestros sensores en la construccion de la matríz DCM. Antes veamos unas propiedades de las operaciones con vectores.

producto escalar de vectores. El producto escalar (dot product) de los vectorea A y B, es un escalar calculado a partir de la multiplicacion matricial de los vectores:
A . B = [Ax Ay Az] . [Bx By Bz] = Ax.Bx + Ay.By + Az.Bz
También se puede escribir el producto escalar como
A . B = |A|.|B|.cos(AB)
esto es el producto entre las magnitudes (longitud) de los vetores y el coseno del ángulo entre ellos.

Producto vectorial de vectores. El producto vectorial (cross product) de losvectores Ay B, es un vector C dado por la siguiente forma:
Cx = Ay.Bz - Az.By
Cy = Az.Bx - Ax.Bz
Cz = Ax.By - Ay.Bx
El vector C será un vector perpendicular al plano formado por los vectores A y B.



Tambien podemos escribir: |C| = |A ^ B| = |A|.|B|.Sin(AB) Esto es, la longitud del vector C es igual al producto entre las longitudes del vector A, el B y el seno del ángulo entre ellos.

Teniendo en cuenta esto estamos en condiciones de incluir los datos obtenidos de los gyróscopos para el cálculo de la DCM.
Por cuestiones de orden, el funcionamiento de los giróscopos estará Aquí

« Última modificación: 13 de Enero de 2014, 21:59:20 por elgarbe »
-
Leonardo Garberoglio

Desconectado elgarbe

  • Moderador Local
  • PIC24H
  • *****
  • Mensajes: 2178
Re: Sistema de navegacion para UAV's
« Respuesta #2 en: 05 de Enero de 2014, 11:45:13 »
reservado
-
Leonardo Garberoglio

Desconectado PCCM

  • PIC16
  • ***
  • Mensajes: 109
Re: Sistema de navegacion para UAV's
« Respuesta #3 en: 06 de Enero de 2014, 03:37:38 »
Excelente que haya un subforo para los UAV.  :mrgreen:
Gracias por el aporte "elgarbe". Intentaré aportar lo poco que se para comprender y saber más sobre los sistemas de navegación para UAV's.

En la representación de la actitud puede haber una aclaración. "La fusión de los datos con filtro Kalman", no es una de ellas, con Kalman se puede usar para estimar estados y como dices "fusionar datos", ya que en este caso estima la deriva de un sensor a partir de otro u otros sensores para después corregirla.

Dentro de los más comunes son los ángulos de euler, DCM y cuaterniones.

Los ángulo de euler son ampliamente conocido: banqueo, cabeceo, guiñada.
La ventaja de este sistema es que es muy intuitivo y la representación de la actitud solo contiene 3 parámetros.
pero su desventaja es que tiene una singularidad por ejemplo para un angulo de cabeceo de +-90 grados, sucede el "bloqueo de cardan" o "gimbal lock"
Donde los ejes de banqueo y guiñada coinciden, provocando que el banqueo y guiñada se comporten igual. en este video se puede apreciar mejor.


La DCM como está explicando "elgarbe" no tiene singularidades, pero se podría decir que la desventaja es que no es intuitiva y su representación de la actitud contiene 9 parámetros.

Los cuaterniones tampoco presentan singularidad además la representación de la actitud solo contiene 4 parámetros, pero también no es intuitiva.

Por ello el camino para la representación de la actitud mayormente son por estos 2 últimos caminos: DCM y cuaterniones.


Desconectado PCCM

  • PIC16
  • ***
  • Mensajes: 109
Re: Sistema de navegacion para UAV's
« Respuesta #4 en: 06 de Enero de 2014, 04:11:47 »
Estoy siguiendo el link que pasaste sobre la DCM, analizando la forma como yo utilizaba la DCM me dí cuenta de errores que no me había percatado. Como el que dijiste que el procesamiento es mucho más lento. Ejecutando tu código es más rápido(simulado en matlab).

Además tengo problemas, ya que a mayor tiempo de simulación, me da valores raros. lo probaré en físico para ver si se comporta igual.
En cuanto al algoritmo que pusiste en el otro post, lo simulé en matlab, pero tengo dudas.

Coloqué como velocidad angular cabeceo: 5 grados/s y guiñada 2 grados/s, en la simulación que dura 9 segundos me debería dar 45 grados cabeceo y 18 grados guiñada.

Código: [Seleccionar]
clear;
clc;
imu_interval=0.02;
dcmEst=[1 0 0; 0 1 0; 0 0 1];
gyro=[0 5*0.017453293 2*0.017453293]*imu_interval;%banqueo, cabeceo,guinada
g=[1 -gyro(3) gyro(2); gyro(3) 1 -gyro(1); -gyro(2) gyro(1) 1];
graf(450,4)=zeros;
for n = 1:450
    dcmEst=dcmEst*g;
    error=-dot(dcmEst(1,:),dcmEst(2,:))*0.5;
    x_est=[0 0 0];
    y_est=[0 0 0];
    x_est = dcmEst(2,:) * error;
    y_est = dcmEst(1,:) * error;
    dcmEst(1,:) = dcmEst(1,:) + x_est;
    dcmEst(2,:) = dcmEst(2,:) + y_est;
    dcmEst(3,:) = cross(dcmEst(1,:), dcmEst(2,:));
    dcmEst(1,:)=dcmEst(1,:)/norm(dcmEst(1,:));
    dcmEst(2,:)=dcmEst(2,:)/norm(dcmEst(2,:));
    dcmEst(3,:)=dcmEst(3,:)/norm(dcmEst(3,:));
    graf(n,1)=n*imu_interval;
    graf(n,2)=dcmEst(2,1);%guinada
    graf(n,3)=dcmEst(1,3);%cabeceo
    graf(n,4)=dcmEst(3,2);%banqueo
    
end
figure
hold on
plot(graf(:,1),graf(:,2)*(180/pi),'+b');%guinada
plot(graf(:,1),graf(:,3)*(180/pi),'.r');%cabeceo
plot(graf(:,1),graf(:,4)*(180/pi),'.g');%banqueo
grid


pero lo que obtengo es lo siguiente:


Donde el rojo es cabeceo: 39.81 grados --- debe ser 45 grados
azul es guiñada: 15.93  ----- debe ser 18 grados
verde es banqueo: 6.67  ---- debe ser 0 grados

Estos datos son así?, cuando aumento más la velocidad angular, el error es mayor.
A que se debe esto o estoy interpretando mal.

Saludos.
« Última modificación: 06 de Enero de 2014, 04:23:51 por PCCM »

Desconectado elgarbe

  • Moderador Local
  • PIC24H
  • *****
  • Mensajes: 2178
Re: Sistema de navegacion para UAV's
« Respuesta #5 en: 06 de Enero de 2014, 07:33:59 »
En cuanto a la velocidad de giro, William dice:
"The only approximation that equation 17 makes is that the time step is short
enough so that the R matrix does not change much from step to step. A
typical time step is around 0.020 seconds, during which an aircraft rotating at
around 60 degrees per second rotates approximately 0.020 radians, which
translates to a maximum change in any of the R matrix coefficients of around
2%. Thus, the second order terms that are being ignored are on the order of
0.02%."
o sea que podrías trabajar con velocidades de 60° por seg.
El problema en tu caso es que ya no puedes extraer los angulos de euler directamente de cada elemento de la DCM, porque no tienes una rotacion básica en un eje, tienes una combinacion de rotaciones, entonces los datos en cada celda de la DCM es una mezcla de cada ángulo. Para sacar los ángulos hay un comando en matlab, creo que es dcm2euler... para extraer los angulos de euler desde la DCM en el micro hay que hacer unos calculos y algunas salvedades... despues busco un paper donde estaba explicado y lo publico....

Saludos y me alegro que valla sirviendo!
PD:ahora corrigo lo que comentaste.
-
Leonardo Garberoglio

Desconectado elgarbe

  • Moderador Local
  • PIC24H
  • *****
  • Mensajes: 2178
Re: Sistema de navegacion para UAV's
« Respuesta #6 en: 07 de Enero de 2014, 21:03:22 »
Usar los gyroscopos para actualizar la DCM

Esta es quizá la parte más complicada, o la que requiere más matemáticas. Por ahora vamos a exponer las formulas finales y la explicacion de como se actualiza y mantiene la DCM.
Por si no ha quedado claro hasta aquí, la DCM es la matriz que nos dice la orientacion actual del UAV y es la que nos permite pasar de un marco de referencia (el del UAV, por ejemplo) al otro (el de la tierra). La DCM se debe actualizar constantemente con los datos de los sensores.

La ecuacion más importante es  la siguiente:


Esta ecuacion nos da la forma de atualizar la DCM usando la señales de los gyros (wx, wy, wz).
Cuando el UAV esta quieto en la tierra, previo al despegue, la DCM es la matriz identidad, ya que los ejes X, Y y Z del UAV coinciden con los ejes X, Y y Z de La Tierra. A partir de alli, cada vez que obtengamos velocidad angular de los gyros y la multipliquemos por el tiempo de actualizacion obtendremos el pequeño ángulo que ha girado en cada eje el UAV. Armaremos la Matriz que vemos en la figura anterior y al multiplicarla por la DCM actual, obtendremos la nueva DCM.

Si bien todo lo expuesto hasta aquí es bastante complicado al principio, despues de varias lecturas y repasando los paper sugeridos verán que no es nada dificil. Lo mismo podrán ver cuando veamos la implementacion.

Esta forma de calcular la DCM introduce pequeños errores, que se van acumulando. Los errores más comunes son de quantizacion y debido al tiempo de muestreo. nuestro uC representará los números decimales con un número fijo de bytes (float o double), lo cual introduce pequeños errores de redondeo. por otra parte, para calcular el ángulo que ha girado el UAV lo hacemos con un tiempo finito (20mseg, por ejemplo) y en la forma de integrar suponemos que entre muestra y muestra el giro del UAV es lineal. Es necesario "reacondicionar" la DCM constante para cancelar esos pequeños errores.
La DCM, por definicion tiene 2 restricciones. Cada vector fila es perpendicular a los otros 2 vectores filas. Cada vector Fila tiene una longitud igual a 1.
Por lo tanto el proceso de acondicionamiento consiste en hacer los vecotres perpendiculares entre sí y hacerlos de longitud 1.
Como vimos el producto escalar entre dos vectores unitarios es el coseno del ángulo entre ellos. Si los vectores son perpendiculares el cos(90)=0. Por lo tanto podemos decir que el producto escalar es una medida de cuán lejos están los vectores de ser perpendiculares. Podemos computar el error de perpendicularidad, entonces, como
error = X . Y
Luego, podemos repartir ese error entre cada uno de los vectores, quedando:
Xortogonal = X - error/2 * Y
Yortogonal = Y - error/2  * X
Luego debemos hallar Z ortogonal con X e Y. Para ellos simplemente multiplicamos vectorialmente X e Y y obtenemos el vector Z.

Con estos tres pasos "ortogonalizamos" la matriz. El paso siguiente es normalizarla, para asegurar que cada vector fila tenga longitud 1.
La normalizacion de un vector suele calcularse de la siguiente manera.
Si partimos de la premisa que la longitud de cada vector no estará muy lejos de 1, podemos simplificar el cálculo usando una serie de Taylor para simplificar el cálculo y ganar velocidad. La ecuacion a utilizar es la siguiente:


Bien, siguiendo estos pasos, podemos mantener actualizada la matriz DCM. Ellos son:
  • Obtener las velocidades angulares de los gyroscopos
  • Multiplicarlos por el tiempo entre actualizaciones (del orden de los 20mseg)
  • Armar la Matriz de rotacion con esos datos
  • Multiplicar la Matriz de rotacion por la DCM
  • Ortogonalizar la Matriz
  • Normalizar la Matriz

Bueno, veamos ahora un pequeño ejemplo en matlab de esta implementacion.

« Última modificación: 07 de Enero de 2014, 22:58:57 por elgarbe »
-
Leonardo Garberoglio

Desconectado elgarbe

  • Moderador Local
  • PIC24H
  • *****
  • Mensajes: 2178
Re: Sistema de navegacion para UAV's
« Respuesta #7 en: 08 de Enero de 2014, 23:18:09 »
Implementacion en Matlab

Bueno, este es el código básico para el testeo de la DCM.

Código: [Seleccionar]
clear;
clc;
imu_interval=0.02;
%La posicion inicial en reposo, los marcos de referencia coincide, por lo
%que la DCM inicial es la matriz identidad
dcmEst=[1 0 0; 0 1 0; 0 0 1];
wx=1;   %Velocidad angular en °/seg en cada eje
wy=2;
wz=3;
W=[wx wy wz]*pi/180;    %Vector velocidad angular en rad/seg
Theta=W*imu_interval;   %Multiplicamos por el tiempo de muestreo para obtener ángulo
%Creamos la matriz de rotacion actual
g=[1 -Theta(3) Theta(2); Theta(3) 1 -Theta(1); -Theta(2) Theta(1) 1];
graf(500,4)=zeros;      %Array para gráfico
for n = 1:500          %50 pasos * 0,02seg = 10 segundos
    %Actualizamos la DCM girándola de acuerdo al array g
    dcmEst=dcmEst*g;
    %Tenemos que hacerla orthogonal nuevamente.
    %Calculamos el error de ortoganalidad
    error=-dot(dcmEst(1,:),dcmEst(2,:))*0.5;
    %Repartimos el error escalando el vetor X e Y de la DCM
    x_est = dcmEst(2,:) * error;
    y_est = dcmEst(1,:) * error;
    dcmEst(1,:) = dcmEst(1,:) + x_est;
    dcmEst(2,:) = dcmEst(2,:) + y_est;
    %Para obtener Z ortogonal, hacemos producto vectorial entre X e Y
    dcmEst(3,:) = cross(dcmEst(1,:), dcmEst(2,:));
    %Ahora hay que renormalizar cada vector fila de la DCM
    dcmEst(1,:)=0.5*(3-dot(dcmEst(1,:),dcmEst(1,:))) * dcmEst(1,:);
    dcmEst(2,:)=0.5*(3-dot(dcmEst(2,:),dcmEst(2,:))) * dcmEst(2,:);
    dcmEst(3,:)=0.5*(3-dot(dcmEst(3,:),dcmEst(3,:))) * dcmEst(3,:);

    graf(n,1)=n*imu_interval;
    graf(n,2)=atan2(dcmEst(3,2),dcmEst(3,3));    %guinada
    graf(n,3)=-asin(dcmEst(3,1));               %cabeceo
    graf(n,4)=atan2(dcmEst(2,1),dcmEst(1,1));    %banqueo
    
end
figure
hold on
plot(graf(:,1),graf(:,2)*(180/pi),'+b');%guinada
plot(graf(:,1),graf(:,3)*(180/pi),'.r');%cabeceo
plot(graf(:,1),graf(:,4)*(180/pi),'.g');%banqueo
grid



En este momento está funcionando sin problemas con rotaciones individuales. Cuando combinamos rotaciones en los tres ejes no estoy obteniendo los resultados esperados. Estoy investigando ese tema....
Parece que los ángulos de Euler tienen sus particularidades y no se llevan bien con la DCM. El tema es que para llevar bien los ángulos de Euler usando la DCM, hay que hacer rotaciones individuales y en orden. Por lo tanto los resultados obtenidos son correctos, pero aplicar rotaciones en cada eje en simultaneo no es lo mismo que aplicar rotaciones individuales y en cierto orden, ver comentario 83 y 84.

Habría que trabajar en crear una GUI que permita visualizar un cubo que rote con los valores leidos de la DCM....

Saludos!
« Última modificación: 11 de Enero de 2014, 00:05:45 por elgarbe »
-
Leonardo Garberoglio

Desconectado PCCM

  • PIC16
  • ***
  • Mensajes: 109
Re: Sistema de navegacion para UAV's
« Respuesta #8 en: 09 de Enero de 2014, 05:12:26 »
Tienes razón estaba mal interpretando la salida, pensaba que la matriz resultante("dcmEst") era diferenciales del angulo. y lo que es en si la DCM modificada en cada diferencial de ángulo.

Hice las pruebas para un angulo inicial de banqueo de 90 grados. A partir de ello hice que en los 10 segundos el cabeceo girara 20 grados, lo cual el cabeceo en el eje del cuerpo se comportaría como guiñada en el eje de la tierra(lo cual seria -20 grados de guiñada en el eje de la tierra). Lo cual se pudo ver que si cumplía la rotación.

Código: [Seleccionar]

clear;
clc;
%%velocidad de muestreo
imu_interval=0.02;
%%%Colocar en una posicion inicial
banqueoT=90*(pi/180);
cabeceoT=0*(pi/180);
guinadaT=0*(pi/180);
dcmEst=angle2dcm(banqueoT,cabeceoT,guinadaT,'XYZ');%banqueo, cabeceo,guinada
%%%
graf(500,4)=zeros;
x_est=[0 0 0];
y_est=[0 0 0];

%%%dura 10 segundos
for n = 1:500
    %Sensado del giroscopo grados/s
    vel_gyro=[0 2 0];%banqueo,cabeceo,guinada
    vel_gyro=vel_gyro*(pi/180);
    %%diferencial de angulo
    gyro=vel_gyro*imu_interval;
    %%matriz dcm diferencial
    g=[1 -gyro(3) gyro(2); gyro(3) 1 -gyro(1); -gyro(2) gyro(1) 1];
    %%matriz dcm sumada a la diferencial
    dcmEst=dcmEst*g;
    error=-dot(dcmEst(1,:),dcmEst(2,:))*0.5;
    x_est = dcmEst(2,:) * error;
    y_est = dcmEst(1,:) * error;
    dcmEst(1,:) = dcmEst(1,:) + x_est;
    dcmEst(2,:) = dcmEst(2,:) + y_est;
    dcmEst(3,:) = cross(dcmEst(1,:), dcmEst(2,:));
    dcmEst(1,:)=dcmEst(1,:)/norm(dcmEst(1,:));
    dcmEst(2,:)=dcmEst(2,:)/norm(dcmEst(2,:));
    dcmEst(3,:)=dcmEst(3,:)/norm(dcmEst(3,:));
    graf(n,1)=n*imu_interval;
   
    %%una vez calculada la DCM,Se calcula la velocidad angular respecto a tierra
    veloc_g=dcmEst*(vel_gyro');
    %%integro para hallar el angulo respecto a tierra
    banqueoT=banqueoT+veloc_g(1,1)*imu_interval;%banqueo
    cabeceoT=cabeceoT+veloc_g(2,1)*imu_interval;%cabeceo
    guinadaT=guinadaT+veloc_g(3,1)*imu_interval;%guinada
   
    almacen_b(n)=banqueoT;
    almacen_c(n)=cabeceoT;
    almacen_g(n)=guinadaT;
   
end

banqueoT=banqueoT*(180/pi)
cabeceoT=cabeceoT*(180/pi)
guinadaT=guinadaT*(180/pi)

hold on
plot(graf(:,1),almacen_g*(180/pi),'+b');%guinada
plot(graf(:,1),almacen_c*(180/pi),'.r');%cabeceo
plot(graf(:,1),almacen_b*(180/pi),'.g');%banqueo
grid




Intento probarlo en el microcontrolador, pero por ahora solo me arroja NaN.
Pero aquí dejo la modificación del código de Matlab para colocarlo en un microcontrolador, por si se puede reducir.

Código: [Seleccionar]

   
clear;
clc;
imu_interval=0.02;
%dcmEst=[1 0 0; 0 1 0; 0 0 1];
dcmEst1=1;dcmEst2=0;dcmEst3=0;dcmEst4=0;dcmEst5=1;dcmEst6=0;dcmEst7=0;dcmEst8=0;dcmEst9=1;
graf(500,4)=zeros;
banqueoT=0*(pi/180);
cabeceoT=0*(pi/180);
guinadaT=0;   
%x_est=[0 0 0];
x_est1=0;x_est2=0;x_est3=0;
%y_est=[0 0 0];
y_est1=0;y_est2=0;y_est3=0;
 
for n = 1:500
    %%velocidad del giroscopo
    vel_gyro1=0;vel_gyro2=5;vel_gyro3=5;%banqueo, cabeceo,guinada
    %%a radianes
    vel_gyro1=vel_gyro1*(pi/180);vel_gyro2=vel_gyro2*(pi/180);vel_gyro3=vel_gyro3*(pi/180);
    %gyro=[0 5*0.017453293 5*0.017453293]*imu_interval;%banqueo, cabeceo,guinada
    gyro1=vel_gyro1*imu_interval;gyro2=vel_gyro2*imu_interval;gyro3=vel_gyro3*imu_interval;
    %g=[1 -gyro(3) gyro(2); gyro(3) 1 -gyro(1); -gyro(2) gyro(1) 1];
    g1=1;g2=-gyro3;g3=gyro2;g4=gyro3;g5=1;g6=-gyro1;g7=-gyro2;g8=gyro1;g9=1;
   
    %%%previo al siguiente algoritmo
    dcm1=dcmEst1;dcm2=dcmEst2;dcm3=dcmEst3;dcm4=dcmEst4;dcm5=dcmEst5;
    dcm6=dcmEst6;dcm7=dcmEst7;dcm8=dcmEst8;dcm9=dcmEst9;
    %%%%  dcmEst=dcmEst*g;
    dcmEst1=dcm1+dcm2*g4+dcm3*g7;
    dcmEst2=dcm1*g2+dcm2+dcm3*g8;
    dcmEst3=dcm1*g3+dcm2*g6+dcm3;
    dcmEst4=dcm4+dcm5*g4+dcm6*g7;
    dcmEst5=dcm4*g2+dcm5+dcm6*g8;
    dcmEst6=dcm4*g3+dcm5*g6+dcm6;
    dcmEst7=dcm7+dcm8*g4+dcm9*g7;
    dcmEst8=dcm7*g2+dcm8+dcm9*g8;
    dcmEst9=dcm7*g3+dcm8*g6+dcm9;
   
    %error=-dot(dcmEst(1,:),dcmEst(2,:))*0.5;
    error=-0.5*(dcmEst1*dcmEst4 + dcmEst2*dcmEst5 +dcmEst3*dcmEst6);

    %x_est = dcmEst(2,:) * error;
    %y_est = dcmEst(1,:) * error;
    x_est1=dcmEst4*error;x_est2=dcmEst5*error;x_est3=dcmEst6*error;
    y_est1=dcmEst1*error;y_est2=dcmEst2*error;y_est3=dcmEst3*error;
   
    %%%previo al siguiente algoritmo
    dcm1=dcmEst1;dcm2=dcmEst2;dcm3=dcmEst3;dcm4=dcmEst4;dcm5=dcmEst5;
    dcm6=dcmEst6;dcm7=dcmEst7;dcm8=dcmEst8;dcm9=dcmEst9;
   
    %dcmEst(1,:) = dcmEst(1,:) + x_est;
    dcmEst1=dcm1+x_est1;dcmEst2=dcm2+x_est2;dcmEst3=dcm3+x_est3;
   
    %dcmEst(2,:) = dcmEst(2,:) + y_est;
    dcmEst4=dcm4+y_est1;dcmEst5=dcm5+y_est2;dcmEst6=dcm6+y_est3;
   
    %dcmEst(3,:) = cross(dcmEst(1,:), dcmEst(2,:));
    dcmEst7=(dcm2*dcm6)-(dcm5*dcm3);
    dcmEst8=(dcm4*dcm3)-(dcm6*dcm1);
    dcmEst9=(dcm1*dcm5)-(dcm4*dcm2);
   
    %dcmEst(1,:)=dcmEst(1,:)/norm(dcmEst(1,:));
    %dcmEst(2,:)=dcmEst(2,:)/norm(dcmEst(2,:));
    %dcmEst(3,:)=dcmEst(3,:)/norm(dcmEst(3,:));
    norm1=sqrt(dcmEst1*dcmEst1+dcmEst2*dcmEst2+dcmEst3*dcmEst3);
    norm2=sqrt(dcmEst4*dcmEst4+dcmEst5*dcmEst5+dcmEst6*dcmEst6);
    norm3=sqrt(dcmEst7*dcmEst7+dcmEst8*dcmEst8+dcmEst9*dcmEst9);
   
    dcmEst1=dcmEst1/norm1;dcmEst2=dcmEst2/norm1;dcmEst3=dcmEst3/norm1;
    dcmEst4=dcmEst4/norm2;dcmEst5=dcmEst5/norm2;dcmEst6=dcmEst6/norm2;
    dcmEst7=dcmEst7/norm3;dcmEst8=dcmEst8/norm3;dcmEst9=dcmEst9/norm3;

    %veloc_g=dcmEst*(vel_gyro');
    veloc_g1=dcmEst1*vel_gyro1+dcmEst2*vel_gyro2+dcmEst3*vel_gyro3;
    veloc_g2=dcmEst4*vel_gyro1+dcmEst5*vel_gyro2+dcmEst6*vel_gyro3;
    veloc_g3=dcmEst7*vel_gyro1+dcmEst8*vel_gyro2+dcmEst9*vel_gyro3;

    banqueoT=banqueoT+veloc_g1*imu_interval;%banqueo
    cabeceoT=cabeceoT+veloc_g2*imu_interval;%cabeceo
    guinadaT=guinadaT+veloc_g3*imu_interval;%guinada
   
    almacen_b(n)=banqueoT;
    almacen_c(n)=cabeceoT;
    almacen_g(n)=guinadaT;
   
    graf(n,1)=n*imu_interval;%%solo sirve para simular en matlab
 
end

 guinadaT*(180/pi)
 cabeceoT*(180/pi)
 banqueoT*(180/pi)

hold on
plot(graf(:,1),almacen_g*(180/pi),'+b');%guinada
plot(graf(:,1),almacen_c*(180/pi),'.r');%cabeceo
plot(graf(:,1),almacen_b*(180/pi),'.g');%banqueo
grid



"elgarbe" tienes una pequeña palabra que esta al revés:
"Los ángulos de Euler (Pitch o cabeceo, Roll o giñada y Yaw o banqueo)"
es "Roll o banqueo" y "Yaw o guiñada"

Saludos

Desconectado elgarbe

  • Moderador Local
  • PIC24H
  • *****
  • Mensajes: 2178
Re: Sistema de navegacion para UAV's
« Respuesta #9 en: 13 de Enero de 2014, 22:03:41 »
"elgarbe" tienes una pequeña palabra que esta al revés:
"Los ángulos de Euler (Pitch o cabeceo, Roll o giñada y Yaw o banqueo)"
es "Roll o banqueo" y "Yaw o guiñada"

Saludos

Corregido. Gracias!

Bueno, mientras terminamos la parte de giróscopos para armar el primer código para uC de la DCM solo con gyros, un pequeño video sobre la fusion de datos en sensores MEMS, es una charla de google:


-
Leonardo Garberoglio

Desconectado elgarbe

  • Moderador Local
  • PIC24H
  • *****
  • Mensajes: 2178
Re: Sistema de navegacion para UAV's
« Respuesta #10 en: 30 de Enero de 2014, 00:15:32 »
Bueno, llegó la hora!!!!! Ya tenemos el gyróscopo funcionando, el acelerómetro y el magnetómetro tambien.
El paso siguiente es utilizar la informacion de los sensores para mantener la matriz DCM.
Recordemos que la matriz DCM o de cosenos directores posee la orientación actual del UAV respecto de la tierra. Se va construyendo partiendo de la matriz identidad y se va actualizando con las pequeñas rotaciones que se van detectando con los sensores.

Bien, como primer medida crearemos el código necesario para calcular la DCM solamente con los giróscopos y luego iremos mejorando el código.
Aclaro que el código que estoy utilizando es un compendio de varios proyectos que fui recopilando en la web y fui adaptando para mi uC, el LPC1769. El proyecto del cual saqué mas cosas es: https://code.google.com/p/picquadcontroller/source/browse/trunk/

Bien, el código mínimo para mantener la matriz DCM tiene un tamaño considerable, por lo que he separado en distintos archivos el código.

Veamos primero algunos archivos de funciones generales necesarias:

macroutil.h
Código: [Seleccionar]
#ifndef MACROUTIL_H
#define MACROUTIL_H
//---------------------------------------------------------------------
// UTIL MACROS
//---------------------------------------------------------------------
#include<math.h>

#define MIN(A,B)  (((A)<(B)) ? (A) : (B) )
#define MAX(A,B)  (((A)>(B)) ? (A) : (B) )

#define PI  3.141592653589793238462643383279502884197f

double squared(float x){
return x*x;
}


double atan2deg(float y, float x){
if(fabs(y) < 0.1 && fabs(x) < 0.1)
return 0.0; //avoid atan2 returning meaningless numbers
else
return atan2(y,x)*180.0 / PI;
}

float low_pass_filter(float vNew,float vPrev,float factor){
return (vPrev*factor + vNew) / ( 1 + factor);
}

short make_word(unsigned char HB, unsigned char LB){
   return ((HB << 8) | LB);
}

void timer0_init(){
// Inicializamos el TIMER 0
LPC_TIM0->MR0 = TIMER0_INTERVAL; //Match register 0
LPC_TIM0->MCR = 3; //Interrumpe y resetea el contador cuando TC alcanza MR0
NVIC_EnableIRQ(TIMER0_IRQn); //Macro en core_cm3 para habilitar una interrupcion
//TIMER0_IRQn sale del LPC17xx.h y es el número de la int del timer
// Deshabilitamos el Timer 0
LPC_TIM1->TCR = 0;
}
void timer0_enable(){
// Habilitamos el Timer 0
LPC_TIM0->TC=0;
LPC_TIM0->TCR = 1;
}
void print_dcm(float vector[3][3]){
uart2_printDouble(vector[0][0], 3);
UART2_Sendchar(',');
uart2_printDouble(vector[0][1], 3);
UART2_Sendchar(',');
uart2_printDouble(vector[0][2], 3);
UART2_Sendchar(',');
uart2_printDouble(vector[1][0], 3);
UART2_Sendchar(',');
uart2_printDouble(vector[1][1], 3);
UART2_Sendchar(',');
uart2_printDouble(vector[1][2], 3);
UART2_Sendchar(',');
uart2_printDouble(vector[2][0], 3);
UART2_Sendchar(',');
uart2_printDouble(vector[2][1], 3);
UART2_Sendchar(',');
uart2_printDouble(vector[2][2], 3);
UART2_Sendchar(',');
uart2_printDouble(imu_interval_ms*1000, 3);
UART2_PrintString ("\r\n");
}
#endif

aqui tenemos algunas macros para ahorrar líneas de código y algunas funciones generales que se usan en varias funciones. Incluso tengo la inicializacion del timer 0, el cual en principio es usado para hacer tareas repetitivas en el tiempo. La última función es la que me permite ver en el terminal los valores de la DCM, así como el tiempo de muestreo de la misma.

vector3d.h
Código: [Seleccionar]
#ifndef VECTOR3D__H
#define VECTOR3D__H

#include <math.h>

//get modulus of a 3d vector sqrt(x^2+y^2+y^2)
float vector3d_modulus(float* vector){
static float R;
R = vector[0]*vector[0];
R += vector[1]*vector[1];
R += vector[2]*vector[2];
return sqrt(R);
}

//convert vector to a vector with same direction and modulus 1
void vector3d_normalize(float* vector){
static float R;
R = vector3d_modulus(vector);
vector[0] /= R;
vector[1] /= R;
vector[2] /= R;
}

//calcuate vector dot-product  c = a . b
float vector3d_dot(float* a,float* b){
return a[0]*b[0]+a[1]*b[1]+a[2]*b[2];
}


//calcuate vector cross-product  c = a x b
void vector3d_cross(float a[3],float b[3], float c[3]){
c[0] = a[1]*b[2] - a[2]*b[1];
c[1] = a[2]*b[0] - a[0]*b[2];
c[2] = a[0]*b[1] - a[1]*b[0];
}

//calcuate vector scalar-product  n = s x a
void vector3d_scale(float s, float* a , float* b){
b[0] = s*a[0];
b[1] = s*a[1];
b[2] = s*a[2];
}


//calcuate vector sum   c = a + b
void vector3d_add(float* a , float* b, float* c){
c[0] = a[0] + b[0];
c[1] = a[1] + b[1];
c[2] = a[2] + b[2];
}


//creates equivalent skew symetric matrix plus identity
//for v = {x,y,z} returns
// m = {{1,-z,y}
//     {z,1,-x}
//     {-y,x,1}}
void vector3d_skew_plus_identity(float *v,float* m){
m[0*3+0]=1;
m[0*3+1]=-v[2];
m[0*3+2]=v[1];
m[1*3+0]=v[2];
m[1*3+1]=1;
m[1*3+2]=-v[0];
m[2*3+0]=-v[1];
m[2*3+1]=v[0];
m[2*3+2]=1;
}

#endif

Aquí tenemos funciones de manejo de vectores en 3 dimensiones. Tenemos el cálculo de la longitud del vector, la normalizacion, el producto escalar y vectorial, el producto por un escalar, la suma de vectores y otras. Ya que la obtension de la orientacion del UAV es en 3 dimensiones, se trabaja mucho en 3d y es bueno reunir las funciones en un lugar.

L3G4200D.h
Código: [Seleccionar]
#ifndef L3G4200D_H_
#define L3G4200D_H_

#include "i2c.h"

extern volatile uint8_t I2CMasterBuffer[I2C_PORT_NUM][BUFSIZE];
extern volatile uint8_t I2CSlaveBuffer[I2C_PORT_NUM][BUFSIZE];
extern volatile uint32_t I2CReadLength[I2C_PORT_NUM];
extern volatile uint32_t I2CWriteLength[I2C_PORT_NUM];

#define L3G4200D_READ_ADDR      0xD3
#define L3G4200D_WRITE_ADDR     0xD2
#define L3G4200D_DATA_ADDR 0x28

#define L3G4200D_WHO_AM_I      0x0F

#define L3G4200D_CTRL_REG1     0x20
#define L3G4200D_CTRL_REG2     0x21
#define L3G4200D_CTRL_REG3     0x22
#define L3G4200D_CTRL_REG4     0x23
#define L3G4200D_CTRL_REG5     0x24
#define L3G4200D_REFERENCE     0x25
#define L3G4200D_OUT_TEMP      0x26
#define L3G4200D_STATUS_REG    0x27

#define L3G4200D_OUT_X_L       0x28
#define L3G4200D_OUT_X_H       0x29
#define L3G4200D_OUT_Y_L       0x2A
#define L3G4200D_OUT_Y_H       0x2B
#define L3G4200D_OUT_Z_L       0x2C
#define L3G4200D_OUT_Z_H       0x2D

#define L3G4200D_FIFO_CTRL_REG 0x2E
#define L3G4200D_FIFO_SRC_REG  0x2F

#define L3G4200D_INT1_CFG      0x30
#define L3G4200D_INT1_SRC      0x31
#define L3G4200D_INT1_THS_XH   0x32
#define L3G4200D_INT1_THS_XL   0x33
#define L3G4200D_INT1_THS_YH   0x34
#define L3G4200D_INT1_THS_YL   0x35
#define L3G4200D_INT1_THS_ZH   0x36
#define L3G4200D_INT1_THS_ZL   0x37
#define L3G4200D_INT1_DURATION 0x38

#define PORT_USED 1

void L3G4200D_init();
unsigned char L3G4200D_read(unsigned char reg);
unsigned char L3G4200D_write(unsigned char reg_address, unsigned char value);
//short make_word(unsigned char HB, unsigned char LB);
char L3G4200D_read_data();


void L3G4200D_init(){

L3G4200D_write(L3G4200D_CTRL_REG1, 0b00001111); // 100Hz, 12.5 CO, all axis enable
L3G4200D_write(L3G4200D_CTRL_REG4, 0x20); // 0x00 250 dps, 0x10 500 dps, 0x20 2000dps
}

unsigned char L3G4200D_read(unsigned char reg){
I2CWriteLength[PORT_USED] = 2;
I2CReadLength[PORT_USED] = 1;
I2CMasterBuffer[PORT_USED][0] = L3G4200D_WRITE_ADDR;
I2CMasterBuffer[PORT_USED][1] = reg;
I2CMasterBuffer[PORT_USED][2] = L3G4200D_READ_ADDR;

I2CEngine( PORT_USED );

return(I2CSlaveBuffer[PORT_USED][0]);
}

unsigned char L3G4200D_write(unsigned char reg_address, unsigned char value){
I2CWriteLength[PORT_USED] = 3;
I2CReadLength[PORT_USED] = 0;
I2CMasterBuffer[PORT_USED][0] = L3G4200D_WRITE_ADDR;
I2CMasterBuffer[PORT_USED][1] = reg_address;
I2CMasterBuffer[PORT_USED][2] = value;

return(I2CEngine( PORT_USED ));
}



char L3G4200D_read_data(){

char result=0;
I2CWriteLength[PORT_USED] = 2;
I2CReadLength[PORT_USED] = 6;
I2CMasterBuffer[PORT_USED][0] = L3G4200D_WRITE_ADDR;
I2CMasterBuffer[PORT_USED][1] = (L3G4200D_DATA_ADDR | (1<<7));
I2CMasterBuffer[PORT_USED][2] = L3G4200D_READ_ADDR;

result=I2CEngine( PORT_USED );

adcAvg[3] = make_word(I2CSlaveBuffer[PORT_USED][1], I2CSlaveBuffer[PORT_USED][0]);
adcAvg[4] = make_word(I2CSlaveBuffer[PORT_USED][3], I2CSlaveBuffer[PORT_USED][2]);
adcAvg[5] = make_word(I2CSlaveBuffer[PORT_USED][5], I2CSlaveBuffer[PORT_USED][4]);

return(result);
}

void L3G4200D_GetBiass(void){

int i;
for (i = 0; i < SAMPLESS_BIASS; i += 1) {
L3G4200D_read_data();
biass_X += adcAvg[3];
biass_Y += adcAvg[4];
biass_Z += adcAvg[5];
delay_ms(1);
}
biass_X /= SAMPLESS_BIASS;
biass_Y /= SAMPLESS_BIASS;
biass_Z /= SAMPLESS_BIASS;
}


#endif /* L3G4200D_H_ */

Este drive es muy parecido al que ya estaba usando, la única diferencia es que los datos leídos por la funcion read_data se almacena en un vector (adcAvg) de 6 posiciones. Las primeras 3 posiciones serán para el acelerómetro y las últimas 3 son para el gyro.

config.h
Código: [Seleccionar]
#ifndef CONFIG_H
#define CONFIG_H

struct {
unsigned char accInv[3];        // invert accl input (for example due to physical mounting position)
double accOffs[3];              // accl output at 0g in ADC units
double accSens[3];              // accl input sensitivity in ADC/g

unsigned char gyroInv[3];       // invert gyro input (for example due to physical mounting position)
double gyroOffs[3];             // gyro zero rate output in ADC  @ 0 deg/s;
double gyroSens[3];             // gyro input sensitivity ADC/(deg/ms)

} config;

void config_default(){
config.gyroOffs[0] = 0;
config.gyroOffs[1] = 0;
config.gyroOffs[2] = 0;

config.gyroSens[0] = GYRO_X_SCALE * PI/180;
config.gyroSens[1] = GYRO_Y_SCALE * PI/180;
config.gyroSens[2] = GYRO_Z_SCALE * PI/180;

config.accInv[0] = 0;
config.accInv[1] = 0;
config.accInv[2] = 0;

config.gyroInv[0] = 0;
config.gyroInv[1] = 0;
config.gyroInv[2] = 0;
}
#endif

Este archivo está pensado para guardar y cargar las configuraciones de los sensores, de los PID, etc. Por ahora solo tenemos la inicializacion del offset, sensivilidad e inversion de sensor. el miembro gyroInv
  • se utiliza para definir si un eje de un sensor hay que invertirlo. Esto es necesario para hacer coincidir los ejes del acelerómetro con el gyróscopo.


calibrate.h
Código: [Seleccionar]
#ifndef CALIBRATE_H
#define CALIBRATE_H

/*********** GYRO CALIBRATION ********/
void calibrate_gyro(){
L3G4200D_GetBiass();
config.gyroOffs[0] = biass_X;
config.gyroOffs[1] = biass_Y;
config.gyroOffs[2] = biass_Z;
}

#endif

Este archivo debe crecer notablemente ya que aquí deberíamos poner las rutinas de calibracion de los sensores. Si hay valores fijos que se obtienen en el labratorio, deberían grabarce en la FLASH y levantarce aquí. Por ahora solo calibramos el offset del gyro.

imu.h
Código: [Seleccionar]
#ifndef IMU__H
#define IMU__H

/*
How to use this module in other projects.

Input variables are:
adcAvg[0..5]  ADC readings of 3 axis accelerometer and 3 axis gyroscope (they are calculated in the background by adcutil.h)
interval_us - interval in microseconds since last call to imu_update

Output variables are:
DcmEst[0..2] which are the direction cosine of the X,Y,Z axis

First you must initialize the module with:
imu_init();

Then call periodically every 5-20ms:
imu_update();
it is assumed that you also update periodicall the adcAvg[0..5] array

*/

//-------------------------------------------------------------------
// Globals
//-------------------------------------------------------------------

unsigned int imu_sequence = 0;                          //incremented on each call to imu_update
float dcmEst[3][3] = {{1,0,0},{0,1,0},{0,0,1}};         //estimated DCM matrix

//-------------------------------------------------------------------
//Get gyro reading (rate of rotation expressed in deg/ms)
//-------------------------------------------------------------------
float getGyroOutput(unsigned char w){
static float tmpf;                              //temporary variable for complex calculations
tmpf = adcAvg[3+w] - config.gyroOffs[w];        //remove offset
tmpf *= config.gyroSens[w];                     //divide by sensitivity
if( config.gyroInv[w]) tmpf = - tmpf;           //invert axis value if needed
return tmpf;
}

//bring dcm matrix in order - adjust values to make orthonormal (or at least closer to orthonormal)
void dcm_orthonormalize(float dcm[3][3]){
//err = X . Y ,  X = X - err/2 * Y , Y = Y - err/2 * X  (DCMDraft2 Eqn.19)
float err = vector3d_dot((float*)(dcm[0]),(float*)(dcm[1]));
float delta[2][3];
vector3d_scale(-err/2,(float*)(dcm[1]),(float*)(delta[0]));
vector3d_scale(-err/2,(float*)(dcm[0]),(float*)(delta[1]));
vector3d_add((float*)(dcm[0]),(float*)(delta[0]),(float*)(dcm[0]));
vector3d_add((float*)(dcm[1]),(float*)(delta[1]),(float*)(dcm[1]));

//Z = X x Y  (DCMDraft2 Eqn. 20) ,
vector3d_cross((float*)(dcm[0]),(float*)(dcm[1]),(float*)(dcm[2]));
//re-nomralization
vector3d_normalize((float*)(dcm[0]));
vector3d_normalize((float*)(dcm[1]));
vector3d_normalize((float*)(dcm[2]));
}


//rotate DCM matrix by a small rotation given by angular rotation vector w
//see http://gentlenav.googlecode.com/files/DCMDraft2.pdf
void dcm_rotate(float dcm[3][3], float w[3]){
//float W[3][3];
//creates equivalent skew symetric matrix plus identity matrix
//vector3d_skew_plus_identity((float*)w,(float*)W);
//float dcmTmp[3][3];
//matrix_multiply(3,3,3,(float*)W,(float*)dcm,(float*)dcmTmp);

int i;
float dR[3];
//update matrix using formula R(t+1)= R(t) + dR(t) = R(t) + w x R(t)
for(i=0;i<3;i++){
vector3d_cross(w,dcm[i],dR);
vector3d_add(dcm[i],dR,dcm[i]);
}

//make matrix orthonormal again
dcm_orthonormalize(dcm);
}


//-------------------------------------------------------------------
// imu_init
//-------------------------------------------------------------------
unsigned int count250us_prev;
unsigned int count250us;

void imu_init(){
count250us_prev=count250us;
}

//-------------------------------------------------------------------
// imu_update
//-------------------------------------------------------------------
void imu_update(){
int i;
imu_sequence++;

//interval since last call
imu_interval_ms = 0.00025*(float)(count250us-count250us_prev);
count250us_prev = count250us;

//---------------
// I,J,K unity vectors of global coordinate system I-North,J-West,K-zenith
// i,j,k unity vectors of body's coordiante system  i-"nose", j-"left wing", k-"top"
//---------------
//        [I.i , I.j, I.k]
// DCM =  [J.i , J.j, J.k]
//        [K.i , K.j, K.k]

//---------------
//dcmEst
//---------------
//gyro rate direction is usually specified (in datasheets) as the device's(body's) rotation
//about a fixed earth's (global) frame, if we look from the perspective of device then
//the global vectors (I,K,J) rotation direction will be the inverse
float w[3];                     //gyro rates (angular velocity of a global vector in local coordinates)
w[0] = -getGyroOutput(0);       //rotation rate about accelerometer's X axis (GY output) in rad/ms
w[1] = -getGyroOutput(1);       //rotation rate about accelerometer's Y axis (GX output) in rad/ms
w[2] = -getGyroOutput(2);       //rotation rate about accelerometer's Z axis (GZ output) in rad/ms
for(i=0;i<3;i++){
w[i] *= imu_interval_ms;        //scale by elapsed time to get angle in radians
}

dcm_rotate(dcmEst,w);
}
#endif

Este es el archivo más importante. Aquí se inicializa y se va manteniendo la DCM. Primero debemos ver la forma en que llevamos el tiempo de integracion del gyro para la DCM. Tenemos un timer, el timer0, el cuál está configurado para interrumpir cada 250useg. Por lo que la presicion en el control de dicho tiempo es 0.25 mseg. La variable que se va incrementando es count250us. Cada vez que llamamos a imu_update, se calcula el tiempo entre el llamado anterior y este en mseg. Se hace uso de otra variable, count250us_prev para llevar el delta T.
Tenemos una funcion getGyroOutput, la cual lee del array de datos adcAvg el eje que le pasemos como parámetro. Le aplica el offset, la sensivilidad, lo invierte se es necesario y devuelve el valor en rad/mseg.
Tenemos la funcion dcm_rotate, la cual toma la matriz DCM actual y le aplica la rotacion (pequeña) w pasada como parámetro.
La funcion dcm_orthonormalize se encarga de hacer que los vectores filas sean de longitud 1 y que cada fila sea perpendicular a las otras dos.
Finalmente tenemos imu_update, en donde, por ahora, solo leemos el gyróscopo, calculamos los rad de giro y mandamos a actualizar la DCM.
De este archivo, debemos llamar a imu_init y luego llamar periódicamente a imu_update.

Bueno, finalmente nuestro main.c
Código: [Seleccionar]
#ifdef __USE_CMSIS
#include "LPC17xx.h"
#endif

volatile uint32_t msTicks;

__INLINE static void delay_ms (uint32_t delayTicks) {
  uint32_t currentTicks;
  currentTicks = msTicks; // read current tick counter
  while ((msTicks - currentTicks) < delayTicks);
}

#define SAMPLESS_BIASS 1000

//TC = T * SystemCoreClok/PCLK_TIMER0 con T en segundos
//PCLK_TIMER0 = PCLK_periph/4;
#define usTim0 250
#define TIMER0_INTERVAL 25*usTim0

#define GYRO_X_SCALE 0.079 //Sensibilidad en cada eje
#define GYRO_Y_SCALE 0.079 //extraído del datasheet
#define GYRO_Z_SCALE 0.079
#define ACEL_X_SCALE 0.003135 //Sensibilidad en cada eje
#define ACEL_Y_SCALE 0.003306 //de la calibracion
#define ACEL_Z_SCALE 0.003311

//short accel_X, accel_Y, accel_Z;
//short gyro_X, gyro_Y, gyro_Z;

double biass_X, biass_Y, biass_Z;
float imu_interval_ms = 0;      //interval since last call to imu_update

//char res_a=0;
//char res_g=0;
//
double adcAvg[6]={0.0,0.0,0.0,0.0,0.0,0.0}; //Primeros 3 para Acc y despues Gyro

//---------------------------------------------------------------------
// LIBS
//---------------------------------------------------------------------
#include <cr_section_macros.h>
#include <NXP/crp.h>
#include <math.h>
#include "uart2.h"
#include "macroutil.h"
#include <stdio.h>
#include "i2c.h"
#include "L3G4200D.h"
#include "config.h"
#include "vector3d.h"
#include "calibrate.h"
#include "imu.h"

float timeStep = 0.010; //Tiempo que tiene que durar el main

char countfor5ms=0;        //this is used in IMU module
char f_leer_data=0;

//Handler de la interrupcion del Timer 0. Sucede cada 250uSeg
void TIMER0_IRQHandler (void)
{
LPC_TIM0->IR = 1; /* clear interrupt flag */
count250us++;
if(++countfor5ms==20){
f_leer_data=1;
countfor5ms=0;
}
}

void SysTick_Handler(void) {
msTicks++; /* increment counter necessary in Delay() */
}


/*******************************************************************************
**   Main Function  main()
*******************************************************************************/
int main (void){
// int i=0;
uint32_t timer=0;
uint32_t timer2=0;

//Configuro el SysTick para que interrumpa cada 1 mseg
if (SysTick_Config(SystemCoreClock / 1000)) {
while (1);
}

//---------------------------------------------------------------------
// CONFIGURATION
//---------------------------------------------------------------------
config_default(); //Cargo una configuracion fija. Ver de grabar en la flash.

//---------------------------------------------------------------------
// UART
//---------------------------------------------------------------------
UART2_Init(115200); // Inicializo el UART a 115200
UART2_PrintString ("\r\nDCM con  Giroscopo L3G4200D\r\n");
UART2_PrintString ("\r\nDCM, tiempo de actualizacion DCM\r\n");

//---------------------------------------------------------------------
// I2C
//---------------------------------------------------------------------
I2C1Init(); /* initialize I2c1 */

L3G4200D_init();

timer0_init();
//---------------------------------------------------------------------
// CALIBRATION
//---------------------------------------------------------------------
calibrate_gyro();

//---------------------------------------------------------------------
// IMU
//---------------------------------------------------------------------
imu_init();

timer0_enable();

timer = msTicks; //get a start value to determine the time the loop takes
timer2 = msTicks;
while ( 1 ){
//Ya pasaron 5mseg por lo que leo los sensores
if(f_leer_data){
L3G4200D_read_data(); //los sensores.
f_leer_data=0;
}
if((msTicks-timer) >= 10 ){
timer=msTicks;
imu_update();
}
if((msTicks-timer2) >= 500){
timer2=msTicks;
print_dcm(dcmEst);
}
}
}

Bueno, aquí primero definimos constantes, variables globales, incluimos archivos y nos encontramos con el handler de la interrupcion del timer 0. Simplemente incrementamos una variable y llevamos la cuenta para actualizar una bandera cuando halla pasado el tiempo deseado para la lectura de sensores. En este caso 5mseg.
Luego, en la funcion main mandamos a inicializar todo, calibramos offset de gyro inicializamos la imu y nos vamos al bucle principal.
En el lazo principal lo que hacemos es verificar el flag que me indica que hay que ir a leer sensores. Luego verificar si ha pasado el tiempo preestablecido para actualizar la imu. Y lo mismo con el tiempo para mandar a imprimir la DCM.

Bueno, con todo esto, esta es una muestra del hyperteminal. El tiempo entre línea y linea es de medio segundo:



Veamos el mismo ejemplo, pero esta vez girando en todas direcciones y volviendo el sensor al punto de partida:



Bastante bien, verdad??!!!!

Bueno, ahora hay que agregar el acelerómetro para que corrija el drift del gyro.... eso, en la próxima!

Saludos
-
Leonardo Garberoglio

Desconectado elgarbe

  • Moderador Local
  • PIC24H
  • *****
  • Mensajes: 2178
Re: Sistema de navegacion para UAV's
« Respuesta #11 en: 02 de Febrero de 2014, 14:46:37 »
Bueno, en el post anterior vimos como crear y mantener la DCM solo con los datos del gyróscopo. Pero tambien vimos que los gyros tienen deriva, esto es, si dejo el sensor quieto en la mesa la DCM irá "girando" sola. Por otra parte, moviemndo el sensor este efecto se hace más notable.
Aquí muestro la DCM cada 0.5seg, giro el sensor para todos lados y luego lo dejo quieto:



como pueden ver los valores de la DCM se alejan de la matriz identidad, lo que indica que la DCM tiene deriva.

Para eliminar la deriva debemos usar sensores adicionales que carezcan de deriva. Por ejemplo, podemos corregir el vector K de la DCM utilizando el acelerómetro. Como vimos anteriormente el producto vectorial entre dos vectores me da otro vector perpendicular a los originale sy cuyo módulo es proporcional al ángulo entre los dos. Si los vectores originales no estan desfasados, el módulo del vector será 0. Entonces si tomamos al acelerómetro como una fuente "confiable" del vector K (o el eje Z) podemos corregir el vector K de la DCM "rotándolo" un pequeño ángulo determinado por el error entre el vector estimado por el Gyro y el obtenido por el Acelerómetro. A este correccion podemos aplicarle un filtro. Aquí es donde entran el filtro complementario, kalman, u otros. En el caso de la DCM podemos empezar con un simple filtro complementario el cual da un "peso" a la correccion por el acelerómetro.
Bien, con estas consideraciones mis archivos quedan:

macroutil.h, vector3d.h, L3G4200D.h quedan como estaban. Agrego el ADXL345.h y los se modifican los siguientes:

config.h

Código: [Seleccionar]
#ifndef CONFIG_H
#define CONFIG_H

struct {
unsigned char accInv[3];        // invert accl input (for example due to physical mounting position)
double accOffs[3];              // accl output at 0g in ADC units
double accSens[3];              // accl input sensitivity in ADC/g

unsigned char gyroInv[3];       // invert gyro input (for example due to physical mounting position)
double gyroOffs[3];             // gyro zero rate output in ADC  @ 0 deg/s;
double gyroSens[3];             // gyro input sensitivity ADC/(deg/ms)

} config;

void config_default(){
config.accOffs[0] = 0;
config.accOffs[1] = 0;
config.accOffs[2] = 0;

config.accSens[0] = ACEL_X_SCALE;
config.accSens[1] = ACEL_Y_SCALE;
config.accSens[2] = ACEL_Z_SCALE;

config.gyroOffs[0] = 0;
config.gyroOffs[1] = 0;
config.gyroOffs[2] = 0;

config.gyroSens[0] = GYRO_X_SCALE * PI/180;
config.gyroSens[1] = GYRO_Y_SCALE * PI/180;
config.gyroSens[2] = GYRO_Z_SCALE * PI/180;

config.accInv[0] = 0;
config.accInv[1] = 0;
config.accInv[2] = 0;

config.gyroInv[0] = 0;
config.gyroInv[1] = 0;
config.gyroInv[2] = 0;
}
#endif

calibrate.h

Código: [Seleccionar]
#ifndef CALIBRATE_H
#define CALIBRATE_H


/*********** GYRO CALIBRATION ********/
void calibrate_gyro(){
L3G4200D_GetBiass();
config.gyroOffs[0] = biass_X;
config.gyroOffs[1] = biass_Y;
config.gyroOffs[2] = biass_Z;
}


/*********** ACCEL CALIBRATION ********/
void calibrate_acc(){
ADXL345_GetBiass();
config.accOffs[0] = biass_X;
config.accOffs[1] = biass_Y;
config.accOffs[2] = biass_Z;
}

#endif

En ambos casos hemos agregado los miembros correspondientes al acelerómetro.

Finalmente mi imu.h

Código: [Seleccionar]
#ifndef IMU__H
#define IMU__H

/*
How to use this module in other projects.

Input variables are:
adcAvg[0..5]  ADC readings of 3 axis accelerometer and 3 axis gyroscope (they are calculated in the background by adcutil.h)
interval_us - interval in microseconds since last call to imu_update

Output variables are:
DcmEst[0..2] which are the direction cosine of the X,Y,Z axis

First you must initialize the module with:
imu_init();

Then call periodically every 5-20ms:
imu_update();
it is assumed that you also update periodicall the adcAvg[0..5] array

*/

#define ACC_WEIGHT_MAX 0.00     //maximum accelerometer weight in accelerometer-gyro fusion formula
//this value is tuned-up experimentally: if you get too much noise - decrease it
//if you get a delayed response of the filtered values - increase it
//starting with a value of  0.01 .. 0.05 will work for most sensors

//-------------------------------------------------------------------
// Globals
//-------------------------------------------------------------------

unsigned int imu_sequence = 0;                          //incremented on each call to imu_update
float dcmEst[3][3] = {{1,0,0},{0,1,0},{0,0,1}};         //estimated DCM matrix


//-------------------------------------------------------------------
//Get accelerometer reading (accelration expressed in g)
//-------------------------------------------------------------------
float getAcclOutput(unsigned char w){
static float tmpf;                      //temporary variable for complex calculations
tmpf = adcAvg[w] - config.accOffs[w];   //remove offset
tmpf *= config.accSens[w];              //divide by sensitivity
if(config.accInv[w]) tmpf = - tmpf;     //invert axis value if needed
return tmpf;
}

//-------------------------------------------------------------------
//Get gyro reading (rate of rotation expressed in deg/ms)
//-------------------------------------------------------------------
float getGyroOutput(unsigned char w){
static float tmpf;                              //temporary variable for complex calculations
tmpf = adcAvg[3+w] - config.gyroOffs[w];        //remove offset
tmpf *= config.gyroSens[w];                     //divide by sensitivity
if( config.gyroInv[w]) tmpf = - tmpf;           //invert axis value if needed
return tmpf;
}

//bring dcm matrix in order - adjust values to make orthonormal (or at least closer to orthonormal)
void dcm_orthonormalize(float dcm[3][3]){
//err = X . Y ,  X = X - err/2 * Y , Y = Y - err/2 * X  (DCMDraft2 Eqn.19)
float err = vector3d_dot((float*)(dcm[0]),(float*)(dcm[1]));
float delta[2][3];
vector3d_scale(-err/2,(float*)(dcm[1]),(float*)(delta[0]));
vector3d_scale(-err/2,(float*)(dcm[0]),(float*)(delta[1]));
vector3d_add((float*)(dcm[0]),(float*)(delta[0]),(float*)(dcm[0]));
vector3d_add((float*)(dcm[1]),(float*)(delta[1]),(float*)(dcm[1]));

//Z = X x Y  (DCMDraft2 Eqn. 20) ,
vector3d_cross((float*)(dcm[0]),(float*)(dcm[1]),(float*)(dcm[2]));
//re-nomralization
vector3d_normalize((float*)(dcm[0]));
vector3d_normalize((float*)(dcm[1]));
vector3d_normalize((float*)(dcm[2]));
}


//rotate DCM matrix by a small rotation given by angular rotation vector w
//see http://gentlenav.googlecode.com/files/DCMDraft2.pdf
void dcm_rotate(float dcm[3][3], float w[3]){
//float W[3][3];
//creates equivalent skew symetric matrix plus identity matrix
//vector3d_skew_plus_identity((float*)w,(float*)W);
//float dcmTmp[3][3];
//matrix_multiply(3,3,3,(float*)W,(float*)dcm,(float*)dcmTmp);

int i;
float dR[3];
//update matrix using formula R(t+1)= R(t) + dR(t) = R(t) + w x R(t)
for(i=0;i<3;i++){
vector3d_cross(w,dcm[i],dR);
vector3d_add(dcm[i],dR,dcm[i]);
}

//make matrix orthonormal again
dcm_orthonormalize(dcm);
}


//-------------------------------------------------------------------
// imu_init
//-------------------------------------------------------------------
unsigned int count250us_prev;
unsigned int count250us;

void imu_init(){
count250us_prev=count250us;
}

//-------------------------------------------------------------------
// imu_update
//-------------------------------------------------------------------
#define ACC_WEIGHT 0.01         //accelerometer data weight relative to gyro's weight of 1
#define ACC_ERR_MAX 0.3         //maximum accelerometer errror relative to 1g , when error exceeds this value accelerometer weight becomes 0
//this helps reject external accelerations (non-gravitational innertial forces caused by device acceleration)

float imu_interval_ms = 0;      //interval since last call to imu_update
float Kacc[3];                  //K(b) vector according to accelerometer in body's coordinates

void imu_update(){
int i;
imu_sequence++;

//interval since last call
imu_interval_ms = 0.00025*(float)(count250us-count250us_prev);
count250us_prev = count250us;

//---------------
// I,J,K unity vectors of global coordinate system I-North,J-West,K-zenith
// i,j,k unity vectors of body's coordiante system  i-"nose", j-"left wing", k-"top"
//---------------
//        [I.i , I.j, I.k]
// DCM =  [J.i , J.j, J.k]
//        [K.i , K.j, K.k]


//---------------
//Acelerometer
//---------------
//Accelerometer measures gravity vector G in body coordinate system
//Gravity vector is the reverse of K unity vector of global system expressed in local coordinates
//K vector coincides with the z coordinate of body's i,j,k vectors expressed in global coordinates (K.i , K.j, K.k)

//Acc can estimate global K vector(zenith) measured in body's coordinate systems (the reverse of gravitation vector)
Kacc[0] = getAcclOutput(0);
Kacc[1] = getAcclOutput(1);
Kacc[2] = getAcclOutput(2);
vector3d_normalize(Kacc);
//calculate correction vector to bring dcmEst's K vector closer to Acc vector (K vector according to accelerometer)
float wA[3];
vector3d_cross(dcmEst[2],Kacc,wA);      // wA = Kgyro x  Kacc , rotation needed to bring Kacc to Kgyro

//---------------
//dcmEst
//---------------
//gyro rate direction is usually specified (in datasheets) as the device's(body's) rotation
//about a fixed earth's (global) frame, if we look from the perspective of device then
//the global vectors (I,K,J) rotation direction will be the inverse
float w[3];                     //gyro rates (angular velocity of a global vector in local coordinates)
w[0] = -getGyroOutput(0);       //rotation rate about accelerometer's X axis (GY output) in rad/ms
w[1] = -getGyroOutput(1);       //rotation rate about accelerometer's Y axis (GX output) in rad/ms
w[2] = -getGyroOutput(2);       //rotation rate about accelerometer's Z axis (GZ output) in rad/ms
for(i=0;i<3;i++){
w[i] *= imu_interval_ms;        //scale by elapsed time to get angle in radians
//compute weighted average with the accelerometer correction vector
w[i] = (w[i] + ACC_WEIGHT*wA[i])/(1.0+ACC_WEIGHT);
}

dcm_rotate(dcmEst,w);
}
#endif

Como cometé más arriba, primero se obtienen los datos del acelerómetro. Luego se calcula el error de rotacion del vector k de la DCM respecto del dato leido por el acelerómetro. Este vector a rotar se almacena en wA. Luego se lee el giróscopo y se obtiene la pequeña rotacion actual del sensor. Luego se corrige el vecto obtenido con los datos del vector de rotacion del acelerómetro, aplicando un "peso" a la contribucion de la correccion por el acelerómetro.
Finalmente se manda a rotar la DCM para que coincida con la posicion real de la IMU.

Con este agregado, esperamos tener el vector K de la DCM libre de deriba. Veamos que obtenemos al hacer el mismo experimento anterior:


(aquí tengo mal el encabezado de los datos, no es pitch roll yaw sino que son los 9 datos de la DCM lo que estoy motrsando)
Bien, aquí vemos que despues de mover la IMU en todas direcciones y dejarla denuevo en el lugar inicial tenemos deriva en los vectores I y J de la DCM, pero no en el vector K, ya que este último se corrige con el acelerómetro.

Bueno, para poder mostrar mejores resultados debemos trabajar con números mas "visibles" y como todos estamos acostumbrados a ver ángulos deberemos extraer los ángulos de euler de la DCM para mostrar esos resultados...

Saludos!
-
Leonardo Garberoglio

Desconectado elgarbe

  • Moderador Local
  • PIC24H
  • *****
  • Mensajes: 2178
Re: Sistema de navegacion para UAV's
« Respuesta #12 en: 03 de Febrero de 2014, 20:48:35 »
Bien, para extraer los ángulos de euler de la DCM podemos usar las siguientes ecuaciones:

Código: [Seleccionar]
double atan2deg(float y, float x){
if(fabs(y) < 0.1 && fabs(x) < 0.1)
return 0.0; //avoid atan2 returning meaningless numbers
else
return atan2(y,x)*180.0 / PI;
}
void euler_angles(float vector[3][3]){
  pitch = asin(-vector[2][0]) * 180/PI;
  roll = atan2deg(vector[2][1],vector[2][2]); //* 180/PI;
  yaw = atan2deg(vector[1][0],vector[0][0]); //* 180/PI;
}
void print_euler(){
uart2_printDouble(pitch, 3);
UART2_Sendchar(',');
uart2_printDouble(roll, 3);
UART2_Sendchar(',');
uart2_printDouble(yaw, 3);
UART2_Sendchar(',');
uart2_printDouble(pitchGyro* 180/PI, 3);
UART2_Sendchar(',');
uart2_printDouble(rollGyro* 180/PI, 3);
UART2_Sendchar(',');
uart2_printDouble(pitchAccel, 3);
UART2_Sendchar(',');
uart2_printDouble(rollAccel, 3);
UART2_PrintString ("\r\n");
}

La funcion atan2deg, devuelve el resultado de atan2, pero en grados sexagecimales y con una restricción, cuando tanto el vector X como el Y están cerca de 0 la función devuelve 0 para evitar una de las singularidades de los ángulos de Euler.
Luego tenemos el cálculo de los ángulos a partir de la DCM y finalmente la impresión de los datos.
En mi bucle principal el código es el siguiente:

Código: [Seleccionar]
while ( 1 ){
//Ya pasaron 5mseg por lo que leo los sensores
if(f_leer_data){
L3G4200D_read_data(); //los sensores.
ADXL345_read_data();
f_leer_data=0;
}
if((msTicks-timer) >= 10 ){
timer=msTicks;
imu_update();
}
if((msTicks-timer2) >= 50 ){
timer2=msTicks;
//print_dcm(dcmEst);
euler_angles(dcmEst);
print_euler();
}
}

Y en el imu.h tengo la función que va leyendo los sensores y calculando los ángulos, integrando el gyro, etc.:

Código: [Seleccionar]
void imu_update(){
int i;
imu_sequence++;

//interval since last call
imu_interval_ms = 0.00025*(float)(count250us-count250us_prev);
count250us_prev = count250us;

//---------------
// I,J,K unity vectors of global coordinate system I-North,J-West,K-zenith
// i,j,k unity vectors of body's coordiante system  i-"nose", j-"left wing", k-"top"
//---------------
//        [I.i , I.j, I.k]
// DCM =  [J.i , J.j, J.k]
//        [K.i , K.j, K.k]


//---------------
//Acelerometer
//---------------
//Accelerometer measures gravity vector G in body coordinate system
//Gravity vector is the reverse of K unity vector of global system expressed in local coordinates
//K vector coincides with the z coordinate of body's i,j,k vectors expressed in global coordinates (K.i , K.j, K.k)

//Acc can estimate global K vector(zenith) measured in body's coordinate systems (the reverse of gravitation vector)
Kacc[0] = getAcclOutput(0);
Kacc[1] = getAcclOutput(1);
Kacc[2] = getAcclOutput(2);
vector3d_normalize(Kacc);

pitchAccel = atan (Kacc[0] / sqrt(Kacc[1]*Kacc[1] + Kacc[2]*Kacc[2])) * 180/PI;
rollAccel = atan (Kacc[1] / sqrt(Kacc[0]*Kacc[0] + Kacc[2]*Kacc[2])) * 180/PI;

//calculate correction vector to bring dcmEst's K vector closer to Acc vector (K vector according to accelerometer)
float wA[3];
vector3d_cross(dcmEst[2],Kacc,wA);      // wA = Kgyro x  Kacc , rotation needed to bring Kacc to Kgyro

//---------------
//dcmEst
//---------------
//gyro rate direction is usually specified (in datasheets) as the device's(body's) rotation
//about a fixed earth's (global) frame, if we look from the perspective of device then
//the global vectors (I,K,J) rotation direction will be the inverse
float w[3];                     //gyro rates (angular velocity of a global vector in local coordinates)
w[0] = -getGyroOutput(0);       //rotation rate about accelerometer's X axis (GY output) in rad/ms
w[1] = -getGyroOutput(1);       //rotation rate about accelerometer's Y axis (GX output) in rad/ms
w[2] = -getGyroOutput(2);       //rotation rate about accelerometer's Z axis (GZ output) in rad/ms

pitchGyro = pitchGyro - w[1] * imu_interval_ms;
rollGyro = rollGyro - w[0] * imu_interval_ms;

for(i=0;i<3;i++){
w[i] *= imu_interval_ms;        //scale by elapsed time to get angle in radians
//compute weighted average with the accelerometer correction vector
w[i] = (w[i] + ACC_WEIGHT*wA[i])/(1.0+ACC_WEIGHT);
}

dcm_rotate(dcmEst,w);
}

hay que tener algunos cuidados con el cálculo de dichos ángulos ya que tienen algunas singularidades. Por ejemplo, cuando el pitch está cercano a los 90° se empieza a perturbar la medición del Roll. A esto se le conoce como Gimbal Lock... Lo ideal es no trabajar con los ángulos Euler, usarlos únicamente para la parte visual, pero no para el control.
Lo que voy a mostrar ahora es la comparación entre los ángulos Pitch y Roll calculado con el acelerómetro, el giróscopo y la DCM:



en esta primer imagen vemos con movimientos suaves como las tres mediociones coinciden. El giróscopo muestra una pequeña diferencia pero se lo atribuyo a la calibración de la sensibilidad. También podemos ver que el acelerómetro muestra algunos picos, propios de la respuesta en altas frecuencias. He podido comprobar también la deriva con el tiempo. Combinando el acelerómetro con el filtro complementarios, en estacionario el vector K de la DCM no tiene deriva para nada, mientras que los otros vectores van derivando lentamente.

Veamos ahora que pasa con movimientos bruscos:



Aquí he movido la IMU en todas direcciones "sacudiéndolo" para todos lados. En la imagen anterior muestro únicamente el ángulo Pitch. Como ven el giróscopo aduiere muchísima deriva con esos movimientos, estará saturando???? lo tengo configurado al máximo (2000dps)... igual, debo decir que si el UAV se mueve como yo moví la IMU es porque algo malo ha pasado!! Tambien vemos como el acelerómetro tiene grandes picos, mientras que la DCM es una versión mas "filtrada". Cuando el movimiento cesa, la DCM va convergiendo a la medición del acelerómetro, sin importar la deriva del giro.



Aquí vemos lo mismo, pero el ángulo de Roll.

Bien, ahora queda agregar el magnetómetro para eliminar la deriva en Yaw. Con dicho sensor podemos dar por finalizada la implementación básica de la DCM.
Veo 2 puntos a seguir a partir de ese momento, por un lado trabajar en alguna GUI que nos permita leer la DCM por el puerto serie y graficarnos el horizonte artificial o un cubo, como esos videos que se ven en youtube. Se podría trabajar en processing, C# o lo que sea. Alguien tiene algo de esto?
El seguno punto a trabajar es en las pruebas arriba del UAV, donde las vibraciones y demás temas afectarán la estabilidad de la DCM.

Por ahora eso es todo!

Saludos
-
Leonardo Garberoglio