waypointTrajectory

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

Описание

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

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

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

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

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

Создание

Описание

пример

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 в качестве аргументов только для значения или пар "имя-значение".

Свойства

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

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

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

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

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

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

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

Количество выборок на выход структурирует в виде положительного скалярного целого числа.

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

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

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

Зависимости

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

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

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

Зависимости

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

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

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

Если скорость задана как ненулевое значение, объект автоматически вычисляет курс траектории. Если скорость задана как нуль, объект выводит курс траектории от смежного waypoints.

Зависимости

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

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

Горизонтальное направление перемещения в виде N - вектор действительных чисел элемента в градусах. Количество выборок, N, должно совпасть с количеством выборок (строки), заданные Waypoints. Если никакой Velocities ни Course задан, курс выведен из waypoints.

Зависимости

Установить это свойство, Velocities свойство не должно быть задано в создании объекта.

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

Groundspeed в каждом waypoint в виде N - вектор действительных чисел элемента в m/s. Если свойство не задано, оно выведено из waypoints. Количество выборок, N, должно совпасть с количеством выборок (строки), заданные Waypoints.

Зависимости

Установить это свойство, Velocities свойство не должно быть задано при создании объекта.

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

Climbrate в каждом waypoint в виде N - вектор действительных чисел элемента в градусах. Количество выборок, N, должно совпасть с количеством выборок (строки), заданные Waypoints. Если никакой Velocities ни Course задан, climbrate выведен из waypoints.

Зависимости

Установить это свойство, Velocities свойство не должно быть задано при создании объекта.

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

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

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

Зависимости

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

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

Выровняйте угол подачи с направлением движения в виде true или false. Когда задано как true, угол подачи автоматически выравнивается с направлением движения. Если задано как false, угол подачи обнуляется (ориентация уровня).

Зависимости

Установить это свойство, Orientation свойство не должно быть задано при создании объекта.

Выровняйте угол вращения, чтобы противодействовать центростремительной силе в виде true или false. Когда задано как true, угол вращения автоматически противодействует центростремительной силе. Если задано как false, угол вращения обнуляется (плоская ориентация).

Зависимости

Установить это свойство, Orientation свойство не должно быть задано при создании объекта.

Система координат траектории в виде 'NED' (Северо-восток вниз) или 'ENU' (Восточный Север).

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

Описание

пример

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

release(obj)

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

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

Примеры

свернуть все

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'

Смотрите значение по умолчанию 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

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