Crear mapas con poses conocidas
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