Cálculo de la posición de la parte móvil
La aplicación de control se ejecuta en un ordenador con MS-DOS, por ello no se requiere una máquina especialmente potente. En la actualidad utilizo un PC industrial tipo PC-104 con un Pentium a 133 MHz y 32 megabytes de RAM.
Está desarrollada con el compilador C++ de GNU para el entorno MS-DOS, djgpp, utilizando el sistema operativo de tiempo real POS386 que ofrece facilidades para crear aplicaciones concurrentes de tiempo real y permite comunicaciones TCP/IP.
Para comunicar con la unidad móvil se utiliza un puerto serie RS232 conectado al transceptor radio. El cambio de recepción a transmisión se hace con la línea de control RTS del puerto serie.
El nivel de enlace del protocolo radio se describe en la parte del dispositivo móvil. El nivel de aplicación consta de una serie de comandos que permiten monitorizar el estado de dispositivo móvil y modificar algunos parámetros.
El estado que se puede monitorizar es el siguiente:
También se puede monitorizar el estado de la CPU de sensores a través del interfaz I2C
El cuanto a los parámetros que se pueden modificar son:
Un resumen de los comandos disponibles y sus parámetros se encuentra en la tabla de comandos.
El software de la aplicación de control consta de 3 procesos principales que tienen las siguientes funciones:
Además se dispone de una interfaz con la parte gráfica que por un socket TCP/IP envía el estado del robot y permite recibir órdenes para realizar ciertos movimientos.
También es posible conectarse a la aplicación de control desde un cliente Telnet que abre una consola de terminal en la que se pueden dar órdenes al robot y visualizar algunos parámetros.
Cálculo de la posición de la parte móvil
El proceso de control utiliza un algoritmo para calcular la variación de la posición a partir de la distancia recorrida por cada una de las ruedas. Si la distancia que recorren estas es la misma el movimiento será en línea recta, pero si hay una diferencia de distancia la parte móvil sufrirá un giro y un desplazamiento.
Para calcular la nueva posición del robot, supondremos que las coordenadas iniciales (x, y) son las del punto central del eje que une el centro de las dos ruedas motrices, y que la orientación inicial es una ángulo α respecto al eje de las X.
Si ambas ruedas recorren la misma distancia la parte móvil no cambiará su dirección y se desplazará en línea recta, con lo que es sencillo calcular su nueva posición. Vamos a suponer para el caso más general que estas dos distancias son diferente, con lo que se producirá un giro.
Si d1 y d2 son las longitudes de los arcos descritos por las ruedas A y B, primero calcularemos el ángulo d y el radio de giro R que sufre el punto (x, y) como consecuencia de la diferencias entre estas dos longitudes (d2 - d1).
El ángulo d será el descrito tanto por d1 como por d2. Y este será, en radianes:
d = d1 / (R - E/2) = d2 / (R + E/2)
donde E es la longitud del eje que separa las dos ruedas motrices.
De la segunda igualdad podemos obtener el radio de giro:
R = E (d1 + d2) / 2(d2 - d1)
Siempre que d1 ≠ d2. En caso contrario el movimiento será en línea recta con radio de giro infinito y el cálculo de las nuevas coordenadas será trivial conociendo el ángulo de orientación a.
Por otro lado, si llamamos dm a la longitud del arco que describe el punto (x, y), que será el valor medio de d1 y d2 tendremos:
dm = (d1 + d2) / 2; d = dm / R
d= (d2 - d1) / E
Con lo que si la dirección original de la parte móvil es a, tras el movimiento pasará a ser a + d.
Y si las coordenadas antes del movimiento son (x1, y1), las nuevas coordenadas tras el movimiento se pueden calcular fácilmente utilizando el punto intermedio (x´, y´) como dos movimientos en línea recta consecutivos de la forma:
x´ = x1 + L cos(a); y´ = y1 + L sen(a)
x2 = x´ + L cos(a+d); y2 = y´ + L sen(a+d)
L = R tan(d/2)
Estos cálculos son válidos siempre que el radio de giro R sea constante. Si muestreamos las distancias recorridas por las ruedas con suficiente frecuencia y tenemos una buena precisión en la medida de las longitudes de los arcos podemos conseguir una buena aproximación de la evolución de la posición de la parte móvil del robot.
En nuestro caso el muestreo se realiza cada 200 mseg. y la distancia correspondiente a un pulso de odómetro es de 0,0135 mm. lo cual ofrece una precisión aceptable.
El software de control con sus algoritmos permiten realizar una serie de procesos de alto nivel que se pueden resumir en las siguientes órdenes:
Moverse a una determinada velocidad (hacia delante o hacia atrás) con un determinado radio de giro. En cambio de velocidad se hace respetando una aceleración o desaceleración máximo.
Avanzar o retroceder una distancia.
Girar un determinado ángulo sobre si mismo, bien con.el eje de giro en el punto central entre las dos ruedas o moviendo solo una de las ruedas.
Girar sobre si mismo apuntando en una dirección.
Desplazarse a unas coordenadas. Si se dan varias coordenadas se van recorriendo consecutivamente trazando una trayectoria. Por ahora si aparece un obstáculo el robot se detiene hasta que desaparece. En un futuro la idea es que se modifique la trayectoria para alcanzar las coordenadas.
El objetivo es seguir avanzando introduciendo funciones cada vez más elaboradas hasta conseguir un funcionamiento autónomo.