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

Planifique rutas de robots móviles utilizando RRT

Este ejemplo muestra cómo utilizar el algoritmo de árbol aleatorio de exploración rápida (RRT) para planificar una ruta para un vehículo a través de un mapa conocido. También se aplican restricciones de vehículos especiales con un espacio de estado personalizado. Puede ajustar su propio planificador con espacios de estado personalizados y objetos de validación de ruta para cualquier aplicación de navegación.

Mapa de ocupación de carga

Cargue un mapa de ocupación que represente un espacio de oficina pequeño. Define las posiciones inicial y final del robot en el mapa.

load("office_area_gridmap.mat","occGrid")
show(occGrid)

% Set start and goal poses.
start = [-1.0,0.0,-pi];
goal = [14,-2.25,0];

% Show start and goal positions of robot.
hold on
plot(start(1),start(2),'ro')
plot(goal(1),goal(2),'mo')

% Show start and goal heading angle using a line.
r = 0.5;
plot([start(1),start(1) + r*cos(start(3))],[start(2),start(2) + r*sin(start(3))],'r-')
plot([goal(1),goal(1) + r*cos(goal(3))],[goal(2),goal(2) + r*sin(goal(3))],'m-')
hold off

Consideremos un modelo circular básico para el robot. Para evitar que el robot se acerque demasiado a los obstáculos, infle ligeramente los obstáculos del mapa.

% Make a copy of the original map and infate it by 0.1 meters. Use this inflated map for path planning. 
% Use the original map for visualization purpose. 
inflatedMap = copy(occGrid);
inflate(inflatedMap,0.1); 

Definir espacio de estados

Especifique el espacio de estado del vehículo utilizando un objeto stateSpaceDubins y especificando los límites de estado. Este objeto limita los estados muestreados a curvas de Dubins factibles para dirigir un vehículo dentro de los límites del estado. Un radio de giro de 0,4 metros permite realizar giros cerrados en este pequeño entorno.

bounds = [inflatedMap.XWorldLimits; inflatedMap.YWorldLimits; [-pi pi]];

ss = stateSpaceDubins(bounds);
ss.MinTurningRadius = 0.4;

Planifica el camino

Para planificar una ruta, el algoritmo RRT toma muestras de estados aleatorios dentro del espacio de estados e intenta conectar una ruta. Estos estados y conexiones deben validarse o excluirse según las restricciones del mapa. El vehículo no debe chocar con los obstáculos definidos en el mapa.

Crea un objeto validatorOccupancyMap con el espacio de estado especificado. Establezca la propiedad Map en el objeto occupancyMap cargado. Establezca un ValdiationDistance de 0,05 m. Esta distancia de validación discretiza las conexiones de rutas y verifica los obstáculos en el mapa en base a esto.

stateValidator = validatorOccupancyMap(ss); 
stateValidator.Map = inflatedMap;
stateValidator.ValidationDistance = 0.05;

Cree el planificador de rutas y aumente la distancia máxima de conexión para conectar más estados. Establezca el número máximo de iteraciones para los estados de muestreo.

planner = plannerRRT(ss,stateValidator);
planner.MaxConnectionDistance = 2.5;
planner.MaxIterations = 30000;

Personaliza la función GoalReached. Esta función auxiliar de ejemplo comprueba si una ruta factible alcanza el objetivo dentro de un umbral establecido. La función devuelve true cuando se alcanza el objetivo y el planificador se detiene.

planner.GoalReachedFcn = @exampleHelperCheckIfGoal;

function isReached = exampleHelperCheckIfGoal(planner, goalState, newState)
    isReached = false;
    threshold = 0.1;
    if planner.StateSpace.distance(newState, goalState) < threshold
        isReached = true;
    end
end

Reinicie el generador de números aleatorios para garantizar resultados reproducibles. Planifica la ruta desde el inicio hasta la pose objetivo.

rng default
[pthObj,solnInfo] = plan(planner,start,goal);

Acortar camino

Elimine los nodos redundantes a lo largo de la ruta utilizando la función shortenpath. La función elimina nodos no deseados y genera una ruta libre de colisiones al conectar nodos no secuenciales que no provocan colisiones.

shortenedPath = shortenpath(pthObj,stateValidator);

Calcular la longitud de la ruta original y la ruta acortada.

originalLength = pathLength(pthObj)

originalLength = 
33.8183

shortenedLength = pathLength(shortenedPath)

shortenedLength = 
29.0853

Se puede observar que el acortamiento resultó en una disminución de la longitud de la ruta.

Trazar la ruta original y la ruta acortada

Mostrar el mapa de ocupación. Grafique el árbol de búsqueda desde solnInfo. Interpola y superpone la ruta final.

show(occGrid)
hold on

% Plot entire search tree.
plot(solnInfo.TreeData(:,1),solnInfo.TreeData(:,2),plannerLineSpec.tree{:})

% Interpolate and plot path.
interpolate(pthObj,300)
plot(pthObj.States(:,1),pthObj.States(:,2),plannerLineSpec.path{:})

% Interpolate and plot path.
interpolate(shortenedPath,300);
plot(shortenedPath.States(:,1),shortenedPath.States(:,2),'g-','LineWidth',3)

% Show start and goal in grid map.
plot(start(1),start(2),'ro')
plot(goal(1),goal(2),'mo')
legend('search tree','original path','shortened path')
hold off

Personalice las restricciones del vehículo de Dubins

Para especificar restricciones de vehículo personalizadas, personalice el objeto de espacio de estado. Este ejemplo utiliza ExampleHelperStateSpaceOneSidedDubins, que se basa en la clase stateSpaceDubins. Esta clase auxiliar limita la dirección de giro a la derecha o a la izquierda según una propiedad booleana, GoLeft. Esta propiedad esencialmente deshabilita los tipos de ruta del objeto dubinsConnection.

Cree el objeto de espacio de estados utilizando el ayudante de ejemplo. Especifique los mismos límites de estado y proporcione el nuevo parámetro booleano como true (solo giros a la izquierda).

% Only making left turns
goLeft = true;

% Create the state space
ssCustom = ExampleHelperStateSpaceOneSidedDubins(bounds,goLeft);
ssCustom.MinTurningRadius = 0.4;

Planificar ruta

Cree un nuevo objeto planificador con las restricciones Dubins personalizadas y un validador basado en esas restricciones. Especifique la misma función GoalReached.

stateValidator2 = validatorOccupancyMap(ssCustom); 
stateValidator2.Map = inflatedMap;
stateValidator2.ValidationDistance = 0.05;

planner = plannerRRT(ssCustom,stateValidator2);
planner.MaxConnectionDistance = 2.5;
planner.MaxIterations = 30000;
planner.GoalReachedFcn = @exampleHelperCheckIfGoal;

Planifica la ruta entre el inicio y la meta. Reinicie el generador de números aleatorios nuevamente.

rng default
[pthObj2,solnInfo] = plan(planner,start,goal);

Acortar camino

Acortar la ruta manteniendo las restricciones de movimiento personalizadas.

shortenedPath2 = shortenpath(pthObj2,stateValidator2);

Calcular la longitud de la ruta original y la ruta acortada

originalLength2 = pathLength(pthObj2)

originalLength2 = 
46.7841

shortenedLength2 = pathLength(shortenedPath2)

shortenedLength2 = 
45.7649

Se puede observar que el acortamiento resultó en una disminución de la longitud de la ruta.

Representar la ruta

Para llegar a la meta, el camino ejecuta únicamente giros a la izquierda.

figure
show(occGrid)
hold on
% Interpolate and plot path.
interpolate(pthObj2,300)
h1 = plot(pthObj2.States(:,1),pthObj2.States(:,2),plannerLineSpec.path{:});

% Interpolate and plot path.
interpolate(shortenedPath2,300)
h2 = plot(shortenedPath2.States(:,1),shortenedPath2.States(:,2),'g-','LineWidth',3);

% Show start and goal in grid map.
plot(start(1),start(2),'ro')
plot(goal(1),goal(2),'mo')
legend([h1 h2],'original path','shortened path')
hold off