Esta página es para la versión anterior. La página correspondiente en inglés ha sido eliminada en la versión actual.

Mapeo con poses conocidas

Este ejemplo muestra cómo crear un mapa del entorno utilizando lecturas del sensor de rango Si la posición del robot se conoce en el momento de la lectura del sensor. Este ejemplo también muestra cómo utilizar las funciones de conversión (como) de Robotics System Toolbox™.quat2eul

Este ejemplo crea un mapa a partir de lecturas del sensor de rango y poses conocidas del robot. Con el propósito de este ejemplo, utilizará un simulador basado en MATLAB® para conducir el robot, observar las lecturas del sensor de rango y las posturas del robot. Puede reemplazar el simulador con un robot real o un robot simulado en el simulador de Gazebo, en cuyo caso necesitará algunos medios para obtener la verdadera posición del robot en el momento de la lectura del sensor.

Requisitos previos:Ruta siguiente para un robot de accionamiento diferencialAcceda al árbol de transformación TF en ROSIntercambiar datos con ROS editores y suscriptores

Inicializar el robot Simulator

Inicie el Master de ROS en MATLAB.

rosinit
Initializing ROS master on http://bat572504glnxa64:38733/. Initializing global node /matlab_global_node_41987 with NodeURI http://bat572504glnxa64:37285/ 

Inicialice el simulador de robot y asigne una pose inicial. El robot simulado es un robot de accionamiento diferencial de dos ruedas con un sensor de rango láser.

sim = ExampleHelperRobotSimulator('simpleMap');

setRobotPose(sim,[2 3 -pi/2]);

Habilite la interfaz de ROS para el simulador. El simulador crea editores y suscriptores para enviar y recibir datos a través de ROS.

enableROSInterface(sim,true);

Aumente la resolución del sensor láser en el simulador para facilitar la creación de mapas.

sim.LaserSensor.NumReadings = 50;

Setup interfaz ROS

Cree editores y suscriptores de ROS para comunicarse con el simulador. Cree para recibir datos de sensores láser del simulador.rossubscriber

scanSub = rossubscriber('scan');

Para construir el mapa, el robot manejará alrededor del mapa y recogerá los datos del sensor. Crear para enviar comandos de velocidad al robot.rospublisher

[velPub, velMsg] = rospublisher('/mobile_base/commands/velocity');

El simulador de MATLAB publica la posición del robot con respecto al origen del mapa como una transformación en el tema, que se utiliza como la fuente para la postura del robot de la verdad del suelo en este ejemplo./tf El simulador utiliza y como nombres de trama en esta transformación.maprobot_base

Crear un objeto de árbol de transformación ROS utilizando la función. Para construir un mapa, es esencial que la postura del robot y la lectura del sensor láser correspondan al mismo tiempo. El árbol de transformación le permite encontrar la pose del robot en el momento en que se observó la lectura del sensor láser.

tftree = rostf;

PAUSE un segundo para que el objeto de árbol de transformación finalice la inicialización.

pause(1)

La interfaz de ROS se comunica con el simulador de robot y el estado del robot simulado se muestra en la figura.

Cree un controlador de ruta

El robot tendrá que conducir alrededor de todo el mapa para recoger los datos del sensor láser y construir un mapa completo. Asigne un trazado con waypoints que cubran todo el mapa.

path = [2 3; 3.25 6.25; 2 11; 6 7; 11 11; 8 6; 10 5; 7 3; 11 1.5];

Visualiza el camino en el simulador de robot

plot(path(:,1),path(:,2),'k--d');

En función de la ruta definida anteriormente y de un modelo de movimiento de robot, necesita una ruta que sigue al controlador para conducir el robot a lo largo del camino. Cree la vía de acceso siguiente controlador utilizando el objeto.robotics.PurePursuit

controller = robotics.PurePursuit('Waypoints',path); controller.DesiredLinearVelocity = 0.4;

Fije la velocidad del regulador para ejecutarse en 10 Hz.

controlRate = robotics.Rate(10);

Defina un punto de objetivo y un radio de objetivo para detener el robot al final de la ruta.

goalRadius = 0.1; robotCurrentLocation = path(1,:); robotGoal = path(end,:); distanceToGoal = norm(robotCurrentLocation - robotGoal);

Definir un mapa vacío

Defina un mapa con alta resolución utilizando para capturar las lecturas del sensor.robotics.OccupancyGrid Esto crea un mapa con un tamaño de 14 m X 13 m y una resolución de 20 celdas por metro.

map = robotics.OccupancyGrid(14,13,20);

Visualice el mapa en la ventana de la figura.

figureHandle = figure('Name', 'Map'); axesHandle = axes('Parent', figureHandle); mapHandle = show(map, 'Parent', axesHandle); title(axesHandle, 'OccupancyGrid: Update 0');

El siguiente bucle while construirá el mapa del entorno mientras se conduce el robot. Se realizan los siguientes pasos:

  • En primer lugar, recibirá los datos de escaneo láser utilizando el suscriptor.scanSub Utilice la función con la marca de tiempo en el mensaje para obtener la transformación entre los fotogramas y en el momento de la lectura del sensor.scanmaprobot_base

  • Obtenga la posición y orientación del robot de la transformación. La orientación del robot es la rotación de guiñada alrededor del eje Z del robot. Puede obtener la rotación de Yaw convirtiendo el cuaternión en ángulos de Euler usando.quat2eul

  • Pre-procesar datos de escaneo láser. El simulador devuelve los rangos NaN para los rayos láser que no golpean ningún obstáculo dentro del rango máximo. Reemplace los rangos NaN por el valor de rango máximo.

  • Inserte la observación del escaneo láser utilizando el método en la rejilla de ocupación.map

  • Calcule los comandos de velocidad lineal y angular utilizando el objeto para conducir el robot.controller

  • Para visualizar el mapa después de cada 50 actualizaciones, quite la marca de comentario de la sección en el bucle while.(Optional)

updateCounter = 1; while( distanceToGoal > goalRadius )     % Receive a new laser sensor reading.     scanMsg = receive(scanSub);          % Get robot pose at the time of sensor reading.     pose = getTransform(tftree, 'map', 'robot_base', scanMsg.Header.Stamp, 'Timeout', 2);          % Convert robot pose to 1x3 vector [x y yaw].     position = [pose.Transform.Translation.X, pose.Transform.Translation.Y];     orientation =  quat2eul([pose.Transform.Rotation.W, pose.Transform.Rotation.X, ...         pose.Transform.Rotation.Y, pose.Transform.Rotation.Z], 'ZYX');     robotPose = [position, orientation(1)];          % Extract the laser scan.     scan = lidarScan(scanMsg);     ranges = scan.Ranges;     ranges(isnan(ranges)) = sim.LaserSensor.MaxRange;     modScan = lidarScan(ranges, scan.Angles);          % Insert the laser range observation in the map.     insertRay(map, robotPose, modScan, sim.LaserSensor.MaxRange);          % Compute the linear and angular velocity of the robot and publish it     % to drive the robot.     [v, w] = controller(robotPose);     velMsg.Linear.X = v;     velMsg.Angular.Z = w;     send(velPub, velMsg);          % Visualize the map after every 50th update.     %     if ~mod(updateCounter,50)     %         mapHandle.CData = occupancyMatrix(map);     %         title(axesHandle, ['OccupancyGrid: Update ' num2str(updateCounter)]);     %     end          % Update the counter and distance to goal.     updateCounter = updateCounter+1;     distanceToGoal = norm(robotPose(1:2) - robotGoal);          % Wait for control rate to ensure 10 Hz rate.     waitfor(controlRate); end

Visualice el mapa final, que ha incorporado todas las lecturas del sensor.

show(map, 'Parent', axesHandle); title(axesHandle, 'OccupancyGrid: Final Map');

Shutdown ROS Network

Apague el maestro ROS y borre el nodo global.

rosshutdown
Shutting down global node /matlab_global_node_41987 with NodeURI http://bat572504glnxa64:37285/ Shutting down ROS master on http://bat572504glnxa64:38733/. 

Consulte también