Main Content

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 una 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 tracción diferencial

diffDrive.WheelSpeedRange = [-10 10]*2*pi;

Modelo cinemático de bicicleta

El modelo de bicicleta trata el 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 Ackermann. En la mayoría de los vehículos similares al de 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.

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 ODE

Los modelos se simulan utilizando la función derivative para actualizar el estado. Este ejemplo utiliza un solver de ecuación diferencial ordinaria (ODE) 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 ODE 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 ODE 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)]);

A continuación, 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");
view(0,90)

Figure contains an axes object. The axes object contains 493 objects of type patch, line.