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
, usado 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 un arreglo de waypoints. El robot sigue estos waypoints para alcanzar la posición objetivo. 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
© Copyright 2019 The MathWorks, Inc.