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.

Fusione varios sensores Lidar utilizando capas de mapas

Abrir script en vivo

Los mapas de ocupación ofrecen una forma simple pero sólida de representar un entorno para aplicaciones robóticas al mapear el espacio-mundo continuo en una estructura de datos discreta. Las celdas de cuadrícula individuales pueden contener información binaria o probabilística sobre información de obstáculos. Sin embargo, una plataforma autónoma puede utilizar una variedad de sensores que pueden necesitar combinarse para estimar tanto el estado actual de la plataforma como el estado del entorno circundante.

Este ejemplo se centra en integrar una variedad de sensores para estimar el estado del medio ambiente y los valores de ocupación de la tienda en diferentes capas del mapa. El ejemplo muestra cómo el objeto multiLayerMap se puede utilizar para visualizar, depurar y fusionar datos recopilados de tres sensores lidar montados en un vehículo autónomo. Las lecturas del sensor en este ejemplo se simulan utilizando un conjunto de objetos lidarPointCloudGenerator (Automated Driving Toolbox) que capturan lecturas del objeto drivingScenario (Automated Driving Toolbox) adjunto.

Cada lidar actualiza su propio objeto validatorOccupancyMap3D que nos permite visualizar el mapa local creado por cada sensor de forma aislada. Estos mapas locales se pueden utilizar para identificar rápidamente fuentes de ruido o errores de montaje y pueden ayudar a elegir una técnica de fusión adecuada. El multiLayerMap contiene un cuarto objeto mapLayer , que utiliza una función de callback personalizada para fusionar los datos contenidos en cada capa de ocupación. Por último, el mapa fusionado se utiliza para actualizar la subregión correspondiente de un mapa mundial a medida que el vehículo autónomo avanza por la ruta planificada previamente.

Escenario de conducción de carga

Primero, cree un objeto drivingScenario y complete la escena con varios edificios usando una función auxiliar de ejemplo. La función también visualiza la escena.

scene = drivingScenario;
groundTruthVehicle = vehicle(scene,'PlotColor',[0 0.4470 0.7410]);

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

Genere una trayectoria que siga la carretera principal en la escena usando un objeto waypointTrajectory .

sampleRate = 100;
speed = 10;
t = [0 20 25 44 46 50 54 56 59 63 90].';
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;
             -15 260 0];
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);

Cree sensores Lidar simulados

Para recopilar lecturas LIDAR del escenario de conducción, cree tres objetos lidarPointcloudGenerator utilizando una función auxiliar de ejemplo. Este vehículo ha sido configurado para tener dos lidars de campo de visión estrecho (FOV) orientados hacia adelante y un único Lidar de campo de visión estrecho orientado hacia atrás. La región superpuesta de ambos sensores orientados hacia adelante debería ayudar a registrar y confirmar rápidamente el espacio libre delante del vehículo, mientras que el rango de los sensores orientados hacia atrás ayuda a mapear la región atravesada.

lidarSensors = exampleHelperCreateVehicleSensors(scene, groundTruthVehicle);
disp(lidarSensors)
    {1×1 lidarPointCloudGenerator}    {1×1 lidarPointCloudGenerator}    {1×1 lidarPointCloudGenerator}

Inicializar mapa egocéntrico

Cree un objeto multiLayerMap compuesto por tres objetos occupancyMap y un objeto mapLayer genérico. Cada occupancyMap local es actualizado por el sensor lidar correspondiente. Para combinar datos de todos los mapas en el objeto mapLayer , establezca el argumento nombre-valor GetTransformFcn en la función exampleHelperFuseOnGet almacenada como un identificador. fGet. La función exampleHelperFuseOnGet fusionó los tres valores de datos de mapas llamando a la función getMapData en cada uno y usando una suma de probabilidades logarítmicas de los valores.

% Define map and parameters.
res = 2;
width = 100*2;
height = 100*2;

% Define equal weights for all sensor readings.
weights = [1 1 1];

% Create mapLayers for each sensor.
fLeftLayer  = occupancyMap(width,height,res,'LayerName','FrontLeft');
fRightLayer = occupancyMap(width,height,res,'LayerName','FrontRight');
rearLayer   = occupancyMap(width,height,res,'LayerName','Rear');

% Create a get callback used to fuse data in the three layers.
fGet = @(obj,values,varargin)...
    exampleHelperFuseOnGet(fLeftLayer,fRightLayer,rearLayer,...
                        weights,obj,values,varargin{:});

% Create a generic mapLayer object whose getMapData function fuses data from all
% three layers.
fusedLayer = mapLayer(width,height,'Resolution',res,'LayerName','FuseLayer',...
                    'GetTransformFcn',fGet,'DefaultValue',0.5);

% Combine layers into a multiLayerMap.
egoMap = multiLayerMap({fLeftLayer, fRightLayer, rearLayer, fusedLayer});

% Set map grid origin so that the robot is located at the center.
egoMap.GridOriginInLocal = -[diff(egoMap.XLocalLimits) diff(egoMap.YLocalLimits)]/2;

Crear mapa de reconstrucción

Crea un mapa mundial vacío. Este mapa se actualiza periódicamente utilizando datos de la capa de fusión. Utilice este mapa para indicar qué tan bien está funcionando el método de fusión lidar.

% Create an empty reconstruction layer covering the same area as world map.
reconLayer = occupancyMap(400,400,res,... % width,height,resolution
    'LayerName','FuseLayer','LocalOriginInWorld',[-25 -50]);

Configuración de visualización

Traza las capas egocéntricas junto al mapa reconstruido. Utilice la función exampleHelperShowEgoMap para mostrar cada mapa local.

% Setup the display window.
axList = exampleHelperSetupDisplay(groundTruthVehicle,lidarSensors);

% Display the reconstructionLayer and submap region.
show(reconLayer,'Parent', axList{1});
hG = findobj(axList{1},'Type','hggroup');
egoOrientation = hG.Children;
egoCenter = hgtransform('Parent',hG);
egoOrientation.Parent = egoCenter;
gridLoc = egoMap.GridLocationInWorld;
xLimits = egoMap.XLocalLimits;
yLimits = egoMap.YLocalLimits;
rectangle('Parent',egoCenter,...
    'Position',[gridLoc diff(xLimits) diff(yLimits)],...
    'EdgeColor','r');

% Display the local maps built by each sensor alongside the reconstruction map.
exampleHelperShowEgoMap(axList,egoMap,[0 0 0],{'FrontLeft Lidar','FrontRight Lidar','Rear Lidar','Fused'});

Simule lecturas de sensores y cree mapas

Mueva el robot a lo largo de la trayectoria mientras actualiza el mapa con las lecturas Lidar simuladas.

Para ejecutar el escenario de conducción, llame a la función auxiliar exampleHelperResetSimulation . Esto restablece la simulación y la trayectoria, borra el mapa y mueve los mapas egocéntricos de regreso al primer punto de la trayectoria.

exampleHelperResetSimulation(scene,traj,lidarSensors,egoMap,reconLayer)

Llame a la función exampleHelperRunSimulation para ejecutar la simulación.

Las operaciones principales del bucle de simulación son:

  • Obtenga la siguiente pose en la trayectoria de traj y extraiga la orientación del eje z (theta) del cuaternión.

  • Mueve el egoMap a la nueva pose [x y theta] .

  • Recupere los datos del sensor del lidarPointCloudGenerators.

  • Actualice los mapas locales con datos de sensores usando insertRay.

  • Actualice el mapa global usando el resultado fusionado mapLayer .

  • Actualizar la visualización.

exampleHelperRunSimulation(scene,traj,groundTruthVehicle,egoMap,lidarSensors,reconLayer,axList)

Los resultados mostrados indican que el sensor delantero derecho está introduciendo grandes cantidades de ruido en el mapa fusionado. Observe que la pared de la derecha tiene más variabilidad a lo largo de la trayectoria. No desea descartar por completo las lecturas de este sensor porque el sensor todavía detecta espacio libre en el frente. En su lugar, reduzca el peso de las lecturas de esos sensores durante la fusión y recree el mapa multicapa completo. Luego, reinicie y vuelva a ejecutar la simulación.

% Construct a new multiLayerMap with a different set of fusion weights
updatedWeights = [1 0.25 1];
egoMap = exampleHelperConstructMultiLayerEgoMap(res,width,height,updatedWeights);

% Rerun the simulation
exampleHelperResetSimulation(scene,traj,lidarSensors,egoMap,reconLayer)
exampleHelperRunSimulation(scene,traj,groundTruthVehicle,egoMap,lidarSensors,reconLayer,axList)

Después de simular nuevamente, observe algunas cosas sobre el mapa:

  • Las regiones cubiertas sólo por el sensor ruidoso aún pueden detectar el espacio libre con poco ruido.

  • Si bien el ruido sigue presente, las lecturas de los otros sensores superan las del sensor ruidoso. El mapa muestra distintos límites de obstáculos (cuadrados negros) en regiones de superposición de sensores.

  • El ruido más allá de los distintos límites permanece porque el ruidoso lidar es el único sensor que informa lecturas en esas áreas, pero no se conecta a otro espacio libre.

EqualWeight.PNG ReducedSensor2Weight.PNG

Siguientes pasos

Este ejemplo muestra un método simple de cómo se pueden fusionar lecturas. Puede personalizar aún más esta fusión con las siguientes sugerencias:

  • Para ajustar los pesos según la confianza del sensor antes de la fusión capa-capa, especifique un modelo de sensor inverso personalizado cuando utilice la función de objeto insertRay en la función examplerHelperUpdateEgoMap .

  • Para asignar valores de ocupación basados ​​en una distribución de confianza más compleja como un modelo inverso gaussiano, use la función de objeto raycast para recuperar las celdas trazadas por cada rayo eminente. Una vez que se ha recuperado un conjunto de celdas, los valores se pueden asignar explícitamente según métodos más complejos.

  • Para reducir la confianza en el envejecimiento de las celdas, utilice capas de mapas adicionales que realicen un seguimiento de las marcas de tiempo de cada celda. Estas marcas de tiempo se pueden utilizar para dar mayor importancia a las celdas actualizadas recientemente e ignorar lentamente las lecturas más antiguas.