Main Content

La traducción de esta página aún no se ha actualizado a la versión más reciente. Haga clic aquí para ver la última versión en inglés.

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)