В этом примере показано, как спроектировать и протестировать алгоритм отслеживания маршрута кратного. Алгоритм тестируется в ведущем сценарии с вероятностными обнаружениями маршрута.
Автоматизированная система маневра изменения маршрута (LCM) позволяет автомобилю, оборудованному датчиком автоматически переместиться от одной полосы до другого маршрута. Чтобы успешно перестроиться на другую полосу, система требует локализации автомобиля, оборудованного датчиком относительно стационарных функций, таких как маркировки маршрута. Алгоритм обнаружения маршрута обычно обеспечивает смещение и информацию об искривлении о текущих и смежных контурах маршрута. Во время изменения маршрута разрыв в боковом смещении вводится в обнаружения маршрута, потому что боковое смещение всегда относительно текущего маршрута, в котором перемещается транспортное средство. Разрыв и скачок в значениях смещения могут заставить систему LCM становиться нестабильной. Один метод, чтобы компенсировать эту проблему разрыва должен использовать многополосное средство отслеживания.
Вы загружаете drivingScenario объект, scenario, это содержит автомобиль, оборудованный датчиком и его датчики от LaneTrackingScenario.mat файл. Вы используете visionDetectionGenerator объект обнаружить маршруты в сценарии.
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 Объект (Sensor Fusion and Tracking Toolbox). Чтобы удалить необнаруженные контуры маршрута быстро, установите средство отслеживания удалять прослеженные маршруты после трех промахов в трех обновлениях. Также определите максимальный номер дорожек к 10.
Используйте singer (Sensor Fusion and Tracking Toolbox) ускоряющая модель, чтобы смоделировать путь контуры маршрута изменяется в зависимости от времени. Ускоряющая модель Зингера позволяет ускорениям модели, которые затухают со временем, и можно установить уровень затухания с помощью затухания постоянный tau. Вы используете initSingerLane функция изменяется от initsingerekf (Sensor Fusion and Tracking Toolbox) функция путем установки затухания постоянный 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
drivingScenario | visionDetectionGenerator | singer (Sensor Fusion and Tracking Toolbox) | trackerGNN (Sensor Fusion and Tracking Toolbox)
