Генератор траектории Waypoint
waypointTrajectory Система object™ генерирует заданный waypoints использования траекторий. Когда вы создаете Системный объект, можно опционально задать время прибытия, скорость и ориентацию в каждом waypoint.
Сгенерировать траекторию от waypoints:
Создайте waypointTrajectory объект и набор его свойства.
Вызовите объект, как будто это была функция.
Чтобы узнать больше, как Системные объекты работают, смотрите то, Что Системные объекты? MATLAB.
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.