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

Planificadores de rutas de referencia para robots de accionamiento diferencial en el mapa del almacén

Este ejemplo muestra cómo elegir el mejor planificador de rutas 2-D para un robot de accionamiento diferencial en un entorno de almacén entre los planificadores de rutas disponibles. Utilice el objeto plannerBenchmark para comparar los planificadores de rutas plannerRRT, plannerRRTStar, plannerBiRRT, plannerPRM y plannerHybridAstar en el entorno del almacén con las posiciones de inicio y destino elegidas aleatoriamente. Compare los planificadores de rutas en función de su capacidad para encontrar una ruta válida, la distancia a los obstáculos, el tiempo necesario para inicializar un planificador, el tiempo necesario para encontrar una ruta, la longitud de la ruta y la suavidad de la ruta. Se elige un planificador adecuado en función del desempeño de cada planificador de ruta en las métricas mencionadas anteriormente.

Configurar el entorno

Crea un objeto binaryOccupancyMap a partir de un mapa de almacén. En el almacén, el robot móvil se desplaza desde la estación de carga a la estación de descarga para colocar la mercancía.

map = load("wareHouseMap.mat").logicalMap;
map = binaryOccupancyMap(map);

Visualiza el mapa.

figure
show(map)
title("Warehouse Floor Plan")

% Set the location of text displayed on the map.
loadingStationTextLoc = [40 9 0];
unloadingStationTextLoc = [7.5 42 0];
hold on
text(loadingStationTextLoc(1),loadingStationTextLoc(2),1,"Loading Station");
text(unloadingStationTextLoc(1),unloadingStationTextLoc(2),1,"Unloading Stations");
hold off

Definir el entorno y los planificadores para la evaluación comparativa

Especifique el radio de giro mínimo para el robot de accionamiento diferencial. Se impone una restricción de radio de giro mínimo para tener una trayectoria relativamente más suave para el robot.

minTurningRadius = 1.4; % In meters.

Cree un objeto stateSpaceDubins con límites de espacio de estado que sean los mismos que los límites del mapa. Establezca el radio de giro mínimo.

stateSpace = stateSpaceDubins([map.XWorldLimits; map.YWorldLimits; [-pi pi]]);
stateSpace.MinTurningRadius = minTurningRadius;

Cree un validador de estado validatorOccupancyMap con el espacio de estado de Dubins usando el mapa. Especifique la distancia para interpolar y validar segmentos de ruta.

validator = validatorOccupancyMap(stateSpace,Map=map);
validator.ValidationDistance = 0.01*(1/map.Resolution); % In meters.

Defina los identificadores de funciones para las funciones de inicialización de cada planificador. Para obtener más información sobre estas funciones de inicialización, consulte Funciones de inicialización para planificadores.

rrtInit = @(validator) plannerRRTWrapper(validator);
rrtStarInit = @(validator) plannerRRTStarWrapper(validator);
birrtInit = @(validator) plannerBiRRTWrapper(validator);
haStarInit = @(validator) plannerHybridAStarWrapper(validator,minTurningRadius);
prmInit = @(validator) plannerPRM(validator.StateSpace,validator,MaxNumNodes=80);

Defina el identificador de función para la función del plan, que es común para todos los planificadores.

planFcn = @(initOutput,start,goal) plan(initOutput,start,goal);

Seleccione aleatoriamente pares de inicio-objetivo en el mapa del almacén

Las ubicaciones de inicio y meta se toman aleatoriamente del área de la estación de carga y descarga, respectivamente. Especifique el número de pares inicio-meta que se deben generar aleatoriamente. En este ejemplo, solo se eligen tres pares inicio-meta para reducir el tiempo de ejecución de este ejemplo. Aumente el número del par inicio-meta para obtener suficiente cobertura del mapa.

% Set default random number for repeatability of results.
rng("default")

% Select the number of start-goal pairs.
numStartGoalPairs = 3;

La ubicación inicial del robot se toma como muestra del área del rectángulo marcada como estación de carga y la ubicación de destino se toma como muestra del área del rectángulo marcada como área de la estación de descarga. Las ubicaciones de los robots se muestrean uniformemente en el área marcada. El área del rectángulo se especifica como un vector con la forma [x y w h]. x y y especifican la coordenada de la esquina inferior izquierda del rectángulo. w y h especifican el ancho y la altura del rectángulo.

loadingArea = [51.5 11 5 10];
startLocations = helperSampleSelectedAreaOnMap(validator,loadingArea,numStartGoalPairs);

unloadingArea = [2 43.5 27 15];
goalLocations = helperSampleSelectedAreaOnMap(validator,unloadingArea,numStartGoalPairs);

Visualice el mapa con todas las ubicaciones de inicio y meta seleccionadas al azar.

show(map)
title("Warehouse Floor Plan")

% Indicate the loading and unloading station on the map.
hold on
text(loadingStationTextLoc(1),loadingStationTextLoc(2),1,"Loading Station");
rectangle(Position=loadingArea)
text(unloadingStationTextLoc(1),unloadingStationTextLoc(2),1,"Unloading Stations");
rectangle(Position=unloadingArea)

% Set the length of the line representing the pose in the start-goal visualization.
r = 2;

% Display all the start-goal pairs on the map.
for i=1:numStartGoalPairs
    start = startLocations(i,:);
    goal = goalLocations(i,:);    
    
    % Define the legend displayed on the map.    
    startString = strcat("start location",num2str(i));
    goalString = strcat("goal location",num2str(i));
    
    % Display start and goal location of robot.
    p1 = plot(start(1,1),start(1,2),"o",DisplayName=startString);
    c1 = p1.Color;
    p2 = plot(goal(1,1),goal(1,2),"o",DisplayName=goalString);
    c2 = p2.Color;
    
    % Display start and goal headings.
    plot([start(1) start(1)+r*cos(start(3))],[start(2) start(2)+r*sin(start(3))],...
         "-",Color=c1,HandleVisibility="off")
    plot([goal(1) goal(1)+r*cos(goal(3))],[goal(2) goal(2)+r*sin(goal(3))],...
         "-",Color=c2,HandleVisibility="off")       
end
hold off
legend(Location="northeastoutside")

Punto de referencia del planificador Creación de objetos y punto de referencia en ejecución

Cree un objeto plannerBenchmark para cada par inicio-objetivo y agregue los planificadores para la evaluación comparativa. El planificador se ejecuta dos veces en el mismo entorno y par inicio-objetivo estableciendo el valor runCount en 2. Esto garantiza que los resultados de las métricas sean precisos ya que los planificadores basados en muestreo como plannerRRT, plannerRRTStar, plannerBiRRT y plannerPRM producen resultados diferentes en el mismo conjunto de pares inicio-objetivo. En este ejemplo, el valor runCount se establece en 2 para reducir el tiempo de ejecución de este ejemplo. Aumente el valor runCount para obtener resultados de referencia más precisos.

% To store the generated benchmark objects for each start-goal pair.
benchmarkList = cell(1,numStartGoalPairs);

% Specify the number of times each planner to be executed on the same
% set of start-goal pair.
runCount = 2;
for i=1:size(startLocations,1)
    
    % Get each start and goal location from all the sampled locations.
    start = startLocations(i,:);
    goal = goalLocations(i,:);
    
    % Set default random number for repeatability of results.
    rng("default")

    % Construct benchmark object.
    benchmark = plannerBenchmark(validator,start,goal);
    
    % Add planners for benchmarking using initialization and plan function
    % handles. Additional optional input NumPlanOutput define the number of
    % outputs returned from the plan function.
    addPlanner(benchmark,planFcn,rrtInit,PlannerName="rrt",NumPlanOutput=2)
    addPlanner(benchmark,planFcn,rrtStarInit,PlannerName="rrtStar",NumPlanOutput=2)
    addPlanner(benchmark,planFcn,birrtInit,PlannerName="biRRT",NumPlanOutput=2)
    addPlanner(benchmark,planFcn,prmInit,PlannerName="PRM",NumPlanOutput=2)
    addPlanner(benchmark,planFcn,haStarInit,PlannerName="hybridAstar",NumPlanOutput=2)    
    
    % Run the benchmark.
    runPlanner(benchmark,runCount)

    % Store the benchmark for further analysis.
    benchmarkList{i} = benchmark;
end

Initializing rrt ...
Done.
Planning a path from the start pose (55.5736 20.1338 1.2229) to the goal pose (27.9063 56.2369 1.9757) using rrt.
Executing run 1.
Executing run 2.
Initializing rrtStar ...
Done.
Planning a path from the start pose (55.5736 20.1338 1.2229) to the goal pose (27.9063 56.2369 1.9757) using rrtStar.
Executing run 1.
Executing run 2.
Initializing biRRT ...
Done.
Planning a path from the start pose (55.5736 20.1338 1.2229) to the goal pose (27.9063 56.2369 1.9757) using biRRT.
Executing run 1.
Executing run 2.
Initializing PRM ...
Done.
Planning a path from the start pose (55.5736 20.1338 1.2229) to the goal pose (27.9063 56.2369 1.9757) using PRM.
Executing run 1.
Executing run 2.
Initializing hybridAstar ...
Done.
Planning a path from the start pose (55.5736 20.1338 1.2229) to the goal pose (27.9063 56.2369 1.9757) using hybridAstar.
Executing run 1.
Executing run 2.
Initializing rrt ...
Done.
Planning a path from the start pose (56.029 17.3236 1.6444) to the goal pose (19.705 57.5099 1.9527) using rrt.
Executing run 1.
Executing run 2.
Initializing rrtStar ...
Done.
Planning a path from the start pose (56.029 17.3236 1.6444) to the goal pose (19.705 57.5099 1.9527) using rrtStar.
Executing run 1.
Executing run 2.
Initializing biRRT ...
Done.
Planning a path from the start pose (56.029 17.3236 1.6444) to the goal pose (19.705 57.5099 1.9527) using biRRT.
Executing run 1.
Executing run 2.
Initializing PRM ...
Done.
Planning a path from the start pose (56.029 17.3236 1.6444) to the goal pose (19.705 57.5099 1.9527) using PRM.
Executing run 1.
Executing run 2.
Initializing hybridAstar ...
Done.
Planning a path from the start pose (56.029 17.3236 1.6444) to the goal pose (19.705 57.5099 1.9527) using hybridAstar.
Executing run 1.
Executing run 2.
Initializing rrt ...
Done.
Planning a path from the start pose (52.1349 11.9754 2.2894) to the goal pose (19.6979 43.9775 0.93797) using rrt.
Executing run 1.
Executing run 2.
Initializing rrtStar ...
Done.
Planning a path from the start pose (52.1349 11.9754 2.2894) to the goal pose (19.6979 43.9775 0.93797) using rrtStar.
Executing run 1.
Executing run 2.
Initializing biRRT ...
Done.
Planning a path from the start pose (52.1349 11.9754 2.2894) to the goal pose (19.6979 43.9775 0.93797) using biRRT.
Executing run 1.
Executing run 2.
Initializing PRM ...
Done.
Planning a path from the start pose (52.1349 11.9754 2.2894) to the goal pose (19.6979 43.9775 0.93797) using PRM.
Executing run 1.
Executing run 2.
Initializing hybridAstar ...
Done.
Planning a path from the start pose (52.1349 11.9754 2.2894) to the goal pose (19.6979 43.9775 0.93797) using hybridAstar.
Executing run 1.
Executing run 2.

Métricas promedio en todos los pares inicio-objetivo

Todos los planificadores se ejecutan runCount veces para cada par inicio-objetivo. Además, se ejecutan todos los planificadores para todos los pares inicio-meta. Esto significa que todos los planificadores se ejecutan runCount*numStartGoalPairs veces. Los gráficos y tablas a continuación muestran el valor métrico promedio en todos los pares de inicio-meta. Las tablas representan la media, la mediana y la desviación estándar de cada métrica promediadas en todos los pares inicio-meta.

La métrica clearance representa la distancia mínima de la ruta desde los obstáculos del entorno. El gráfico muestra que plannerPRM tiene el espacio libre más alto.

helperPlotAveragedMetrics(benchmarkList,runCount,"clearance")

clearanceAverage = helperCalculateAverageMetricTable(benchmarkList,"clearance")

clearanceAverage=5×3 table
                    Mean     Median     stdDev 
                   ______    ______    ________

    rrt             1.069      1       0.097631
    rrtStar             1      1              0
    biRRT               1      1              0
    PRM            1.3333      1         0.4714
    hybridAstar         1      1              0

La métrica isPathValid representa la tasa de éxito de cada planificador expresada en porcentaje. El gráfico muestra que todos los planificadores de rutas produjeron una ruta válida para todos los pares inicio-meta.

helperPlotAveragedMetrics(benchmarkList,runCount,"isPathValid")

isPathValidAverage = helperCalculateAverageMetricTable(benchmarkList,"isPathValid")

isPathValidAverage=5×3 table
                   Mean    Median    stdDev
                   ____    ______    ______

    rrt            100      100        0   
    rrtStar        100      100        0   
    biRRT          100      100        0   
    PRM            100      100        0   
    hybridAstar    100      100        0   

La métrica executionTime representa el tiempo que tarda la función del plan en ejecutarse. La gráfica muestra que plannerBiRRT tomó el menor tiempo. plannerRRTStar tomó el máximo tiempo ya que la propiedad ContinueAfterGoalReached está habilitada y el planificador intenta encontrar rutas óptimas.

helperPlotAveragedMetrics(benchmarkList,runCount,"executionTime")

execTimeAverage = helperCalculateAverageMetricTable(benchmarkList,"executionTime")

execTimeAverage=5×3 table
                     Mean       Median      stdDev  
                   ________    ________    _________

    rrt             0.19835     0.14587     0.090927
    rrtStar          1.0908      1.0811     0.051066
    biRRT          0.028399    0.027958    0.0061547
    PRM            0.026128    0.014155     0.017666
    hybridAstar     0.11892     0.12376     0.082961

La métrica initializationTime indica el tiempo que lleva ejecutar la función de inicialización de cada planificador. plannerPRM tiene el tiempo de inicialización más largo y plannerBiRRT tiene el menor tiempo de ejecución.

El tiempo total de ejecución es la suma de initializationTime y executionTime.

helperPlotAveragedMetrics(benchmarkList,runCount,"initializationTime")

initTimeAverage = helperCalculateAverageMetricTable(benchmarkList,"initializationTime")

initTimeAverage=5×3 table
                     Mean       Median      stdDev  
                   ________    ________    _________

    rrt            0.012932    0.012481    0.0051273
    rrtStar        0.013889     0.01755    0.0073465
    biRRT          0.012578    0.006345     0.010121
    PRM              1.3445       1.443      0.31218
    hybridAstar    0.017089    0.009796     0.013259

La métrica pathLength representa la longitud de la ruta generada. plannerHybridAstar tiene la ruta más corta, seguida de plannerRRTStar y plannerBiRRT con una longitud de ruta promedio similar.

helperPlotAveragedMetrics(benchmarkList,runCount,"pathLength")

pathLengthAverage = helperCalculateAverageMetricTable(benchmarkList,"pathLength")

pathLengthAverage=5×3 table
                    Mean     Median    stdDev
                   ______    ______    ______

    rrt             65.44    65.103    8.8336
    rrtStar        61.179    61.903    6.2276
    biRRT            61.6    63.412     4.743
    PRM            87.775    70.339     26.23
    hybridAstar     52.24    49.248    5.2636

La métrica smoothness representa la suavidad de la ruta para todas las poses. plannerBiRRT produjo la ruta más suave (un valor de suavidad más bajo indica una ruta más suave), seguida de plannerPRM. Observe que plannerRRTStar produjo una ruta considerablemente más suave en comparación con plannerRRT.

helperPlotAveragedMetrics(benchmarkList,runCount,"smoothness")

smoothnessAverage = helperCalculateAverageMetricTable(benchmarkList,"smoothness")

smoothnessAverage=5×3 table
                    Mean       Median      stdDev 
                   _______    ________    ________

    rrt            0.92635     0.77845      0.2669
    rrtStar        0.53077      0.5488    0.066147
    biRRT          0.10321     0.10285    0.006485
    PRM            0.19031    0.082896     0.18029
    hybridAstar     1.7269      1.6006     0.44726

Si la ruta ligeramente más larga no es un problema, considere usar plannerBiRRT para la planificación de ruta en una configuración de mapa de almacén como se muestra en este ejemplo, ya que tiene la ruta más suave y el menor tiempo de ejecución. Si el camino recorrido por el robot fuera el menor entonces se podría considerar plannerHybridAstar.

Aumentar el número de pares inicio-objetivo y runCount a un valor considerablemente mayor producirá resultados de métricas más precisos y un mejor criterio sobre qué planificador de ruta elegir.

Visualice resultados de métricas para un par de inicio-meta específico

Las tablas y gráficos anteriores mostraron los resultados métricos promediados en todos los pares inicio-objetivo. plannerBiRRT produjo una ruta más suave en general, pero la longitud de la ruta fue levemente más larga que plannerHybridAstar, que produjo la menor longitud de ruta. Pruebe los pares inicio-objetivo para ver qué par inicio-objetivo produjo la longitud de ruta más larga para plannerBiRRT.

pathLengthMean = zeros(1,numStartGoalPairs);

% Iterate over all the benchmarks and store the mean path length for each
% start-goal pair.
for i=1:numStartGoalPairs
    benchmark = benchmarkList{i};
    pathLengthTable= benchmark.metric("pathLength");
    pathLengthMean(i) = pathLengthTable.Mean("biRRT"); 
end

% Find the index of the benchmark which produced largest mean path length
% value for plannerBiRRT.
[~,largestPathLengthIdx] = max(pathLengthMean);
benchmarkLargestPathlength = benchmarkList{largestPathLengthIdx};
show(benchmarkLargestPathlength,"pathLength")

pathLength = metric(benchmarkLargestPathlength,"pathLength")

pathLength=5×4 table
                    Mean     Median    StdDev    sampleSize
                   ______    ______    ______    __________

    rrt            76.424    76.424    1.9865        2     
    rrtStar        68.419    68.419    4.7522        2     
    biRRT          66.287    66.287    1.7031        2     
    PRM            70.339    70.339         0        2     
    hybridAstar    59.639    59.639         0        2     

Visualizar la ruta de todos los planificadores para un par de objetivos de inicio específicos

Visualice la ruta de salida de todos los planificadores para el par inicio-objetivo que produjo la ruta más larga para plannerBiRRT.

% Retrieve the start and goal location from the benchmark object.
start = benchmarkLargestPathlength.Start;
goal = benchmarkLargestPathlength.Goal;

% Display the path from start location to goal location for all the path
% planners for all the runs in runCount.
for run=1:runCount
    figure
    show(map)
    title(['Path output from all planners for Run ' num2str(run)])
    hold on
    
    % Show start and goal positions of robot.
    plot(start(1,1),start(1,2),"o")
    plot(goal(1,1),goal(1,2),"o")
    
    % Find the planner names used for benchmarking.
    plannerNames = fieldnames(benchmarkLargestPathlength.PlannerOutput);
    numPlanners = length(plannerNames);
    runString = strcat("Run",num2str(run));
    
    % Iterate and plot path of each planner for the specified run.
    for i=1:numPlanners
        plannerName = plannerNames{i};
        plannerOutput = benchmarkLargestPathlength.PlannerOutput.(plannerName).PlanOutput.(runString);
        pathObj = plannerOutput{1};
        
        % Insert more states in the path to visualize the dubins curve. 
        interpolate(pathObj,200);
        plot(pathObj.States(:,1),pathObj.States(:,2),"-","LineWidth",2)
    end
    
    % Specify the legends.
    labels = [{"start location","goal location"} plannerNames'];
    legend(labels,location="northeastoutside")  
    hold off
end

Evaluación adicional

De los resultados se puede observar que la ruta generada por todos los planificadores tiene nodos redundantes. Puede eliminar estos nodos redundantes utilizando la función shortenpath. La función shortenpath refina las rutas eliminando nodos redundantes y también reduce significativamente la longitud de la ruta. Para evaluar los planificadores de rutas después de eliminar los nodos redundantes, reemplace la entrada del identificador de función planFcn en la función addPlanner con el siguiente código.

planFcnWithShorten = @(initOutput,start,goal) planWithShorten(initOutput,start,goal);

% To store the generated benchmark objects for each start-goal pair.
rrtBenchmarkList = cell(1,numStartGoalPairs);

% Specify the number of times each planner to be executed on the same
% set of start-goal pair.
rrtRunCount = 2;

Este ejemplo muestra el resultado de refinar la ruta generada por el planificador de ruta RRT utilizando la función shortenpath. Este resultado se compara luego con la ruta producida por el planificador de rutas RRTStar.

for i=1:height(startLocations)
    
    % Get each start and goal location from all the sampled locations.
    start = startLocations(i,:);
    goal = goalLocations(i,:);

    % Construct benchmark object.
    benchmark = plannerBenchmark(validator,start,goal);
    
    % Add planners for benchmarking using initialization and plan function
    % handles. Additional optional input NumPlanOutput define the number of
    % outputs returned from the plan function.
    addPlanner(benchmark,planFcnWithShorten,rrtInit,PlannerName="rrtWithShorten",NumPlanOutput=2)
    addPlanner(benchmark,planFcn,rrtStarInit,PlannerName="rrtStar",NumPlanOutput=2)
    
    % Run the benchmark.
    runPlanner(benchmark,rrtRunCount, Verbose="off")

    % Store the benchmark for further analysis.
    rrtBenchmarkList{i} = benchmark;
end

Métricas de la trama

helperPlotAveragedMetrics(rrtBenchmarkList,rrtRunCount,"pathLength")

rrtClearanceAvg = helperCalculateAverageMetricTable(rrtBenchmarkList,"pathLength")

rrtClearanceAvg=2×3 table
                       Mean     Median    stdDev
                      ______    ______    ______

    rrtWithShorten    53.842    52.574    5.0681
    rrtStar           64.576    62.545    5.0423

helperPlotAveragedMetrics(rrtBenchmarkList,rrtRunCount,"executionTime")

rrtExecTimeAvg = helperCalculateAverageMetricTable(rrtBenchmarkList,"executionTime") 

rrtExecTimeAvg=2×3 table
                       Mean      Median      stdDev 
                      _______    _______    ________

    rrtWithShorten    0.17628    0.14177      0.1148
    rrtStar            1.1045     1.1252    0.032152

helperPlotAveragedMetrics(rrtBenchmarkList,rrtRunCount,"smoothness")

rrtSmoothnessAvg = helperCalculateAverageMetricTable(rrtBenchmarkList,"smoothness")

rrtSmoothnessAvg=2×3 table
                        Mean        Median      stdDev  
                      _________    ________    _________

    rrtWithShorten    0.0039555    0.004813    0.0015887
    rrtStar             0.76739     0.75112     0.069996

Si bien se sabe que RRTStar genera rutas más suaves que RRT, implica un mayor tiempo de ejecución. Al utilizar la función shortenpath en las rutas generadas por RRT, se reduce la longitud de la ruta y se logra una suavidad mejor que RRTStar, pero con un tiempo de ejecución significativamente menor.

Sin embargo, es importante tener en cuenta que para un mayor número de iteraciones, plannerRRTStar puede converger a una ruta óptima, mientras que plannerRRT con acortamiento no garantiza una ruta óptima.

Funciones de inicialización para planificadores

Función de inicialización para plannerHybridAStar.

function planner = plannerHybridAStarWrapper(validator,minTurningRadius)
    map = validator.Map;
    ss = stateSpaceSE2;
    sv = validatorOccupancyMap(ss,Map=map);
    sv.ValidationDistance = validator.ValidationDistance;
    planner = plannerHybridAStar(sv,MinTurningRadius=minTurningRadius);
end

Función de inicialización para plannerBiRRT.

function planner = plannerBiRRTWrapper(sv)
    planner = plannerBiRRT(sv.StateSpace,sv);
    planner.EnableConnectHeuristic = true;
    planner.MaxIterations = 5e4;
    planner.MaxNumTreeNodes = 5e4;
    planner.MaxConnectionDistance = 5;
end

Función de inicialización para plannerRRTStar.

function planner = plannerRRTStarWrapper(sv)
    planner = plannerRRTStar(sv.StateSpace,sv);
    planner.MaxIterations = 2.5e3;
    planner.MaxNumTreeNodes = 2.5e3;
    planner.MaxConnectionDistance = 3;
    planner.ContinueAfterGoalReached = true;
end

Función de inicialización para plannerRRT.

function planner = plannerRRTWrapper(sv)
    planner = plannerRRT(sv.StateSpace,sv);
    planner.MaxIterations = 2.5e3;
    planner.MaxNumTreeNodes = 2.5e3;
    planner.MaxConnectionDistance = 3;
end

Función de planificación para plannerRRT con shortenpath

function [shortenedPath,solnInfo] = planWithShorten(planner, start, goal)
    [path,solnInfo] = plan(planner, start, goal);
    shortenedPath = shortenpath(path, planner.StateValidator);
end