В этом примере показано, как сгенерировать ведущий сценарий путем извлечения зарегистрированных данных из систем глобального позиционирования (GPS) и датчиков лидара, смонтированных на автомобиле, оборудованном датчиком. В этом примере вы используете записанные заранее данные о датчике и выполняете эти шаги, чтобы сгенерировать сценарий:
Считайте данные из датчика GPS. Используйте данные о GPS для:
Импортируйте данные о карте от OpenStreetMap®.
Извлеките дорожную информацию и управляющий маршрутом из импортированных данных о карте.
Вычислите waypoints для автомобиля, оборудованного датчиком.
Оцените скорость эго и траекторию.
Считайте данные из датчика лидара. Используйте данные о лидаре для:
Найдите размерность и тип агентов неэго в ведущей сцене.
Оцените времена записи, выходные времена, положения, скорости, скорости, рыскания и траектории агентов неэго при помощи данных о лидаре.
Сгенерируйте ведущий сценарий при помощи извлеченной дорожной сети и предполагаемого эго и данных неэго.
Создайте папку во временной директории на вашем компьютере, чтобы хранить данные о датчике.
sensorData = fullfile(tempdir,"AutomotiveDataset"); if ~isfolder(sensorData) mkdir(sensorData) end
Загрузите .mat файл, содержащий записанные данные о датчике GPS и, сохраняет его в заданную папку во временной директории. Загрузите данные о датчике GPS из .mat файл в рабочую область MATLAB®.
if ~exist("data","var") url = "https://ssd.mathworks.com/supportfiles/driving/data/highwayLaneChangeDatase/ins2.mat"; filePath = fullfile(sensorData,"ins.mat"); if ~isfile(filePath) websave(filePath,url); end data = load(filePath); end
Отобразите данные о датчике GPS. Данные хранятся как структура с полями timeStamp, velocity, yawRate, latitude, longitude, и altitude. Широта и координаты долготы задают ведущий маршрут автомобиля, оборудованного датчиком.
data.ins
ans=1×1158 struct array with fields:
timeStamp
velocity
yawRate
latitude
longitude
altitude
Выберите часть записанных данных о датчике путем определения значений метки времени начала и конца. Модули находятся в микросекундах. Как правило, модули зависят от настройки регистрации данных датчиков.
sim.startTime = 1461634424950000; sim.endTime = 1461634434950000;
Вычислите количество точек данных в заданных метках времени начала и конца. Кроме того, найдите индексы начала и конца меток времени при помощи helperTimestampIndex функция.
[count.gps,sim.gps.startstep,sim.gps.endstep] = helperTimestampIndex([data.ins.timeStamp].',sim.startTime,sim.endTime);
Извлеките метку времени, широту, долготу и параметры вертикального изменения всех точек данных между индексами начала и конца. Сохраните извлеченные параметры в таблице.
data.ins = data.ins(sim.gps.startstep : sim.gps.endstep); gps = table([data.ins.timeStamp].',[data.ins.latitude].',[data.ins.longitude].',[data.ins.altitude].',VariableNames=["timestamp","lat","lon","elevation"]);
Задайте минимальные и максимальные значения координат широты и долготы, для выбранных данных, чтобы загрузить дорожную сеть с помощью веб-сайта OpenStreetMap®. Можно также задать значение смещения для того, чтобы извлечь расширенные данные о карте, которые находятся в области значений датчика GPS.
map.gpsExtend = 0.0001; map.minLat = min(gps.lat) - map.gpsExtend; map.maxLat = max(gps.lat) + map.gpsExtend; map.minLon = min(gps.lon) - map.gpsExtend; map.maxLon = max(gps.lon) + map.gpsExtend;
Загрузите данные о карте из веб-сайта OpenStreetMap® https://www.openstreetmap.org, который обеспечивает доступ к полученным толпой данным о карте со всего мира. Данные лицензируются под Открытыми Данными палата общин Открытая Лицензия Базы данных (ODbL), https://opendatacommons.org/licenses/odbl/.
Извлеките данные о карте из минимальных и максимальных координат широты и долготы. Задайте имя файла, чтобы сохранить загруженные данные о карте. В этом примере имя файла установлено как имя дороги в ведущем маршруте.
url = ['https://api.openstreetmap.org/api/0.6/map?bbox=' ... num2str(map.minLon, '%.10f') ',' num2str(map.minLat,'%.10f') ',' ... num2str(map.maxLon, '%.10f') ',' num2str(map.maxLat,'%.10f')]; fileName = "BanfieldFreeway.osm"; websave(fileName,url,weboptions(ContentType="xml"));
Постройте ведущий маршрут на карте при помощи geoPlayer функция.
map.midLat = (map.minLat + map.maxLat)/2; map.midLon = (map.minLon + map.maxLon)/2; zoomLevel = 17; player = geoplayer(map.midLat,map.midLon,zoomLevel); figure plotRoute(player,gps.lat,gps.lon); for i = 1:count.gps plotPosition(player,gps.lat(i),gps.lon(i)); end
Чтобы получить информацию от извлеченной дорожной сети, необходимо сначала импортировать данные о карте к drivingScenario объект. Затем используйте helperGetRoadHistoryAttributes функционируйте, чтобы извлечь дорожную информацию.
Вычислите общее время симуляции при помощи меток времени начала и конца данных о GPS. Величина в секундах.
sim.TotalTime = (sim.endTime - sim.startTime)/10^6;
Задайте шаг расчета для ведущего сценария. Шаг расчета является временным интервалом между шагами симуляции сценария, и также задает шаг расчета для автомобиля, оборудованного датчиком. Величина в секундах.
sim.egoSampleTime = 0.01;
Создайте пустой ведущий объект сценария при помощи drivingScenario функция.
importedScenario = drivingScenario(SampleTime=sim.egoSampleTime,StopTime=sim.TotalTime);
Импортируйте извлеченные данные о карте к ведущему сценарию при помощи roadNetwork функция.
roadNetwork(importedScenario,OpenStreetMap=fileName);
Считайте дорожную информацию, хранившую в объекте сценария при помощи helperGetRoadHistoryAttributes функция. Дорожные свойства включают дорожные центры, дорожную ширину, банковские углы, технические требования маршрута и дорожные имена.
[roadCenters,roadWidth,bankingAngles,laneSpec,roadNames] = helperGetRoadHistoryAttributes(importedScenario);
Создайте новый drivingScenario возразите и добавьте дороги к новому сценарию. Установите свойства этих дорог при помощи извлеченной дорожной информации.
Задайте шаг расчета для нового сценария.
sim.sampleTime = 0.1; scenario = drivingScenario(SampleTime=sim.sampleTime,StopTime=sim.TotalTime);
Задайте геоконтрольную точку и используйте latlon2local функционируйте, чтобы преобразовать данные о датчике GPS от координат GPS до локальных Декартовых координат "восточный север".
refPoint = [map.midLat map.midLon,0];
[gps.x,gps.y,gps.elevation] = latlon2local(gps.lat,gps.lon,gps.elevation,refPoint);
filteredData = smoothdata([gps.x,gps.y gps.elevation],'sgolay');
gps.x = filteredData(:,1);
gps.y = filteredData(:,2);
gps.elevation = filteredData(:,3);Задайте ограничительную рамку, которые задают область значений для координат карты.
map.localExtend = 50; map.xmin = min(gps.x) - map.localExtend; map.xmax = max(gps.x) + map.localExtend; map.ymin = min(gps.y) - map.localExtend; map.ymax = max(gps.y) + map.localExtend;
Задайте имена дорог в экспортируемой дорожной сети, которая будет добавлена к сценарию. Можно найти имена дорог в roadNames выход helperGetRoadHistoryAttributes функция.
map.keepRoads = "Banfield Freeway";Проверяйте, ли желаемые дороги в ограничительной рамке и, если они, добавьте их в ведущий сценарий.
for i = 1:size(roadNames,1) flag = 0; for j = 1: size(map.keepRoads,1) if contains(roadNames{i,1},map.keepRoads(j,1),IgnoreCase=true) flag = 1; end end if flag k = 0; for l = 1:size(roadCenters{i,1},1) k = k+1; % Remove road centers that lie outside the bounding box. if roadCenters{i,1}(k,1) < map.xmin || ... roadCenters{i,1}(k,1) > map.xmax || ... roadCenters{i,1}(k,2) < map.ymin || ... roadCenters{i,1}(k,2) > map.ymax roadCenters{i,1}(k,:) = []; k = k-1; end end if k > 1 % Add roads by using the road centers. Set the road width to 12. roadwidth = 12; road(scenario,roadCenters{i,1},roadwidth,Name=roadNames{i,1}); end end end
Данные о датчике GPS соответствуют автомобилю, оборудованному датчиком, таким образом, можно использовать измерения GPS, чтобы вычислить автомобиль, оборудованный датчиком waypoints и скорость.
Значения широты и долготы задают waypoints для автомобиля, оборудованного датчиком в координатах GPS. Вычислите время GPS относительно времени начала симуляции и найдите значение скорости эго в каждом waypoint.
gps.relativeTime = double(gps.timestamp - sim.startTime)/10^6; gps.speed = helperComputeSpeed(gps.x,gps.y,gps.relativeTime);
Добавьте автомобиль, оборудованный датчиком в импортированную дорожную сеть и вычислите ее траекторию.
egoVehicleGPS = vehicle(importedScenario,ClassID=1,Position=[gps.x(1),gps.y(1),0]); trajectory(egoVehicleGPS,[gps.x,gps.y,zeros(count.gps,1)],gps.speed);
Извлеките информацию о положении, скорости и рыскании автомобиля, оборудованного датчиком из импортированного сценария при помощи actorPoses функция.
positionIndex = [1 3 6]; velocityIndex = [2 4 7]; i = 1; while advance(importedScenario) position(1,:) = actorPoses(importedScenario).Position; simEgo.data(i,positionIndex) = position; velocity(1,:) = actorPoses(importedScenario).Velocity; simEgo.data(i,velocityIndex) = velocity; simEgo.data(i,5) = i; yaw = actorPoses(importedScenario).Yaw; simEgo.data(i,8) = yaw; simEgo.data(i,9) = importedScenario.SimulationTime; i = i + 1; end
Вычисленные значения данных эго включают время, waypoints, скорость и рыскание автомобиля, оборудованного датчиком. Храните данные об эго к таблице и смотрите значения.
table(simEgo.data(:,9),simEgo.data(:,positionIndex), ... simEgo.data(:,velocityIndex),simEgo.data(:,8), ... VariableNames=["Time","Waypoints","Velocity","Yaw"])
ans=995×4 table
Time Waypoints Velocity Yaw
____ _____________________________ ____________________________ _______
0.01 -106.57 64.44 0 24.011 -8.6344 0 -19.779
0.02 -106.33 64.354 0 24.02 -8.6418 0 -19.787
0.03 -106.09 64.267 0 24.029 -8.6517 0 -19.802
0.04 -105.85 64.181 0 24.036 -8.664 0 -19.822
0.05 -105.6 64.094 0 24.043 -8.6787 0 -19.848
0.06 -105.36 64.007 0 24.028 -8.6866 0 -19.876
0.07 -105.12 63.92 0 24.013 -8.6937 0 -19.903
0.08 -104.88 63.833 0 23.998 -8.7003 0 -19.928
0.09 -104.64 63.746 0 23.984 -8.7061 0 -19.951
0.1 -104.4 63.659 0 23.969 -8.7111 0 -19.973
0.11 -104.16 63.572 0 23.935 -8.7088 0 -19.994
0.12 -103.93 63.485 0 23.901 -8.7067 0 -20.015
0.13 -103.69 63.398 0 23.867 -8.7047 0 -20.037
0.14 -103.45 63.311 0 23.833 -8.7029 0 -20.06
0.15 -103.21 63.224 0 23.8 -8.7016 0 -20.083
0.16 -102.97 63.137 0 23.845 -8.7289 0 -20.106
⋮
Добавить эго и агентов неэго к сценарию:
Считайте данные, зарегистрированные датчиком лидара.
Оцените положение, скорость, скорость и рыскание для автомобиля, оборудованного датчиком путем рассмотрения и GPS и лоцируйте метки времени. Вычислите траекторию эго.
Найдите количество агентов неэго в сценарии. Вычислите их waypoints, скорости, скорости и рыскания при помощи записанного датчика лидара. Вычислите траектории агентов неэго.
Преобразуйте waypoints агентов неэго от системы координат эго до системы координат сценария.
Добавьте эго и агентов неэго к ведущему сценарию.
Загрузите .mat файл, содержащий записанные данные о датчике лидара и, сохраняет его в заданную папку во временной директории. Загрузите данные о датчике лидара из .mat файл в рабочее пространство MATLAB.
if ~exist("newData","var") url = "https://ssd.mathworks.com/supportfiles/driving/data/highwayLaneChangeDatase/lidar2.mat"; filePath = fullfile(sensorData,'lidar.mat'); if ~isfile(filePath) websave(filePath,url); end newData = load(filePath); end
Вычислите количество точек данных в заданных метках времени начала и конца. Кроме того, найдите индексы начала и конца меток времени при помощи helperTimestampIndex функция.
[count.lidar,sim.lidar.startstep,sim.lidar.endstep] = helperTimestampIndex([newData.lidar.timeStamp].',sim.startTime,sim.endTime);
Извлеките метку времени и данные об облаке точек, которые находятся между индексами начала и конца. Сохраните извлеченные параметры в таблице.
newData.lidar = newData.lidar(sim.lidar.startstep : sim.lidar.endstep);
lidar = table([newData.lidar.timeStamp].',{newData.lidar.ptCloud}.',VariableNames=["timestamp","ptCloud"])lidar=100×2 table
timestamp ptCloud
________________ ________________
1461634425028360 {1×1 pointCloud}
1461634425128114 {1×1 pointCloud}
1461634425228122 {1×1 pointCloud}
1461634425327849 {1×1 pointCloud}
1461634425427574 {1×1 pointCloud}
1461634425528347 {1×1 pointCloud}
1461634425627513 {1×1 pointCloud}
1461634425728613 {1×1 pointCloud}
1461634425828486 {1×1 pointCloud}
1461634425928594 {1×1 pointCloud}
1461634426028230 {1×1 pointCloud}
1461634426128400 {1×1 pointCloud}
1461634426228515 {1×1 pointCloud}
1461634426327968 {1×1 pointCloud}
1461634426427685 {1×1 pointCloud}
1461634426527961 {1×1 pointCloud}
⋮
Найдите метки времени лидара относительно времени начала симуляции. Величина в секундах.
lidar.relativeTime = double(lidar.timestamp - sim.startTime)/10^6;
Найдите положение эго, скорость, скорость и рыскание при помощи данных об эго из измерений GPS как ссылка.
temp = zeros(1,9); i = 1; for j = 2 : size(simEgo.data,1) t = simEgo.data(j,9); if i <= count.lidar && lidar.relativeTime(i) <= t tratio = (t - lidar.relativeTime(i)) / (t - simEgo.data(j-1,9)); for k = [1:4,8] temp(1,k) = simEgo.data(j,k) - ... ((simEgo.data(j,k) - simEgo.data(j-1,k))*tratio); end ego.position(i,:) = temp(1,positionIndex); ego.velocity(i,:) = temp(1,velocityIndex); ego.speed(i,:) = sqrt(temp(1,2)^2 + temp(1,4)^2); ego.yaw(i,:) = temp(1,8); i = i + 1; elseif i == count.lidar && size(simEgo.data,1) == j ego.position(i,:) = simEgo.data(j,positionIndex); ego.velocity(i,:) = simEgo.data(j,velocityIndex); ego.speed(i,:) = sqrt(simEgo.data(j,2)^2 + simEgo.data(j,4)^2); ego.yaw(i,:) = simEgo.data(j,8); end end ego.position = smoothdata(ego.position,'sgolay'); ego.yaw = smoothdata(ego.yaw);
Добавьте автомобиль, оборудованный датчиком в ведущий сценарий и вычислите его траекторию.
egoVehicle = vehicle(scenario,ClassID=1,Position=ego.position(1,:),Yaw=ego.yaw(1), ...
Mesh=driving.scenario.carMesh);
trajectory(egoVehicle,ego.position,ego.speed,Yaw=ego.yaw);Создайте список дорожек из данных об облаке точек лидара при помощи helperPointCloudToTracks функция. Список дорожек содержит информацию об объектах, обнаруженных в данных об облаке точек.
if ~exist("trackList","var") trackList.object = helperPointCloudToTracks(lidar.ptCloud); trackList.timestamp = lidar.timestamp; trackList.relativeTime = lidar.relativeTime; for i = 1:count.lidar trackList.nObj(i,1) = size(trackList.object{i,1},1); end end
Преобразуйте данные о списке дорожек в данные неэго при помощи helperComputeNonEgoData функция. Функция вычисляет размерности, времена записи, выходные времена, рыскания и скорости агентов неэго в сценарии. Функция также преобразует положения агентов неэго, вычисленных в системе координат эго к системе координат сценария или карты, и затем сглаживает значения положения.
[count.nonEgo,nonEgo] = helperComputeNonEgoData(trackList,count,ego)
count = struct with fields:
gps: 200
lidar: 100
nonEgo: 5
nonEgo=5×17 table
trackCount id length width height age entryIndex entryTime exitIndex exitTime classID mesh posInEgoFrame yaw posInMapFrame speed smoothPos
__________ ___ ______ ______ ______ ___ __________ _________ _________ ________ _______ ______________________ _____________ _____________ _____________ _____________ _____________
94 37 4.6931 1.7953 1.4497 100 7 0.7 100 10 1 1×1 extendedObjectMesh {94×3 double} {94×1 double} {94×3 double} {94×1 double} {94×3 double}
94 38 4.6931 1.7932 1.4506 100 7 0.7 100 10 1 1×1 extendedObjectMesh {94×3 double} {94×1 double} {94×3 double} {94×1 double} {94×3 double}
94 49 4.6942 1.7974 1.4007 100 7 0.7 100 10 1 1×1 extendedObjectMesh {94×3 double} {94×1 double} {94×3 double} {94×1 double} {94×3 double}
28 99 4.6924 1.7942 1.399 34 8 0.8 35 3.5 1 1×1 extendedObjectMesh {28×3 double} {28×1 double} {28×3 double} {28×1 double} {28×3 double}
44 941 4.6938 1.7981 1.4171 50 54 5.4 97 9.7 1 1×1 extendedObjectMesh {44×3 double} {44×1 double} {44×3 double} {44×1 double} {44×3 double}
Добавьте агентов неэго в ведущий сценарий и вычислите их траектории.
for i= 1:count.nonEgo if nonEgo.classID(i) == 1 nonEgoVehicle(i) = vehicle(scenario, ... ClassID=nonEgo.classID(i),Name=nonEgo.id(i,:), ... Position=nonEgo.smoothPos{i,1}(1,:), ... Length=nonEgo.length(i,1),Width=nonEgo.width(i,1), ... Height=nonEgo.height(i,1),Mesh=nonEgo.mesh(i,1), ... EntryTime=nonEgo.entryTime(i,1), ... ExitTime=nonEgo.exitTime(i,1)); else nonEgoVehicle(i) = actor(scenario, ... ClassID=nonEgo.classID(i),Name=nonEgo.id(i,:), ... Position=nonEgo.smoothPos{i,1}(1,:), ... Length=nonEgo.length(i,1),Width=nonEgo.width(i,1), ... Height=nonEgo.height(i,1),Mesh=nonEgo.mesh(i,1), ... EntryTime=nonEgo.entryTime(i,1), ... ExitTime=nonEgo.exitTime(i,1)); end trajectory(nonEgoVehicle(i),nonEgo.smoothPos{i,1},nonEgo.speed{i,1}); end
Отобразите на графике данные о карте и сценарий, сгенерированный с помощью GPS, и лоцируйте данные о датчике.
scfig = figure(Position=[0,0,800,500]); hfig = uipanel(scfig,Title="Imported Map Data",Position=[0 0 0.5 1]); plaxis = axes(hfig); plot(importedScenario,Parent=plaxis) hfig1 = uipanel(scfig,Title="Extracted Road Network and Generated Scenario",Position=[0.5 0 0.5 1]); plaxis1 = axes(hfig1); plot(scenario,Parent=plaxis1); xlim([-100,100]) while advance(scenario) pause(sim.sampleTime); end
Можно также визуально смотреть точность сгенерированного сценария путем графического вывода его вместе с датчиком камеры и лоцировать данные о датчике.
Считайте данные, зарегистрированные датчиком камеры.
if ~exist("camera","var") url = "https://ssd.mathworks.com/supportfiles/driving/data/highwayLaneChangeDatase/camera2.mat"; filePath = fullfile(sensorData,"camera.mat"); if ~isfile(filePath) websave(filePath,url); end img = load(filePath,"imageData"); [count.camera,sim.camera.startstep,sim.camera.endstep] = ... helperTimestampIndex([img.imageData.timeStamp].', ... sim.startTime,sim.endTime); img.imageData = img.imageData(sim.camera.startstep:sim.camera.endstep); camera = table([img.imageData.timeStamp].',{img.imageData.mov}.', ... VariableNames=["timestamp","img"]); for i = 1 : count.camera camera.img{i,1} = camera.img{i,1}.cdata; end camera.relativeTime = double(camera.timestamp - sim.startTime)/10^6; end
Отобразите графики вида сверху и представления преследования сгенерированного сценария.
restart(scenario) close all fig = figure; set(fig,Position=[40 40 1000 800]); % Ego Top View hPanel = uipanel(fig, ... Title="Top-View of Generated Scenario",Position=[0 0.5 0.5 0.5]); hPlot = axes(hPanel); chasePlot(egoVehicle,Parent=hPlot, ... ViewPitch=90,ViewHeight=120,ViewLocation=[0, 0]); % Ego ChasePlot hPanel2 = uipanel(fig, ... Title="Chase-View of Generated Scenario",Position=[0.5 0.5 0.5 0.5]); hPlot2 = axes(hPanel2); chasePlot(egoVehicle,Parent=hPlot2,Meshes='on');
Задайте оси для отображения данных о камере.
% Image from camera hPanel3 = uipanel(fig, ... Title="Recorded Camera Data",Position=[0 0 0.5 0.5]); hPlot3 = axes(hPanel3); % Initialize last used indices of the sensor last.lidarIndex = 0; last.cameraIndex = 0;
Отобразите данные об облаке точек лидара на графике.
hPanel4 = uipanel(fig, ... Title ="Recorded Lidar Data",Position=[0.5 0 0.5 0.5]); hPlot4 = axes(hPanel4); % Initialize Display xlimits = [-60 60]; ylimits = [-30 30]; zlimits = [-5 20]; player = pcplayer(xlimits,ylimits,zlimits,Parent=hPlot4);
Вызовите advance функция в цикле, чтобы усовершенствовать симуляцию один временной шаг за один раз.
while advance(scenario) if last.cameraIndex + 1 <= count.camera && ... scenario.SimulationTime >= camera.relativeTime(last.cameraIndex+1) condition = true; while condition last.cameraIndex = last.cameraIndex + 1; condition = scenario.SimulationTime < ... camera.relativeTime(last.cameraIndex); end image(camera.img{last.cameraIndex,1},Parent=hPlot3); end % if last.lidarIndex + 1 <= count.lidar && ... scenario.SimulationTime >= lidar.relativeTime(last.lidarIndex + 1) condition = true; while condition last.lidarIndex = last.lidarIndex + 1; condition = scenario.SimulationTime < ... lidar.relativeTime(last.lidarIndex); end % Plot point cloud with bounding box and label ptCloud = lidar.ptCloud{last.lidarIndex,1}; confirmedTracks = trackList.object{last.lidarIndex,1}; view(player,ptCloud); % The helperParseTracks function returns position, dimension, and orientation of the 3-D bounding boxes. posIndex = [1 3 6]; velocityIndex = [2 4 7]; yawIndex = 8; dimIndex = [9 10 11]; if ~isempty(confirmedTracks) [pos,dims,orients,labels] = helperParseTracks(confirmedTracks,posIndex,dimIndex,yawIndex); yaw = zeros(size(pos,1),3); yaw(:,3) = orients'; bboxes = [pos,dims,yaw]; showShape(cuboid=bboxes,Label=labels',Parent=hPlot4); end end pause(sim.sampleTime) end
[1] Парк, Seo-Wook, Кунал Патил, Уилл Вильсон, Марк Корлесс, Габриэль Чой и Пол Адам. “Создавая Управление Сценариями из Записанных Данных о Транспортном средстве для Проверки Системы Центрирования Маршрута в Уличном движении”, 2020-01–0718, 2020. https://doi.org/10.4271/2020-01-0718.
road | roadNetwork | vehicle | actor | trajectory | drivingScenario | advance | restart | plot