Programa de pruebas aleatorias de los algoritmos de estimación de orientación

El programa está desarrollado para funcionar en el entorno de cálculo científico Scilab. El programa tiene una estructura modular, lo que facilita su uso para la evaluación de distintos algoritmos de estimación de orientación, de los vehículos aéreos de diferentes tipos.

En modo de prueba automatizada, el programa se ejecuta automáticamente sin intervención humana. Sin embargo, para controlar el flujo de pruebas o para depurar el programa, se proporciona un modo interactivo en el que el usuario puede observar la animación del movimiento del avión (Fig. 1), ver los gráficos con los resultados del modelo dinámico del avión, las señales de los sensores, la estimación de orientación del avión y guardar los resultados de la simulación con los archivos en el disco.

Figura 1. Animación de los resultados del modelo dinámico del avión.

Estructura y funcionamiento del programa

Desde el punto de vista de la optimización, el programa realiza una optimización aleatoria [5, 6] para calcularla mayor diferencia entre la orientación de la aeronave obtenida con el modelo dinámico del UAV y de la orientación obtenida por el algoritmo de estimación de la orientación en estudio. En otras palabras, el programa busca los valores iniciales de las variables de estado y las variables de control del modelo dinámico de un vehículo aéreo, que resultan en el máximo error de un algoritmo de estimación de orientación sometido a prueba. El error se define como la diferencia entre las variables de estado obtenidas por el modelo dinámico y las variables de estado recuperadas de las señales delos sensores, usando el algoritmo de estimación de orientación de la aeronave.

El Diagrama de bloques del programa de pruebas aleatorias de los algoritmos de estimación de orientación se muestra en la Fig. 2.

Figura 2. Diagrama de bloques del programa.

El bloque "Archivos de los Parámetros" representa cuatro archivos de parámetros: TestParam.sce, UAVParam.sce, SimParam.sce y SensParam.sce. Los parámetros de pruebas, como la cantidad de pruebas y fronteras de los intervalos de cambio de las variables del estado y del control del vehículo, están definidos en el archivo TestParam.sce. El archivo UAVParam.sce describe los parámetros de la aeronave como los parámetros físicos de la estructura del avión y los coeficientes aerodinámicos. Los parámetros de simulación, como el valor de la aceleración de la gravedad, presión atmosférica, paso y tiempos de simulación inicial y final son definidos en el archivo SimParam.sce. El archivo SensParam.sce define los parámetros de los modelos de sensores.

El modelo dinámico del vehículo aéreo está representado por el bloque "Modelo dinámico de UAV". El estado inicial del modelo (posición, orientación velocidad lineal y angular) así como los valores de las variables de control de UAV (los ángulos de las superficies de control como los alerones, los elevadores, el timón y la variable de empuje) se aplican a la entrada del modelo. A la salida del modelo, obtenemos la dependencia del estado del modelo con el tiempo.

Esta dependencia del estado del modelo con el tiempo se utiliza luego en el bloque "Generador de las señales de sensores". El generador de señales de sensores es un conjunto de los modelos de sensores de presión, acelerómetros, giroscopios y GPS. Las señales de salida de los sensores entran en el algoritmo de estimación de la orientación en estudio. El bloque “Comparador” compara la orientación obtenida por el algoritmo de estimación de orientación y la orientación obtenida utilizando el modelo dinámico del UAV. El error de orientación está usado por el bloque “Control de pruebas” como valor de aptitud del algoritmo de estimación para realizar la optimización aleatoria.

El algoritmo del programa se muestra en la Fig. 3.

Figura 3. El algoritmo del programa.

El programa comienza con la inicialización del ciclo de pruebas. A propósito, lee los parámetros de cuatro archivos (TestParam.sce, UAVParam.sce, SimParam.sce, SensParam.sce). A continuación, el programa entra en el ciclo principal de pruebas.

En el ciclo principal se realiza lo siguiente:

1. El programa genera nuevos valores iniciales para las variables de estado y de control, de un modelo dinámico del vehículo aéreo, usando el generador de números aleatorios y fronteras de cambios de variables definidas en el archivo TestParam.sce.
2. Usando los resultados del modelo dinámico, el programa genera las señales de los sensores del vehículo, como señales de giroscopios, acelerómetros, sensores de presión y GPS.
3. Un algoritmo de estimación de orientación usa las señales de sensores producidas por el programa para recuperar las variables de estado del modelo de vehículo (posición, velocidades, ángulos de Euler y velocidades angulares).
4. El programa calcula la diferencia entre las variables de estado obtenidas por el modelo dinámico (paso 2) y las variables de estado recuperadas de las señales de sensores usando el algoritmo de estimación de orientación de la aeronave (paso 3).
5. El programa repite los pasos 1-5 hasta alcanzar la cantidad de pruebas definida por el usuario. Una vez que el programa ha completado el número de pruebas, se muestra el tiempo de ejecución y se genera un archivo de texto llamado TestResFile.log, que contiene el número de pruebas, el error máximo de estimación de orientación, los valores de posición y velocidades iniciales, y los valores de los controles del vehículo que llevaron a este error.

Modelo dinámico de UAV

Modelo dinámico de UAV es un sistema de 12 ecuaciones diferenciales ordinarias, no lineales, acopladas. Actualmente el programa usa el modelo dinámico de un vehículo aéreo de alas fijas con representación de orientación en los ángulos de Euler. Las fórmulas del modelo y los parámetros aerodinámicos del fuselaje se tomaron de [7]. Para integrar el modelo, se usa la función estándar de Scilab ode().

El modelo dinámico de la aeronave consta de dos archivos. El archivo UAVParam.sce describe los parámetros de la aeronave como los parámetros físicos de la estructura del avión y los coeficientes aerodinámicos. El usuario puede cambiar estos parámetros para simular una aeronave del mismo tipo (con alas fijas). Para cambiar el tipo de vehículo, el usuario debe cambiar el archivo DynModel.sce, que contiene los códigos del modelo dinámico. Los parámetros de simulación son definidos en el archivo SimParam.sce.

Generador de las señales de sensores

El generador de señales de los sensores utiliza los resultados del modelo dinámico para generar las señales de los giroscopios, acelerómetros, sensores de presión y GPS. El diagrama de bloques del generador de señales de los sensores se muestra en la Fig. 4.

Figura 4. Diagrama esquemático del generador de señales de los sensores.

Los errores de medición de aceleraciones, presiones y velocidades angulares están modelados por ruidos con distribución de Gauss. Los errores de medición de posición con el uso de GPS, son modelados con las secuencias aleatorias de Markov y ruidos de Gauss. Los parámetros de los sensores están definidos en el archivo SensParam.sce.

Acelerómetros. Los acelerómetros son modelados como:

$y_{\mathit{accel},x}=\frac{f_x}{m}+g\sin \theta +{\eta }_{\mathit{accel},x}$   (1)

$y_{\mathit{accel},y}=\frac{f_y}{m}-g\cos \theta \sin \varphi +{\eta }_{\mathit{accel},y}$   (2)

$y_{\mathit{accel},z}=\frac{f_z}{m}-g\cos \theta \cos \varphi +{\eta }_{\mathit{accel},z}$   (3)

donde: fx, fy, fz son componentes de la fuerza obtenida del modelo dinámico; m es la masa del vehículo; g la aceleración de la gravedad; ϕ, θ, ψ son los ángulos de Euler del UAV, y ηaccel es ruido gaussiano de media cero.

Giroscopios. Las salidas de los giroscopios son modeladas como:

$y_{\mathit{gyro},x}=p+{\eta }_{\mathit{gyro},x}$   (4)

$y_{\mathit{gyro},y}=q+{\eta }_{\mathit{gyro},y}$   (5)

$y_{\mathit{gyro},z}=r+{\eta }_{\mathit{gyro},z}$   (6)

donde: p, q y r son tazas angulares de UAV y ηgyro representa procesos gaussianos de media cero con varianza σ2gyro  respectivamente.

Sensores de presión.

La medición del sensor de presión atmosférica absoluta resulta:

$y_{\text{abs}\mathit{pres}}={\mathit{\rho gh}}_{\mathit{AGL}}+{\beta }_{\text{abs}\mathit{pres}}+{\eta }_{\text{abs}\mathit{pres}}$   (7)

donde ρ es la densidad del aire, g es la aceleración de la gravedad, hAGL=h-hground es la diferencia entre la altitud del avión y la altitud del terreno, βabspres es una deriva de sesgo relacionada con la temperatura, ηabspres  es ruido gaussiano de media cero con varianza σ2abs pres.

La medición del sensor de presión diferencial resulta:

$y_{\mathit{di}\mathit{ff}\mathit{pres}}=\frac{\rho V_a^2}{2}+{\beta }_{\mathit{diff}\mathit{pres}}+{\eta }_{\mathit{diff}\mathit{pres}}$   (8)

donde ρ es la densidad del aire, Va es el valor absoluto de la velocidad aérea, βdiffpres es una deriva de sesgo relacionada con la temperatura, ηdiffpres es ruido gaussiano de media cero con varianza σ2diff pres.

Sistema GPS. Las señales de las salidas de GPS se modelan mediante las siguientes ecuaciones:

$y_{\mathit{GPS},n}[n]=p_n[n]+v_n[n]$   (9)

$y_{\mathit{GPS},e}[n]=p_e[n]+v_e[n]$   (10)

$y_{\mathit{GPS},h}\left[n\right]=-p_d[n]+v_h[n]$   (11)

donde: yGPS,n , yGPS,e y yGPS,h  son señales de salida de los sensores GPS en los canales de posición norte, este y altura, respectivamente; pn , pe y pd son coordenadas del UAV en el sistema inercial; n es número de muestra; y vn, ve, vh las componentes norte, este y altura del proceso Gauss-Markov, que simula la señal de error en el sistema GPS.

El error en el posicionamiento está modelado como:

$v[n+1]=e^{k_{\mathit{GPS}}{T}_s}v[n]+{\eta }_{\mathit{GPS}}[n]$   (12)

donde: kGPS es un parámetro de modelo; Ts es el periodo de la muestra; y ηGPS es ruido blanco gaussiano de media cero.

Estimación del estado de UAV

El bloque "Algoritmo de estimación de orientación" contiene el algoritmo de estimación de orientación del vehículo aéreo sometido a pruebas. Para revisar el funcionamiento del programa actualmente se utilizó el algoritmo de estimación de orientación basado en la integración de los modelos matemáticos inversos de sensores [7-9]. El algoritmo es sencillo y no permite obtener una estimación precisa del estado en condiciones reales, pero es suficiente para justificar el funcionamiento del programa. Esquemáticamente, el algoritmo de estimación de orientación del vehículo se muestra en la Fig. 5.

La integración de los datos discretos de los sensores está realizada por el método de Heun [10].

Figura 5. Diagrama del algoritmo de estimación de orientación.

Comparador

El módulo Comparator.sce, calcula la diferencia entre las variables de estado obtenidas por el modelo dinámico y las variables de estado recuperadas por el algoritmo de estimación de orientación. El estado del vehículo aéreo en cada momento i está representado por el vector de estado xi.

$x_i={(p_{n,i},p_{e,i},p_{d,i},u_i,v_i,w_i,{\phi }_i,{\theta }_i,{\psi }_i,p_i,q_i,r_i)}^T$   (13)

donde las variables de estado pn,i, pe,i, pd,i definen la posición inercial del UAV, ui, vi, wi son velocidades de traslación de UAV, φi, θi, ψi son ángulos de orientación, pi, qi, ri son velocidades angulares del UAV.

Si ${\widehat{\Phi }}_i=\left({\widehat{\phi }}_i,{\widehat{\theta }}_i,{\widehat{\psi }}_i\right)$  es valor estimado del vector de los ángulos de orientación, resultado del algoritmo de estimación sometido a pruebas y ${\Phi }_i=({\phi }_i,{\theta }_i,{\psi }_i)$  es el vector de los ángulos de orientación obtenido al utilizar el modelo dinámico del UAV, el comparador calcula el error cuadrático medio (RMSE) definido por la siguiente expresión:

${\phi }_{\mathit{RMSE}}=\sqrt{\frac{1}{N}\sum _{i=1}^N{\left({\widehat{\phi }}_i-{\phi }_i\right)}^2}$   (14)

${\theta }_{\mathit{RMSE}}=\sqrt{\frac{1}{N}\sum _{i=1}^N{\left({\widehat{\theta }}_i-{\theta }_i\right)}^2}$   (15)

${\psi }_{\mathit{RMSE}}=\sqrt{\frac{1}{N}\sum _{i=1}^N{\left({\widehat{\psi }}_i-{\psi }_i\right)}^2}$   (16)

donde N es cantidad de muestreos.

La aptitud del algoritmo o estimación cuantitativa de la calidad del algoritmo es el máximo de tres errores de ángulos

${\Phi }_{\mathit{Err}}=\mathit{max}({\phi }_{\mathit{RMSE}},{\theta }_{\mathit{RMSE}},{\psi }_{\mathit{RMSE}})$   (17)

donde función max(x) significa la norma máxima del vector x.

Una vez que la prueba ha sido completada, el programa muestra la estadística y el tiempo de ejecución de la prueba.

Referencias

1. Crassidis, J. L., Markley, F. L., & Cheng, Y. (2007). Survey of nonlinear attitude estimation methods. Journal of guidance, control, and dynamics, 30(1), 12-28.

2. Cavallo, A., Cirillo, A., Cirillo, P., De Maria, G., Falco, P., Natale, C., &Pirozzi, S. (2014). Experimental comparison of sensor fusion algorithms for attitude estimation. IFAC Proceedings Volume, 47(3), 7585-7591.

3. Teague, H. (2016). Comparison of Attitude Estimation Techniquesfor Low-cost Unmanned Aerial Vehicles. arXiv preprint arXiv:1602.07733.

4. Moutinho, A., Figueirôa, M., &Azinheira, J. R. (2015). Attitude estimation in so (3): A comparative UAV case study. Journal of Intelligent & Robotic Systems, 80(3-4), 375-384.

5. Matyas, J., (1965). Random optimization. Automation and Remote control, 26(2), pp.246-253.

6. Hamlet, R., (1994). “Random testing.” Encyclopediaof Software Engineering, Wiley, NewYork.

7. Beard, R.W. and McLain, T.W., (2012). Small unmanned aircraft: Theory and practice. Princeton university press.

8. Barton, J.D., (2012). Fundamentals of small unmanned aircraft flight. Johns Hopkins APL technical digest, 31(2), pp.132-149.

9. Woodman, O.J., (2007). An introduction to inertial navigation (No. UCAM-CL-TR-696). University of Cambridge, Computer Laboratory.

10. Mathews, J.H. and Fink, K.D., (2004). Numerical methods using MATLAB (4th ed.). Upper Saddle River, NJ: Pearson Prentice Hall.