Esta página se ha traducido mediante traducción automática. Haga clic aquí para ver la última versión en inglés.

Localice TurtleBot utilizando el algoritmo de localización Monte Carlo

Este ejemplo utiliza:

Aplique el algoritmo de localización Monte Carlo en un robot TurtleBot® en un entorno Gazebo® simulado.

La localización de Monte Carlo (MCL) es un algoritmo para localizar un robot mediante 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 en función del 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 de acuerdo con dicha creencia. Estas partículas se propagan siguiendo el modelo de movimiento del robot cada vez que cambia la pose del robot. Al recibir nuevas lecturas del sensor, cada partícula evaluará su precisión comprobando la probabilidad de que reciba dichas lecturas del sensor en su posición actual. A continuación, el algoritmo redistribuirá (volverá a muestrear) las partículas para sesgar las partículas que sean más precisas. Continúe repitiendo estos pasos de movimiento, detección y remuestreo, y todas las partículas deberían converger en un solo grupo cerca de la verdadera pose del robot si la localización es exitosa.

La localización adaptativa de Monte Carlo (AMCL) es la variante de MCL implementada en monteCarloLocalization. AMCL ajusta dinámicamente la cantidad de partículas según la distancia KL [1] para garantizar que la distribución de partículas converja a la verdadera distribución del estado del robot según todas las mediciones anteriores de sensores y movimiento con alta probabilidad.

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

La simulación Gazebo TurtleBot debe estar ejecutándose para que este ejemplo funcione.

Requisitos previos: Get Started with Gazebo and Simulated TurtleBot (ROS Toolbox), Access the tf Transformation Tree in ROS (ROS Toolbox), Exchange Data with ROS Publishers and Subscribers (ROS Toolbox).

Conéctese al TurtleBot en Gazebo

Primero, genere un TurtleBot simulado dentro de un entorno de oficina en una máquina virtual siguiendo los pasos en Get Started with Gazebo and Simulated TurtleBot (ROS Toolbox) para iniciar Gazebo Office World desde el escritorio, como se muestra a continuación.

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

ipaddress = '192.168.2.150';
rosinit(ipaddress,11311);

Initializing global node /matlab_global_node_73841 with NodeURI http://192.168.2.1:53461/

El diseño del entorno de oficina simulado:

Cargar 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 manejando TurtleBot dentro del entorno de oficina. El mapa se construye utilizando lecturas de alcance de Kinect® y poses reales del tema gazebo/model_states .

load officemap.mat
show(map)

Configurar 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 Noise define la incertidumbre en el movimiento lineal y rotacional del robot. Aumentar la propiedad odometryModel.Noise permitirá una mayor dispersión al propagar partículas mediante mediciones de odometría. Consulte odometryMotionModel para obtener detalles de la propiedad.

odometryModel = odometryMotionModel;
odometryModel.Noise = [0.2 0.2 0.2 0.2];

El sensor de TurtleBot es un telémetro simulado convertido a partir de lecturas de Kinect. El método del campo de probabilidad se utiliza para calcular la probabilidad de percibir un conjunto de mediciones comparando los puntos finales de las mediciones del telémetro 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 real del sensor para lograr mejores resultados de prueba. La propiedad SensorLimits define el rango mínimo y máximo de lecturas del sensor. La propiedad Map define el mapa de ocupación utilizado para calcular el campo de probabilidad. Consulte likelihoodFieldSensorModel para obtener detalles de la propiedad.

rangeFinderModel = likelihoodFieldSensorModel;
rangeFinderModel.SensorLimits = [0.45 8];
rangeFinderModel.Map = map;

Establezca rangeFinderModel.SensorPose en la transformación de coordenadas de la cámara fija con respecto a la base del robot. Esto se utiliza para transformar las lecturas del láser desde el marco de la cámara al marco base de TurtleBot. Consulte Access the tf Transformation Tree in ROS (ROS Toolbox) para obtener detalles sobre las transformaciones de coordenadas.

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

% Query the Transformation Tree (tf tree) in ROS.
tftree = rostf;
waitForTransform(tftree,'/base_link','/base_scan');
sensorTransform = getTransform(tftree,'/base_link', '/base_scan');

% 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)];

Recibir mediciones de sensores y enviar comandos de velocidad

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

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

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

[velPub,velMsg] = ...
    rospublisher('/cmd_vel','geometry_msgs/Twist');

Inicializar objeto AMCL

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

amcl = monteCarloLocalization;
amcl.UseLidarScan = true;

Asigne las propiedades MotionModel y SensorModel en el objeto amcl .

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

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

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

Configure el objeto AMCL para la localización con una estimación de pose inicial.

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

Ahora establezca amcl.GlobalLocalization en falso y proporcione una pose inicial estimada a AMCL. Al hacerlo, AMCL mantiene la creencia inicial de que la verdadera pose del robot sigue una distribución gaussiana con una media igual a amcl.InitialPose y una matriz de covarianza igual a amcl.InitialCovariance. La estimación de la pose inicial debe obtenerse de acuerdo con su configuración. Este ayudante de ejemplo recupera la verdadera pose actual del robot de Gazebo.

Consulte la sección Configurar el objeto AMCL para la localización global para ver un ejemplo sobre el uso de la localización global.

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

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

Ejemplo de configuraciónHelperAMCLVisualización para trazar el mapa y actualizar la pose 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 AMCL. En este ejemplo, manejamos TurtleBot aleatoriamente usando la clase EjemploHelperAMCLWanderer, que conduce al robot dentro del entorno mientras evita obstáculos usando la clase controllerVFH .

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

Procedimiento de localización

El algoritmo AMCL se actualiza con lecturas de odometría y sensores en cada paso de tiempo cuando el robot se mueve. Espere unos segundos antes de que las partículas se inicialicen y se representen en la figura. En este ejemplo ejecutaremos actualizaciones de numUpdates AMCL. Si el robot no converge a la pose correcta, considere usar un numUpdates más grande.

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 TurtleBot y apague ROS en MATLAB

stop(wanderHelper);
rosshutdown

Shutting down global node /matlab_global_node_73841 with NodeURI http://192.168.2.1:53461/

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

AMCL es un algoritmo probabilístico, el resultado de la simulación en su computadora puede ser ligeramente diferente de la ejecución de muestra que se muestra aquí.

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

Después de 8 actualizaciones, las partículas comienzan a converger hacia áreas con mayor probabilidad:

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

Configure el objeto AMCL para la localización global.

En caso de que no haya una estimación inicial de la pose del robot disponible, AMCL intentará localizar el robot sin conocer su posición inicial. Inicialmente, el algoritmo supone que el robot tiene la misma probabilidad de estar en cualquier lugar del espacio libre de la oficina y genera partículas uniformemente distribuidas 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 función de localización global de AMCL, reemplace las secciones de código en Configurar objeto AMCL para localización con estimación de pose inicial con el código de esta sección.

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

Resultados de muestra para la localización global de AMCL

AMCL es un algoritmo probabilístico, el resultado de la simulación en su computadora puede ser ligeramente diferente de la ejecución de muestra que se muestra aquí.

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 comienzan a converger hacia áreas con mayor probabilidad:

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

Referencias

[1] S. Thrun, W. Burgard y D. Fox, Robótica probabilística. Cambridge, MA: Prensa del MIT, 2005.