Генератор траектории путевой точки
The 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 секунд.Аргументы создания являются свойствами, которые задаются во время создания Системного объекта и не могут быть изменены позже. Если вы явным образом не задаете значение аргумента создания, значение свойства выводится.
Если вы задаете любой аргумент создания, то необходимо задать как аргументы создания точек пути, так и аргументы создания TimeOfArrivate. Можно задать Waypoints и TimeOfArrival как аргументы только для значения или пар "имя-значение".
[ выводит систему координат данных траектории на основе заданных аргументов и свойств создания.position,orientation,velocity,acceleration,angularVelocity] = trajectory()
Чтобы использовать функцию объекта, задайте системный объект в качестве первого входного параметра. Например, чтобы освободить системные ресурсы системного объекта с именем obj, используйте следующий синтаксис:
release(obj)
waypointTrajectory по умолчаниюtrajectory = 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
The 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 создает путь через указанные путевые точки, который минимизирует ускорение и скорость вращения. После создания траектории дуги можно ограничить траекторию заданными границами.
Создайте дуговую траекторию
Задайте матрицу ограничений, состоящую из точек пути, времени прибытия и ориентации для дуговой траектории. Сгенерированная траектория проходит через точки пути в заданное время с заданной ориентацией. The 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
Смотрите ориентацию, скорость, ускорение и скорость вращения с течением времени. The 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
Обратите внимание, что хотя сгенерированная траектория теперь находится в пространственных контурах, ускорение и скорость вращения траектории являются несколько изменчивыми. Это связано с переопределением путевых точек.
The waypointTrajectory Системный объект определяет траекторию, которая плавно проходит через путевые точки. Траектория соединяет точки пути через интерполяцию, которая принимает, что направление тяжести, выраженное в опорной системе координат траектории, является постоянным. Обычно можно использовать waypointTrajectory для моделирования траекторий платформы или транспортного средства в пределах расстояния в сотни километров.
Планарный путь траектории (проекция x - y плоскость) состоит из кусочно-хлороидных кривых. Кривизна кривой между двумя последовательными путевыми точками изменяется линейно с длиной кривой между ними. Направление тангенса пути в каждой точке пути выбирается таким образом, чтобы минимизировать разрывы в кривизне, если только курс не задан явным образом через Course свойство или неявно через Velocities свойство. Когда путь установлен, объект использует кубическую интерполяцию Гермита, чтобы вычислить местоположение транспортного средства на протяжении пути как функцию времени и пройденного планарного расстояния.
Нормальный компонент (z -компонент) траектории впоследствии выбирается, чтобы удовлетворить сохраняющему форму кусочно-сплайну (PCHIP), если только скорость набора высоты не задана явно через ClimbRate свойство или третий столбец Velocities свойство. Выберите знак скорости подъема на основе выбранных ReferenceFrame:
Когда выбирается система координат 'ENU', установка положительной скорости подъема приводит к увеличению значения z.
Когда выбирается система координат 'NED', установка положительной скорости подъема приводит к уменьшению значения z.
Вы можете задать ориентацию транспортного средства через путь двумя основными способами:
Если на Orientation задано свойство, тогда объект использует кусочно-кубический кватернионный сплайн, чтобы вычислить ориентацию вдоль пути как функцию времени.
Если на Orientation свойство не задано, тогда рыскание транспортного средства всегда выравнивается по пути. Затем крен и тангаж управляются AutoBank и AutoPitch значения свойств, соответственно.
AutoBank | AutoPitch | Описание |
|---|---|---|
false | false | Транспортное средство всегда ровно (нулевой тангаж и крен). Обычно это используется для крупных морских судов. |
false | true | Шаг транспортного средства совмещается с путем, и его крен всегда равен нулю. Обычно это используется для наземных транспортных средств. |
true | false | Тангаж и крен транспортного средства выбираются так, чтобы его локальная ось z была выровнена с ускорением сети (включая силу тяжести). Обычно это используется для винтокрылых судов. |
true | true | Транспортное средство крен выбрано так, чтобы его локальная поперечная плоскость совпала с ускорением сети (включая силу тяжести). Шаг транспортного средства совмещен с путем. Обычно это используется для двухколесных транспортных средств и самолетов. |