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 pose a partir de sensores asíncronos

Abrir script

Este ejemplo muestra cómo se pueden fusionar sensores a diferentes velocidades para estimar la pose. Se utilizan acelerómetro, giroscopio, magnetómetro y GPS para determinar la orientación y posición de un vehículo que se mueve a lo largo de una trayectoria circular. Puede usar los controles en la ventana de la figura para variar las velocidades del sensor y experimentar con la caída del sensor mientras observa el efecto en la pose estimada.

Configuración de simulación

Cargue datos de sensores pregrabados. Los datos del sensor se basan en una trayectoria circular creada utilizando la clase waypointTrajectory . Los valores del sensor se crearon utilizando las clases gpsSensor y imuSensor . El archivo CircularTrajectorySensorData.mat utilizado aquí se puede generar con la función generateCircularTrajSensorData .

ld = load('CircularTrajectorySensorData.mat');

Fs = ld.Fs; % maximum MARG rate
gpsFs = ld.gpsFs; % maximum GPS rate
ratio = Fs./gpsFs;
refloc = ld.refloc;

trajOrient = ld.trajData.Orientation;
trajVel = ld.trajData.Velocity;
trajPos = ld.trajData.Position;
trajAcc = ld.trajData.Acceleration;
trajAngVel = ld.trajData.AngularVelocity;

accel = ld.accel;
gyro = ld.gyro;
mag = ld.mag;
lla = ld.lla;
gpsvel = ld.gpsvel;

Filtro de fusión

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

Este insfilterAsync tiene varios métodos para procesar los datos del sensor: fuseaccel, fusegyro, fusemag y fusegps. Debido a que insfilterAsync utiliza un EKF discreto continuo, el método predict puede hacer avanzar el filtro una cantidad de tiempo arbitraria.

fusionfilt = insfilterAsync('ReferenceLocation', refloc);

Inicialice el vector de estado del insfilterAsync

El insfilterAsync rastrea los estados de pose en un vector de 28 elementos. Los estados son:

     States                          Units    Index
  Orientation (quaternion parts)             1:4
  Angular Velocity (XYZ)            rad/s    5:7
  Position (NED)                    m        8:10
  Velocity (NED)                    m/s      11:13
  Acceleration (NED)                m/s^2    14:16
  Accelerometer Bias (XYZ)          m/s^2    17:19
  Gyroscope Bias (XYZ)              rad/s    20:22
  Geomagnetic Field Vector (NED)    uT       23:25
  Magnetometer Bias (XYZ)           uT       26:28

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

Nav = 100;
initstate = zeros(28,1);
initstate(1:4) = compact( meanrot(trajOrient(1:Nav)));
initstate(5:7) = mean( trajAngVel(10:Nav,:), 1);
initstate(8:10) = mean( trajPos(1:Nav,:), 1);
initstate(11:13) = mean( trajVel(1:Nav,:), 1);
initstate(14:16) = mean( trajAcc(1:Nav,:), 1);
initstate(23:25) = ld.magField;

% The gyroscope bias initial value estimate is low for the Z-axis. This is
% done to illustrate the effects of fusing the magnetometer in the
% simulation.
initstate(20:22) = deg2rad([3.125 3.125 3.125]);
fusionfilt.State = initstate;

Establezca los valores de ruido del proceso del insfilterAsync

La variación del ruido del proceso describe la incertidumbre del modelo de movimiento que utiliza el filtro.

fusionfilt.QuaternionNoise = 1e-2;
fusionfilt.AngularVelocityNoise = 100;
fusionfilt.AccelerationNoise = 100;
fusionfilt.MagnetometerBiasNoise = 1e-7;
fusionfilt.AccelerometerBiasNoise = 1e-7;
fusionfilt.GyroscopeBiasNoise = 1e-7;

Definir los valores de ruido de medición utilizados para fusionar datos del sensor

Cada sensor tiene algo de ruido en las mediciones. Estos valores normalmente se pueden encontrar en la hoja de datos de un sensor.

Rmag = 0.4;
Rvel = 0.01;
Racc = 610;
Rgyro = 0.76e-5;
Rpos = 3.4;

fusionfilt.StateCovariance = diag(1e-3*ones(28,1));

Inicializar ámbitos

El alcance HelperScrollingPlotter permite trazar variables a lo largo del tiempo. Se utiliza aquí para rastrear errores en la pose. El alcance PoseViewerWithSwitches permite la visualización 3D 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 falso.

useErrScope = true; % Turn on the streaming error plot.
usePoseView = true; % Turn on the 3D pose viewer.
if usePoseView
    posescope = PoseViewerWithSwitches(...
        'XPositionLimits', [-30 30], ...
        'YPositionLimits', [-30, 30], ...
        'ZPositionLimits', [-10 10]);
end
f = gcf;

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

Bucle de simulación

La simulación del algoritmo de fusión le permite inspeccionar los efectos de las diferentes frecuencias de muestreo del sensor. Además, se puede evitar la fusión de sensores individuales desmarcando la casilla de verificación correspondiente. Esto se puede utilizar para simular la caída del sensor.

Algunas configuraciones producen resultados espectaculares. Por ejemplo, apagar el sensor GPS hace que la estimación de posición se desvíe rápidamente. Apagar el sensor del magnetómetro hará que la estimación de orientación se desvíe lentamente de ground-truth ya que la estimación gira demasiado rápido. Por el contrario, si el giroscopio está apagado y el magnetómetro está encendido, la orientación estimada muestra una oscilación y carece de la suavidad presente si se utilizan ambos sensores.

Al encender todos los sensores pero configurarlos para que funcionen a la velocidad más baja se produce una estimación que se desvía visiblemente de ground-truth y luego vuelve a un resultado más correcto cuando se fusionan los sensores. Esto se ve más fácilmente en el HelperScrollingPlotter de los errores de estimación corriente.

La simulación principal se ejecuta a 100 Hz. Cada iteración inspecciona las casillas de verificación en la ventana de la figura y, si el sensor está habilitado, fusiona los datos para ese sensor a la velocidad adecuada.

for ii=1:size(accel,1)
    fusionfilt.predict(1./Fs);

    % Fuse Accelerometer
    if (f.UserData.Accelerometer) && ...
        mod(ii, fix(Fs/f.UserData.AccelerometerSampleRate)) == 0

        fusionfilt.fuseaccel(accel(ii,:), Racc);
    end

    % Fuse Gyroscope
    if (f.UserData.Gyroscope) && ...
        mod(ii, fix(Fs/f.UserData.GyroscopeSampleRate)) == 0

        fusionfilt.fusegyro(gyro(ii,:), Rgyro);
    end

    % Fuse Magnetometer
    if (f.UserData.Magnetometer) && ...
        mod(ii, fix(Fs/f.UserData.MagnetometerSampleRate)) == 0

        fusionfilt.fusemag(mag(ii,:), Rmag);
    end

    % Fuse GPS
    if (f.UserData.GPS) && mod(ii, fix(Fs/f.UserData.GPSSampleRate)) == 0
        fusionfilt.fusegps(lla(ii,:), Rpos, gpsvel(ii,:), Rvel);
    end

    % Plot the pose error
    [p,q] = pose(fusionfilt);
    posescope(p, q, trajPos(ii,:), trajOrient(ii));

    orientErr = rad2deg(dist(q, trajOrient(ii) ));
    posErr = p - trajPos(ii,:);
    errscope(orientErr, posErr(1), posErr(2), posErr(3));
end

Conclusión

El insfilterAsync permite varias y variables frecuencias de muestreo. La calidad de los resultados estimados depende en gran medida de las tasas de fusión de sensores individuales. Cualquier caída del sensor tendrá un efecto profundo en la salida.