Управляемый скоростью генератор траектории
The 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).
The 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 объект соответствует состоянию, сообщаемому свойствами перед вызовом объекта. The 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'
The 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
The 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')
Сгенерируйте выборку по траектории в цикле. The 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™.
Создайте круговую траекторию для 1000-секундной длительности и частоты дискретизации 10 Гц. Установите радиус круга равным 5000 метров, а скорость - 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)')
