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

Cree mapas de ocupación egocéntricos utilizando sensores de distancia

Los mapas de ocupación ofrecen una forma simple pero sólida de representar un entorno para aplicaciones robóticas al mapear el espacio mundial continuo en una estructura de datos discreta. Las celdas de cuadrícula individuales pueden contener información binaria o probabilística, donde 0 indica espacio libre y 1 indica espacio ocupado. Puede acumular esta información a lo largo del tiempo utilizando mediciones de sensores y almacenarlas de manera eficiente en el mapa. Esta información también es útil para flujos de trabajo más avanzados, como la detección de colisiones y la planificación de rutas.

Este ejemplo muestra cómo crear un mapa de ocupación egocéntrico, que realiza un seguimiento del entorno inmediato del robot y que puede moverse eficientemente por el entorno. Se genera una trayectoria planificando una ruta en el entorno y dicta el movimiento del robot. A medida que el robot se mueve, el mapa se actualiza utilizando información del sensor de un lidar simulado y un mapa ground-truth.

Cargar un mapa de ocupación Ground-Truth prediseñado

Cree un mapa no egocéntrico a partir de un archivo de datos generado previamente, que se considera ground-truth para el lidar simulado. Cargue el mapa, mapData, que contiene el campo Data como una matriz probabilística y conviértalo a valores binarios.

Cree un objeto binaryOccupancyMap con la matriz binaria y especifique la resolución del mapa.

% Load saved map information
load mapData_rayTracingTrajectory
binaryMatrix = mapData.Data > 0.5;
worldMap = binaryOccupancyMap(binaryMatrix,mapData.Resolution);

Establece la ubicación de la esquina inferior izquierda del mapa como origen del mundo.

worldMap.LocalOriginInWorld = mapData.GridLocationInWorld;

Trazar ground-truth. Este ejemplo configura una gráfica secundaria para mostrar dos mapas uno al lado del otro.

set(gcf,'Visible','on')
worldAx = subplot(1,2,1);
worldHandle = show(worldMap,'Parent',worldAx);
hold all

Crear un Lidar simulado

Cree un objeto rangeSensor , que se puede utilizar para recopilar lecturas LIDAR de la simulación. Puede modificar varias propiedades en el rangeSensor para representar con mayor precisión un modelo particular de lidar, o agregar ruido del sensor para probar la solidez de su solución. Para este ejemplo, configure el rango [min max] y el parámetro de ruido. Después de crear el objeto, recupere y trace una lectura del sensor proporcionando una pose [x y theta] relativa al marco mundial. Los ayudantes de ejemplo trazan el robot y las lecturas del lidar superan el worldMap.

% Create rangeSensor
lidar = rangeSensor;
lidar.Range = [0 5];
lidar.RangeNoise = 0;
pos = [0 0 0];

% Show lidar readings in world map
[ranges, angles] = lidar(pos, worldMap);
hSensorData = exampleHelperPlotLidar(worldAx, pos, ranges, angles);

% Show robot in world map
hRobot = exampleHelperPlotRobot(worldAx, pos);

Inicializar un mapa egocéntrico

Cree un objeto occupancyMap para representar el mapa egocéntrico. Establezca el origen local en la ubicación central de la cuadrícula. Cambie el origen de la cuadrícula a la mitad del ancho de los límites.

% By default, GridOriginInLocal = [0 0]
egoMap = occupancyMap(10,10,worldMap.Resolution);

% Offset the GridOriginInLocal such that the "local frame" is located in the
% center of the "map-window"
egoMap.GridOriginInLocal = -[diff(egoMap.XWorldLimits) diff(egoMap.YWorldLimits)]/2;

Traza el mapa egocéntrico al lado del mapa mundial en la gráfica secundaria.

% Update local plot
localAx = subplot(1,2,2);
show(egoMap,'Parent',localAx);
hold all
localMapFig = plot(localAx,egoMap.LocalOriginInWorld+[0 1], egoMap.LocalOriginInWorld+[0 0],'r-','LineWidth',3);

Planificar ruta entre puntos

Ahora podemos usar nuestro mapa de ground-truth para planificar una ruta entre dos puntos libres. Cree una copia del mapa mundial e infle según el tamaño del robot y el espacio libre deseado. Este ejemplo utiliza un robot similar a un automóvil, que tiene restricciones no holonómicas, especificadas con un objeto stateSpaceDubins . El planificador de rutas utiliza este espacio de estados para muestrear aleatoriamente estados factibles para el robot. Por último, cree un objeto validatorOccupancyMap , que utiliza el mapa para validar los estados generados y los movimientos que los conectan verificando la ocupación de las celdas correspondientes.

% Copy the world map and inflate it.
binaryMap = binaryOccupancyMap(worldMap);
inflate(binaryMap, 0.1);

% Create a state space object.
stateSpace = stateSpaceDubins;

% Reduce the turning radius to better fit the size of map and obstacle
% density.
stateSpace.MinTurningRadius = 0.5;

% Create a state validator object.
validator = validatorOccupancyMap(stateSpace);
validator.Map = binaryMap;
validator.ValidationDistance = 0.1;

Utilice el algoritmo de planificación RRT* como un objeto plannerRRTStar y especifique el espacio de estados y el validador de estados como entradas. Especifique las ubicaciones de inicio y finalización del planificador y genere una ruta.

% Create our planner using the previously created StateSpace and
% StateValidator objects.
planner = plannerRRTStar(stateSpace, validator);
planner.MaxConnectionDistance = 2;
planner.MaxIterations = 20000;

% Set a seed for the randomly generated path for reproducible results.
rng(1, 'twister')

% Set the start and end points.
startPt = [-6   -5      0];
goalPt  = [ 8    7   pi/2];

% Plan a path between start and goal points.
path = plan(planner, startPt, goalPt);
interpolate(path, size(path.States,1)*10);
plot(worldAx, path.States(:,1),path.States(:,2),'b-');

Generar una trayectoria a lo largo de la ruta

El planificador generó un conjunto de estados, pero para ejecutar una trayectoria se necesitan tiempos para los estados. El objetivo de este ejemplo es mover el robot a lo largo de la trayectoria con una velocidad lineal constante de 0.5 m/s. Para obtener marcas de tiempo para cada punto, calcule las distancias entre puntos, súmelas acumulativamente, luego divida esto por la velocidad lineal para obtener un arreglo de marcas de tiempo que aumenta monótonamente, tStamps.

% Get distance between each waypoint
pt2ptDist = distance(stateSpace,path.States(1:end-1,:),path.States(2:end,:))

pt2ptDist = 129×1

    0.2000
    0.2000
    0.2000
    0.2000
    0.2000
    0.2000
    0.2000
    0.2000
    0.2000
    0.2000
      ⋮

linVel = 0.5; % m/s
tStamps = cumsum(pt2ptDist)/linVel;

Genere una trayectoria simulada final con waypointTrajectory, especificando los estados de la ruta, las marcas de tiempo correspondientes y una frecuencia de muestreo deseada de 10 Hz.

traj = waypointTrajectory(path.States, [0; tStamps], 'SampleRate', 10);

Simule lecturas de sensores y cree un mapa

Por último, mueva el robot a lo largo de la trayectoria mientras actualiza el mapa con las lecturas Lidar simuladas.

Para inicializar la simulación, restablezca la trayectoria, establezca el origen local en el primer punto xy de la trayectoria y borre el mapa.

reset(traj);
robotCurrentPose = path.States(1,:);
move(egoMap, robotCurrentPose(1:2));
setOccupancy(egoMap,repmat(egoMap.DefaultValue,egoMap.GridSize));

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 [xy theta].

• Recuperar datos del sensor del lidar.

• Actualice el mapa local con datos del sensor usando insertRay.

• Actualizar la visualización.

Tenga en cuenta que el robot recorre el entorno, simula lecturas de sensores y construye un mapa de ocupación a medida que avanza.

while ~isDone(traj)
    % Increment robot along trajectory
    [pts, quat] = step(traj);
    
    % Get orientation angle from quaternion
    rotMatrix = rotmat(quat,'point');
    orientZ = rotm2eul(rotMatrix);
    
    % Move the robot to the new location
    robotCurrentPose = [pts(1:2) orientZ(1)];
    move(egoMap, robotCurrentPose(1:2),'MoveType','Absolute');
    
    % Retrieve sensor information from the lidar and insert it into the egoMap
    [ranges, angles] = lidar(robotCurrentPose, worldMap);
    insertRay(egoMap, robotCurrentPose,ranges,angles,lidar.Range(2));

    % Update egoMap-centric plot
    show(egoMap, 'Parent', localAx, 'FastUpdate', 1);
    
    % Update orientation vector
    set(localMapFig, 'XData', robotCurrentPose(1)+[0 cos(robotCurrentPose(3))], 'YData', robotCurrentPose(2)+[0 sin(robotCurrentPose(3))]);

    % Update world plot
    exampleHelperUpdateRobotAndLidar(hRobot, hSensorData, robotCurrentPose, ranges, angles);

    % Call drawnow to push updates to the figure
    drawnow limitrate
end