Planificar una ruta para un robot de monociclo 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ística (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 monociclo 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 = [12 3];
Visión general del modelo
Abra el modelo de Simulink
open_system('pathPlanningUnicycleSimulinkModel.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 Unicycle Kinematic Model crea un modelo de vehículo para simular la cinemática simplificada del 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
Para simular el modelo
simulation = sim('pathPlanningUnicycleSimulinkModel.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)
map = binaryOccupancyMap with properties: mapLayer Properties LayerName: 'binaryLayer' DataType: 'logical' DefaultValue: 0 GridLocationInWorld: [0 0] GridOriginInLocal: [0 0] LocalOriginInWorld: [0 0] Resolution: 1 GridSize: [26 27] XLocalLimits: [0 27] YLocalLimits: [0 26] XWorldLimits: [0 27] YWorldLimits: [0 26]
robotPose = simulation.UnicyclePose
robotPose = 428×3
5.0000 5.0000 0
5.0000 5.0000 -0.0002
5.0001 5.0000 -0.0012
5.0006 5.0000 -0.0062
5.0031 5.0000 -0.0313
5.0156 4.9988 -0.1569
5.0707 4.9707 -0.7849
5.0945 4.9354 -1.1140
5.1075 4.9059 -1.1828
5.1193 4.8759 -1.2030
⋮
numRobots = size(robotPose, 2) / 3; 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; for k = 1:size(xyz, 1) show(map) hold on; % Plot Start Location plotTransforms([startLoc, 0], eul2quat([0, 0, 0])) text(startLoc(1), startLoc(2), 2, 'Start'); % Plot Goal Location plotTransforms([goalLoc, 0], eul2quat([0, 0, 0])) text(goalLoc(1), goalLoc(2), 2, 'Goal'); % Plot Robot's XY locations plot(robotPose(:, 1), robotPose(:, 2), '-b') % Plot Robot's pose as it traverses the path quat = eul2quat(thetaEuler(k, :), 'xyz'); plotTransforms(xyz(k,:), quat, 'MeshFilePath',... 'groundvehicle.stl'); pause(0.01) hold off; end
© Copyright 2019 The MathWorks, Inc.