Отследите несколько контуров маршрута с глобальным самым близким соседним средством отслеживания

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

Скрипт Open 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 Задают модель движения Зингера для фильтра контура маршрута

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

Поддержка

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