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

Estimar la orientación y la altura utilizando IMU, magnetómetro y altímetro

Este ejemplo muestra cómo fusionar datos de un acelerómetro de 3 ejes, un giroscopio de 3 ejes, un magnetómetro de 3 ejes (comúnmente denominados sensores MARG para velocidad magnética, angular y gravedad) y un altímetro de 1 eje para estimar la orientación. y altura.

Configuración de simulación

Esta simulación procesa datos de sensores a múltiples velocidades. La IMU (acelerómetro y giroscopio) normalmente funciona a la velocidad más alta. El magnetómetro generalmente funciona a una velocidad más baja que la IMU y el altímetro funciona a la velocidad más baja. Cambiar las frecuencias de muestreo hace que partes del algoritmo de fusión se ejecuten con más frecuencia y pueden afectar el rendimiento.

% Set the sampling rate for IMU sensors, magnetometer, and altimeter.
imuFs = 100;
altFs = 10;
magFs = 25;
imuSamplesPerAlt = fix(imuFs/altFs);
imuSamplesPerMag = fix(imuFs/magFs);

% Set the number of samples to simulate. 
N = 1000; 

% Construct object for other helper functions.
hfunc = Helper10AxisFusion;

Definir trayectoria

El cuerpo del sensor gira alrededor de los tres ejes mientras oscila en posición vertical. Las oscilaciones aumentan en magnitud a medida que continúa la simulación.

% Define the initial state of the sensor body
initPos = [0, 0, 0];       % initial position (m)
initVel = [0, 0, -1];      % initial linear velocity (m/s)
initOrient = ones(1, 'quaternion'); 

% Define the constant angular velocity for rotating the sensor body
% (rad/s).
angVel = [0.34 0.2 0.045];  

% Define the acceleration required for simple oscillating motion of the
% sensor body.
fc = 0.2; 
t = 0:1/imuFs:(N-1)/imuFs;
a = 1; 
oscMotionAcc = sin(2*pi*fc*t);
oscMotionAcc = hfunc.growAmplitude(oscMotionAcc);

% Construct the trajectory object
traj = kinematicTrajectory('SampleRate', imuFs, ...
    'Velocity', initVel, ...
    'Position', initPos, ...
    'Orientation', initOrient);

Configuración de sensores

El acelerómetro, giroscopio y magnetómetro se simulan usando imuSensor. El altímetro se modela utilizando el altimeterSensor. Los valores utilizados en las configuraciones del sensor corresponden a valores reales del sensor MEMS.

imu = imuSensor('accel-gyro-mag', 'SampleRate', imuFs);

% Accelerometer
imu.Accelerometer.MeasurementRange =  19.6133;
imu.Accelerometer.Resolution = 0.0023928;
imu.Accelerometer.ConstantBias = 0.19;
imu.Accelerometer.NoiseDensity = 0.0012356;

% Gyroscope
imu.Gyroscope.MeasurementRange = deg2rad(250);
imu.Gyroscope.Resolution = deg2rad(0.0625);
imu.Gyroscope.ConstantBias = deg2rad(3.125);
imu.Gyroscope.AxesMisalignment = 1.5;
imu.Gyroscope.NoiseDensity = deg2rad(0.025);

% Magnetometer
imu.Magnetometer.MeasurementRange = 1000;
imu.Magnetometer.Resolution = 0.1;
imu.Magnetometer.ConstantBias = 100;
imu.Magnetometer.NoiseDensity = 0.3/sqrt(50);

% altimeter
altimeter = altimeterSensor('UpdateRate', altFs, 'NoiseDensity', 2*0.1549);

Filtro de fusión

Construya un ahrs10filter y configúrelo.

fusionfilt = ahrs10filter; 
fusionfilt.IMUSampleRate = imuFs;

Establezca valores iniciales para el filtro de fusión.

initstate = zeros(18,1);
initstate(1:4) = compact(initOrient);
initstate(5) = initPos(3);
initstate(6) = initVel(3);
initstate(7:9) = imu.Gyroscope.ConstantBias/imuFs;
initstate(10:12) = imu.Accelerometer.ConstantBias/imuFs;
initstate(13:15) = imu.MagneticField;
initstate(16:18) = imu.Magnetometer.ConstantBias;
fusionfilt.State = initstate;

Inicialice la matriz de covarianza de estado del filtro de fusión. La ground-truth se utiliza para los estados iniciales, por lo que debería haber poco error en las estimaciones.

icv = diag([1e-8*[1 1 1 1 1 1 1], 1e-3*ones(1,11)]);
fusionfilt.StateCovariance = icv;

Los ruidos de medición del magnetómetro y altímetro son los ruidos de observación asociados con los sensores utilizados por el filtro Kalman interno en el ahrs10filter. Estos valores normalmente provendrían de una hoja de datos del sensor.

magNoise =  2*(imu.Magnetometer.NoiseDensity(1).^2)*imuFs;
altimeterNoise = 2*(altimeter.NoiseDensity).^2 * altFs;

Los ruidos del proceso de filtrado se utilizan para ajustar el filtro al rendimiento deseado.

fusionfilt.AccelerometerNoise = [1e-1 1e-1 1e-4];
fusionfilt.AccelerometerBiasNoise = 1e-8;
fusionfilt.GeomagneticVectorNoise = 1e-12;
fusionfilt.MagnetometerBiasNoise = 1e-12;
fusionfilt.GyroscopeNoise = 1e-12;

Opción de simulación adicional: Visor

De forma predeterminada, esta simulación traza los errores de estimación al final de la simulación. Para ver tanto la posición estimada como la orientación junto con ground-truth a medida que se ejecuta la simulación, establezca la variable usePoseViewer en true.

usePoseViewer = false;

Bucle de simulación

q = initOrient;
firstTime = true;

actQ = zeros(N,1, 'quaternion');
expQ = zeros(N,1, 'quaternion');
actP = zeros(N,1);
expP = zeros(N,1);

for ii = 1: N
    % Generate a new set of samples from the trajectory generator 
    accBody = rotateframe(q, [0 0 +oscMotionAcc(ii)]);
    omgBody = rotateframe(q, angVel);
    [pos, q, vel, acc] = traj(accBody, omgBody);
    
    % Feed the current position and orientation to the imuSensor object
    [accel, gyro, mag] = imu(acc, omgBody, q);
    fusionfilt.predict(accel, gyro);
   
    % Fuse magnetometer samples at the magnetometer sample rate
    if ~mod(ii,imuSamplesPerMag)
        fusemag(fusionfilt, mag, magNoise);
    end

    % Sample and fuse the altimeter output at the altimeter sample rate
    if ~mod(ii,imuSamplesPerAlt)
        altHeight = altimeter(pos);
       
        % Use the |fusealtimeter| method to update the fusion filter with
        % the altimeter output.
        fusealtimeter(fusionfilt,altHeight,altimeterNoise);
    end

    % Log the actual orientation and position
    [actP(ii), actQ(ii)] = pose(fusionfilt);
    
    % Log the expected orientation and position
    expQ(ii) = q;
    expP(ii) = pos(3);

    if usePoseViewer
        hfunc.view(actP(ii), actQ(ii),expP(ii), expQ(ii)); %#ok<*UNRCH>
    end

end

Rendimiento del filtro de trazado

Trazar el rendimiento del filtro. La pantalla muestra el error en la orientación utilizando la distancia del cuaternión y el error de altura.

hfunc.plotErrs(actP, actQ, expP, expQ);

Conclusión

Este ejemplo muestra cómo el ahrs10filter realiza la fusión de sensores de 10 ejes para altura y orientación.