Filtro Complementario para la Estimación del ángulo de roll y de pitch

En este capítulo se presenta un sistema para estimación de actitud del QA3. Para determinar la inclinación se utilizan acelerómetros tipo MEMS, que poseen numerosas ventajas para esta aplicación, como ser buen ancho de banda, suficiente resolución, peso reducido, robustez y bajo costo. Mediante estos sensores es posible determinar la dirección y el sentido del vector de aceleración de la gravedad respecto a un marco de referencia fijo en la aeronave. La desventaja fundamental de estos sensores radica en que las mediciones son fuertemente afectadas por las vibraciones en la estructura donde se encuentran montados.

Otra alternativa para medir la inclinación es computar la rotación integrando la señal de sensores giroscópicos. Por las mismas razones que el caso anterior, los dispositivos de tecnología MEMS resultan los más apropiados para esta aplicación, aunque si bien son más inmunes a las vibraciones presentan derivas sostenidas en el tiempo debido a la integración de los errores de offset.

Ensayo individual de los sensores

Para los ensayos que se describen a continuación se construyó un montaje experimental que consta de un balancín con un propulsor en cada extremo, un potenciómetro en el pivot y el sensor con el sistema de control montado en el centro. Este montaje con un grado de libertad permite simular el comportamiento dinámico aproximado del sistema respecto al rolido o el cabeceo. Aparte del sistema mecánico, el montaje cuenta con un controlador PID digital implementado en el microcontrolador, el cual cierra el lazo del sistema a través del control de la velocidad de los motores. Para lograr una notación consistente supondremos que el ángulo a medir es el de rolido (

).

Acelerómetro

En el desarrollo experimental se utilizó el acelerómetro ADXL345 de tres ejes dispuesto de manera que los ejes de medición del mismo coincidan con los ejes del sistema de referencia del UAV. De esta manera el ángulo se obtiene como:

,

donde la aceleración

se mide con una resolución de 10 bits, y un ancho de banda ajustable por software desde 0.05 Hz. hasta 1600 Hz.

Las siguientes muestran comparativamente las señales adquiridas del sistema a lazo abierto en estado de equilibrio con una inclinación de 10º con los propulsores apagados y con los propulsores encendidos (50\%) respectivamente.

Angulo medido con el giroscopio, con el montaje en equilibrio y los propulsores apagados.

Ángulo medido con el acelerómetro, con el montaje en equilibrio formando un ángulo de 10º y los propulsores encendidos al 50% de su potencia:

Respuesta en frecuencia del sistema completo a lazo cerrado:

Respuesta temporal del sistema de lazo cerrado de tipo regulador con una perturbación sumada a la medición ideal de de amplitud para frecuencias de 5Hz y 50Hz:

Frecuencia de la perturación de 5Hz:

Frecuencia de la perturación de 50Hz:

closeLoop_tpo_rta02_v1.ps

Respuestas temporales del sistema a lazo cerrado de para señales no ruidosas, pero previamente pasadas por un filtro pasa bajos:

Sin filtrar:

• :

Si bien el sistema en lazo cerrado formado por el balancín más el controlador PID presenta un buen rechazo a las componentes ruidosas de alta frecuencia de las vibraciones, esto se aprecia en la curva de bode simulada para un sistema ideal y en las curvas de respuesta en el tiempo del mismo sistema que se ven en las figuras anteriores, las variaciones bruscas de la señal de referencia producen acciones de control, debidas a la rama del derivador del compensador, que llevan a los motores a zonas no lineales (saturación o corte), provocando la inestabilidad del sistema real.

El filtrado de la señal ruidosa mediante un filtro pasa-bajos introduce otras desventajas: si bien se pueden mantener acotadas las derivadas en la señal, la atenuación y el corrimiento de fase degradan la respuesta dinámica e incluso provocan inestabilidad cuando las frecuencias de corte son demasiado bajas. La Fig. \ref{fig:closeLoop_tpo_rta03} muestra la respuesta temporal del sistema para una medición no ruidosa y no filtrada, en la Fig. \ref{fig:closeLoop_tpo_rta04} la misma señal se pasa por un filtro pasa-bajos de 5 Hz de frecuencia de corte, finalmente si se disminuye la frecuencia de corte del mismo a 2 Hz el sistema se desestabiliza, tal como lo muestra la gráfica de simulación de la Fig. \ref{fig:closeLoop_tpo_rta05}.\\ Como consecuencia de las características ruidosas de las mediciones adquiridas con el acelerómetro no sería posible utilizarlo como inclinómetro en el sistema de estabilización. A raíz de esto se evalúa la posibilidad de utilizar un giróscopo.

\subsection{Giróscopo}

Para realizar los siguientes experimentos se utilizó un giróscopo de 3 ejes ITG\-3200 de la firma InvenSense. Éste se dispuso de manera que sus ejes estén alineados con los del sistema de referencia de abordo. Este dispositivo posee un rango dinámico de y una sensibilidad de . En las Fig. \ref{fig:gyromotorapagado}, \ref{fig:gyromotorapagadofft}, \ref{fig:gyromotorencendido},\ref{fig:gyromotorencendidofft} se observan comparativamente las señales en el tiempo y el espectro para el caso que los propulsores están apagados y encendidos con la misma configuración que la descripta para el caso de la medición con el acelerómetro. Haciendo una comparación de la Fig. \ref{fig:acel_ence_25_tpo} con la Fig. \ref{fig:gyromotorencendido} se puede ver que la influencia de las vibraciones en las mediciones del giróscopo son sensiblemente menores que en el caso del acelerómetro. Sin embargo se han observado intérvalos de tiempo relativamente cortos, la Fig. \ref{DerivaGyro} muestra que para tiempos mayores la medida de este sensor comienza a derivar.

\begin{figure}[ht]

• \centering \subfloat[Ángulo de rolido en función del tiempo.]{\label{fig:gyromotorapagado}\includegraphics[scale=.45]{figuras/gyromotorapagado_v1.ps}}\\ \subfloat[Espectro de frecuencias.]{\label{fig:gyromotorapagadofft}\includegraphics[scale=.45]{figuras/gyromotorapagado_fft_v1.ps}}\\ \caption{Ángulo medido con el giroscopio, con el montaje en equilibrio y los propulsores apagados.} \label{}

\end{figure}

\begin{figure}[ht]

• \centering \subfloat[Ángulo de rolido en función del tiempo.]{\label{fig:gyromotorencendido}\includegraphics[scale=.45]{figuras/gyromotorencendido_v1.ps}}\\ \subfloat[Espectro de frecuencias.]{\label{fig:gyromotorencendidofft}\includegraphics[scale=.45]{figuras/gyromotorencendido_fft_v1.ps}}\\ \caption{Ángulo medido con el giroscopio, con el montaje en equilibrio y los propulsores encendidos al 50\% de su potencia..} \label{}

\end{figure}

%\begin{figure}[t] % \begin{center} % \includegraphics[scale=.45]{figuras/gyromotorencendido_v1.ps} % \end{center} % \caption{} % \label{fig:gyromotorencendido} %\end{figure} %\begin{figure}[ht] % \begin{center} % \includegraphics[scale=.45]{figuras/gyromotorencendido_fft_v1.ps} % \end{center} % \caption{} % \label{fig:gyromotorencendidofft} %\end{figure}

\begin{figure}[ht]

• \centering \includegraphics[width=3in]{figuras/Gyro_Pote_deriva_v1.ps} \caption{Deriva del ángulo medido por el giróscopo}

  \label{DerivaGyro}

\end{figure}

\section{Filtro Complementario} En forma intuitiva surge la idea de usar la medición obtenida por el giróscopo para tiempos cortos y realizar la corrección de la deriva con la medición realizada por el acelerómetro en tiempos largos, debido a que esta última medición tiende a ser la aceleración de la gravedad para períodos largos. Los filtros complementarios son muy usados en sistemas de navegación inercial. Aplicaciones típicas son la combinación de las medidas de aceleración vertical y velocidad barométrica vertical para obtener una estimación de la velocidad vertical o mediciones de unidades inerciales y sistemas de visión. \cite{buskey2004helicopter}

Un filtro complementario es en sí un filtro de Kalman de estado estacionario para una cierta clase de problemas de filtrado \cite{higgins2007comparison}, este no considera ninguna descripción estadística del ruido que corrompe a las señales y es obtenido solamente por un análisis en el dominio de la frecuencia. El filtro complementario resulta sencillo de tratar matemáticamente y en razón de su baja complejidad de implementación consume pocos recursos computacionales. La idea básica del filtro complementario es combinar la salida del acelerómetro y del giróscopo para obtener una buena estimación del ángulo de orientación de la plataforma, compensando la deriva del giróscopo con la dinámica lenta del inclinómetro \cite{baerveldt2002low}.

El filtro complementario propuesto es el que se muestra en la figura \ref{comp_usado}. Donde es el ángulo medido por el acelerómetro cuya señal esta corrompida por ruidos de alta frecuencia proveniente de las vibraciones, es el ángulo medido por el giróscopo, afectado por la deriva y es el ángulo estimado.

\begin{figure}[ht]

• \centering \includegraphics[width=3in]{figuras/comp_usado_1_v1.ps} \caption{Diagrama en bloques del filtro complementario utilizado.} \label{comp_usado}

\end{figure}

Las funciones de transferencia del filtro deben ser elegidas de acuerdo a la ecuación \ref{equ1}:

\begin{equation}

• \label{equ1} H_{a(s)}G_{(s)} + H_{g(s)}(1-G_{(s)}) = 1

\end{equation}

en donde y representan las funciones de transferencia del acelerómetro y el giróscopo respectivamente.

Suponemos que las funciones de transferencia de las sensores son iguales a 1. Esto es .

La función de transferencia elegida para es un filtro pasa bajos de primer orden, lo cual hace que la estimación en baja frecuencia dependa de la medición del acelerómetro.

\begin{equation}

• G_{(s)} = \frac{\alpha}{s+\alpha}

\end{equation}

Y la función de transferencia para :

\begin{equation}

• 1-G(s) = \frac{s}{s + \alpha}

\end{equation}

• este filtro nos permite hacer que las componentes de alta frecuencia de la medición estimada estén dominadas por el aporte de las mediciones provenientes del giróscopo.

Como podemos ver en la figura \ref{comp_usado}, si ambas mediciones son ideales, la función de transferencia total del filtro resulta:

\begin{equation}

• \frac{\hat{\theta}_{(s)}}{\theta_{(s)}} = G_{(s)} + ( 1- G_{(s)}) = 1

\end{equation}

Y esto hace que:

\begin{equation}

• \hat{\theta}_{(s)} = \theta_{(s)}

\end{equation}

%En la Fig. \ref{fig:filt_comp_teorico} se muestran las gráficas correspondientes a una señal libre de ruido (línea punteada) y de frecuencia igual a %la de corte de los filtros en que se inyecta, ambos calculados según la premisa anterior y se ve la salida de cada rama; %con pequeños círculos se grafica la señal filtrada por el filtro pasa-bajos en tanto que con estrellas se grafica la %salida del filtro pasa-altos. La suma de estas dos salidas se encuentra superpuesta con la señal de entrada. %\begin{figure}[ht] % \begin{center} % \includegraphics[scale=.4]{figuras/filt_comp_teorico.ps} % \end{center} % \caption{Filtrado de señal libre de ruido} % \label{fig:filt_comp_teorico} %\end{figure}

• \subsection{Discretización de los filtros} Para la implementación de los filtros digitales en el microcontrolador se parte discretizando las funciones de transferencia de los mismos usando la transformada z y suponiendo un retenedor de orden cero a la entrada. Con esto se obtiene una expresión compacta para el filtro completo.

  Si definimos: y , entonces:

\begin{equation}

• G_1(z) = \frac{(1-e^{{\frac{-T}{\tau}})z}{-1}}{1 - e^{{\frac{-T}{\tau}}z}{-1}}

\end{equation}

\begin{equation}

• G_2(z) = \frac{1- z^{{-1}}{1 - e}{\frac{-T}{\tau}}z^{-1}}

\end{equation}

• En donde y representa la constante de ambos filtros. \subsection{Algoritmo del microcontrolador} El algoritmo del microcontrolador surge del paso a ecuaciones en diferencias de las funciones de transferencia de cada filtro.

\begin{equation}

• \hat{\theta}_{[k]} = \hat{\theta}_{a[k]} + \hat{\theta}_{g[k]}

\end{equation}

\begin{equation}

• \hat{\theta}_{a[k]} = e^{{\frac{-T}{\tau}}\hat{\theta}_{a[k-1]} + (1-e}{\frac{-T}{\tau}})\theta_{a[k-1]}

\end{equation}

\begin{equation}

• \hat{\theta}_{g[k]} = e^{\frac{-T}{\tau}}\hat{\theta}_{g[k-1]} + \theta_{g[k]} - \theta_{g[k-1]}

\end{equation}

\begin{equation}

• \omega_{[k]}T = \theta_{g[k]} - \theta_{g[k-1]}

\end{equation}

\begin{equation}

• \hat{\theta}_{g[k]} = e^{\frac{-T}{\tau}}\hat{\theta}_{g[k-1]} + \omega_{[k]}T

\end{equation}

\section{Resultados de la implementación} Se ensayó el sistema utilizando el filtrado complementario verificándose una buena estimación del ángulo que resultó en la correcta estabilización y una buena respuesta dinámica.\\ En la figura \ref{ensayo1} se muestran las mediciones individuales del acelerómetro y el giróscopo; y la estimación del filtro complementario; mientras se sometió al sistema a perturbaciones forzándolo a salir de la posición de equilibrio y permitiéndole que se restituya en forma autónoma. Como se observa en la gráfica se calibró débilmente el offset del giróscopo para permitir una deriva exagerada a fin de mostrar el rechazo del filtro. En la Figura \ref{fig:FiltroPote} se observa la medida del ángulo estimada por el filtro contrastada contra la medición del potenciómetro durante la evolución del sistema desde un ángulo inicial de -10 grados hasta el equilibrio en cero grado. En este ensayo se utilizó la medida del filtro para la realimentación del lazo de control y se ajustaron las constantes del controlador PID. Este ajuste se realizó

• según los análisis de estabilidad hechos del sistema a lazo cerrado para obtener la respuesta subamortiguada que se observa, aunque hubo que realizar algunos retoques empíricos los cuales se pueden deber a que se idealizaron algunas partes del sistema y también a falta de una buena calibración de los sensores y propulsores.

\begin{figure}[ht]

• \centering \includegraphics[scale=.5]{figuras/ensayo1_v1.ps} \caption{Mediciones y estimación durante ensayo del sistema.} \label{ensayo1}

\end{figure}

\begin{figure}[ht]

• \centering \includegraphics[width=3in]{figuras/Filtro_vs_Pote_v1.ps} \caption{Respuesta dinámica del sistema ante una perturbación}

  \label{fig:FiltroPote}

\end{figure} \section{Conclusiones}

Se ensayaron tres métodos de medición de inclinación de una plataforma experimental que se comporta como el UAV con cinco grados de libertad restringido, permitiendose rotaciones que corresponderían al cabeceo o el rolido de la aeronave. Los dos primeros métodos ensayados utilizaron acelerómetro por un lado y giróscopo por otro de manera individual. En el primer caso la influencia de las vibraciones impide el correcto funcionamiento del lazo de control, en tanto que si se filtra la señal de la medición se degrada la respuesta dinámica o incluso se llega a la inestabilidad del sistema para frecuencias de corte demasiado bajas. Por el lado del giróscopo se obtuvo mejores resultados en el comportamiento del lazo de control, no obstante esto la deriva producida por el offset del sensor hace que el error de inclinación se incremente de manera constante en el tiempo. Ninguno de los dos métodos resulta viable para el control de actitud del UAV.\\ Finalmente se aplicó un filtro complementario para combinar la medición de ambos sensores tomando de cada uno la parte del espectro de la señal de medición menos corrompida y atenuando la otra a fin de que la suma permita una estimación aproximada de la variable de interés. El comportamiento dinámico del sistema realimentado con la medición estimada por el filtro resulta satisfactorio para ser aplicado al prototipo real.