Main Content

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

Orientación de estimación con un filtro complementario y datos IMU

Abrir script

Este ejemplo muestra cómo transmitir datos IMU desde un Arduino y estimar la orientación utilizando un filtro complementario.

Conectar hardware

Conecte los pines SDA, SCL, GND y VCC del sensor MPU-9250 a los pines correspondientes del hardware Arduino®. Este ejemplo utiliza una placa Arduino® Uno con las siguientes conexiones:

  • ADV - A4

  • SCL-A5

  • VCC-+3,3 V

  • Tierra - Tierra

Asegúrese de que las conexiones a los sensores estén intactas. Se recomienda acoplar/conectar el sensor a un escudo prototipo para evitar conexiones sueltas mientras el sensor está en movimiento. Consulte la página para depurar los problemas relacionados con el sensor.

Crear objeto sensor

Cree un objeto arduino y un objeto mpu9250 . Especifique la frecuencia de muestreo del sensor Fs y la cantidad de tiempo para ejecutar los bucles. Opcionalmente, habilite el indicador isVerbose para verificar si alguna muestra está desbordada. Al deshabilitar el indicador useHW , también puede ejecutar el ejemplo con los datos del sensor guardados en el archivo MAT loggedMPU9250Data.mat.

Los datos en loggedMPU9250Data.mat se registraron mientras la IMU generalmente miraba hacia el sur y luego se rotaron:

  • +90 grados alrededor del eje z

  • -180 grados alrededor del eje z

  • +90 grados alrededor del eje z

  • +90 grados alrededor del eje y

  • -180 grados alrededor del eje y

  • +90 grados alrededor del eje y

  • +90 grados alrededor del eje x

  • -270 grados alrededor del eje x

  • +180 grados alrededor del eje x

Observe que las dos últimas rotaciones alrededor del eje x son 90 grados adicionales. Esto se hizo para voltear el dispositivo al revés. La orientación final de la IMU es la misma que la orientación inicial, hacia el sur.

Fs = 100;
samplesPerRead = 10;
runTime = 20;
isVerbose = false;
useHW = true;

if useHW
    a = arduino;
    imu = mpu9250(a, 'SampleRate', Fs, 'OutputFormat', 'matrix', ...
        'SamplesPerRead', samplesPerRead);
else
    load('loggedMPU9250Data.mat', 'allAccel', 'allGyro', 'allMag', ...
        'allT', 'allOverrun', ...
        'numSamplesAccelGyro', 'numSamplesAccelGyroMag')
end

Alinear ejes del sensor MPU-9250 con coordenadas NED

Los ejes del acelerómetro, giroscopio y magnetómetro del MPU-9250 no están alineados entre sí. Especifique el índice y el signo de los ejes x, y y z de cada sensor para que el sensor esté alineado con el sistema de coordenadas noreste-abajo (NED) cuando esté en reposo. En este ejemplo, los ejes del magnetómetro se cambian mientras que los ejes del acelerómetro y del giroscopio permanecen fijos. Para sus propias aplicaciones, cambie los siguientes parámetros según sea necesario.

% Accelerometer axes parameters.
accelXAxisIndex = 1;
accelXAxisSign = 1;
accelYAxisIndex = 2;
accelYAxisSign = 1;
accelZAxisIndex = 3;
accelZAxisSign = 1;

% Gyroscope axes parameters.
gyroXAxisIndex = 1;
gyroXAxisSign = 1;
gyroYAxisIndex = 2;
gyroYAxisSign = 1;
gyroZAxisIndex = 3;
gyroZAxisSign = 1;

% Magnetometer axes parameters.
magXAxisIndex = 2;
magXAxisSign = 1;
magYAxisIndex = 1;
magYAxisSign = 1;
magZAxisIndex = 3;
magZAxisSign = -1;

% Helper functions used to align sensor data axes.

alignAccelAxes = @(in) [accelXAxisSign, accelYAxisSign, accelZAxisSign] ...
    .* in(:, [accelXAxisIndex, accelYAxisIndex, accelZAxisIndex]);

alignGyroAxes = @(in) [gyroXAxisSign, gyroYAxisSign, gyroZAxisSign] ...
    .* in(:, [gyroXAxisIndex, gyroYAxisIndex, gyroZAxisIndex]);

alignMagAxes = @(in) [magXAxisSign, magYAxisSign, magZAxisSign] ...
    .* in(:, [magXAxisIndex, magYAxisIndex, magZAxisIndex]);

Realizar una calibración adicional del sensor

Si es necesario, puede calibrar el magnetómetro para compensar las distorsiones magnéticas. Para obtener más detalles, consulte la sección Compensación de distorsiones de hierro duro del ejemplo Estimating Orientation Using Inertial Sensor Fusion and MPU-9250 (Sensor Fusion and Tracking Toolbox) .

Especificar parámetros de filtro complementarios

El complementaryFilter tiene dos parámetros ajustables. El parámetro AccelerometerGain determina en qué medida se confía más en la medición del acelerómetro que en la medición del giroscopio. El parámetro MagnetometerGain determina en qué medida se confía más en la medición del magnetómetro que en la medición del giroscopio.

compFilt = complementaryFilter('SampleRate', Fs)

compFilt = 

  complementaryFilter with properties:

           SampleRate: 100
    AccelerometerGain: 0.0100
     MagnetometerGain: 0.0100
      HasMagnetometer: 1
    OrientationFormat: 'quaternion'

Estimar la orientación con acelerómetro y giroscopio

Establezca la propiedad HasMagnetometer en false para deshabilitar la entrada de medición del magnetómetro. En este modo, el filtro solo toma como entradas mediciones del acelerómetro y giroscopio. Además, el filtro supone que la orientación inicial de la IMU está alineada con el marco de navegación principal. Si la IMU no está alineada inicialmente con el marco de navegación, habrá un desplazamiento constante en la estimación de la orientación.

compFilt = complementaryFilter('HasMagnetometer', false);

tuner = HelperOrientationFilterTuner(compFilt);

if useHW
    tic
else
    idx = 1:samplesPerRead;
    overrunIdx = 1;
end
while true
    if useHW
        [accel, gyro, mag, t, overrun] = imu();
        accel = alignAccelAxes(accel);
        gyro = alignGyroAxes(gyro);
    else
        accel = allAccel(idx,:);
        gyro = allGyro(idx,:);
        mag = allMag(idx,:);
        t = allT(idx,:);
        overrun = allOverrun(overrunIdx,:);

        idx = idx + samplesPerRead;
        overrunIdx = overrunIdx + 1;
        pause(samplesPerRead/Fs)
    end

    if (isVerbose && overrun > 0)
        fprintf('%d samples overrun ...\n', overrun);
    end

    q = compFilt(accel, gyro);
    update(tuner, q);

    if useHW
        if toc >= runTime
            break;
        end
    else
        if idx(end) > numSamplesAccelGyro
            break;
        end
    end
end

Estimar la orientación con acelerómetro, giroscopio y magnetómetro

Con los valores predeterminados de AccelerometerGain y MagnetometerGain, el filtro confía más en las mediciones del giroscopio en el corto plazo, pero confía más en las mediciones del acelerómetro y magnetómetro en el corto plazo. a largo plazo. Esto permite que el filtro sea más reactivo a los cambios rápidos de orientación y evita que las estimaciones de orientación se desvíen durante períodos de tiempo más largos. Para sensores IMU específicos y fines de aplicación, es posible que desee ajustar los parámetros del filtro para mejorar la precisión de la estimación de la orientación.

compFilt = complementaryFilter('SampleRate', Fs);

tuner = HelperOrientationFilterTuner(compFilt);

if useHW
    tic
end
while true
    if useHW
        [accel, gyro, mag, t, overrun] = imu();
        accel = alignAccelAxes(accel);
        gyro = alignGyroAxes(gyro);
        mag = alignMagAxes(mag);
    else
        accel = allAccel(idx,:);
        gyro = allGyro(idx,:);
        mag = allMag(idx,:);
        t = allT(idx,:);
        overrun = allOverrun(overrunIdx,:);

        idx = idx + samplesPerRead;
        overrunIdx = overrunIdx + 1;
        pause(samplesPerRead/Fs)
    end

    if (isVerbose && overrun > 0)
        fprintf('%d samples overrun ...\n', overrun);
    end

    q = compFilt(accel, gyro, mag);
    update(tuner, q);

    if useHW
        if toc >= runTime
            break;
        end
    else
        if idx(end) > numSamplesAccelGyroMag
            break;
        end
    end
end

Resumen

Este ejemplo mostró cómo estimar la orientación de una IMU utilizando datos de un Arduino y un filtro complementario. Este ejemplo también mostró cómo configurar la IMU y analizó los efectos de ajustar los parámetros del filtro complementario.