smoothTrajectory
Создайте плавную, ограниченную рывком траекторию актёра в сценарии вождения
Синтаксис
Описание
smoothTrajectory функция создает плавную, ограниченную рывком траекторию для актёра в сценарии вождения. Сгенерированная траектория характеризуется плавным переходом ускорений между путевыми точками, что делает ее совместимой для генерации измерений синтетической инерционной навигационной системы (INS) и глобальной навигационной спутниковой системы (GNSS) из insSensor Системные object™. Для получения дополнительной информации о том, как smoothTrajectory генерирует траектории, см. Алгоритмы.
smoothTrajectory(___, задает опции, использующие одну или несколько пары "имя-значение" и любую из комбинаций входных аргументов из предыдущих синтаксисов. Например, можно задать угол ориентации рыскания актёра или транспортного средства в каждой путевой точке или максимальное количество рывка в траектории.Name,Value)
Примеры
Создайте траекторию с различными скоростями
Создайте сценарий вождения, содержащий изогнутую двухполосную дорогу.
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]);
Постройте график сценария вождения и отобразите путевые точки траектории.
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');
Определите траекторию пешехода. Пешехода приближаются к перекрестку, ненадолго паузы, а затем принимает резкий правый поворот на перекрестке. Чтобы задать резкий правый поворот, задайте две точки пути на пересечении, которые близки друг к другу. Для этих точек пути задайте угол ориентации рыскания второй точки пути под углом 90 градусов от первой точки пути.
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);
Постройте график сценария вождения и отобразите путевые точки пешехода.
plot(scenario,'Waypoints','on') while advance(scenario) pause(0.001) end
Сгенерируйте измерения INS из сценария вождения
Сгенерируйте измерения с датчика INS, который установлен на транспортном средстве в сценарии вождения. Постройте график измерений INS в зависимости от состояния основной истины транспортного средства и визуализируйте скорость и профиль ускорения транспортного средства.
Создайте сценарий вождения
Загрузите географические данные для маршрута в кампусе MathWorks Apple Hill в Натике, штат Массачусетс.
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 этого состояния. Преобразуйте эти показания в географические координаты и в каждой путевой точке визуализируйте показания основной истины и 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функция не может вычислить плавную, ограниченную рывком траекторию, учитывая входные параметры, попробуйте внести эти корректировки в сценарий:Увеличьте расстояния между путевыми точками, чтобы дать транспортному средству больше времени для ускорения до заданных скоростей.
Уменьшите скорости в каждой путевой точке. Попробуйте преобразовать значения скорости из метров в секунду в мили в час, чтобы увидеть, реалистичны ли скорости, учитывая сценарий. Например, маловероятно, что алгоритм может вычислить плавную траекторию для резкого поворота, который берётся со скоростью 30 м/с (около 67 миль/ч).
Увеличьте максимальный рывок. Увеличение максимума рывка позволяет алгоритму вычислять больше возможных траекторий за счет снижения комфорта пассажиров.
Алгоритмы
smoothTrajectory функция создает ограниченную рывком траекторию, используя профиль ускорения трапеций. Эта траектория имеет плавные переходы ускорения между путевыми точками, что приводит к комфортной поездке для пассажиров. Функция вычисляет отдельный профиль ускорения трапеций для каждого из сегментов N - 1 между путевыми точками траектории.
Рассмотрим простой сценарий, при котором машина проезжает расстояние 50 метров по 100-метровой дороге. Траектория состоит из одного 50-метрового сегмента, в котором автомобиль должен увеличить скорость с 5 м/с до 10 м/с к концу сегмента. Траектория имеет дополнительное ограничение, при котором максимальный продольный рывок не должен превышать 0,5 м/с3.
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)
Учитывая ограничения расстояния, скорости и рывка этого сегмента путевой точки, smoothTrajectory функция генерирует трехфазный профиль трапеций ускорения:
Увеличьте ускорение линейно. Удерживайте рывок константой при значении не больше
jerk.Удерживайте ускорение постоянным. Уменьшите рывок до 0.
Уменьшите ускорение линейно. Удерживайте рывок константой при значении не менее
-jerk.
Эти графики визуализируют профиль расстояния, скорости, ускорения и рывка вдоль этого сегмента путевой точки с течением времени. Три фазы профиля ускорения образуют трапецию.
Когда скорость уменьшается между путевыми точками, smoothTrajectory функция генерирует трехфазный профиль трапеций ускорения в обратном порядке. В случае с уменьшенной скоростью форма профиля ускорения является обратной из той, которая показана на предыдущем графике.
Ссылки
[1] Bae, Il, Jeyoung Moon и Jeongseok Seo. «К комфортному вождению для беспилотного маршрутного автобуса». Электроника 8, № 9 (27 августа 2019): 943. https://doi.org/10.3390/electronics8090943.