Отслеживание границ нескольких полос с помощью глобального ближайшего соседнего трекера

В этом примере используются:

В этом примере показано, как проектировать и тестировать алгоритм отслеживания нескольких полос движения. Алгоритм тестируется в сценарии вождения с вероятностными обнаружениями полосы движения.

Введение

Автоматизированная система маневра изменения полосы движения (LCM) позволяет эго-транспортному средству автоматически перемещаться с одной полосы движения на другую. Для успешного изменения полос движения система требует локализации эго-транспортного средства относительно стационарных элементов, таких как разметка полос движения. Алгоритм обнаружения полосы движения обычно предоставляет информацию о смещении и кривизне текущей и смежных границ полосы движения. Во время изменения полосы, разрыв в боковом смещении вводится в детекторы полосы, потому что боковое смещение всегда относительно текущей полосы, по которой движется транспортное средство. Нарушение непрерывности и скачок значений смещения могут привести к тому, что система LCM станет нестабильной. Одной из методик компенсации этой проблемы нарушения непрерывности является использование многополосного трекера.

Определение полос движения в сценарии изменения полосы движения

Вы загружаете drivingScenario (Автоматизированная панель инструментов вождения), scenario, который содержит эго-транспортное средство и его датчики из LaneTrackingScenario.mat файл. Вы используете visionDetectionGenerator Объект (Automated Driving Toolbox) для обнаружения полос движения в сценарии.

load('LaneTrackingScenario.mat','scenario','egoVehicle','sensors');
laneDetector = sensors{1};

Для визуализации сценария в конструкторе сценариев управления используется следующее:

drivingScenarioDesigner(scenario)

В этом сценарии эго-автомобиль едет по криволинейной дороге с несколькими полосами движения. Эго-транспортное средство оснащено детектором полосы движения, который обнаруживает границы полосы движения и сообщает о двух границах полосы движения с каждой стороны эго-транспортного средства. Для прохождения более медленного движущегося транспортного средства, движущегося по эго-полосе, эго-транспортное средство меняет полосы движения с исходной полосы движения на полосу движения слева. Измерение, сообщаемое детектором полосы движения, содержит смещение, курс и кривизну полосы движения.

Следующий блок кода запускает сценарий и отображает результаты обнаружения полосы движения.

% Setup plotting area
egoCarBEP = createDisplay(scenario,egoVehicle);

% Setup data logs
timeHistory = 0:scenario.SampleTime:(scenario.StopTime-scenario.SampleTime);
laneDetectionOffsetHistory = NaN(5,length(timeHistory));
timeStep = 1;
restart(scenario)
running = true;
while running
    % Simulation time
    simTime  = scenario.SimulationTime;
    
    % Get the ground truth lane boundaries in the ego vehicle frame
    groundTruthLanes = laneBoundaries(egoVehicle,'XDistance',0:5:70,'AllBoundaries',true);
    [egoBoundaries,egoBoundaryExist] = findEgoBoundaries(groundTruthLanes); %% ego lane and adjacent lanes ground truth
    laneBoundaryDetections = laneDetector(egoBoundaries(egoBoundaryExist),simTime); %% ego lane and adjacent lanes detections
    laneObjectDetections = packLanesAsObjectDetections(laneBoundaryDetections); %% convert lane boundary to object detections
    
    % Log lane detections
    for laneIDX = 1:length(laneObjectDetections)
        laneDetectionOffsetHistory(laneIDX,timeStep) = laneObjectDetections{laneIDX}.Measurement(1);
    end
    
    % Visualization road and lane ground truth in ego frame
    updateDisplay(egoCarBEP,egoVehicle,egoBoundaries);
    
    % Advance to the next step
    timeStep = timeStep + 1;
    running = advance(scenario);
end

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

f=figure;
plot(timeHistory,laneDetectionOffsetHistory(1:4,:),'LineWidth',2)
xlabel('Time (s)')
ylabel('Lane detections lateral offset (m)')
legend('Adjacent left', 'Left', 'Right', 'Adjacent Right','Orientation','horizontal','Location','northoutside')
grid
p = snapnow;

close(f)

Определение многополосного трекера и полос движения

Определение многополосного трекера с помощью trackerGNN объект. Чтобы быстро удалить необнаруженные границы полос, настройте трекер на удаление отслеживаемых полос после трех промахов в трех обновлениях. Также установите максимальное количество дорожек равным 10.

Используйте singer модель ускорения для моделирования изменения границ полосы движения с течением времени. Модель ускорения Сингера позволяет моделировать ускорения, которые замедляются со временем, и можно задать скорость распада с помощью константы распада tau. Вы используете initSingerLane функция, измененная из initsingerekf функция путем установки константы затухания tau 1, поскольку время маневра изменения полосы относительно мало. Функция прикрепляется в конце сценария. Обратите внимание, что тремя размерами, определенными для состояния ускорения Сингера, являются смещение, курс и кривизна границы полосы движения, которые совпадают с теми, о которых сообщается при обнаружении полосы движения.

laneTracker = trackerGNN('FilterInitializationFcn', @initSingerLane, 'DeletionThreshold', [3 3], 'MaxNumTracks',10);

Повторно запустите сценарий для отслеживания границ полосы движения.

laneTrackOffsetHistory = NaN(5,length(timeHistory));
timeStep = 1;
restart(scenario)
restart(egoVehicle);
reset(laneDetector);
running = true;
while running
    % Simulation time
    simTime  = scenario.SimulationTime;
    
    % Get the ground truth lane boundaries in the ego vehicle frame
    groundTruthLanes = laneBoundaries(egoVehicle,'XDistance',0:5:70,'AllBoundaries',true);
    [egoBoundaries,egoBoundaryExist] = findEgoBoundaries(groundTruthLanes); %% ego lane and adjacent lanes ground truth
    laneBoundaryDetections = laneDetector(egoBoundaries(egoBoundaryExist),simTime); %% ego lane and adjacent lanes detections
    laneObjectDetections = packLanesAsObjectDetections(laneBoundaryDetections); %% convert lane boundary to object detections
    
    % Track the lanes
    laneTracks = laneTracker(laneObjectDetections,simTime);
    
    % Log data
    timeHistory(timeStep) = simTime;
    for laneIDX = 1:length(laneTracks)
        laneTrackOffsetHistory(laneTracks(laneIDX).TrackID,timeStep) = laneTracks(laneIDX).State(1);
    end
    
    % Visualization road and lane ground truth in ego frame
    updateDisplay(egoCarBEP,egoVehicle,egoBoundaries);
    
    % Advance to the next step
    timeStep = timeStep + 1;
    running = advance(scenario);
end

Постройте график бокового смещения отслеживаемых границ полосы движения. Обратите внимание, что отслеживаемые границы полосы являются непрерывными и не разрываются, когда эго-транспортное средство выполняет маневр смены полосы.

figure
plot(timeHistory,laneTrackOffsetHistory(:,:),'LineWidth',2)
xlabel('Time (s)')

ylabel('Lane tracks lateral offset (m)')
legend('Lane1', 'Lane2', 'Lane3', 'Lane4', 'Lane5', 'Orientation','horizontal','Location','northoutside')
grid

Резюме

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

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

createDisplay Создание экрана для этого примера

function egoCarBEP = createDisplay(scenario,egoVehicle)
hFigure = figure;
hPanel1 = uipanel(hFigure,'Units','Normalized','Position',[0 1/4 1/2 3/4],'Title','Scenario Plot');
hPanel2 = uipanel(hFigure,'Units','Normalized','Position',[0 0 1/2 1/4],'Title','Chase Plot');
hPanel3 = uipanel(hFigure,'Units','Normalized','Position',[1/2 0 1/2 1],'Title','Bird''s-Eye Plot');
hAxes1 = axes('Parent',hPanel1);
hAxes2 = axes('Parent',hPanel2);
hAxes3 = axes('Parent',hPanel3);
legend(hAxes3,'AutoUpdate','off')
scenario.plot('Parent',hAxes1)     % plot is a method of drivingScenario Class
chasePlot(egoVehicle,'Parent',hAxes2); % chase plot following the egoVehicle
egoCarBEP = birdsEyePlot('Parent',hAxes3,'XLimits',[-10 70],'YLimits',[-40 40]);
% Set up plotting type
outlinePlotter(egoCarBEP,'Tag','Platforms');
laneBoundaryPlotter(egoCarBEP,'Tag','Roads');
laneBoundaryPlotter(egoCarBEP,'Color','r','LineStyle','-','Tag','Left1');
laneBoundaryPlotter(egoCarBEP,'Color','g','LineStyle','-','Tag','Right1');
laneBoundaryPlotter(egoCarBEP,'Color','r','LineStyle','-','Tag','Left2');
laneBoundaryPlotter(egoCarBEP,'Color','g','LineStyle','-','Tag','Right2');
laneMarkingPlotter(egoCarBEP,'DisplayName','Lane markings','Tag','LaneMarkings');
coverageAreaPlotter(egoCarBEP,'DisplayName','Vision coverage','FaceAlpha',0.1,'FaceColor','b','EdgeColor','b','Tag','Vision');
end

updateDisplay Обновить экран для этого примера

function updateDisplay(egoCarBEP,egoVehicle,LaneBdryIn)
[position,yaw,leng,width,originOffset,color] = targetOutlines(egoVehicle); 
outlineplotter = findPlotter(egoCarBEP,'Tag','Platforms');
plotOutline(outlineplotter, position, yaw, leng, width, ...
    'OriginOffset',originOffset,'Color',color)
rbdry = egoVehicle.roadBoundaries;
roadPlotter = findPlotter(egoCarBEP,'Tag','Roads');
roadPlotter.plotLaneBoundary(rbdry)
lbllPlotter = findPlotter(egoCarBEP,'Tag','Left2');
plotLaneBoundary(lbllPlotter,{LaneBdryIn(1).Coordinates})
lblPlotter = findPlotter(egoCarBEP,'Tag','Left1');
plotLaneBoundary(lblPlotter,{LaneBdryIn(2).Coordinates})
lbrPlotter = findPlotter(egoCarBEP,'Tag','Right1');
plotLaneBoundary(lbrPlotter,{LaneBdryIn(3).Coordinates})
lbrrPlotter = findPlotter(egoCarBEP,'Tag','Right2');
plotLaneBoundary(lbrrPlotter,{LaneBdryIn(4).Coordinates})
end

findEgoBoundaries Возврат двух ближайших границ полосы движения с каждой стороны эго-транспортного средства

function [egoBoundaries,egoBoundaryExist] = findEgoBoundaries(groundTruthLanes)
%findEgoBoundaries  Find the two adjacent lane boundaries on each side of the ego
%  [egoBoundaries,egoBoundaryExist] = findEgoBoundaries(groundTruthLanes)
%  egoBoundaries - A 4x1 struct of lane boundaries ordered as: adjacent
%  left, left, right, and adjacent right
egoBoundaries = groundTruthLanes(1:4);
lateralOffsets = [groundTruthLanes.LateralOffset];
[sortedOffsets, inds] = sort(lateralOffsets);
egoBoundaryExist = [true;true;true;true];

% Left lane and left adjacent lane
idxLeft = find(sortedOffsets>0,2,'first');
numLeft = length(idxLeft);
egoBoundaries(2) = groundTruthLanes(inds(idxLeft(1)));
if numLeft>1
    egoBoundaries(1) = groundTruthLanes(inds(idxLeft(2)));
else % if left adjacent lane does not exist
    egoBoundaries(1) = egoBoundaries(2);
    egoBoundaryExist(1) = false;
end

% Right lane and right adjacent lane
idxRight = find(sortedOffsets<0,2,'last');
numRight = length(idxRight);
egoBoundaries(3) = groundTruthLanes(inds(idxRight(end)));
if numRight>1
    egoBoundaries(4) = groundTruthLanes(inds(idxRight(1)));
else % if right adjacent lane does not exist
    egoBoundaries(4) = egoBoundaries(3);
    egoBoundaryExist(4) = false;
end
end

packLanesAsObjectDetections Обнаружение границ полосы возврата в виде массива ячеек objectDetection объекты

function laneObjectDetections = packLanesAsObjectDetections(laneBoundaryDetections)
%packLanesAsObjectDetections  Packs lane detections as a cell array of objectDetection
laneStrengths = [laneBoundaryDetections.LaneBoundaries.Strength];
IdxValid = find(laneStrengths>0);
numLaneDetections = length(IdxValid);
meas = zeros(3,1);
measurementParameters = struct(...
    'Frame', 'rectangular', ...
    'OriginPosition', [0 0 0]', ...
    'Orientation', eye(3,3), ...
    'HasVelocity', false, ...
    'HasElevation', false);

detection = objectDetection(laneBoundaryDetections.Time,meas, ...
    'MeasurementNoise', eye(3,3)/10, ...
    'SensorIndex', laneBoundaryDetections.SensorIndex, ...
    'MeasurementParameters' ,measurementParameters);
laneObjectDetections = repmat({detection},numLaneDetections,1);
for i = 1:numLaneDetections
    meas = [laneBoundaryDetections.LaneBoundaries(IdxValid(i)).LateralOffset ...
        laneBoundaryDetections.LaneBoundaries(IdxValid(i)).HeadingAngle/180*pi ...
        laneBoundaryDetections.LaneBoundaries(IdxValid(i)).Curvature/180*pi];
    laneObjectDetections{i}.Measurement = meas;
end
end

initLaneLane Определение модели движения Сингера для фильтра границ полосы движения

function filter = initSingerLane(detection)
filter = initsingerekf(detection);
tau = 1;
filter.StateTransitionFcn = @(state,dt)singer(state,dt,tau);
filter.StateTransitionJacobianFcn = @(state,dt)singerjac(state,dt,tau);
filter.ProcessNoise = singerProcessNoise(zeros(9,1),1,tau,1);
end

Документация