Генератор траектории Waypoint
waypointTrajectory Система object™ генерирует заданный waypoints использования траекторий. Когда вы создаете Системный объект, можно опционально задать время прибытия, скорость и ориентацию в каждом waypoint. Дополнительную информацию см. в Алгоритмах.
Сгенерировать траекторию от waypoints:
Создайте waypointTrajectory объект и набор его свойства.
Вызовите объект, как будто это была функция.
Чтобы узнать больше, как Системные объекты работают, смотрите то, Что Системные объекты?.
trajectory = waypointTrajectorytrajectory, это генерирует траекторию на основе стационарного waypoints по умолчанию.
trajectory = waypointTrajectory(Waypoints,TimeOfArrival)Waypoints то, что сгенерированная траектория проходит и TimeOfArrival в каждом waypoint.
trajectory = waypointTrajectory(Waypoints,TimeOfArrival,Name,Value)Name к заданному Value. Незаданные свойства и аргументы создания имеют значение по умолчанию или выведенные значения.
trajectory = waypointTrajectory([10,10,0;20,20,0;20,20,10],[0,0.5,10]) создает waypoint Системный объект траектории, trajectory, это запускается в waypoint [10,10,0], и затем проходит через [20,20,0] после 0,5 секунд и [20,20,10] после 10 секунд.Аргументы создания являются свойствами, которые установлены во время создания Системного объекта и не могут быть изменены позже. Если вы явным образом не устанавливаете значение аргумента создания, значение свойства выведено.
Если вы задаете какой-либо аргумент создания, то необходимо задать и Вейпойнтов и аргументы создания TimeOfArrival. Можно задать Waypoints и TimeOfArrival в качестве аргументов только для значения или пар "имя-значение".
[ выводит систему координат данных о траектории на основе заданных аргументов создания и свойств.position,orientation,velocity,acceleration,angularVelocity] = trajectory()
Чтобы использовать объектную функцию, задайте Системный объект как первый входной параметр. Например, чтобы выпустить системные ресурсы Системного объекта под названием 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'
Смотрите значение по умолчанию waypoints и времена прибытия путем вызова waypointInfo. По умолчанию waypoints указывают на стационарное положение в течение одной секунды.
waypointInfo(trajectory)
ans=2×2 table
TimeOfArrival Waypoints
_____________ ___________
0 0 0 0
1 0 0 0
Создайте квадратную траекторию и исследуйте отношение между waypoint ограничениями, частотой дискретизации и сгенерированной траекторией.
Создайте квадратную траекторию путем определения вершин квадрата. Задайте ориентацию в каждом waypoint как указывание в направлении движения. Задайте частоту дискретизации на 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
До сих пор объект траектории только вывел waypoints, которые были заданы во время конструкции. Чтобы интерполировать между waypoints, увеличьте частоту дискретизации до уровня быстрее, чем время прибытия waypoints. Установите 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 алгоритм интерполирует waypoints, чтобы создать сглаженную траекторию. Чтобы возвратиться к квадратной траектории, обеспечьте больше waypoints, особенно вокруг резких изменений. Чтобы отследить соответствующие времена, waypoints, и ориентацию, задают всю информацию о траектории в одной матрице.
% 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
Траектория выход теперь кажется более подобной квадрату, особенно вокруг вершин с waypoints.
Преобразуйте кватернионы ориентации в Углы Эйлера для легкой интерпретации, и затем стройте ориентацию в зависимости от времени.
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 создает путь через заданный waypoints, который минимизирует ускорение и скорость вращения. После создания траектории дуги вы ограничиваете траекторию, чтобы быть в предварительно установленных границах.
Создайте траекторию дуги
Задайте ограничительную матрицу, состоящую из waypoints, времена прибытия и ориентации для траектории дуги. Сгенерированная траектория проходит через waypoints в требуемые времена с заданной ориентацией. 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
Ограничьте траекторию дуги в предварительно установленных границах
Можно задать дополнительный waypoints, чтобы создать траектории в данных границах. Создайте верхние и нижние границы для траектории дуги.
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)
Чтобы создать траекторию в границах, добавьте дополнительный waypoints. Создайте новый 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
Обратите внимание на то, что, в то время как сгенерированная траектория теперь соответствует в пространственных контурах, ускорение и скорость вращения траектории несколько ошибочны. Это происходит из-за чрезмерного определения waypoints.
waypointTrajectory Системный объект задает траекторию, которая гладко проходит через waypoints. Траектория соединяет waypoints посредством интерполяции, которая принимает, что направление силы тяжести, описанное в системе координат траектории, является постоянным. Обычно можно использовать waypointTrajectory смоделировать платформу или траектории транспортного средства в сотни промежутка расстояния километров.
Плоский путь траектории (x-y плоская проекция) состоит из кусочных, кривых клотоиды. Искривление кривой между двумя последовательными waypoints варьируется линейно с длиной кривой между ними. Направление касательной пути в каждом waypoint выбрано, чтобы минимизировать разрывы в искривлении, если курс не задан явным образом через 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 | Крен транспортного средства выбран так, чтобы его локальная поперечная плоскость выровнялась с сетевым ускорением (включая силу тяжести). Тангаж транспортного средства выравнивается с путем. Это обычно используется для двухколесных транспортных средств и самолета с неподвижным крылом. |