Esta página aún no se ha traducido para esta versión. Puede ver la versión más reciente de esta página en inglés.

Asignación con poses conocidas

En este ejemplo se muestra cómo crear un mapa de un entorno mediante lecturas de sensores de rango y poses de robot para un robot de accionamiento diferencial. Puede crear un mapa a partir de lecturas de sensores de rango que se simulan con el objeto.rangeSensor El modelo de movimiento simula la conducción del robot alrededor de la habitación en función de los comandos de velocidad.differentialDriveKinematics El da lecturas de rango basadas en la pose del robot, ya que sigue el camino.rangeSensor

Mapa de referencia y figuras

Cargue un conjunto de cuadrículas de ocupación binaria de ejemplo desde , incluidas , que en este ejemplo se utiliza.exampleMapssimpleMap

load exampleMaps.mat

Cree el mapa de ocupación binaria de referencia utilizando una resolución de 1.simpleMap 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 modelo de movimiento y controlador

Cree un modelo de movimiento cinemático de accionamiento diferencial. El modelo de movimiento representa el movimiento del robot de accionamiento diferencial simulado. Este modelo toma velocidades de ruedas izquierda y derecha o velocidades lineales y angulares para el rumbo del robot. Para este ejemplo, utilice la velocidad del vehículo y la velocidad de rumbo para el archivo .VehicleInputs

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

Crea un controlador de persecución puro. Este controlador genera las entradas de velocidad para que el robot simulado siga la ruta deseada. Establezca la velocidad lineal y la velocidad angular máxima deseadas, especificadas en metros por segundo y radianes por segundo respectivamente.

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

Configurar sensor de rango

Cree un sensor con un alcance máximo de 10 metros. Este sensor simula las lecturas de rango basadas en una pose y un mapa determinados. El mapa de referencia se utiliza con este sensor de rango para simular la recopilación de lecturas del sensor en un entorno desconocido.

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

Crear la ruta planificada

Cree una ruta para conducir a través del mapa para recopilar lecturas de sensores de rango.

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

Trazar 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 de persecución puro.

controller.Waypoints = path;

Seguir la ruta y el entorno de mapas

Establezca la pose inicial y la ubicación final del objetivo en función de la ruta. Cree variables globales para almacenar la pose actual y un índice para realizar el seguimiento de 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 auxiliar proporcionada .exampleHelperDiffDriveControl La función auxiliar contiene el bucle principal para la navegación de la ruta de acceso, obtener lecturas de rango y asignar el entorno.

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

  • Escanee el mapa de referencia utilizando el sensor de rango y la pose actual. Esto simula las lecturas de rango normales para conducir en un entorno desconocido.

  • Actualice el mapa con las lecturas de rango.

  • Obtener comandos de control de controlador de persecución puro para conducir al siguiente waypoint.

  • Calcular la derivada del movimiento del robot basado en comandos de control.

  • Incremente la pose del robot en función de la derivada.

Deberías ver al robot conduciendo alrededor del mapa vacío y llenando paredes a medida que el sensor de rango las detecta.

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

Goal position reached 

Función de control de accionamiento diferencial

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

  • Escanee el mapa de referencia utilizando el sensor de rango y la pose actual. Esto simula las lecturas de rango normales para conducir en un entorno desconocido.

  • Actualice el mapa con las lecturas de rango.

  • Obtener comandos de control de controlador de persecución puro para conducir al siguiente waypoint.

  • Calcular la derivada del movimiento del robot basado en comandos de control.

  • Incremente 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