La traducción de esta página aún no se ha actualizado a la versión más reciente. Haga clic aquí para ver la última versión en inglés.

Crear mapas con poses conocidas

Abrir script en vivo

En este ejemplo se muestra cómo crear un mapa de un entorno mediante lecturas del sensor de distancia y poses del robot para un robot de tracción diferencial. Puede crear un mapa a partir de las lecturas del sensor de distancia que están simuladas mediante el objeto rangeSensor. El modelo de movimiento differentialDriveKinematics simula cómo el robot se traslada por el recinto en función de los comandos de velocidad. El objeto rangeSensor proporciona las lecturas de distancia en función de la pose del robot a medida que avanza por la ruta.

Mapas de referencia y figuras

Cargue un conjunto de cuadrículas de ocupación binarias desde exampleMaps, incluida simpleMap, que es la que se utiliza en este ejemplo.

load exampleMaps.mat

Cree el mapa de ocupación binario de referencia mediante simpleMap con una resolución de 1. Muestre la figura y guarde el identificador de la figura.

refMap = binaryOccupancyMap(simpleMap,1);
refFigure = figure('Name','SimpleMap');
show(refMap);

Cree un mapa vacío de las mismas dimensiones que el mapa seleccionado con una resolución de 10. Muestre la figura y guarde el identificador de la figura. Bloquee los ejes en el tamaño del mapa.

[mapdimx,mapdimy] = size(simpleMap);
map = binaryOccupancyMap(mapdimy,mapdimx,10);
mapFigure = figure('Name','Unknown Map');
show(map);

Inicializar un modelo de movimiento y un controlador

Cree un modelo de movimiento cinemático de tracción diferencial. El modelo de movimiento representa el movimiento del robot de tracción diferencial simulado. Este modelo toma la velocidad de las ruedas derechas e izquierdas o las velocidades lineales y angulares para la dirección del robot. Para este ejemplo, utilice la velocidad del vehículo y la tasa de dirección para las VehicleInputs.

diffDrive = differentialDriveKinematics("VehicleInputs","VehicleSpeedHeadingRate");

Cree un controlador de Pure Pursuit. Este controlador genera las entradas de velocidad para que el robot simulado siga una ruta deseada. Establezca la velocidad lineal deseada y una velocidad angular máxima, especificadas en metros por segundo y en radianes por segundo, respectivamente.

controller = controllerPurePursuit('DesiredLinearVelocity',2,'MaxAngularVelocity',3);

Configurar un sensor de distancia

Cree un sensor con una distancia máxima de 10 metros. Este sensor simula lecturas de distancia en función de una pose y de un mapa dados. El mapa de referencia se utiliza con este sensor de distancia para simular la recopilación de lecturas del sensor en un entorno desconocido.

sensor = rangeSensor;
sensor.Range = [0,10];

Crear la ruta planeada

Cree una ruta con para atravesar el mapa y recopilar lecturas del sensor de distancia.

path = [4 6; 6.5 12.5; 4 22; 12 14; 22 22; 16 12; 20 10; 14 6; 22 3];

Represente la ruta en la figura del mapa de referencia.

figure(refFigure);
hold on
plot(path(:,1),path(:,2), 'o-');
hold off

Establezca la ruta como los waypoints del controlador Pure Pursuit.

controller.Waypoints = path;

Seguir la ruta y el entorno del mapa

Establezca la pose inicial y la ubicación objetivo final en función de la ruta. Cree variables globales para almacenar la pose actual y un índice para seguir las iteraciones.

initPose = [path(1,1) path(1,2), pi/2];
goal = [path(end,1) path(end,2)]';
poses(:,1) = initPose';

Utilice la función helper exampleHelperDiffDriveCtrl proporcionada. La función helper contiene el bucle principal para desplazarse por la ruta, obtener lecturas de distancia y crear mapas del entorno.

La función exampleHelperDiffDriveControl tiene el siguiente flujo de trabajo:

Escanear el mapa de referencia con el sensor de distancia y la pose actual. Esto simula lecturas de distancia normales para avanzar en un entorno desconocido.
Actualizar el mapa con las lecturas de distancia.
Obtener comandos de control del controlador Pure Pursuit para avanzar al siguiente waypoint.
Calcular la derivada del movimiento del robot en función de los comandos de control.
Incrementar la pose del robot en función de la derivada.

Podrá observar al robot desplazándose por el mapa vacío y dibujando paredes a medida que el sensor de distancia las detecta.

exampleHelperDiffDriveCtrl(diffDrive,controller,initPose,goal,refMap,map,refFigure,mapFigure,sensor)

Goal position reached

Función de control de tracción diferencial

La función exampleHelperDiffDriveControl tiene el siguiente flujo de trabajo:

Escanear el mapa de referencia con el sensor de distancia y la pose actual. Esto simula lecturas de distancia normales para avanzar en un entorno desconocido.
Actualizar el mapa con las lecturas de distancia.
Obtener comandos de control del controlador Pure Pursuit para avanzar al siguiente waypoint.
Calcular la derivada del movimiento del robot en función de los comandos de control.
Incrementar la pose del robot en función de la derivada.

function exampleHelperDiffDriveControl(diffDrive,ppControl,initPose,goal,map1,map2,fig1,fig2,lidar)
sampleTime = 0.05;             % Sample time [s]
t = 0:sampleTime:100;         % Time array
poses = zeros(3,numel(t));    % Pose matrix
poses(:,1) = initPose';

% Set iteration rate
r = rateControl(1/sampleTime);

% Get the axes from the figures
ax1 = fig1.CurrentAxes;
ax2 = fig2.CurrentAxes;

    for idx = 1:numel(t)
        position = poses(:,idx)';
        currPose = position(1:2);
        
        % End if pathfollowing is vehicle has reached goal position within tolerance of 0.2m
        dist = norm(goal'-currPose);
        if (dist < .2)
            disp("Goal position reached")
            break;
        end
        
        % Update map by taking sensor measurements
        figure(2)
        [ranges, angles] = lidar(position, map1);
        scan = lidarScan(ranges,angles);
        validScan = removeInvalidData(scan,'RangeLimits',[0,lidar.Range(2)]);
        insertRay(map2,position,validScan,lidar.Range(2));
        show(map2);
        
        % Run the Pure Pursuit controller and convert output to wheel speeds
        [vRef,wRef] = ppControl(poses(:,idx));
    
        % Perform forward discrete integration step
        vel = derivative(diffDrive, poses(:,idx), [vRef wRef]);
        poses(:,idx+1) = poses(:,idx) + vel*sampleTime; 
    
    
        % Update visualization
        plotTrvec = [poses(1:2, idx+1); 0];
        plotRot = axang2quat([0 0 1 poses(3, idx+1)]);
        
        % Delete image of the last robot to prevent displaying multiple robots
        if idx > 1
           items = get(ax1, 'Children');
           delete(items(1)); 
        end
    
        %plot robot onto known map
        plotTransforms(plotTrvec', plotRot, 'MeshFilePath', 'groundvehicle.stl', 'View', '2D', 'FrameSize', 1, 'Parent', ax1);
        %plot robot on new map
        plotTransforms(plotTrvec', plotRot, 'MeshFilePath', 'groundvehicle.stl', 'View', '2D', 'FrameSize', 1, 'Parent', ax2);
    
        % waiting to iterate at the proper rate
        waitfor(r);
    end
end