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.

Estimación de la orientación mediante la fusión del sensor inercial y el MPU-9250

Este ejemplo utiliza:

Abrir script en vivo

Este ejemplo muestra cómo obtener datos de un sensor IMU InvenSense MPU-9250 y cómo utilizar los algoritmos de fusión de 6 y 9 ejes en los datos del sensor para calcular la orientación del dispositivo.

MPU-9250 es un sensor de 9 ejes con acelerómetro, giroscopio y magnetómetro. El acelerómetro mide la aceleración, el giroscopio mide la velocidad angular y el magnetómetro mide el campo magnético en los ejes x, y y z. El eje del sensor depende de la marca del sensor.

Productos MathWorks® necesarios

  • MATLAB®

  • Paquete de soporte MATLAB para hardware Arduino®

  • Navigation Toolbox™ o Sensor Fusion and Tracking Toolbox™

Hardware requerido

  • ArduinoUno

  • InvenSense MPU-9250

Conexión de hardware

Conecte los pines SDA, SCL, GND y VCC del sensor MPU-9250 a los pines correspondientes en el hardware Arduino®. Este ejemplo utiliza la 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 utilizar un escudo prototipo y soldar el sensor a él para evitar conexiones sueltas al mover el sensor. Consulte la página para sensores para depurar los problemas relacionados con los sensores.

Crear objeto sensor

Cree un objeto arduino e incluya la biblioteca I2C.

a = arduino('COM9', 'Uno', 'Libraries', 'I2C');
Updating server code on board Uno (COM9). This may take a few minutes.

Cree el objeto sensor MPU-9250. 

fs = 100; % Sample Rate in Hz   
imu = mpu9250(a,'SampleRate',fs,'OutputFormat','matrix');

Compensación de distorsiones del magnetómetro

Los algoritmos de fusión utilizan lecturas de magnetómetros que deben compensarse por las distorsiones magnéticas. En general, existen dos efectos: Distorsiones de hierro duro y distorsiones de hierro dulce. Para obtener más información sobre esta distorsión, consulte Más acerca de. Estas distorsiones se pueden corregir utilizando los valores de corrección que se pueden determinar siguiendo estos pasos:

  1. Gire el sensor de 0 a 360 grados a lo largo de cada eje.

  2. Utilice la función magcal , como se muestra a continuación, para obtener los coeficientes de corrección. 

ts = tic; 
stopTimer = 50; 
magReadings=[]; 
while(toc(ts) < stopTimer) 
    % Rotate the sensor along x axis from 0 to 360 degree. 
    % Take 2-3 rotations to improve accuracy. 
    % For other axes, rotate along that axes. 
   [accel,gyro,mag] = read(imu); 
   magReadings = [magReadings;mag]; 
end 

[A, b] = magcal(magReadings); % A = 3x3 matrix for soft iron correction 
                              % b = 3x1 vector for hard iron correction

Alineación del eje del sensor MPU-9250 con coordenadas NED

Los algoritmos de fusión de sensores utilizados en este ejemplo utilizan Noreste-Abajo (NED) como sistema de coordenadas principal fijo. 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. 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. Los algoritmos utilizados aquí esperan que todos los sensores del objeto tengan sus ejes alineados y están de acuerdo con la convención NED.

MPU-9250 tiene dos dispositivos, el magnetómetro y el acelerómetro-giroscopio, en la misma placa. Los ejes de estos dispositivos son diferentes entre sí. El eje del magnetómetro está alineado con las coordenadas NED. El eje del acelerómetro-giroscopio es diferente al del magnetómetro del MPU-9250. El acelerómetro y el eje del giroscopio deben intercambiarse y/o invertirse para que coincidan con el eje del magnetómetro. Para más información consultar el apartado “Orientación de Ejes” en MPU-9250 datasheet.

Para alinear los ejes del acelerómetro-giroscopio MPU-9250 con las coordenadas NED, haga lo siguiente:

: –1. Definir ejes del dispositivo: Defina el eje imaginario como el eje del dispositivo en el sensor de acuerdo con el sistema de coordenadas NED, que puede o no ser el mismo que los ejes del sensor. Para MPU-9250, el eje del magnetómetro se puede considerar como eje del dispositivo.

2. Intercambie los valores x e y de las lecturas del acelerómetro y giroscopio, de modo que el eje del acelerómetro y giroscopio esté alineado con el eje del magnetómetro.

3 Determine los valores de polaridad para el acelerómetro y el giroscopio.

a. Acelerómetro

  • Coloque el sensor de manera que el eje X del dispositivo apunte hacia abajo, perpendicular a la superficie en la que se mantiene el sensor. Las lecturas del acelerómetro deben ser aproximadamente [9,8 0 0]. Si no, niegue los valores x del acelerómetro.

  • Coloque el sensor de manera que el eje Y del dispositivo apunte hacia abajo, perpendicular a la superficie en la que se mantiene el sensor. Las lecturas del acelerómetro deben ser aproximadamente [0 9,8 0]. Si no, niegue los valores y del acelerómetro.

  • Coloque el sensor de manera que el eje Z del dispositivo apunte hacia abajo, perpendicular a la superficie en la que se mantiene el sensor. Las lecturas del acelerómetro deben ser aproximadamente [0 0 9,8]. Si no, niegue los valores z del acelerómetro.

b. Giroscopio

Gire el sensor a lo largo de cada eje y capture las lecturas. Utilice la regla del tornillo del lado derecho para corregir la polaridad de rotación.

El método anterior se utiliza para configurar el eje del sensor en este ejemplo.

Ajuste de los parámetros del filtro

Los algoritmos utilizados en este ejemplo, cuando se ajustan correctamente, permiten la estimación de la orientación y son robustos frente a fuentes de ruido ambiental. Debe considerar las situaciones en las que se utilizan los sensores y ajustar los filtros en consecuencia. Consulte Custom Tuning of Fusion Filters (Sensor Fusion and Tracking Toolbox) para obtener más detalles relacionados con el ajuste de los parámetros del filtro.

El ejemplo demuestra tres algoritmos para determinar la orientación, a saber, ahrsfilter, imufilter y ecompass. Consulte Determine Orientation Using Inertial Sensors (Sensor Fusion and Tracking Toolbox) para obtener más detalles relacionados con los algoritmos de fusión inercial.

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 del dispositivo. El ahrsfilter produce una estimación que cambia suavemente de la orientación del dispositivo, mientras estima correctamente la dirección norte. El ahrsfilter tiene la capacidad de eliminar el sesgo del giroscopio y también puede detectar y rechazar interferencias magnéticas leves.

Los siguientes fragmentos de código utilizan el System object ahrsfilter para determinar la orientación del sensor y crean una figura que se actualiza a medida que mueve el sensor. El sensor debe estar estacionario antes del inicio de este ejemplo. 

% GyroscopeNoise and AccelerometerNoise is determined from datasheet.
GyroscopeNoiseMPU9250 = 3.0462e-06; % GyroscopeNoise (variance value) in units of rad/s
AccelerometerNoiseMPU9250 = 0.0061; % AccelerometerNoise(variance value)in units of m/s^2
viewer = HelperOrientationViewer('Title',{'AHRS Filter'});
FUSE = ahrsfilter('SampleRate',imu.SampleRate, 'GyroscopeNoise',GyroscopeNoiseMPU9250,'AccelerometerNoise',AccelerometerNoiseMPU9250);
stopTimer = 100;

Mientras se ejecuta el siguiente código, mueva lentamente el sensor y verifique si el movimiento en la figura coincide con el movimiento del sensor. 

% The correction factors A and b are obtained using magcal function as explained in one of the
% previous sections, 'Compensating Magnetometer Distortions'.
magx_correction = b(1);
magy_correction = b(2);
magz_correction = b(3);
ts = tic;
while(toc(ts) < stopTimer)
    [accel,gyro,mag] = read(imu);
    % Align coordinates in accordance with NED convention
    accel = [-accel(:,2), -accel(:,1), accel(:,3)];
    gyro = [gyro(:,2), gyro(:,1), -gyro(:,3)];
    mag = [mag(:,1)-magx_correction, mag(:, 2)- magy_correction, mag(:,3)-magz_correction] * A;
    rotators = FUSE(accel,gyro,mag);
    for j = numel(rotators)
        viewer(rotators(j));
    end
end

Cuando el eje X del dispositivo del sensor apunta hacia el norte, el eje Y del dispositivo apunta hacia el este y el eje Z del dispositivo apunta hacia abajo.

Cuando el eje X del dispositivo del sensor apunta hacia el norte, el eje Y del dispositivo apunta hacia el oeste y el eje Z del dispositivo apunta hacia arriba.

Fusión Acelerómetro-Giroscopio

El System object imufilter fusiona datos de acelerómetro y giroscopio utilizando un filtro Kalman de estado de error interno. El filtro es capaz de eliminar el ruido de polarización del giroscopio, que varía con el tiempo. El filtro no procesa datos del magnetómetro, por lo que no estima correctamente la dirección del norte. El algoritmo supone que la posición inicial del sensor es de tal manera que el eje X del dispositivo del sensor apunta hacia el norte magnético, el eje Y del dispositivo del sensor apunta hacia el este y el eje Z del dispositivo del sensor está apuntando hacia abajo. El sensor debe estar estacionario antes del inicio de este ejemplo.

Los siguientes fragmentos de código utilizan el objeto imufilter para determinar la orientación del sensor y crean una figura que se actualiza a medida que mueve el sensor. 

displayMessage(['This section uses  IMU filter to determine orientation of the sensor by collecting live sensor data from the \slmpu9250 \rm' ...
    'system object. Move the sensor to visualize orientation of the sensor in the figure window. Keep the sensor stationery before you'...
    'click OK'],...
    'Estimate Orientation using IMU filter and MPU-9250.')
% GyroscopeNoise and AccelerometerNoise is determined from datasheet.
GyroscopeNoiseMPU9250 = 3.0462e-06; % GyroscopeNoise (variance) in units of rad/s
AccelerometerNoiseMPU9250 = 0.0061; % AccelerometerNoise (variance) in units of m/s^2
viewer = HelperOrientationViewer('Title',{'IMU Filter'});
FUSE = imufilter('SampleRate',imu.SampleRate, 'GyroscopeNoise',GyroscopeNoiseMPU9250,'AccelerometerNoise', AccelerometerNoiseMPU9250);
stopTimer=100;

Mientras se ejecuta el siguiente código, mueva lentamente el sensor y verifique si el movimiento en la figura coincide con el movimiento del sensor.

% Use imufilter to estimate orientation and update the viewer as the sensor moves for time specified by stopTimer 
tic; 
while(toc < stopTimer)
    [accel,gyro] = read(imu);
    accel = [-accel(:,2), -accel(:,1), accel(:,3)]; 
    gyro = [gyro(:,2), gyro(:,1), -gyro(:,3)]; 
    rotators = FUSE(accel,gyro); 
    for j = numel(rotators) 
        viewer(rotators(j)); 
    end 
end

El algoritmo imufilter también se puede utilizar con MPU6050, ya que no requiere valores de magnetómetro.

Cuando el eje X del dispositivo del sensor apunta hacia el norte, el eje Z del dispositivo apunta hacia abajo y el eje Y del dispositivo apunta hacia el este.

Cuando el eje X del dispositivo del sensor apunta hacia arriba, el eje Y del dispositivo apunta hacia el oeste y el eje Z del dispositivo apunta hacia el sur.

Fusión Acelerómetro-Magnetómetro

El System object ecompass fusiona los datos del acelerómetro y el magnetómetro. El algoritmo Ecopass es un algoritmo sin memoria que no requiere ajuste de parámetros pero es muy susceptible al ruido del sensor. Podría utilizar la interpolación lineal esférica (SLERP) para filtrar de paso bajo una trayectoria ruidosa. Consulte el ejemplo Lowpass Filter Orientation Using Quaternion SLERP (Sensor Fusion and Tracking Toolbox) para obtener más detalles.

displayMessage(['This section uses \slecompass \rmfunction to determine orientation of the sensor by collecting live sensor data from the \slmpu9250 ' ...
    '\rmsystem object. Move the sensor to visualize orientation of the sensor in the figure window. Keep the sensor stationery before you click OK'],...
    'Estimate Orientation using Ecompass algorithm.')
viewer = HelperOrientationViewer('Title',{'Ecompass Algorithm'});
% Use ecompass algorithm to estimate orientation and update the viewer as the sensor moves for time specified by stopTimer.
% The correction factors A and b are obtained using magcal function as explained in one of the
% previous sections, 'Compensating Magnetometer Distortions'.
magx_correction = b(1);
magy_correction = b(2);
magz_correction = b(3);
stopTimer = 100;
tic;
while(toc < stopTimer)
    [accel,~,mag] = read(imu);
    accel = [-accel(:,2), -accel(:,1), accel(:,3)];
    mag = [mag(:,1)-magx_correction, mag(:, 2)- magy_correction, mag(:,3)-magz_correction] * A;
    rotators = ecompass(accel,mag);
    for j = numel(rotators)
        viewer(rotators(j));
    end
end

Limpiar

Cuando la conexión ya no sea necesaria, suelte y borre los objetos.

release(imu);
clear;

Cosas para probar

Puede probar este ejemplo con otros sensores como InvenSense MPU-6050 y STMicroelectronics LSM9DS1. Tenga en cuenta que el sensor MPU-6050 solo se puede utilizar con el System object imufilter .