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

Odometría visual-inercial utilizando datos sintéticos

Este ejemplo muestra cómo estimar la pose (posición y orientación) de un vehículo terrestre utilizando una unidad de medición inercial (IMU) y una cámara monocular. En este ejemplo, usted:

Cree un escenario de conducción que contenga la trayectoria real del vehículo en el terreno.
Utilice una IMU y un modelo de odometría visual para generar mediciones.
Fusione estas medidas para estimar la pose del vehículo y luego muestre los resultados.

Las estimaciones de odometría visual-inercial representan fusionando la estimación de pose de odometría visual de la cámara monocular y la estimación de pose de la IMU. La IMU devuelve una estimación de pose precisa para intervalos de tiempo pequeños, pero sufre una gran deriva debido a la integración de las mediciones del sensor inercial. La cámara monocular arroja una estimación precisa de la pose durante un intervalo de tiempo mayor, pero adolece de una ambigüedad de escala. Dadas estas fortalezas y debilidades complementarias, la fusión de estos sensores mediante odometría visual-inercial es una opción adecuada. Este método se puede utilizar en escenarios donde las lecturas de GPS no están disponibles, como en un cañón urbano.

Cree un escenario de conducción con trayectoria

Cree un objeto drivingScenario (Automated Driving Toolbox) que contenga:

La vía por la que circula el vehículo.
Los edificios que rodean ambos lados de la carretera.
Posición inicial del vehículo
La pose estimada del vehículo.

La pose real del vehículo en el terreno se muestra como un cuboide azul sólido. La pose estimada se muestra como un cuboide azul transparente. Tenga en cuenta que la pose estimada no aparece en la visualización inicial porque ground-truth y las poses estimadas se superponen.

Genere la trayectoria de referencia para el vehículo terrestre utilizando el waypointTrajectory (Sensor Fusion and Tracking Toolbox) System object™. Tenga en cuenta que waypointTrajectory se utiliza en lugar de drivingScenario/trajectory ya que se necesita la aceleración del vehículo. La trayectoria se genera a una frecuencia de muestreo específica utilizando un conjunto de puntos de referencia, tiempos de llegada y velocidades.

% Create the driving scenario with both the ground truth and estimated
% vehicle poses.
scene = drivingScenario;
groundTruthVehicle = vehicle(scene, 'PlotColor', [0 0.4470 0.7410]);
estVehicle = vehicle(scene, 'PlotColor', [0 0.4470 0.7410]);

% Generate the baseline trajectory.
sampleRate = 100;
wayPoints = [  0   0 0;
             200   0 0;
             200  50 0;
             200 230 0;
             215 245 0;
             260 245 0;
             290 240 0;
             310 258 0;
             290 275 0;
             260 260 0;
             -20 260 0];
t = [0 20 25 44 46 50 54 56 59 63 90].';
speed = 10;
velocities = [ speed     0 0;
               speed     0 0;
                   0 speed 0;
                   0 speed 0;
               speed     0 0;
               speed     0 0;
               speed     0 0;
                   0 speed 0;
              -speed     0 0;
              -speed     0 0;
              -speed     0 0];

traj = waypointTrajectory(wayPoints, 'TimeOfArrival', t, ...
    'Velocities', velocities, 'SampleRate', sampleRate);

% Add a road and buildings to scene and visualize.
helperPopulateScene(scene, groundTruthVehicle);

Crear un filtro de fusión

Cree el filtro para fusionar IMU y mediciones de odometría visual. Este ejemplo utiliza un método débilmente acoplado para fusionar las medidas. Si bien los resultados no son tan precisos como los de un método estrechamente acoplado, la cantidad de procesamiento requerido es significativamente menor y los resultados son adecuados. El filtro de fusión utiliza un filtro de Kalman de estado de error para rastrear la orientación (como un cuaternión), la posición, la velocidad y los sesgos del sensor.

El objeto insfilterErrorState tiene las siguientes funciones para procesar datos del sensor: predict y fusemvo.

La función predict toma las mediciones del acelerómetro y giroscopio de la IMU como entradas. Llame a la función predict cada vez que se muestreen el acelerómetro y el giroscopio. Esta función predice el estado hacia adelante en un paso de tiempo basándose en las mediciones del acelerómetro y el giroscopio, y actualiza la covarianza del estado de error del filtro.

La función fusemvo toma como entrada las estimaciones de la pose de odometría visual. Esta función actualiza los estados de error basándose en las estimaciones de pose de odometría visual calculando una ganancia de Kalman que pesa las distintas entradas según su incertidumbre. Al igual que con la función predict , esta función también actualiza la covarianza del estado de error, esta vez teniendo en cuenta la ganancia de Kalman. Luego, el estado se actualiza utilizando el nuevo estado de error y se restablece el estado de error.

filt = insfilterErrorState('IMUSampleRate', sampleRate, ...
    'ReferenceFrame', 'ENU')
% Set the initial state and error state covariance.
helperInitialize(filt, traj);

filt = 

  insfilterErrorState with properties:

        IMUSampleRate: 100               Hz         
    ReferenceLocation: [0 0 0]           [deg deg m]
                State: [17⨯1 double]                
      StateCovariance: [16⨯16 double]               

   Process Noise Variances
            GyroscopeNoise: [1e-06 1e-06 1e-06]       (rad/s)²
        AccelerometerNoise: [0.0001 0.0001 0.0001]    (m/s²)² 
        GyroscopeBiasNoise: [1e-09 1e-09 1e-09]       (rad/s)²
    AccelerometerBiasNoise: [0.0001 0.0001 0.0001]    (m/s²)²

Especificar el modelo de odometría visual

Definir los parámetros del modelo de odometría visual. Estos parámetros modelan un sistema de odometría visual basado en seguimiento y coincidencia de características utilizando una cámara monocular. El parámetro scale representa la escala desconocida de los fotogramas de visión posteriores de la cámara monocular. Los otros parámetros modelan la deriva en la lectura de odometría visual como una combinación de ruido blanco y un proceso de Gauss-Markov de primer orden.

% The flag useVO determines if visual odometry is used:
% useVO = false; % Only IMU is used.
useVO = true; % Both IMU and visual odometry are used.

paramsVO.scale = 2;
paramsVO.sigmaN = 0.139;
paramsVO.tau = 232;
paramsVO.sigmaB = sqrt(1.34);
paramsVO.driftBias = [0 0 0];

Especificar el sensor IMU

Defina un modelo de sensor IMU que contenga un acelerómetro y un giroscopio usando el System object imuSensor . El modelo de sensor contiene propiedades para modelar fuentes de ruido tanto deterministas como estocásticas. Los valores de propiedad establecidos aquí son típicos de sensores MEMS de bajo coste.

% Set the RNG seed to default to obtain the same results for subsequent
% runs.
rng('default')

imu = imuSensor('SampleRate', sampleRate, 'ReferenceFrame', 'ENU');

% Accelerometer
imu.Accelerometer.MeasurementRange =  19.6; % m/s^2
imu.Accelerometer.Resolution = 0.0024; % m/s^2/LSB
imu.Accelerometer.NoiseDensity = 0.01; % (m/s^2)/sqrt(Hz)

% Gyroscope
imu.Gyroscope.MeasurementRange = deg2rad(250); % rad/s
imu.Gyroscope.Resolution = deg2rad(0.0625); % rad/s/LSB
imu.Gyroscope.NoiseDensity = deg2rad(0.0573); % (rad/s)/sqrt(Hz)
imu.Gyroscope.ConstantBias = deg2rad(2); % rad/s

Configurar la simulación

Especifique la cantidad de tiempo para ejecutar la simulación e inicializar las variables que se registran durante el ciclo de simulación.

% Run the simulation for 60 seconds.
numSecondsToSimulate = 60;
numIMUSamples = numSecondsToSimulate * sampleRate;

% Define the visual odometry sampling rate.
imuSamplesPerCamera = 4;
numCameraSamples = ceil(numIMUSamples / imuSamplesPerCamera);

% Preallocate data arrays for plotting results.
[pos, orient, vel, acc, angvel, ...
    posVO, orientVO, ...
    posEst, orientEst, velEst] ...
    = helperPreallocateData(numIMUSamples, numCameraSamples);

% Set measurement noise parameters for the visual odometry fusion.
RposVO = 0.1;
RorientVO = 0.1;

Ejecute el bucle de simulación

Ejecute la simulación a la frecuencia de muestreo de IMU. Cada muestra de IMU se utiliza para predecir el estado del filtro en un paso de tiempo. Una vez que está disponible una nueva lectura de odometría visual, se utiliza para corregir el estado actual del filtro.

Hay cierta desviación en las estimaciones del filtro que se puede corregir aún más con un sensor adicional como un GPS o una restricción adicional como un mapa de límites de carreteras.

cameraIdx = 1;
for i = 1:numIMUSamples
    % Generate ground truth trajectory values.
    [pos(i,:), orient(i,:), vel(i,:), acc(i,:), angvel(i,:)] = traj();

    % Generate accelerometer and gyroscope measurements from the ground truth
    % trajectory values.
    [accelMeas, gyroMeas] = imu(acc(i,:), angvel(i,:), orient(i));

    % Predict the filter state forward one time step based on the
    % accelerometer and gyroscope measurements.
    predict(filt, accelMeas, gyroMeas);

    if (1 == mod(i, imuSamplesPerCamera)) && useVO
        % Generate a visual odometry pose estimate from the ground truth
        % values and the visual odometry model.
        [posVO(cameraIdx,:), orientVO(cameraIdx,:), paramsVO] = ...
            helperVisualOdometryModel(pos(i,:), orient(i,:), paramsVO);

        % Correct filter state based on visual odometry data.
        fusemvo(filt, posVO(cameraIdx,:), RposVO, ...
            orientVO(cameraIdx), RorientVO);

        cameraIdx = cameraIdx + 1;
    end

    [posEst(i,:), orientEst(i,:), velEst(i,:)] = pose(filt);

    % Update estimated vehicle pose.
    helperUpdatePose(estVehicle, posEst(i,:), velEst(i,:), orientEst(i));

    % Update ground truth vehicle pose.
    helperUpdatePose(groundTruthVehicle, pos(i,:), vel(i,:), orient(i));

    % Update driving scenario visualization.
    updatePlots(scene);
    drawnow limitrate;
end

Representar los resultados

Trace la trayectoria del vehículo ground-truth, la estimación de la odometría visual y la estimación del filtro de fusión.

figure
if useVO
    plot3(pos(:,1), pos(:,2), pos(:,3), '-.', ...
        posVO(:,1), posVO(:,2), posVO(:,3), ...
        posEst(:,1), posEst(:,2), posEst(:,3), ...
        'LineWidth', 3)
    legend('Ground Truth', 'Visual Odometry (VO)', ...
        'Visual-Inertial Odometry (VIO)', 'Location', 'northeast')
else
    plot3(pos(:,1), pos(:,2), pos(:,3), '-.', ...
        posEst(:,1), posEst(:,2), posEst(:,3), ...
        'LineWidth', 3)
    legend('Ground Truth', 'IMU Pose Estimate')
end
view(-90, 90)
title('Vehicle Position')
xlabel('X (m)')
ylabel('Y (m)')
grid on

El gráfico muestra que la estimación de la odometría visual es relativamente precisa al estimar la forma de la trayectoria. La fusión de las mediciones de IMU y de odometría visual elimina la incertidumbre del factor de escala de las mediciones de odometría visual y la deriva de las mediciones de IMU.

Funciones de apoyo

helperVisualOdometryModel

Calcule la medición de odometría visual a partir de la estructura de parámetros y la entrada ground-truth. Para modelar la incertidumbre en la escala entre fotogramas posteriores de la cámara monocular, se aplica un factor de escala constante combinado con una deriva aleatoria a la posición ground-truth.

function [posVO, orientVO, paramsVO] ...
    = helperVisualOdometryModel(pos, orient, paramsVO)

% Extract model parameters.
scaleVO = paramsVO.scale;
sigmaN = paramsVO.sigmaN;
tau = paramsVO.tau;
sigmaB = paramsVO.sigmaB;
sigmaA = sqrt((2/tau) + 1/(tau*tau))*sigmaB;
b = paramsVO.driftBias;

% Calculate drift.
b = (1 - 1/tau).*b + randn(1,3)*sigmaA;
drift = randn(1,3)*sigmaN + b;
paramsVO.driftBias = b;

% Calculate visual odometry measurements.
posVO = scaleVO*pos + drift;
orientVO = orient;
end

helperInitialize

Establezca el estado inicial y los valores de covarianza para el filtro de fusión.

function helperInitialize(filt, traj)

% Retrieve the initial position, orientation, and velocity from the
% trajectory object and reset the internal states.
[pos, orient, vel] = traj();
reset(traj);

% Set the initial state values.
filt.State(1:4) = compact(orient(1)).';
filt.State(5:7) = pos(1,:).';
filt.State(8:10) = vel(1,:).';

% Set the gyroscope bias and visual odometry scale factor covariance to
% large values corresponding to low confidence.
filt.StateCovariance(10:12,10:12) = 1e6;
filt.StateCovariance(end) = 2e2;
end

helperPreallocateData

Preasigne datos para registrar los resultados de la simulación.

function [pos, orient, vel, acc, angvel, ...
    posVO, orientVO, ...
    posEst, orientEst, velEst] ...
    = helperPreallocateData(numIMUSamples, numCameraSamples)

% Specify ground truth.
pos = zeros(numIMUSamples, 3);
orient = quaternion.zeros(numIMUSamples, 1);
vel = zeros(numIMUSamples, 3);
acc = zeros(numIMUSamples, 3);
angvel = zeros(numIMUSamples, 3);

% Visual odometry output.
posVO = zeros(numCameraSamples, 3);
orientVO = quaternion.zeros(numCameraSamples, 1);

% Filter output.
posEst = zeros(numIMUSamples, 3);
orientEst = quaternion.zeros(numIMUSamples, 1);
velEst = zeros(numIMUSamples, 3);
end

helperUpdatePose

Actualiza la pose del vehículo.

function helperUpdatePose(veh, pos, vel, orient)

veh.Position = pos;
veh.Velocity = vel;
rpy = eulerd(orient, 'ZYX', 'frame');
veh.Yaw = rpy(1);
veh.Pitch = rpy(2);
veh.Roll = rpy(3);
end

Referencias

Solá, J. "Cinemática de cuaterniones para el filtro de Kalman de estado de error". Impresiones electrónicas de ArXiv, arXiv:1711.02508v1 [cs.RO] 3 de noviembre de 2017.
R. Jiang, R., R. Klette y S. Wang. "Modelado de deriva ilimitada de largo alcance en odometría visual". 2010 Cuarto Simposio de la Cuenca del Pacífico sobre tecnología de imagen y vídeo. Noviembre de 2010, págs. 121-126.