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 RRT Star Planner

Este ejemplo utiliza:

Abrir script en vivo

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 RRT*. Después de verificar el algoritmo en MATLAB ®, use la app MATLAB Coder (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

Crea una función, codegenRRTStar, que utiliza un objeto plannerRRTStar para planificar una ruta desde la pose inicial hasta la pose objetivo en el mapa.

function [path,treeData] = codegenRRTStar(mapData,startPose,goalPose)
    %#codegen
   
    % Create an occupancy map.
    omap = occupancyMap(mapData);

    % Create a state space object.
    stateSpace = stateSpaceSE2;

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

    % Construct a state validator object using the statespace and map
    % object.
    stateValidator = validatorOccupancyMap(stateSpace,Map=omap);
    
    % Set the validation distance for the validator.
    stateValidator.ValidationDistance = 0.01;
    
    % Create RRT* path planner and allow further optimization after goal is
    % reached. Reduce the maximum iterations and increase the maximum 
    % connection distance.
    planner = plannerRRTStar(stateSpace,stateValidator, ...
                             ContinueAfterGoalReached=true, ...
                             MaxIterations=2500, ...
                             MaxConnectionDistance=1);
    
    % Compute a path for the given start and goal poses.
    [pathObj,solnInfo] = plan(planner,startPose,goalPose);
    
    % Extract the path poses from the path object.
    path = pathObj.States;

    % Extract the tree expansion data from the solution information 
    % structure.
    treeData = solnInfo.TreeData;
end

Esta función actúa como contenedor para una llamada de planificación de ruta RRT* 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 codegenRRTStar en su carpeta actual.

Verificar el algoritmo de planificación de rutas en MATLAB

Verifique el algoritmo de planificación de ruta en MATLAB antes de generar código utilizando el archivo de prueba, codegenRRTStar_testbench.m.

% Generate a random 2-D maze map.
map = mapMaze(5,MapSize=[25 25],MapResolution=1);
mapData = occupancyMatrix(map);

% Define start and goal poses.
startPose = [3 3 pi/2];
goalPose = [22 22 0];

%Plan the path for the specified start pose, and goal pose, and map.
[path,treeData] = codegenRRTStar(mapData,startPose,goalPose);

%Visualize the computed path.
show(occupancyMap(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")
% Tree expansion
plot(treeData(:,1),treeData(:,2),"c.-")
% Path
plot(path(:,1),path(:,2),"r-",LineWidth=2)
legend("Start Pose","Goal Pose","Tree expansion","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 utilizando la appMATLAB Coder™.

Abra la app MATLAB Coder y seleccione los archivos fuente

  1. En la pestaña Aplicaciones de la barra de herramientas MATLAB, en Generación de código, haga clic en el ícono de la app MATLAB Coder. La app abre el cuadro de diálogo Crear proyecto MATLAB Coder.

  2. Proporcione el nombre del archivo del proyecto MATLAB Coder y la carpeta en la que desea colocar el archivo. Se abre la pestaña de barra de herramientas MATLAB Coder y el panel MATLAB Coder.

  3. Haga clic en el botón Agregar puntos de entrada en la sección Entradas del panel MATLAB Coder o en el botón Puntos de entrada en la barra de herramientas. Se abre la pestaña Puntos de entrada.

  4. Introduzca o seleccione el nombre de la función de punto de entrada codegenRRTStar. Una función de punto de entrada es una función de MATLAB de nivel superior a partir de la cual se genera código.

Definir tipos de entrada

  1. Para especificar los tipos de datos de entrada para la función de punto de entrada, utilice Definir automáticamente tipos de entrada. Ingrese o seleccione el archivo de prueba codegenRRTStar_testbench.m.

  2. Haga clic en Ejecutar. El archivo de prueba, codegenRRTStar_testbench.m, llama a la función de punto de entrada, codegenRRTStar, con los tipos de entrada esperados. La app determina que la entrada mapData es lógica (25x25), la entrada startPose es doble (1x3) y la entrada goalPose es doble (1x3).

Verificar problemas de tiempo de ejecución

  1. Verifique la función de punto de entrada para detectar problemas que surgen en el tiempo de ejecución generando y ejecutando una función MEX. Para hacer esto, haga clic en Ejecutar MEX generado en la barra de herramientas y proporcione un script que ejercite su función de punto de entrada. Para este ejemplo, utilice el archivo de prueba codegenRRTStar_testbench que utilizó para definir los tipos de entrada.

  2. La app genera y ejecuta automáticamente una función MEX. Ejecuta el script de prueba codegenRRTStar_testbench reemplazando las llamadas a codegenRRTStar con llamadas al MEX generado. Si la app detecta problemas durante la generación o ejecución de la función MEX, proporciona mensajes de advertencia y error. Haga clic en estos mensajes para navegar hasta el código problemático y solucionar el problema. En este ejemplo, la app no detecta problemas.

Generar archivo MEX

  1. En la barra de herramientas, configure Tipo de salida en MEX y Idioma en C. Use los valores predeterminados para las demás configuraciones de compilación del proyecto. En lugar de generar un MEX, puede optar por generar tipos de compilación C/C++. Hay diferentes configuraciones de proyecto disponibles para los tipos de compilación MEX y C/C++. Cuando cambia entre la generación de código MEX y C/C++, verifique la configuración que elija.

  2. Haga clic en Generar código > Generar código y compilar. MATLAB Coder genera un archivo MEX codegenRRTStar_mex en su carpeta actual. La sección Salida del panel MATLAB Coder indica que la generación de código se realizó correctamente y muestra enlaces a la carpeta de salida generada y al informe de generación de código.

  3. Haga clic en Informe de generación de código para ver el informe en el Visor de informes. Si el generador de código detecta errores o advertencias durante la generación del código, el informe describe los problemas y proporciona enlaces al código MATLAB problemático. Para obtener más información, consulte Code Generation Reports (MATLAB Coder).

Para obtener información adicional sobre las capacidades de MATLAB Coder y los archivos que genera, consulte Generate C Code by Using the MATLAB Coder App (MATLAB Coder).

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 = codegenRRTStar_mex(mapData,startPose,goalPose);

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

plot(mexPath(:,1),mexPath(:,2),"b:",LineWidth=2)
legend("Start Pose","Goal Pose","Tree expansion","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 utilizando timeit.

time = timeit(@() codegenRRTStar(mapData,startPose,goalPose))
time = 
              1.5130424783


mexTime = timeit(@() codegenRRTStar_mex(mapData,startPose,goalPose))
mexTime = 
              0.1934125783


time/mexTime
ans = 
          7.82287528349442

En este ejemplo, la función MEX se ejecuta más de siete 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 0];

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

[pathNew,treeDataNew] = codegenRRTStar_mex(mapDataNew,startPoseNew,goalPoseNew);

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

show(occupancyMap(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")
% Tree expansion
plot(treeDataNew(:,1),treeDataNew(:,2),"c.-")
% Path
plot(pathNew(:,1),pathNew(:,2),"r-",LineWidth=2)
legend("Start Pose","Goal Pose","Tree expansion","MEX Generated Path")
legend(Location='northeast')