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 globales de planificación de rutas 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ácilmente determinadas 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 característica 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. Muestrear un estado objetivo.

2. Encuentre el vecino más cercano aproximado al estado objetivo 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. Compruebe si alguno de los estados devueltos ha alcanzado el objetivo.

6. Si se ha encontrado el objetivo, salir. En caso contrario agregar 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 Kino-Dynamic Planning Algorithm a la clase y método responsable:

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, salir de la planificación o agregar 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 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 tractor-remolque está compuesto por dos cuerpos rígidos conectados en un enganche kingpin, que se modela como una junta de revolución. 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 final (izquierda, M < 0), configuración OverCenter (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$- Global$\mathit{x}$-posición del centro del eje trasero del remolque.

• ${\mathit{y}}_2$- Global$\mathit{y}$-posición del centro del eje trasero del remolque.

• ${\theta }_2$— Ángulo global de 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 de 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 (hacia adelante o hacia atrás) o una velocidad requerida, asegurando que el sistema se propague hacia la meta 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 coste 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 conexiones más óptimas localmente. 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 obtener 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-remolques [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]$$

Por simplicidad, este modelo trata${\mathit{u}}_{\mathrm{app}}=\left[\mathit{v},{\alpha }_{\mathrm{steer}}\right]$como el comando instantáneo de velocidad y ángulo de dirección aplicado al sistema, generado por un controlador de nivel superior. Para obtener más información, consulte Create Control Laws for Stable Forward and Reverse Motion.

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

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

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 pure pursuit calcula un punto de referencia,$\mathit{P}=[{\mathit{X}}_{\mathit{p}},{\mathit{Y}}_{\mathit{p}}]$, entre una pose actual,${\mathit{q}}_{\mathit{i}}$y estado objetivo,${\mathit{q}}_{\mathrm{tgt}}$. El controlador tiene dos modos, avance y retroceso. Para obtener información sobre las bases 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 en reversa, el eje trasero del remolque pasa a 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 distancias de anticipación para el controlador de pure pursuit, exampleHelperPurePursuitGetSteering , e información de velocidad y período de tiempo para nuestro simulador de modelo, 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 coste fuera de línea usando 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

Adapte el sistema a un marco utilizable por el planificador de ruta cinemático, plannerControlRRT. Crea 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.

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

Debido a que el planificador cinemático utiliza la función distance del propagador de estado 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 indefinidos.

% 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 coste 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: [1x1 exampleHelperTruckPropagator]
             States: [23x6 double]
           Controls: [22x4 double]
          Durations: [22x1 double]
       TargetStates: [22x6 double]
          NumStates: 23
        NumSegments: 22

treeInfo = struct with fields:
     IsPathFound: 1
        ExitFlag: 1
     NumTreeNode: 2201
    NumIteration: 1196
    PlanningTime: 16.4134
        TreeInfo: [6x6602 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.