Simular la planificación de rutas con un robot móvil
Cree un escenario para simular la navegación de un robot móvil por un recinto. El ejemplo muestra cómo crear un escenario, modelar una plataforma robótica a partir de un objeto de árbol de cuerpo rígido, obtener un mapa de cuadrícula de ocupación binario a partir del escenario y planificar una ruta que el robot móvil seguirá utilizando el algoritmo de planificación de rutas mobileRobotPRM
.
Crear un escenario con plano terrestre y mallas estáticas
Un objeto robotScenario
consta de un conjunto de obstáculos estáticos y objetos móviles denominados plataformas. Utilice el objeto robotPlatform
para modelar el robot móvil dentro del escenario. Este ejemplo crea un escenario que consiste en un plano terrestre y mallas de cuadro para crear un recinto.
scenario = robotScenario(UpdateRate=5);
Añada una malla de plano como plano terrestre en el escenario.
floorColor = [0.5882 0.2941 0];
addMesh(scenario,"Plane",Position=[5 5 0],Size=[10 10],Color=floorColor);
Las paredes del recinto se modelan como mallas de cuadro. Las mallas estáticas se añaden con el valor IsBinaryOccupied
establecido en true
, de modo que estos obstáculos se incorporan al mapa de ocupación binario utilizado para la planificación de rutas.
wallHeight = 1; wallWidth = 0.25; wallLength = 10; wallColor = [1 1 0.8157]; % Add outer walls. addMesh(scenario,"Box",Position=[wallWidth/2, wallLength/2, wallHeight/2],... Size=[wallWidth, wallLength, wallHeight],Color=wallColor,IsBinaryOccupied=true); addMesh(scenario,"Box",Position=[wallLength-wallWidth/2, wallLength/2, wallHeight/2],... Size=[wallWidth, wallLength, wallHeight],Color=wallColor,IsBinaryOccupied=true); addMesh(scenario,"Box",Position=[wallLength/2, wallLength-wallWidth/2, wallHeight/2],... Size=[wallLength, wallWidth, wallHeight],Color=wallColor,IsBinaryOccupied=true); addMesh(scenario,"Box",Position=[wallLength/2, wallWidth/2, wallHeight/2],... Size=[wallLength, wallWidth, wallHeight],Color=wallColor,IsBinaryOccupied=true); % Add inner walls. addMesh(scenario,"Box",Position=[wallLength/8, wallLength/3, wallHeight/2],... Size=[wallLength/4, wallWidth, wallHeight],Color=wallColor,IsBinaryOccupied=true); addMesh(scenario,"Box",Position=[wallLength/4, wallLength/3, wallHeight/2],... Size=[wallWidth, wallLength/6, wallHeight],Color=wallColor,IsBinaryOccupied=true); addMesh(scenario,"Box",Position=[(wallLength-wallLength/4), wallLength/2, wallHeight/2],... Size=[wallLength/2, wallWidth, wallHeight],Color=wallColor,IsBinaryOccupied=true); addMesh(scenario,"Box",Position=[wallLength/2, wallLength/2, wallHeight/2],... Size=[wallWidth, wallLength/3, wallHeight],Color=wallColor,IsBinaryOccupied=true);
Visualice el escenario.
show3D(scenario); lightangle(-45,30); view(60,50);
Obtener el mapa de ocupación binario a partir del escenario
Obtenga un mapa de ocupación como objeto binaryOccupancyMap
a partir del escenario para planificar la ruta. Agrande los espacios ocupados del mapa en 0,3 m.
map = binaryOccupancyMap(scenario,GridOriginInLocal=[-2 -2],... MapSize=[15 15],... MapHeightLimits=[0 3]); inflate(map,0.3);
Visualice el mapa de ocupación 2D.
show(map)
Planificación de rutas
Utilice el planificador de rutas mobileRobotPRM
para encontrar una ruta libre de obstáculos entre las posiciones inicial y objetivo en el mapa obtenido.
Especifique las posiciones inicial y objetivo del robot móvil.
startPosition = [1 1]; goalPosition = [8 8];
Establezca el valor inicial rng
para la repetibilidad.
rng(100)
Cree un objeto mobileRobotPRM
con el mapa de ocupación binario y especifique el número máximo de nodos. Especifique la distancia máxima entre los dos nodos conectados.
numnodes = 2000; planner = mobileRobotPRM(map,numnodes); planner.ConnectionDistance = 1;
Trace una ruta entre las posiciones inicial y objetivo.
waypoints = findpath(planner,startPosition,goalPosition);
Generación de trayectorias
Genere la trayectoria que debe seguir el robot móvil con los waypoints de la ruta planificada utilizando el System object™ waypointTrajectory
.
% Robot height from base. robotheight = 0.12; % Number of waypoints. numWaypoints = size(waypoints,1); % Robot arrival time at first waypoint. firstInTime = 0; % Robot arrival time at last waypoint. lastInTime = firstInTime + (numWaypoints-1); % Generate waypoint trajectory with waypoints from planned path. traj = waypointTrajectory(SampleRate=10,... TimeOfArrival=firstInTime:lastInTime, ... Waypoints=[waypoints, robotheight*ones(numWaypoints,1)], ... ReferenceFrame="ENU");
Añadir una plataforma robótica al escenario
Cargue el robot móvil Clearpath Husky desde la biblioteca de robots como un objeto rigidBodyTree
.
huskyRobot = loadrobot("clearpathHusky");
Cree un objeto robotPlatform
con el modelo de robot especificado como un objeto rigidBodyTree
y su trayectoria especificada como un System object waypointTrajectory
.
platform = robotPlatform("husky",scenario, RigidBodyTree=huskyRobot,... BaseTrajectory=traj);
Visualice el escenario con el robot.
[ax,plotFrames] = show3D(scenario); lightangle(-45,30) view(60,50)
Simular el robot móvil
Visualice la ruta planificada.
hold(ax,"on") plot(ax,waypoints(:,1),waypoints(:,2),"-ms",... LineWidth=2,... MarkerSize=4,... MarkerEdgeColor="b",... MarkerFaceColor=[0.5 0.5 0.5]); hold(ax,"off")
Configure la simulación. Dado que todas las poses del robot se conocen de antemano, basta con recorrer la simulación y actualizar la visualización en cada paso.
setup(scenario) % Control simulation rate at 20 Hz. r = rateControl(20); % Status of robot in simulation. robotStartMoving = false; while advance(scenario) show3D(scenario,Parent=ax,FastUpdate=true); waitfor(r); currentPose = read(platform); if ~any(isnan(currentPose)) % implies that robot is in the scene and performing simulation. robotStartMoving = true; end if any(isnan(currentPose)) && robotStartMoving % break, once robot reaches goal position. break; end end
Para volver a ejecutar la simulación y visualizar de nuevo los resultados, restablezca la simulación en el escenario.
restart(scenario)