derivative
Derivada de tiempo del estado del vehículo
Descripción
stateDot = derivative(motionModel,state,cmds)stateDot, como un vector de tres elementos [xDot yDot thetaDot] si el modelo de movimiento es un objeto bicycleKinematics, differentialDriveKinematics o unicycleKinematics. Devuelve state 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 de la dirección del vehículo y psiDot es la velocidad angular de la dirección del vehículo, ambas especificadas en radianes por segundo.
Ejemplos
Simule un modelo de robot móvil que utilice la dirección de Ackermann con restricciones en su ángulo de giro. Durante la simulación, el modelo mantiene el ángulo de giro 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 giro y la otra sin ninguna restricción de dirección.
Definir el modelo
Defina el modelo cinemático de Ackermann. En este modelo similar al de un 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 giro. Al girar, las ruedas delanteras reciben la entrada de la dirección como velocidad de cambio del ángulo de giro.
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 giro. 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 giro máximo en 45 grados.
Para una descripción de cómo definir detectSteeringSaturation, consulte Definir una 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 giro 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
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
Argumentos de salida
Referencias
[1] Lynch, Kevin M., and Frank C. Park. Modern Robotics: Mechanics, Planning, and Control. 1st ed. Cambridge, MA: Cambridge University Press, 2017.
