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