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

Seguimiento de objetos y planificación de movimiento mediante la ruta de referencia de Frenet

Este ejemplo utiliza:

Este ejemplo le muestra cómo replanificar dinámicamente el movimiento de un vehículo autónomo en función de la estimación del entorno circundante. Se utiliza una ruta de referencia de Frenet y un rastreador de asociación de datos probabilísticos conjuntos (JPDA) para estimar y predecir el movimiento de otros vehículos en la carretera. En comparación con el ejemplo Planificación de la trayectoria de la carretera mediante la ruta de referencia de Frenet, en este ejemplo se utilizan estas trayectorias estimadas del rastreador de múltiples objetos en lugar de la ground-truth para la planificación del movimiento.

Introducción

La replanificación dinámica para vehículos autónomos normalmente se realiza con un planificador de movimiento local. El planificador de movimiento local es responsable de generar una trayectoria óptima basada en un plan global e información en tiempo real sobre el entorno circundante. El plan global para la planificación de la trayectoria de la carretera se puede describir como una lista de coordenadas pregenerada de la línea central de la carretera. El entorno circundante se puede describir principalmente de dos maneras:

Conjunto discreto de objetos del entorno circundante con geometrías definidas.
Cuadrícula discretizada con estimaciones sobre regiones libres y ocupadas en el entorno circundante.

En presencia de obstáculos dinámicos, un planificador de movimiento local también requiere predicciones sobre el entorno para evaluar la validez de las trayectorias planificadas. En este ejemplo, representa el entorno circundante utilizando el enfoque de conjunto discreto de objetos. Para ver un ejemplo que utiliza una cuadrícula discretizada, consulte el ejemplo Motion Planning in Urban Environments Using Dynamic Occupancy Grid Map (Sensor Fusion and Tracking Toolbox).

Modelado de medición y transición de estado de objetos

La lista de objetos y sus predicciones futuras para la planificación del movimiento normalmente se estiman mediante un rastreador de objetos múltiples. El rastreador de objetos múltiples acepta datos de sensores y estima la lista de objetos. En la comunidad de seguimiento, esta lista de objetos a menudo se denomina lista de seguimiento.

En este ejemplo, utiliza sensores de radar y cámara y estima la lista de seguimiento utilizando un rastreador multiobjeto JPDA. El primer paso para utilizar cualquier rastreador multiobjeto es definir el estado del objeto, cómo evoluciona el estado con el tiempo (modelo de transición de estado) y cómo lo percibe el sensor (modelo de medición). Los modelos de transición de estado comunes incluyen el modelo de velocidad constante, el modelo de aceleración constante, etc. Sin embargo, en presencia de información cartográfica, la red de carreteras se puede integrar en el modelo de movimiento. En este ejemplo, se utiliza un sistema de coordenadas de Frenet para describir el estado del objeto en cualquier paso de tiempo determinado, $k$ .

$x_{k} = [s_{k} \dot{s_{k}} d_{k} \dot{d_{k}}]$

donde $s_{k}$ y $d_{k}$ representan la distancia del objeto a lo largo y perpendicular a la línea central de la carretera, respectivamente. Se utiliza un modelo de transición de estado de velocidad constante para describir el movimiento del objeto a lo largo de la carretera y un modelo de velocidad decreciente para describir el movimiento perpendicular a la línea central de la carretera. Este modelo de velocidad decreciente le permite representar las maniobras de cambio de carril de otros vehículos en la carretera.

$[\begin{array}{c} s_{k + 1} \\ \dot{s_{k + 1}} \\ d_{k + 1} \\ \dot{d_{k + 1}} \end{array}] = [\begin{array}{c} 1 & Δ T & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & τ (1 - e^{(- \frac{Δ T}{τ})}) \\ 0 & 0 & 0 & e^{(- \frac{Δ T}{τ})} \end{array}] [\begin{array}{c} s_{k} \\ \dot{s_{k}} \\ d_{k} \\ \dot{d_{k}} \end{array}] + [\begin{array}{c} \frac{Δ T^{2}}{2} & 0 \\ Δ T & 0 \\ 0 & \frac{Δ T^{2}}{2} \\ 0 & Δ T \end{array}] [\begin{array}{c} w_{s} \\ w_{d} \end{array}]$

donde $Δ T$ es la diferencia de tiempo entre los pasos $k$ y $k + 1$ , $w_{s}$ y $w_{d}$ son ruido gaussiano de media cero que representa una aceleración desconocida en coordenadas de Frenet, y $τ$ es una constante en descomposición.

Esta elección de coordenadas al modelar el movimiento del objeto le permite integrar la ruta de referencia de la carretera en el marco de seguimiento de múltiples objetos. La integración de la ruta de referencia actúa como información adicional para el rastreador y le permite mejorar las estimaciones del estado actual, así como las trayectorias previstas de los objetos estimados. Puede obtener un modelo de medición transformando primero el estado del objeto en posición y velocidad cartesianas y luego convirtiéndolos a las cantidades medidas respectivas, como azimut y rango.

Configuración

Escenario y sensores

El escenario utilizado en este ejemplo se crea utilizando la función Driving Scenario Designer (Automated Driving Toolbox) y luego se exporta a una función MATLAB ®. El vehículo ego está equipado con 1 radar orientado hacia adelante y 5 cámaras que brindan una cobertura de 360 grados. El radar y las cámaras se simulan utilizando los objetos del sistema drivingRadarDataGenerator (Automated Driving Toolbox) y visionDetectionGenerator (Automated Driving Toolbox), respectivamente.

Todo el escenario y la configuración del sensor se definen en la función auxiliar, helperTrackingAndPlanningScenario, adjunta a este ejemplo. Usted define el plan global que describe la línea central de la autopista utilizando un objeto referencePathFrenet. Como varios algoritmos en este ejemplo necesitan acceso a la ruta de referencia, se define la función helperGetReferencePath, que utiliza un objeto persistent al que puede acceder cualquier función.

rng(2022); % For reproducible results

% Setup scenario and sensors
[scenario, egoVehicle, sensors] = helperTrackingAndPlanningScenario();

Rastreador conjunto de asociación de datos probabilísticos

Configura un rastreador de asociaciones de datos probabilísticos conjuntos utilizando trackerJPDA (Sensor Fusion and Tracking Toolbox) System object ™. Establece la propiedad FilterInitializationFcn del rastreador en la función helperInitRefPathFilter. Esta función auxiliar define un filtro Kalman extendido, trackerJPDA (Sensor Fusion and Tracking Toolbox), que se utiliza para estimar el estado de un solo objeto. Las funciones locales dentro del archivo helperInitRefPathFilter definen la transición de estado así como el modelo de medición para el filtro. Además, para predecir las pistas en un momento futuro para el planificador de movimiento, utilice la función predictTracksToTime (Sensor Fusion and Tracking Toolbox) del rastreador.

tracker = trackerJPDA('FilterInitializationFcn',@helperInitRefPathFilter,...
    'AssignmentThreshold',[200 inf],...
    'ConfirmationThreshold',[8 10],...
    'DeletionThreshold',[5 5]);

Planificador de movimiento

Utiliza un planificador de movimiento de trayectoria de autopista similar al descrito en el ejemplo Planificación de la trayectoria de la carretera mediante la ruta de referencia de Frenet. El planificador de movimiento utiliza un horizonte de planificación de 5 segundos y considera tres modos para muestrear trayectorias para el vehículo ego: control de crucero, seguimiento del vehículo líder y cambio de carril básico. Todo el proceso para generar una trayectoria óptima está envuelto en la función auxiliar, helperPlanHighwayTrajectory.

La función auxiliar acepta un objeto dynamicCapsuleList como entrada para encontrar trayectorias que no colisionen. La comprobación de colisiones se realiza en todo el horizonte de planificación en un intervalo de 0,5 segundos. Como los estados de la pista varían con el tiempo, actualiza el objeto dynamicCapsuleList en el bucle de simulación utilizando la función helperUpdateCapsuleList, adjunta a este ejemplo.

% Collision check time stamps
tHorizon = 5; % seconds
deltaT = 0.5; % seconds
tSteps = deltaT:deltaT:tHorizon;

% Create the dynamicCapsuleList object
capList = dynamicCapsuleList;
capList.MaxNumSteps = numel(tSteps) + 1;

% Specify the ego vehicle geometry
carLen = 4.7;
carWidth = 1.8;
rearAxleRatio = 0.25;
egoID = 1;
[egoID, egoGeom] = egoGeometry(capList,egoID);

% Inflate to allow uncertainty and safety gaps
egoGeom.Geometry.Length = 2*carLen; % in meters
egoGeom.Geometry.Radius = carWidth/2; % in meters
egoGeom.Geometry.FixedTransform(1,end) = -2*carLen*rearAxleRatio; % in meters
updateEgoGeometry(capList,egoID,egoGeom);

Ejecutar simulación

En esta sección, avanzará en la simulación, generará datos de sensores y realizará una replanificación dinámica utilizando estimaciones sobre el entorno. Todo el proceso se divide en 5 pasos principales:

Usted recopila datos de sensores simulados de sensores de radar y cámara.
Usted alimenta los datos del sensor al rastreador JPDA para estimar el estado actual de los objetos.
Predice el estado de los objetos utilizando la función predictTracksToTime.
Actualiza la lista de objetos para el planificador y planifica una trayectoria de carretera.
Mueves el vehículo ego simulado en la trayectoria planificada.

% Create display for visualizing results
display = HelperTrackingAndPlanningDisplay;

% Initial state of the ego vehicle
refPath = helperGetReferencePath;
egoState = frenet2global(refPath,[0 0 0 0.5*3.6 0 0]);
helperMoveEgoToState(egoVehicle, egoState);

while advance(scenario)
    % Current time
    time = scenario.SimulationTime;

    % Step 1. Collect data
    detections = helperGenerateDetections(sensors, egoVehicle, time);
    
    % Step 2. Feed detections to tracker
    tracks = tracker(detections, time);

    % Step 3. Predict tracks in planning horizon
    timesteps = time + tSteps;
    predictedTracks = repmat(tracks,[1 numel(timesteps)+1]);
    for i = 1:numel(timesteps)
        predictedTracks(:,i+1) = predictTracksToTime(tracker,'confirmed',timesteps(i));
    end
    
    % Step 4. Update capsule list and plan highway trajectory
    currActorState = helperUpdateCapsuleList(capList, predictedTracks);
    [optimalTrajectory, trajectoryList] = helperPlanHighwayTrajectory(capList, currActorState, egoState);

    % Visualize the results
    display(scenario, egoVehicle, sensors, detections, tracks, capList, trajectoryList);

    % Step 5. Move ego on planned trajectory
    egoState = optimalTrajectory(2,:);
    helperMoveEgoToState(egoVehicle,egoState);
end

Resultados

En la siguiente animación, puedes observar las trayectorias planificadas de los vehículos del ego resaltadas en color verde. La animación también muestra todas las demás trayectorias muestreadas del vehículo ego. Para estas otras trayectorias, las trayectorias en colisión se muestran en rojo, las trayectorias no evaluadas se muestran en gris y las trayectorias cinemáticamente inviables se muestran en color cian. Cada pista está marcada por un ID que representa su identidad única. Observe que el vehículo ego maniobra con éxito alrededor de los obstáculos de la escena.

En las siguientes subsecciones, analizará las estimaciones del rastreador en determinados pasos de tiempo y comprenderá cómo afecta las decisiones tomadas por el planificador de movimiento.

Predicción de movimiento integrada en la carretera

En esta sección, aprenderá cómo el modelo de movimiento integrado en la carretera permite al rastreador obtener predicciones a largo plazo más precisas sobre los objetos en la carretera. A continuación se muestra una instantánea de la simulación tomada en el tiempo = 30 segundos. Observe la trayectoria predicha para el vehículo verde a la derecha del vehículo ego azul. La trayectoria prevista sigue el carril del vehículo porque la información de la red de carreteras está integrada con el rastreador. Si, en cambio, utiliza un supuesto de modelo de velocidad constante para los objetos, la trayectoria predicha seguirá la dirección de la velocidad instantánea y el planificador de movimiento la tratará falsamente como una colisión. En este caso, el planificador de movimiento puede generar una maniobra peligrosa.

showSnaps(display,2,4); % Shows snapshot while publishing

Figure contains an axes object and an object of type uipanel. The hidden axes object with xlabel X (m), ylabel Y (m) contains 26 objects of type patch, line, text. One or more of the lines displays its values using only markers These objects represent Tracks, (history), Radar Detections, Vision Detections.

Predicción de cambio de carril

En la primera sección, aprendió cómo se capturan las maniobras de cambio de carril utilizando un modelo de velocidad lateral decreciente de los objetos. Ahora, observe la instantánea tomada en el tiempo = 17,5 segundos. En este momento, el vehículo amarillo en el lado derecho del vehículo ego inicia un cambio de carril y tiene la intención de ingresar al carril del vehículo ego. Observe que su trayectoria prevista captura esta maniobra y el rastreador predice que estará en el mismo carril que el vehículo ego al final del horizonte de planificación. Esta predicción informa al planificador de movimiento sobre una posible colisión con este vehículo, por lo que el planificador primero procede a probar la viabilidad de que el vehículo ego cambie de carril a la izquierda. Sin embargo, la presencia del vehículo violeta a la izquierda y su trayectoria prevista hace que el vehículo ego cambie de carril a la derecha. También puede observar estas trayectorias de colisión coloreadas en rojo en la instantánea a continuación.

showSnaps(display,2,1); % Shows snapshot while publishing

Figure contains an axes object and an object of type uipanel. The hidden axes object with xlabel X (m), ylabel Y (m) contains 28 objects of type patch, line, text. One or more of the lines displays its values using only markers These objects represent Tracks, (history), Radar Detections, Vision Detections.

Imperfecciones del rastreador

Un rastreador multiobjeto puede tener ciertas imperfecciones que pueden afectar las decisiones de planificación de movimiento. Específicamente, un rastreador de objetos múltiples puede pasar por alto objetos, informar de seguimientos falsos o, a veces, informar de seguimientos redundantes. En la siguiente instantánea tomada en el tiempo = 20 segundos, el rastreador deja huellas en dos vehículos delante del vehículo ego debido a una oclusión. En esta situación particular, es menos probable que estos objetivos perdidos influyan en la decisión del planificador de movimiento debido a su distancia del vehículo ego.

showSnaps(display,2,2); % Shows snapshot while publishing

Figure contains an axes object and an object of type uipanel. The hidden axes object with xlabel X (m), ylabel Y (m) contains 24 objects of type patch, line, text. One or more of the lines displays its values using only markers These objects represent Tracks, (history), Radar Detections, Vision Detections.

Sin embargo, a medida que el vehículo ego se acerca a estos vehículos, aumenta su influencia en la decisión del vehículo ego. Observe que el rastreador puede establecer una pista en estos vehículos en un tiempo = 20,4 segundos, como se muestra en la siguiente instantánea, lo que hace que el sistema sea ligeramente robusto ante estas imperfecciones. Al configurar un algoritmo de seguimiento para la planificación del movimiento, es importante considerar estas imperfecciones del rastreador y ajustar las lógicas de confirmación y eliminación de la pista.

showSnaps(display,2,3); % Show snapshot while publishing

Resumen

Aprendió a utilizar un rastreador conjunto de asociación de datos probabilísticos para rastrear vehículos utilizando una ruta de referencia Frenet con sensores de radar y cámara. Configuró el rastreador para utilizar datos de mapas de carreteras para proporcionar predicciones a largo plazo sobre objetos. También utilizó estas predicciones a largo plazo para impulsar un planificador de movimiento para planificar trayectorias en la carretera.

Funciones de apoyo

function detections = helperGenerateDetections(sensors, egoVehicle, time)
    detections = cell(0,1);
    for i = 1:numel(sensors)
        thisDetections = sensors{i}(targetPoses(egoVehicle),time);
        detections = [detections;thisDetections]; %#ok<AGROW> 
    end

    detections = helperAddEgoVehicleLocalization(detections,egoVehicle);
    detections = helperPreprocessDetections(detections);
end

function detectionsOut = helperAddEgoVehicleLocalization(detectionsIn, egoPose)

defaultParams = struct('Frame','Rectangular',...
    'OriginPosition',zeros(3,1),...
    'OriginVelocity',zeros(3,1),...
    'Orientation',eye(3),...
    'HasAzimuth',false,...
    'HasElevation',false,...
    'HasRange',false,...
    'HasVelocity',false);

fNames = fieldnames(defaultParams);

detectionsOut = cell(numel(detectionsIn),1);

for i = 1:numel(detectionsIn)
    thisDet = detectionsIn{i};
    if iscell(thisDet.MeasurementParameters)
        measParams = thisDet.MeasurementParameters{1};
    else
        measParams = thisDet.MeasurementParameters(1);
    end

    newParams = struct;
    for k = 1:numel(fNames)
        if isfield(measParams,fNames{k})
            newParams.(fNames{k}) = measParams.(fNames{k});
        else
            newParams.(fNames{k}) = defaultParams.(fNames{k});
        end
    end

    % Add parameters for ego vehicle
    thisDet.MeasurementParameters = [newParams;newParams];
    thisDet.MeasurementParameters(2).Frame = 'Rectangular';
    thisDet.MeasurementParameters(2).OriginPosition = egoPose.Position(:);
    thisDet.MeasurementParameters(2).OriginVelocity = egoPose.Velocity(:);
    thisDet.MeasurementParameters(2).Orientation = rotmat(quaternion([egoPose.Yaw egoPose.Pitch egoPose.Roll],'eulerd','ZYX','frame'),'frame')';
    
    
    % No information from object class and attributes
    thisDet.ObjectClassID = 0;
    thisDet.ObjectAttributes = struct;
    detectionsOut{i} = thisDet;
end

end

function detections = helperPreprocessDetections(detections)
    % This function pre-process the detections from radars and cameras to
    % fit the modeling assumptions used by the tracker

    % 1. It removes velocity information from camera detections. This is
    % because those are filtered estimates and the assumptions from camera
    % may not align with defined prior information for tracker.
    %
    % 2. It fixes the bias for camera sensors that arise due to camera
    % projections for cars just left or right to the ego vehicle.
    % 
    % 3. It inflates the measurement noise for range-rate reported by the
    % radars to match the range-rate resolution of the sensor
    for i = 1:numel(detections)
        if detections{i}.SensorIndex > 1 % Camera
            % Remove velocity
            detections{i}.Measurement = detections{i}.Measurement(1:3);
            detections{i}.MeasurementNoise = blkdiag(detections{i}.MeasurementNoise(1:2,1:2),25);
            detections{i}.MeasurementParameters(1).HasVelocity = false;

            % Fix bias
            pos = detections{i}.Measurement(1:2);
            if abs(pos(1)) < 5 && abs(pos(2)) < 5
                [az, ~, r] = cart2sph(pos(1),pos(2),0);
                [pos(1),pos(2)] = sph2cart(az, 0, r + 0.7); % Increase range
                detections{i}.Measurement(1:2) = pos;
                detections{i}.MeasurementNoise(2,2) = 0.25;
            end
        else % Radars
            detections{i}.MeasurementNoise(3,3) = 0.5^2/4;
        end
    end
end

function helperMoveEgoToState(egoVehicle, egoState)
egoVehicle.Position(1:2) = egoState(1:2);
egoVehicle.Velocity(1:2) = [cos(egoState(3)) sin(egoState(3))]*egoState(5);
egoVehicle.Yaw = egoState(3)*180/pi;
egoVehicle.AngularVelocity(3) = 180/pi*egoState(4)*egoState(5);
end