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

Planificación de movimiento con capacidad inversa para modelo de tractor-remolque mediante planificadorControlRRT

Este ejemplo muestra cómo encontrar soluciones de planificación de rutas globales para sistemas con cinemática compleja utilizando el planificador basado en cinemática, plannerControlRRT. El ejemplo está organizado en tres secciones principales:

1. Introducción a la planificación cinedinámica.

2. Adaptación de plannerControlRRT a un sistema tractor-remolque.

3. Búsqueda de trayectorias libres de colisiones mediante un sistema cinemático.

Introducción a la planificación cinedinámica

Descripción del planificador

Los planificadores geométricos convencionales, como RRT, RRT* e hybridA*, son algoritmos rápidos y extensibles capaces de encontrar soluciones completas y óptimas a una amplia variedad de problemas de planificación. Sin embargo, una desventaja es que hacen suposiciones sobre el espacio de planificación que pueden no ser ciertas para un sistema del mundo real. Los planificadores geométricos convencionales suponen que dos estados cualesquiera en un espacio pueden conectarse mediante una trayectoria sin error residual, y que el sistema físico puede rastrear las rutas devueltos por el planificador geométrico.

Para sistemas con cinemática compleja, o aquellos que no tienen soluciones de forma cerrada fáciles de determinar para conectar estados, utilice planificadores kinodinámicos, como plannerControlRRT. Los planificadores kinodinámicos intercambian integridad por flexibilidad de planificación y aprovechan el modelo cinemático y el controlador propios del sistema para generar trayectorias factibles y rastreables.

Para planificar un sistema determinado, la función plannerControlRRT requiere esta información:

• Una utilidad para muestrear estados en el espacio de planificación.

• Una métrica para estimar el coste de conectar dos estados.

• Un mecanismo para propagar deterministamente el sistema de un estado a otro.

• Una utilidad para determinar si un estado ha alcanzado la meta.

Este ejemplo demuestra cómo formular estas diferentes utilidades para un sistema de camión con remolque y adaptarlas a la infraestructura de planificación. Esta vista previa muestra el resultado:

% Attempt to generate MEX to accelerate collision checking
genFastCC

Code generation successful.

% load preplannedScenarios
parkingScenario = load("parkingScenario.mat");
planner = parkingScenario.planner;
start   = parkingScenario.start;
goal    = parkingScenario.goal;

% Display the scenario and play back the trajectory
show(planner.StatePropagator.StateValidator);
axis equal 
hold on
trajectory = plan(planner,start,goal);
demoParams = trajectory.StatePropagator.StateSpace.TruckParams;
exampleHelperPlotTruck(demoParams,start); % Display start config
exampleHelperPlotTruck(demoParams,goal);  % Display goal config

% Plan path using previously-built utilities
rng(1)
trajectory = plan(planner,start,goal);

% Animate the trajectory
exampleHelperPlotTruck(trajectory); 
hold off

Algoritmo de planificación cinedinámica

Si ya está familiarizado con los planificadores geométricos, una iteración del algoritmo de búsqueda plannerControlRRT debería resultarle familiar, pero con algunos pasos adicionales:

1. Muestra de un estado objetivo.

2. Encuentre el vecino más cercano aproximado al estado de destino en el árbol de búsqueda.

3. Generar una entrada de control (o valor de referencia) y una duración de control que impulse el sistema hacia el objetivo.

4. Propagar (extender) el sistema hacia la muestra mientras valida estados intermedios a lo largo de la trayectoria propagada. Devuelve la secuencia válida de estados, controles y duraciones.

5. Verificar si alguno de los estados devueltos ha alcanzado el objetivo.

6. Si se ha encontrado el objetivo, salir. De lo contrario, agregue el estado final, el control inicial, la duración acumulada y el objetivo al árbol.

7. (Opcional) Pruebe una configuración y control de objetivos y repita los pasos 4, 5 y 6 la cantidad de veces deseada.

Componentes requeridos por plannerControlRRT Framework

Para adaptar el planificador a un problema específico, debe implementar clases personalizadas nav.StatePropagator y nav.StateSpace. La siguiente sección asigna los pasos descritos en la sección Algoritmo de planificación kinodinámica a la clase y al método responsables:

1. $${\mathit{q}}_{\mathrm{tgt}}=\mathrm{sampleUniform}\left(\mathit{space}\right)$$

2. $$\left[\mathit{dist},\mathit{err}\right]=\mathrm{distance}\left(\mathit{propagator},{\mathit{Q}}_{\mathrm{tree}},{\mathit{q}}_{\mathrm{tgt}}\right)$$

3. $$\left[\mathit{u},{\mathit{N}}_{\mathrm{step}}\right]=\mathrm{sampleControl}\left(\mathit{propagator},{\mathit{q}}_0,{\mathit{u}}_0,{\mathit{q}}_{\mathrm{tgt}}\right)$$

4. $${\left[{\mathit{Q}}_{1:\mathit{n}},{\mathit{U}}_{1:\mathit{n}},{\mathit{n}}_{{\mathrm{step}}_{1:\mathit{n}}}\right]}_{\mathrm{valid}}=\mathrm{propagateWhileValid}\left(\mathbf{propagator},{\mathit{q}}_0,{\mathit{u}}_0,{\mathit{q}}_{\mathrm{tgt}},{\mathit{N}}_{\mathrm{step}}\right)$$

5. plannerControlRRT comprueba si alguno de los estados devueltos ha alcanzado el objetivo.

6. De forma predeterminada, la salida de la planificación o la adición al árbol de búsqueda se realiza mediante la función distance del propagador. Puede anularlo proporcionando al planificador un controlador de función durante la construcción.

7. De forma predeterminada, cada vez que el planificador agrega con éxito un nodo al árbol, intentará propagar este estado recién agregado hacia la configuración de objetivos. Esta propagación de objetivos puede ocurrir varias veces seguidas o deshabilitarse por completo modificando la propiedad NumGoalExtension del planificador.

Para obtener una sintaxis de función más detallada, requisitos de argumentos y ejemplos, consulte plannerControlRRT, nav.StateSpace, y nav.StatePropagator.

Adaptación de plannerControlRRT a un sistema de camión-remolque

Esta sección demuestra cómo formular, modelar y controlar un sistema de camión-remolque de dos cuerpos. Este sistema es inherentemente inestable durante el movimiento inverso, por lo que es poco probable que las trayectorias devueltas por los planificadores geométricos convencionales produzcan buenos resultados durante el seguimiento de la trayectoria. Para modelar con precisión este sistema, debe:

• Defina una función de transición de estado.

• Definir los parámetros del modelo geométrico.

• Cree una ley de control que se ajuste al modelo parametrizado.

• Cree una heurística de distancia adaptada al modelo parametrizado.

• Empaquete el modelo cinemático, la métrica de distancia y la ley de control dentro de un espacio de estados personalizado y un propagador de estados.

• Planifique una ruta para varios escenarios de problemas.

Definir la función de transición de estado

Este sistema de tractor-remolque está compuesto por dos cuerpos rígidos conectados a un enganche de pivote, que se modela como una junta giratoria. La carrocería del remolque hace contacto con el suelo en el eje trasero y la parte delantera del remolque está sostenida por el enganche, ubicado en algún lugar a lo largo de la línea central axial del camión.

Configuración de seguimiento (izquierda, M < 0), configuración sobre el centro (derecha, M < 0)

Utilice la configuración geométrica para un sistema de camión-remolque de dos cuerpos del ejemplo de Estacionamiento automático de camiones y remolques usando MPC no lineal de etapas múltiples, expresada como:

$${\mathit{q}}_{\mathrm{sys}}=\left[{\mathit{x}}_2{\mathit{y}}_2{\theta }_2\beta \right]$$,

donde:

• ${\mathit{x}}_2$ — Posición global $\mathit{x}$ del centro del eje trasero del remolque.

• ${\mathit{y}}_2$ — Posición global $\mathit{y}$ del centro del eje trasero del remolque.

• ${\theta }_2$ — Ángulo global de la orientación del remolque, donde 0 es el este.

• $\beta$ — Orientación del camión con respecto al remolque, donde 0 está alineado.

Para fines de planificación, agregue un indicador de dirección, ${\mathit{v}}_{\mathrm{sign}}$, y la distancia total recorrida desde la configuración inicial, ${\mathit{s}}_{\mathrm{tot}}$ al vector de estado, para la notación del estado final:

$\mathit{q}=\left[{\mathit{x}}_2{\mathit{y}}_2{\theta }_2\beta {\mathit{v}}_{\mathrm{sign}}{\mathit{s}}_{\mathrm{tot}}\right]$

• ${\mathit{v}}_{\mathrm{sign}}$ — Indica el modo de control deseado (avance o retroceso) o la velocidad requerida, asegurando que el sistema se propague hacia el objetivo en la dirección deseada.

• ${\mathit{s}}_{\mathrm{tot}}$ — Modifica el comportamiento de la función distance del propagador. Puede modificar la propiedad TravelBias del propagador para cambiar la frecuencia con la que la comparación del vecino más cercano incluye o excluye el costo de raíz a nodo. Los valores más cercanos a 0 dan como resultado una búsqueda más rápida del espacio de planificación, a expensas de que se realicen más conexiones localmente óptimas. Los valores más cercanos a 1 pueden mejorar las soluciones del planificador, pero aumentan el tiempo de planificación.

Esta función de transición de estado es una extensión de TruckTrailerStateFcn. Para una derivación completa de la función de transición de estado, consulte exampleHelperStateDerivative, que es una simplificación de este modelo de N-trailer [1]:

$$\dot{\mathit{q}}\left({\mathit{L}}_1,{\mathit{L}}_2,\mathit{M},\mathit{q},{\mathit{u}}_{\mathrm{app}}\right)=\left[\dot{{\mathit{x}}_2}\dot{{\mathit{y}}_2}\dot{{\theta }_2}\dot{\beta }0\mathit{v}\right]$$

Para simplificar, este modelo trata ${\mathit{u}}_{\mathrm{app}}=\left[\mathit{v},{\alpha }_{\mathrm{steer}}\right]$ como el comando de velocidad instantánea y ángulo de dirección aplicado al sistema, generado por un controlador de nivel superior. Para obtener más información, consulte Crear leyes de control para movimiento estable hacia adelante y hacia atrás.

Definir parámetros del modelo geométrico y demostrar inestabilidad

Las características cinemáticas de este vehículo están fuertemente influenciadas por la elección de los parámetros del modelo. Crear una estructura que describa un sistema de tractor-remolque.

truckParams = struct;
truckParams.L1 = 6;     % Truck body length
truckParams.L2 = 10;    % Trailer length
truckParams.M  = 1;     % Distance between hitch and truck rear axle along truck body +X-axis

Defina propiedades relacionadas con la visualización y la verificación de colisiones.

truckParams.W1 = 2.5;               % Truck width
truckParams.W2 = 2.5;               % Trailer width
truckParams.Lwheel = 1;             % Wheel length
truckParams.Wwheel = 0.4;           % Wheel width

Utilizando la función exampleHelperShowOpenLoopDynamics y los parámetros de modelo especificados, propague el estado del sistema utilizando su dinámica de bucle abierto para demostrar que el ángulo interior es autoestabilizador durante el movimiento hacia adelante e inestable durante el movimiento hacia atrás para cualquier ${\beta }_0$ distinto de cero.

exampleHelperShowOpenLoopDynamics(truckParams);

Cree leyes de control para un movimiento estable hacia adelante y hacia atrás

Este ejemplo emplea un controlador de cambio de modo multicapa para controlar el modelo de camión-remolque. En el nivel superior, un controlador de persecución pura calcula un punto de referencia, $\mathit{P}=[{\mathit{X}}_{\mathit{p}},{\mathit{Y}}_{\mathit{p}}]$, entre una pose actual, ${\mathit{q}}_{\mathit{i}}$, y un estado objetivo, ${\mathit{q}}_{\mathrm{tgt}}$. El controlador tiene dos modos, avance y retroceso. Para obtener información sobre la base de esta ley de control, consulte [1]. Para el movimiento hacia adelante, el controlador calcula un ángulo de dirección que, cuando se mantiene constante, impulsa el eje trasero del camión a lo largo de un arco que cruza el punto de referencia:

$${{\mathit{q}}_1}_{\mathit{i}}={\left[{\mathit{x}}_{1,}{\mathit{y}}_1{\theta }_1\right]}_{\mathit{i}}$$

$${\theta }_{\mathrm{err}}=\mathrm{atan2}\left({\mathit{X}}_{\mathit{p}}-{\mathit{x}}_1,{\mathit{Y}}_{\mathit{p}}-{\mathit{y}}_1\right)-{\theta }_1$$

$${\alpha }_{\mathrm{fwd}}=\mathit{f}\left({\theta }_{\mathrm{err}}\right)$$

Para el movimiento inverso, el eje trasero del remolque pasa al estado controlado, ${\mathit{q}}_2=\left[{\mathit{x}}_2,{\mathit{y}}_2,{\theta }_2\right]$. Además, debido a que el sistema es inherentemente inestable durante el movimiento en reversa, la ley de control trata el ángulo de dirección devuelto por el controlador de nivel superior como un valor de referencia para un controlador LQ de ganancia programada, que busca estabilizar el ángulo interior del vehículo:

$${\theta }_{\mathrm{err}}=\mathrm{atan2}\left({{\mathit{X}}_{\mathit{p}}}^{\prime }-{\mathit{x}}_2,{{\mathit{Y}}_{\mathit{p}}}^{\prime }-{\mathit{y}}_2\right)-{\theta }_2$$

$${\beta }_{\mathit{d}}=-\mathrm{atan}\left(\frac{2{\mathit{L}}_2\sin \left({\theta }_{\mathrm{err}}\right)}{\mathit{R}}\right)$$

$${\beta }_{\mathrm{ref}}={\beta }_{\mathit{d}}+{\mathit{K}}_{\mathit{p}}\left({\beta }_{\mathit{d}}-\beta \right)$$

Estabilización de retroalimentación LQ

Durante el movimiento en reversa, puede agregar una ganancia al ángulo de dirección que lleva el ángulo interior a un punto de equilibrio. Utilice el controlador LQ para calcular el ángulo de dirección deseado y las ganancias de retroalimentación. Las ganancias son la solución óptima a la ecuación algebraica de Ricatti, almacenada como una tabla de búsqueda que depende del ángulo de dirección deseado. Para obtener más información sobre la derivación de este controlador de retroalimentación, consulte exampleHelperCalculateLQGains:

% Define Q/R weight matrices for LQR controller
Q = 10; % Weight driving betaDot -> 0
R = 1; % Weight minimizing steering angle command

% Derive geometric steering limits and solve for LQR feedback gains
[alphaLimit, ...        % Max steady-state steering angle before jack-knife
 betaLimit, ...         % Max interior angle before jack-knife
 alphaDesiredEq, ...    % Sampled angles from stable alpha domain
 alphaGain ...          % LQ gain corresponding to desired alpha
] = exampleHelperCalculateLQGains(truckParams,Q,R);

% Add limits to the truck geometry
truckParams.AlphaLimit = alphaLimit;
truckParams.BetaLimit = betaLimit;

Especifique las distancias de anticipación para el controlador de persecución pura, exampleHelperPurePursuitGetSteering,, y la información de velocidad y lapso de tiempo para nuestro simulador de modelos, exampleHelperPropagateTruck. Debido a que el movimiento en reversa es inestable y solo tiene control indirecto sobre el ángulo interior, especifique una distancia de anticipación mayor en reversa. Esto le da al sistema más tiempo para minimizar el error de dirección virtual mientras mantiene un ángulo interior estable.

% Select forward and reverse lookahead distances. For this example,
% use a reverse lookahead distance twice as long as the forward
% lookahead distance, which itself is slightly longer than the 
% wheelbase. You can tune these parameters can be tuned for
% improved performance for a given geometry.
rFWD = truckParams.L1*1.2;
rREV = rFWD*2;

% Define parameters for the fixed-rate propagator and add them to the
% control structure
controlParams.MaxVelocity = 3; % m/s
controlParams.StepSize = .1; % s
controlParams.MaxNumSteps = 50; %#ok<STRNU> % Max number of steps per propagation

Crear una estructura de la información relacionada con el control.

% Store gains and control parameters in a struct
controlParams = struct(...
    'MaxSteer',alphaLimit, ...          % (rad)
    'Gains',alphaGain, ...              % ()
    'AlphaPoints',alphaDesiredEq, ...   % (rad)
    'ForwardLookahead',rFWD, ...        % (m)
    'ReverseLookahead',rREV, ...        % (m)
    'MaxVelocity', 3, ...               % (m/s)
    'StepSize', 0.1, ...                % (s)
    'MaxNumSteps', 200 ...               % ()
    );

Definir una heurística de distancia

Defina una heurística de distancia para aproximar el coste entre diferentes configuraciones en el espacio de estados del sistema. El propagador de estado usa esto cuando el planificador intenta encontrar el nodo del árbol más cercano a un estado muestreado. Calcule el costo sin conexión utilizando la función exampleHelperCalculateDistanceMetric, que lo almacena en una tabla de búsqueda.

distanceHeuristic = exampleHelperCalculateDistanceMetric(truckParams,controlParams);

Adaptar el sistema para su uso con el planificador cinemático

Adaptar el sistema a un marco utilizable por el planificador de trayectoria cinemático, plannerControlRRT. Cree 3 clases personalizadas: exampleHelperTruckStateSpace, exampleHelperTruckPropagator y exampleHelperTruckValidator, que heredan de las subclases nav.StateSpace, nav.StatePropagator y nav.StateValidator, respectivamente.

Inicializar espacio de estados

El objeto del espacio de estados es el principal responsable de:

• Muestreo de estados aleatorios para el planificador.

• Definir y hacer cumplir los límites estatales dentro de una trayectoria.

Define límites xy- para la región de búsqueda. Inicialice el espacio de estados utilizando estos límites y los límites $\beta$ determinados por las propiedades geométricas del remolque del camión.

Dado que el planificador cinemático utiliza la función distance del propagador de estados para su búsqueda del vecino más cercano, deje el método distance del espacio de estados sin definir. Deje los métodos interpolate y sampleGaussian del espacio de estados igualmente sin definir.

% Define limits of searchable region
xyLimits = [-60 60; -40 40];

% Construct our state-space
stateSpace = exampleHelperTruckStateSpace(xyLimits, truckParams);

Inicializar validador de estado

En este ejemplo, el validador de estado utiliza cuadros delimitadores orientados (OBB) para verificar si el vehículo colisiona con el entorno. El camión está representado por dos rectángulos con poses definidas por el estado y la geometría del vehículo del camión contenido en el objeto del espacio de estados. El entorno está representado por una lista de rectángulos con sus ubicaciones, orientaciones y dimensiones especificadas en una estructura de 1 por N. Cargue un archivo MAT que contenga el entorno en el espacio de trabajo y utilice los obstáculos y el objeto del espacio de estado para construir el validador de estado.

% Load a set of obstacles
load("Slalom.mat");

% Construct the state validator using the state space and list of obstacles
validator = exampleHelperTruckValidator(stateSpace,obstacles);

Inicializar propagador de estado

En la planificación cinemática, el objeto propagador de estado es responsable de:

• Estimación de la distancia entre estados.

• Muestreo de controles iniciales para propagarlos a lo largo de un intervalo de control, o capturar entradas de control como valores de referencia para controladores de nivel inferior durante la propagación.

• Propagar el sistema hacia un objetivo y devolver la parte válida de la trayectoria al planificador.

Para obtener mejores resultados, la función distance debe estimar el costo de la trayectoria generada al propagarse entre estados.

% Construct the state propagator using the state validator, control parameters,
% and distance lookup table
propagator = exampleHelperTruckPropagator(validator,controlParams,distanceHeuristic);

Planificador de construcción y ruta del plan

Construya un planificador de rutas cinemático que utilice el propagador de estado para buscar una ruta entre las dos configuraciones.

% Define start configuration
start = [35 5 pi/2 0 0 0];

% Define the goal configuration such that the truck must reverse into
% position.
goal  = [-35 20 -pi/2 0 -1 nan];

% Display the problem
figure
show(validator)
hold on
configs = [start; goal];
quiver(configs(:,1),configs(:,2),cos(configs(:,3)),sin(configs(:,3)),.1)

% Define a function to check if planner has reached the true goal state
goalFcn = @(planner,q,qTgt)exampleHelperGoalReachedFunction(goal,planner,q,qTgt);

% Construct planner
planner = plannerControlRRT(propagator,GoalReachedFcn=goalFcn,MaxNumIteration=30000);
planner.NumGoalExtension = 3;
planner.MaxNumTreeNode = 30000;
planner.GoalBias = .25;

% Search for the path
rng(0)
[trajectory,treeInfo] = plan(planner,start,goal)

trajectory = 
  navPathControl with properties:

    StatePropagator: [1×1 exampleHelperTruckPropagator]
             States: [23×6 double]
           Controls: [22×4 double]
          Durations: [22×1 double]
       TargetStates: [22×6 double]
          NumStates: 23
        NumSegments: 22

treeInfo = struct with fields:
     IsPathFound: 1
        ExitFlag: 1
     NumTreeNode: 2201
    NumIteration: 1196
    PlanningTime: 5.3061
        TreeInfo: [6×6602 double]

% Visualize path and waypoints
exampleHelperPlotTruck(trajectory);
hold off

En lugar de producir soluciones óptimas, como las generadas en el ejemplo de estacionamiento automático de camiones y remolques utilizando MPC no lineal de etapas múltiples, la ventaja de este planificador radica en su capacidad para encontrar trayectorias viables para una amplia variedad de problemas. En el siguiente escenario, por ejemplo, la ruta generada se puede usar tal cual, o como una suposición inicial para un solver MPC, lo que ayuda a evitar mínimos locales en el espacio del problema no convexo:

% Load scenario obstacles
nonConvexProblem = load("NonConvex.mat");

% Update validator and state space
validator.Obstacles = nonConvexProblem.obstacles;
validator.StateSpace.StateBounds(1:2,:) = [-100 100; -40 40];
figure
show(validator)

% Define start and goal
start = [10 0 0 0 NaN 0];
goal  = [-10 0 pi 0 -1 0];

% Update goal reached function
goalFcn = @(planner,q,qTgt)exampleHelperGoalReachedFunction(goal,planner,q,qTgt);
planner.GoalReachedFcn = goalFcn;

% Turn off optional every-step goal propagation
planner.NumGoalExtension = 0;

% Increase goal sampling frequency
planner.GoalBias = .25;

% Balanced search vs path optimality
propagator.TravelBias = .5;

% Plan path
rng(0)
trajectory = plan(planner,start,goal);

% Visualize result
exampleHelperPlotTruck(trajectory);

Referencias

[1] Holmer, Olov. "Planificación del movimiento para un sistema reversible de camiones y remolques a gran escala". EM. tesis, Universidad de Linköping, 2016.