derivative

Derivada de tiempo del estado del vehículo

Sintaxis

stateDot = derivative(motionModel,state,cmds)

Descripción

stateDot = derivative(motionModel,state,cmds) devuelve la derivada del estado actual, stateDot, como un vector de tres elementos [xDot yDot thetaDot] si el modelo de movimiento es un objeto bicycleKinematics, differentialDriveKinematics o unicycleKinematics.

Devuelve stateDot como un vector de cuatro elementos, [xDot yDot thetaDot psiDot], si el modelo de movimiento es un objeto ackermannKinematics. xDot e yDot hacen referencia a la velocidad del vehículo especificada en metros por segundo. thetaDot es la velocidad angular del rumbo del vehículo y psiDot es la velocidad angular de la dirección del vehículo, ambas especificadas en radianes por segundo.

Para el modelo de movimiento articulatedSteeringKinematics, devuelve stateDot como un vector de cuatro elementos, [xDot yDot thetaDot gammaDot]. xDot e yDot hacen referencia a la velocidad del vehículo, especificada en metros por segundo. thetaDot es la velocidad angular del cuerpo delantero y gammaDot es la velocidad angular del cuerpo delantero en relación con el cuerpo trasero, ambas especificadas en radianes por segundo.

Ejemplos

Simular el modelo cinemático de Ackermann con restricciones de ángulo de dirección

Simule un modelo de robot móvil que utilice la dirección de Ackermann con restricciones en su ángulo de dirección. Durante la simulación, el modelo mantiene el ángulo de dirección máximo después de alcanzar el límite de dirección. Para ver el efecto de la saturación de la dirección, se compara la trayectoria de dos robots, una con las restricciones del ángulo de dirección y la otra sin ninguna restricción de dirección.

Definir el modelo

Defina el modelo cinemático de Ackermann. En este modelo tipo automóvil, las ruedas delanteras están a una distancia determinada. Para que giren en círculos concéntricos, las ruedas tienen diferentes ángulos de dirección. Al girar, las ruedas delanteras reciben la entrada de la dirección como velocidad de cambio del ángulo de dirección.

carLike = ackermannKinematics; 

Configurar los parámetros de la simulación

Establezca el robot móvil para que siga una velocidad lineal constante y reciba una tasa de dirección constante como entrada. Simule el robot restringido durante un período más largo para demostrar la saturación de la dirección.

velo = 5;    % Constant linear velocity 
psidot = 1;  % Constant left steering rate 

% Define the total time and sample rate 
sampleTime = 0.05;                  % Sample time [s]
timeEnd1 = 1.5;                     % Simulation end time for unconstrained robot 
timeEnd2 = 10;                      % Simulation end time for constrained robot 
tVec1 = 0:sampleTime:timeEnd1;      % Time array for unconstrained robot 
tVec2 = 0:sampleTime:timeEnd2;      % Time array for constrained robot  

initPose = [0;0;0;0];               % Initial pose (x y theta phi) 

Crear la estructura de opciones para un solver EDO

En este ejemplo, se pasa una estructura de options como argumento al solver EDO. La estructura de options contiene la información sobre el límite del ángulo de dirección. Para crear la estructura de options, utilice la opción Events de odeset y la función de evento creada, detectSteeringSaturation. detectSteeringSaturation establece el ángulo de dirección máximo en 45 grados.

Para una descripción de cómo definir detectSteeringSaturation, consulte Definir la función de evento al final de este ejemplo.

options = odeset('Events',@detectSteeringSaturation);

Simular el modelo con un solver EDO

Luego, se utiliza la función derivative y un solver EDO, ode45, para resolver el modelo y generar la solución.

% Simulate the unconstrained robot 
[t1,pose1] = ode45(@(t,y)derivative(carLike,y,[velo psidot]),tVec1,initPose);

% Simulate the constrained robot 
[t2,pose2,te,ye,ie] = ode45(@(t,y)derivative(carLike,y,[velo psidot]),tVec2,initPose,options);

Detectar la saturación de la dirección

Cuando el modelo alcanza el límite de dirección, registra una marca de tiempo del evento. El tiempo que se tarda en alcanzar el límite se almacena en te.

if te < timeEnd2
    str1 = "Steering angle limit was reached at ";
    str2 = " seconds";
    comp = str1 + te + str2; 
    disp(comp)
end 

Steering angle limit was reached at 0.785 seconds

Simular un robot restringido con nuevas condiciones iniciales

Utilice ahora el estado del robot restringido antes de terminar la integración como condición inicial para la segunda simulación. Modifique el vector de entrada para representar la saturación de la dirección, es decir, establezca la tasa de dirección en cero.

saturatedPsiDot = 0;             % Steering rate after saturation 
cmds = [velo saturatedPsiDot];   % Command vector 
tVec3 = te:sampleTime:timeEnd2;  % Time vector 
pose3 = pose2(length(pose2),:); 
[t3,pose3,te3,ye3,ie3] = ode45(@(t,y)derivative(carLike,y,cmds),tVec3,pose3,options);

Representar los resultados

Represente la trayectoria del robot usando plot y los datos almacenados en pose.

figure(1)
plot(pose1(:,1),pose1(:,2),LineWidth=2)
hold on
plot([pose2(:,1); pose3(:,1)],[pose2(:,2);pose3(:,2)])
title("Trajectory X-Y")
xlabel("X")
ylabel("Y") 
legend("Unconstrained Robot","Constrained Robot",Location="northwest")
axis equal

El robot sin restricciones sigue una trayectoria en espiral con radio de curvatura decreciente, mientras que el robot restringido sigue una trayectoria circular con radio de curvatura constante una vez alcanzado el límite de dirección.

Definir la función de evento

Establezca la función de evento de manera que la integración termine cuando el 4º estado, theta, sea igual al ángulo de dirección máximo.

function [state,isterminal,direction] = detectSteeringSaturation(t,y)
  maxSteerAngle = 0.785;               % Maximum steering angle (pi/4 radians)
  state(4) = (y(4) - maxSteerAngle);   % Saturation event occurs when the 4th state, theta, is equal to the max steering angle    
  isterminal(4) = 1;                   % Integration is terminated when event occurs 
  direction(4) = 0;                    % Bidirectional termination 

end

Calcular la derivada de varios estados a la vez

Cree un modelo cinemático de bicicleta.

model = bicycleKinematics;

Establezca los cuatro estados y los comandos de control para los respectivos estados.

state = [0 0 0;
         1 1 0;
         2 2 0;
         3 3 0];
control = [0.1 pi/10;
           1.0 pi/10;
           5.0 pi/10;
           9.0 pi/10];

Calcule las derivadas de estado para todos los comandos de control.

stateDot = model.derivative(state,control)

stateDot = 3×4

    0.1000    1.0000    5.0000    9.0000
         0         0         0         0
    0.0325    0.3249    1.6246    2.9243

Argumentos de entrada

motionModel — Objeto de modelo cinemático móvil

objeto ackermannKinematics | objeto bicycleKinematics | objeto differentialDriveKinematics | objeto unicycleKinematics | objeto articulatedSteeringKinematics

Objeto modelo cinemático móvil, que define las propiedades del modelo de movimiento, especificado como objeto ackermannKinematics, bicycleKinematics, differentialDriveKinematics, unicycleKinematics o articulatedSteeringKinematics.

state — Estado actual del vehículo

vector de tres elementos | matriz de N por 3 | vector de cuatro elementos | matriz de N por 4

Estado actual del vehículo devuelto como un vector de tres o cuatro elementos, según la entrada de motionModel:

• unicycleKinematics –– [x y theta]

• bicycleKinematics –– [x y theta]

• differentialDriveKinematics –– [x y theta]

• ackermannKinematics –– [x y theta psi]

• articulatedSteeringKinematics –– [x y theta gamma]

x e y hacen referencia a la posición del vehículo, especificada en metros por segundo, y theta es el rumbo del vehículo, especificado en radianes para todos los modelos de movimiento de vehículos. psi es el ángulo de dirección del vehículo para el modelo de movimiento ackermannKinematics y gamma es el ángulo de articulación para el modelo de movimiento articulatedSteeringKinematics, ambos especificados en radianes. El ángulo de articulación hace referencia al ángulo de dirección del cuerpo delantero en relación con el cuerpo trasero para un modelo de vehículo con dirección articulada.

Puede especificar más de un estado especificando state como matriz de N por 3 o N por 4, dependiendo del modelo de movimiento. Cada fila corresponde a un estado. Observe que el número de estados debe ser 1 o un número igual al de comandos. Si especifica 1 estado, la derivada se calcula para el estado usando cada comando de cmds. Si especifica un mismo número de estados y comandos, la derivada se calcula para cada estado usando el comando correspondiente.

cmds — Comandos de entrada para el modelo de movimiento

vector de dos elementos | matriz de N por 2

Comandos de entrada para el modelo de movimiento, especificados como un vector de dos elementos o matriz de N por 2 que depende del modelo de movimiento. Cuando cmds es una matriz de N por 2, cada fila es un comando.

Para los objetos ackermannKinematics los comandos son [v psiDot].

Para los objetos articulatedSteeringKinematics los comandos son [v gammaDot].

Para otros modelos de movimiento, la propiedad VehicleInputs de motionModel determina el formato del comando:

• "VehicleSpeedSteeringAngle" –– [v psiDot]

• "VehicleSpeedHeadingRate" –– [v omegaDot]

• "WheelSpeedHeadingRate" (solo unicycleKinematics) –– [wheelSpeed omegaDot]

• "WheelSpeeds" (solo differentialDriveKinematics) –– [wheelL wheelR]

v es la velocidad del vehículo en la dirección del movimiento en metros por segundo. psiDot es la velocidad del ángulo de dirección en radianes por segundo. gammaDot es la velocidad del ángulo de articulación en radianes por segundo. omegaDot es la velocidad angular en el eje trasero en radianes por segundo. wheelL y wheelR son las velocidades de las ruedas izquierda y derecha en radianes por segundo, respectivamente.

Argumentos de salida

stateDot — Derivada de estado del estado actual

vector columna de tres elementos | Matriz de 3 por N | vector columna de cuatro elementos | matriz de 4 por N

Derivada del estado actual devuelta como un vector de tres o cuatro elementos, según la entrada de motionModel:

• unicycleKinematics –– [xDot yDot thetaDot]

• bicycleKinematics –– [xDot yDot thetaDot]

• differentialDriveKinematics –– [xDot yDot thetaDot]

• ackermannKinematics –– [xDot yDot thetaDot psiDot]

• articulatedSteeringKinematics –– [xDot yDot thetaDot gammaDot]

xDot e yDot hacen referencia a la velocidad del vehículo, especificada en metros por segundo. thetaDot es la velocidad angular del rumbo del vehículo, psiDot es la velocidad angular de la dirección del vehículo y gammaDot es la velocidad angular del cuerpo delantero en relación con el cuerpo trasero, todas ellas especificadas en radianes por segundo.

Si cmds se especifica como matriz de N por 2, stateDot es una matriz de 3 por N o de 4 por N, dependiendo del objeto del modelo de movimiento. N es el número de comandos especificados. Si el número de estados especificado en state es igual a N, cada columna de stateDot corresponde a su estado y comando respectivos. Por ejemplo, la primera columna es la derivada de estado del primer estado de state que usa el primer comando de cmds. Si state especifica un estado, cada columna de stateDot es la derivada de estado del comando correspondiente y el estado especificado. Esto significa que la derivada de estado se calcula para el estado único usando cada comando.

Referencias

[1] Lynch, Kevin M., and Frank C. Park. Modern Robotics: Mechanics, Planning, and Control. 1st ed. Cambridge, MA: Cambridge University Press, 2017.

Capacidades ampliadas

Generación de código C/C++
Genere código C y C++ mediante MATLAB® Coder™.

Historial de versiones

Introducido en R2019b

R2024b: Especificar múltiples comandos y estados a la vez

El argumento cmds de la función de objeto derivative ahora acepta múltiples estados y comandos a la vez.

Consulte también

ackermannKinematics | bicycleKinematics | differentialDriveKinematics | unicycleKinematics | articulatedSteeringKinematics

Temas

• Simular diferentes modelos cinemáticos para robots móviles
• Ecuaciones de cinemática para robots móviles