Генератор траектории ППМ
waypointTrajectory Система object™ генерирует траектории с использованием указанных ППМ. При создании объекта «Система» можно дополнительно указать время прибытия, скорость и ориентацию в каждой точке ППМ. Дополнительные сведения см. в разделе Алгоритмы.
Чтобы сформировать траекторию из ППМ, выполните следующие действия.
Создать waypointTrajectory и задайте его свойства.
Вызовите объект, как если бы это была функция.
Дополнительные сведения о работе системных объектов см. в разделе Что такое системные объекты?.
trajectory = waypointTrajectorytrajectory, которая генерирует траекторию на основе стационарных ППМ по умолчанию.
trajectory = waypointTrajectory(Waypoints,TimeOfArrival)Waypoints что сгенерированная траектория проходит и TimeOfArrival на каждом ППМ.
trajectory = waypointTrajectory(Waypoints,TimeOfArrival,Name,Value)Name к указанному Value. Неуказанные свойства и аргументы создания имеют значения по умолчанию или выводимые значения.
trajectory = waypointTrajectory([10,10,0;20,20,0;20,20,10],[0,0.5,10]) создает объект системы траектории ППМ, trajectory, которая начинается с ППМ [10,10,0], а затем проходит через [20,20,0] через 0,5 секунды и [20,20,10] через 10 секунд.Аргументы создания - это свойства, которые задаются во время создания объекта System и не могут быть изменены позже. Если явно значение аргумента создания не задано, значение свойства выводится.
Если указан какой-либо аргумент создания, то необходимо указать аргументы создания Waypoints и TimeOfArrival. Можно указать Waypoints и TimeOfArrival в качестве аргументов «только значение» или пар «имя-значение».
[ выводит кадр данных траектории на основе заданных аргументов и свойств создания.position,orientation,velocity,acceleration,angularVelocity] = trajectory()
Чтобы использовать функцию объекта, укажите объект System в качестве первого входного аргумента. Например, для освобождения системных ресурсов объекта System с именем obj, используйте следующий синтаксис:
release(obj)
waypointTrajectorytrajectory = waypointTrajectory
trajectory =
waypointTrajectory with properties:
SampleRate: 100
SamplesPerFrame: 1
Waypoints: [2x3 double]
TimeOfArrival: [2x1 double]
Velocities: [2x3 double]
Course: [2x1 double]
GroundSpeed: [2x1 double]
ClimbRate: [2x1 double]
Orientation: [2x1 quaternion]
AutoPitch: 0
AutoBank: 0
ReferenceFrame: 'NED'
Проверка ППМ по умолчанию и времени прибытия по телефону waypointInfo. По умолчанию ППМ указывают стационарное положение на одну секунду.
waypointInfo(trajectory)
ans=2×2 table
TimeOfArrival Waypoints
_____________ ___________
0 0 0 0
1 0 0 0
Создайте квадратную траекторию и проверьте взаимосвязь между ограничениями ППМ, частотой выборки и созданной траекторией.
Создайте квадратную траекторию, определив вершины квадрата. Определите ориентацию в каждой точке ППМ как указывающую в направлении движения. Укажите частоту дискретизации 1 Гц и используйте значение по умолчанию SamplesPerFrame из 1.
waypoints = [0,0,0; ... % Initial position 0,1,0; ... 1,1,0; ... 1,0,0; ... 0,0,0]; % Final position toa = 0:4; % time of arrival orientation = quaternion([0,0,0; ... 45,0,0; ... 135,0,0; ... 225,0,0; ... 0,0,0], ... 'eulerd','ZYX','frame'); trajectory = waypointTrajectory(waypoints, ... 'TimeOfArrival',toa, ... 'Orientation',orientation, ... 'SampleRate',1);
Создайте фигуру и постройте график начального положения платформы.
figure(1) plot(waypoints(1,1),waypoints(1,2),'b*') title('Position') axis([-1,2,-1,2]) axis square xlabel('X') ylabel('Y') grid on hold on
В контуре выполните шаг по траектории для вывода текущего положения и текущей ориентации. Постройте график текущего положения и запишите ориентацию. Использовать pause имитировать обработку в реальном времени.
orientationLog = zeros(toa(end)*trajectory.SampleRate,1,'quaternion'); count = 1; while ~isDone(trajectory) [currentPosition,orientationLog(count)] = trajectory(); plot(currentPosition(1),currentPosition(2),'bo') pause(trajectory.SamplesPerFrame/trajectory.SampleRate) count = count + 1; end hold off
Преобразуйте кватернионы ориентации в углы Эйлера для упрощения интерпретации, а затем постройте график ориентации с течением времени.
figure(2) eulerAngles = eulerd([orientation(1);orientationLog],'ZYX','frame'); plot(toa,eulerAngles(:,1),'ko', ... toa,eulerAngles(:,2),'bd', ... toa,eulerAngles(:,3),'r.'); title('Orientation Over Time') legend('Rotation around Z-axis','Rotation around Y-axis','Rotation around X-axis') xlabel('Time (seconds)') ylabel('Rotation (degrees)') grid on
До сих пор объект траектории выводил только те ППМ, которые были указаны во время строительства. Для интерполяции между ППМ увеличьте частоту выборки до скорости, превышающей время прибытия ППМ. Установите trajectory частота дискретизации до 100 Гц и вызов reset.
trajectory.SampleRate = 100; reset(trajectory)
Создайте фигуру и постройте график начального положения платформы. В контуре выполните шаг по траектории для вывода текущего положения и текущей ориентации. Постройте график текущего положения и запишите ориентацию. Использовать pause имитировать обработку в реальном времени.
figure(1) plot(waypoints(1,1),waypoints(1,2),'b*') title('Position') axis([-1,2,-1,2]) axis square xlabel('X') ylabel('Y') grid on hold on orientationLog = zeros(toa(end)*trajectory.SampleRate,1,'quaternion'); count = 1; while ~isDone(trajectory) [currentPosition,orientationLog(count)] = trajectory(); plot(currentPosition(1),currentPosition(2),'bo') pause(trajectory.SamplesPerFrame/trajectory.SampleRate) count = count + 1; end hold off
Выход траектории теперь выглядит круговым. Это потому, что waypointTrajectory Система object™ минимизирует ускорение и угловую скорость при интерполяции, что приводит к более плавным и реалистичным движениям в большинстве сценариев.
Преобразуйте кватернионы ориентации в углы Эйлера для упрощения интерпретации, а затем постройте график ориентации с течением времени. Ориентация также интерполируется.
figure(2) eulerAngles = eulerd([orientation(1);orientationLog],'ZYX','frame'); t = 0:1/trajectory.SampleRate:4; plot(t,eulerAngles(:,1),'ko', ... t,eulerAngles(:,2),'bd', ... t,eulerAngles(:,3),'r.'); title('Orientation Over Time') legend('Rotation around Z-axis','Rotation around Y-axis','Rotation around X-axis') xlabel('Time (seconds)') ylabel('Rotation (degrees)') grid on
waypointTrajectory алгоритм интерполирует ППМ для создания гладкой траектории. Чтобы вернуться на квадратную траекторию, предоставьте больше ППМ, особенно вокруг резких изменений. Чтобы отслеживать соответствующее время, ППМ и ориентацию, укажите всю информацию о траектории в одной матрице.
% Time, Waypoint, Orientation trajectoryInfo = [0, 0,0,0, 0,0,0; ... % Initial position 0.1, 0,0.1,0, 0,0,0; ... 0.9, 0,0.9,0, 0,0,0; ... 1, 0,1,0, 45,0,0; ... 1.1, 0.1,1,0, 90,0,0; ... 1.9, 0.9,1,0, 90,0,0; ... 2, 1,1,0, 135,0,0; ... 2.1, 1,0.9,0, 180,0,0; ... 2.9, 1,0.1,0, 180,0,0; ... 3, 1,0,0, 225,0,0; ... 3.1, 0.9,0,0, 270,0,0; ... 3.9, 0.1,0,0, 270,0,0; ... 4, 0,0,0, 270,0,0]; % Final position trajectory = waypointTrajectory(trajectoryInfo(:,2:4), ... 'TimeOfArrival',trajectoryInfo(:,1), ... 'Orientation',quaternion(trajectoryInfo(:,5:end),'eulerd','ZYX','frame'), ... 'SampleRate',100);
Создайте фигуру и постройте график начального положения платформы. В контуре выполните шаг по траектории для вывода текущего положения и текущей ориентации. Постройте график текущего положения и запишите ориентацию. Использовать pause имитировать обработку в реальном времени.
figure(1) plot(waypoints(1,1),waypoints(1,2),'b*') title('Position') axis([-1,2,-1,2]) axis square xlabel('X') ylabel('Y') grid on hold on orientationLog = zeros(toa(end)*trajectory.SampleRate,1,'quaternion'); count = 1; while ~isDone(trajectory) [currentPosition,orientationLog(count)] = trajectory(); plot(currentPosition(1),currentPosition(2),'bo') pause(trajectory.SamplesPerFrame/trajectory.SampleRate) count = count+1; end hold off
Выход траектории теперь выглядит более квадратным, особенно вокруг вершин с ППМ.
Преобразуйте кватернионы ориентации в углы Эйлера для упрощения интерпретации, а затем постройте график ориентации с течением времени.
figure(2) eulerAngles = eulerd([orientation(1);orientationLog],'ZYX','frame'); t = 0:1/trajectory.SampleRate:4; eulerAngles = plot(t,eulerAngles(:,1),'ko', ... t,eulerAngles(:,2),'bd', ... t,eulerAngles(:,3),'r.'); title('Orientation Over Time') legend('Rotation around Z-axis', ... 'Rotation around Y-axis', ... 'Rotation around X-axis', ... 'Location', 'SouthWest') xlabel('Time (seconds)') ylabel('Rotation (degrees)') grid on
В этом примере показано, как создать траекторию дуги с помощью waypointTrajectory object™ системы. waypointTrajectory создает траекторию через указанные ППМ, которая минимизирует ускорение и угловую скорость. После создания дуговой траектории траектория ограничивается предварительно установленными границами.
Создание траектории дуги
Определите матрицу зависимостей, состоящую из ППМ, времени прибытия и ориентации для траектории дуги. Сформированная траектория проходит через ППМ в указанное время с заданной ориентацией. waypointTrajectory Для системного объекта необходимо указать ориентацию с помощью кватернионов или матриц вращения. Преобразование углов Эйлера, сохраненных в матрице зависимостей, в кватернионы при задании Orientation собственность.
% Arrival, Waypoints, Orientation constraints = [0, 20,20,0, 90,0,0; 3, 50,20,0, 90,0,0; 4, 58,15.5,0, 162,0,0; 5.5, 59.5,0,0 180,0,0]; trajectory = waypointTrajectory(constraints(:,2:4), ... 'TimeOfArrival',constraints(:,1), ... 'Orientation',quaternion(constraints(:,5:7),'eulerd','ZYX','frame'));
Звонить waypointInfo на trajectory для возврата таблицы указанных ограничений. Свойства создания Waypoints, TimeOfArrival, и Orientation являются переменными таблицы. Таблица удобна для индексирования при печати.
tInfo = waypointInfo(trajectory)
tInfo =
4x3 table
TimeOfArrival Waypoints Orientation
_____________ ____________________ ________________
0 20 20 0 {1x1 quaternion}
3 50 20 0 {1x1 quaternion}
4 58 15.5 0 {1x1 quaternion}
5.5 59.5 0 0 {1x1 quaternion}
Объект траектории выводит текущее положение, скорость, ускорение и угловую скорость при каждом вызове. Звонить trajectory в цикле и постройте график положения с течением времени. Кэширование других выходов.
figure(1) plot(tInfo.Waypoints(1,1),tInfo.Waypoints(1,2),'b*') title('Position') axis([20,65,0,25]) xlabel('North') ylabel('East') grid on daspect([1 1 1]) hold on orient = zeros(tInfo.TimeOfArrival(end)*trajectory.SampleRate,1,'quaternion'); vel = zeros(tInfo.TimeOfArrival(end)*trajectory.SampleRate,3); acc = vel; angVel = vel; count = 1; while ~isDone(trajectory) [pos,orient(count),vel(count,:),acc(count,:),angVel(count,:)] = trajectory(); plot(pos(1),pos(2),'bo') pause(trajectory.SamplesPerFrame/trajectory.SampleRate) count = count + 1; end
Проверьте ориентацию, скорость, ускорение и угловую скорость во времени. waypointTrajectory Система object™ создает траекторию через заданные ограничения, которые минимизируют ускорение и угловую скорость.
figure(2)
timeVector = 0:(1/trajectory.SampleRate):tInfo.TimeOfArrival(end);
eulerAngles = eulerd([tInfo.Orientation{1};orient],'ZYX','frame');
plot(timeVector,eulerAngles(:,1), ...
timeVector,eulerAngles(:,2), ...
timeVector,eulerAngles(:,3));
title('Orientation Over Time')
legend('Rotation around Z-axis', ...
'Rotation around Y-axis', ...
'Rotation around X-axis', ...
'Location','southwest')
xlabel('Time (seconds)')
ylabel('Rotation (degrees)')
grid on
figure(3)
plot(timeVector(2:end),vel(:,1), ...
timeVector(2:end),vel(:,2), ...
timeVector(2:end),vel(:,3));
title('Velocity Over Time')
legend('North','East','Down')
xlabel('Time (seconds)')
ylabel('Velocity (m/s)')
grid on
figure(4)
plot(timeVector(2:end),acc(:,1), ...
timeVector(2:end),acc(:,2), ...
timeVector(2:end),acc(:,3));
title('Acceleration Over Time')
legend('North','East','Down','Location','southwest')
xlabel('Time (seconds)')
ylabel('Acceleration (m/s^2)')
grid on
figure(5)
plot(timeVector(2:end),angVel(:,1), ...
timeVector(2:end),angVel(:,2), ...
timeVector(2:end),angVel(:,3));
title('Angular Velocity Over Time')
legend('North','East','Down')
xlabel('Time (seconds)')
ylabel('Angular Velocity (rad/s)')
grid on
Ограничить траекторию дуги в пределах заданных границ
Можно указать дополнительные ППМ для создания траекторий в заданных границах. Создайте верхние и нижние границы для траектории дуги.
figure(1) xUpperBound = [(20:50)';50+10*sin(0:0.1:pi/2)';60*ones(11,1)]; yUpperBound = [20.5.*ones(31,1);10.5+10*cos(0:0.1:pi/2)';(10:-1:0)']; xLowerBound = [(20:49)';50+9*sin(0:0.1:pi/2)';59*ones(11,1)]; yLowerBound = [19.5.*ones(30,1);10.5+9*cos(0:0.1:pi/2)';(10:-1:0)']; plot(xUpperBound,yUpperBound,'r','LineWidth',2); plot(xLowerBound,yLowerBound,'r','LineWidth',2)
Чтобы создать траекторию в пределах границ, добавьте дополнительные ППМ. Создание нового waypointTrajectory object™ системы, а затем вызовите ее в цикле для построения графика сформированной траектории. Кэширование ориентации, скорости, ускорения и угловой скорости на выходе trajectory объект.
% Time, Waypoint, Orientation constraints = [0, 20,20,0, 90,0,0; 1.5, 35,20,0, 90,0,0; 2.5 45,20,0, 90,0,0; 3, 50,20,0, 90,0,0; 3.3, 53,19.5,0, 108,0,0; 3.6, 55.5,18.25,0, 126,0,0; 3.9, 57.5,16,0, 144,0,0; 4.2, 59,14,0, 162,0,0; 4.5, 59.5,10,0 180,0,0; 5, 59.5,5,0 180,0,0; 5.5, 59.5,0,0 180,0,0]; trajectory = waypointTrajectory(constraints(:,2:4), ... 'TimeOfArrival',constraints(:,1), ... 'Orientation',quaternion(constraints(:,5:7),'eulerd','ZYX','frame')); tInfo = waypointInfo(trajectory); figure(1) plot(tInfo.Waypoints(1,1),tInfo.Waypoints(1,2),'b*') count = 1; while ~isDone(trajectory) [pos,orient(count),vel(count,:),acc(count,:),angVel(count,:)] = trajectory(); plot(pos(1),pos(2),'gd') pause(trajectory.SamplesPerFrame/trajectory.SampleRate) count = count + 1; end
Созданная траектория теперь вписывается в заданные границы. Визуализация ориентации, скорости, ускорения и угловой скорости создаваемой траектории.
figure(2) timeVector = 0:(1/trajectory.SampleRate):tInfo.TimeOfArrival(end); eulerAngles = eulerd(orient,'ZYX','frame'); plot(timeVector(2:end),eulerAngles(:,1), ... timeVector(2:end),eulerAngles(:,2), ... timeVector(2:end),eulerAngles(:,3)); title('Orientation Over Time') legend('Rotation around Z-axis', ... 'Rotation around Y-axis', ... 'Rotation around X-axis', ... 'Location','southwest') xlabel('Time (seconds)') ylabel('Rotation (degrees)') grid on figure(3) plot(timeVector(2:end),vel(:,1), ... timeVector(2:end),vel(:,2), ... timeVector(2:end),vel(:,3)); title('Velocity Over Time') legend('North','East','Down') xlabel('Time (seconds)') ylabel('Velocity (m/s)') grid on figure(4) plot(timeVector(2:end),acc(:,1), ... timeVector(2:end),acc(:,2), ... timeVector(2:end),acc(:,3)); title('Acceleration Over Time') legend('North','East','Down') xlabel('Time (seconds)') ylabel('Acceleration (m/s^2)') grid on figure(5) plot(timeVector(2:end),angVel(:,1), ... timeVector(2:end),angVel(:,2), ... timeVector(2:end),angVel(:,3)); title('Angular Velocity Over Time') legend('North','East','Down') xlabel('Time (seconds)') ylabel('Angular Velocity (rad/s)') grid on
Следует отметить, что в то время как сформированная траектория теперь укладывается в пространственные границы, ускорение и угловая скорость траектории являются несколько неустойчивыми. Это связано с чрезмерным заданием ППМ.
waypointTrajectory Системный объект определяет траекторию, плавно проходящую через ППМ. Траектория соединяет ППМ через интерполяцию, которая предполагает, что направление гравитации, выраженное в системе координат траектории, является постоянным. Как правило, можно использовать waypointTrajectory для моделирования траекторий платформы или транспортного средства в пределах расстояния в сотни километров.
Плоская траектория траектории (проекция плоскости x-y) состоит из кусочно-клотоидных кривых. Кривизна кривой между двумя последовательными ППМ изменяется линейно в зависимости от длины кривой между ними. Касательное направление траектории в каждой точке ППМ выбирается таким образом, чтобы минимизировать разрывы кривизны, если только курс не указан явно через Course или неявно через Velocities собственность. Как только путь установлен, объект использует кубическую интерполяцию Эрмита для вычисления местоположения транспортного средства по всему пути как функции времени и пройденного планарного расстояния.
Нормальная составляющая (z-составляющая) траектории впоследствии выбирается в соответствии с сохраняющим форму кусочным сплайном (PCHIP), если скорость набора не указана явно через ClimbRate свойство или третий столбец Velocities собственность. Выберите знак скорости подъема на основе выбранного ReferenceFrame:
Когда выбрана опорная рамка «RUS», задание положительной скорости подъема приводит к увеличению значения z.
Когда выбрана опорная рамка «NED», задание положительной скорости подъема приводит к уменьшению значения z.
Ориентацию транспортного средства по пути можно определить двумя основными способами:
Если Orientation задается свойство, то объект использует кусочно-кубический, кватернионный сплайн для вычисления ориентации вдоль траектории как функции времени.
Если Orientation свойство не указано, тогда рыскание транспортного средства всегда выравнивается с траекторией. Затем валком и шагом управляют AutoBank и AutoPitch значения свойств соответственно.
AutoBank | AutoPitch | Описание |
|---|---|---|
false | false | Транспортное средство всегда на уровне (нулевой шаг и крен). Это обычно используется для крупных морских судов. |
false | true | Шаг транспортного средства совмещен с траекторией, и его крен всегда равен нулю. Это обычно используется для наземных транспортных средств. |
true | false | Шаг и крен транспортного средства выбираются таким образом, чтобы его локальная ось z была совмещена с чистым ускорением (включая гравитацию). Обычно он используется для вертолетов. |
true | true | Крен транспортного средства выбирают таким образом, чтобы его локальная поперечная плоскость совпадала с чистым ускорением (включая гравитацию). Шаг транспортного средства совмещен с траекторией движения. Это обычно используется для двухколесных транспортных средств и самолетов. |