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

Mover muebles en una habitación desordenada con RRT

Este ejemplo muestra cómo planificar una ruta para mover muebles voluminosos en un espacio reducido evitando los postes. Este ejemplo muestra un flujo de trabajo del "Problema de Piano Mover", que se utiliza para probar algoritmos de planificación de rutas con espacios de estado restringidos. Este ejemplo utiliza el objeto plannerRRTStar para implementar un algoritmo personalizado optimizado de árbol de exploración rápida (RRT*). Los ayudantes de ejemplo proporcionados ilustran cómo definir espacios de estado personalizados y validación de estado para cualquier aplicación de planificación de movimiento.

Modelar la escena

Para ayudar a visualizar y resolver este problema de planificación de rutas, se proporcionan dos clases auxiliares, ExampleHelperFurniture y ExampleHelperRoom. La clase ExampleHelperFurniture ensambla los muebles juntando tres cajas de colisión rectangulares. El mobiliario está dispuesto de la siguiente manera:

El ExampleHelperRoom define la dimensión de la habitación y proporciona funciones para insertar muebles en la habitación y verificar si los muebles chocan con las paredes o los postes. Estas dos clases se utilizan para el validador de estado del planificador.

Muestre la habitación con un largo y ancho determinados. Añade un mueble con una pose determinada.

len = 6;
wid = 2;
room = ExampleHelperRoom(len, wid);
chair = ExampleHelperFurniture;
addFurniture(room,chair,trvec2tform([0 3.5 0]));
show(room,gca)
axis equal

Configurar el espacio de estado

Crea un objeto stateSpaceSE2 para los muebles. Establezca los límites según las dimensiones de la habitación. El espacio de estados muestra estados aleatorios en el espacio de estados. En este ejemplo, el estado de los muebles es un vector de 3 elementos, [x y theta], para las coordenadas xy y el ángulo de rotación en radianes.

bounds = [-0.8 0.8; [-1 5]; [-pi pi]];

ss = stateSpaceSE2(bounds);
ss.WeightTheta = 2;

Crear un validador de estado personalizado

El planificador requiere un validador de estado personalizado para permitir la verificación de colisiones entre los muebles y los accesorios de la habitación. La clase proporcionada, ExampleHelperFurnitureInRoomValidator, verifica la validez de los estados de los muebles en función de las funciones de verificación de colisiones de polígonos convexos por pares. La clase crea automáticamente una habitación y coloca en ella muebles de formas extrañas durante la construcción.

% Set the initial pose of the furniture
initPose = trvec2tform([0 3.5 0]);

% Create a customized state validator
sv = ExampleHelperFurnitureInRoomValidator(ss, initPose);

% Reduce the validation distance
% Validation distance determines the granularity of interpolation when
% checking the motion that connects two states.
sv.ValidationDistance = 0.1;

Configurar el planificador de rutas

Utilice plannerRRTStar como planificador y especifique el espacio de estado personalizado y el validador de estado. Especifique parámetros adicionales para el planificador. La función auxiliar de ejemplo GoalReached devuelve true cuando una ruta factible se acerca lo suficiente al objetivo dentro de un umbral. Esto sale del planificador.

% Create the planner
rrt = plannerRRTStar(ss, sv);

% Set ball radius for searching near neighbors
rrt.BallRadiusConstant = 1000;

% Exit as soon as a path is found
rrt.ContinueAfterGoalReached = false;

% The motion length between two furniture poses should be less than 0.4 m
rrt.MaxConnectionDistance = 0.4;

% Increase the max iterations
rrt.MaxIterations = 20000;

% Use a customized goal function 
rrt.GoalReachedFcn = @exampleHelperGoalFunc;

Planifique la mudanza

Establece una pose inicial y final para los muebles. Este ejemplo se mueve de un poste al otro y gira la silla pi radianes. Planifica la ruta entre poses. Visualice el árbol de búsqueda y la ruta final.

% Set the init and goal poses
start = [0 3.5 0];
goal = [0 -0.2 pi];

% Set random number seed for repeatability
rng(0, 'twister');
[path, solnInfo] = plan(rrt,start,goal);

hold on
% Search tree
plot(solnInfo.TreeData(:,1), solnInfo.TreeData(:,2), '.-');
% Interpolate path and plot points
interpolate(path,300)
plot(path.States(:,1), path.States(:,2), 'r-', 'LineWidth', 2)

hold off

Visualiza el movimiento

Se proporciona un ejemplo de ayuda para suavizar la ruta cortando esquinas de la ruta siempre que sea posible. Anima el movimiento de los muebles desde la pose inicial hasta la meta. La gráfica de animación muestra estados intermedios a medida que los muebles navegan hacia la posición objetivo.

f = figure;

% Smooth the path, cut the corners wherever possible
pathSm = exampleHelperSmoothPath(path, sv);

interpolate(pathSm,100);
animateFurnitureMotion(sv.Room,1,pathSm.States, axes(f))

% show the trace of furniture
skip = 6;
states = pathSm.States([1:skip:end, pathSm.NumStates], :);
exampleHelperShowFurnitureTrace(sv.Room.FurnituresInRoom{1}, states);