You are on page 1of 5

Robótica Móvil

Modelado y Control Cinemático
con el RP5
Descripción de la Práctica
El objetivo de la práctica es la utilización e implementación de algoritmos de modelado
y control cinemático con el robot RP5. Se pretende que por un lado se utilice un
modelo cinemático propio, para estimar la posición del robot, además de implementar
un control cinemático para el seguimiento de trayectorias y determinar las velocidades
de rueda a aplicar al robot. Además, utilizando los sensores de infrarrojos se podrá
hacer que el robot se mueva entre dos paredes de forma que siempre se sitúe en el
medio.

Modelado Cinemático
Tal y como se ha descrito en la teoría, el modelado cinemático permite obtener la posición del
robot a partir de las velocidades de rueda. Se ha implementado una función
“KinematicModel” que calcula la posición a partir de las velocidades de rueda.

Internamente la función implementa las siguientes ecuaciones:

Donde, , y es un parámetro que de ha determinado
experimentalmente para corregir los errores de deslizamiento causados por la configuración
oruga del robot RP5 al realizar giros. En concreto, y de cara a implementar el control
cinemático utilizaremos

¿Cómo utilizarlo?
La función “KinematicModel” devuelve una estructura del tipo “Pose”:

Pose pose;
pose=KinematicModel();

En el código anterior se está instanciando la variable “pose” que es del tipo “Pose” para
almacenar los valores devueltos por la función “KinematicModel”. El tipo de datos “Pose”
contiene la posición X, Y y la orientación del robot (ojo: las variables internas son del tipo
“float” y están expresadas en milímetros para X e Y y en radianes para la orientación):

typedef struct {
float x; //in millimeters
float y; //in millimeters
Robótica Móvil
float orientation; //in radians
}Pose;

El código anterior se incluye sólo para que sepáis cómo está definido el tipo “Pose”, pero no
tenéis que incluirlo en vuestro programa. Por tanto, si queremos acceder a la posición X,
debemos utilizar “pose.x” o de igual forma deberíamos utilizar “pose.y” o
“pose.orientation”.

Ejercicio 1
Implementar un sencillo programa que mueva el robot hacia delante y se pare una vez se haya
movido en X un metro (1000 milímetros). Tened en cuenta que las variables que hay en “Pose”
son del tipo “float” para introducir “1000.0” en vez de “1000” en la comparación.

Ejercicio 2
Implementar un sencillo programa que gire el robot hacia la izquierda o derecha y detenga el
movimiento cuando se haya movido 90º (pi/2 radianes).

Seguimiento de Trayectorias
Se aportan un conjunto de funciones para generar diferentes trayectorias para que el robot las
siga, tales como: “SetInfiniteTrajectory” y “SetCircularTrajectory” que deben de
ser llamadas una vez al arrancar el robot (sólo una de ellas). Además se dispone de una función
“GetNextTrajectoryPoint” que permite obtener cuál es el siguiente punto a seguir de la
trayectoria.

La función “SetInfiniteTrajectory” permite generar una trayectoria con forma del
infinito y se define como:

void SetInfiniteTrajectory(float xCenter, float yCenter, float axisX,
float axisY, float omega, float T);

Mientras que la función “SetCircularTrajectory” se define como:

void SetCircularTrajectory(float xCenter, float yCenter, float axisX,
float axisY, float omega, float T);

Donde xCenter e yCenter son las coordenadas del centro de la trayectoria. xAxis y yAxis
son las longitudes de los ejes (la mitad de los ejes), omega es la velocidad angular con la que se
recorrerá la trayectoria y sirve para determinar la velocidad del vehículo y T es el periodo de
muestreo. Como resultado de llamar a esta función, se generará una trayectoria en forma de
infinito cuya longitud (en número de puntos) es y por tanto es el tiempo que
tardará el robot en recorrer la trayectoria de forma completa. En general funcionan bien los
ejercicios si escogemos un valor de . Siendo el periodo de muestreo, ya
especificado, de . El centro del robot normalmente será el (0,0), mientras que
podemos especificar cualquier tamaño de los ejes que queramos (dentro de las posibilidades
físicas del robot).

La función “GetNextTrajectoryPoint” se define como:
Robótica Móvil
TrajectoryPoint GetNextTrajectoryPoint(void);

Que devuelve un “TrajectoryPoint”, una estructura con el siguiente punto de la
trayectoria de referencia con su posición y su velocidad Cartesiana a seguir:

typedef struct {
float x;
float y;
float velX;
float velY;
}TrajectoryPoint;

¿Cómo utilizarlo?
En la subrutina inicial debemos escribir (entre otras tareas de inicialización) aportando los
parámetros adecuados:

SetInfiniteTrajectory(…);

En la subrutina periódica:
TrajectoryPoint trajectoryPoint;
trajectoryPoint=GetNextTrajectoryPoint();

Por lo tanto, podemos acceder a los valores de referencia de la trayectoria a partir de
“trajectoryPoint.x”, “trajectoryPoint.y”, “trajectoryPoint.velX” y
“trajectoryPoint.velY”.

Así mismo se ha generado una función “SetMotion” que permite que le pasemos una variable
del tipo “Motion” con las velocidades de la rueda izquierda y derecha, esto es:

void SetMotion(Motion motion);

typedef struct {
float left;
float right;
}Motion;

Por tanto las siguientes instrucciones establecen un velocidad de ruedas según el valor de
vLeft y vRight (supuestamente previamente definidas):

Motion motion;
motion.left=vLeft;
motion.right=vRight;
SetMotion(motion);

Ejercicio 3
Se pide implementar un control cinemático por punto descentrado en base al siguiente
conjunto de ecuaciones:
Robótica Móvil

Siendo , , , los valores de referencia de la trayectoria en cada instante de
tiempo, esto es, “trajectoryPoint.x”, “trajectoryPoint.y”,
“trajectoryPoint.velX” y “trajectoryPoint.velY”. y son parámetros del
controlador cinemático, así como que es el parámetro que representa la distancia del punto
de control, p.e.: .

Tened en cuenta que todas las variables con las que debéis trabajar son del tipo “float”.

Para ahorrar cálculos y por tanto tiempo, es conveniente almacenar en variables temporales el
resultado de hacer el coseno o el seno de la orientación del robot.

Probar tanto para el caso de una trayectoria en forma de infinito como para el caso de una
trayectoria circular.

Ejercicio 4
Implementar ahora vuestra propia trayectoria de forma que el robot siga una línea recta a una
velocidad constante y que esté separada a una cierta distancia de la posición inicial del robot.
Deberéis implementar el siguiente conjunto de ecuaciones:

,

Siendo el periodo de muestreo.

Lateral Tracking
El objetivo ahora es mantenerse a la misma distancia de las paredes que tiene el robot a sus
lados. Para generar un “camino” podéis ayudaros de los bloques de madera que están en la
mesa de pruebas del propio laboratorio, dándole forma de pared.

La siguiente figura representa el problema planteado:
Robótica Móvil

En los robots las distancias , , y deberían corresponder a los sensores #0, #1, #2 y
#3 respectivamente y que deberéis de revisar.

Con todo ello, se estima una línea “intermedia” como resultado de calcular las líneas
correspondientes a cada pared conforme al siguiente conjunto de ecuaciones:

Siendo la distancia entre los sensores, y el ángulo y distancia de la línea
intermedia, respectivamente.

Ejercicio 5
Se pide implementar el algoritmo de “Persecución Pura” para que el robot converja a la línea
intermedia y por tanto permanezca lo más equidistante posible a las paredes. Por tanto
deberéis aplicar el siguiente conjunto de ecuaciones:

Donde es un parámetro de controlador, donde para reducir tiempo de cómputo deberéis
proporcionar directamente con el fin de evitar una multiplicación innecesaria. De igual
manera, deberéis de proporcionar el valor ya calculado para evitar una división innecesaria.