P3: Autoparking_A
En esta práctica tenemos un coche holonómico, el cual debe buscar hueco para aparcar y posteriormente aparcar.
Para esta labor tenemos acceso al GPS del robot, 3 sensores láser (frontal, lateral y trasero) y podemos controlarlo mandándole velocidades lineales y angulares.
Retos
- Encontrar un hueco en el lado derecho del robot, donde este quepa con cierta holgura haciendo uso de los láser.
- Aparcar en el hueco detectado.
Primera etapa: Buscar hueco
Mi primera aproximación fue avanzar con velocidad lineal constante y chequear que en cierto cono del láser derecho todos los valores eran mayores que el rango máximo del láser. Ese cono fue elegido mediante experimentación para que coincidiera con el largo del coche y un poco más. Con esto el coche se quedaba al lado del hueco, luego tenía que avanzar hacia delante 2metros.
Tras conseguir esta aproximación pensé, ¿por qué no aprovechar los 3 láser del robot en lugar de solo una parte del láser derecho?
Primera etapa: Buscar hueco V2
Para el desarrollo de esta segunda versión, lo primero que hice es recorrer los 180 valores de cada láser. Cada valor es la distancia o radio de una coordenada polar y además sabemos el ángulo, por lo que el primer paso es pasar de coordenadas polares a cartesianas:
El siguiente paso será pasar las coordenadas cartesianas calculadas a relativas al centro del robot, sabemos por experimentación que la mitad del largo del coche es 2.64 m y la mitad del ancho es 1.2 m. Por tanto habrá que girar y trasladar las coordenadas para conseguir las relativas al centro del coche (el ángulo y traslación dependerá de si es el láser frontal, lateral o trasero).
El último paso para saber si hemos encontrado hueco es crearse un rectángulo y ver si alguno de los puntos ya en relativas al robot cae dentro de nuestro rectángulo imaginario, si es así seguiremos buscando, en caso contrario ya habríamos encontrado hueco. En mi caso el rectángulo estará detrás del coche y a su derecha, esto hará que el coche avance hasta quedarse por delante del hueco, quedándose de esta manera en posición para empezar a aparcar.
Segunda etapa: Aparcar
En esta fase decidí prescindir del GPS para que mi coche pueda aparcar en un túnel o en un parking subterráneo. Mi solución consiste en usar la brújula del robot, es decir, el ángulo yaw.
El coche irá hacia atrás con velocidad lineal constante y girando también a velocidad constante hacia la izquierda hasta que yaw sea mayor o igual a 70º.
Una vez cumplida esta condición, seguirá avanzando hacia atrás a velocidad constante pero esta vez girando hacia la derecha hasta enderezar el coche (yaw menor o igual a 0º).
Cuando se cumple esta última condición se termina de aparcar, ponemos todas las velocidades, tanto angular como lineal a 0.
Resultado final
Mejoras añadidas
Para que el coche pueda comenzar un poco girado hacia la derecha o izquierda, no completamente paralelo a los coches de la derecha, se calcula la pendiente media que tiene la recta formada por los coches de la derecha, conforme a esta se gira a derecha o izquierda hasta que dicha pendiente sea 0. La primera pendiente se calcula antes de comenzar el bucle principal, por ello se precisa de launchear el código tras haber girado el coche en Gazebo.
Para que se pueda ver la robustez a la hora de aparcar gracias a haber usado el método del rectángulo imaginario para encontrar hueco, he grabado estos dos vídeos para que se pueda ver que el coche no aparca cogiendo como referencia ni el coche de delante ni el de detrás pudiendo así aparcar en cualquier situación.
Por último, para demostrar que a pesar de aparcar guiándome con la brújula es robusto y no influye que la calle sea Norte-Sur o Sur-Norte, gracias a que el ángulo de comprobación de giro no es fijo y varía en función de la orientación del coche una vez está paralelo a los coches de su derecha, hice la siguiente demo con coches aparcados en la izquierda y el coche circulando en el lado contrario de Norte a Sur:
