INVE MEM 2012 130127

X Congreso Latinoamericano y del Caribe de Ingeniería Agrícola e
XLI Congresso Brasileiro de Engenharia Agrícola
CLIA/CONBEA 2012
Londrina - PR, Brasil, 15 a 19 de julho 2012
ALGORITMOS PARA LA PLANIFICACIÓN Y SEGUIMIENTO DE TRAYECTORIAS
EN ROBOTS AGRICOLAS
CHRISTIAN CORREA 1, LORENZO VÁSQUEZ 2
1
2
Ingeniero Civil Agrícola, M.Sc., Dr ©, Profesor Asistente, Facultad de Ing. Agrícola, Dpto. de Mecanización y Energía,
Universidad de Concepción, Chile, [email protected].
Ingeniero Civil Eléctrico, M.Sc. ©. Profesor Instructor. Universidad de Tarapacá, Chile. [email protected]
Apresentado no
X Congreso Latinoamericano y del Caribe de Ingeniería Agrícola - CLIA 2012
XL Congresso Brasileiro de Engenharia Agrícola - CONBEA 2012
15 a 19 de julho de 2012 - Londrina - PR, Brasil
RESUMEN: La base para realizar cualquier tarea agrícola mediante robots, es la planificación y seguimiento de
rutas o trayectorias. Así, el objetivo de esta investigación es desarrollar e implementar algoritmos de seguimiento y
planificación (global y local) de trayectorias de robots agrícolas. La planificación global se realizó mediante el
algoritmo A* aplicado sobre mapas de cultivo y la planificación local se realizó aplicando A* sobre un mapa 2D
obtenido a partir de imágenes 3D de los obstáculos encontrados en el camino. En cuanto el seguimiento de trayectorias,
esta se realizó implementando una aproximación numérica de la trayectoria mediante el método de Euler. Los
parámetros correspondientes a la dinámica del controlador de la trayectoria del robot fueron obtenidos mediante
algoritmos genéticos. El mapa 3D fue generado a partir del sensor Kinect de Microsoft y sus datos procesados usando
Matlab 2010b. Los resultados preliminares muestran que es posible implementar estos algoritmos en pequeños robots
diseñados para cultivos hilerados. Proveyendo así, una metodología robusta que permite seguir las rutas asignadas con
errores inferiores a RMSE=0.1m en trayectorias de 30m.
PALABRAS CLAVE: Planificación, trayectorias, robot móvil.
ALGORITHMS, FOR PATH FOLLOWING AND PLANNING, FOR AGRICULTURAL
ROBOTS.
ABSTRACT: The basis to perform any agricultural task by robots is to planning and following paths or trajectories
inside the crops. This research aims to developed and implemented algorithms for following and planning (global and
local) trajectories for agricultural robots. The global planning was performed using the A * algorithm applied over crop
maps and the local planning was performed using A* applied over a 2D map obtained from 3D images of the obstacles
present on the way. Trajectory following, was done by implementing a numerical approximation of the trajectory by
Euler's method. The parameters for the dynamics of the robot trajectory’s controller were obtained by genetic
algorithms. The 3D map was generated from the Microsoft Kinect sensor, and its data processed by Matlab 2010b.
Preliminary results show that these algorithms can be implemented in small robots designed to be used in crop rows.
Thereby providing a robust methodology to following assigned paths with errors less than RMSE= 0.1 m in trajectories
of 30 m.
KEYWORDS: Path Planning, tracking, mobile robot.
INTRODUCCION
Un robot móvil necesita tener un completo entendimiento del terreno y características
circundantes para estar en condiciones de navegar de forma segura en entornos complejos
(SOFTMAN et al., 2006). Razón por la cual se hace imprescindible dotarlo de una trayectoria
segura a seguir, así como de sensores que le permitan detectar y evadir los obstáculos que se le
presenten.
En esta investigación, se propone una serie de algoritmos que apuntan a resolver el problema
de eliminación de malezas en cultivos hilerados. En particular abordaremos la planificación de las
trayectorias a seguir, la generación de curvas que aproximen dichas trayectorias, la implementación
y optimización de un controlador de la trayectoria y finalmente la evasión de obstáculos presentes
en el camino.
MATERIALES Y MÉTODOS
La metodología la dividiremos en tres secciones: 1) Planificación de trayectoria con A*. 2)
Suavizado de la trayectoria mediante curvas de Bézier. 3) Modelo cinemático del robot. 4)
Algoritmo de seguimiento de trayectoria.
1) Planificación de trayectoria con A*.
El algoritmo A* propuesto por HART et al. (1968), es un algoritmo que busca la ruta más
corta (de menor costo) entre dos puntos. Es uno de los algoritmos de planificación más usados
(FLETCHER et al. 2008), debido a que de haber una solución, la encuentra y garantiza que es
óptima.
En el escenario propuesto, el robot debe ir desde su posición actual, hasta el punto con maleza
más cercano. La planificación de este recorrido la denominamos Planificación Global.
a) Planificación Global. Supóngase que el cultivo es dividido en una cuadrícula, y que el robot es
de tamaño menor o igual que una casilla de esta cuadrícula. El Algoritmo A* se encarga de
encontrar la ruta más corta, entre la posición actual y el punto de maleza más cercano (Figura
1a).
a)
b)
c)
FIGURA 1. a) Trayectorias generadas por el algoritmo A*. b) Detalle de la trayectoria global usada
para evitar un obstáculo conocido. c) Generación del offset.
En la Figura 1a, se puede observar en puntos de color rojo, la trayectoria que sigue el robot
desde su ubicación inicial al punto más cercano que contiene maleza. Luego toma ese punto
como punto de partida y genera una nueva trayectoria, marcada con puntos de color negro,
hasta el punto más cercano que contiene maleza. Este es un proceso cíclico en el que avanza
desde un punto con maleza hasta el siguiente.
b) Planificación Local. Para evitar los obstáculos inesperados presentes en la ruta, el robot extrae
información del mapa del entorno 3D, generada por el sensor Kinect de Microsoft (Figura 2a). El
mapa 3D es adquirido empleando el Microsoft Kinect Windows SDK. A modo de interface con
el modulo de planificación, se desarrolló una rutina en Matlab que se comunica con el SDK.
El proceso de extracción de información desde el mapa 3D, consiste en considerar que todo
objeto de más de 8 cm de altura, es un obstáculo a evitar. De este modo, cada casilla de la grilla
que contenga in objeto con valores de z > 8 cm, se considera una casilla ocupada (Figura 2d).
A estos obstáculos se les asigna una dimensión, en el plano xy, mayor a la real (la mitad del
ancho del robot), con el propósito de evitar colisiones. En otras palabras, se hace un offset del
obstáculo (Figura 2e).
Una vez que se tiene el mapa del entorno y los obstáculos en el, se genera una nueva trayectoria
desde la posición actual a la de destino. Esta vez con una grilla de mayor densidad, con el
propósito de generar suficientes puntos intermedios, que garanticen evitar la colisión con el
obstáculo (Figura 2e).
Cuando el robot se encuentra con objetos que no están en su mapa, cambia al modo evasión de
obstáculos. Este modo, genera un mapa de dos dimensiones en el plano xy.
a)
b)
c)
d)
e)
FIGURA 2. a) Robot y sensor Kinect. b) Vista plano xy. c) Síntesis del plano xy. d) Elimina objetos
menores a 8 cm y decide si pude o no seguir. e) Offset de objetos y nueva trayectoria.
2) Suavizado de la trayectoria mediante curvas de Bézier.
Una vez que se dispone de los puntos (generados por A*) por los que el robot debe pasar, estos
son unidos mediante curvas de Bézier implementadas mediante el algoritmo de Casteljau como se
describe en CHOI et al. (2008).
Las curvas de Bézier son paramétricas y diseñadas para unir suavemente un grupo de puntos.
Estas curvas pueden ser de diversos grados, dependiendo del nivel de suavizado que se desee.
El requerimiento mínimo en planificación de trayectorias, es pasar por el punto de inicio y por
el final con una dirección especifica. Así, el mínimo grado de la curvas de Bézier que satisface esta
condición es tres.
3) Modelo cinemático del robot
Se considera un robot móvil Pioneer 3DX, tipo uniciclo, detallado y validado
experimentalmente en De la CRUZ et al. (2006). Un esquema del robot se muestra en la Figura 3 y
se presenta su modelo cinemático en la Ecuación (1).
La posición del robot está definida por el punto h = [x,y], este punto está ubicado a una
distancia a desde el centro del eje trasero del robot B. G es el centro de gravedad, E es el punto de
ubicación del señor Kinect, C es el punto de ubicación de la rueda castor. ω, es la velocidad angular
del robot. u y u , corresponden a las velocidades longitudinal y transversal del centro de masa del
robot. ψ , corresponde al ángulo de orientación del robot.
a)
b)
FIGURA 3. a) Puntos y variables de interés sobre el robot. b) Aspecto real del robot.
(1)
La Ecuación (1) puede escribirse de manera compacta como se muestra en la Ecuación (2).
(2)
x(t) =[x y ψ u ω]T es el vector de estado del sistema, v(t)=[uc ⍵c]T es el vector de control y
δ , es el vector de perturbaciones o incertidumbre. En otras palabras la Ecuación (2) dice que la
variación del estado del robot, es función del estado actual más una función de las acciones de
control y de las perturbaciones que actúen sobre él.
Los parámetros δx y δy dependen del deslizamiento de las ruedas (patinaje) y a la orientación
del robot; δu y δω dependen de parámetros mecánicos del robot.
Los parámetros θ del robot móvil son: θ1 =0.2604 s; θ 2 =0.2509 s; θ 3 =-0.000499 s m rad -2;
θ 4 = 0.99065 s, θ 5 = 0.00263 s m-1; θ 6 = 1.0768, identificados en DE LA CRUZ, (2006).
4) Algoritmo de seguimiento de trayectoria mediante la aproximación de Euler.
Utilizando la metodología propuesta por RÓZALES et al.(2007), considérese la siguiente
ecuación diferencial:
y ' = f ( y, u, t ) con y(0) = y0
(3)
Donde, y es la salida del sistema a controlar, y’ la primera derivada, u la acción de control y t, el
tiempo.
Los valores de y(t) en el tiempo discreto t = kTo , serán llamados yk , donde: To representa el
período de muestreo, y k ∈{0,1, 2,3,…}.
Luego, si se desea calcular el valor de yk+1 conociendo el valor de yk, se deberá integrar la
Ecuación (4) sobre el intervalo de tiempo kTo ≤ t ≤ (k +1)To .
(4)
Existen muchos métodos de integración numérica, cada uno de ellos con sus correspondientes
algoritmos para realizar el cálculo de yk+1. Por ejemplo, puede hacerse una aproximación con la
Ecuación (5).
(5)
Donde el valor de yk+1 no se conoce, pero puede ser aproximado con la Ecuación (5). En el
área de métodos numéricos, esta aproximación de denomina método de Euler. Euler es un método
de primer orden que aproxima la solución por la recta tangente (MARTINS et al. 2009).
El uso de los métodos numéricos en el cálculo de la evolución de sistemas, se basa
principalmente, en la posibilidad de aproximar el estado del sistema en el instante de tiempo k +1,
si el estado y la acción de control en el instante de tiempo k son conocidas.
Entonces, el valor yk+1 de puede ser sustituido por la trayectoria deseada y luego de ello,
calcular las acciones de control necesarias para hacer que la salida del sistema, vaya de su valor
actual k, a su valor deseado.
a) Modelo del controlador.
Se discretiza el modelo dinámico del robot móvil mediante la aproximación de Euler
(Ecuación (3)). De esta forma el modelo dinámico discreto del robot móvil puede ser expresado
como se muestra en la Ecuación (6).
(6)
Que en términos de Euler pude escribirse como en la Ecuación (7)
(7)
Luego se sustituye el vector de estados:
Por el vector de estados deseados (posición, velocidad, orientación):
(8)
Si hacemos:
,
(9)
(10)
Podemos rescribir la Ecuación (6) como: Au=b. La Ecuación (6), es un sistema de 5
ecuaciones con 2 incógnitas y dado que A es una matriz orto normal, (A-1=AT) es posible encontrar
una solución optima para u mediante mínimos cuadrados (STRANG 1980, citado por RÓSALES et
al., 2009), solución mostrada en Ecuación (11).
(11)
Empleando una metodología similar a la utilizada para obtener la Ecuación (11), DE LA
CRUZ, (2006) propone que udk+1 y wdk+1 pueden ser calculados como mediante la Ecuación (12).
(12)
Donde:
b) Ajuste de los parámetros del controlador mediante algoritmos genéticos
Como se observa en la Ecuación (12), las velocidades del robot dependen de los parámetros ku
y kw, los que deben ser ajustados para el mejor seguimiento de la trayectoria. De este modo, si se
conoce la trayectoria a seguir, es posible determinar los parámetros ku y kw que hagan que el error
de seguimiento sea mínimo.
Ya a comienzos de los noventas, diversos autores demostraban (GILL et al. 1995 y
KRISTINSSON et al. 1992) las ventajas de usar algoritmos genéticos en la optimización de
controladores de trayectorias en robots.
De éste modo, se empleó el Global Optimization Toolbox de Matlab, en particular el Genetic
Algorithm solver. Dicha herramienta se empleó para minimizar la siguiente función objetivo:
Mínimo [ RMSE(Xc(t)- Xr(t))]
(13)
Donde: RMSE, es la raíz cuadrada del error cuadrático medio. Xc(t) es la trayectoria seguida
por el robot y Xr(t) es la trayectoria real o trayectoria deseada.
RESULTADOS Y DISCUSION
Se realizaron diversas simulaciones para diferentes trayectorias, con y sin obstáculos, y se
midió mediante el RMSE, el error cometido en cada una de ellas.
Primer Escenario. Presentado en la Figura 4, consiste en la búsqueda del camino más corto entre la
posición actual del robot (coordenada [1.5, 18.5]) y la maleza más cercana (coordenada [25.5,
13.5]) en un cultivo de 16 hileras, espaciadas a 2 m. El objetivo es alcanzado y la trayectoria de 30
m es generada en 162 ms (Procesador Intel ® Core 2 duo de 1.6 GHz).
FIGURA 4. Generación de la trayectoria entre las hileras del viñedo simulado (50x20m). Largo
trayectoria: 30 m. Entorno de simulación Matlab 2010b.
Segundo Escenario. Mostrado en la Figura 5, presenta un cultivo de 11 hileras, espaciadas a 3 m y
contiene 14 obstáculos (cuadrados de color azul). El robot parte de las coordenadas [2.5, 12.5] y
debe alcanzar la maleza de las coordenadas [42.5, 4.5]. En este caso la trayectoria de 84.4 m es
generada exitosamente en 238 ms.
FIGURA 5. Generación de la trayectoria entre las hileras del viñedo simulado (50x20m) con
múltiples obstáculos en la ruta. Largo trayectoria: 84,4 m.
Suavizado de trayectorias mediante curvas de Bézier. A modo de ejemplo en la Figura 6 se
pude apreciar que la trayectoria generada es suave y permite unir todos los puntos generados en el
Primer Escenario. Para ello necesita 340 ms.
P1
Pn
FIGURA 6. Generación de trayectoria suave mediante curvas de Bézier cúbicas.
Controlador de trayectoria. En la Figura 7a, se pude apreciar el desempeño del controlador
siguiendo la trayectoria, así como siguiendo las velocidades deseadas en el primer escenario usando
parámetros del controlador no optimizados. El error RMSE, es de 0.54 m para una trayectoria de 30
m. Además, puede observarse que la velocidad se mantiene oscilando entorno a los 0.4 m s-1, que es
mayor a los 0.2 m s-1 de velocidad impuesta como referencia.
Nótese, que el robot no alcanza su objetivo, quedando a 0.53 m de ésta. En la Figura 7b, se puede
observar la desviación de la velocidad deseada versus la generada por el controlador.
P1
Trayectoria a seguir
Trayectoria controlador
Pn
a)
b)
FIGURA 7. Resultados del seguimiento de la trayectoria con parámetros Kω y Ku no óptimos.
Por otra parte, al usar parámetros óptimos, como se observa en la Figura 8, el robot llega a su
destino con un RMSE=0.1 m, con velocidades de hasta 1 m s-1.
P1
Trayectoria a seguir
Trayectoria controlador
Pn
FIGURA 8. Resultados del seguimiento de la trayectoria con parámetros Kω y Ku óptimos.
Tercer Escenario. Para el caso de detectar un objeto inesperado en el camino, se añadieron en
forma aleatoria imágenes 3D de objetos previamente grabados en terreno, como se muestra en la
Figura 9.
Pk+1
Punto de la
trayectoria global
Pk
FIGURA 9. Trayectoria para evasión de un obstáculo local y suavizado mediante curvas cubicas de
Bézier. Offset del obstáculo.
De los resultados obtenidos en el todos los Escenarios, podemos decir que, la generación de
trayectorias resulta robusta y eficiente en términos de tiempo de computo. Además, para la evasión
local de obstáculos resulta una alternativa fiable ya que de haber espacio para que el robot pase, el
algoritmo A*, encontrara la ruta de menor costo. Pero sin embargo, esta ruta tiende a ser muy
rectilínea, es decir con aristas pronunciadas, por lo que se hace necesario reducir la velocidad del
robot a la mitad (0.1 m s-1) para que el controlador pueda seguir la trayectoria de forma ajustada y
evitar así las colisiones en espacios reducidos.
Por otra parte la evasión de obstáculos resulta exitosa, siempre que la altura del objeto sea
menor que la altura a la que se encuentra el sensor Kinect. Esto, debido a que si no ve su punto de
destino (punto intermedio de la trayectoria global Pk+1), el robot necesita generar una nueva
trayectoria al siguiente punto intermedio Pk+2, lo que no siempre resulta ser la trayectoria de menor
costo para evadir un obstáculo.
En cuanto al controlador, este resulta adecuado, ya que considera el modelo cinemático del
robot, pero dicha cinemática cambia si es que se le añade peso adicional, como por ejemplo un
pequeño estanque con herbicida. Por lo que los parámetros θ deben ser recalculados.
Adicionalmente, el desempeño del controlador está fuertemente ligado a los parámetros Kω y
Ku. Para nuestro análisis los valores seleccionados son los que ofrecen el menor RMSE posible. En
el Primer Escenario el RMSE es de apenas 0.1 m, lo que resulta altamente deseable en entornos
agrícolas, ya que no posee oscilaciones que le pudiesen provocar una colisión.
Finalmente, el mayor aporte de este trabajo, creemos estriba en la integración de diversas
técnicas de planificación, navegación, seguimiento de trayectorias, optimización y evasión de
obstáculos mediante visión 3D. Técnicas que permiten asegurar la navegación segura y efectiva, en
entornos agrícolas. Combinando técnicas clásicas como A*, con nuevas herramientas de visión
3D, como la que nos ofrece Kinect.
CONCLUSIONES
El marco de integración de algoritmos de seguimiento y planificación de trayectorias
propuesto, muestra bajos errores en el seguimiento de las trayectorias (RMSE=0.1 m en 30 m),
debido a la optimización de los parámetros del controlador, para cada trayectoria. Es decir el
controlador, genera la mejor solución para cada trayectoria.
Los parámetros del controlador obtenidos mediante algoritmos genéticos garantizan una
solución casi óptima, ya que evita caer en mínimos locales a diferencia de otros métodos de
optimización.
El ampliamente probado algoritmo de planificación A*, ha probado su eficacia en la
generación de trayectorias tanto locales como globales y su eficiencia en términos de tiempo de
computo (<300 ms, para la trayectoria más larga). Lo que lo hace adecuado para las necesidades de
los robots en entornos agrícolas.
La evasión local de obstáculos detectados por el sensor Kinect, resulta adecuado para la
detección de objetos en el camino del robot, aunque estos sean de pequeño tamaño. Pero se debe
tener en cuenta en su implementación en terreno, el efecto de la luz sobre el sensor de profundidad
y los efectos de la vibración en la determinación de las alturas de los objetos detectados.
REFERENCIAS
DE LA CRUZ C., “Control de Formación de Robots Móviles”, Tesis Doctoral, Instituto de
Automática, Universidad Nacional de San Juan, Argentina, San Juan, (2006).
FLETCHER, L.; TELLER, S.; OLSON, E.; MOORE, D.; KUWATA, Y.; LEONARD, J.;
MILLER, I.; CAMPBELL, M. The MIT–Cornell Collision and Why It Happened. 2008.
GILL, M. & ZOMAYA, A. Genetic algorithms for robot control Evolutionary Computation, 1995.,
IEEE International Conference on, , 1, 462.
HART, P. E.; NILSSON, N. J. & RAPHAEL, B. A Formal Basis for the Heuristic Determination of
minimum Cost Paths Systems Science and Cybernetics, IEEE Transactions on, Systems Science and
Cybernetics, IEEE Transactions on, Artificial Intelligence Group of the Applied Physics
Laboratory, Stanford Research Institute, Menlo Park, Calif., IEEE, 1968, 4, 100-107.
KRISHNA, K., AND G. A. DUMONT, "System identification and control using genetic
algorithms", IEEE Trans on System, Man and Cybernetics, vol. 22, no. 5, pp. 1033-1O46, 1992.
MARTINS, F.N. SARCINELLI-FILHO, M. BASTOS, T.F. CARELLI, R. Dynamic modeling and
adaptive dynamic compensation for unicycle-like mobile robots. Advanced Robotics, 2009. ICAR
2009. International Conference on. Junio 2009.
MATTHEW MCNAUGHTON, CHRISTOPHER R. BAKER, T. G. B. S. C. U. & ZIGLAR, J.
ROUFF, C. & HINCHEY, M. (Eds.) Experience from the DARPA Urban Challenge, SpringerVerlag London Limited, 2012.
ROSALES A., G SCAGLIA, V. MULT, “Tracking control of a mobile robot based on Taylor
Formula”. 2007. AJIEE. Jornadas en Ingeniería Eléctrica Y Electrónica.
ROSALES A., “Dynamic nonlinear model based predictive control for mobile robots”. En V
Seminario Euro Latinoamericano de Sistemas de Ingeniería. Nov. de 2009.
SCAGLIA G., MUT V., QUINTERO O., DI SCIASCIO F., “Trajectory Control Of Mobile Robots
Based On Numerical Methods”, XII Congreso Latinoamericano de Control Automático - CLCA,
Salvador, Brasil, (2006a).
SOFMAN, B.; BAGNELL, J. A.; STENTZ, A. & VANDAPEL, N. Terrain classification from
aerial data to support ground vehicle navigation Robotics, Citeseer, 2006