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

Genere código para la planificación de rutas utilizando Hybrid A Star

Este ejemplo muestra cómo realizar la generación de código para planificar una ruta libre de colisiones para un vehículo a través de un mapa utilizando el algoritmo Hybrid A*. Después de verificar el algoritmo en MATLAB®, use la función codegen (MATLAB Coder) para generar una función MEX. Utilice el archivo MEX generado en el algoritmo para visualizar la ruta planificada.

Escribir algoritmo para planificar la ruta

Cree una función, codegenPathPlanner, que utilice un objeto plannerHybridAStar para planificar una ruta desde la pose inicial hasta la pose objetivo en el mapa.

function path = codegenPathPlanner(mapData,startPose,goalPose)
    %#codegen
       
    % Create a binary occupancy map
    map = binaryOccupancyMap(mapData);

    % Create a state space object
    stateSpace = stateSpaceSE2;

    % Update state space bounds to be the same as map limits.
    stateSpace.StateBounds = [map.XWorldLimits;map.YWorldLimits;[-pi pi]];

    % Construct a state validator object using the statespace and map object
    validator = validatorOccupancyMap(stateSpace,Map=map);
    
    % Set the validation distance for the validator
    validator.ValidationDistance = 0.01;
    
    % Assign the state validator object to the plannerHybridAStar object
    planner = plannerHybridAStar(validator);
    
    % Compute a path for the given start and goal poses
    pathObj = plan(planner,startPose,goalPose);
    
    % Extract the path poses from the path object
    path = pathObj.States;

end

Esta función actúa como contenedor para una llamada de planificación de ruta híbrida A* estándar. Acepta entradas estándar y devuelve una ruta libre de colisiones como un arreglo. Como no puede utilizar un objeto de identificador como entrada o salida de una función compatible con la generación de código, cree el objeto planificador dentro de la función. Guarde la función codegenPathPlanner en su carpeta actual.

Verificar el algoritmo de planificación de rutas en MATLAB

Verifique el algoritmo de planificación de rutas en MATLAB antes de generar código.

Genere el mapa de un laberinto 2D aleatorio.

map = mapMaze(5,MapSize=[25 25],MapResolution=1);
mapData = occupancyMatrix(map);

Defina las poses inicial y meta como vectores [x y theta]. x e y especifican la posición en metros, y theta especifica el ángulo de orientación en radianes.

startPose = [3 3 pi/2];
goalPose = [22 22 pi/2];

Planifique la ruta para la pose inicial, la pose objetivo y el mapa especificados.

path = codegenPathPlanner(mapData,startPose,goalPose);

Visualice la ruta calculada.

show(binaryOccupancyMap(mapData))
hold on
% Start state
scatter(startPose(1,1),startPose(1,2),"g","filled")
% Goal state
scatter(goalPose(1,1),goalPose(1,2),"r","filled")
% Path
plot(path(:,1),path(:,2),"r-",LineWidth=2)
legend("Start Pose","Goal Pose","MATLAB Generated Path")
legend(Location="northwest")

Generar código para el algoritmo de planificación de rutas

Puede utilizar la función codegen (MATLAB Coder) o la app MATLAB Coder (MATLAB Coder) para generar código. Para este ejemplo, genere un archivo MEX llamando a codegen en la línea de comando de MATLAB . Especifique argumentos de entrada de muestra para cada entrada de la función utilizando la opción -args y el argumento de entrada func_inputs .

Llame a la función codegen y especifique los argumentos de entrada en un arreglo de celdas. Esta función crea una función codegenPathPlanner_mex separada para usar. También puede producir código C utilizando el argumento de entrada options . Este paso puede llevar algún tiempo.

codegen -config:mex codegenPathPlanner -args {mapData,startPose,goalPose}

Code generation successful.

Verificar resultados utilizando la función MEX generada

Planifique la ruta llamando a la versión MEX del algoritmo de planificación de ruta para la pose inicial, la pose objetivo y el mapa especificados.

mexPath = codegenPathPlanner_mex(mapData,startPose,goalPose);

Visualice la ruta calculada por la versión MEX del algoritmo de planificación de ruta.

scatter(mexPath(:,1),mexPath(:,2),...
        Marker="o",...
        MarkerFaceColor="b",...
        MarkerEdgeColor="b")
legend("Start Pose","Goal Pose","MATLAB Generated Path","MEX Generated Path")
legend(Location="northwest")
hold off

Verificar el rendimiento del código generado

Compare el tiempo de ejecución de la función MEX generada con el tiempo de ejecución de su función original usando timeit.

time = timeit(@() codegenPathPlanner(mapData,startPose,goalPose))

time = 0.3735

mexTime = timeit(@() codegenPathPlanner_mex(mapData,startPose,goalPose))

mexTime = 0.0474

time/mexTime

ans = 7.8872

En este ejemplo, la función MEX se ejecuta más de cinco veces más rápido. Los resultados pueden variar para su sistema.

Planificar ruta en un nuevo mapa usando la función MEX generada

Planifica una ruta para un nuevo comienzo y plantea objetivos en un nuevo mapa. El tamaño del nuevo mapa debe ser el mismo que el mapa utilizado para generar la función MEX.

Genere el mapa de un laberinto 2D aleatorio.

mapNew = mapMaze(5,MapSize=[25 25],MapResolution=1);
mapDataNew = occupancyMatrix(mapNew);

Especifique las poses de inicio y objetivo.

startPoseNew = [22 3 pi/2];
goalPoseNew = [3 22 pi/2];

Planifique la ruta para la pose inicial, la pose objetivo y el mapa especificados.

pathNew = codegenPathPlanner_mex(mapDataNew,startPoseNew,goalPoseNew);

Visualice la nueva ruta calculada por la función MEX.

show(binaryOccupancyMap(mapDataNew))
hold on
% Start state
scatter(startPoseNew(1,1),startPoseNew(1,2),"g","filled")
% Goal state
scatter(goalPoseNew(1,1),goalPoseNew(1,2),"r","filled")
% Path
plot(pathNew(:,1),pathNew(:,2),"r-",LineWidth=2)
legend("Start Pose","Goal Pose","MEX Generated Path")
legend(Location='northeast')