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

SLAM basado en EKF con puntos de referencia

Este ejemplo muestra cómo utilizar el objeto ekfSLAM para una implementación confiable de localización y mapeo simultáneos (SLAM) de puntos de referencia utilizando el algoritmo de filtro de Kalman extendido (EKF) y el algoritmo de máxima verosimilitud para la asociación de datos. En este ejemplo, usted crea un mapa de puntos de referencia del entorno inmediato de un vehículo y simultáneamente rastrea la trayectoria del vehículo. Genere una trayectoria moviendo el vehículo usando los ruidosos comandos de control y forme el mapa usando los puntos de referencia que encuentre a lo largo de la ruta. Corrija la trayectoria del vehículo y las estimaciones de los puntos de referencia observando los puntos de referencia nuevamente.

Cargar conjunto de datos

Cargue una versión modificada del conjunto de datos de Victoria Park que contenga las entradas del controlador, las mediciones, la latitud y longitud del GPS y la navegación a estima generada utilizando las entradas del controlador y el modelo de movimiento.

load("victoriaParkDataset.mat","controllerInput", ...
     "measurements","gpsLatLong","deadReckoning");

Configurar parámetros

Especifique el estado inicial del vehículo y la covarianza del estado.

initialState = [gpsLatLong(1,2) gpsLatLong(1,1) deg2rad(37)]';
initialCovar = eps*eye(3);

Especifique la covarianza del ruido del proceso en los comandos de velocidad y dirección.

sigmaVelocity = 2;      % [m/s]
sigmaSteer= deg2rad(6); % [rad]
processNoise = [sigmaVelocity^2 0; 0 sigmaSteer^2];

Especifique la covarianza de medición en alcance y demora.

sigmaRange = 1;            % [m]
sigmaBearing = deg2rad(3); % [rad]
measCovar = [sigmaRange^2 sigmaBearing^2];

Especifique el rango máximo en el que verificar la asociación de puntos de referencia.

maxSensorRange = 30; % [m]

Especifique el tamaño del paso de tiempo en el que se mueve el vehículo.

timeStep = 0.025; % [sec]

Especifique los umbrales para la función de asociación de datos, nav.algs.associateMaxLikelihood. El umbral de rechazo del punto de referencia es el chi-cuadrado ($${\chi }^2$$) valor de la tabla de distribución para una asociación correcta del 95%.

landmarkRejectionThres = 5.991;  % maximum distance for association
landmarkAugmentationThres = 200; % minimum distance for creation of new landmark
validationGate = [landmarkRejectionThres landmarkAugmentationThres];

Establezca un indicador que determine si se trazará el mapa durante la ejecución.

plotOnTheRun = false;

Obtenga el número de muestras del conjunto de datos.

numSamples = size(controllerInput,1);

Inicializar figura o barra de progreso

Si la opción de trazado en vivo está habilitada, cree una nueva figura y configure controladores para trazar varios componentes. De lo contrario, cree una barra de espera que se actualice con la proporción del conjunto de datos que se ha ejecutado. Configure la barra de espera para que al hacer clic en Cancelar se detenga un cálculo en curso. Cerrar la barra de espera detiene la ejecución.

if plotOnTheRun
    [robotHandle,covarianceHandle,sensorHandle, ...
     observationHandle,landmarkHandle, ...
     deadRecHandle,estTrajHandle] = ...
        exampleHelperInitializeVisualizationEKFSLAM(initialState,gpsLatLong);
else
    waitBarHandle = waitbar(0,'Example initializing...', ...
        'Name','ekfSLAM example', ...
        'CreateCancelBtn','setappdata(gcbf,''canceling'',1)');
    setappdata(waitBarHandle,'canceling',0);
end

Configurar ekfSLAM

Configure el objeto ekfSLAM utilizando el estado inicial del vehículo, la covarianza del estado inicial, la covarianza del ruido del proceso y el modelo de movimiento del vehículo.

ekfSlamObj = ekfSLAM('State',initialState, ...
    'StateCovariance',initialCovar, ...
    'StateTransitionFcn',@exampleHelperVictoriaParkStateTransition);
ekfSlamObj.ProcessNoise = processNoise;
ekfSlamObj.MaxAssociationRange = maxSensorRange;

Bucle principal

El bucle principal consta de estas operaciones primarias:

• Predicción: predice el siguiente estado según el comando de control y el estado actual.

• Extracción de puntos de referencia: obtenga los puntos de referencia del entorno.

• Corrección: actualice el estado y la covarianza del estado utilizando los puntos de referencia observados.

Predicción

En este ejemplo, el vehículo se mueve con respecto a su estado anterior según la entrada del control mientras los puntos de referencia permanecen estacionarios. De este modo, sólo se propaga el estado del vehículo. Utilice el modelo de movimiento del vehículo para propagar el estado del vehículo al siguiente paso utilizando el conjunto de datos.

El método predict llama a la función especificada en la propiedad StateTransitionFcn de ekfSlamObj para predecir el estado del vehículo. El método predict pasa la entrada de control, otras entradas necesarias y la pose actual del vehículo al StateTransitionFcn.

for count = 1:numSamples
    predict(ekfSlamObj,controllerInput(count,:),timeStep);

Extracción de punto de referencia

Este ejemplo utiliza una lista de puntos de referencia observados del conjunto de datos, por lo que no es necesario extraer puntos de referencia del entorno mediante un sensor. Los puntos de referencia se encuentran en una zona semicircular situada delante del vehículo. El radio de la región semicircular está definido por el alcance máximo del sensor.

    observedLandmarks = measurements{count};

Corrección

El objeto ekfSLAM corrige el estado en función de una medida determinada y devuelve una lista de puntos de referencia coincidentes y una lista de puntos de referencia nuevos. El método correct utiliza la función especificada en la propiedad DataAssociationFcn de ekfSlamObj para asociar los puntos de referencia o mediciones observados a puntos de referencia conocidos. La función de asociación de datos devuelve una lista de asociaciones y una lista de nuevos puntos de referencia. El método correct utiliza las asociaciones para actualizar la creencia del vehículo y el mapa utilizando la correlación entre los puntos de referencia observados y los puntos de referencia conocidos. Además, el método correct aumenta la posición y la covarianza de cualquier punto de referencia nuevo en los vectores State y StateCovariance , respectivamente.

Si la opción de trazado en vivo está habilitada, actualice la figura con los escaneos en el momento actual.

  if ~isempty(observedLandmarks)
        correct(ekfSlamObj,observedLandmarks,measCovar,validationGate);
        % Update the corrected position of vehicle in the figure
        if plotOnTheRun
            exampleHelperUpdateScans(ekfSlamObj.State, ...
                              ekfSlamObj.MaxAssociationRange, ...
                              observedLandmarks, ...
                              sensorHandle,observationHandle);
        end
  end

Utilice drawnow para actualizar la cifra con la posición actual del vehículo y todos los puntos de referencia conocidos y su covarianza.

Si el gráfico en vivo no está habilitado y la barra de espera está activa, actualice la fracción del conjunto de datos ejecutado.

    if plotOnTheRun
        addpoints(deadRecHandle,deadReckoning(count,1),deadReckoning(count,2));
        addpoints(estTrajHandle,ekfSlamObj.State(1),ekfSlamObj.State(2));
        exampleHelperUpdateRobotAndLandmarks(ekfSlamObj.State,ekfSlamObj.StateCovariance, ...
            robotHandle,covarianceHandle,landmarkHandle);
        drawnow limitrate
    else
        % update the wait bar
        waitbar(count/numSamples,waitBarHandle, ...
            sprintf("%2.2f%% executed",count/numSamples*100));
        % Check for clicked Cancel button
        if getappdata(waitBarHandle,"canceling")
            break
        end
    end
end

Utilice la función delete para cerrar la barra de espera una vez que se hayan completado todos los cálculos.

if ~plotOnTheRun
    delete(waitBarHandle);
end

Visualizar mapa

Cree un gráfico geográfico con ground-truth a partir de los datos del GPS. Trazar la trayectoria del vehículo, junto con todos los puntos de referencia detectados y su covarianza asociada, en el mismo gráfico geográfico.

% get the corrected and predicted poses
[corrPoses,predPoses] = poseHistory(ekfSlamObj);

% show the map
exampleHelperShowMap(ekfSlamObj.State,ekfSlamObj.StateCovariance, ...
                     gpsLatLong,corrPoses,predPoses);