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

Localize TurtleBot utilizando la localización de Montecarlo

En este ejemplo se muestra una aplicación del algoritmo de localización de Montecarlo (MCL) en TurtleBot® en entorno de Gazebo® simulado.

Monte Carlo Localization (MCL) es un algoritmo para localizar un robot usando un filtro de partículas. El algoritmo requiere un mapa conocido y la tarea es estimar la pose (posición y orientación) del robot dentro del mapa basado en el movimiento y la detección del robot. El algoritmo comienza con una creencia inicial de la distribución de probabilidad de la pose del robot, que está representada por partículas distribuidas según dicha creencia. Estas partículas se propagan siguiendo el modelo de movimiento del robot cada vez que la pose del robot cambia. Al recibir nuevas lecturas del sensor, cada partícula evaluará su precisión comprobando la probabilidad de que recibiría tales lecturas del sensor en su pose actual. A continuación, el algoritmo redistribuirá (remuestreará) las partículas para sesgar las partículas que son más precisas. Siga iterando estos pasos de movimiento, detección y remuestreo, y todas las partículas deben converger en un solo clúster cerca de la verdadera pose de robot si la localización es exitosa.

Adaptación de Montecarlo adaptable (AMCL) es la variante de MCL implementado en. AMCL ajusta dinámicamente el número de partículas basadas en KL-Distance [1] para garantizar que la distribución de partículas converjan en la verdadera distribución del estado del robot en función de todas las mediciones anteriores de sensor y movimiento con alta probabilidad.

La implementación actual de MATLAB® AMCL se puede aplicar a cualquier robot de accionamiento diferencial equipado con un buscador de rango.

The Gazebo TurtleBot simulation must be running for this example to work.

Requisitos previos:.Empiece con gazebo y un TurtleBot simuladoAcceda al árbol de transformación TF en ROSIntercambiar datos con ROS editores y suscriptores

Nota: A partir de R2016b, en lugar de utilizar el método STEP para realizar la operación definida por el objeto System, puede llamar al objeto con argumentos, como si se tratara de una función. Por ejemplo, y realizar operaciones equivalentes.y = step(obj,x)y = obj(x)

Conéctese a la TurtleBot en gazebo

En primer lugar, genere un TurtleBot simulado dentro de un entorno de oficina en una máquina virtual siguiendo los pasos en el para iniciar el desde el escritorio.Empiece con gazebo y un TurtleBot simuladoGazebo TurtleBot World

En su instancia de MATLAB en el equipo host, ejecute los siguientes comandos para inicializar el nodo global de ROS en MATLAB y conéctese al maestro de ROS en la máquina virtual a través de su dirección IP.ipaddress Reemplace con la dirección IP de su TurtleBot en la máquina virtual.ipaddress

ipaddress = '192.168.203.129'; rosinit(ipaddress);
Initializing global node /matlab_global_node_68982 with NodeURI http://192.168.203.1:55528/ 

El diseño del entorno de oficina simulado:

Cargue el mapa del mundo de simulación

Cargue una cuadrícula de ocupación binaria del entorno de oficina en gazebo. El mapa se genera mediante la conducción de TurtleBot dentro del entorno de oficina. El mapa se construye utilizando lecturas de alcance de Kinect® y la verdad del suelo plantea el tema.gazebo/model_states Consulte para obtener una explicación más detallada.Mapeo con poses conocidas

load officemap.mat show(map)

Configure el modelo de sensor láser y el modelo de movimiento TurtleBot

TurtleBot se puede modelar como un robot de accionamiento diferencial y su movimiento se puede estimar utilizando datos de odometría. La propiedad define la incertidumbre en el movimiento rotacional y lineal del robot.Noise El aumento de la propiedad permitirá mayor propagación al propagar partículas utilizando mediciones de odometría.odometryModel.Noise Por favor, consulte los detalles de la propiedad.

odometryModel = robotics.OdometryMotionModel; odometryModel.Noise = [0.2 0.2 0.2 0.2];

El sensor de TurtleBot es un buscador de rango simulado convertido de las lecturas de Kinect. El método de campo de probabilidad se utiliza para calcular la probabilidad de percibir un conjunto de mediciones comparando los puntos finales de las mediciones del buscador de rango con el mapa de ocupación. Si los puntos finales coinciden con los puntos ocupados en el mapa de ocupación, la probabilidad de percibir tales mediciones es alta. El modelo del sensor debe ajustarse para que coincida con la propiedad del sensor real para lograr mejores resultados de la prueba. La propiedad define el rango mínimo y máximo de lecturas del sensor.SensorLimits La propiedad define el mapa de ocupación utilizado para calcular el campo de probabilidad.Map Por favor, consulte los detalles de la propiedad.

rangeFinderModel = robotics.LikelihoodFieldSensorModel; rangeFinderModel.SensorLimits = [0.45 8]; rangeFinderModel.Map = map;

Se establece en la transformación de coordenadas de la cámara fija con respecto a la base del robot.rangeFinderModel.SensorPose Esto se utiliza para transformar las lecturas del láser desde el marco de la cámara hasta el marco base de TurtleBot. Consulte para obtener más información sobre las transformaciones de coordenadas.Acceda al árbol de transformación TF en ROS

Tenga en cuenta que actualmente solo es compatible con sensores que están fijados en el bastidor del robot, lo que significa que la transformación del sensor es constante.SensorModel

% Query the Transformation Tree (tf tree) in ROS. tftree = rostf; waitForTransform(tftree,'/base_link','/camera_depth_frame'); sensorTransform = getTransform(tftree,'/base_link', '/camera_depth_frame');  % Get the euler rotation angles. laserQuat = [sensorTransform.Transform.Rotation.W sensorTransform.Transform.Rotation.X ...     sensorTransform.Transform.Rotation.Y sensorTransform.Transform.Rotation.Z]; laserRotation = quat2eul(laserQuat, 'ZYX');  % Setup the |SensorPose|, which includes the translation along base_link's % +X, +Y direction in meters and rotation angle along base_link's +Z axis % in radians. rangeFinderModel.SensorPose = ...     [sensorTransform.Transform.Translation.X sensorTransform.Transform.Translation.Y laserRotation(1)];

Interfaz para la recepción de mediciones de sensor desde TurtleBot y enviando comandos de velocidad a TurtleBot.

Cree suscriptores de ROS para recuperar mediciones de sensores y odometría de TurtleBot.

laserSub = rossubscriber('/scan'); odomSub = rossubscriber('/odom');

Cree un editor de ROS para enviar comandos de velocidad a TurtleBot. TurtleBot se suscribe a los comandos de velocidad.'/mobile_base/commands/velocity'

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

Inicializar el objeto AMCL

Crear una instancia de un objeto AMCL.amcl Consulte para obtener más información sobre la clase.

amcl = robotics.MonteCarloLocalization; amcl.UseLidarScan = true;

Asigne las propiedades y en el objeto.MotionModelSensorModelamcl

amcl.MotionModel = odometryModel; amcl.SensorModel = rangeFinderModel;

El filtro de partículas sólo actualiza las partículas cuando el movimiento del robot excede el, lo que define el desplazamiento mínimo en [x, y, guiñada] para desencadenar la actualización del filtro.UpdateThresholds Esto previene las actualizaciones demasiado frecuentes debido al ruido del sensor. El remuestreo de partículas ocurre después de que el filtro se actualiza.amcl.ResamplingInterval El uso de números más grandes conduce a un agotamiento de partículas más lento al precio de la convergencia de partículas más lenta también.

amcl.UpdateThresholds = [0.2,0.2,0.2]; amcl.ResamplingInterval = 1;

Configurar objeto de AMCL para la localización con estimación de pose inicial.

define el límite inferior y superior del número de partículas que se generarán durante el proceso de remuestreo.amcl.ParticleLimits Permitir que se generen más partículas puede mejorar la posibilidad de converger a la postura verdadera del robot, pero tiene un impacto en la velocidad de cálculo y las partículas pueden tomar más tiempo o incluso no converger. Por favor refiérase a la sección ' KL-D Sampling ' en [1] para calcular un valor límite razonable en el número de partículas. Tenga en cuenta que la localización global puede necesitar significativamente más partículas en comparación con la localización con una estimación de la postura inicial. Si el robot conoce su postura inicial con cierta incertidumbre, dicha información adicional puede ayudar a AMCL a localizar robots más rápido con un menor número de partículas, es decir, se puede utilizar un valor menor de límite superior en.amcl.ParticleLimits

Ahora se establece en false y proporciona una pose inicial estimada a AMCL.amcl.GlobalLocalization Al hacerlo, AMCL mantiene la creencia inicial de que la verdadera pose del robot sigue una distribución gaussiana con una media igual a y una matriz de covarianza igual a.amcl.InitialPoseamcl.InitialCovariance La estimación de la pose inicial debe obtenerse de acuerdo con su configuración. Este ejemplo auxiliar recupera la pose verdadera actual del robot de gazebo.

Por favor refiérase a la sección para un ejemplo sobre el uso de la localización global.Configure AMCL object for global localization

amcl.ParticleLimits = [500 5000]; amcl.GlobalLocalization = false; amcl.InitialPose = ExampleHelperAMCLGazeboTruePose; amcl.InitialCovariance = eye(3)*0.5;

Ayudante de configuración para visualización y conducción de TurtleBot.

Setup ExampleHelperAMCLVisualization para trazar el mapa y actualizar la postura estimada del robot, las partículas y las lecturas del escaneo láser en el mapa.

visualizationHelper = ExampleHelperAMCLVisualization(map);

El movimiento del robot es esencial para el algoritmo de AMCL. En este ejemplo, conducimos TurtleBot aleatoriamente usando la clase ExampleHelperAMCLWanderer, que impulsa al robot dentro del entorno evitando obstáculos usando la clase.

wanderHelper = ...     ExampleHelperAMCLWanderer(laserSub, sensorTransform, velPub, velMsg);

Procedimiento de localización

El algoritmo de AMCL se actualiza con las lecturas de odametría y sensor en cada paso de tiempo cuando el robot se mueve alrededor. Espere unos segundos antes de que las partículas se inicialicen y se trazen en la figura. En este ejemplo vamos a ejecutar las actualizaciones de AMCL.numUpdates Si el robot no convergen a la postura correcta del robot, considere el uso de un más grande.numUpdates

numUpdates = 60; i = 0; while i < numUpdates     % Receive laser scan and odometry message.     scanMsg = receive(laserSub);     odompose = odomSub.LatestMessage;          % Create lidarScan object to pass to the AMCL object.     scan = lidarScan(scanMsg);          % For sensors that are mounted upside down, you need to reverse the     % order of scan angle readings using 'flip' function.          % Compute robot's pose [x,y,yaw] from odometry message.     odomQuat = [odompose.Pose.Pose.Orientation.W, odompose.Pose.Pose.Orientation.X, ...         odompose.Pose.Pose.Orientation.Y, odompose.Pose.Pose.Orientation.Z];     odomRotation = quat2eul(odomQuat);     pose = [odompose.Pose.Pose.Position.X, odompose.Pose.Pose.Position.Y odomRotation(1)];          % Update estimated robot's pose and covariance using new odometry and     % sensor readings.     [isUpdated,estimatedPose, estimatedCovariance] = amcl(pose, scan);          % Drive robot to next pose.     wander(wanderHelper);          % Plot the robot's estimated pose, particles and laser scans on the map.     if isUpdated         i = i + 1;         plotStep(visualizationHelper, amcl, estimatedPose, scan, i)     end      end

Detenga el TurtleBot y el Shutdown ROS en MATLAB

stop(wanderHelper); rosshutdown
Shutting down global node /matlab_global_node_68982 with NodeURI http://192.168.203.1:55528/ 

Resultados de muestra para la localización de AMCL con estimación de pose inicial

AMCL is a probabilistic algorithm, the simulation result on your computer may be slightly different from the sample run shown here.

Después de la primera actualización de AMCL, las partículas se generan muestreando la distribución gaussiana con una media igual a y covarianza igual a.amcl.InitialPoseamcl.InitialCovariance

Después de 8 actualizaciones, las partículas empiezan a convergen en áreas con mayor probabilidad:

Después de 60 actualizaciones, todas las partículas deben converger a la postura correcta del robot y los escaneos láser deben alinearse estrechamente con los contornos del mapa.

Configurar objeto de AMCL para la localización global.

En caso de que no se disponga de una estimación inicial de la pose del robot, AMCL intentará localizar el robot sin conocer la posición inicial del robot. El algoritmo inicialmente asume que el robot tiene la misma probabilidad de estar en cualquier lugar en el espacio libre de la oficina y genera partículas distribuidas uniformemente dentro de dicho espacio. Por lo tanto, la localización global requiere significativamente más partículas en comparación con la localización con estimación de pose inicial.

Para habilitar la característica de localización global de AMCL, reemplace las secciones de código con el código de esta sección.Configure AMCL object for localization with initial pose estimate

     amcl.GlobalLocalization = true;      amcl.ParticleLimits = [500 50000]; 

Resultados de la muestra de AMCL Global Localization

AMCL is a probabilistic algorithm, the simulation result on your computer may be slightly different from the sample run shown here.

Después de la primera actualización de AMCL, las partículas se distribuyen uniformemente dentro del espacio de oficina libre:

Después de 8 actualizaciones, las partículas empiezan a convergen en áreas con mayor probabilidad:

Después de 60 actualizaciones, todas las partículas deben converger a la postura correcta del robot y los escaneos láser deben alinearse estrechamente con los contornos del mapa.

Consulte también

, .Mapeo con poses conocidas

Referencias

[1] S. Thrun, W. Burgard y D. Fox, Probabilistic Robotics. Cambridge, MA: MIT Press, 2005.