Main Content

Planificar una ruta para un robot de tracción diferencial en Simulink

Este ejemplo muestra cómo ejecutar una ruta libre de obstáculos entre dos ubicaciones de un determinado mapa en Simulink®. La ruta se genera mediante un algoritmo de planificación de hojas de ruta probabilístico (PRM) (mobileRobotPRM)). Los controles de comando para desplazarse por esta ruta se generan utilizando el bloque de controlador Pure Pursuit. Un modelo de movimiento cinemático de tracción diferencial simula el movimiento del robot a partir de estos comandos.

Cargar el mapa y el modelo de Simulink

Cargue el mapa de ocupación, que define los límites y los obstáculos del mapa. exampleMaps.mat contiene varios mapas, incluido simpleMap, utilizado en este ejemplo.

load exampleMaps.mat

Especifique la ubicación inicial y la ubicación final en el mapa.

startLoc = [5 5];
goalLoc = [20 20];

Visión general del modelo

Abra el modelo de Simulink.

open_system('pathPlanningSimulinkModel.slx')

El modelo se compone de tres partes principales:

  • Planificación

  • Control

  • Modelo de planta

Planificación

El bloque de función de MATLAB® Planner utiliza el planificador de rutas mobileRobotPRM y toma como entradas una ubicación inicial, una ubicación objetivo y un mapa. Los bloques generan el arreglo de waypoints que sigue el robot. Posteriormente, los waypoints planificados son usados por el bloque de controlador Pure Pursuit.

Control

Pure Pursuit

El bloque de controlador Pure Pursuit genera los comandos de velocidad lineal y angular en función de los waypoints y la pose actual del robot.

Comprobar si se alcanza el objetivo

El subsistema Check Distance to Goal calcula la distancia actual hasta el objetivo y, si se encuentra dentro de un umbral, la simulación se detiene.

Modelo de planta

El bloque Differential Drive Kinematic Model crea un modelo de vehículo para simular la cinemática simplificada de un vehículo. El bloque toma velocidades lineales y angulares como entradas de comando desde el bloque de controlador Pure Pursuit y genera los estados de posición y velocidad actuales.

Ejecutar el modelo

simulation = sim('pathPlanningSimulinkModel.slx');

Visualizar el movimiento del robot

Después de simular el modelo, visualice cómo el robot se desplaza por la ruta libre de obstáculos en el mapa.

map = binaryOccupancyMap(simpleMap);
robotPose = simulation.Pose;
thetaIdx = 3;

% Translation
xyz = robotPose;
xyz(:, thetaIdx) = 0;

% Rotation in XYZ euler angles
theta = robotPose(:,thetaIdx);
thetaEuler = zeros(size(robotPose, 1), 3 * size(theta, 2));
thetaEuler(:, end) = theta;

% Plot the robot poses at every 10th step.
for k = 1:10:size(xyz, 1) 
    show(map)
    hold on;
    
    % Plot the start location.
    plotTransforms([startLoc, 0], eul2quat([0, 0, 0]))
    text(startLoc(1), startLoc(2), 2, 'Start');
    
    % Plot the goal location.
    plotTransforms([goalLoc, 0], eul2quat([0, 0, 0]))
    text(goalLoc(1), goalLoc(2), 2, 'Goal');
    
    % Plot the xy-locations.
    plot(robotPose(:, 1), robotPose(:, 2), '-b')
    
    % Plot the robot pose as it traverses the path.
    quat = eul2quat(thetaEuler(k, :), 'xyz');
    plotTransforms(xyz(k,:), quat, 'MeshFilePath',...
        'groundvehicle.stl');
    light;
    drawnow;
    hold off;
end

Figure contains an axes object. The axes object with title Binary Occupancy Grid contains 16 objects of type patch, line, image, text.

© Copyright 2019 The MathWorks, Inc.