derivative
Descripción
devuelve la derivada del estado actual, 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
Simular diferentes modelos cinemáticos para robots móviles
Este ejemplo muestra cómo modelar diferentes modelos cinemáticos de robot en un entorno y compararlos.
Definir robots móviles con restricciones cinemáticas
Existen varias formas de modelar la cinemática de los robots móviles. Todas ellas determinan la relación que existe entre las velocidades de rueda y el estado del robot: [x y theta]
, como coordenadas xy y la dirección del robot, theta
, en radianes.
Modelo cinemático de monociclo
La forma más sencilla de representar la cinemática de vehículo de un robot móvil es con un modelo de monociclo, cuya velocidad de rueda está definida por una rotación sobre un eje central y puede girar en torno a su eje z. Tanto los modelos cinemáticos con tracción diferencial como los de bicicleta reducen la cinemática del monociclo cuando las entradas se proporcionan como la velocidad y la tasa de dirección del vehículo, y se ignoran otras restricciones.
unicycle = unicycleKinematics(VehicleInputs="VehicleSpeedHeadingRate");
Modelo cinemático con tracción diferencial
El modelo con tracción diferencial utiliza un eje motor posterior para controlar la velocidad y la tasa de dirección del vehículo. Las ruedas del eje motor pueden girar en ambas direcciones. Dado que la mayoría de los robots móviles tienen una interfaz con comandos de rueda de nivel bajo, este modelo volverá a usar la velocidad y la tasa de dirección del vehículo como entradas para simplificar el control del vehículo.
diffDrive = differentialDriveKinematics(VehicleInputs="VehicleSpeedHeadingRate");
Para diferenciar el comportamiento del modelo de monociclo, puede añadir una restricción de velocidad de la rueda al modelo cinemático con transmisión diferencial.
diffDrive.WheelSpeedRange = [-10 10]*2*pi;
Modelo cinemático de bicicleta
El modelo de bicicleta trata al robot como un modelo similar al de un automóvil con dos ejes: un eje motor trasero y un eje delantero que gira sobre el eje z. El modelo de bicicleta parte del supuesto de que las ruedas de cada eje se pueden modelar como una sola rueda centrada y que la dirección de la rueda delantera se puede ajustar directamente, como en una bicicleta.
bicycle = bicycleKinematics(VehicleInputs="VehicleSpeedHeadingRate",MaxSteeringAngle=pi/8);
Otros modelos
El modelo cinemático de Ackermann es un modelo similar al de un automóvil modificado que asume una dirección de Ackermann. En la mayoría de los vehículos similares a un automóvil, las ruedas delanteras no giran sobre el mismo eje, sino que giran en ejes ligeramente diferentes para asegurar que realizan círculos concéntricos alrededor del centro del giro del vehículo. Esta diferencia en el ángulo de giro se denomina dirección de Ackermann y, por lo general, se aplica a través de un mecanismo en vehículos reales. Desde el punto de vista de la cinemática del vehículo y las ruedas, se puede aplicar considerando el ángulo de giro como una entrada de guiñada. Esta simulación no incluye este modelo cinemático, pero para obtener más información sobre el modelo, consulte ackermannKinematics
.
carLike = ackermannKinematics;
Configurar los parámetros de la simulación
Estos robots móviles seguirán un conjunto de waypoints diseñado para mostrar algunas diferencias causadas por las distintas cinemáticas.
waypoints = [0 0; 0 10; 10 10; 5 10; 11 9; 4 -5]; % Define the total time and the sample rate sampleTime = 0.05; % Sample time [s] tVec = 0:sampleTime:20; % Time array initPose = [waypoints(1,:)'; 0]; % Initial pose (x y theta)
Crear un controlador del vehículo
Los vehículos siguen un conjunto de waypoints utilizando un controlador Pure Pursuit. A partir de un conjunto de waypoints, el estado actual del robot y algunos otros parámetros, el controlador genera la velocidad y la tasa de dirección del vehículo.
% Define a controller. Each robot requires its own controller
controller1 = controllerPurePursuit(Waypoints=waypoints,DesiredLinearVelocity=3,MaxAngularVelocity=3*pi);
controller2 = controllerPurePursuit(Waypoints=waypoints,DesiredLinearVelocity=3,MaxAngularVelocity=3*pi);
controller3 = controllerPurePursuit(Waypoints=waypoints,DesiredLinearVelocity=3,MaxAngularVelocity=3*pi);
Simular los modelos con un solver EDO
Los modelos se simulan utilizando la función derivative
para actualizar el estado. Este ejemplo utiliza un solver de ecuación diferencial ordinaria (EDO) para generar una solución. Otra posibilidad sería actualizar el estado mediante un bucle, como se muestra en Seguimiento de rutas para un robot de tracción diferencial.
Dado que el solver EDO requiere que todas las salidas se proporcionen como una salida única, el controlador Pure Pursuit debe estar envuelto en una función que genere la velocidad lineal y la velocidad angular de dirección como una misma salida. Para dicho efecto se usa un helper de ejemplo, exampleHelperMobileRobotController
. El helper de ejemplo asegura también que el robot se detenga cuando se encuentra dentro de un radio especificado del objetivo.
goalPoints = waypoints(end,:)'; goalRadius = 1;
ode45
se llama una vez por cada tipo de modelo. La función derivada calcula las salidas de estado, y el estado inicial viene definido por initPose
. Cada derivada acepta el objeto de modelo cinemático correspondiente, la pose actual del robot y la salida del controlador en dicha pose.
% Compute trajectories for each kinematic model under motion control
[tUnicycle,unicyclePose] = ode45(@(t,y)derivative(unicycle,y,exampleHelperMobileRobotController(controller1,y,goalPoints,goalRadius)),tVec,initPose);
[tBicycle,bicyclePose] = ode45(@(t,y)derivative(bicycle,y,exampleHelperMobileRobotController(controller2,y,goalPoints,goalRadius)),tVec,initPose);
[tDiffDrive,diffDrivePose] = ode45(@(t,y)derivative(diffDrive,y,exampleHelperMobileRobotController(controller3,y,goalPoints,goalRadius)),tVec,initPose);
Representar los resultados
Los resultados del solver EDO se pueden visualizar fácilmente en una sola gráfica utilizando plotTransforms
para visualizar los resultados de todas las trayectorias a la vez.
Las salidas de pose se deben convertir primero a matrices indexadas de traslaciones y cuaterniones.
unicycleTranslations = [unicyclePose(:,1:2) zeros(length(unicyclePose),1)]; unicycleRot = axang2quat([repmat([0 0 1],length(unicyclePose),1) unicyclePose(:,3)]); bicycleTranslations = [bicyclePose(:,1:2) zeros(length(bicyclePose),1)]; bicycleRot = axang2quat([repmat([0 0 1],length(bicyclePose),1) bicyclePose(:,3)]); diffDriveTranslations = [diffDrivePose(:,1:2) zeros(length(diffDrivePose),1)]; diffDriveRot = axang2quat([repmat([0 0 1],length(diffDrivePose),1) diffDrivePose(:,3)]);
Luego, se puede representar y visualizar en la parte superior el conjunto de todas las transformadas. Las rutas de los robots de monociclo, bicicleta y tracción diferencial se muestran en color rojo, azul y verde, respectivamente. Para simplificar la gráfica, muestre solo una de cada diez salidas.
figure plot(waypoints(:,1),waypoints(:,2),"kx-",MarkerSize=20); hold all plotTransforms(unicycleTranslations(1:10:end,:),unicycleRot(1:10:end,:),MeshFilePath="groundvehicle.stl",MeshColor="r"); plotTransforms(bicycleTranslations(1:10:end,:),bicycleRot(1:10:end,:),MeshFilePath="groundvehicle.stl",MeshColor="b"); plotTransforms(diffDriveTranslations(1:10:end,:),diffDriveRot(1:10:end,:),MeshFilePath="groundvehicle.stl",MeshColor="g"); axis equal view(0,90)
Simular el modelo cinemático de Ackermann con restricciones de ángulo de giro
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 mediante plot
y los datos almacenados en pose.
figure(1) plot(pose1(:,1),pose1(:,2),'--r','LineWidth',2); hold on; plot([pose2(:,1); pose3(:,1)],[pose2(:,2);pose3(:,2)],'g'); 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
Argumentos de entrada
motionModel
— Objeto de modelo cinemático móvil
objeto ackermannKinematics
| objeto bicycleKinematics
| objeto differentialDriveKinematics
| objeto unicycleKinematics
Objeto de modelo cinemático móvil, que define las propiedades del modelo de movimiento, especificado como un objeto ackermannKinematics
, bicycleKinematics
, differentialDriveKinematics
o unicycleKinematics
.
state
— Estado actual del vehículo
vector de tres elementos | vector de cuatro elementos
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]
x e y hacen referencia a la posición del vehículo, especificada en metros por segundo. theta es la dirección del vehículo y psi es el ángulo de giro del vehículo, ambos especificados en radianes por segundo.
cmds
— Comandos de entrada para el modelo de movimiento
vector de dos elementos
Comandos de entrada para el modelo de movimiento, especificados como un vector de dos elementos que depende del modelo de movimiento.
Para los objetos ackermannKinematics
los comandos son [v psiDot]
.
Para otros modelos de movimiento, la propiedad VehicleInputs
de motionModel
determina el vector de comando:
"VehicleSpeedSteeringAngle"
––[v psiDot]
"VehicleSpeedHeadingRate"
––[v omegaDot]
"WheelSpeedHeadingRate"
(solounicycleKinematics
) ––[wheelSpeed omegaDot]
"WheelSpeeds"
(solodifferentialDriveKinematics
) ––[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 giro en radianes por segundo. omegaDot es la velocidad angular en el eje trasero. wwheelL y wheelR son las velocidades de las ruedas izquierda y derecha respectivamente.
Argumentos de salida
stateDot
— Derivada de estado del estado actual
vector de tres elementos | vector de cuatro elementos
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]
xDot y 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.
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
Comando de MATLAB
Ha hecho clic en un enlace que corresponde a este comando de MATLAB:
Ejecute el comando introduciéndolo en la ventana de comandos de MATLAB. Los navegadores web no admiten comandos de MATLAB.
Select a Web Site
Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .
You can also select a web site from the following list:
How to Get Best Site Performance
Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)