Создание плавной, ограниченной толчком траектории движения в сценарии вождения
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 в зависимости от истинного состояния транспортного средства на земле и визуализируйте профиль скорости и ускорения транспортного средства.
Создание сценария управления
Загрузите географические данные для маршрута вождения в кампусе 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] Бэ, Иль, Джейюнг Мун и Чонсек Со. «К комфортному вождению автобуса». Электроника 8, № 9 (27 августа 2019 г.): 943. https://doi.org/10.3390/electronics8090943.