smoothTrajectory
Создайте сглаженную, ограниченную толчками траекторию агента в ведущем сценарии
Синтаксис
Описание
smoothTrajectory функция создает сглаженную, ограниченную толчками траекторию для агента в ведущем сценарии. Генерированные признаки траектории плавный переход ускорений между waypoints, делая его совместимым для генерации синтетической инерционной системы навигации (INS) и измерениями глобальной навигационной спутниковой системы (GNSS) от insSensor Система object™. Для получения дополнительной информации о как smoothTrajectory генерирует траектории, см. Алгоритмы.
smoothTrajectory(___, задает опции с помощью одной или нескольких пар "имя-значение" и любой из комбинаций входных аргументов от предыдущих синтаксисов. Например, можно задать угол ориентации рыскания агента или транспортного средства в каждом waypoint или максимальном объеме толчка в траектории.Name,Value)
Примеры
Создайте траекторию с различными скоростями
Создайте ведущий сценарий, содержащий кривую дорогу 2D маршрута.
scenario = drivingScenario('SampleTime',0.05); roadcenters = [0 0; 24.2 27.7; 50 30]; lspec = lanespec(2); road(scenario,roadcenters,'Lanes',lspec);
Добавьте транспортное средство в сценарий. Установите траекторию, в которой замедляется транспортное средство, когда оно вводит кривую.
v = vehicle(scenario,'ClassID',1);
waypoints = [2.6 1.0; 23.6 24.9; 45.5 28.6];
speed = [9 8 9];
smoothTrajectory(v,waypoints,speed)
Постройте сценарий и запустите симуляцию.
plot(scenario,'Waypoints','on','RoadCenters','on') while advance(scenario) pause(scenario.SampleTime) end
Создайте траекторию со временем ожидания на пересечении
Создайте ведущий сценарий, содержащий пересечение с четырьмя путями.
scenario = drivingScenario('SampleTime',0.02,'StopTime',20); roadCenters = [0 0; 50 0]; laneSpecification = lanespec([1 1]); road(scenario,roadCenters,'Lanes',laneSpecification); roadCenters = [25 25; 25 -25]; road(scenario,roadCenters,'Lanes',laneSpecification);
Добавьте автомобиль, оборудованный датчиком, который перемещается на север, но ожидает в течение одной секунды на пересечении.
ego = vehicle(scenario,'ClassID',1,'Position',[2 -2 0]); waypoints = [2 -2; 17.5 -2; 45 -2]; speed = [5 0 5]; waittime = [0 1 0]; smoothTrajectory(ego,waypoints,speed,waittime);
Добавьте велосипедиста, который перемещается на восток через пересечение на постоянной скорости без остановки.
bicycle = actor(scenario, ... 'ClassID',3, ... 'Length',1.7, ... 'Width',0.45, ... 'Height',1.7, ... 'Position',[23 23 0]); waypoints = [23 23; 23 -23]; speed = 4; smoothTrajectory(bicycle,waypoints,speed);
Постройте сценарий. Остановки транспортного средства на пересечении в течение одной секунды, затем продолжает управлять после того, как велосипедист пересекает пересечение.
plot(scenario) while advance(scenario) pause(scenario.SampleTime) end
Создайте противоположную траекторию движения
Симулируйте ведущий сценарий, в котором автомобиль управляет наоборот, чтобы отступить в место для парковки.
Создайте ведущий сценарий. Добавьте дорожные сегменты, чтобы задать парковку. Первый дорожный сегмент задает парковочные места. Второй дорожный сегмент задает ведущий маршрут и накладывает первый дорожный сегмент.
scenario = drivingScenario; roadCentersParking = [6 0; 24 0]; lmParking = [laneMarking('Unmarked') ... repmat(laneMarking('Solid'),1,5) ... laneMarking('Unmarked')]; lspecParking = lanespec(6,'Width',3,'Marking',lmParking); road(scenario,roadCentersParking,'Lanes',lspecParking); roadCentersDriving = [12 0; 18 0]; lmDriving = [laneMarking('Unmarked') laneMarking('Unmarked')]; lspecDriving = lanespec(1,'Width',18,'Marking',lmDriving); road(scenario,roadCentersDriving,'Lanes',lspecDriving);
Добавьте транспортное средство в ведущий сценарий.
car = vehicle(scenario,'ClassID',1,'Position',[15 -6 0],'Yaw',90);
Задайте траекторию транспортного средства. Диски транспортного средства вперед, остановки, и затем управляют наоборот, пока это не отступает в место для парковки. Когда транспортное средство вводит место для парковки, оно имеет угол ориентации рыскания, который является 90 градусами против часовой стрелки от того, где оно запустилось.
waypoints = [15 -6; 15 5; 12 -1.5; 7.3 -1.5];
speed = [3; 0; -2; 0];
smoothTrajectory(car,waypoints,speed,'Yaw',[90 90 180 180]);
Постройте ведущий сценарий и отобразите waypoints траектории.
plot(scenario,'Waypoints','on') while advance(scenario) pause(0.001) end
Создайте пешеходную траекторию
Создайте траекторию пешехода, который поворачивает направо на пересечении.
Создайте ведущий сценарий. Добавьте дорожные сегменты, которые задают пересечение.
scenario = drivingScenario; roadCenters = [0 10; 0 -10]; road(scenario,roadCenters); road(scenario,flip(roadCenters,2));
Добавьте пешеходного агента в сценарий.
pedestrian = actor(scenario, ... 'ClassID',4, ... 'Length',0.24, ... 'Width',0.45, ... 'Height',1.7, ... 'Position',[-9 0 0], ... 'RCSPattern',[-8 -8; -8 -8], ... 'Mesh',driving.scenario.pedestrianMesh, ... 'Name','Pedestrian');
Задайте траекторию пешехода. Пешеход приближается к пересечению, делает паузу кратко, и затем поворачивает направо на пересечении. Чтобы задать резкий правый поворот, задайте два waypoints на пересечении, которые являются близко друг к другу. Для этих waypoints задайте угол ориентации рыскания второго waypoint в углу в 90 градусов сначала waypoint.
waypoints = [-9 0; -0.25 0; 0 -0.25; 0 -9];
speed = [1.5; 0; 0.5; 1.5];
yaw = [0; 0; -90; -90];
waittime = [0; 0.2; 0; 0];
smoothTrajectory(pedestrian,waypoints,speed,waittime,'Yaw',yaw);
Постройте ведущий сценарий и отобразите waypoints пешехода.
plot(scenario,'Waypoints','on') while advance(scenario) pause(0.001) end
Сгенерируйте измерения INS от управления сценарием
Сгенерируйте измерения от датчика INS, который смонтирован к транспортному средству в ведущем сценарии. Постройте измерения INS против состояния основной истины транспортного средства и визуализируйте скорость и ускоряющий профиль транспортного средства.
Создайте ведущий сценарий
Загрузите географические данные для ведущего маршрута в кампусе MathWorks Apple Hill в Натике, MA.
data = load('ahroute.mat');
latIn = data.latitude;
lonIn = data.longitude;
Преобразуйте координаты широты и долготы маршрута к Декартовым координатам. Установите источник на первую координату в ведущем маршруте. Для простоты примите высоту 0 для маршрута.
alt = 0; origin = [latIn(1),lonIn(1),alt]; [xEast,yNorth,zUp] = latlon2local(latIn,lonIn,alt,origin);
Создайте ведущий сценарий. Установите источник конвертированного маршрута как географическая контрольная точка.
scenario = drivingScenario('GeoReference',origin);
Создайте дорогу на основе Декартовых координат маршрута.
roadCenters = [xEast,yNorth,zUp]; road(scenario,roadCenters);
Создайте транспортное средство, которое следует за центральной линией дороги. Транспортное средство перемещается между 4 и 5 метрами в секунду (9 - 11 миль в час), замедляющихся в кривых на дороге. Чтобы создать траекторию, используйте smoothTrajectory функция. Вычисленная траектория минимизирует толчок и избегает разрывов на ускорении, которое является требованием для моделирования датчиков INS.
egoVehicle = vehicle(scenario,'ClassID',1);
egoPath = roadCenters;
egoSpeed = [5 5 5 4 4 4 5 4 4 4 4 5 5 5 5 5];
smoothTrajectory(egoVehicle,egoPath,egoSpeed);
Постройте сценарий и покажите 3-D представление из-за автомобиля, оборудованного датчиком.
plot(scenario) chasePlot(egoVehicle)
Создайте датчик INS
Создайте датчик INS, который принимает вход времен симуляции. Введите шум в измерения датчика путем установки стандартного отклонения измерений скорости и точности к 0,1 и 0.05, соответственно.
INS = insSensor('TimeInput',true, ... 'VelocityAccuracy',0.1, ... 'AccelerationAccuracy',0.05);
Визуализируйте измерения INS
Инициализируйте географический проигрыватель для отображения измерений INS и основной истины агента. Сконфигурируйте проигрыватель, чтобы отобразить его последние 10 положений и установить уровень изменения масштаба на 17.
zoomLevel = 17; player = geoplayer(latIn(1),lonIn(1),zoomLevel, ... 'HistoryDepth',10,'HistoryStyle','line');
Предварительно выделите место для времен симуляции, скоростных измерений и ускоряющих измерений, которые получены в процессе моделирования.
numWaypoints = length(latIn); times = zeros(numWaypoints,1); gTruthVelocities = zeros(numWaypoints,1); gTruthAccelerations = zeros(numWaypoints,1); sensorVelocities = zeros(numWaypoints,1); sensorAccelerations = zeros(numWaypoints,1);
Симулируйте сценарий. Во время цикла симуляции получите состояние основной истины автомобиля, оборудованного датчиком и измерение INS того состояния. Преобразуйте эти показания в географические координаты, и в каждом waypoint, визуализируйте основную истину и показания INS на географическом проигрывателе. Также получите скорость и ускоряющие данные для графического вывода ускоряющие профили и скорость.
nextWaypoint = 2; while advance(scenario) % Obtain ground truth state of ego vehicle. gTruth = state(egoVehicle); % Obtain INS sensor measurement. measurement = INS(gTruth,scenario.SimulationTime); % Convert readings to geographic coordinates. [latOut,lonOut] = local2latlon(measurement.Position(1), ... measurement.Position(2), ... measurement.Position(3),origin); % Plot differences between ground truth locations and locations reported by sensor. reachedWaypoint = sum(abs(roadCenters(nextWaypoint,:) - gTruth.Position)) < 1; if reachedWaypoint plotPosition(player,latIn(nextWaypoint),lonIn(nextWaypoint),'TrackID',1) plotPosition(player,latOut,lonOut,'TrackID',2,'Label','INS') % Capture simulation times, velocities, and accelerations. times(nextWaypoint,1) = scenario.SimulationTime; gTruthVelocities(nextWaypoint,1) = gTruth.Velocity(2); gTruthAccelerations(nextWaypoint,1) = gTruth.Acceleration(2); sensorVelocities(nextWaypoint,1) = measurement.Velocity(2); sensorAccelerations(nextWaypoint,1) = measurement.Acceleration(2); nextWaypoint = nextWaypoint + 1; end if nextWaypoint > numWaypoints break end end
Постройте скоростной профиль
Сравните основную истину продольная скорость транспортного средства в зависимости от времени против скоростных измерений, полученных датчиком INS.
Удалите нули из временного вектора и векторов скорости.
times(times == 0) = []; gTruthVelocities(gTruthVelocities == 0) = []; sensorVelocities(sensorVelocities == 0) = []; figure hold on plot(times,gTruthVelocities) plot(times,sensorVelocities) title('Longitudinal Velocity Profile') xlabel('Time (s)') ylabel('Velocity (m/s)') legend('Ground truth','INS') hold off
Постройте ускоряющий профиль
Сравните основную истину продольное ускорение транспортного средства в зависимости от времени против ускоряющих измерений, полученных датчиком INS.
gTruthAccelerations(gTruthAccelerations == 0) = []; sensorAccelerations(sensorAccelerations == 0) = []; figure hold on plot(times,gTruthAccelerations) plot(times,sensorAccelerations) title('Longitudinal Acceleration Profile') xlabel('Time (s)') ylabel('Acceleration (m/s^2)') legend('Ground truth','INS') hold off
Входные параметры
Советы
Если
smoothTrajectoryфункция не может вычислить сглаженную, ограниченную толчками траекторию, учитывая входные параметры, попытаться внести эти корректировки в сценарий:Расширьте расстояния между waypoints, чтобы дать транспортному средству больше времени, чтобы ускориться к заданным скоростям.
Понизьте скорости в каждом waypoint. Попытайтесь преобразовать значения скорости от метров в секунду к милям в час, чтобы видеть, реалистичны ли скорости, учитывая сценарий. Например, маловероятно, что алгоритм может вычислить сглаженную траекторию для крутого оборота, который принят со скоростью 30 м/с (приблизительно 67 миль в час).
Увеличьте максимальный толчок. Увеличение максимума толчка позволяет алгоритму вычислить более возможные траектории за счет уменьшаемого человеческого пассажирского комфорта.
Алгоритмы
smoothTrajectory функция создает ограниченную толчками траекторию с помощью трапециевидного ускоряющего профиля. Эта траектория имеет сглаженные ускоряющие переходы между waypoints, приводящим к удобной поездке для пассажиров - людей. Функция вычисляет отдельный трапециевидный ускоряющий профиль для каждого из сегментов N - 1 между траекторией waypoints.
Рассмотрите простой сценарий, в котором автомобиль путешествует на расстояние 50 метров вдоль 100-метровой дороги. Траектория состоит из одного 50-метрового сегмента, в котором автомобиль должен увеличить свою скорость с 5 м/с до 10 м/с к концу сегмента. Траектория имеет дополнительное ограничение, в котором максимальный продольный толчок не должен превышать 0.5 m/s3.
scenario = drivingScenario; car = vehicle(scenario); road(scenario,[0 -25; 0 75]); % m waypoints = [0 0; 0 50]; % m speed = [5 10]; % m/s jerk = 0.5; % m/s^3 smoothTrajectory(car,waypoints,speed,'Jerk',jerk)
Учитывая расстояние, скорость и ограничения толчка этого waypoint сегмента, smoothTrajectory функция генерирует трехфазный трапециевидный ускоряющий профиль:
Увеличьте ускорение линейно. Содержите толчок, постоянный в значении, не больше, чем
jerk.Содержите постоянное ускорение. Уменьшите толчок к 0.
Уменьшите ускорение линейно. Содержите толчок, постоянный в значении не меньше, чем
-jerk.
Эти графики визуализируют расстояние, скорость, ускорение и профиль толчка вдоль этого waypoint сегмента в зависимости от времени. Три фазы ускоряющего профиля формируют трапециевидную форму.
Когда скорость уменьшается между waypoints, smoothTrajectory функция генерирует трехфазный трапециевидный ускоряющий профиль в обратном порядке. В уменьшенном случае скорости форма ускоряющего профиля является инверсией один показанный в предыдущем графике.
Ссылки
[1] Бэй, Il, Луна Jaeyoung и SEO Jeongseok. "К Удобному Водительскому стажу для Самоходного Автобуса". Электроника 8, № 9 (27 августа 2019): 943. https://doi.org/10.3390/electronics8090943.