Введение

Динамическое перепланирование автономных транспортных средств обычно делается с локальным планировщиком движения. Локальный планировщик движения ответственен за генерацию оптимальной траектории на основе глобального плана и информации в реальном времени об окружающей среде. Глобальный план относительно магистрального планирования траектории может быть описан как предсгенерированный координатный список магистральной средней линии. Окружающая среда может быть описана в основном двумя способами:

В присутствии динамических препятствий локальный планировщик движения также требует, чтобы предсказания о среде оценили валидность запланированных траекторий. В этом примере вы представляете окружающую среду с помощью дискретного набора подхода объектов. Для примера с помощью дискретизированной сетки обратитесь к Движению, Планирующему в Городских Средах Используя Динамическую Карту Сетки Заполнения (Sensor Fusion and Tracking Toolbox) пример.

Объектное моделирование изменения состояния и измерения

Список объектов и их будущие предсказания для планирования движения обычно оцениваются мультиобъектным средством отслеживания. Мультиобъектное средство отслеживания принимает данные из датчиков и оценивает список объектов. В сообществе отслеживания этот список объектов часто называют как список дорожек.

В этом примере вы используете датчики радара и камеры и оцениваете список дорожек с помощью средства отслеживания мультиобъекта JPDA. Первый шаг к использованию любого мультиобъектного средства отслеживания задает объектное состояние, как состояние развивается со временем (модель изменения состояния) и как датчик чувствует его (модель измерения). Общие модели изменения состояния включают модель постоянной скорости, модель постоянного ускорения и т.д. Однако в присутствии информации о карте, дорожная сеть может быть интегрирована в модель движения. В этом примере вы используете систему координат Frenet, чтобы описать объектное состояние, в любой момент времени продвигаются, $\mathit{k}$.

где ${\mathit{s}}_{\mathit{k}}$ и ${\mathit{d}}_{\mathit{k}}$представляет расстояние объекта вперед и перпендикуляра к магистральной средней линии, соответственно. Вы используете модель изменения состояния постоянной скорости, чтобы описать объектное движение вдоль магистрали и модели скорости затухания, чтобы описать перпендикуляр движения к магистральной средней линии. Эта затухающая модель скорости позволяет вам представлять маневры изменения маршрута другими транспортными средствами на магистрали.

где $\Delta \mathit{T}$ разница во времени между шагами $\mathit{k}$ и $\mathit{k}+1$, ${\mathit{w}}_{\mathit{s}}$ и ${\mathit{w}}_{\mathit{d}}$ нулевой средний Гауссов шум, представляющий неизвестное ускорение в координатах Frenet, и $\tau$ постоянное затухание.

Этот выбор координаты в моделировании объектного движения позволяет вам интегрировать магистральный путь к ссылке в среду отслеживания мультиобъекта. Интегрирование ссылочного пути действует как дополнительная информация для средства отслеживания и позволяет средству отслеживания улучшать оценки текущего состояния, а также предсказанные траектории предполагаемых объектов. Можно получить модель измерения первым преобразованием объектного состояния в Декартово положение и скорость и затем преобразование их к соответствующим измеренным количествам, таким как азимут и область значений.

Настройка

Сценарий и датчики

Сценарий, используемый в этом примере, создан с помощью Driving Scenario Designer и затем экспортирован в функцию MATLAB®. Автомобиль, оборудованный датчиком смонтирован с 1 перспективным радаром и 5 камерами, предоставляющими полную страховую защиту. Радар и камеры симулированы с помощью drivingRadarDataGenerator и visionDetectionGenerator Системные объекты, соответственно.

Целая настройка сценария и датчика задана в функции помощника, helperTrackingAndPlanningScenario, присоединенный с этим примером. Вы задаете глобальный план, описывающий магистральную среднюю линию с помощью referencePathFrenet Объект (Navigation Toolbox). Когда для нескольких алгоритмов в этом примере нужен доступ к ссылочному пути, вы задаете helperGetReferencePath функция, которая использует persistent объект, к которому может получить доступ любая функция.

rng(2022); % For reproducible results

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

Соедините вероятностное средство отслеживания ассоциации данных

Вы настраиваете объединенное вероятностное средство отслеживания ассоциации данных с помощью trackerJPDA (Sensor Fusion and Tracking Toolbox) Системный объект. Вы устанавливаете FilterInitializationFcn свойство средства отслеживания к helperInitRefPathFilter функция. Эта функция помощника задает расширенный Фильтр Калмана, trackerJPDA (Sensor Fusion and Tracking Toolbox), используемый, чтобы оценить состояние отдельного объекта. Локальные функции в helperInitRefPathFilter файл задает модель изменения состояния, а также измерения для фильтра. Далее, чтобы предсказать дорожки в будущее время для планировщика движения, вы используете predictTracksToTime (Sensor Fusion and Tracking Toolbox) функция средства отслеживания.

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

Планировщик движения

Вы используете подобного магистрального планировщика движения траектории, как обрисовано в общих чертах в Магистральном Планировании Траектории Используя Путь к Ссылке Frenet (Navigation Toolbox) пример. Планировщик движения использует горизонт планирования 5 секунд и рассматривает три режима для выборки траекторий для автомобиля, оборудованного датчиком — круиз-контроль, ведущее транспортное средство следует, и основное изменение маршрута. Целый процесс для генерации оптимальной траектории перенесен в функцию помощника, helperPlanHighwayTrajectory.

Функция помощника принимает dynamicCapsuleList Объект (Navigation Toolbox) как вход, чтобы найти несталкивающиеся траектории. Проверка столкновения выполняется в целом горизонте планирования в интервале 0,5 секунд. Когда состояния дорожки меняются в зависимости от времени, вы обновляете dynamicCapsuleList объект в цикле симуляции с помощью helperUpdateCapsuleList функция, присоединенная с этим примером.

% 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);

Запустите симуляцию

В этом разделе вы совершенствуете симуляцию, генерируете данные о датчике и выполняете динамическое перепланирование с помощью оценок о среде. Целый процесс разделен на 5 основных шагов:

% 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

Результаты

В анимации ниже, можно наблюдать запланированные траектории автомобиля, оборудованного датчиком, подсвеченные в зеленом цвете. Анимация также показывает все другие произведенные траектории для автомобиля, оборудованного датчиком. Для этих других траекторий сталкивающиеся траектории показывают в красных, неоцененных траекториях, показаны в серых, и кинематическим образом неосуществимых траекториях, отображены голубым цветом. Каждая дорожка аннотируется ID, представляющим его уникальную идентичность. Заметьте, что автомобиль, оборудованный датчиком успешно маневрирует вокруг препятствий в сцене.

В следующих подразделах вы анализируете оценки от средства отслеживания на определенных временных шагах и изучаете, как оно влияет на выбор, сделанный планировщиком движения.

Интегрированное с дорогой предсказание движения

В этом разделе вы изучаете, как интегрированная с дорогой модель движения позволяет средству отслеживания получать более точные долгосрочные предсказания об объектах на магистрали. Показанный ниже снимок состояния от симуляции, взятой во время = 30 секунд. Заметьте траекторию, предсказанную для зеленого транспортного средства справа от синего автомобиля, оборудованного датчиком. Предсказанная траектория следует за маршрутом транспортного средства, потому что информация о дорожной сети интегрирована со средством отслеживания. Если вместо этого, вы будете использовать предположение модели постоянной скорости для объектов, предсказанная траектория будет следовать за направлением мгновенной скорости и будет ложно обработана как столкновение планировщиком движения. В этом случае планировщик движения может возможно сгенерировать небезопасный маневр.

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

Предсказание изменения маршрута

В первом разделе вы изучили, как маневры изменения маршрута получены при помощи затухающей модели поперечной скорости объектов. Теперь заметьте снимок состояния, взятый во время = 17,5 секунд. В это время желтое транспортное средство на правой стороне автомобиля, оборудованного датчиком инициирует изменение маршрута и намеревается ввести маршрут автомобиля, оборудованного датчиком. Заметьте, что его предсказанная траектория получает этот маневр, и средство отслеживания предсказывает его, чтобы быть в том же маршруте как автомобиль, оборудованный датчиком в конце планирования горизонта. Это предсказание сообщает планировщику движения о возможном столкновении с этим транспортным средством, таким образом планировщик сначала продолжает тестировать выполнимость на автомобиль, оборудованный датчиком, чтобы изменить маршрут налево. Однако присутствие фиолетового транспортного средства слева и его предсказанной траектории заставляет автомобиль, оборудованный датчиком вносить правильное изменение маршрута. Можно также наблюдать эти сталкивающиеся траектории, окрашенные как красные в снимке состояния ниже.

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

Недостатки средства отслеживания

Мультиобъектное средство отслеживания может иметь определенные недостатки, которые могут влиять на решения планирования движения. А именно, мультиобъектное средство отслеживания может пропустить объекты, сообщить о ложных дорожках, или иногда сообщать об избыточных дорожках. В снимке состояния ниже взятого во время = 20 секунд, средство отслеживания пропускает дорожки на два транспортных средства перед автомобилем, оборудованным датчиком из-за поглощения газов. В этой конкретной ситуации эти пропущенные цели, менее вероятно, будут влиять на решение планировщика движения из-за их расстояния от автомобиля, оборудованного датчиком.

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

Однако, когда автомобиль, оборудованный датчиком приближается к этим транспортным средствам, их влиянию на увеличения решения автомобиля, оборудованного датчиком. Заметьте, что средство отслеживания может установить дорожку на этих транспортных средствах ко времени = 20,4 секунды, как показано в снимке состояния ниже, таким образом делая систему немного устойчивой к этим недостаткам. При конфигурировании алгоритма отслеживания для планирования движения важно рассмотреть эти недостатки от средства отслеживания и настроить подтверждение дорожки и логики удаления дорожки.

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

Сводные данные

Вы изучили, как использовать объединенное вероятностное средство отслеживания ассоциации данных, чтобы отследить транспортные средства с помощью пути к ссылке Frenet с датчиками радара и камеры. Вы сконфигурировали средство отслеживания, чтобы использовать магистральные данные о карте, чтобы предоставить долгосрочные предсказания об объектах. Вы также использовали эти долгосрочные предсказания, чтобы управлять планировщиком движения для планирования траекторий на магистрали.

Вспомогательные Функции

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