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

Fusión IMU y GPS para navegación inercial

Este ejemplo muestra cómo se podría construir un algoritmo de fusión IMU + GPS adecuado para vehículos aéreos no tripulados (UAV) o cuadricópteros.

Este ejemplo utiliza acelerómetros, giroscopios, magnetómetros y GPS para determinar la orientación y posición de un VANT.

Configuración de simulación

Establezca las tasas de muestreo. En un sistema típico, el acelerómetro y el giroscopio funcionan a velocidades de muestreo relativamente altas. La complejidad de procesar datos de esos sensores en el algoritmo de fusión es relativamente baja. Por el contrario, el GPS, y en algunos casos el magnetómetro, funcionan a velocidades de muestreo relativamente bajas y la complejidad asociada con su procesamiento es alta. En este algoritmo de fusión, las muestras del magnetómetro y del GPS se procesan juntas a la misma velocidad baja, y las muestras del acelerómetro y del giroscopio se procesan juntas a la misma velocidad alta.

Para simular esta configuración, la IMU (acelerómetro, giroscopio y magnetómetro) se muestrea a 160 Hz y el GPS a 1 Hz. Sólo una de cada 160 muestras del magnetómetro se aplica al algoritmo de fusión, por lo que en un sistema real el magnetómetro podría muestrearse a una velocidad mucho menor.

imuFs = 160;
gpsFs = 1;

% Define where on the Earth this simulated scenario takes place using the
% latitude, longitude and altitude.
refloc = [42.2825 -72.3430 53.0352];


% Validate that the |gpsFs| divides |imuFs|. This allows the sensor sample 
% rates to be simulated using a nested for loop without complex sample rate
% matching.

imuSamplesPerGPS = (imuFs/gpsFs);
assert(imuSamplesPerGPS == fix(imuSamplesPerGPS), ...
    'GPS sampling rate must be an integer factor of IMU sampling rate.');

Filtro de fusión

Cree el filtro para fusionar las mediciones de IMU + GPS. El filtro de fusión utiliza un filtro de Kalman extendido para rastrear la orientación (como un cuaternión), la velocidad, la posición, los sesgos del sensor y el vector geomagnético.

Este insfilterMARG tiene algunos métodos para procesar datos de sensores, incluidos predict, fusemag y fusegps. El método predict toma las muestras del acelerómetro y giroscopio de la IMU como entradas. Llame al método predict cada vez que se muestreen el acelerómetro y el giroscopio. Este método predice los estados un paso adelante basándose en el acelerómetro y el giroscopio. La covarianza de error del filtro de Kalman extendido se actualiza aquí.

El método fusegps toma muestras de GPS como entrada. Este método actualiza los estados del filtro basándose en muestras de GPS calculando una ganancia de Kalman que pondera las distintas entradas del sensor según su incertidumbre. Aquí también se actualiza una covarianza de error, esta vez utilizando también la ganancia de Kalman.

El método fusemag es similar pero actualiza los estados, la ganancia de Kalman y la covarianza del error en función de las muestras del magnetómetro.

Aunque el insfilterMARG toma muestras de acelerómetro y giroscopio como entradas, estas están integradas para calcular velocidades y ángulos delta, respectivamente. El filtro rastrea la polarización del magnetómetro y estas señales integradas.

fusionfilt = insfilterMARG;
fusionfilt.IMUSampleRate = imuFs;
fusionfilt.ReferenceLocation = refloc;

Trayectoria del VANT

Este ejemplo utiliza una trayectoria guardada registrada desde un VANT como ground-truth. Esta trayectoria se envía a varios simuladores de sensores para calcular flujos de datos simulados de acelerómetro, giroscopio, magnetómetro y GPS.

% Load the "ground truth" UAV trajectory.
load LoggedQuadcopter.mat trajData;
trajOrient = trajData.Orientation;
trajVel = trajData.Velocity;
trajPos = trajData.Position;
trajAcc = trajData.Acceleration;
trajAngVel = trajData.AngularVelocity;

% Initialize the random number generator used in the simulation of sensor
% noise.
rng(1)

Sensor GPS

Configure el GPS a la frecuencia de muestreo y ubicación de referencia especificadas. Los otros parámetros controlan la naturaleza del ruido en la señal de salida.

gps = gpsSensor('UpdateRate', gpsFs);
gps.ReferenceLocation = refloc;     
gps.DecayFactor = 0.5;              % Random walk noise parameter 
gps.HorizontalPositionAccuracy = 1.6;   
gps.VerticalPositionAccuracy =  1.6;
gps.VelocityAccuracy = 0.1;

Sensores IMU

Normalmente, un VANT utiliza un sensor MARG integrado (magnético, velocidad angular, gravedad) para estimar la pose. Para modelar un sensor MARG, defina un modelo de sensor IMU que contenga un acelerómetro, giroscopio y magnetómetro. En una aplicación del mundo real, los tres sensores podrían provenir de un único circuito integrado o de circuitos separados. Los valores de propiedad establecidos aquí son típicos de sensores MEMS de bajo coste.

imu = imuSensor('accel-gyro-mag', 'SampleRate', imuFs);
imu.MagneticField = [19.5281 -5.0741 48.0067];

% 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);

Inicialice el vector de estado del insfilterMARG

El insfilterMARG rastrea los estados de pose en un vector de 22 elementos. Los estados son:

     State                           Units        State Vector Index
 Orientation as a quaternion                      1:4
 Position (NED)                      m            5:7
 Velocity (NED)                      m/s          8:10 
 Delta Angle Bias (XYZ)              rad          11:13
 Delta Velocity Bias (XYZ)           m/s          14:16
 Geomagnetic Field Vector (NED)      uT           17:19
 Magnetometer Bias (XYZ)             uT           20:22

La ground-truth se utiliza para ayudar a inicializar los estados del filtro, de modo que el filtro converja rápidamente hacia buenas respuestas.

% Initialize the states of the filter 

initstate = zeros(22,1);
initstate(1:4) = compact( meanrot(trajOrient(1:100))); 
initstate(5:7) = mean( trajPos(1:100,:), 1);
initstate(8:10) = mean( trajVel(1:100,:), 1);
initstate(11:13) =  imu.Gyroscope.ConstantBias./imuFs;
initstate(14:16) =  imu.Accelerometer.ConstantBias./imuFs;
initstate(17:19) =  imu.MagneticField;
initstate(20:22) = imu.Magnetometer.ConstantBias;

fusionfilt.State = initstate;

Inicializar las variaciones del insfilterMARG

Los ruidos de medición insfilterMARG describen cuánto ruido está corrompiendo la lectura del sensor. Estos valores se basan en los parámetros imuSensor y gpsSensor .

Los ruidos del proceso describen qué tan bien las ecuaciones de filtro describen la evolución del estado. Los ruidos del proceso se determinan empíricamente mediante barrido de parámetros para optimizar conjuntamente las estimaciones de posición y orientación del filtro.

% Measurement noises
Rmag = 0.0862; % Magnetometer measurement noise
Rvel = 0.0051; % GPS Velocity measurement noise
Rpos = 5.169; % GPS Position measurement noise

% Process noises
fusionfilt.AccelerometerBiasNoise =  0.010716; 
fusionfilt.AccelerometerNoise = 9.7785; 
fusionfilt.GyroscopeBiasNoise = 1.3436e-14; 
fusionfilt.GyroscopeNoise =  0.00016528; 
fusionfilt.MagnetometerBiasNoise = 2.189e-11;
fusionfilt.GeomagneticVectorNoise = 7.67e-13;

% Initial error covariance
fusionfilt.StateCovariance = 1e-9*eye(22);

Inicializar ámbitos

El alcance HelperScrollingPlotter permite trazar variables a lo largo del tiempo. Se utiliza aquí para rastrear errores en la pose. El alcance HelperPoseViewer permite la visualización tridimensional de la estimación del filtro y la pose de ground-truth. Los alcances pueden ralentizar la simulación. Para deshabilitar un alcance, establezca la variable lógica correspondiente en false.

useErrScope = true;  % Turn on the streaming error plot
usePoseView = true;  % Turn on the 3-D pose viewer

if useErrScope
    errscope = HelperScrollingPlotter(...
        'NumInputs', 4, ...
        'TimeSpan', 10, ...
        'SampleRate', imuFs, ...
        'YLabel', {'degrees', ...
        'meters', ...
        'meters', ...
        'meters'}, ...
        'Title', {'Quaternion Distance', ...
        'Position X Error', ...
        'Position Y Error', ...
        'Position Z Error'}, ...
        'YLimits', ...
        [ -3, 3
        -2, 2
        -2 2
        -2 2]);
end

if usePoseView
    posescope = HelperPoseViewer(...
        'XPositionLimits', [-15 15], ...
        'YPositionLimits', [-15, 15], ...
        'ZPositionLimits', [-10 10]);
end

Bucle de simulación

El bucle de simulación principal es un bucle while con un bucle for anidado. El bucle while se ejecuta en gpsFs, que es la frecuencia de muestreo del GPS. El bucle for anidado se ejecuta en imuFs, que es la frecuencia de muestreo de IMU. Los alcances se actualizan a la frecuencia de muestreo de IMU.

% Loop setup - |trajData| has about 142 seconds of recorded data.
secondsToSimulate = 50; % simulate about 50 seconds
numsamples = secondsToSimulate*imuFs;

loopBound = floor(numsamples);
loopBound = floor(loopBound/imuFs)*imuFs; % ensure enough IMU Samples

% Log data for final metric computation.
pqorient = quaternion.zeros(loopBound,1);
pqpos = zeros(loopBound,3);

fcnt = 1;

while(fcnt <=loopBound)
    % |predict| loop at IMU update frequency.
    for ff=1:imuSamplesPerGPS
        % Simulate the IMU data from the current pose.
        [accel, gyro, mag] = imu(trajAcc(fcnt,:), trajAngVel(fcnt, :), ...
            trajOrient(fcnt));
        
        % Use the |predict| method to estimate the filter state based
        % on the simulated accelerometer and gyroscope signals.
        predict(fusionfilt, accel, gyro);
        
        % Acquire the current estimate of the filter states.
        [fusedPos, fusedOrient] = pose(fusionfilt);
        
        % Save the position and orientation for post processing.
        pqorient(fcnt) = fusedOrient;
        pqpos(fcnt,:) = fusedPos;
        
        % Compute the errors and plot.
        if useErrScope
            orientErr = rad2deg(dist(fusedOrient, ...
                trajOrient(fcnt) ));
            posErr = fusedPos - trajPos(fcnt,:); 
            errscope(orientErr, posErr(1), posErr(2), posErr(3));
        end
        
        % Update the pose viewer.
        if usePoseView
            posescope(pqpos(fcnt,:), pqorient(fcnt),  trajPos(fcnt,:), ...
                trajOrient(fcnt,:) );
        end
        fcnt = fcnt + 1;
    end
    
    % This next step happens at the GPS sample rate.
    % Simulate the GPS output based on the current pose.
    [lla, gpsvel] = gps( trajPos(fcnt,:), trajVel(fcnt,:) );
    
    % Correct the filter states based on the GPS data and magnetic
    % field measurements.
    fusegps(fusionfilt, lla, Rpos, gpsvel, Rvel);
    fusemag(fusionfilt, mag, Rmag);
 
end

Cálculo de métricas de error

Las estimaciones de posición y orientación se registraron durante toda la simulación. Ahora calcule un error cuadrático medio de extremo a extremo para la posición y la orientación.

posd = pqpos(1:loopBound,:) - trajPos( 1:loopBound, :);

% For orientation, quaternion distance is a much better alternative to
% subtracting Euler angles, which have discontinuities. The quaternion
% distance can be computed with the |dist| function, which gives the
% angular difference in orientation in radians. Convert to degrees
% for display in the command window. 

quatd = rad2deg(dist(pqorient(1:loopBound), trajOrient(1:loopBound)) );

% Display RMS errors in the command window.
fprintf('\n\nEnd-to-End Simulation Position RMS Error\n');

End-to-End Simulation Position RMS Error

msep = sqrt(mean(posd.^2));
fprintf('\tX: %.2f , Y: %.2f, Z: %.2f   (meters)\n\n',msep(1), ...
    msep(2), msep(3));

	X: 0.50 , Y: 0.79, Z: 0.65   (meters)

fprintf('End-to-End Quaternion Distance RMS Error (degrees) \n');

End-to-End Quaternion Distance RMS Error (degrees) 

fprintf('\t%.2f (degrees)\n\n', sqrt(mean(quatd.^2)));

	1.45 (degrees)