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.

Estimar la posición y orientación de un vehículo terrestre

Abrir script en vivo

Este ejemplo muestra cómo estimar la posición y orientación de vehículos terrestres fusionando datos de una unidad de medición inercial (IMU) y un receptor del sistema de posicionamiento global (GPS).

Configuración de simulación

Establezca las tasas de muestreo. En un sistema típico, el acelerómetro y el giroscopio de la IMU 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 funciona a una frecuencia de muestreo relativamente baja y la complejidad asociada con su procesamiento es alta. En este algoritmo de fusión, las muestras de GPS se procesan a una velocidad baja y las muestras del acelerómetro y el giroscopio se procesan juntas a la misma velocidad alta.

Para simular esta configuración, la IMU (acelerómetro y giroscopio) se muestrea a 100 Hz y el GPS a 10 Hz.

imuFs = 100;
gpsFs = 10;

% Define where on the Earth this simulation takes place using latitude, 
% longitude, and altitude (LLA) coordinates.
localOrigin = [42.2825 -71.343 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 posición, la velocidad y los sesgos del sensor.

El objeto insfilterNonholonomic tiene dos métodos principales: predict y fusegps. El método predict toma las muestras del acelerómetro y giroscopio de la IMU como entrada. Llame al método predict cada vez que se muestreen el acelerómetro y el giroscopio. Este método predice los estados hacia adelante en un paso de tiempo basándose en el acelerómetro y el giroscopio. En este paso se actualiza la covarianza de error del filtro de Kalman extendido.

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

El objeto insfilterNonholonomic tiene dos propiedades principales: IMUSampleRate y DecimationFactor. El vehículo terrestre tiene dos restricciones de velocidad que suponen que no rebota ni se desliza sobre el suelo. Estas restricciones se aplican utilizando las ecuaciones de actualización del filtro de Kalman extendidas. Estas actualizaciones se aplican a los estados del filtro a una velocidad de IMUSampleRate/DecimationFactor Hz.

gndFusion = insfilterNonholonomic('ReferenceFrame', 'ENU', ...
    'IMUSampleRate', imuFs, ...
    'ReferenceLocation', localOrigin, ...
    'DecimationFactor', 2);

Crear trayectoria de vehículo terrestre

El objeto waypointTrajectory calcula la pose en función de la frecuencia de muestreo, los puntos de referencia, los tiempos de llegada y la orientación especificados. Especifique los parámetros de una trayectoria circular para el vehículo terrestre.

% Trajectory parameters
r = 8.42; % (m)
speed = 2.50; % (m/s)
center = [0, 0]; % (m)
initialYaw = 90; % (degrees)
numRevs = 2;

% Define angles theta and corresponding times of arrival t.
revTime = 2*pi*r / speed;
theta = (0:pi/2:2*pi*numRevs).';
t = linspace(0, revTime*numRevs, numel(theta)).';

% Define position.
x = r .* cos(theta) + center(1);
y = r .* sin(theta) + center(2);
z = zeros(size(x));
position = [x, y, z];

% Define orientation.
yaw = theta + deg2rad(initialYaw);
yaw = mod(yaw, 2*pi);
pitch = zeros(size(yaw));
roll = zeros(size(yaw));
orientation = quaternion([yaw, pitch, roll], 'euler', ...
    'ZYX', 'frame');

% Generate trajectory.
groundTruth = waypointTrajectory('SampleRate', imuFs, ...
    'Waypoints', position, ...
    'TimeOfArrival', t, ...
    'Orientation', orientation);

% Initialize the random number generator used to simulate sensor noise.
rng('default');

Receptor 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, 'ReferenceFrame', 'ENU');
gps.ReferenceLocation = localOrigin;
gps.DecayFactor = 0.5;                % Random walk noise parameter 
gps.HorizontalPositionAccuracy = 1.0;   
gps.VerticalPositionAccuracy =  1.0;
gps.VelocityAccuracy = 0.1;

Sensores IMU

Normalmente, los vehículos terrestres utilizan un sensor IMU de 6 ejes para estimar la pose. Para modelar un sensor IMU, defina un modelo de sensor IMU que contenga un acelerómetro y un giroscopio. En una aplicación del mundo real, los dos 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', ...
    'ReferenceFrame', 'ENU', 'SampleRate', imuFs);

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

% Gyroscope
imu.Gyroscope.MeasurementRange = deg2rad(250);
imu.Gyroscope.Resolution = deg2rad(0.0625);
imu.Gyroscope.NoiseDensity = deg2rad(0.025);

Inicializar los Estados del insfilterNonholonomic

Los estados son:

States                            Units    Index
Orientation (quaternion parts)             1:4  
Gyroscope Bias (XYZ)              rad/s    5:7  
Position (NED)                    m        8:10 
Velocity (NED)                    m/s      11:13
Accelerometer Bias (XYZ)          m/s^2    14:16

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

% Get the initial ground truth pose from the first sample of the trajectory
% and release the ground truth trajectory to ensure the first sample is not 
% skipped during simulation.
[initialPos, initialAtt, initialVel] = groundTruth();
reset(groundTruth);

% Initialize the states of the filter
gndFusion.State(1:4) = compact(initialAtt).';
gndFusion.State(5:7) = imu.Gyroscope.ConstantBias;
gndFusion.State(8:10) = initialPos.';
gndFusion.State(11:13) = initialVel.';
gndFusion.State(14:16) = imu.Accelerometer.ConstantBias;

Inicializar las variaciones del insfilterNonholonomic

Los ruidos de medición describen cuánto ruido está corrompiendo la lectura del GPS según los parámetros gpsSensor y cuánta incertidumbre hay en el modelo dinámico del vehículo.

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
Rvel = gps.VelocityAccuracy.^2;
Rpos = gps.HorizontalPositionAccuracy.^2;

% The dynamic model of the ground vehicle for this filter assumes there is
% no side slip or skid during movement. This means that the velocity is 
% constrained to only the forward body axis. The other two velocity axis 
% readings are corrected with a zero measurement weighted by the 
% |ZeroVelocityConstraintNoise| parameter.
gndFusion.ZeroVelocityConstraintNoise = 1e-2;

% Process noises
gndFusion.GyroscopeNoise = 4e-6;
gndFusion.GyroscopeBiasNoise = 4e-14;
gndFusion.AccelerometerNoise = 4.8e-2;
gndFusion.AccelerometerBiasNoise = 4e-14;

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

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 3D 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', ...
            [-1, 1
             -1, 1
             -1, 1
             -1, 1]);
end

if usePoseView
    viewer = HelperPoseViewer( ...
        'XPositionLimits', [-15, 15], ...
        'YPositionLimits', [-15, 15], ...
        'ZPositionLimits', [-5, 5], ...
        'ReferenceFrame', 'ENU');
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 velocidad de medición 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.

totalSimTime = 30; % seconds

% Log data for final metric computation.
numsamples = floor(min(t(end), totalSimTime) * gpsFs);
truePosition = zeros(numsamples,3);
trueOrientation = quaternion.zeros(numsamples,1);
estPosition = zeros(numsamples,3);
estOrientation = quaternion.zeros(numsamples,1);

idx = 0;

for sampleIdx = 1:numsamples
    % Predict loop at IMU update frequency.
    for i = 1:imuSamplesPerGPS
        if ~isDone(groundTruth)
            idx = idx + 1;
            
            % Simulate the IMU data from the current pose.
            [truePosition(idx,:), trueOrientation(idx,:), ...
                trueVel, trueAcc, trueAngVel] = groundTruth();
            [accelData, gyroData] = imu(trueAcc, trueAngVel, ...
                trueOrientation(idx,:));
            
            % Use the predict method to estimate the filter state based
            % on the accelData and gyroData arrays.
            predict(gndFusion, accelData, gyroData);
            
            % Log the estimated orientation and position.
            [estPosition(idx,:), estOrientation(idx,:)] = pose(gndFusion);
            
            % Compute the errors and plot.
            if useErrScope
                orientErr = rad2deg( ...
                    dist(estOrientation(idx,:), trueOrientation(idx,:)));
                posErr = estPosition(idx,:) - truePosition(idx,:);
                errscope(orientErr, posErr(1), posErr(2), posErr(3));
            end

            % Update the pose viewer.
            if usePoseView
                viewer(estPosition(idx,:), estOrientation(idx,:), ...
                    truePosition(idx,:), estOrientation(idx,:));
            end
        end
    end
    
    if ~isDone(groundTruth)
        % This next step happens at the GPS sample rate.
        % Simulate the GPS output based on the current pose.
        [lla, gpsVel] = gps(truePosition(idx,:), trueVel);

        % Update the filter states based on the GPS data.
        fusegps(gndFusion, lla, Rpos, gpsVel, Rvel);
    end
end

Figure Scrolling Plotter contains 4 axes objects. Axes object 1 with title Position Z Error, xlabel Time (s), ylabel meters contains an object of type line. Axes object 2 with title Position Y Error, xlabel Time (s), ylabel meters contains an object of type line. Axes object 3 with title Position X Error, xlabel Time (s), ylabel meters contains an object of type line. Axes object 4 with title Quaternion Distance, xlabel Time (s), ylabel degrees contains an object of type line.

MATLAB figure

Cálculo de métricas de error

La posición y la 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 = estPosition - truePosition;

% 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(estOrientation, trueOrientation));

% 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: 1.16 , Y: 0.98, Z: 0.03   (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)));
	0.11 (degrees)