Генератор траектории с приводом от скорости
kinematicTrajectory Система object™ формирует траектории с использованием заданных ускорения и угловой скорости.
Чтобы создать траекторию из скоростей, выполните следующие действия.
Создать kinematicTrajectory и задайте его свойства.
Вызовите объект с аргументами, как если бы это была функция.
Дополнительные сведения о работе системных объектов см. в разделе Что такое системные объекты?.
trajectory = kinematicTrajectorytrajectory, которая формирует траекторию на основе ускорения и угловой скорости.
trajectory = kinematicTrajectory(Name,Value)Name к указанному Value. Неопределенные свойства имеют значения по умолчанию.
trajectory = kinematicTrajectory('SampleRate',200,'Position',[0,1,10]) создает кинематическую траекторию объект System, 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™ системы.
Создайте круговую траекторию длительностью 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)')
