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 existente de un espacio de oficina pequeño. Traza las posiciones inicial y final del vehículo en la parte superior del 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 headings.
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

Definir espacio de estados

Especifique el espacio de estado del vehículo utilizando un objeto stateSpaceDubins y especificando los límites del 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 = [occGrid.XWorldLimits; occGrid.YWorldLimits; [-pi pi]];

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

Planifica la ruta

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.

Cree 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 = occGrid;
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.0;
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

Planifica la ruta entre el inicio y la meta. Reinicie el generador de números aleatorios para obtener resultados reproducibles.

rng default

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

Trazar el camino

Mostrar el mapa de ocupación. Traza 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),'.-');

% Interpolate and plot path.
interpolate(pthObj,300)
plot(pthObj.States(:,1),pthObj.States(:,2),'r-','LineWidth',2)

% Show start and goal in grid map.
plot(start(1),start(2),'ro')
plot(goal(1),goal(2),'mo')
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 hacia la derecha o hacia 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 = occGrid;
stateValidator2.ValidationDistance = 0.05;

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

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

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

Representar la ruta

Dibuja la ruta nueva en el mapa. La ruta sólo debe ejecutar giros a la izquierda para llegar a la meta.

figure
show(occGrid)

hold on

% Show the search tree.
plot(solnInfo.TreeData(:,1),solnInfo.TreeData(:,2),'.-');

% Interpolate and plot path.
pthObj2.interpolate(300)
plot(pthObj2.States(:,1), pthObj2.States(:,2), 'r-', 'LineWidth', 2)

% Show start and goal in grid map.
plot(start(1), start(2), 'ro')
plot(goal(1), goal(2), 'mo')
hold off