MEDIR LA

INCLINACIÓN CON IMU,

ARDUINO Y FILTRO
COMPLEMENTARIO

7 SEPTIEMBRE, 2016

En entradas anteriores hemos visto el funcionamiento de acelerómetros y giroscopios como

sensores que pueden ser empleados para determinar la orientación de un dispositivo.

También vimos las ventajas y limitaciones de cada uno de los dispositivos, y adelantamos
que se combinan muy bien porque las características de su medición se complementan.

En esta última entrada de la serie veremos los IMU’s, dispositivos que combinan las
ventajas de ambos sensores para obtener mejores funcionalidades que la que podríamos obtener
usando un acelerómetro o giroscopio independientemente.

En la actualidad su bajo coste ha hecho que sean componentes frecuentes en una gran
cantidad de aparatos cotidianos. Existen muchos fabricantes de IMU’s, como Panasonic,
Robert Bosch GmbH, InvenSense, Seiko Epson, Sensonor, STMicroelectronics, Freescale
Semiconductor, y AnalogDevices.
 Nosotros los usaremos frecuentemente en nuestros proyectos de electrónica y Arduino, por
ejemplo, para determinar la dirección de avance de un vehículo, la orientación de un accionador,
la inclinación de una plataforma, o incluso como forma de actuar con un ordenador con un guante
o un ratón 3D.

¿QUÉ ES UN IMU?
Una unidad de medición inercial (IMU) es el nombre genérico para denominar a un
dispositivo que es capaz de medir la velocidad, orientación y aceleración de un sistema.

En realidad, los acelerómetros y giroscopios pueden ser considerados como IMU’s simples pero, en
general, el nombre de IMU se reserva para dispositivos que integran más de un sistema de
medición.
Al hablar de IMU’s es frecuente hacer referencia a la cantidad de grados de libertad (DOF)
que dispone. Los grados de libertad de un sensor representan la cantidad de magnitudes
independientes que es capaz de medir.

Así, un acelerómetro de 3 ejes ortogonales es un sensor de 3 DOF. Así mismo, un giroscopio

que mida ángulos en 3 ejes ortogonales es un sensor de 3DOF.

Los IMU’sque encontraremos más frecuentemente son:

 IMU de 6 DOF, combinando un acelerómetro de 3 ejes y un giroscopio de 3 ejes.

 IMU de 9 DOF, añadiendo una brújula magnética de 3 ejes.

 IMU de 10 DOF, que añade un barómetro para la estimación de la altura del sensor.

Como vemos, la mayoría de los IMU’s parten de una combinación de acelerómetro y

giroscopio, siendo este el IMU más habitual. El motivo es que ambos dispositivos combinan muy
bien y compensan las limitaciones del otro.

 Los acelerómetros no tienen deriva (drift) a medio o largo plazo, ya que realizan la
medición medida absoluta del ángulo que forma el sensor con la dirección vertical, marcada
por la gravedad. Sin embargo, se ven influenciados por los movimientos del sensor y el ruido
por lo que no son fiables a corto plazo.

 Los giroscopios funcionan muy bien para movimientos cortos o bruscos, pero al usar
giroscopios de vibración que realmente miden la velocidad angular, y obtienen el ángulo por
integración respecto al tiempo, acumulan los errores y el ruido en la medición, por lo que a
medio o largo plazo tienen deriva (drift).

Por tanto combinar las mediciones de ambos dispositivos permite a las IMU obtener
mediciones de la orientación más precisas que la de un acelerómetro y un giroscopio por
separado. Veremos esto a continuación, pero antes haremos una reflexión sobre que es la
orientación.
OBTENER LA ORIENTACIÓN
CON UN IMU
Para determinar la distribución de un objeto respecto a una base en un espacio
tridimensional deberemos establecer su posición y orientación. Se requieren tres parámetros
para determinar la posición relativa y otros tres para determinar su orientación.

En la vida cotidiana estamos acostumbrados a tratar la posición de un objeto, y sabemos que

existe más de una forma de expresar su posición relativa matemáticamente. Por ejemplo, podemos
expresar sus coordenadas respecto en otra base, mediante coordenadas cartesianas (X, Y, Z),
cilíndricas o esféricas.

Sin embargo, no estamos tan acostumbrados a trabajar con la parametrización de la

orientación, ya que matemáticamente es algo más compleja. Al igual que la posición, existe más
de una forma de expresar la orientación relativa de dos sistemas, como por ejemplo los
ángulos de Euler, las matrices de rotación o los cuaterniones.

Seguramente la más extendida e intuitiva son los ángulos de navegación, o ángulos de Tait-
Bryan, en los que la orientación se representa como tres rotaciones ortogonales en torno al
eje X (roll), Y (pitch), y Z (yaw).

Los ángulos de Tait-Bryan son una variación de los ángulos de Euler, introducidos por el
matemático Leonhard Euler durante su estudio sobre la mecánica del sólido rígido.

Es frecuente encontrar incorrectamente los ángulos de Tait-Brayn denominados como ángulos de

Euler. Los ángulos de Euler son una formulación más general. Otra diferencia es que los ángulos de
Euler toman como origen de la medición la vertical, mientras que Tait-Brayn mide los ángulos
respecto a la horizontal.
Los ángulos ángulos de Tait-Bryan son intuitivos y sencillos. Además tienen la ventaja de
ser conmutativos, es decir, la orientación final obtenida es independiente de orden en el que
apliquemos la rotación (algo que en normalmente no ocurre en la representación de rotaciones.
 Sin embargo, adolecen de una gran desventaja denominada bloqueo cardán (gimbal
lock)que sufren todos los sistemas de ángulos de Euler. Cuando el eje Y gira +-90º, el eje X y el
Z coinciden por lo que el sistema degenera a 2DOF, lo cuál da lugar a un punto singular no
diferenciable que puede suponer problemas de estabilidad en los cálculos.

El gimbal lock es una desventaja tan importante que la forma habitual de representar la
rotación mediante el uso de cuaterinones. Los cuaterniones son una extensión de los números
complejos introducida por Hamilton en 1843, que emplean tres unidades imaginarias i, j, y k tal
que,

Es sabido que números complejos pueden usarse para representar vectores en el plano. De
forma equivalente, un cuaternión permite expresar un vector en el espacio tridimensional.

Para expresar la rotación de un punto, un ángulo α, en torno a un vector arbitrario de

coordenadas Ex, Ey, Ez, el cuaternión resultante adopta la siguiente expresión

Comparados con los ángulos de Euler, los cuaterniones son más simples de componer y
evitan el problema del bloqueo del cardán. Comparados con las matrices de rotación, son más
eficientes y más estables numéricamente

Por este motivo los cuaterniones son una de las formas más habituales de expresar la
orientación en gráficos de ordenador y serán la forma preferida a emplear cuando queramos
trabajar con orientaciones en IMU’s y en proyectos de Arduino.

FILTRADO DE SEÑAL EN UN
IMU
Para poder tener las ventajas en tiempos cortos del giroscopio y las ventajas a medio y largo
plazo del acelerómetro es necesario que combinemos y filtremos la señal registrada en bruto
(RAW).

Existen varios filtros posibles siendo el más famoso el filtro de Kalman, desarrollado en
1960 por Rudolf E. Kalman. Es considerado como uno de los grandes descubrimientos del siglo
XX por sus implicaciones en el proceso de filtrado en sensores (no solo IMU’s) y es uno de
artífices de la carrera espacial.

A grandes rasgos el filtro de Kalman realiza una estimación del valor futuro de la medición,
y después compara el valor real mediante un análisis estadístico para compensar el error en futuras
mediciones.
 Sin embargo, el filtro de Kalman en su versión general implica la realización de cálculos
complejos (ver Wikipedia) que suponen una implementación y tiempo de cálculo excesivo
para Arduino.

Por este motivo es frecuente emplear un filtro más sencillo denominado filtro
complementario. En realidad, el filtro complementario puede considerarse una simplificación
del filtro de Kalman que prescinde por completo del análisis estadístico.

Existen varias formulaciones para un filtro complementario. En su expresión más sencilla,

el filtro complementario puede expresarse.

Donde A y B son dos constantes que, inicialmente, puede tomarse 0.98 y 0.02
respectivamente. Podemos calibrar el filtro simplemente variando los valores de A y B siempre
que cumplamos la condición de que sumen 1 entre ellos.

El filtro complementario se comporta como un filtro de paso alto para la medición del
giroscopio y un filtro de paso bajo para la señal del acelerómetro. Es decir, la señal del giroscopio
manda a corto plazo, y la del acelerómetro y medio y largo, que es exactamente lo que queremos
para compensar sus ventajas y defectos.
 En la mayoría de aplicaciones domésticas, el filtro complementario es suficiente y
proporciona valores muy similares al filtro Kalman, siempre que los valores de A y B esten
debidamente calibrados.

Existen versiones más sofisticadas del filtro complementario, así como versiones
simplificadas unidimensionales del filtro de Kalman que sí pueden ser implementadas en
Arduino. Veremos esto en una próxima entrada avanzada.

Mientras tanto, emplearemos las IMU’s como el MPU6050 y los filtros empleados en
nuestros proyectos de electrónica y Arduino.
&%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

PCB cuadrotor (sin cepillo) (11 / 20 paso)

Paso 11: Ejes y orientación

Antes de entrar en detalles sobre el control de la cuadrotor, es importante llegar a una

definición consistente de las direcciones, ejes y orientación para el quad. La imagen
muestra el sistema de coordenadas utilizado en 4pcb. Es un cuadrotor "+ modo", lo que
significa que la dirección de avance es paralela a uno de los brazos del motor. (La
alternativa es "x-mode", donde hacia adelante es directamente entre dos brazos, y todo lo
que se gira 45 º.)
La imagen también muestra el chip minIMU-9 en la orientación que se montarían en el
cuadrotor. (Fuente imagen: http://www.pololu.com/picture/view/0J3680.)
En + modo de:
-La dirección de "frente" corresponde al eje de las X de la IMU.
-La dirección "izquierda" corresponde al eje de la IMU.
-La dirección de "up" corresponde al eje de Z de la IMU.
Tenga en cuenta que el sistema de coordenadas utilizado es un derecho, así:
-Que los dedos de la mano derecha de X a Y, sus puntos del pulgar en la dirección Z.
-Positivos de rotación se define como la dirección de los dedos curl si agarras el eje con el
pulgar apuntado en la dirección positiva.
También tenga en cuenta que:
-Pitch positivo está lanzando hacia abajo.
-Rodillo de rueda a la derecha positivos.
-Yaw positivo es desviación a la izquierda.

PCB cuadrotor (sin cepillo) (12 / 20 paso)

Paso 12: Comando matriz

Un cuadrotor tiene cuatro motores que pueden girar a diferentes velocidades, por lo que
parece razonable que tiene cuatro "grados de libertad" que puede ser controladas. Los
cuatro gobiernos controlables son:
1. válvula reguladora
2. pitch
3. rodillo
4. desvío
Cómo esas entradas de control son derivados y como comandos de radio para el
cuadrotor serán cubiertos en un paso posterior. Este paso se centra en cómo llegar desde
los cuatro comandos (las entradas) a las cuatro velocidades de motor (las salidas).
Puede ver la imagen en el paso anterior e intuitivamente adivinar cuál debe ser la
asignación de entrada/salida para un cuadrotor. Es más fácil de representar como una
matriz de 4 x 4, que es simplemente una tabla donde las filas son las entradas y las
columnas son salidas (o viceversa). Imagen de este paso muestra la matriz de comando
para un cuadrotor + modo con hélice direcciones indicadas en imagen del paso anterior.
[+] Indica que el motor debe ser acelerado en respuesta a un comando de entrada positivo.
[-] Indica que el motor debe ser retrasado en respuesta a un comando de entrada positivo.
[0] indica que la velocidad del motor se ve afectada por un comando de entrada positivo.
La válvula reguladora, echada y rodillo trabajo aumentando o reduciendo el empuje
producido por los motores especiales para crear un esfuerzo de torsión neto en el marco
de cuadrotor. Obras de desvío en un principio diferente, con par de reacción del motor
para girar el bastidor. El par de reacción para cada motor es en sentido contrario a la
hélice y es proporcional a la resistencia vista por el propulsor que empuja a través del aire.

PCB cuadrotor (sin cepillo) (13 / 20 paso)

Paso 13: Retroalimentación Control

En un mundo ideal, podría tomar las cuatro entradas (acelerador, pitch, roll, desvío) y
directamente se les asignan a las cuatro salidas motor utilizando la matriz de comandos
del paso anterior. Pero hay algunas cosas que hacen poco práctico hacerlo en la vida real:
1. disturbios tales como viento y no ideales tales como diferencias en los motores y hélices
causa la dinámica de la vida real ser ruidoso y variable. Asignación de mando directo no
toma en cuenta, y nuestra mente, ojos y manos podrían no ser lo suficientemente rápidas
como para reaccionar a éstos en tiempo real, sobre todo en un pequeño cuadrotor.
2. queremos cuadrotor tener cierto grado de "autonomía". En particular, sería bueno si el
cuadrotor podría uno mismo-nivel, volviendo a casi horizontal cuando mandamos cero
pitch, roll y yaw. Con asignación directa, estamos ordenando la cuadrotor para girar pero
no sabe volver a la horizontal cuando hemos terminado.
Esto es donde control de regeneración . A pesar de sus notaciones matemáticas
complejas, el concepto de control por realimentación es simple. Imagine que lavarse las
manos en un fregadero que nunca has usado antes. Se establece el mando del grifo en
alguna posición intermedia. Si el agua 's demasiado frío, usted súbalo. Si el agua 's
demasiado caliente, gire hacia abajo. Cuánto usted gire hacia arriba o hacia abajo
depende de cuán caliente o frío se compara a tu gusto. Si el agua comienza frío, pero
rápidamente se calienta, usted puede preventivamente girar la perilla abajo a prevenirla de
superación y quema las manos. Todos estos conceptos son matemáticamente
formalizados en el control de retroalimentación.
Control de retroalimentación en un cuadrotor:
Una estructura común para el control de retroalimentación se denomina control PID
(proporcional-Integral-derivado). Lo que se desea controlar en este caso es
el ángulo (pitch, roll o ángulo de desvío) de la cuadrotor. Esto es análogo a la temperatura
del agua en el fregadero. Con ningún comando entrada, tratamos de controlar el ángulo
para ser cero. Sin embargo, también podemos mandar a un ángulo distinto de cero para
mover el cuadrotor. Los comandos que envía a los motores se basan en el error entre el
ángulo que queremos y el ángulo que realmente tienen, como es medido por la IMU.
- Proporcional (P): el comando es proporcional a cuánto error angular tenemos. Ayuda a
devolver la cuadrotor cero ángulo, o empujar a cualquier ángulo que te mando.
-Integral (I): El comando es proporcional al error acumulado en el tiempo. Puede ayudar a
luchar contra los disturbios como el viento o el rendimiento del motor asimétrico. No utilice
esto en mi quad, aunque dejé código de marcador de posición para en el proyecto de
Arduino.
- Derivado (D): el comando es proporcional a la tasa de cambio del error *. Resiste el
movimiento y mantiene el ángulo de la superación de la meta.
Analogía masa-resorte-amortiguador:
Los efectos del control PD (sin término integral) en un cuadrotor son similares a agregar un
resorte virtual y un regulador de tiro virtual (amortiguador) a la cuadrotor, que es la masa.
Ver la primera imagen para una representación gráfica de esto. La rigidez se establece por
una constante, Kp, la "ganancia proporcional". La tasa de amortiguación se encuentra por
otra constante, Kd, el "beneficio derivado". El ángulo θr, define el ángulo en el que los
resortes se estiran uniformemente. Imaginar los resortes a una tabla movible y la tabla
para ordenar cuadrotor para ir a un determinado ángulo de rotación.
-Aumento de Kp empuja el cuadrotor hacia el ángulo de referencia más rápido, pero
también puede resultar en más overshoot y oscilación.
-Aumento de Kd se ralentiza la velocidad de rotación de la cuadrotor, pero puede también
húmedo a oscilaciones.
Tenga en cuenta que las ganancias se pueden aumentar a un punto donde este modelo se
rompe. Si Kp, Kd, o ambos son demasiado altos, el controlador iniciará amplificar ruido,
conduce a inestabilidad y oscilaciones. Estas oscilaciones tienden a ser en una frecuencia
más rápida que las oscilaciones que se vería desde una alta Kp: relación entreKd . Si ves
oscilaciones rápidas, la mejor cosa a hacer es bajar ambos beneficios.
Las ganancias requiere práctica y experiencia y depende del exacto y preferencia de
vuelo. Una muy buena guía para PID Sintonía multirotors, con vídeo ejemplo para mostrar
los distintos tipos de oscilaciones, pueden encontrarse aquí. Que la analogía del
resorte/apagador le ayuda a pensar intuitivamente las ganancias.
* En realidad, en la práctica a veces es más fácil utilizar el índice medido de rotación
directamente, en lugar de intentar medir la tasa de cambio del error. El error sólo existe en
software y computación que el derivado de la misma puede ser ruidoso. Prácticamente
hablando, utilizando la tasa medida de rotación directamente en el giroscopio funciona muy
bien. Para esto en la analogía masa-resorte-amortiguador, los amortiguadores están
conectados a tierra (ángulo cero) en vez de a la tabla. En esta configuración, resisten toda
la rotación, incluso mandó a rotación.
Pitch, Roll y Yaw controladores:
El cuadrotor tiene en realidad tres controladores de retroalimentación independientes, uno
para pitch, roll y yaw. Acelerador se asigna directamente a los cuatro motores sin control
de retroalimentación en este cuadrotor. Con un sensor de altitud, podría añadirse un cuarto
regulador de retroalimentación.
Los controladores de la echada y del rodillo son controladores PD que coincidan
exactamente con la primera imagen. En la medida en que el cuadrotor es simétrica, las
ganancias de la echada y del rodillo deben ser el mismo. Las salidas de la proporcional y
de los beneficios derivados se suman juntos y enviadas a los motores a través de la matriz
de comando en el paso anterior. La segunda imagen muestra los controladores PD echada
y el rodillo en forma de diagrama de bloque. Las variables son:
Kp - ganancia proporcional
Kd - derivado de la ganancia
Θr - ángulo de referencia o el comando
Θ - ángulo medido de la IMU
Ω - medida tasa de rotación de la IMU.
y - el comando de salida.
El control de guiñada se basa sólo en tarifa, por lo que no coincide exactamente con las
imágenes. El error es la diferencia entre la tasa de desvío ordenada y la medida del desvío
de la IMU. El magnetómetro en la IMU podría utilizarse para medir el ángulo de desvío
absoluto para implementar un completo controlador PD, pero no he probado esto todavía.

PCB cuadrotor (sin cepillo) (14 / 20 paso)

Paso 14: IMU parte 1: sensores
Control de regeneración sólo es útil si hay una señal de medición de calidad para alimentar
nuevamente. En el caso de un cuadrotor, es fundamental para tener una medida exacta
de ángulo y velocidad angular. Esto requiere dos tipos de sensores. ¿Suena familiar? Yo
no estaba bromeando cuando dije que quadrotors están el nuevo Segway. De hecho, son
más como dos Segways vuelos, uno para echada y rodillo. Cada eje tiene su propia
medida de acelerómetro y giroscopio, y la forma de la señal de estos dos sensores se
puede combinar es idéntica a lo que utilicé en el Seg... Instructable del palillo y en
este libro blanco. Antes de mirar el filtro complementario, sin embargo, es importante
entender lo que cada uno de los sensores hace:
El acelerómetro:
El acelerómetro no medir aceleración. Mide la fuerza por unidad de masa a lo largo de
cada eje. Puede utilizar esta medida para determinar la dirección de la gravedad, que
ejerce una fuerza hacia abajo, como el cuadrotor no está acelerando.
El Polulu minIMU-9 es mucho menos problemas que el sensor analógico original porque es
la puesta a cero y reporta un valor digital con un factor de escala conocido. El factor de
escala es incluso un buen número simple: 1 mg/dígito con el rango más sensible. Es
decir, 1/1000 la aceleración debido a la gravedad por dígitos. Tengo este valor
directamente en la hoja de datos del acelerómetro. Si señalas con el sensor eje X
directamente hacia abajo, debe informar un valor de 1.000.
Conversión de una medida de la aceleración a un ángulo requiere un montón de
trigonometría o una linearización. Puesto que el cuadrotor pasa la mayor parte de su
tiempo horizontalmente y se desvía por más de 30 º en cualquier dirección, elegí la
linearización. La primera imagen muestra cómo alinear sobre 0 º. La pendiente de la línea
que es tangente a la salida del sensor vs curva de ángulo a 0 º es de 1000digit/rad. El
inverso es lo que se llama A_GAIN en el código:
A_GAIN = 0.001 rad/dígito = 0.0573º / dígitos.
Este es el número por el que multiplicar el valor bruto acelerómetro para conseguir un
ángulo aproximado en grados.
El giro:
El giroscopio no es realmente un giroscopio; no hay volantes de giro dentro de la viruta. Se
llama más exactamente un sensor de velocidad angular y utiliza una estructura de horquilla
de MEMS a medida tasa de rotación basado en la fuerza de Coriolis. Como el
acelerómetro, hay uno para cada eje.
Ya que este es un bonito IMU digital, el valor reportado es directamente en unidades
físicas, aunque el factor de escala es un poco extraño. Se da en la hoja de datos para el
giro como 8.75mdps /, o 0.00875(º/s)/dígitos. Utilizar este valor directamente como
G_GAIN en el código.
G_GAIN = 0.00875(º/s)/digit.
Este es el número por el que multiplicar el valor del crudo giro para obtener una velocidad
angular en grados por segundo.
Motivación a la fusión de sensores para calcular el ángulo:
El giróscopo mide directamente velocidad angular, por lo que la información sobre el ritmo
es atendido por los giroscopios solo. De hecho debería ser posible volar el cuadrotor sólo
información sobre el ritmo, pero necesitaría una estructura de control diferente y no
autonivela. Para obtener el control de buena actitud, comentarios de ángulo también son
necesario.
Desafortunadamente, la estimación de acelerómetro de ángulo no es perfecta. Aceleración
horizontal alterará el cálculo de ángulo, ya que el acelerómetro no puede distinguir la
diferencia entre aceleración hacia delante y que se inclina hacia atrás. El cuadrotor no
pasa mucho tiempo acelerando horizontalmente, pero cuando lo hace, puede fácilmente
tirar grandes fracciones de 1g, que alterará sustancialmente la estimación del ángulo.
El giro también puede producir una estimación del ángulo, por Integración numérica: si
sabes qué tan rápido está girando, puede predecir qué ángulo le estará en el paso del
tiempo multiplicando la velocidad por el tiempo de la muestra. Esta estimación del ángulo
puede trabajar por períodos cortos de tiempo, pero durante mucho tiempo una se deriva
del ángulo verdadero. No importa lo buena y bien cero el giro es, Integración numérica
siempre hará que la estimación del ángulo producida por el giro solo a la deriva con el
tiempo.
Así, el acelerómetro es malo para la estimación de ángulo a corto plazo, pero proporciona
a un equivalente promedio a largo plazo estable al vector gravedad. El giroscopio es bueno
para los cálculos de ángulo a corto plazo, pero se deriva durante largos períodos de
tiempo. El filtro complementario, en el siguiente paso, se combina los sensores juntos
suavemente en diferentes escalas de tiempo para obtener una mejor estimación del ángulo
de cada sensor solo podría producir.

PCB cuadrotor (sin cepillo) (15 / 20 paso)

Paso 15: IMU parte 2: filtro complementario
Mi método de elección para combinar las señales de acelerómetro y giroscopio en una
estimación del ángulo limpio y estable es el filtro complementario. Es computacionalmente
eficiente y dirige directamente a aceleración horizontal transitoria y a largo plazo deriva de
giro. La idea es utilizar el giroscopio para ángulo de estimaciones a corto plazo, por
Integración numérica, pero el interruptor para el acelerómetro para estimaciones a largo
plazo, por un promedio de. Para hacer esto de manera continua, se utilizan filtros de paso
bajo y paso alto. Esto es idéntico al método que utilicé en el Seg... palo Instructable, paso
10.
La imagen muestra el un filtro complementario en forma de diagrama de bloque. Las
señales del sensor, convertidas a grados y grados por segundo, son las entradas. El
ángulo del acelerómetro es low-pass filtrado, reduciendo la influencia de las señales de
corta duración pero manteniendo un promedio a largo plazo (de gravedad). Primero se
integra en la velocidad angular de giro, para obtener una estimación del ángulo y entonces
high-pass filtraron, para eliminar cualquier deriva a largo plazo. Las dos señales filtradas
se suman juntos para crear una estimación de ángulo único que combina las mejores
partes de dos señales de los sensores.
Es uno de los pocos casos donde el código es más conciso que el diagrama de bloques o
la explicación por escrito:
Extracto de pd_digitalIMU.pde en 4pcb_ARD:
// -------------------------------------------------------------------
rate_pitch = (float) g_pitch * G_GAIN;
angle_pitch = AA * (angle_pitch + rate_pitch * DT);
angle_pitch += (1,0 - AA) * (float) a_pitch * A_GAIN;
// -------------------------------------------------------------------
La primera línea convierte la señal de giro crudo, g_pitch, en º / s, como se explica en el
paso anterior. Al final de la segunda línea es la integración numérica. DT es el bucle del
tiempo, 0.01s. Un pequeño cambio en el ángulo, rate_pitch * DT, se añade a la anterior
estimación del ángulo.
El resto de la segunda línea y tercera línea forman los filtros paso alto y paso bajo, en
forma discreta. El paso alto actúa sobre la estimación del ángulo integrado y el low-pass
actúa sobre la señal del acelerómetro, a_pitch. La constante AAdetermina la escala de
tiempo de los filtros de paso alto y bajo y es un valor que se puede modificar dependiendo
de tu cuadrotor. Mayor AA confía en la señal de giro integrado por más tiempo. Más
corto AA combina la señal del acelerómetro en más rápido.
El valor de AA en 4pcb corresponde con una constante de tiempo de 1s. Esta es la
longitud aproximada del tiempo para que la estimación del ángulo de giro es de confianza
y la longitud aproximada del tiempo que es un promedio de la señal de accerometer.
Puesto que éstos son infinitos filtros de respuesta del impulso (IIR), la transición es suave
y continuo y la constante de tiempo representa sólo donde la ponderación de los dos filtros
es igual.
Escribió una explicación más profunda del filtro complementario aplicado a uno mismo-
equilibrio sistema en este documento. Cubre algunos de los detalles de cómo AA se
relaciona con la constante de tiempo de los filtros paso bajo y paso alto, como tipo afecta
bies sensor, el filtro y algunos detalles más.

PCB cuadrotor (sin cepillo) (16 / 20 paso)

Paso 16: Poner juntos: el código
El código para mi quad de PCB está escrito en estilo Arduino C y C++ en el IDE de
Arduino. Si nunca has usado un Arduino antes, usted puede ponerse al día rápidamente
utilizando los recursos en la hopepage de Arduino. El proyecto de Arduino está incluido en
la documentación (paso 1). En total, el código es aproximadamente de 500 líneas,
incluyendo no incluye. Incluso en el archivo principal, mucho del código ejecuta setup o
fondo cosas como funciones para leer los datos en serie IMU. (Pelado abajo de las
librerías de Pololu de aquí y aquí para incluir sólo las funciones que necesitaba).
La parte más interesante del código que implementa los conceptos de los cuatro pasos
anteriores, cabe en una sola página (o las dos imágenes en este paso). Aquí está, con
breves descripciones de lo que hace cada parte:
readGyro();
readAcc();

// START OF + MODE ----------------------------------------------------------------------

a_pitch = (accel_x_zero - accel_x);
a_roll = (accel_y - accel_y_zero);
a_z = (int firmado) analogRead(A_Z) - a_z_z;
g_pitch = (gyro_y - gyro_y_zero);
g_roll = (gyro_x - gyro_x_zero);
g_yaw = (gyro_z - gyro_z_zero);
Esta parte toma las señales crudas de acelerómetro y giroscopio. También lo hace la
reducción a cero, aunque tengo todos mis ceros establecer... cero... ya que el mínimo de
Pololu-9 hace un buen trabajo de puesta a cero de sus resultados ya. Si es necesario, se
podían cambiar los valores de cero para recortar la cuadrotor. Nota: se invierte
a_pitch. Este es un tema físico. En tono positivo, el vector de la gravedad apunta hacia el
eje X de la IMU. Esto se registra como aceleración negativa en X, puesto que el
accelrometer no puede distinguir entre la que se inclina hacia adelante y acelerar hacia
atrás. a_roll no tiene esta inversión.
rate_pitch = (float) g_pitch * G_GAIN;
rate_roll = (float) g_roll * G_GAIN;
rate_yaw = (float) g_yaw * G_GAIN;
Convertir las señales del girocompás crudo en unidades físicas de º / s.
angle_pitch = AA * (angle_pitch + rate_pitch * DT);
angle_pitch += (1,0 - AA) * (float) a_pitch * A_GAIN;
pitch_error = (float) (pitch_command - 127) / 127.0 * MAX_ANGLE - angle_pitch;
pitch_error_integral += pitch_error * DT;
Si (pitch_error_integral > = INT_SAT) {pitch_error_integral = INT_SAT;}
Si (pitch_error_integral < = - INT_SAT) {pitch_error_integral = - INT_SAT;}

angle_roll = AA * (angle_roll + rate_roll * DT);

angle_roll += (1,0 - AA) * (float) a_roll * A_GAIN;
roll_error = (float) (roll_command - 127) / 127.0 * MAX_ANGLE - angle_roll;
roll_error_integral += roll_error * DT;
Si (roll_error_integral > = INT_SAT) {roll_error_integral = INT_SAT;}
Si (roll_error_inttegral < = - INT_SAT) {roll_error_integral = - INT_SAT;}
Echada y rodillo Filtros complementarios, así como cálculo de error del
ángulo. pitch_command y roll_commandprovienen de la radio y se transmiten desde la
estación de tierra basada en entradas del usuario. MAX_ANGLE es una constante que
define el ángulo de la echada y del rodillo de ordenada máximo en grados de definir
usuario. Los términos control integral son todos comentados, pero no dudes en jugar con
ellos.
Control
front_command_f = pitch_error * KP;
front_command_f = pitch_error_integral * KI;
front_command_f += rate_pitch * KD;
front_command_f-= ((float) (yaw_command - 127) / 127.0 * MAX_RATE - rate_yaw) * KY;

rear_command_f += pitch_error * KP;

rear_command_f += pitch_error_integral * KI;
rear_command_f = rate_pitch * KD;
rear_command_f-= ((float) (yaw_command - 127) / 127.0 * MAX_RATE - rate_yaw) * KY;

right_command_f = roll_error * KP;

right_command_f = roll_error_integral * KI;
right_command_f += 1.3 * rate_roll * KD;
right_command_f += ((float) (yaw_command - 127) / 127.0 * MAX_RATE - rate_yaw) * KY;

left_command_f += roll_error * KP;

left_command_f += roll_error_integral * KI;
left_command_f = rate_roll * KD;
left_command_f += ((float) (yaw_command - 127) / 127.0 * MAX_RATE - rate_yaw) * KY;
// END OF + MODE ------------------------------------------------------------------------
Esta es la matriz de comandos del paso 12. El mando de guiñada es en-alineado, que
clase de tonto. yaw_commandviene de la radio y se transmite por la estación de
tierra. MAX_RATE es una constante definida por el usuario que establece la máxima al
mando de guiñada tarifa de la rotación en grados por segundo.

PCB cuadrotor (sin cepillo) (17 / 20 paso)

Paso 17: Un Tweak menor: Ley de Control de la raíz cuadrada
Mucho de la teoría de control utilizada en este proyecto, particularmente la analogía masa-
resorte-amortiguador para el controlador PD, se basa en el hecho de que los motores
producen empuje proporcional para ángulo de error, o velocidad angular. Los comandos
enviados al motor no son valores de empuje, aunque. Representan una tensión, o más
precisamente una fracción de la tensión de la batería, aplicada en los bornes del motor. La
tensión y la velocidad del motor están estrechamente relacionados, razón por la cual
controladores de motor son a menudo llamados controladores de velocidad. Pero el
empuje está más estrechamente relacionada con el cuadrado de la velocidad de la hélice.
(Al igual que la elevación es proporcional al cuadrado de la velocidad).
Puesto que el cuadrotor pasa la mayor parte de su tiempo en un pequeño grupo de
velocidades alrededor de libración, la diferencia puede importa mucho. Pero jugaba con la
idea de "deshacer" la relación cuadrada pasando los comandos de motor a través de una
función raíz cuadrada antes de enviarlos a los motores. La función raíz cuadrada debe ser
normalizado (véase la imagen), pero se puede hacer con una tabla de búsqueda para el
tiempo de CPU disponible.
Encontré que esta modificación mejora la estabilidad, especialmente para sostener
manualmente la altitud.
Vea el paso

PCB cuadrotor (sin cepillo) (18 / 20 paso)

Paso 18: Interfaz de la Radio
interfaz de radio de 4pcb se basa en módulos XBee serie 1 . Son pequeños módulos de
radio digital de 2,4 GHz que se comportan como puertos serie sin hilos. En la lista de
materiales, especifica dos de los transceptores XBee Pro de mayor potencia. En $32, son
aproximadamente el 50% más caros que los módulos XBee normales. Pero le dará mucho
mayor rango de la radio, que es importante para un cuadrotor.
Un XBee se conecta directamente a la cuadrotor, en la parte inferior del tablero.
(Superglued de la mina, para que no caiga hacia fuera durante aterrizajes.) El otro se
conecta a un ordenador de la estación de tierra usando un adaptador USB XBee como los
de Sparkfun XBee Explorer. Una ventaja XBee radios tienen sobre el equipo estándar de
RC es que la información fluye ambas maneras. Comandos se envían desde la estación
de tierra a la cuadrotor y telemetría se envía detrás.
Los datos se envían en paquetes de un byte a la vez, a 57600 bits por segundo. Elegí este
bitrate porque también es el ratio de morosidad para el bootloader de Arduino, así que
usted puede programar el Arduino Pro Mini sobre XBee sin necesidad de un cable de
programación independiente. (Todavía tienes darle reset en el Mini Pro.) La segunda
imagen muestra el flujo de datos y las definiciones de bytes de los paquetes de comando y
telemetría. La tercera imagen muestra cómo recuperar el ángulo y tipos de angulares en
grados (por segundo) de los datos de telemetría crudo.
El cuadrotor transmite y lee datos a 100Hz. Puede enviar comandos en esta velocidad
hasta. (Un poco más rápido o más lento está bien también). Usted puede buscar a través
del código fuente en el proyecto de Arduino para ver cómo se implementa la interfaz de
radio en detalle.

PCB cuadrotor (sin cepillo) (19 / 20 paso)

Paso 19: La estación de tierra
La estación terrestre transmite los comandos {acelerador, pitch, roll, desvío} cuadrotor
basado en entradas del usuario. Consta de tres partes: un usuario entrada de dispositivo,
una radio XBee como se explica en el paso anterior, y algunos software para mapear el
dispositivo de entrada de las hachas a los comandos.
Dispositivo de entrada: Logitiech Gamepad
Opté por usar un gamepad Logitech doble acción como mi dispositivo de entrada. La
primera imagen muestra como ejes de palanca de mando del gamepad Logitech
conseguirán asignados a los cuatro comandos cuadrotor. Experimentados pilotos RC
podrían reconocer este diseño de joystick como modo 2. A diferencia de un RC de radio, la
palanca de aceleración en el pad será resorte. Esto dificulta un poco más mantener la
altitud, pero no es demasiado difícil acostumbrarse.
Puede usar cualquier otro dispositivo USB HID como el controlador, pero usted tendrá que
modificar el software de estación de tierra para asignar correctamente los ejes del joystick
a los comandos. También he probado un joystick simulador de vuelo. Me gustó la
sensación tanto, pero esto era nuevo cuando yo estaba primero aprender a volar.
También se puede utilizar un transmisor estándar de vuelo RC. En este caso, ni siquiera
necesita un ordenador para conectarse. Un Arduino pueda leer las señales de modulación
(PPM) de posición de pulso puerto del entrenador del transmisor y enviar comandos por
medio de XBee. Aquí está una Guía sobre cómo asignar las señales del puerto de
entrenador del transmisor para el XBee.
Software de interconexión:
Si decides utilizar un controlador USB HID, necesitarás algún software funcionando en una
computadora con interfaz con el controlador y para enviar comandos a la radio XBee. Este
software puede también agarrar la telemetría de la cuadrotor y mostrarla o registro para su
posterior análisis.
Escribí un programa de la estación de tierra simple en Visual Basic, que se incluye en la
documentación del proyecto (paso 1). El ejecutable está en la carpeta 4pcb_EXE y la
fuente está en la carpeta 4pcb_VB. Para ejecutar el archivo ejecutable, necesita los
archivos de runtime. net, que pueden descargarse desde aquí. Si usted está interesado en
modificar el software de estación de tierra, puede usar Visual Basic 2010 Express, que es
gratuito.
Mi estación de tierra es en ninguna parte cerca tan funcional como podría ser. En primer
lugar, sólo algunos de los adornos son activos y no ahorran sus valores cuando usted
cierra el programa. También sólo registra la telemetría (en un archivo de texto llamado
4pcbdata.txt). Podría hacer mucho más - como mostrar una cuadrotor 3D virtual en la
pantalla que imita la orientación de la cuadrotor real.
La. Estación de tierra basada en la red sólo se ejecutará en Windows. Si estás en un
sistema operativo diferente, todavía puede escribir software de la estación de tierra en
cualquier lenguaje de programación que puede interfaz HID dispositivos de juego y un
puerto serie virtual (por lo tanto, bastante mucho cualquier idioma). Una opción fácil
es procesamiento, un lenguaje de programación de aplicación/GUI con y como Arduino
IDE. Hay una biblioteca de terceros para Procssing llamado ProCONROLL que interfaces
para controladores HID. Escribí una muy rápida estación de tierra basada en
procesamiento/ProCONTROLL para el control de robots que podría modificarse fácilmente
para enviar el paquete de comandos necesario para la cuadrotor.