Exponenta Banner

kinematicTrajectory

Управляемый уровнем генератор траектории

расширьте все на странице

Описание

Система kinematicTrajectory object™ генерирует траектории с помощью заданного ускорения и угловой скорости.

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

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

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

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

Создание

Синтаксис

trajectory = kinematicTrajectory
trajectory = kinematicTrajectory(Name,Value)

Описание

пример

trajectory = kinematicTrajectory возвращает Системный объект, trajectory, который генерирует траекторию на основе ускорения и угловой скорости.

пример

trajectory = kinematicTrajectory(Name,Value) наборы каждое свойство Name к заданному Value. Незаданные свойства имеют значения по умолчанию.

Пример: trajectory = kinematicTrajectory('SampleRate',200,'Position',[0,1,10]) создает кинематический Системный объект траектории, trajectory, с частотой дискретизации 200 Гц и набором исходного положения к [0,1,10].

Свойства

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

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

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

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

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

Состояние положения в локальной системе координат NED в метрах, заданных как трехэлементный вектор - строка.

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

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

Скоростное состояние в локальной системе координат NED в m/s, заданном как трехэлементный вектор - строка.

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

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

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

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

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

Источник ускоряющего состояния, заданного как 'Input' или 'Property'.

  • входной параметр задайте ускоряющее состояние как входной параметр к кинематическому объекту траектории

  • Свойство задайте ускоряющее состояние путем установки свойства Acceleration

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

Типы данных: char | string

Ускоряющее состояние в m/s2, заданном как трехэлементный вектор - строка.

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

Зависимости

Чтобы включить это свойство, установите AccelerationSource на 'Property'.

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

Источник углового скоростного состояния, заданного как 'Input' или 'Property'.

  • входной параметр задайте угловое скоростное состояние как входной параметр к кинематическому объекту траектории

  • Свойство задайте угловое скоростное состояние путем установки свойства AngularVelocity

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

Типы данных: char | string

Угловое скоростное состояние в rad/s, заданном как трехэлементный вектор - строка.

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

Зависимости

Чтобы включить это свойство, установите AngularVelocitySource на 'Property'.

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

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

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

Зависимости

Чтобы включить это свойство, установите AngularVelocitySource на 'Property' и AccelerationSource к 'Property'.

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

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

Синтаксис

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

Описание

пример

[position,orientation,velocity,acceleration,angularVelocity] = trajectory(bodyAcceleration,bodyAngularVelocity) выводит состояние траектории и затем обновляет состояние траектории на основе bodyAcceleration и bodyAngularVelocity.

Этот синтаксис только допустим, если AngularVelocitySource установлен в 'Input', и AccelerationSource установлен в 'Input'.

[position,orientation,velocity,acceleration,angularVelocity] = trajectory(bodyAngularVelocity) выводит состояние траектории и затем обновляет состояние траектории на основе bodyAngularAcceleration.

Этот синтаксис только допустим, если AngularVelocitySource установлен в 'Input', и AccelerationSource установлен в 'Property'.

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

Этот синтаксис только допустим, если AngularVelocitySource установлен в 'Property', и AccelerationSource установлен в 'Input'.

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

Этот синтаксис только допустим, если AngularVelocitySource установлен в 'Property', и AccelerationSource установлен в 'Property'.

Входные параметры

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

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

N является количеством выборок в текущем кадре.

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

N является количеством выборок в текущем кадре.

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

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

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

N является количеством выборок в текущем кадре.

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

Ориентация в локальной системе координат NED, возвращенной как N-by-1 вектор-столбец кватерниона или 3 3 N действительным массивом. Каждый кватернион или 3х3 матрица вращения являются вращением кадра от локальной системы координат NED до текущей системы координат тела.

N является количеством выборок в текущем кадре.

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

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

N является количеством выборок в текущем кадре.

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

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

N является количеством выборок в текущем кадре.

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

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

N является количеством выборок в текущем кадре.

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

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

stepЗапустите алгоритм Системного объекта

Примеры

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

Открыть скрипт

Создайте Систему kinematicTrajectory по умолчанию object™ и исследуйте отношение между входом, свойствами и сгенерированными траекториями.

trajectory = kinematicTrajectory

trajectory = 

  kinematicTrajectory with properties:

               SampleRate: 100
                 Position: [0 0 0]
              Orientation: [1x1 quaternion]
                 Velocity: [0 0 0]
       AccelerationSource: 'Input'
    AngularVelocitySource: 'Input'

По умолчанию объект kinematicTrajectory имеет исходное положение [0 0 0] и начальная скорость [0 0 0]. Ориентация описана кватернионом один (1 + 0i + 0j + 0k).

Объект kinematicTrajectory поддерживает видимое и перезаписываемое состояние в свойствах Position, Velocity и Orientation. Когда вы вызываете объект, состояние выводится и затем обновляется.

Например, вызовите объект путем определения ускорения и угловой скорости относительно системы координат тела.

bodyAcceleration = [5,5,0];
bodyAngularVelocity = [0,0,1];
[position,orientation,velocity,acceleration,angularVelocity] = trajectory(bodyAcceleration,bodyAngularVelocity)

position =

     0     0     0


orientation = 

  quaternion


     1 + 0i + 0j + 0k


velocity =

     0     0     0


acceleration =

     5     5     0


angularVelocity =

     0     0     1

Положение, ориентация и скорость вывод от объекта trajectory соответствуют состоянию, о котором сообщают свойства прежде, чем вызвать объект. Состояние trajectory обновляется, будучи названным и заметно из свойств:

trajectory

trajectory = 

  kinematicTrajectory with properties:

               SampleRate: 100
                 Position: [2.5000e-04 2.5000e-04 0]
              Orientation: [1x1 quaternion]
                 Velocity: [0.0500 0.0500 0]
       AccelerationSource: 'Input'
    AngularVelocitySource: 'Input'

Вывод acceleration и angularVelocity от объекта trajectory соответствует bodyAcceleration и bodyAngularVelocity, за исключением того, что они возвращены в системе координат NED. Используйте orientation вывод, чтобы вращать acceleration и angularVelocity к системе координат тела и проверить, что они приблизительно эквивалентны bodyAcceleration и bodyAngularVelocity.

rotatedAcceleration = rotatepoint(orientation,acceleration)
rotatedAngularVelocity = rotatepoint(orientation,angularVelocity)

rotatedAcceleration =

     5     5     0


rotatedAngularVelocity =

     0     0     1

Система kinematicTrajectory object™ позволяет вам изменить состояние траектории через свойства. Установите положение на [0,0,0] и затем вызовите объект с заданным ускорением и угловой скоростью в системе координат тела. В иллюстративных целях клонируйте объект trajectory прежде, чем изменить свойство Position. Вызовите оба объекта и заметьте, что положения отличаются.

trajectoryClone = clone(trajectory);
trajectory.Position = [0,0,0];

position = trajectory(bodyAcceleration,bodyAngularVelocity)
clonePosition = trajectoryClone(bodyAcceleration,bodyAngularVelocity)

position =

     0     0     0


clonePosition =

   1.0e-03 *

    0.2500    0.2500         0
Открыть скрипт

Этот пример показывает, как создать траекторию, колеблющуюся вдоль Северной оси локальной системы координат NED с помощью Системы kinematicTrajectory object™.

Создайте объект kinematicTrajectory по умолчанию. Начальная ориентация по умолчанию выравнивается с локальной системой координат NED.

traj = kinematicTrajectory

traj = 

  kinematicTrajectory with properties:

               SampleRate: 100
                 Position: [0 0 0]
              Orientation: [1x1 quaternion]
                 Velocity: [0 0 0]
       AccelerationSource: 'Input'
    AngularVelocitySource: 'Input'

Задайте траекторию на срок 10 секунд, состоящих из вращения вокруг Восточной оси (подача) и колебание вдоль Северной оси локальной системы координат NED. Используйте частоту дискретизации kinematicTrajectory по умолчанию.

fs = traj.SampleRate;
duration = 10;

numSamples = duration*fs;

cyclesPerSecond = 1;
samplesPerCycle = fs/cyclesPerSecond;
numCycles = ceil(numSamples/samplesPerCycle);
maxAccel = 20;

triangle = [linspace(maxAccel,1/fs-maxAccel,samplesPerCycle/2), ...
    linspace(-maxAccel,maxAccel-(1/fs),samplesPerCycle/2)]';
oscillation = repmat(triangle,numCycles,1);
oscillation = oscillation(1:numSamples);

accNED = [zeros(numSamples,2),oscillation];

angVelNED = zeros(numSamples,3);
angVelNED(:,2) = 2*pi;

Постройте ускоряющий управляющий сигнал.

timeVector = 0:1/fs:(duration-1/fs);

figure(1)
plot(timeVector,oscillation)
xlabel('Time (s)')
ylabel('Acceleration (m/s)^2')
title('Acceleration in Local NED Coordinate System')

Сгенерируйте выборку выборкой траектории в цикле. Системный объект kinematicTrajectory принимает ускорение, и угловые скоростные входные параметры находятся в локальной системе координат корпуса датчика. Вращайте ускорение и угловые скоростные управляющие сигналы от системы координат NED до системы координат корпуса датчика с помощью rotateframe и состояния Orientation. Обновите 3-D график положения каждый раз. Добавьте pause, чтобы подражать обработке в режиме реального времени. Если цикл завершен, стройте положение в зависимости от времени. Вращение accNED и управляющих сигналов angVelNED к системе координат органа местного самоуправления гарантирует, что движение остается вперед Вниз ось.

figure(2)
plotHandle = plot3(traj.Position(1),traj.Position(2),traj.Position(3),'bo');
grid on
xlabel('North')
ylabel('East')
zlabel('Down')
axis([-1 1 -1 1 0 1.5])
hold on

q = ones(numSamples,1,'quaternion');
for ii = 1:numSamples
     accBody = rotateframe(traj.Orientation,accNED(ii,:));
     angVelBody = rotateframe(traj.Orientation,angVelNED(ii,:));

    [pos(ii,:),q(ii),vel,ac] = traj(accBody,angVelBody);

    set(plotHandle,'XData',pos(ii,1),'YData',pos(ii,2),'ZData',pos(ii,3))

    pause(1/fs)
end

figure(3)
plot(timeVector,pos(:,1),'bo',...
     timeVector,pos(:,2),'r.',...
     timeVector,pos(:,3),'g.')
xlabel('Time (s)')
ylabel('Position (m)')
title('NED Position Over Time')
legend('North','East','Down')

Преобразуйте записанную ориентацию в Углы Эйлера и график. Несмотря на то, что ориентация платформы изменялась в зависимости от времени, ускорение всегда действовало вдоль Северной оси.

figure(4)
eulerAngles = eulerd(q,'ZYX','frame');
plot(timeVector,eulerAngles(:,1),'bo',...
     timeVector,eulerAngles(:,2),'r.',...
     timeVector,eulerAngles(:,3),'g.')
axis([0,duration,-180,180])
legend('Roll','Pitch','Yaw')
xlabel('Time (s)')
ylabel('Rotation (degrees)')
title('Orientation')

Открыть скрипт

Этот пример показывает, как сгенерировать обмоточную траекторию с помощью Системы kinematicTrajectory object™.

Создайте круговую траекторию на 1 000 второго времени и частоту дискретизации 10 Гц. Установите радиус круга к 5 000 метров и скорости к 80 метрам в секунду. Установите скороподъемность на 100 метров в секунду и подачу до 15 градусов. Задайте начальную ориентацию, как указано в направлении движения.

duration = 1000; % seconds
fs = 10;         % Hz
N = duration*fs; % number of samples

radius = 5000;   % meters
speed = 80;      % meters per second
climbRate = 50;  % meters per second
initialYaw = 90; % degrees
pitch = 15;      % degrees

initPos = [radius, 0, 0];
initVel = [0, speed, climbRate];
initOrientation = quaternion([initialYaw,pitch,0],'eulerd','zyx','frame');

trajectory = kinematicTrajectory('SampleRate',fs, ...
    'Velocity',initVel, ...
    'Position',initPos, ...
    'Orientation',initOrientation);

Задайте постоянное ускорение и угловую скорость в системе координат тела. Вращайте каркас кузова, чтобы составлять подачу.

accBody = zeros(N,3);
accBody(:,2) = speed^2/radius;
accBody(:,3) = 0.2;

angVelBody = zeros(N,3);
angVelBody(:,3) = speed/radius;

pitchRotation = quaternion([0,pitch,0],'eulerd','zyx','frame');
angVelBody = rotateframe(pitchRotation,angVelBody);
accBody = rotateframe(pitchRotation,accBody);

Вызовите trajectory с заданным ускорением и угловой скоростью в системе координат тела. Стройте положение, ориентацию и скорость в зависимости от времени.

[position, orientation, velocity] = trajectory(accBody,angVelBody);

eulerAngles = eulerd(orientation,'ZYX','frame');
speed = sqrt(sum(velocity.^2,2));

timeVector = (0:(N-1))/fs;

figure(1)
plot3(position(:,1),position(:,2),position(:,3))
xlabel('North (m)')
ylabel('East (m)')
zlabel('Down (m)')
title('Position')
grid on

figure(2)
plot(timeVector,eulerAngles(:,1),...
     timeVector,eulerAngles(:,2),...
     timeVector,eulerAngles(:,3))
axis([0,duration,-180,180])
legend('Yaw (Rotation Around Down)','Pitch (Rotation Around East)','Roll (Rotation Around North)')
xlabel('Time (s)')
ylabel('Rotation (degrees)')
title('Orientation')

figure(3)
plot(timeVector,speed)
xlabel('Time (s)')
ylabel('Speed (m/s)')
title('Speed')

Открыть скрипт

Задайте постоянную угловую скорость и постоянное ускорение, которые описывают растущую круговую траекторию.

Fs = 100;
r = 10;
speed = 2.5;
initialYaw = 90;

initPos = [r 0 0];
initVel = [0 speed 0];
initOrient = quaternion([initialYaw 0 0], 'eulerd', 'ZYX', 'frame');

accBody = [0 speed^2/r 0.01];
angVelBody = [0 0 speed/r];

Создайте кинематический объект траектории.

traj = kinematicTrajectory('SampleRate',Fs, ...
    'Position',initPos, ...
    'Velocity',initVel, ...
    'Orientation',initOrient, ...
    'AccelerationSource','Property', ...
    'Acceleration',accBody, ...
    'AngularVelocitySource','Property', ...
    'AngularVelocity',angVelBody);

Вызовите кинематический объект траектории в цикле и регистрируйте положение вывод. Стройте положение в зависимости от времени.

N = 10000;
pos = zeros(N, 3);
for i = 1:N
    pos(i,:) = traj();
end

plot3(pos(:,1), pos(:,2), pos(:,3))
title('Position')
xlabel('X (m)')
ylabel('Y (m)')
zlabel('Z (m)')

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

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

|

Введенный в R2018b

Документация Sensor Fusion and Tracking Toolbox
Поддержка