Управляемый уровнем генератор траектории
kinematicTrajectory Система object™ генерирует траектории с помощью заданного ускорения и скорости вращения.
Сгенерировать траекторию от уровней:
Создайте kinematicTrajectory объект и набор его свойства.
Вызовите объект с аргументами, как будто это была функция.
Чтобы узнать больше, как Системные объекты работают, смотрите то, Что Системные объекты?.
trajectory = kinematicTrajectorytrajectory, это генерирует траекторию на основе ускорения и скорости вращения.
trajectory = kinematicTrajectory(Name,Value)Name к заданному Value. Незаданные свойства имеют значения по умолчанию.
trajectory = kinematicTrajectory('SampleRate',200,'Position',[0,1,10]) создает кинематический Системный объект траектории, trajectory, с частотой дискретизации 200 Гц и набором исходного положения к [0,1,10].[ выводит состояние траектории и затем обновляет состояние траектории на основе 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'.
kinematicTrajectoryСоздайте 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 = 1×3
0 0 0
orientation = quaternion
1 + 0i + 0j + 0k
velocity = 1×3
0 0 0
acceleration = 1×3
5 5 0
angularVelocity = 1×3
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, за исключением того, что они возвращены в системе координат навигации. Используйте orientation выведите, чтобы вращать acceleration и angularVelocity к системе координат тела и проверяют, что они приблизительно эквивалентны bodyAcceleration и bodyAngularVelocity.
rotatedAcceleration = rotatepoint(orientation,acceleration)
rotatedAcceleration = 1×3
5 5 0
rotatedAngularVelocity = rotatepoint(orientation,angularVelocity)
rotatedAngularVelocity = 1×3
0 0 1
kinematicTrajectory Система object™ позволяет вам изменить состояние траектории через свойства. Установите положение на [0,0,0] и затем вызовите объект с заданным ускорением и скоростью вращения в системе координат тела. В иллюстративных целях клонируйте trajectory объект прежде, чем изменить Position свойство. Вызовите оба объекта и заметьте, что положения отличаются.
trajectoryClone = clone(trajectory); trajectory.Position = [0,0,0]; position = trajectory(bodyAcceleration,bodyAngularVelocity)
position = 1×3
0 0 0
clonePosition = trajectoryClone(bodyAcceleration,bodyAngularVelocity)
clonePosition = 1×3
10-3 ×
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('Yaw','Pitch','Roll') 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)')
