Esta página se ha traducido mediante traducción automática. Haga clic aquí para ver la última versión en inglés.

waypointTrajectory

Generador de trayectoria de waypoint

Descripción

El waypointTrajectory System object™ genera trayectorias basadas en puntos de referencia específicos. Cuando crea el System object, puede elegir especificar la hora de llegada, la velocidad o la velocidad terrestre en cada punto de ruta. Opcionalmente, puede especificar otras propiedades, como la orientación en cada punto de referencia. Consulte Algoritmos para obtener más detalles.

Para generar una trayectoria a partir de waypoints:

1. Cree el objeto waypointTrajectory y establezca sus propiedades.

2. Llame al objeto como si fuera una función.

Para obtener más información sobre cómo funcionan los System objects, consulte What Are System Objects?.

Creación

Sintaxis

trajectory = waypointTrajectory

trajectory = waypointTrajectory(Waypoints,TimeOfArrival)

trajectory = waypointTrajectory(Waypoints,GroundSpeed=groundSpeed)

trajectory = waypointTrajectory(Waypoints,Velocities=velocities)

trajectory = waypointTrajectory(___,Name=Value)

Descripción

trajectory = waypointTrajectory devuelve un System object, trajectory, que genera una trayectoria basada en puntos de referencia estacionarios predeterminados.

trajectory = waypointTrajectory(Waypoints,TimeOfArrival) especifica la hora de llegada a la que la trayectoria generada pasa por cada waypoint. Consulte la propiedad TimeOfArrival para obtener más detalles.

Sugerencia

Cuando especifica el argumento TimeOfArrival , no debe especificar estas propiedades:

• JerkLimit

• InitialTime

• WaitTime

trajectory = waypointTrajectory(Waypoints,GroundSpeed=groundSpeed) especifica la velocidad de desplazamiento a la que pasa la trayectoria generada en cada punto de referencia. Consulte la propiedad GroundSpeed para obtener más detalles.

trajectory = waypointTrajectory(Waypoints,Velocities=velocities) especifica la velocidad a la que pasa la trayectoria generada en cada punto de ruta. Consulte la propiedad Velocities para obtener más detalles.

trajectory = waypointTrajectory(___,Name=Value) establece cada propiedad utilizando argumentos de nombre-valor. Las propiedades no especificadas tienen valores predeterminados o inferidos. Puede utilizar esta sintaxis con cualquiera de las sintaxis anteriores.

Ejemplo: trajectory = waypointTrajectory([10,10,0;20,20,0;20,20,10],[0,0.5,10]) crea un System object de trayectoria de waypoint, trajectory, que comienza en el waypoint [10,10,0] y luego pasa por [20,20,0] después de 0,5 segundos y [20,20,10] después de 10 segundos.

Propiedades

A menos que se indique lo contrario, las propiedades son no ajustables, lo que significa que no se pueden cambiar sus valores después de llamar al objeto. Los objetos se bloquean cuando los llamas y la función release los desbloquea.

Si una propiedad es ajustable, puedes cambiar su valor en cualquier momento.

Para obtener más información sobre cómo cambiar los valores de las propiedades, consulte System Design in MATLAB Using System Objects.

SampleRate — Frecuencia de muestreo de trayectoria (Hz)

100 (predeterminado) | escalar positivo

Frecuencia de muestreo de la trayectoria en Hz, especificada como un escalar positivo.

Ajustable: Yes

Tipos de datos: double

SamplesPerFrame — Número de muestras por cuadro de salida

1 (predeterminado) | entero escalar positivo

Número de muestras por cuadro de salida, especificado como un entero escalar positivo.

Tipos de datos: double

Waypoints — Posiciones en el sistema de coordenadas de navegación (m)

N-por-3 matriz

Posiciones en el sistema de coordenadas de navegación en metros, especificadas como una matriz N-por-3. Las columnas de la matriz corresponden al primer, segundo y tercer eje, respectivamente. Las filas de la matriz, N, corresponden a puntos de referencia individuales.

Sugerencia

Para dejar que la trayectoria espere en un waypoint específico, utilice una de las dos opciones:

• Si especificó el argumento de entrada TimeOfArrival , repita la coordenada del punto de ruta en dos filas consecutivas.

• Si no especificó el argumento de entrada TimeOfArrival , especifique el tiempo de espera usando la propiedad WaitTime .

Tipos de datos: double

TimeOfArrival — Tiempo en cada waypoint(s)

N-elemento vector columna de números crecientes no negativos

Tiempo correspondiente a la llegada a cada waypoint en segundos, especificado como un vector columna de elemento N . El primer elemento de TimeOfArrival debe ser 0. El número de muestras, N, debe ser igual al número de muestras (filas) definidas por Waypoints.

Dependencias

Para establecer esta propiedad, no debe especificar estas propiedades:

• JerkLimit

• InitialTime

• WaitTime

Tipos de datos: double

Velocities — Velocidad en el sistema de coordenadas de navegación en cada waypoint (m/s)

N-por-3 matriz

Velocidad en el sistema de coordenadas de navegación en cada punto de referencia en metros por segundo, especificada como una matriz N-por-3. Las columnas de la matriz corresponden al primer, segundo y tercer eje, respectivamente. El número de muestras, N, debe ser igual al número de muestras (filas) definidas por Waypoints.

Si la velocidad se especifica como un valor distinto de cero, el objeto calcula automáticamente el curso de la trayectoria en función de la velocidad. Si la velocidad se especifica como cero, el objeto infiere el curso de la trayectoria a partir de puntos de referencia adyacentes.

Tipos de datos: double

Course — Dirección de viaje horizontal (grados)

N-elemento vector real

Dirección de viaje horizontal, especificada como un vector real de elemento N en grados. El número de muestras, N, debe ser igual al número de muestras (filas) definidas por Waypoints. Si no se especifica ni Velocities ni Course , el rumbo se deduce de los waypoints.

Dependencias

Para establecer esta propiedad, no se debe especificar la propiedad Velocities .

Tipos de datos: double

GroundSpeed — Velocidad terrestre en cada punto de referencia (m/s)

N-elemento vector real

Velocidad terrestre en cada punto de referencia, especificada como un vector real de elemento N en m/s. Si no se especifica la propiedad, se deduce de los puntos de referencia. El número de muestras, N, debe ser igual al número de muestras (filas) definidas por Waypoints.

• Para representar el movimiento hacia adelante, especifique valores positivos de velocidad de desplazamiento.

• Para representar el movimiento hacia atrás, especifique valores negativos de velocidad de desplazamiento.

• Para representar el movimiento en reversa, separe los valores de velocidad sobre el terreno positivos y negativos por un valor de velocidad sobre el terreno cero.

Dependencias

Para establecer esta propiedad, no se debe especificar la propiedad Velocities .

Tipos de datos: double

ClimbRate — Velocidad de ascenso en cada waypoint (m/s)

N-elemento vector real

Velocidad de ascenso en cada punto de ruta en metros por segundo, especificada como un vector real de elemento N. El número de muestras, N, debe ser igual al número de muestras (filas) definidas por Waypoints. Si no se especifica ni Velocities ni Course , la velocidad de ascenso se infiere de los puntos de referencia.

Dependencias

Para establecer esta propiedad, no se debe especificar la propiedad Velocities .

Tipos de datos: double

JerkLimit — Límite de sacudida longitudinal (m/s³)

Inf (predeterminado) | escalar positivo

Límite de sacudida longitudinal, especificado como un escalar positivo en m/s³. Jerk es la derivada temporal de la aceleración. Cuando especifica esta propiedad, el objeto produce un perfil de aceleración trapezoidal horizontal basado en el límite de sacudida. Si el objeto waypointTrajectory no puede alcanzar el JerkLimit especificado, el objeto emite un error. Puede establecer esta propiedad solo durante la creación del objeto.

Dependencias

Para establecer esta propiedad, no se debe especificar la propiedad TimeOfArrival .

Tipos de datos: double

InitialTime — Tiempo antes de que comience la trayectoria (s)

0 (predeterminado) | escalar no negativo

Tiempo antes de que comience la trayectoria, especificado como un escalar no negativo en segundos. El objeto informa cantidades, como la posición y la velocidad, como NaN antes de que comience la trayectoria. Puede establecer esta propiedad solo durante la creación del objeto.

Dependencias

Para establecer esta propiedad, no se debe especificar la propiedad TimeOfArrival . En su lugar, debe especificar la propiedad GroundSpeed o Velocities al crear el objeto.

Tipos de datos: double

WaitTime — Tiempo de espera en cada waypoint(s)

N-vector elemento de 0 (predeterminado) | N-vector de elementos de escalares no negativos

Tiempo de espera en cada punto de ruta, especificado como un vector de elementos N de escalares no negativos. N debe ser igual al número de muestras (filas) definidas por Waypoints. Puede establecer esta propiedad solo durante la creación del objeto.

Dependencias

Para establecer esta propiedad, no se debe especificar la propiedad TimeOfArrival .

Si especificó la propiedad TimeOfArrival , no puede especificar el tiempo de espera a través de esta propiedad. En su lugar, especifique el tiempo de espera repitiendo las coordenadas del punto de referencia en dos filas consecutivas en la propiedad Waypoints .

Tipos de datos: double

Orientation — Orientación en cada waypoint

N-elemento vector columna de cuaternión | 3 por 3 por N arreglo de números reales

Orientación en cada punto de ruta, especificada como un elemento N quaternion o un vector columna de 3 por 3 por N de numeros reales. Cada quaternion debe tener una norma de 1. Cada matriz de rotación de 3 por 3 debe ser una matriz ortonormal. El número de cuaterniones o matrices de rotación, N, debe ser igual al número de muestras (filas) definidas por Waypoints.

Si Orientation se especifica mediante cuaterniones, la clase subyacente debe ser double.

Tipos de datos: double

AutoPitch — Alinear el ángulo de paso con la dirección del movimiento

false (predeterminado) | true

Alinee el ángulo de paso con la dirección del movimiento, especificada como true o false. Cuando se especifica como true, el ángulo de paso se alinea automáticamente con la dirección del movimiento. Si se especifica como false, el ángulo de paso se establece en cero (orientación nivelada).

Dependencias

Para establecer esta propiedad, no se debe especificar la propiedad Orientation .

AutoBank — Alinear el ángulo de giro para contrarrestar la fuerza centrípeta

false (predeterminado) | true

Alinee el ángulo de balanceo para contrarrestar la fuerza centrípeta, especificada como true o false. Cuando se especifica como true, el ángulo de balanceo contrarresta automáticamente la fuerza centrípeta. Si se especifica como false, el ángulo de balanceo se establece en cero (orientación plana).

Dependencias

Para establecer esta propiedad, no se debe especificar la propiedad Orientation .

ReferenceFrame — Marco de referencia de trayectoria.

'NED' (predeterminado) | 'ENU'

Marco de referencia de la trayectoria, especificado como 'NED' (Noreste-Abajo) o 'ENU' (Este-Norte-Arriba).

Uso

Sintaxis

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

Descripción

[position,orientation,velocity,acceleration,angularVelocity] = trajectory() genera un marco de datos de trayectoria basado en propiedades y argumentos de creación especificados.

Argumentos de salida

position — Posición en el sistema de coordenadas de navegación local (m)

M-por-3 matriz

Posición en el sistema de coordenadas de navegación local en metros, devuelta como una matriz M-por-3.

M se especifica mediante la propiedad SamplesPerFrame .

Tipos de datos: double

orientation — Orientación en el sistema de coordenadas de navegación local.

M-elemento vector columna de cuaternión | 3 por 3 por M arreglo real

Orientación en el sistema de coordenadas de navegación local, devuelta como un vector de columna M-por-1 quaternion o un vector columna de 3 por 3 por M arreglo real.

Cada cuaternión o matriz de rotación de 3 por 3 es una rotación de cuadro desde el sistema de coordenadas de navegación local hasta el sistema de coordenadas del cuerpo actual.

M se especifica mediante la propiedad SamplesPerFrame .

Tipos de datos: double

velocity — Velocidad en el sistema de coordenadas de navegación local (m/s)

M-por-3 matriz

Velocidad en el sistema de coordenadas de navegación local en metros por segundo, devuelta como una matriz M-por-3.

M se especifica mediante la propiedad SamplesPerFrame .

Tipos de datos: double

acceleration — Aceleración en sistema de coordenadas de navegación local (m/s²)

M-por-3 matriz

Aceleración en el sistema de coordenadas de navegación local en metros por segundo al cuadrado, devuelta como una matriz M-por-3.

M se especifica mediante la propiedad SamplesPerFrame .

Tipos de datos: double

angularVelocity — Velocidad angular en el sistema de coordenadas de navegación local (rad/s)

M-por-3 matriz

Velocidad angular en el sistema de coordenadas de navegación local en radianes por segundo, devuelta como una matriz M-por-3.

M se especifica mediante la propiedad SamplesPerFrame .

Tipos de datos: double

Funciones del objeto

Para utilizar una función de objeto, especifique System object como primer argumento de entrada. Por ejemplo, para liberar recursos del sistema de un System object llamado obj, use esta sintaxis:

release(obj)

Específico para waypointTrajectory

waypointInfo	Obtener tabla de información de waypoints
lookupPose	Obtener información de pose durante un tiempo determinado.
perturbations	Perturbación definida en el objeto.
perturb	Aplicar perturbaciones al objeto.

Comunes a todos los System objects

clone	Create duplicate System object
step	Ejecutar el algoritmo System object
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

Ejemplos

Crear 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'

Inspeccione los waypoints predeterminados y los horarios de llegada llamando a waypointInfo. Por defecto, los waypoints indican una posición estacionaria durante un segundo.

waypointInfo(trajectory)

ans=2×2 table
    TimeOfArrival     Waypoints 
    _____________    ___________

          0          0    0    0
          1          0    0    0

Crear trayectoria cuadrada

Cree una trayectoria cuadrada y examine la relación entre las restricciones de los puntos de referencia, la frecuencia de muestreo y la trayectoria generada.

Crea una trayectoria cuadrada definiendo los vértices del cuadrado. Defina la orientación en cada punto de referencia como apuntando en la dirección del movimiento. Especifique una frecuencia de muestreo de 1 Hz y utilice el SamplesPerFrame predeterminado 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);

Crea una figura y traza la posición inicial de la plataforma.

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

En un bucle, recorra la trayectoria para generar la posición actual y la orientación actual. Trace la posición actual y registre la orientación. Utilice pause para imitar el procesamiento en tiempo real.

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

Convierta los cuaterniones de orientación a ángulos de Euler para facilitar la interpretación y luego trace la orientación en el tiempo.

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

Hasta ahora, el objeto de trayectoria solo ha generado los waypoints que se especificaron durante la construcción. Para interpolar entre puntos de referencia, aumente la frecuencia de muestreo a una velocidad más rápida que el tiempo de llegada de los puntos de referencia. Establezca la frecuencia de muestreo de trajectory en 100 Hz y llame a reset.

trajectory.SampleRate = 100;
reset(trajectory)

Crea una figura y traza la posición inicial de la plataforma. En un bucle, recorra la trayectoria para generar la posición actual y la orientación actual. Trace la posición actual y registre la orientación. Utilice pause para imitar el procesamiento en tiempo real.

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 salida de la trayectoria ahora parece circular. Esto se debe a que el waypointTrajectory System object™ minimiza la aceleración y la velocidad angular al interpolar, lo que resulta en movimientos más suaves y realistas en la mayoría de los escenarios.

Convierta los cuaterniones de orientación a ángulos de Euler para facilitar la interpretación y luego trace la orientación en el tiempo. La orientación también se interpola.

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

El algoritmo waypointTrajectory interpola los puntos de referencia para crear una trayectoria suave. Para volver a la trayectoria cuadrada, proporcione más puntos de referencia, especialmente alrededor de cambios bruscos. Para realizar un seguimiento de los tiempos, puntos de referencia y orientación correspondientes, especifique toda la información de la trayectoria en una única matriz.

               % 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);

Crea una figura y traza la posición inicial de la plataforma. En un bucle, recorra la trayectoria para generar la posición actual y la orientación actual. Trace la posición actual y registre la orientación. Utilice pause para imitar el procesamiento en tiempo real.

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 salida de la trayectoria ahora parece más cuadrada, especialmente alrededor de los vértices con puntos de referencia.

Convierta los cuaterniones de orientación a ángulos de Euler para facilitar la interpretación y luego trace la orientación en el tiempo.

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

Crear trayectoria de arco

Este ejemplo muestra cómo crear una trayectoria de arco usando el objeto waypointTrajectory System object™. waypointTrajectory crea una ruta a través de puntos de referencia específicos que minimiza la aceleración y la velocidad angular. Después de crear una trayectoria de arco, restringe la trayectoria para que esté dentro de límites preestablecidos.

Crear una trayectoria de arco

Defina una matriz de restricciones que consta de puntos de referencia, tiempos de llegada y orientación para una trayectoria de arco. La trayectoria generada pasa por los puntos de referencia en los momentos especificados con la orientación especificada. El System object waypointTrajectory requiere que se especifique la orientación mediante cuaterniones o matrices de rotación. Convierta los ángulos de Euler guardados en la matriz de restricciones a cuaterniones al especificar la propiedad 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"));

Llame a waypointInfo en trajectory para devolver una tabla de las restricciones especificadas. Las propiedades de creación Waypoints, TimeOfArrival y Orientation son variables de la tabla. La tabla es conveniente para indexar mientras se traza.

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}

El objeto de trayectoria genera la posición actual, la velocidad, la aceleración y la velocidad angular en cada llamada. Llame a trajectory en un bucle y trace la posición en el tiempo. Almacene en caché las otras salidas.

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

Inspeccione la orientación, velocidad, aceleración y velocidad angular a lo largo del tiempo. El waypointTrajectory System object™ crea una ruta a través de las restricciones especificadas que minimiza la aceleración y la velocidad angular.

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

Restringir la trayectoria del arco dentro de límites preestablecidos

Puede especificar puntos de referencia adicionales para crear trayectorias dentro de límites determinados. Cree límites superior e inferior para la trayectoria del arco.

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)

Para crear una trayectoria dentro de los límites, agregue puntos de referencia adicionales. Cree un nuevo waypointTrajectory System object™ y luego llámelo en un bucle para trazar la trayectoria generada. Almacene en caché la salida de orientación, velocidad, aceleración y velocidad angular del objeto 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 trayectoria generada ahora encaja dentro de los límites especificados. Visualice la orientación, velocidad, aceleración y velocidad angular de la trayectoria generada.

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

Tenga en cuenta que si bien la trayectoria generada ahora encaja dentro de los límites espaciales, la aceleración y la velocidad angular de la trayectoria son algo erráticas. Esto se debe a que se especifican demasiado los puntos de referencia.

Generar trayectoria de pista usando waypointTrajectory

Considere una trayectoria en una pista de carreras como la siguiente.

Los cuatro puntos de las esquinas de la trayectoria son (0,0,0), (20,0,0), (20,5,0) y (0,5,0) en metros, respectivamente. Por lo tanto, especifique los puntos de referencia de un bucle como:

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

Supongamos que la trayectoria tiene una velocidad constante de 2 m/s y, por tanto, las velocidades en los cinco puntos de referencia son:

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

La hora de llegada a los cinco waypoints es:

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

La orientación de la trayectoria en los cinco puntos de referencia son:

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");

Especifique la frecuencia de muestreo como 100 para suavizar las líneas de trayectoria.

fs = 100;

Construya el waypointTrajectory.

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

Muestra y traza la trayectoria.

[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

Traza la trayectoria y los puntos de referencia especificados.

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

Crear trayectoria utilizando waypoints y velocidad terrestre

Cree un objeto waypointTrajectory que conecte dos waypoints. La velocidad de la trayectoria en los dos puntos de referencia es 0.$\mathrm{m}/\mathrm{s}$y 10$\mathrm{m}/\mathrm{s}$, respectivamente. Restringir el límite de sacudidas a 0,5$\mathrm{m}/{\mathrm{s}}^3$para habilitar el perfil de aceleración trapezoidal.

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

Obtenga el tiempo inicial y el tiempo final de la trayectoria consultando la propiedad TimeOfArrival . Cree marcas de tiempo para muestrear la trayectoria.

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

Obtenga la información de posición, velocidad y aceleración en estas marcas de tiempo muestreadas utilizando la función de objeto lookupPose .

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

Trazar la trayectoria.

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

Trazar el perfil de velocidad.

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)")

Trazar el perfil de aceleración. De los resultados se desprende que el perfil de aceleración del movimiento plano es trapezoidal.

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)")

Algoritmos

El System object waypointTrajectory define una trayectoria que pasa suavemente a través de los puntos de referencia. La trayectoria conecta los puntos de referencia a través de una interpolación que asume que la dirección de la gravedad expresada en el marco de referencia de la trayectoria es constante. Generalmente, puede utilizar waypointTrajectory para modelar trayectorias de plataformas o vehículos dentro de un lapso de distancia de cientos de kilómetros.

La trayectoria plana de la trayectoria (la proyección plana x- y ) consta de curvas clotoides por partes. La curvatura de la curva entre dos puntos de referencia consecutivos varía linealmente con la longitud de la curva entre ellos. La dirección tangente de la ruta en cada punto de ruta se elige para minimizar las discontinuidades en la curvatura, a menos que el rumbo se especifique explícitamente mediante la propiedad Course o implícitamente mediante la propiedad Velocities. . Una vez establecido la ruta, el objeto utiliza la interpolación cúbica de Hermite para calcular la ubicación del vehículo a lo largo de la ruta en función del tiempo y la distancia plana recorrida. Si se especifica la propiedad JerkLimit , los objetos producen un perfil de aceleración trapezoidal horizontal para cualquier segmento que se encuentre entre dos puntos de referencia. El perfil de aceleración trapezoidal consta de tres subsegmentos:

• Un subsegmento de sacudida de magnitud constante

• Un subsegmento de aceleración de magnitud constante

• Un subsegmento de sacudida de magnitud constante

El componente normal (z-componente) de la trayectoria se elige posteriormente para satisfacer un spline por partes que preserva la forma (PCHIP), a menos que la velocidad de ascenso se especifique explícitamente mediante el ClimbRate propiedad o la tercera columna de la propiedad Velocities . Elija el signo de la tasa de ascenso según el ReferenceFrame seleccionado:

• Cuando se selecciona un marco de referencia 'ENU', especificar una tasa de ascenso positiva da como resultado un valor creciente de z.

• Cuando se selecciona un marco de referencia 'NED', especificar una tasa de ascenso positiva da como resultado un valor decreciente de z.

Puede definir la orientación del vehículo a través de la ruta de dos formas principales:

• Si se especifica la propiedad Orientation , entonces el objeto utiliza una spline de cuaternión cúbica por partes para calcular la orientación a lo largo de la ruta como una función del tiempo.

• Si no se especifica la propiedad Orientation , entonces la orientación del vehículo siempre está alineada con la ruta. El balanceo y el cabeceo se rigen entonces por los valores de propiedad AutoBank y AutoPitch , respectivamente.

  AutoBank	AutoPitch	Descripción
  false	false	El vehículo está siempre nivelado (cabeceo y balanceo cero). Esto se utiliza normalmente para embarcaciones marinas grandes.
  false	true	El paso del vehículo está alineado con la trayectoria y su balanceo es siempre cero. Normalmente se utiliza para vehículos terrestres.
  true	false	El cabeceo y balanceo del vehículo se eligen de modo que su eje local z esté alineado con la aceleración neta (incluida la gravedad). Esto se utiliza normalmente para embarcaciones de ala giratoria.
  true	true	El balanceo del vehículo se elige de modo que su plano transversal local se alinee con la aceleración neta (incluida la gravedad). El paso del vehículo está alineado con la vía. Normalmente se utiliza para vehículos de dos ruedas y aviones de ala fija.

Capacidades ampliadas

Generación de código C/C++

Genere código C y C++ mediante MATLAB® Coder™.

La función de objeto, waypointInfo, no admite la generación de código.

Notas y limitaciones de uso:

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

Historial de versiones

Introducido en R2019b

R2023a: Especifique waypointTrajectory usando la velocidad de desplazamiento o la entrada de velocidad y nuevas propiedades

Al crear un objeto waypointTrajectory , si especifica la entrada de velocidad o velocidad de desplazamiento, la entrada de hora de llegada ya no es necesaria. Cuando no especifica la entrada de hora de llegada, puede utilizar estas nuevas propiedades:

• JerkLimit — Límite longitudinal del tirón de la trayectoria. Jerk es la derivada de la aceleración de traslación. Si especifica un valor finito para el límite de sacudidas, waypointTrajectory produce un perfil de aceleración trapezoidal horizontal basado en JerkLimit.

• InitialTime — Tiempo antes de que comience la trayectoria. Si se especifica como distinto de cero, waypointTrajectory retrasa el inicio de la trayectoria en el tiempo inicial.

• WaitTime— Tiempo de espera en cada punto de ruta. Si se especifica como distinto de cero para un waypoint, waypointTrajectory espera en el waypoint.

R2022b: Especificar el movimiento de espera y retroceso para la trayectoria del waypoint

Ahora puede especificar el movimiento de espera y retroceso utilizando el System object waypointTrajectory .

• Para dejar que la trayectoria espere en un punto de ruta específico, simplemente repita la coordenada del punto de ruta en dos filas consecutivas al especificar la propiedad Waypoints .

• Para representar el movimiento inverso, separe los valores de velocidad terrestre positivos (hacia adelante) y negativos (hacia atrás) por un valor cero en la propiedad GroundSpeed .

Consulte también

Objetos

• kinematicTrajectory