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

Suavizado de ruta basado en optimización para vehículos autónomos

Este ejemplo le muestra cómo optimizar la trayectoria de un robot similar a un automóvil manteniendo una curvatura suave y una distancia segura de los obstáculos en un estacionamiento.

En este ejemplo, puede utilizar la función optimizePath junto con el objeto optimizePathOptions para optimizar una ruta planificada. Puede utilizar cualquier planificador de rutas 2-D como plannerRRT, plannerRRTStar, plannerAstar, plannerHybridAStar, etc. para planificar una ruta desde la entrada del estacionamiento hasta el espacio de estacionamiento deseado. En un entorno similar a un estacionamiento, el automóvil a menudo necesita tomar curvas cerradas y evitar obstáculos como otros automóviles, pilares, letreros, etc. Es posible que la ruta generada por los planificadores de rutas no siempre sea seguro, fácil de navegar, fluido o cinemáticamente factible. En tales situaciones, la optimización de la ruta se vuelve esencial. El objeto optimizePathOptions tiene un gran conjunto de parámetros y pesos que puede ajustar para tener en cuenta el entorno y las limitaciones del vehículo; los pesos le permiten establecer su importancia relativa mientras optimiza la ruta.

Configurar Estacionamiento Medio Ambiente

Cree un objeto binaryOccupancyMap a partir de un mapa de estacionamiento y establezca la resolución del mapa en 3 celdas/metro.

load("parkingMap.mat");
resolution = 3;
map = binaryOccupancyMap(map,resolution);

Visualiza el mapa. El mapa contiene el plano de un aparcamiento con algunas de las plazas ya ocupadas.

show(map)
title("Parking Lot Map")

Elija lugar de estacionamiento

La posición actual del automóvil en la entrada del estacionamiento se toma como pose inicial y se elige una plaza de estacionamiento desocupada deseada como pose objetivo del vehículo.

Defina las poses de inicio y meta para el vehículo como vectores [x y theta]. x y y especifican la posición en metros, y theta especifica el ángulo de orientación en radianes.

startPose = [2 9 0];
goalPose = [27 18 pi/2];

Visualiza las poses.

show(map)
hold on
quiver(startPose(1),startPose(2),cos(startPose(3)),sin(startPose(3)),2,...
       color=[0 0.75 0.23],LineWidth=2,...
       Marker="o",MarkerFaceColor=[0 0.75 0.23],MarkerSize=5,...
       DisplayName="Start Pose",ShowArrowHead="off");
quiver(goalPose(1),goalPose(2),cos(goalPose(3)),sin(goalPose(3)),2, ...
       color=[1 0 0],LineWidth=2,...
       Marker='o',MarkerFaceColor=[1 0 0],MarkerSize=5,...
       DisplayName="Goal Pose",ShowArrowHead="off");
legend(Location="southeast");
title("Start and Goal Poses in Parking Lot Map")
hold off

Planificación de rutas

Cree un validador de estado validatorOccupancyMap utilizando la definición stateSpaceSE2 . Especifique el mapa y la distancia para interpolar y validar segmentos de ruta.

validator = validatorOccupancyMap(stateSpaceSE2,map=map);
validator.ValidationDistance = 0.1;

Inicialice el objeto plannerHybridAStar con el objeto validador de estado. Especifique las propiedades MinTurningRadius y MotionPrimitiveLength del planificador.

planner = plannerHybridAStar(validator,MinTurningRadius=3,MotionPrimitiveLength=4);

Establezca un número aleatorio predeterminado para la repetibilidad.

rng("default");

Planifica una ruta desde la pose inicial hasta la pose objetivo.

refPath = plan(planner,startPose,goalPose);
path = refPath.States;

Visualice la ruta planificada.

show(planner,Tree="off");
legend(Location="southeast");
hold off

Visualice la orientación del vehículo.

plot(rad2deg(refPath.States(:,3)));
title("Orientation of Vehicle Along the Path in degrees")

Configurar los parámetros de optimización de ruta

La ruta generada por el planificador se compone de segmentos de ruta continuos, pero los cruces pueden ser discontinuos. Estos cruces pueden provocar cambios bruscos en el ángulo de dirección. La ruta también puede contener segmentos que añaden tiempo de conducción adicional. Para evitar dicho movimiento, es necesario optimizar y suavizar la ruta. Las rutas a veces están muy cerca de los obstáculos, lo que puede ser riesgoso, especialmente para vehículos pesados ​​como camiones.

Crear opciones de optimización

Cree el objeto optimizePathOptions para configurar el comportamiento de la función optimizePath y la ruta resultante de la misma.

options = optimizePathOptions

options = 
optimizePathOptions

   Trajectory Parameters
                MaxPathStates: 200
           ReferenceDeltaTime: 0.3000
             MinTurningRadius: 1
                  MaxVelocity: 0.4000
           MaxAngularVelocity: 0.3000
              MaxAcceleration: 0.5000
       MaxAngularAcceleration: 0.5000

   Obstacle Parameters
         ObstacleSafetyMargin: 0.5000
       ObstacleCutOffDistance: 2.5000
    ObstacleInclusionDistance: 0.7500

   Solver Parameters
                 NumIteration: 4
           MaxSolverIteration: 15

   Weights
                   WeightTime: 10
             WeightSmoothness: 1000
       WeightMinTurningRadius: 10
               WeightVelocity: 100
        WeightAngularVelocity: 10
           WeightAcceleration: 10
    WeightAngularAcceleration: 10
              WeightObstacles: 50

Las opciones de optimización se agrupan en cuatro categorías:

1. Parámetros de trayectoria

2. Parámetros de obstáculos

3. Parámetros del solver

4. Pesos

Parámetros de trayectoria

Los parámetros de trayectoria se utilizan para especificar las limitaciones del vehículo mientras se mueve a lo largo de la ruta, como velocidad, aceleración, radio de giro, etc. Son límites suaves, lo que significa que el solver puede cambiarlos ligeramente mientras optimiza la ruta. Ajuste los siguientes parámetros,

1. MinTurningRadius - El radio de giro del vehículo. Aumentar el MinTurningRadius dará lugar a curvaturas de trayectoria más grandes.

2. MaxVelocity - La velocidad máxima que puede alcanzar el vehículo. Cambiar esto modificaría la velocidad del vehículo y el tiempo de trayectoria.

3. MaxAcceleration - La aceleración máxima posible para el vehículo.

4. ReferenceDeltaTime - Duración del recorrido entre dos poses consecutivas. Aumentar esto aumentaría la velocidad del vehículo.

5. MaxPathStates - Número máximo de poses permitidas en la ruta. Aumentar esto puede dar una trayectoria más suave pero también podría aumentar el tiempo de optimización.

options.MinTurningRadius = 3; % meters
options.MaxVelocity = 5; % m/s
options.MaxAcceleration = 1; % m/s/s
options.ReferenceDeltaTime = 0.1; % second

separationBetweenStates = 0.2; % meters
numStates = refPath.pathLength/separationBetweenStates;
options.MaxPathStates = round(numStates);

Parámetros de obstáculos

Los parámetros de obstáculos especifican la influencia de los obstáculos en la ruta. Si existe la posibilidad de que los obstáculos se muevan o no se conocen con precisión sus dimensiones, entonces debe mantener el margen de seguridad más alto. En este ejemplo, dado que la mayoría de los obstáculos son marcas de carril y vehículos estacionados, el margen de seguridad puede ser menor. Ajuste los siguientes parámetros,

1. ObstacleSafetyMargin - El margen de seguridad que debe mantener la ruta respecto de los obstáculos. Aumentar el margen de seguridad hará que la ruta sea más seguro y aumentará la distancia desde los obstáculos.

2. ObstacleInclusionDistance - Los obstáculos dentro de esta distancia de la ruta se incluirán para la optimización.

3. ObstacleCutOffDistance : los obstáculos más allá de esta distancia de la ruta se ignorarán durante la optimización. Este valor siempre debe ser mayor que la distancia de inclusión del obstáculo.

Para obstáculos entre ObstacleInclusionDistance y ObstacleCutOffDistance se consideran los obstáculos más cercanos en los lados izquierdo y derecho entre la inclusión y la distancia de corte.

options.ObstacleSafetyMargin = 2; % meters
options.ObstacleInclusionDistance = 0.75; % meters
options.ObstacleCutOffDistance = 2.5; %i meters

Parámetros del solver

Los parámetros del solver especifican las opciones del solver utilizado al optimizar la ruta. Los valores más altos de estos parámetros mejoran los resultados de optimización pero también afectan el tiempo de optimización, por lo que deben ajustarse según la necesidad. Ajuste los siguientes parámetros,

1. NumIteration : número de veces que se invoca el solver durante la optimización; antes de cada invocación, el número de poses en la ruta se ajusta según el tiempo delta de referencia.

2. MaxSolverIteration - Número máximo de iteraciones por invocación del solver .

Si la ruta ya es densa, entonces se puede reducir NumIteration y se puede aumentar MaxSolverIteration . Si la ruta es escasa, entonces NumIteration puede ser mayor y MaxSolverIteration puede mantenerse menor.

options.NumIteration = 4; 
options.MaxSolverIteration = 15;

Pesos

Los pesos definen la importancia relativa de varias restricciones discutidas anteriormente. Dado que la mayoría de las restricciones son límites suaves, los pesos deciden qué tan importante es una restricción al optimizar la ruta. Ajustelos siguientes pesos,

1. WeightTime - Peso del componente de tiempo; aumentarlo conduciría a un duración del recorrido y una ruta más cortos.

2. WeightSmoothness - Aumentar este peso hará que la ruta sea más suave.

3. WeightMinTurningRadius - Al aumentar esto se intentará mantener el radio de giro mayor que el valor mínimo durante la mayor parte de la ruta.

4. WeightVelocity - Aumentar esto asegurará que las restricciones de velocidad se sigan más de cerca.

5. WeightObstacles - Aumentar esto garantizará que el vehículo no cruce un obstáculo.

options.WeightTime = 10;
options.WeightSmoothness = 1000;
options.WeightMinTurningRadius = 10;
options.WeightVelocity = 10;
options.WeightObstacles = 50;

Optimización de ruta

Utilice la función optimizePath para optimizar la ruta generada por el planificador de acuerdo con las opciones de optimización definidas anteriormente.

[optimizedPath,kineticInfo] = optimizePath(path,map,options);
drivingDir = sign(kineticInfo.Velocity);

Visualice la ruta optimizada.

show(planner,Tree="off");
hold on
forwardMotion = optimizedPath(drivingDir==1,:);
reverseMotion = optimizedPath(drivingDir==-1,:);
quiver(forwardMotion(:,1),forwardMotion(:,2),cos(forwardMotion(:,3)),sin(forwardMotion(:,3)),...
    0.1,Color=[0 0.45 0.74],LineWidth=1,DisplayName="Optimized Forward Path");
quiver(reverseMotion(:,1),reverseMotion(:,2),cos(reverseMotion(:,3)),sin(reverseMotion(:,3)),...
    0.1,Color=[0.47 0.68 0.19],LineWidth=1,DisplayName="Optimized Reverse Path");
legend(Location="southeast");
title("Planned Path and Optimized Path")
hold off

Trazar la orientación del vehículo a lo largo de la ruta optimizada.

plot(rad2deg(optimizedPath(:,3)))
title("Orientation of Vehicle Along the Optimized Path in degrees")

Ajustar parámetros con controles en vivo

En la sección anterior, la ruta se optimizó en función de algunos valores de parámetros preestablecidos. Sin embargo, establecer un buen conjunto de parámetros puede tener un impacto significativo en la ruta.

En esta sección puedes actualizar los parámetros usando los controles deslizantes y visualizar el impacto que tienen en la ruta optimizada. Esto le ayudará a comprender mejor cómo los parámetros afectan la ruta final suave.

Parámetros de trayectoria

options.MinTurningRadius    =1.5; % m
options.MaxVelocity         =5; % m/s
options.MaxAcceleration     =1; % m/s/s

ReferenceDeltaTime es un parámetro importante y puede tener un gran impacto en los resultados.

options.ReferenceDeltaTime  =0.2; % s
separationBetweenStates     =0.2; % m
numStates = refPath.pathLength/separationBetweenStates;
options.MaxPathStates = round(numStates);

Obstacle Parameters

options.ObstacleSafetyMargin        =1.5;  % m
options.ObstacleCutOffDistance      =4;  % m
options.ObstacleInclusionDistance   =2;  % m

Parámetros del solver

options.NumIteration        =2;
options.MaxSolverIteration  =8;

Pesas

options.WeightTime              =200;
options.WeightSmoothness        =1000;
options.WeightMinTurningRadius  =140;
options.WeightVelocity          =230;
options.WeightObstacles         =320;

Optimizar ruta y visualizar

La nueva ruta optimizada se traza junto con la ruta optimizada y generada previamente. Esto le ayudará a comparar los resultados de la optimización.

Primero trace la ruta original planificada por el planificador y los resultados de la optimización anterior.

show(planner,Tree="off")
hold on
quiver(forwardMotion(:,1),forwardMotion(:,2),cos(forwardMotion(:,3)),sin(forwardMotion(:,3)),...
    0.1,Color=[0 0.45 0.74],LineWidth=1,DisplayName="Previous Optimized Forward Path");
quiver(reverseMotion(:,1),reverseMotion(:,2),cos(reverseMotion(:,3)),sin(reverseMotion(:,3)),...
    0.1,Color=[0.47 0.68 0.19],LineWidth=1,DisplayName="Previous Optimized Reverse Path");

Ahora optimice la ruta según el nuevo conjunto de opciones de optimización.

[optimizedPath,kineticInfo] = optimizePath(path,map,options);

Finalmente trace la nueva ruta optimizada.

drivingDir = sign(kineticInfo.Velocity);

forwardMotion = optimizedPath(drivingDir==1,:);
reverseMotion = optimizedPath(drivingDir==-1,:);

quiver(forwardMotion(:,1),forwardMotion(:,2),cos(forwardMotion(:,3)),sin(forwardMotion(:,3)),...
    0.1,Color=[0.3 0.75 0.93],LineWidth=1,DisplayName="Optimized Forward Path");
quiver(reverseMotion(:,1),reverseMotion(:,2),cos(reverseMotion(:,3)),sin(reverseMotion(:,3)),...
    0.1,Color=[0.85 0.33 0.1],LineWidth=1,DisplayName="Optimized Reverse Path");

legend(Location="southeast");
title("Previous and Updated Optimized Path")

hold off

Referencia

Rosmann, Christoph, Frank Hoffmann y Torsten Bertram. "Optimización y control de trayectoria cinedinámica para robots similares a automóviles". En Conferencia internacional IEEE/RSJ de 2017 sobre robots y sistemas inteligentes (IROS), 5681–86. Vancouver, Columbia Británica: IEEE, 2017. https://doi.org/10.1109/IROS.2017.8206458.