Генератор траектории Waypoint
Система waypointTrajectory object™ генерирует заданный waypoints использования траекторий. Когда вы создаете Системный объект, можно опционально задать время прибытия, скорость и ориентацию в каждом waypoint.
Сгенерировать траекторию от waypoints:
Создайте объект waypointTrajectory и установите его свойства.
Вызовите объект, как будто это была функция.
Чтобы узнать больше, как Системные объекты работают, смотрите то, Что Системные объекты? MATLAB.
trajectory = waypointTrajectorytrajectory = waypointTrajectory(Waypoints,TimeOfArrival)trajectory = waypointTrajectory(Waypoints,TimeOfArrival,Name,Value)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 секунд.[position, orientation, velocity, acceleration, angularVelocity] = trajectory()[ выводит кадр данных о траектории на основе заданных аргументов создания и свойств.position, orientation, velocity, acceleration, angularVelocity] = trajectory()
Чтобы использовать объектную функцию, задайте Системный объект как первый входной параметр. Например, чтобы выпустить системные ресурсы Системного объекта под названием obj, используйте этот синтаксис:
release(obj)
waypointTrajectorytrajectory = waypointTrajectory
trajectory =
waypointTrajectory with properties:
SampleRate: 100
SamplesPerFrame: 1
Осмотрите значение по умолчанию 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.