Estimación de la orientación mediante la fusión de sensores inerciales
Este ejemplo muestra cómo utilizar algoritmos de fusión de 6 y 9 ejes para calcular la orientación. Existen varios algoritmos para calcular la orientación a partir de unidades de medida inercial (IMU) y unidades de gravedad magnética-angular (MARG). Este ejemplo cubre los conceptos básicos de la orientación y cómo utilizar estos algoritmos.
Orientación
La orientación de un objeto describe su rotación relativa a algún sistema de coordenadas, a veces llamado sistema de coordenadas principal, en tres dimensiones.
Para los siguientes algoritmos, el sistema de coordenadas principal fijo utilizado es Noreste-Abajo (NED). A veces se hace referencia a NED como sistema de coordenadas global o marco de referencia. En el marco de referencia NED, el eje X apunta al norte, el eje Y apunta al este y el eje Z apunta hacia abajo. El plano X-Y de NED se considera el plano tangente local de la Tierra. Dependiendo del algoritmo, el norte puede ser el norte magnético o el norte verdadero. Los algoritmos de este ejemplo utilizan el norte magnético.
Si se especifica, los siguientes algoritmos pueden estimar la orientación relativa al sistema de coordenadas principal Este-Norte-Arriba (ENU) en lugar de NED.
Se puede considerar que un objeto tiene su propio sistema de coordenadas, a menudo llamado sistema de coordenadas local o secundario. Este sistema de coordenadas secundario gira con el objeto en relación con el sistema de coordenadas principal. Si no hay traslación, los orígenes de ambos sistemas de coordenadas se superponen.
La cantidad de orientación calculada es una rotación que lleva cantidades del sistema de referencia principal al sistema de referencia secundario. La rotación está representada por un cuaternión o matriz de rotación.
Tipos de sensores
Para la estimación de la orientación se utilizan habitualmente tres tipos de sensores: acelerómetros, giroscopios y magnetómetros. Los acelerómetros miden la aceleración adecuada. Los giroscopios miden la velocidad angular. Los magnetómetros miden el campo magnético local. Se utilizan diferentes algoritmos para fusionar diferentes combinaciones de sensores para estimar la orientación.
Datos del sensor
En la mayor parte de este ejemplo, se utiliza el mismo conjunto de datos del sensor. Los datos de los sensores del acelerómetro, giroscopio y magnetómetro se registraron mientras un dispositivo giraba alrededor de tres ejes diferentes: primero alrededor de su eje Y local, luego alrededor de su eje Z y finalmente alrededor de su eje X. El eje X del dispositivo estuvo generalmente apuntado hacia el sur durante la duración del experimento.
ld = load("rpy_9axis.mat");
acc = ld.sensorData.Acceleration;
gyro = ld.sensorData.AngularVelocity;
mag = ld.sensorData.MagneticField;
pp = poseplot;
Fusión Acelerómetro-Magnetómetro
La función ecompass
fusiona datos del acelerómetro y del magnetómetro. Este es un algoritmo sin memoria que no requiere ajuste de parámetros, pero el algoritmo es muy susceptible al ruido del sensor.
qe = ecompass(acc, mag); for ii=1:size(acc,1) set(pp,"Orientation",qe(ii)) drawnow limitrate end
Tenga en cuenta que el algoritmo ecompass
encuentra correctamente la ubicación del norte. Sin embargo, debido a que la función no tiene memoria, el movimiento estimado no es fluido. El algoritmo podría usarse como un paso de inicialización en un filtro de orientación o algunas de las técnicas presentadas en el Lowpass Filter Orientation Using Quaternion SLERP (Sensor Fusion and Tracking Toolbox) podrían usarse para suavizar el movimiento.
Fusión Acelerómetro-Giroscopio
Los siguientes objetos estiman la orientación utilizando un filtro de Kalman de estado de error o un filtro complementario. El filtro de Kalman de estado de error es el filtro de estimación estándar y permite ajustar muchos aspectos diferentes del sistema utilizando los parámetros de ruido correspondientes. El filtro complementario se puede utilizar como sustituto de sistemas con limitaciones de memoria y tiene parámetros sintonizables mínimos, lo que permite una configuración más sencilla a costa de un ajuste más preciso.
Los System objects imufilter
y complementaryFilter
fusionan datos de acelerómetro y giroscopio. El imufilter
usa un filtro Kalman de estado de error interno y el complementaryFilter
usa un filtro complementario. Los filtros son capaces de eliminar el ruido de polarización del giroscopio, que se desplaza con el tiempo.
ifilt = imufilter(SampleRate=ld.Fs); for ii=1:size(acc,1) qimu = ifilt(acc(ii,:),gyro(ii,:)); set(pp,"Orientation",qimu) drawnow limitrate end
% Disable magnetometer input. cfilt = complementaryFilter(SampleRate=ld.Fs,HasMagnetometer=false); for ii=1:size(acc,1) qimu = cfilt(acc(ii,:),gyro(ii,:)); set(pp,"Orientation",qimu) drawnow limitrate end
Aunque los algoritmos imufilter
y complementaryFilter
producen estimaciones del movimiento significativamente más suaves, en comparación con el ecompass
, no estiman correctamente la dirección del norte. El imufilter
no procesa datos del magnetómetro, por lo que simplemente asume que el eje X del dispositivo apunta inicialmente hacia el norte. La estimación de movimiento dada por imufilter
es relativa a la orientación estimada inicial. El complementaryFilter
hace la misma suposición cuando la propiedad HasMagnetometer
se establece en false
.
Fusión Acelerómetro-Giroscopio-Magnetómetro
Un sistema de referencia de actitud y rumbo (AHRS) consta de un sistema de 9 ejes que utiliza un acelerómetro, giroscopio y magnetómetro para calcular la orientación. Los ahrsfilter
y complementaryFilter
System object™ combinan lo mejor de los algoritmos anteriores para producir una estimación que cambia suavemente de la orientación del dispositivo, mientras se estima correctamente la dirección del norte. El complementaryFilter
utiliza el mismo algoritmo de filtro complementario que antes, con un paso adicional para incluir el magnetómetro y mejorar la estimación de orientación. Al igual que imufilter
, el algoritmo ahrsfilter
también utiliza un filtro de Kalman de estado de error. Además de eliminar la polarización del giroscopio, el ahrsfilter
tiene cierta capacidad para detectar y rechazar interferencias magnéticas leves.
ifilt = ahrsfilter(SampleRate=ld.Fs); for ii=1:size(acc,1) qahrs = ifilt(acc(ii,:),gyro(ii,:),mag(ii,:)); set(pp,"Orientation",qahrs) drawnow limitrate end
cfilt = complementaryFilter(SampleRate=ld.Fs); for ii=1:size(acc,1) qahrs = cfilt(acc(ii,:),gyro(ii,:),mag(ii,:)); set(pp,"Orientation",qahrs) drawnow limitrate end
Ajuste de los parámetros del filtro
Los complementaryFilter
, imufilter
y ahrsfilter
System object™ tienen parámetros ajustables. Ajustar los parámetros en función de los sensores especificados que se utilizan puede mejorar el rendimiento.
Los parámetros complementaryFilter
AccelerometerGain
y MagnetometerGain
se pueden ajustar para cambiar la cantidad en la que las mediciones de cada sensor impactan la estimación de orientación. Cuando AccelerometerGain
se establece en 0
, solo se utiliza el giroscopio para la orientación de los ejes x e y. Cuando AccelerometerGain
se establece en 1
, solo se utiliza el acelerómetro para la orientación de los ejes x e y. Cuando MagnetometerGain
se establece en 0
, solo se utiliza el giroscopio para la orientación del eje z. Cuando MagnetometerGain
se establece en 1
, solo se utiliza el magnetómetro para la orientación del eje z.
Los System objects ahrsfilter
y imufilter
tienen más parámetros que pueden permitir que los filtros coincidan más estrechamente con sensores de hardware específicos. También es importante tener en cuenta el entorno del sensor. Los parámetros imufilter
son un subconjunto de los parámetros ahrsfilter
. Los AccelerometerNoise
, GyroscopeNoise
, MagnetometerNoise
y GyroscopeDriftNoise
son ruidos de medición. Las hojas de datos de los sensores ayudan a determinar esos valores.
Los LinearAccelerationNoise
y LinearAccelerationDecayFactor
gobiernan la respuesta del filtro a la aceleración lineal (traslacional). Agitar un dispositivo es un ejemplo sencillo de cómo añadir aceleración lineal.
Considere cómo un imufilter
con un LinearAccelerationNoise
de 9e-3 responde a una trayectoria temblorosa, en comparación con una con un LinearAccelerationNoise
de 9e-4 .
ld = load("shakingDevice.mat"); accel = ld.sensorData.Acceleration; gyro = ld.sensorData.AngularVelocity; highVarFilt = imufilter(SampleRate=ld.Fs, ... LinearAccelerationNoise=0.009); qHighLANoise = highVarFilt(accel,gyro); lowVarFilt = imufilter(SampleRate=ld.Fs, ... LinearAccelerationNoise=0.0009); qLowLANoise = lowVarFilt(accel,gyro);
Una forma de ver el efecto del LinearAccelerationNoise
es observar el vector de gravedad de salida. El vector de gravedad es simplemente la tercera columna de la matriz de rotación de orientación.
rmatHigh = rotmat(qHighLANoise,"frame"); rmatLow = rotmat(qLowLANoise,"frame"); gravDistHigh = sqrt(sum((rmatHigh(:,3,:)-[0;0;1]).^2, 1)); gravDistLow = sqrt(sum((rmatLow(:,3,:)-[0;0;1]).^2, 1)); figure; plot([squeeze(gravDistHigh),squeeze(gravDistLow)]); title("Euclidean Distance to Gravity"); legend("LinearAccelerationNoise = 0.009", ... "LinearAccelerationNoise = 0.0009");
El lowVarFilt
tiene un LinearAccelerationNoise
bajo, por lo que espera estar en un entorno con baja aceleración lineal. Por lo tanto, es más susceptible a la aceleración lineal, como lo ilustran las grandes variaciones anteriores en el gráfico. Sin embargo, debido a que espera estar en un entorno con una aceleración lineal baja, se deposita mayor confianza en la señal del acelerómetro. Como tal, la estimación de la orientación vuelve rápidamente a la vertical una vez que termina el temblor. Lo contrario es cierto para highVarFilt
. El filtro se ve menos afectado por las sacudidas, pero la estimación de la orientación tarda más en converger a la vertical cuando las sacudidas han cesado.
La propiedad MagneticDisturbanceNoise
permite modelar perturbaciones magnéticas (fuentes de ruido no geomagnéticas) de la misma manera que LinearAccelerationNoise
modela la aceleración lineal.
Las dos propiedades del factor de caída (MagneticDisturbanceDecayFactor
y LinearAccelerationDecayFactor
) modelan la tasa de variación de los ruidos. Para fuentes de ruido que varían lentamente, establezca estos parámetros en un valor más cercano a 1. Para ruidos no correlacionados y que varían rápidamente, ajuste estos parámetros más cerca de 0. Un LinearAccelerationDecayFactor
más bajo permite que la estimación de orientación encuentre "abajo" más rápidamente. Un MagneticDisturbanceDecayFactor
más bajo permite que la estimación de orientación encuentre el norte más rápidamente.
Las perturbaciones magnéticas cortas y muy grandes son rechazadas casi por completo por el ahrsfilter
. Considere un pulso de [0 250 0] uT aplicado mientras se registra desde un sensor estacionario. Idealmente, no debería haber cambios en la estimación de orientación.
ld = load("magJamming.mat"); hpulse = ahrsfilter(SampleRate=ld.Fs); len = 1:10000; qpulse = hpulse(ld.sensorData.Acceleration(len,:), ... ld.sensorData.AngularVelocity(len,:), ... ld.sensorData.MagneticField(len,:)); figure; timevec = 0:ld.Fs:(ld.Fs*numel(qpulse) - 1); plot( timevec, eulerd(qpulse,"ZYX","frame") ); title(["Stationary Trajectory Orientation Euler Angles" newline ... "Magnetic Jamming Response"]); legend("Z-rotation","Y-rotation","X-rotation"); ylabel("Degrees"); xlabel("Seconds");
Tenga en cuenta que el filtro rechaza casi por completo este pulso magnético como interferencia. Cualquier intensidad de campo magnético superior a cuatro veces el ExpectedMagneticFieldStrength
se considera una fuente de interferencia y la señal del magnetómetro se ignora para esas muestras.
Conclusión
Los algoritmos presentados aquí, cuando se ajustan adecuadamente, permiten la estimación de la orientación y son robustos frente a fuentes de ruido ambiental. Es importante considerar las situaciones en las que se utilizan los sensores y ajustar los filtros en consecuencia.