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

Этот пример использует:

Открыть Live Script

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

Введение

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

Обнаружение полос в сценарии изменения маршрута

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

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

Чтобы визуализировать сценарий в Driving Scenario Designer, используйте:

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

initSingerLane Задайте модель движения Singer для пограничного фильтра маршрута

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

Документация по Sensor Fusion and Tracking Toolbox

Поддержка

Для просмотра документации необходимо авторизоваться на сайте