Esta página se ha traducido mediante traducción automática. Haga clic aquí para ver la última versión en inglés.

Planificar la ruta hacia la región objetivo personalizada para Mobile Robot

Este ejemplo muestra cómo planificar una ruta para un robot móvil hacia una región objetivo utilizando un planificador de rutas de árbol aleatorio (RRT) de exploración rápida. En este ejemplo, puede definir una región objetivo personalizada como un polígono 2D y luego planificar una ruta hacia ella.

Cuando se utilizan robots domésticos o robots de entrega de paquetes, puede que no sea posible definir el objetivo como un punto único en un entorno dinámico. Por ejemplo, si el punto objetivo está ocupado temporalmente, esto puede provocar que el planificador falle. En tales escenarios, puede ser más eficiente planificar para llegar a un punto en una región objetivo aceptable. Puede aprovechar la propiedad GoalReachedFcn de los objetos plannerRRT y plannerRRTStar para planificar una ruta hacia una región objetivo. Este ejemplo le muestra cómo definir una región objetivo como un polígono utilizando la función polyshape y luego planificar una ruta hacia ella utilizando el planificador de rutas plannerRRT.

Para este ejemplo, considere el caso de uso de un robot de entrega de paquetes en un almacén. El robot recoge un paquete del almacén y lo deja en un área específica cerca de la entrada del almacén, en lugar de en un punto concreto.

Configurar el entorno y la posición inicial del robot

Cree un objeto occupancyMap para un mapa de almacén y especifique la posición inicial del robot.

% Load the map and create an occupancy map with resolution of 1 cell/meter.
load wareHouseMap.mat
map = occupancyMap(logicalMap);

% Visualize the map.
show(map)
hold on

% Set the initial position of the robot.
initialPos = [10 40 -pi/4];
helperFunctionPlotRobot(initialPos)
hold off

Definir la región del objetivo y la función personalizada para alcanzar el objetivo

Define una región objetivo cerca de la entrada del almacén utilizando la función polyshape. La función polyshape le permite definir un polígono de n lados especificando los vértices del polígono. También puedes utilizar la función nsidedpoly para crear polígonos regulares.

% Specify vertices of the goal Region.
vertices = [1 17;
            1 15;
            25 15;
            25 17];

% Create goal region using the vertices.
goalRegion = polyshape(vertices);

Visualice el robot y la región objetivo en el mapa del almacén.

hold on
plot(goalRegion,FaceColor="red",FaceAlpha=0.2)
hold off

Cree una función anónima personalizada para evaluar si un punto en particular está dentro de la región objetivo.

goalReachedRegionCheck = @(planner,currentPos,goalPos) isinterior(goalRegion,currentPos(1:2));

Definir espacio de estados y validador de estados

Crea un espacio de estados de Dubins.

ss = stateSpaceDubins;

Cree un validador de estado utilizando el espacio de estado creado y el mapa.

sv = validatorOccupancyMap(ss,Map=map);

Establezca la distancia para interpolar y validar segmentos de ruta.

sv.ValidationDistance=0.1;

Actualice los límites del espacio de estados para que sean los mismos que los límites del mapa.

ss.StateBounds=[map.XWorldLimits; map.YWorldLimits; [-pi pi]];

Planificar ruta

Cree el planificador de rutas, reduzca el número de iteraciones y aumente la distancia máxima de conexión. Especifique su función personalizada de objetivo alcanzado para comprobar si se alcanza el objetivo.

planner = plannerRRT(ss,sv, ...
                     MaxIterations=2500, ...
                     MaxConnectionDistance=5, ...
                     GoalReachedFcn=goalReachedRegionCheck);

Planifique la ruta desde la posición actual del robot hasta la región objetivo en el mapa. Considere el centroide de la región objetivo como la posición objetivo. En algunos casos el centroide puede estar ocupado. En tal caso, tome como posición de destino cualquier otro lugar libre de obstáculos en la región.

% Set the goal position to the centroid of the goal region.
[goal_x,goal_y] = centroid(goalRegion);
goal = [goal_x goal_y 0];

% Plan the path.
rng(10,"twister") % for repeatable result
[pthObj,solnInfo] = plan(planner,initialPos,goal);

Visualizar ruta

Trace la ruta calculada por el planificador desde la posición actual del robot hasta la región objetivo en el mapa del almacén.

hold on
plot(pthObj.States(:,1),pthObj.States(:,2),"r-",LineWidth=1)
hold off

Optimizar ruta

Optimice la ruta para obtener una ruta suave con suficiente espacio para que el robot navegue por el mapa.

Cree un objeto de opciones de configuración para su optimización.

options = optimizePathOptions;

Mantenga una distancia segura de 2 metros de los obstáculos.

options.ObstacleSafetyMargin = 2;

Optimizar la ruta generado por el planificador.

optPath = optimizePath(pthObj.States,map,options);
hold on
plot(optPath(:,1),optPath(:,2),"m-",LineWidth=1)
legend("","Planned Path","Optimized Path")
hold off