smoothTrajectory

Создайте сглаженную, ограниченную толчками траекторию агента в ведущем сценарии

Описание

smoothTrajectory функция создает сглаженную, ограниченную толчками траекторию для агента в ведущем сценарии. Генерированные признаки траектории плавный переход ускорений между waypoints, делая его совместимым для генерации синтетической инерционной системы навигации (INS) и измерениями глобальной навигационной спутниковой системы (GNSS) от insSensor Система object™. Для получения дополнительной информации о как smoothTrajectory генерирует траектории, см. Алгоритмы.

smoothTrajectory(ac,waypoints) создает сглаженную траекторию для агента или транспортного средства, ac, следовать из набора waypoints. Агент перемещается на постоянной скорости 30 метров в секунду.

пример

smoothTrajectory(ac,waypoints,speed) также задает скорость, на которой агент или транспортное средство перемещаются вдоль траектории, или во вперед или в противоположное движение.

пример

smoothTrajectory(ac,waypoints,speed,waittime) также задает время ожидания для агента или транспортного средства. Используйте этот синтаксис, чтобы приостановить агента или транспортное средство в определенном waypoints.

пример

smoothTrajectory(___,Name,Value) задает опции с помощью одной или нескольких пар "имя-значение" и любой из комбинаций входных аргументов от предыдущих синтаксисов. Например, можно задать угол ориентации рыскания агента или транспортного средства в каждом waypoint или максимальном объеме толчка в траектории.

Примеры

свернуть все

Создайте ведущий сценарий, содержащий кривую дорогу 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 против состояния основной истины транспортного средства и визуализируйте скорость и ускоряющий профиль транспортного средства.

Создайте ведущий сценарий

Загрузите географические данные для ведущего маршрута в кампусе 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

Входные параметры

свернуть все

Агент, принадлежащий drivingScenario объект в виде Actor или Vehicle объект. Чтобы создать эти объекты, используйте actor и vehicle функции, соответственно.

Траектория waypoints, в метрах в виде N с действительным знаком-by-2 или N-by-3 матрица. N является количеством waypoints.

  • Если waypoints N-by-2 матрица, затем каждая строка матрицы представляет (x, y) координаты waypoint. z - координата каждого waypoint является нулем.

  • Если waypoints N-by-3 матрица, затем каждая строка матрицы представляет (x, y, z) координаты waypoint.

Каждый из сегментов N - 1 между waypoints задает кривую, искривление которой варьируется линейно с длиной. Если первые и последние waypoint идентичны, то траектория формирует цикл.

Waypoints находятся в системе мировой координаты.

Пример: [1 0 0; 2 7 7; 3 8 8]

Типы данных: single | double

Скорость агента в каждом waypoint, в метрах в секунду в виде скаляра с действительным знаком или N - элемент вектор с действительным знаком. N является количеством waypoints, заданного waypoints.

  • Когда speed скаляр, скорость является постоянной в движении агента.

  • Когда speed вектор, векторные значения задают скорость в каждом waypoint. Для движения вперед задайте положительные значения скорости. Для противоположного движения задайте отрицательные значения скорости. Чтобы изменить направления движения, разделите положительные и отрицательные скорости waypoint с 0 скорость.

Скорости интерполированы между waypoints. Можно задать speed значения как 0 в любом waypoint, но вы не можете задать 0 скорость в двух последовательных waypoints.

Если вы не задаете speed, затем по умолчанию агент перемещается на постоянной скорости 30 м/с.

Пример: [10 8 9] задает скорости 10 м/с, 8 м/с и 9 м/с.

Пример: [10 0 -10] задает скорость 10 м/с в движении вперед, паузе для того, чтобы изменить направления и скорость 10 м/с наоборот.

Типы данных: single | double

Время ожидания агента в каждом waypoint, в секундах в виде N - элемент вектор с действительным знаком. N является количеством waypoints, заданного waypoints.

Когда вы задаете неотрицательное время ожидания для агента в waypoint, агент приостанавливает в этом waypoint для конкретного количества секунд. Когда вы задаете неотрицательное время ожидания, необходимо установить соответствующий speed значение к 0. Можно установить waitime к 0 в любом waypoint, но вы не можете установить waittime в двух последовательных waypoints к ненулевым значениям.

Пример: [0 0 5 0] приостанавливает агента в течение пяти секунд, когда он достигает третьего waypoint.

Типы данных: single | double

Аргументы в виде пар имя-значение

Задайте дополнительные разделенные запятой пары Name,Value аргументы. Name имя аргумента и Value соответствующее значение. Name должен появиться в кавычках. Вы можете задать несколько аргументов в виде пар имен и значений в любом порядке, например: Name1, Value1, ..., NameN, ValueN.

Пример: 'Yaw',[0 90 90 0],'Jerk',0.9

Угол ориентации рыскания агента в каждом waypoint, в градусах в виде разделенной запятой пары, состоящей из 'Yaw' и N - элемент вектор с действительным знаком. N является количеством waypoints, заданного waypoints. Углы положительны в направлении против часовой стрелки.

Если вы не задаете yaw, затем рысканием в каждом waypoint является NaN, подразумевать, что рыскание не имеет никаких ограничений.

Пример: [0 90] задает агента под углом с 0 степенями в первом waypoint и углом в 90 градусов во втором waypoint.

Пример: [0 NaN] задает агента под углом с 0 степенями в первом waypoint. Агент не имеет никаких ограничений на свое рыскание во втором waypoint.

Типы данных: single | double

Максимальный продольный толчок агента, в метрах, в секунду возведенных в куб в виде разделенной запятой пары, состоящей из 'Jerk' и скаляр с действительным знаком, больше, чем или равный 0,1.

Чтобы ограничить толчок областью значений, которая создает удобные траектории для пассажиров - людей, устанавливает 'Jerk' в диапазоне от 0.3 к 0.9 [1].

Типы данных: single | double

Советы

  • Если 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 функция генерирует трехфазный трапециевидный ускоряющий профиль:

  1. Увеличьте ускорение линейно. Содержите толчок, постоянный в значении, не больше, чем jerk.

  2. Содержите постоянное ускорение. Уменьшите толчок к 0.

  3. Уменьшите ускорение линейно. Содержите толчок, постоянный в значении не меньше, чем -jerk.

Эти графики визуализируют расстояние, скорость, ускорение и профиль толчка вдоль этого waypoint сегмента в зависимости от времени. Три фазы ускоряющего профиля формируют трапециевидную форму.

Four plots stacked vertically. Plot 1 shows distance over time. Plot 2 shows speed over time. Plot 3 shows acceleration over time and forms a trapezoid shape. Plot 4 shows jerk over time.

Когда скорость уменьшается между waypoints, smoothTrajectory функция генерирует трехфазный трапециевидный ускоряющий профиль в обратном порядке. В уменьшенном случае скорости форма ускоряющего профиля является инверсией один показанный в предыдущем графике.

Ссылки

[1] Бэй, Il, Луна Jaeyoung и SEO Jeongseok. "К Удобному Водительскому стажу для Самоходного Автобуса". Электроника 8, № 9 (27 августа 2019): 943. https://doi.org/10.3390/electronics8090943.

Введенный в R2021a
Для просмотра документации необходимо авторизоваться на сайте