Esta página se ha traducido mediante traducción automática. Haga clic aquí para ver la última versión en inglés.

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 $$(m/s^2{)}^2$$responde a una trayectoria temblorosa, en comparación con una con un LinearAccelerationNoise de 9e-4 $$(m/s^2{)}^2$$.

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.