Cette page a été traduite par traduction automatique. Cliquez ici pour voir la dernière version en anglais.

waypointTrajectory

Générateur de trajectoire de waypoint

Depuis R2019b

Description

Le waypointTrajectory System object™ génère des trajectoires basées sur des waypoints spécifiés. Lorsque vous créez le System object, vous pouvez choisir de spécifier l'heure d'arrivée, la vitesse ou la vitesse sol à chaque waypoint. Vous pouvez éventuellement spécifier d'autres propriétés telles que l'orientation à chaque waypoint. Voir Algorithmes pour plus de détails.

Pour générer une trajectoire à partir de waypoints :

1. Créez l'objet waypointTrajectory et définissez ses propriétés.

2. Appelez l'objet comme s'il s'agissait d'une fonction.

Pour en savoir plus sur le fonctionnement des objets système, consultez What Are System Objects?.

Création

Syntaxe

trajectory = waypointTrajectory

trajectory = waypointTrajectory(Waypoints,TimeOfArrival)

trajectory = waypointTrajectory(Waypoints,GroundSpeed=groundSpeed)

trajectory = waypointTrajectory(Waypoints,Velocities=velocities)

trajectory = waypointTrajectory(___,Name=Value)

Description

trajectory = waypointTrajectory renvoie un System object, trajectory, qui génère une trajectoire basée sur des waypoints stationnaires par défaut.

trajectory = waypointTrajectory(Waypoints,TimeOfArrival) précise l'heure d'arrivée à laquelle la trajectoire générée passe par chaque waypoint. Voir la propriété TimeOfArrival pour plus de détails.

Conseil

Lorsque vous spécifiez l'argument TimeOfArrival , vous ne devez pas spécifier ces propriétés :

• JerkLimit

• InitialTime

• WaitTime

trajectory = waypointTrajectory(Waypoints,GroundSpeed=groundSpeed) spécifie la vitesse sol à laquelle passe la trajectoire générée à chaque waypoint. Voir la propriété GroundSpeed pour plus de détails.

trajectory = waypointTrajectory(Waypoints,Velocities=velocities) spécifie la vitesse à laquelle la trajectoire générée passe à chaque waypoint. Voir la propriété Velocities pour plus de détails.

trajectory = waypointTrajectory(___,Name=Value) définit chaque propriété en utilisant des arguments nom-valeur. Les propriétés non spécifiées ont des valeurs par défaut ou déduites. Vous pouvez utiliser cette syntaxe avec n'importe laquelle des syntaxes précédentes.

Exemple : trajectory = waypointTrajectory([10,10,0;20,20,0;20,20,10],[0,0.5,10]) crée un System object de trajectoire de point de cheminement, trajectory, qui commence au point de cheminement [10,10,0], puis passe par [20,20,0] après 0,5 seconde et [20,20,10] après 10 secondes.

Propriétés

Sauf indication contraire, les propriétés sont non réglables , ce qui signifie que vous ne pouvez pas modifier leurs valeurs après avoir appelé l'objet. Les objets se verrouillent lorsque vous les appelez et la fonction release les déverrouille.

Si une propriété est ajustable , vous pouvez modifier sa valeur à tout moment.

Pour plus d'informations sur la modification des valeurs des propriétés, voir System Design in MATLAB Using System Objects.

SampleRate — Taux d'échantillonnage de la trajectoire (Hz)

100 (par défaut) | scalaire positif

Taux d'échantillonnage de la trajectoire en Hz, spécifié sous forme de scalaire positif.

Ajustable : Yes

Types de données : double

SamplesPerFrame — Nombre d'échantillons par trame de sortie

1 (par défaut) | entier scalaire positif

Nombre d'échantillons par trame de sortie, spécifié sous la forme d'un entier scalaire positif.

Types de données : double

Waypoints — Positions dans le système de coordonnées de navigation (m)

N-matrice par 3

Positions dans le système de coordonnées de navigation en mètres, spécifiées sous la forme d'une matrice N-by-3. Les colonnes de la matrice correspondent respectivement aux premier, deuxième et troisième axes. Les lignes de la matrice, N, correspondent à des waypoints individuels.

Conseil

Pour laisser la trajectoire attendre à un waypoint spécifique, utilisez l'une des deux options :

• Si vous avez spécifié l'argument d'entrée TimeOfArrival , répétez les coordonnées du point de cheminement sur deux lignes consécutives.

• Si vous n'avez pas spécifié l'argument d'entrée TimeOfArrival , spécifiez le temps d'attente à l'aide de la propriété WaitTime .

Types de données : double

TimeOfArrival — Temps à chaque waypoint(s)

N-élément colonne vecteur de nombres croissants non négatifs

Temps correspondant à l'arrivée à chaque waypoint en secondes, spécifié sous la forme d'un vecteur colonne d'éléments N. Le premier élément de TimeOfArrival doit être 0. Le nombre d'échantillons, N, doit être le même que le nombre d'échantillons (lignes) défini par Waypoints.

Dépendances

Pour définir cette propriété, vous ne devez pas spécifier ces propriétés :

• JerkLimit

• InitialTime

• WaitTime

Types de données : double

Velocities — Vitesse dans le système de coordonnées de navigation à chaque waypoint (m/s)

N-matrice par 3

Vitesse dans le système de coordonnées de navigation à chaque point de cheminement en mètres par seconde, spécifiée sous la forme d'une matrice N-by-3. Les colonnes de la matrice correspondent respectivement aux premier, deuxième et troisième axes. Le nombre d'échantillons, N, doit être le même que le nombre d'échantillons (lignes) défini par Waypoints.

Si la vitesse est spécifiée comme une valeur non nulle, l'objet calcule automatiquement le tracé de la trajectoire en fonction de la vitesse. Si la vitesse est spécifiée comme nulle, l'objet déduit le tracé de la trajectoire à partir des waypoints adjacents.

Types de données : double

Course — Sens de déplacement horizontal (degré)

N-élément vecteur réel

Direction horizontale du déplacement, spécifiée comme un vecteur réel d'élément N en degrés. Le nombre d'échantillons, N, doit être le même que le nombre d'échantillons (lignes) défini par Waypoints. Si ni Velocities ni Course ne sont spécifiés, le cap est déduit des waypoints.

Dépendances

Pour définir cette propriété, la propriété Velocities ne doit pas être spécifiée.

Types de données : double

GroundSpeed — Vitesse sol à chaque waypoint (m/s)

N-élément vecteur réel

Vitesse sol à chaque point de cheminement, spécifiée comme un vecteur réel d'élément N en m/s. Si la propriété n'est pas spécifiée, elle est déduite des waypoints. Le nombre d'échantillons, N, doit être le même que le nombre d'échantillons (lignes) défini par Waypoints.

• Pour restituer un mouvement vers l'avant, spécifiez des valeurs de vitesse au sol positives.

• Pour restituer un mouvement vers l'arrière, spécifiez des valeurs de vitesse au sol négatives.

• Pour restituer le mouvement inverse, séparez les valeurs de vitesse sol positives et négatives par une valeur de vitesse sol nulle.

Dépendances

Pour définir cette propriété, la propriété Velocities ne doit pas être spécifiée.

Types de données : double

ClimbRate — Taux de montée à chaque waypoint (m/s)

N-élément vecteur réel

Taux de montée à chaque waypoint en mètres par seconde, spécifié comme vecteur réel d'élément N. Le nombre d'échantillons, N, doit être le même que le nombre d'échantillons (lignes) défini par Waypoints. Si ni Velocities ni Course ne sont spécifiés, le taux de montée est déduit des waypoints.

Dépendances

Pour définir cette propriété, la propriété Velocities ne doit pas être spécifiée.

Types de données : double

JerkLimit — Limite de secousse longitudinale (m/s³)

Inf (par défaut) | scalaire positif

Limite de secousse longitudinale, spécifiée sous la forme d'un scalaire positif en m/s³. Jerk est la dérivée temporelle de l'accélération. Lorsque vous spécifiez cette propriété, l'objet produit un profil d'accélération trapézoïdale horizontale basé sur la limite d'à-coup. Si l'objet waypointTrajectory ne peut pas atteindre le JerkLimit spécifié, l'objet génère une erreur. Vous pouvez définir cette propriété uniquement lors de la création de l'objet.

Dépendances

Pour définir cette propriété, la propriété TimeOfArrival ne doit pas être spécifiée.

Types de données : double

InitialTime — Temps avant le début de la trajectoire (s)

0 (par défaut) | scalaire non négatif

Temps avant le début de la trajectoire, spécifié sous forme d'un scalaire non négatif en secondes. L'objet signale des quantités, telles que la position et la vitesse, sous la forme NaN avant le début de la trajectoire. Vous pouvez définir cette propriété uniquement lors de la création de l'objet.

Dépendances

Pour définir cette propriété, la propriété TimeOfArrival ne doit pas être spécifiée. Au lieu de cela, vous devez spécifier la propriété GroundSpeed ou Velocities lors de la création de l'objet.

Types de données : double

WaitTime — Temps d'attente à chaque waypoint(s)

N-élément vecteur de 0 (par défaut) | N-élément vecteur de scalaires non négatifs

Temps d'attente à chaque point de cheminement, spécifié comme un vecteur d'élément N de scalaires non négatifs. N doit être le même que le nombre d'échantillons (lignes) défini par Waypoints. Vous pouvez définir cette propriété uniquement lors de la création de l'objet.

Dépendances

Pour définir cette propriété, la propriété TimeOfArrival ne doit pas être spécifiée.

Si vous avez spécifié la propriété TimeOfArrival , vous ne pouvez pas spécifier le temps d'attente via cette propriété. Au lieu de cela, spécifiez le temps d'attente en répétant les coordonnées du point de cheminement sur deux lignes consécutives dans la propriété Waypoints .

Types de données : double

Orientation — Orientation à chaque waypoint

N-élément vecteur colonne de quaternion | 3-by-3-by- N tableau de nombres réels

Orientation à chaque point de cheminement, spécifiée sous la forme d'un vecteur colonne N-élément quaternion ou d'un tableau 3-en-3-par- N de nombres réels. Chaque quaternion doit avoir une norme de 1. Chaque matrice de rotation 3x3 doit être une matrice orthonormée. Le nombre de quaternions ou de matrices de rotation, N, doit être le même que le nombre d'échantillons (lignes) défini par Waypoints.

Si Orientation est spécifié par des quaternions, la classe sous-jacente doit être double.

Types de données : double

AutoPitch — Aligner l'angle de tangage avec la direction du mouvement

false (par défaut) | true

Alignez l'angle de tangage avec la direction du mouvement, spécifiée comme true ou false. Lorsqu'il est spécifié comme true, l'angle de tangage s'aligne automatiquement sur la direction du mouvement. S'il est spécifié comme false, l'angle de tangage est défini sur zéro (orientation de niveau).

Dépendances

Pour définir cette propriété, la propriété Orientation ne doit pas être spécifiée.

AutoBank — Alignez l'angle de roulis pour contrecarrer la force centripète

false (par défaut) | true

Alignez l'angle de roulis pour contrecarrer la force centripète, spécifié comme true ou false. Lorsqu'il est spécifié comme true, l'angle de roulis contrecarre automatiquement la force centripète. S'il est spécifié comme false, l'angle de roulis est défini sur zéro (orientation à plat).

Dépendances

Pour définir cette propriété, la propriété Orientation ne doit pas être spécifiée.

ReferenceFrame — Référentiel de trajectoire

'NED' (par défaut) | 'ENU'

Référentiel de référence de la trajectoire, spécifié comme 'NED' (Nord-Est-Down) ou 'ENU' (Est-Nord-Up).

Utilisation

Syntaxe

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

Description

[position,orientation,velocity,acceleration,angularVelocity] = trajectory() génère une trame de données de trajectoire basée sur les arguments et propriétés de création spécifiés.

Arguments de sortie

position — Position dans le système de coordonnées de navigation local (m)

M-matrice par 3

Position dans le système de coordonnées de navigation local en mètres, renvoyée sous forme de matrice M-by-3.

M est spécifié par la propriété SamplesPerFrame .

Types de données : double

orientation — Orientation dans le système de coordonnées de navigation local

M-élément vecteur colonne de quaternion | 3-by-3-by- M tableau réel

Orientation dans le système de coordonnées de navigation local, renvoyée sous la forme d'un vecteur colonne M-by-1 quaternion ou d'un vecteur colonne 3-by-3-by- M tableau réel.

Chaque quaternion ou matrice de rotation 3 x 3 est une rotation de trame du système de coordonnées de navigation local vers le système de coordonnées du corps actuel.

M est spécifié par la propriété SamplesPerFrame .

Types de données : double

velocity — Vitesse dans le système de coordonnées de navigation local (m/s)

M-matrice par 3

Vitesse dans le système de coordonnées de navigation local en mètres par seconde, renvoyée sous la forme d'une matrice M-by-3.

M est spécifié par la propriété SamplesPerFrame .

Types de données : double

acceleration — Accélération dans le système de coordonnées de navigation local (m/s²)

M-matrice par 3

Accélération dans le système de coordonnées de navigation local en mètres par seconde carrée, renvoyée sous la forme d'une matrice M-by-3.

M est spécifié par la propriété SamplesPerFrame .

Types de données : double

angularVelocity — Vitesse angulaire dans le système de coordonnées de navigation local (rad/s)

M-matrice par 3

Vitesse angulaire dans le système de coordonnées de navigation local en radians par seconde, renvoyée sous la forme d'une matrice M-by-3.

M est spécifié par la propriété SamplesPerFrame .

Types de données : double

Fonctions d'objet

Pour utiliser une fonction objet, spécifiez le System object comme premier argument d'entrée. Par exemple, pour libérer les ressources système d'un System object nommé obj, utilisez cette syntaxe :

release(obj)

Spécifique au waypointTrajectory

waypointInfo	Obtenir le tableau d'informations sur les points de cheminement
lookupPose	Obtenir des informations de pose pendant un certain temps
perturbations	Perturbation définie sur l'objet
perturb	Appliquer des perturbations à un objet

Commun à tous les objets système

clone	Create duplicate System object
step	Run System object algorithm
release	Release resources and allow changes to System object property values and input characteristics
reset	Reset internal states of System object
isDone	End-of-data status

Exemples

Créer un waypointTrajectory

trajectory = waypointTrajectory

trajectory = 
  waypointTrajectory with properties:

         SampleRate: 100
    SamplesPerFrame: 1
          Waypoints: [2x3 double]
      TimeOfArrival: [2x1 double]
         Velocities: [2x3 double]
             Course: [2x1 double]
        GroundSpeed: [2x1 double]
          ClimbRate: [2x1 double]
        Orientation: [2x1 quaternion]
          AutoPitch: 0
           AutoBank: 0
     ReferenceFrame: 'NED'

Inspectez les waypoints par défaut et les heures d'arrivée en appelant waypointInfo. Par défaut, les waypoints indiquent une position stationnaire pendant une seconde.

waypointInfo(trajectory)

ans=2×2 table
    TimeOfArrival     Waypoints 
    _____________    ___________

          0          0    0    0
          1          0    0    0

Créer une trajectoire carrée

Créez une trajectoire carrée et examinez la relation entre les contraintes de point de cheminement, la fréquence d'échantillonnage et la trajectoire générée.

Créez une trajectoire carrée en définissant les sommets du carré. Définissez l'orientation à chaque waypoint comme pointant dans la direction du mouvement. Spécifiez une fréquence d'échantillonnage de 1 Hz et utilisez la valeur par défaut SamplesPerFrame de 1.

waypoints = [0,0,0; ... % Initial position
             0,1,0; ...
             1,1,0; ...
             1,0,0; ...
             0,0,0];    % Final position

toa = 0:4; % time of arrival

orientation = quaternion([0,0,0; ...
                          45,0,0; ...
                          135,0,0; ...
                          225,0,0; ...
                          0,0,0], ...
                          "eulerd","ZYX","frame");

trajectory = waypointTrajectory(waypoints, ...
    TimeOfArrival=toa, ...
    Orientation=orientation, ...
    SampleRate=1);

Créez une figure et tracez la position initiale de la plate-forme.

figure(1)
plot(waypoints(1,1),waypoints(1,2),"b*")
title("Position")
axis([-1,2,-1,2])
axis square
xlabel("X")
ylabel("Y")
grid on
hold on

Dans une boucle, parcourez la trajectoire pour afficher la position actuelle et l'orientation actuelle. Tracez la position actuelle et enregistrez l'orientation. Utilisez pause pour imiter le traitement en temps réel.

orientationLog = zeros(toa(end)*trajectory.SampleRate,1,"quaternion");
count = 1;
while ~isDone(trajectory)
   [currentPosition,orientationLog(count)] = trajectory();

   plot(currentPosition(1),currentPosition(2),"bo")

   pause(trajectory.SamplesPerFrame/trajectory.SampleRate)
   count = count + 1;
end
hold off

Convertissez les quaternions d'orientation en angles d'Euler pour une interprétation facile, puis tracez l'orientation au fil du temps.

figure(2)
eulerAngles = eulerd([orientation(1);orientationLog],"ZYX","frame");
plot(toa,eulerAngles(:,1),"ko", ...
     toa,eulerAngles(:,2),"bd", ...
     toa,eulerAngles(:,3),"r.");
title("Orientation Over Time")
legend("Rotation around Z-axis","Rotation around Y-axis","Rotation around X-axis")
xlabel("Time (seconds)")
ylabel("Rotation (degrees)")
grid on

Jusqu'à présent, l'objet trajectoire n'a généré que les waypoints spécifiés lors de la construction. Pour interpoler entre les waypoints, augmentez la fréquence d'échantillonnage jusqu'à une fréquence plus rapide que l'heure d'arrivée des waypoints. Réglez la fréquence d'échantillonnage trajectory sur 100 Hz et appelez reset.

trajectory.SampleRate = 100;
reset(trajectory)

Créez une figure et tracez la position initiale de la plate-forme. Dans une boucle, parcourez la trajectoire pour afficher la position actuelle et l'orientation actuelle. Tracez la position actuelle et enregistrez l'orientation. Utilisez pause pour imiter le traitement en temps réel.

figure(1)
plot(waypoints(1,1),waypoints(1,2),"b*")
title("Position")
axis([-1,2,-1,2])
axis square
xlabel("X")
ylabel("Y")
grid on
hold on

orientationLog = zeros(toa(end)*trajectory.SampleRate,1,"quaternion");
count = 1;
while ~isDone(trajectory)
   [currentPosition,orientationLog(count)] = trajectory();

   plot(currentPosition(1),currentPosition(2),"bo")

   pause(trajectory.SamplesPerFrame/trajectory.SampleRate)
   count = count + 1;
end
hold off

La sortie de trajectoire apparaît désormais circulaire. En effet, l' System object™ waypointTrajectory minimise l'accélération et la vitesse angulaire lors de l'interpolation, ce qui se traduit par des mouvements plus fluides et plus réalistes dans la plupart des scénarios.

Convertissez les quaternions d'orientation en angles d'Euler pour une interprétation facile, puis tracez l'orientation au fil du temps. L'orientation est également interpolée.

figure(2)
eulerAngles = eulerd([orientation(1);orientationLog],"ZYX","frame");
t = 0:1/trajectory.SampleRate:4;
plot(t,eulerAngles(:,1),"ko", ...
     t,eulerAngles(:,2),"bd", ...
     t,eulerAngles(:,3),"r.");
title("Orientation Over Time")
legend("Rotation around Z-axis","Rotation around Y-axis","Rotation around X-axis")
xlabel("Time (seconds)")
ylabel("Rotation (degrees)")
grid on

L'algorithme waypointTrajectory interpole les waypoints pour créer une trajectoire fluide. Pour revenir à la trajectoire carrée, prévoyez plus de waypoints, notamment autour des changements brusques. Pour suivre les temps, les waypoints et l'orientation correspondants, spécifiez toutes les informations de trajectoire dans une seule matrice.

               % Time, Waypoint, Orientation
trajectoryInfo = [0,   0,0,0,    0,0,0; ... % Initial position
                  0.1, 0,0.1,0,  0,0,0; ...

                  0.9, 0,0.9,0,  0,0,0; ...
                  1,   0,1,0,    45,0,0; ...
                  1.1, 0.1,1,0,  90,0,0; ...

                  1.9, 0.9,1,0,  90,0,0; ...
                  2,   1,1,0,    135,0,0; ...
                  2.1, 1,0.9,0,  180,0,0; ...

                  2.9, 1,0.1,0,  180,0,0; ...
                  3,   1,0,0,    225,0,0; ...
                  3.1, 0.9,0,0,  270,0,0; ...

                  3.9, 0.1,0,0,  270,0,0; ...
                  4,   0,0,0,    270,0,0];    % Final position

trajectory = waypointTrajectory(trajectoryInfo(:,2:4), ...
    TimeOfArrival=trajectoryInfo(:,1), ...
    Orientation=quaternion(trajectoryInfo(:,5:end),"eulerd","ZYX","frame"), ...
    SampleRate=100);

Créez une figure et tracez la position initiale de la plate-forme. Dans une boucle, parcourez la trajectoire pour afficher la position actuelle et l'orientation actuelle. Tracez la position actuelle et enregistrez l'orientation. Utilisez pause pour imiter le traitement en temps réel.

figure(1)
plot(waypoints(1,1),waypoints(1,2),"b*")
title("Position")
axis([-1,2,-1,2])
axis square
xlabel("X")
ylabel("Y")
grid on
hold on

orientationLog = zeros(toa(end)*trajectory.SampleRate,1,"quaternion");
count = 1;
while ~isDone(trajectory)
   [currentPosition,orientationLog(count)] = trajectory();

   plot(currentPosition(1),currentPosition(2),"bo")

   pause(trajectory.SamplesPerFrame/trajectory.SampleRate)
   count = count+1;
end
hold off

La sortie de trajectoire apparaît désormais plus carrée, en particulier autour des sommets avec des waypoints.

Convertissez les quaternions d'orientation en angles d'Euler pour une interprétation facile, puis tracez l'orientation au fil du temps.

figure(2)
eulerAngles = eulerd([orientation(1);orientationLog],"ZYX","frame");
t = 0:1/trajectory.SampleRate:4;
eulerAngles = plot(t,eulerAngles(:,1),"ko", ...
                   t,eulerAngles(:,2),"bd", ...
                   t,eulerAngles(:,3),"r.");
title("Orientation Over Time")
legend("Rotation around Z-axis", ...
       "Rotation around Y-axis", ...
       "Rotation around X-axis", ...
       "Location", "SouthWest")
xlabel("Time (seconds)")
ylabel("Rotation (degrees)")
grid on

Créer une trajectoire d'arc

Cet exemple montre comment créer une trajectoire d'arc à l'aide de l' waypointTrajectory System object™. waypointTrajectory crée un chemin à travers des points de cheminement spécifiés qui minimise l'accélération et la vitesse angulaire. Après avoir créé une trajectoire d'arc, vous limitez la trajectoire à l'intérieur de limites prédéfinies.

Créer une trajectoire d'arc

Définissez une matrice de contraintes composée de points de cheminement, d'heures d'arrivée et d'orientation pour une trajectoire en arc. La trajectoire générée passe par les waypoints aux heures spécifiées avec l'orientation spécifiée. L' System object waypointTrajectory nécessite que l'orientation soit spécifiée à l'aide de quaternions ou de matrices de rotation. Convertissez les angles d'Euler enregistrés dans la matrice de contraintes en quaternions lors de la spécification de la propriété Orientation .

          % Arrival, Waypoints, Orientation
constraints = [0,    20,20,0,    90,0,0;
               3,    50,20,0,    90,0,0;
               4,    58,15.5,0,  162,0,0;
               5.5,  59.5,0,0    180,0,0];

trajectory = waypointTrajectory(constraints(:,2:4), ...
    TimeOfArrival=constraints(:,1), ...
    Orientation=quaternion(constraints(:,5:7),"eulerd","ZYX","frame"));

Appelez waypointInfo sur trajectory pour renvoyer un tableau de vos contraintes spécifiées. Les propriétés de création Waypoints, TimeOfArrival et Orientation sont des variables de la table. Le tableau est pratique pour l’indexation lors du traçage.

tInfo = waypointInfo(trajectory)

tInfo =

  4x3 table

    TimeOfArrival         Waypoints            Orientation   
    _____________    ____________________    ________________

           0           20      20       0    {1x1 quaternion}
           3           50      20       0    {1x1 quaternion}
           4           58    15.5       0    {1x1 quaternion}
         5.5         59.5       0       0    {1x1 quaternion}

L'objet trajectoire génère la position, la vitesse, l'accélération et la vitesse angulaire actuelles à chaque appel. Appelez trajectory en boucle et tracez la position au fil du temps. Mettez en cache les autres sorties.

figure(1)
plot(tInfo.Waypoints(1,1),tInfo.Waypoints(1,2),"b*")
title("Position")
axis([20,65,0,25])
xlabel("North")
ylabel("East")
grid on
daspect([1 1 1])
hold on

orient = zeros(tInfo.TimeOfArrival(end)*trajectory.SampleRate,1,"quaternion");
vel = zeros(tInfo.TimeOfArrival(end)*trajectory.SampleRate,3);
acc = vel;
angVel = vel;

count = 1;
while ~isDone(trajectory)
   [pos,orient(count),vel(count,:),acc(count,:),angVel(count,:)] = trajectory();

   plot(pos(1),pos(2),"bo")

   pause(trajectory.SamplesPerFrame/trajectory.SampleRate)
   count = count + 1;
end

Inspectez l’orientation, la vitesse, l’accélération et la vitesse angulaire au fil du temps. L' System object™ waypointTrajectory crée un chemin à travers les contraintes spécifiées qui minimisent l'accélération et la vitesse angulaire.

figure(2)
timeVector = 0:(1/trajectory.SampleRate):tInfo.TimeOfArrival(end);
eulerAngles = eulerd([tInfo.Orientation{1};orient],"ZYX","frame");
plot(timeVector,eulerAngles(:,1), ...
     timeVector,eulerAngles(:,2), ...
     timeVector,eulerAngles(:,3));
title("Orientation Over Time")
legend("Rotation around Z-axis", ...
       "Rotation around Y-axis", ...
       "Rotation around X-axis", ...
       "Location","southwest")
xlabel("Time (seconds)")
ylabel("Rotation (degrees)")
grid on

figure(3)
plot(timeVector(2:end),vel(:,1), ...
     timeVector(2:end),vel(:,2), ...
     timeVector(2:end),vel(:,3));
title("Velocity Over Time")
legend("North","East","Down")
xlabel("Time (seconds)")
ylabel("Velocity (m/s)")
grid on

figure(4)
plot(timeVector(2:end),acc(:,1), ...
     timeVector(2:end),acc(:,2), ...
     timeVector(2:end),acc(:,3));
title("Acceleration Over Time")
legend("North","East","Down","Location","southwest")
xlabel("Time (seconds)")
ylabel("Acceleration (m/s^2)")
grid on

figure(5)
plot(timeVector(2:end),angVel(:,1), ...
     timeVector(2:end),angVel(:,2), ...
     timeVector(2:end),angVel(:,3));
title("Angular Velocity Over Time")
legend("North","East","Down")
xlabel("Time (seconds)")
ylabel("Angular Velocity (rad/s)")
grid on

Restreindre la trajectoire de l'arc dans des limites prédéfinies

Vous pouvez spécifier des waypoints supplémentaires pour créer des trajectoires dans des limites données. Créez des limites supérieure et inférieure pour la trajectoire de l'arc.

figure(1)
xUpperBound = [(20:50)';50+10*sin(0:0.1:pi/2)';60*ones(11,1)];
yUpperBound = [20.5.*ones(31,1);10.5+10*cos(0:0.1:pi/2)';(10:-1:0)'];

xLowerBound = [(20:49)';50+9*sin(0:0.1:pi/2)';59*ones(11,1)];
yLowerBound = [19.5.*ones(30,1);10.5+9*cos(0:0.1:pi/2)';(10:-1:0)'];

plot(xUpperBound,yUpperBound,"r","LineWidth",2);
plot(xLowerBound,yLowerBound,"r","LineWidth",2)

Pour créer une trajectoire dans les limites, ajoutez des waypoints supplémentaires. Créez un nouveau waypointTrajectory System object™, puis appelez-le en boucle pour tracer la trajectoire générée. Mettez en cache l’orientation, la vitesse, l’accélération et la vitesse angulaire de l’objet trajectory .

            % Time,  Waypoint,     Orientation
constraints = [0,    20,20,0,      90,0,0;
               1.5,  35,20,0,      90,0,0;
               2.5   45,20,0,      90,0,0;
               3,    50,20,0,      90,0,0;
               3.3,  53,19.5,0,    108,0,0;
               3.6,  55.5,18.25,0, 126,0,0;
               3.9,  57.5,16,0,    144,0,0;
               4.2,  59,14,0,      162,0,0;
               4.5,  59.5,10,0     180,0,0;
               5,    59.5,5,0      180,0,0;
               5.5,  59.5,0,0      180,0,0];

trajectory = waypointTrajectory(constraints(:,2:4), ...
    TimeOfArrival=constraints(:,1), ...
    Orientation=quaternion(constraints(:,5:7),"eulerd","ZYX","frame"));
tInfo = waypointInfo(trajectory);

figure(1)
plot(tInfo.Waypoints(1,1),tInfo.Waypoints(1,2),"b*")

count = 1;
while ~isDone(trajectory)
   [pos,orient(count),vel(count,:),acc(count,:),angVel(count,:)] = trajectory();

   plot(pos(1),pos(2),"gd")

   pause(trajectory.SamplesPerFrame/trajectory.SampleRate)
   count = count + 1;
end

La trajectoire générée s'inscrit désormais dans les limites spécifiées. Visualisez l'orientation, la vitesse, l'accélération et la vitesse angulaire de la trajectoire générée.

figure(2)
timeVector = 0:(1/trajectory.SampleRate):tInfo.TimeOfArrival(end);
eulerAngles = eulerd(orient,"ZYX","frame");
plot(timeVector(2:end),eulerAngles(:,1), ...
     timeVector(2:end),eulerAngles(:,2), ...
     timeVector(2:end),eulerAngles(:,3));
title("Orientation Over Time")
legend("Rotation around Z-axis", ...
       "Rotation around Y-axis", ...
       "Rotation around X-axis", ...
       "Location","southwest")
xlabel("Time (seconds)")
ylabel("Rotation (degrees)")
grid on

figure(3)
plot(timeVector(2:end),vel(:,1), ...
     timeVector(2:end),vel(:,2), ...
     timeVector(2:end),vel(:,3));
title("Velocity Over Time")
legend("North","East","Down")
xlabel("Time (seconds)")
ylabel("Velocity (m/s)")
grid on

figure(4)
plot(timeVector(2:end),acc(:,1), ...
     timeVector(2:end),acc(:,2), ...
     timeVector(2:end),acc(:,3));
title("Acceleration Over Time")
legend("North","East","Down")
xlabel("Time (seconds)")
ylabel("Acceleration (m/s^2)")
grid on

figure(5)
plot(timeVector(2:end),angVel(:,1), ...
     timeVector(2:end),angVel(:,2), ...
     timeVector(2:end),angVel(:,3));
title("Angular Velocity Over Time")
legend("North","East","Down")
xlabel("Time (seconds)")
ylabel("Angular Velocity (rad/s)")
grid on

Notez que même si la trajectoire générée s'inscrit désormais dans les limites spatiales, l'accélération et la vitesse angulaire de la trajectoire sont quelque peu erratiques. Cela est dû à une trop grande spécification des waypoints.

Générer une trajectoire de piste de course à l'aide de waypointTrajectory

Considérons une trajectoire de piste de course comme suit.

Les quatre points d'angle de la trajectoire sont respectivement (0,0,0), (20,0,0), (20,5,0) et (0,5,0) en mètres. Par conséquent, spécifiez les waypoints d'une boucle comme suit :

wps = [0 0 0;
      20 0 0;
      20 5 0;
      0  5 0;
      0  0 0];

Supposons que la trajectoire ait une vitesse constante de 2 m/s, et donc les vitesses aux cinq points de cheminement sont :

vels = [2 0 0;
        2 0 0;
       -2 0 0;
       -2 0 0;
        2 0 0];

L’heure d’arrivée pour les cinq waypoints est :

t = cumsum([0 20/2 5*pi/2/2 20/2 5*pi/2/2]');

L'orientation de la trajectoire aux cinq waypoints est :

eulerAngs = [0 0 0;
             0 0 0;
           180 0 0;
           180 0 0;
             0 0 0]; % Angles in degrees.
% Convert Euler angles to quaternions.
quats = quaternion(eulerAngs,"eulerd","ZYX","frame");

Spécifiez la fréquence d'échantillonnage sur 100 pour lisser les lignes de trajectoire.

fs = 100;

Construisez le waypointTrajectory.

traj = waypointTrajectory(wps,SampleRate=fs, ...
        Velocities=vels,...
        TimeOfArrival=t,...
        Orientation=quats);

Échantillonnez et tracez la trajectoire.

[pos, orient, vel, acc, angvel] = traj();
i = 1;

spf = traj.SamplesPerFrame;
while ~isDone(traj)
    idx = (i+1):(i+spf);
    [pos(idx,:), orient(idx,:), ...
        vel(idx,:), acc(idx,:), angvel(idx,:)] = traj();
    i = i+spf;
end

Tracez la trajectoire et les waypoints spécifiés.

plot(pos(:,1),pos(:,2), wps(:,1),wps(:,2), "--o")
xlabel("X (m)")
ylabel("Y (m)")
zlabel("Z (m)")
legend({"Trajectory", "Waypoints"})
axis equal

Créer une trajectoire à l'aide de points de cheminement et de vitesse au sol

Créez un objet waypointTrajectory qui connecte deux waypoints. La vitesse de la trajectoire aux deux waypoints est 0$\mathrm{m}/\mathrm{s}$et 10$\mathrm{m}/\mathrm{s}$, respectivement. Limiter la limite d'à-coup à 0,5$\mathrm{m}/{\mathrm{s}}^3$pour activer le profil d'accélération trapézoïdale.

waypoints = [0  0  0;
            10 50 10];
speeds = [0 10];
jerkLimit = 0.5;
trajectory = waypointTrajectory(waypoints,GroundSpeed=speeds,JerkLimit=jerkLimit);

Obtenez l'heure initiale et l'heure finale de la trajectoire en interrogeant la propriété TimeOfArrival . Créez des horodatages pour échantillonner la trajectoire.

t0 = trajectory.TimeOfArrival(1);
tf = trajectory.TimeOfArrival(end);
sampleTimes = linspace(t0,tf,100);

Obtenez les informations de position, de vitesse et d'accélération à ces horodatages échantillonnés à l'aide de la fonction objet lookupPose .

[position,~,velocity,acceleration,~] = lookupPose(trajectory,sampleTimes);

Tracez la trajectoire.

figure()
plot3(position(:,1),position(:,2),position(:,3))
xlabel("x (m)")
ylabel("y (m)")
zlabel("z (m)")
title("Trajectory")

Tracez le profil de vitesse.

figure()
subplot(3,1,1)
plot(sampleTimes,velocity(:,1));
ylabel("v_x (m/s)")
title("Velocity Profile")
subplot(3,1,2)
plot(sampleTimes,velocity(:,2));
ylabel("v_y (m/s)")
subplot(3,1,3)
plot(sampleTimes,velocity(:,3));
ylabel("v_z (m/s)")
xlabel("Time (sec)")

Tracez le profil d'accélération. D’après les résultats, le profil d’accélération du mouvement plan est trapézoïdal.

figure()
subplot(3,1,1)
plot(sampleTimes,acceleration(:,1));
axis padded
ylabel("a_x (m/s^2)")
title("Acceleration Profile")
subplot(3,1,2)
plot(sampleTimes,acceleration(:,2));
ylabel("a_y (m/s^2)")
axis padded
subplot(3,1,3)
plot(sampleTimes,acceleration(:,3));
ylabel("a_z (m/s^2)")
xlabel("Time (sec)")

Algorithmes

L' System object waypointTrajectory définit une trajectoire qui passe en douceur par des waypoints. La trajectoire relie les points de cheminement via une interpolation qui suppose que la direction de la gravité exprimée dans le référentiel de trajectoire est constante. Généralement, vous pouvez utiliser waypointTrajectory pour modéliser les trajectoires d'une plate-forme ou d'un véhicule sur une distance de plusieurs centaines de kilomètres.

Le chemin planaire de la trajectoire (la projection x- y ) se compose de courbes Clothoïdes par morceaux. La courbure de la courbe entre deux waypoints consécutifs varie linéairement avec la longueur de la courbe qui les sépare. La direction tangente du chemin à chaque waypoint est choisie pour minimiser les discontinuités dans la courbure, sauf si le cap est spécifié explicitement via la propriété Course ou implicitement via la propriété Velocities. . Une fois le chemin établi, l'objet utilise l'interpolation cubique Hermite pour calculer l'emplacement du véhicule tout au long du chemin en fonction du temps et de la distance planaire parcourue. Si la propriété JerkLimit est spécifiée, les objets produisent un profil d'accélération trapézoïdale horizontale pour tout segment situé entre deux points de cheminement. Le profil d'accélération trapézoïdale se compose de trois sous-segments :

• Un sous-segment de secousse d'ampleur constante

• Un sous-segment d'accélération à magnitude constante

• Un sous-segment de secousse d'ampleur constante

La composante normale (z-component) de la trajectoire est ensuite choisie pour satisfaire une spline par morceaux préservant la forme (PCHIP) à moins que le taux de montée ne soit spécifié explicitement via le ClimbRate ou la troisième colonne de la propriété Velocities . Choisissez le signe du taux de montée en fonction du ReferenceFrame sélectionné :

• Lorsqu'un référentiel 'ENU' est sélectionné, la spécification d'un taux de montée positif entraîne une valeur croissante de z.

• Lorsqu'un cadre de référence 'NED' est sélectionné, la spécification d'un taux de montée positif entraîne une valeur décroissante de z.

Vous pouvez définir l'orientation du véhicule sur le trajet de deux manières principales :

• Si la propriété Orientation est spécifiée, l'objet utilise une spline quaternion cubique par morceaux pour calculer l'orientation le long du chemin en fonction du temps.

• Si la propriété Orientation n'est pas spécifiée, alors le lacet du véhicule est toujours aligné avec la trajectoire. Le roulis et le tangage sont alors régis par les valeurs de propriété AutoBank et AutoPitch , respectivement.

  AutoBank	AutoPitch	Description
  false	false	Le véhicule est toujours de niveau (tangage et roulis nuls). Ceci est généralement utilisé pour les grands navires marins.
  false	true	Le tangage du véhicule est aligné avec la trajectoire et son roulis est toujours nul. Ceci est généralement utilisé pour les véhicules terrestres.
  true	false	Le tangage et le roulis du véhicule sont choisis de manière à ce que son axe local z soit aligné avec l'accélération nette (y compris la gravité). Ceci est généralement utilisé pour les avions à voilure tournante.
  true	true	Le roulis du véhicule est choisi de manière à ce que son plan transversal local s'aligne avec l'accélération nette (y compris la gravité). L'inclinaison du véhicule est alignée avec le chemin. Ceci est généralement utilisé pour les véhicules à deux roues et les avions à voilure fixe.

Capacités étendues

Génération de code C/C++

Générez du code C et C++ avec MATLAB® Coder™.

La fonction objet, waypointInfo, ne prend pas en charge la génération de code.

Notes d'utilisation et limitations :

Voir System Objects in MATLAB Code Generation (MATLAB Coder).

Historique des versions

Introduit dans R2019b

R2023a: Spécifiez waypointTrajectory en utilisant la vitesse au sol ou l'entrée de vitesse et de nouvelles propriétés

Lors de la création d'un objet waypointTrajectory , si vous spécifiez l'entrée de vitesse ou de vitesse sol, l'entrée d'heure d'arrivée n'est plus requise. Lorsque vous ne spécifiez pas l'heure d'arrivée, vous pouvez utiliser ces nouvelles propriétés :

• JerkLimit — Limite longitudinale de l'à-coup de trajectoire. Jerk est la dérivée de l'accélération de translation. Si vous spécifiez une valeur finie pour la limite d'à-coup, waypointTrajectory produit un profil d'accélération trapézoïdale horizontale basé sur JerkLimit.

• InitialTime — Temps avant le début de la trajectoire. S'il est spécifié comme différent de zéro, waypointTrajectory retarde le début de la trajectoire de l'heure initiale.

• WaitTime— Temps d'attente à chaque waypoint. S'il est spécifié comme différent de zéro pour un waypoint, waypointTrajectory attend au waypoint.

R2022b: Spécifier le mouvement d'attente et d'inversion pour la trajectoire du point de cheminement

Vous pouvez maintenant spécifier le mouvement d'attente et d'inversion à l'aide de l' System object waypointTrajectory .

• Pour laisser la trajectoire attendre à un waypoint spécifique, répétez simplement les coordonnées du waypoint sur deux lignes consécutives en spécifiant la propriété Waypoints .

• Pour restituer le mouvement inverse, séparez les valeurs de vitesse sol positives (avant) et négatives (arrière) par une valeur zéro dans la propriété GroundSpeed .

Voir aussi

Objets

• kinematicTrajectory