В этом примере показано, как спроектировать и протестировать несколько алгоритмов отслеживания маршрута. Алгоритм проверяется в сценарии вождения с вероятностными обнаружениями маршрута.
Система автоматического маневра смены маршрута (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
