Main Content

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

Cree una función, codegenRRTStar, que utilice 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 usando 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 app MATLAB Coder .

Abra la app MATLAB Coder y seleccione los archivos fuente

1. En la pestaña Apps de la tira de herramientas de MATLAB , en Generación de código, haga clic en el icono de la app MATLAB Coder. La app abre la página Seleccionar archivos fuente.

2. En la página Seleccionar archivos fuente, ingrese 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. La app crea un proyecto con el nombre predeterminado codegenRRTStar.prj en la carpeta actual.

3. Haga clic en Siguiente para ir a la página Definir tipos de entrada.

Definir tipos de entrada

1. Ingrese o seleccione el archivo de prueba codegenRRTStar_testbench.m.

2. Haga clic en Autodefinir tipos de entrada. 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).

3. Haga clic en Siguiente para ir al paso Verificar problemas de tiempo de ejecución.

Verificar problemas de tiempo de ejecución

1. En el cuadro de diálogo Verificar problemas de tiempo de ejecución, especifique un archivo de prueba o ingrese un código que llame a la función de punto de entrada con entradas de ejemplo. Para este ejemplo, utilice el archivo de prueba codegenRRTStar_testbench que utilizó para definir los tipos de entrada.

2. Haga clic en Buscar problemas. La app genera una función MEX. Ejecuta el script de prueba codegenRRTStar_testbench reemplazando 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.

3. Haga clic en Siguiente para ir al paso Generar código.

Generar archivo MEX

1. En el cuadro de diálogo Generar código, establezca Tipo de compilación en MEX y Idioma en C. Utilice los valores predeterminados para los demás ajustes de configuración 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.

MATLAB Coder genera un archivo MEX codegenRRTStar_mex en su carpeta actual. La app MATLAB Coder indica que la generación del código se realizó correctamente. Muestra los archivos MATLAB de origen y los archivos de salida generados en el lado izquierdo de la página. En la pestaña Variables, muestra información sobre las variables fuente de MATLAB . En la pestaña Registro de compilación de destino, muestra el registro de compilación, incluidas las advertencias y errores del compilador C/C++. De forma predeterminada, en la ventana de código, la app muestra el archivo de código fuente C, codegenRRTStar.c. Para ver un archivo diferente, en el panel Código fuente o Archivos de salida, haga clic en el nombre del archivo.

3 Haga clic en Ver informe 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).

4 Haga clic en Siguiente para abrir la página Finalizar flujo de trabajo.

Revisar la página Finalizar flujo de trabajo

La página Finalizar flujo de trabajo indica que la generación del código se realizó correctamente. Proporciona un resumen del proyecto y enlaces a los resultados generados.

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 usando 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')