waypointTrajectory

Генератор траектории Waypoint

Описание

Система waypointTrajectory object™ генерирует заданный waypoints использования траекторий. Когда вы создаете Системный объект, можно опционально задать время прибытия, скорость и ориентацию в каждом waypoint.

Сгенерировать траекторию от waypoints:

  1. Создайте объект waypointTrajectory и установите его свойства.

  2. Вызовите объект, как будто это была функция.

Чтобы узнать больше, как Системные объекты работают, смотрите то, Что Системные объекты? MATLAB.

Создание

Синтаксис

trajectory = waypointTrajectory
trajectory = waypointTrajectory(Waypoints,TimeOfArrival)
trajectory = waypointTrajectory(Waypoints,TimeOfArrival,Name,Value)

Описание

пример

trajectory = waypointTrajectory возвращает Системный объект, trajectory, который генерирует траекторию на основе стационарного 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 в качестве аргументов только для значения или пар "имя-значение".

Положения в системе координат NED в метрах, заданных как N-by-3 матрица. Столбцы матрицы соответствуют Северу, Востоку, и Вниз осям, соответственно. Строки матрицы, N, соответствуют отдельному waypoints.

Зависимости

Чтобы установить это свойство, необходимо также установить допустимые значения для свойства TimeOfArrival.

Типы данных: double

Время соответствуя прибытию в каждый waypoint в секундах, заданных как N - вектор-столбец элемента. Первым элементом TimeOfArrival должен быть 0. Количество выборок, N, должно совпасть с количеством выборок (строки), заданные Waypoints.

Зависимости

Чтобы установить это свойство, необходимо также установить допустимые значения для свойства Waypoints.

Типы данных: double

Скорость в системе координат NED в каждом пути указывает в метрах в секунду, заданный как N-by-3 матрица. Столбцы матрицы соответствуют Северу, Востоку, и Вниз осям, соответственно. Количество выборок, N, должно совпасть с количеством выборок (строки), заданные Waypoints.

Зависимости

Чтобы установить это свойство, необходимо также установить допустимые значения для свойств Waypoints и TimeOfArrival.

Типы данных: double

Ориентация в каждом waypoint, заданном как N - элемент вектор-столбец quaternion или 3 3 N массивом вещественных чисел. Количество кватернионов или матриц вращения, N, должно совпасть с количеством выборок (строки), заданные Waypoints.

Если Orientation задан кватернионами, базовым классом должен быть double.

Зависимости

Чтобы установить это свойство, необходимо также установить допустимые значения для свойств Waypoints и TimeOfArrival.

Типы данных: quaternion | double

Свойства

развернуть все

Если в противном случае не обозначено, свойства являются ненастраиваемыми, что означает, что вы не можете изменить их значения после вызова объекта. Объекты блокируют, когда вы вызываете их, и функция release разблокировала их.

Если свойство является настраиваемым, можно изменить его значение в любое время.

Для получения дополнительной информации об изменении значений свойств смотрите Разработку системы в MATLAB Используя Системные объекты (MATLAB).

Частота дискретизации траектории в Гц, заданном как положительная скалярная величина.

Настраиваемый: да

Типы данных: double

Количество выборок в выводе, заданном как положительное скалярное целое число.

Настраиваемый: нет

Типы данных: double

Использование

Синтаксис

[position, orientation, velocity, acceleration, angularVelocity] = trajectory()

Описание

пример

[position, orientation, velocity, acceleration, angularVelocity] = trajectory() выводит кадр данных о траектории на основе заданных аргументов создания и свойств.

Выходные аргументы

развернуть все

Положение в локальной системе координат NED в метрах, возвращенных как M-by-3 матрица.

M задан как свойство SamplesPerFrame.

Типы данных: double

Ориентация в локальной системе координат NED, возвращенной как M-by-1 вектор-столбец quaternion или 3 3 M действительным массивом.

Каждый кватернион или 3х3 матрица вращения являются вращением кадра от локальной системы координат NED до текущей системы координат тела.

M задан как свойство SamplesPerFrame.

Типы данных: double

Скорость в локальной системе координат NED в метрах в секунду, возвращенный как M-by-3 матрица.

M задан как свойство SamplesPerFrame.

Типы данных: double

Ускорение в локальной системе координат NED в метрах в секунду придало квадратную форму, возвращенный как M-by-3 матрица.

M задан как свойство SamplesPerFrame.

Типы данных: double

Угловая скорость в локальной системе координат NED в радианах в секунду, возвращенный как M-by-3 матрица.

M задан как свойство SamplesPerFrame.

Типы данных: double

Функции объекта

Чтобы использовать объектную функцию, задайте Системный объект как первый входной параметр. Например, чтобы выпустить системные ресурсы Системного объекта под названием obj, используйте этот синтаксис:

release(obj)

развернуть все

waypointInfoПолучите waypoint информационную таблицу
stepЗапустите алгоритм Системного объекта
releaseВысвободите средства и позвольте изменения в значениях свойств Системного объекта и введите характеристики
resetСбросьте внутренние состояния Системного объекта
isDoneСостояние конца данных

Примеры

развернуть все

trajectory = 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.

Расширенные возможности

Смотрите также

|

Введенный в R2018b

Для просмотра документации необходимо авторизоваться на сайте