{ "cells": [ { "cell_type": "code", "execution_count": 1, "metadata": { "slideshow": { "slide_type": "skip" } }, "outputs": [], "source": [ "from IPython.display import IFrame" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Plataformas móviles\n", "\n", "_Entendiendo la bases detrás de las plataformas_" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# ¿Cómo mover una plataforma móvil? \n", "\n", "¿Qué necesitamos para llevar un robot de un punto $A$ a un punto $B$?\n", "\n", "![robot to goal](robot-to-goal.png)" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# ¿Cómo mover una plataforma móvil? \n", "\n", "![lazo cerrado del robot](closed-loop.png)" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Divide y Conquista\n", "\n", "- El mundo es dinámico y desconocido\n", "- El controlador debe responder a todas las condiciones ambientales\n", "- En vez de construir un complejo controlador, usemos la idea de divide y conquista.\n", "\n", "## División de los comportamientos\n", "\n", "- Ir a la meta\n", "- Evitar obstaculos\n", "- Seguir el muro\n", "- Sigue un objetivo\n", "- ..." ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Comportamiento" ] }, { "cell_type": "code", "execution_count": 3, "metadata": { "slideshow": { "slide_type": "-" } }, "outputs": [ { "data": { "text/html": [ "\n", " \n", " " ], "text/plain": [ "" ] }, "metadata": {}, "output_type": "display_data" } ], "source": [ "display(IFrame(\"https://www.youtube.com/embed/Ro7T3q14uDY\",width=\"100%\",height=\"400px\"))" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Plataformas diferenciales \n", "\n", "- Para controlar un sistema necesitamos su modelo.\n", "- Las plataformas móviles diferenciales son muy comunes. \n", "\n", "![plataforma diferencial](differential-robot.png)" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Modelo 1.0\n", "\n", "![modelo del robot](differential-robot-model.png)\n", "\n", "$$\\cases{ \\dot{x} = \\frac{R}{2}(v_r+v_l) \\cos(\\phi)\\cr\n", " \\dot{y} = \\frac{R}{2}(v_r+v_l) \\sin(\\phi)\\cr\n", " \\dot{\\phi} = \\frac{R}{L}(v_r-v_l)\n", " }\n", "$$" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Modelo uniciclo\n", "\n", "Como no es natural pensar en velocidades de las ruedas entonces vamos a ir directamente a la velocidad traslacional y rotacional. \n", "\n", "- Entradas \n", "\n", "$$v\\qquad w$$\n", "\n", "- Dinámica\n", "\n", "$$\\cases{ \\dot{x} = v \\cos(\\phi)\\cr\n", " \\dot{y} = v \\sin(\\phi)\\cr\n", " \\dot{\\phi} = w\n", " }\n", "$$" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Modelo 2.0\n", "\n", "- Se diseña para este modelo:\n", "\n", "$$\\cases{ \\dot{x} = v \\cos(\\phi)\\cr\n", " \\dot{y} = v \\sin(\\phi)\\cr\n", " \\dot{\\phi} = w\n", " }\n", "$$\n", "\n", "- Se implementa este modelo:\n", "\n", "\n", "$$\\cases{ \\dot{x} = \\frac{R}{2}(v_r+v_l) \\cos(\\phi)\\cr\n", " \\dot{y} = \\frac{R}{2}(v_r+v_l) \\sin(\\phi)\\cr\n", " \\dot{\\phi} = \\frac{R}{L}(v_r-v_l)\n", " }\n", "$$\n", "\n", "- en donde:\n", "\n", "$$v_r = \\frac{2v + w\\,L}{2R}\\qquad v_l = \\frac{2v - w\\,L}{2R}$$" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Odometria\n", "\n", "- ¿Cuál es el estado del robot?\n", "- ¿Cómo obtenemos la información del estado? " ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "fragment" } }, "source": [ "- Dos posibilidades\n", " - Sensores externos\n", " - Sensores internos:\n", " - Orientación: compas,...\n", " - Posición: acelerometro, giroscopo,\n", " - Encoder de las ruedas " ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Encoder de las ruedas\n", "\n", "- Los encoder dan la distancia (angular) recorrida por cada rueda\n", "- Asumamos que las rueda describen un arco, en una pequeña escala de tiempo\n", "\n", "\n", "\n", "$$\\cases{ x' = x + D_c \\cos(\\phi)\\cr\n", " y' = y + D_c \\sin(\\phi)\\cr\n", " \\phi' = \\phi + \\frac{D_r-D_l}{L}\n", " }\n", " \\qquad D_c = \\frac{D_r+D_l}{2}\n", "$$" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Encoder de las ruedas\n", "\n", "- ¿Cómo sabemos cuanto se ha movido cada rueda? " ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "fragment" } }, "source": [ "- Asumimos que cada rueda tiene $N$ pulsos por revolución\n", "- La mayoría de encoders dan un conteo de pulso desde el inicio. \n", "- Para cada rueda: \n", " - Calculamos la diferencia entre pulsos\n", " $$ \\Delta Pulso = Pulso' - Pulso $$\n", " - Y luego calculamos la distancia recorrida\n", " $$ D = 2\\pi \\, R \\frac{\\Delta Pulso}{N}$$\n", " \n", "**Usar Encoders genera un gran problema**" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Sensores\n", "\n", "Los robots necesitan saber a cerca de su mundo alrededor.\n", "\n", "Los sesores más utilizados con una \"falda\" sobre el robot:\n", "\n", "- IR\n", "- Ultrasonido\n", "- Lidar\n", "\n", "Otros sensores externos incluyen\n", "\n", "- Vision\n", "- Tacto\n", "- \"GPS\"" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Abstracción de disco\n", "\n", "En vez de preocuparnos por la resolución de los sensores, asumiremos que conocemos la distancia y dirección de todos los obtaculos al rededor de nosotros (que estén lo suficientemente cerca).\n", "\n", "" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "fragment" } }, "source": [ "Si conocemos nuestra propia pose (posición y orientación), entonces \n", "\n", "$$\\cases{\n", " x_1 = x + d_1 \\cos(\\phi_1+\\phi) \\\\\n", " y_1 = y + d_1 \\sin(\\phi_1+\\phi)\n", "}$$" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# ! Ejemplo : Rendezvous" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Behavior-Based Robotics\n", "\n", "_Robótica basada en comportamientos_\n", "\n", "\n", "\n", "El mundo cambia constantemente y es desconocido, por lo que no tiene sentido sobre-planear. **La idea clave:** Desarrollar un a biblioteca de controladores utiles (comportamientos). Cambiar entre los controladores en respuesta a los cambios ambientales. " ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Construyendo un comportamiento (controlador) \n", "\n", "Asumamos que tenemos un robot diferencial que se mueve a una velocidad translacional constante. \n", "\n", "\n", "$$\\cases{ \\dot{x} = v_0 \\cos(\\phi)\\cr\n", " \\dot{y} = v_0 \\sin(\\phi)\\cr\n", " \\dot{\\phi} = w\n", " }\n", "$$\n", "\n", "y queremos orientar el robot a una dirección deseada, ¿cuál debe ser el valor de $w = ?$ ?" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "fragment" } }, "source": [ "Tenemos una referencia, un modelo, una señal de control y un seguimiento del error: \n", "\n", "$$r=\\phi_d \\qquad e = \\phi_d - \\phi \\qquad \\dot{\\phi}=w$$\n", "\n", "Por que no usar un PID?\n", "\n", "$$ w = K_P \\, e + K_I \\int ed\\tau + K_D \\dot{e}$$" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "## Lidiando con el angulo\n", "\n", "Normalmente esto no funciona ya que estamos lidiando con angulos y entonces: \n", "\n", "$$\\phi_d =0 \\quad \\phi = 100\\pi \\qquad \\to \\qquad e = -100\\pi$$\n", "\n", "La solución es asegurarnos que $e \\in [-\\pi,\\pi]$\n", "\n", "Podemos usar el truco con **atan2**:\n", "\n", "$$e' = \\text{atan2}(\\sin(e),\\cos(e)) \\in [-\\pi,\\pi]$$" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Ejemplo de Navegación\n", "\n", "El problema de la navegación es ir al objetivo sin chocar con obstaculos. \n", "\n", "![robot to goal](robot-to-goal.png)\n", "\n", "Como mínimo necesitamos dos comportamientos:\n", "\n", "1. Ir al objetivo/meta\n", "2. Evitar obstaculos" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "## Comportamiento: Ir a la meta\n", "\n", "¿Cómo llevar el robot al objetivo? \n", "\n", "$$\\cases{ \\dot{x} = v_0 \\cos(\\phi)\\cr\n", " \\dot{y} = v_0 \\sin(\\phi)\\cr\n", " \\dot{\\phi} = w\n", " }\n", "\\qquad e = \\phi_d - \\phi\n", "\\qquad w = PID(e)\n", "$$\n", "\n", "\n", "\n", "$$\\phi_d = \\arctan\\left(\\frac{y_m-y}{x_m-x}\\right)$$\n", "\n", "- [Simulador para implementar el controlador](robot_model_without_controller.slx)\n", "- [Función para graficar el resultado](plot_xy.m)" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "### Primer intento\n", "\n", "$$w = K (\\phi_d - \\phi)$$\n", "\n", "![](controller-wrong-angles.gif)" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "fragment" } }, "source": [ "**¡Error en el cálculo de los angulos!**" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "### Segundo intento\n", "\n", "$$\"w = K (\\phi_d - \\phi)\"$$\n", "\n", "![](controller-low-gain.gif)" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "fragment" } }, "source": [ "**¡Una ganancia del controlador muy bajita!**" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "### Tercer intento\n", "\n", "$$\"w = K_{BIG} (\\phi_d - \\phi)\"$$\n", "\n", "![](controller-right.gif)" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "## Comportamiento: Evadir obstaculos\n", "\n", "¿Cómo evadir ir directamente a los obstaculos. \n", "\n", "Podemos usar la mismaidea para definir ir a la meta? " ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "fragment" } }, "source": [ "## Hacia donde ir\n", "\n", "$$\\phi_1= \\phi_{obst}+ \\pi \\qquad \\phi_2 = \\phi_{obst}\\pm \\pi/2 \\qquad \\phi_3 = \\phi_{meta}$$\n", "\n", "La dirección depende de la dirección al objetivo, no puede ser puro sino combinado. \n", "\n", "$$\\phi_4 = F(\\phi_{obst},\\phi_{meta})$$\n" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Mecanismo de decisión\n", "\n", "Este ejemplo ilustra 2 mecanismos de decisión diferentes:\n", "\n", "- El ganador toma todo o cambios fuertes.\n", "- Combinar comportamientos.\n", "\n", "Las dos situaciones tienen merito en diferentes situaciones:\n", "\n", "- Desempeño\n", "- Análisis\n", "\n", "Veremos con diseñar comportamientos sistematicos y mecanismos de decisión" ] } ], "metadata": { "celltoolbar": "Slideshow", "kernelspec": { "display_name": "Python 3 (ipykernel)", "language": "python", "name": "python3" }, "language_info": { "codemirror_mode": { "name": "ipython", "version": 3 }, "file_extension": ".py", "mimetype": "text/x-python", "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", "version": "3.9.8" } }, "nbformat": 4, "nbformat_minor": 2 }