kinematicTrajectory
Générateur de trajectoire piloté par le taux
Description
Le kinematicTrajectory System object™ génère des trajectoires en utilisant une accélération et une vitesse angulaire spécifiées.
Pour générer une trajectoire à partir des tarifs :
Créez l'objet
kinematicTrajectoryet définissez ses propriétés.Appelez l'objet avec des arguments, comme s'il s'agissait d'une fonction.
Pour en savoir plus sur le fonctionnement des objets système, voir What Are System Objects?
Création
Description
trajectory = kinematicTrajectorytrajectory, qui génère une trajectoire basée sur l'accélération et la vitesse angulaire.
trajectory = kinematicTrajectory(Name,Value)Name sur le Value spécifié. Les propriétés non spécifiées ont des valeurs par défaut.
Exemple : trajectory = kinematicTrajectory('SampleRate',200,'Position',[0,1,10]) crée un System object de trajectoire cinématique, trajectory, avec une fréquence d'échantillonnage de 200 Hz et la position initiale définie sur [0,1,10].
Propriétés
Utilisation
Syntaxe
Description
[ génère l'état de la trajectoire, puis met à jour l'état de la trajectoire en fonction de position,orientation,velocity,acceleration,angularVelocity] = trajectory(bodyAcceleration,bodyAngularVelocity)bodyAcceleration et bodyAngularVelocity.
Cette syntaxe n'est valide que si AngularVelocitySource est défini sur 'Input' et AccelerationSource est défini sur 'Input'.
[ génère l'état de la trajectoire, puis met à jour l'état de la trajectoire en fonction de position,orientation,velocity,acceleration,angularVelocity] = trajectory(bodyAngularVelocity)bodyAngularVelocity.
Cette syntaxe n'est valide que si AngularVelocitySource est défini sur 'Input' et AccelerationSource est défini sur 'Property'.
[ génère l'état de la trajectoire, puis met à jour l'état de la trajectoire en fonction de position,orientation,velocity,acceleration,angularVelocity] = trajectory(bodyAcceleration)bodyAcceleration.
Cette syntaxe n'est valide que si AngularVelocitySource est défini sur 'Property' et AccelerationSource est défini sur 'Input'.
[ génère l'état de la trajectoire, puis met à jour l'état de la trajectoire.position,orientation,velocity,acceleration,angularVelocity] = trajectory()
Cette syntaxe n'est valide que si AngularVelocitySource est défini sur 'Property' et AccelerationSource est défini sur 'Property'.
Arguments en entrée
Arguments de sortie
Fonctions d'objet
Exemples
Créez un System object™ kinematicTrajectory par défaut et explorez la relation entre l'entrée, les propriétés et les trajectoires générées.
trajectory = kinematicTrajectory
trajectory =
kinematicTrajectory with properties:
SampleRate: 100
Position: [0 0 0]
Orientation: [1x1 quaternion]
Velocity: [0 0 0]
AccelerationSource: 'Input'
AngularVelocitySource: 'Input'
Par défaut, l'objet kinematicTrajectory a une position initiale de [0 0 0] et une vitesse initiale de [0 0 0]. L'orientation est décrite par un quaternion (1 + 0i + 0j + 0k).
L'objet kinematicTrajectory conserve un état visible et inscriptible dans les propriétés Position, Velocity et Orientation. Lorsque vous appelez l'objet, l'état est affiché puis mis à jour.
Par exemple, appelez l'objet en spécifiant une accélération et une vitesse angulaire par rapport au système de coordonnées du corps.
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
La position, l'orientation et la vitesse sorties de l'objet trajectory correspondent à l'état signalé par les propriétés avant d'appeler l'objet. L'état trajectory est mis à jour après avoir été appelé et est observable à partir des propriétés :
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'
Les sorties acceleration et angularVelocity de l'objet trajectory correspondent aux bodyAcceleration et bodyAngularVelocity, sauf qu'ils sont renvoyés dans le système de coordonnées de navigation. Utilisez la sortie orientation pour faire pivoter acceleration et angularVelocity vers le système de coordonnées du corps et vérifier qu'ils sont approximativement équivalents à bodyAcceleration et bodyAngularVelocity.
rotatedAcceleration = rotatepoint(orientation,acceleration)
rotatedAcceleration = 1×3
5 5 0
rotatedAngularVelocity = rotatepoint(orientation,angularVelocity)
rotatedAngularVelocity = 1×3
0 0 1
L' System object™ kinematicTrajectory vous permet de modifier l'état de la trajectoire à travers les propriétés. Définissez la position sur [0,0,0], puis appelez l'objet avec une accélération et une vitesse angulaire spécifiées dans le système de coordonnées du corps. À des fins d'illustration, clonez l'objet trajectory avant de modifier la propriété Position . Appelez les deux objets et observez que les positions divergent.
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
Cet exemple montre comment créer une trajectoire oscillant le long de l'axe Nord d'un système de coordonnées NED local à l'aide de l' kinematicTrajectory System object™.
Créez un objet kinematicTrajectory par défaut. L'orientation initiale par défaut est alignée sur le système de coordonnées NED local.
traj = kinematicTrajectory
traj =
kinematicTrajectory with properties:
SampleRate: 100
Position: [0 0 0]
Orientation: [1x1 quaternion]
Velocity: [0 0 0]
AccelerationSource: 'Input'
AngularVelocitySource: 'Input'
Définir une trajectoire d'une durée de 10 secondes composée d'une rotation autour de l'axe Est (tangage) et d'une oscillation le long de l'axe Nord du système de coordonnées NED local. Utilisez la fréquence d'échantillonnage par défaut 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;
Tracez le signal de contrôle d’accélération.
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')
Générez la trajectoire échantillon par échantillon dans une boucle. L' System object kinematicTrajectory suppose que les entrées d'accélération et de vitesse angulaire se trouvent dans le système de coordonnées local du corps du capteur. Faites pivoter les signaux de contrôle de l'accélération et de la vitesse angulaire du système de coordonnées NED vers le système de coordonnées du corps du capteur en utilisant rotateframe et l'état Orientation . Mettez à jour un tracé 3D de la position à chaque fois. Ajoutez pause pour imiter le traitement en temps réel. Une fois la boucle terminée, tracez la position au fil du temps. La rotation des signaux de commande accNED et angVelNED vers le système de coordonnées du corps local garantit que le mouvement reste le long de l'axe bas.
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')
Convertissez l'orientation enregistrée en angles d'Euler et tracez. Bien que l’orientation de la plate-forme ait changé au fil du temps, l’accélération s’est toujours produite le long de l’axe Nord.
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')
Cet exemple montre comment générer une trajectoire de bobine à l'aide de l' kinematicTrajectory System object™.
Créez une trajectoire circulaire d'une durée de 1 000 secondes et une fréquence d'échantillonnage de 10 Hz. Réglez le rayon du cercle sur 5 000 mètres et la vitesse sur 80 mètres par seconde. Réglez le taux de montée à 100 mètres par seconde et l'inclinaison à 15 degrés. Spécifiez l'orientation initiale comme pointée dans la direction du mouvement.
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);
Spécifiez une accélération et une vitesse angulaire constantes dans le système de coordonnées du corps. Faites pivoter le cadre du corps pour tenir compte du pas.
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);
Appelez trajectory avec l'accélération et la vitesse angulaire spécifiées dans le système de coordonnées du corps. Tracez la position, l'orientation et la vitesse au fil du temps.
[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')
Définissez une vitesse angulaire constante et une accélération constante qui décrivent une trajectoire circulaire en spirale.
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];
Créez un objet de trajectoire cinématique.
traj = kinematicTrajectory('SampleRate',Fs, ... 'Position',initPos, ... 'Velocity',initVel, ... 'Orientation',initOrient, ... 'AccelerationSource','Property', ... 'Acceleration',accBody, ... 'AngularVelocitySource','Property', ... 'AngularVelocity',angVelBody);
Appelez l'objet trajectoire cinématique dans une boucle et enregistrez la sortie de position. Tracez la position au fil du temps.
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)')
Capacités étendues
Historique des versions
Introduit dans R2019b
