Robot formation motion planning using Fast Marching

This paper presents the application of the Voronoi Fast Marching (VFM) method to path planning of mobile formation robots. The VFM method uses the propagation of a wave (Fast Marching) operating on the world model to determine a motion plan over a viscosity map (similar to the refraction index in optics) extracted from the updated map model. The computational efficiency of the method allows the planner to operate at high rate sensor frequencies. This method allows us to maintain good response time and smooth and safe planned trajectories. The navigation function can be classified as a type of potential field, but it has no local minima, it is complete (it finds the solution path if it exists) and it has a complexity of order n(O(n)), where n is the number of cells in the environment map. The results presented in this paper show how the proposed method behaves with mobile robot formations and generates trajectories of good quality without problems of local minima when the formation encounters non-convex obstacles.This work has been supported by the CAM Project S2009/DPI-1559/ROBOCITY2030 II, developed by the research team RoboticsLab at the University Carlos III of Madrid.Publicad

Fast Marching Methods in path and motion planning: improvements and high-level applications

Mención Internacional en el título de doctorPath planning is defined as the process to establish the sequence of states a system must go through in order to reach a desired state. Additionally, motion planning (or trajectory planning) aims to compute the sequence of motions (or actions) to take the system from one state to another. In robotics path planning can refer for instance to the waypoints a robot should follow through a maze or the sequence of points a robotic arm has to follow in order to grasp an object. Motion planning is considered a more general problem, since it includes kinodynamic constraints. As motion planning is a more complex problem, it is often solved in a two-level approach: path planning in the first level and then a control layer tries to drive the system along the specified path. However, it is hard to guarantee that the final trajectory will keep the initial characteristics. The objective of this work is to solve different path and motion planning problems under a common framework in order to facilitate the integration of the different algorithms that can be required during the nominal operation of a mobile robot. Also, other related areas such as motion learning are explored using this framework. In order to achieve this, a simple but powerful algorithm called Fast Marching will be used. Originally, it was proposed to solve optimal control problems. However, it has became very useful to other related problems such as path and motion planning. Since Fast Marching was initially proposed, many different alternative approaches have been proposed. Therefore, the first step is to formulate all these methods within a common framework and carry out an exhaustive comparison in order to give a final answer to: which algorithm is the best under which situations? This Thesis shows that the different versions of Fast Marching Methods become useful when applied to motion and path planning problems. Usually, high-level problems as motion learning or robot formation planning are solved with completely different algorithms, as the problem formulation are mixed. Under a common framework, task integration becomes much easier bringing robots closer to everyday applications. The Fast Marching Method has also inspired modern probabilistic methodologies, where computational cost is enormously improved at the cost of bounded, stochastic variations on the resulting paths and trajectories. This Thesis also explores these novel algorithms and their performance.Programa Oficial de Doctorado en Ingeniería Eléctrica, Electrónica y AutomáticaPresidente: Carlos Balaguer Bernaldo de Quirós.- Secretario: Antonio Giménez Fernández.- Vocal: Isabel Lobato de Faria Ribeir

Planning robot formations with fast marching square including uncertainty conditions

This paper presents a novel algorithm to solve the robot formation path planning problem working under uncertainty conditions such as errors the in robot's positions, errors when sensing obstacles or walls, etc. The proposed approach provides a solution based on a leader-followers architecture (real or virtual leaders) with a prescribed formation geometry that adapts dynamically to the environment. The algorithm described herein is able to provide safe, collision-free paths, avoiding obstacles and deforming the geometry of the formation when required by environmental conditions (e.g. narrow passages). To obtain a better approach to the problem of robot formation path planning the algorithm proposed includes uncertainties in obstacles' and robots' positions. The algorithm applies the Fast Marching Square (FM2) method to the path planning of mobile robot formations, which has been proved to work quickly and efficiently. The FM2 method is a path planning method with no local minima that provides smooth and safe trajectories to the robots creating a time function based on the properties of the propagation of the electromagnetic waves and depending on the environment conditions. This method allows to easily include the uncertainty reducing the computational cost significantly. The results presented here show that the proposed algorithm allows the formation to react to both static and dynamic obstacles with an easily changeable behavior.This work is included in the project number DPI2010-17772 funded by the Spanish Ministry of Science and Innovation and has been supported by the CAM Project S2009/DPI-1559/ROBOCITY2030 II, developed by the research team RoboticsLab at the University Carlos III of Madrid.Publicad

General Path Planning Methodology for Leader-Follower Robot Formations

This paper describes a robust algorithm for mobile robot formations based on the Voronoi Fast Marching path planning method. This is based on the propagation of a wave throughout the model of the environment, the wave expanding faster as the wave's distance from obstacles increases. This method provides smooth and safe trajectories and its computational efficiency allows us to maintain a good response time. The proposed method is based on a local-minima-free planner; it is complete and has an O(n) complexity order where n is the number of cells of the map. Simulation results show that the proposed algorithm generates good trajectories.Comunidad de Madri

Robot Formations Control Using Fast Marching

This paper presents the application of the Voronoi Fast Marching (V FM) method to the Control of Robot Formations. The V FM method uses the propagation of a wave (Fast Marching) operating on the world model to de- termine a motion plan over a viscosity map (similar to the refraction index in optics) extracted from the updated map model. The computational effciency of the method allows the planner to operate at high rate sensor frequencies. This method allows us to maintain good response time and smooth and safe planned trajectories. The navigation function can be classiffed as a type of potential field, but it has no local minima, it is complete (it finds the solu- tion path if it exists) and it has a complexity of order n (O(n)), where n is the number of cells in the environment map. The results presented in this paper show how the proposed method behaves with mobile robot formations and generates trajectories of good quality without problems of local minima when the formation encounters non-convex obstacles

3D robot formations path planning with fast marching square

This work presents a path planning algorithm for 3D robot formations based on the standard Fast Marching Square (FM2) path planning method. This method is enlarged in order to apply it to robot formations motion planning. The algorithm is based on a leader-followers scheme, which means that the reference pose for the follower robots is defined by geometric equations that place the goal pose of each follower as a function of the leader’s pose. Besides, the Frenet-Serret frame is used to control the orientation of the formation. The algorithm presented allows the formation to adapt its shape so that the obstacles are avoided. Additionally, an approach to model mobile obstacles in a 3D environment is described. This model modifies the information used by the FM2 algorithm in favour of the robots to be able to avoid obstacles. The shape deformation scheme allows to easily change the behaviour of the formation. Finally, simulations are performed in different scenarios and a quantitative analysis of the results has been carried out. The tests show that the proposed shape deformation method, in combination with the FM2 path planner, is robust enough to manage autonomous movements through an indoor 3D environment.Acknowledgments This work is funded by the project num ber DPI2010-17772, by the Spanish Ministry of Science and Innovation, and also by RoboCity2030-II-CM project (S2009/DPI-1559), funded by Programas de Actividades I+D en la Comunidad de Madrid and co-funded by Structural Funds of the EU.Publicad

Topology-Guided Path Integral Approach for Stochastic Optimal Control in Cluttered Environment

This paper addresses planning and control of robot motion under uncertainty that is formulated as a continuous-time, continuous-space stochastic optimal control problem, by developing a topology-guided path integral control method. The path integral control framework, which forms the backbone of the proposed method, re-writes the Hamilton-Jacobi-Bellman equation as a statistical inference problem; the resulting inference problem is solved by a sampling procedure that computes the distribution of controlled trajectories around the trajectory by the passive dynamics. For motion control of robots in a highly cluttered environment, however, this sampling can easily be trapped in a local minimum unless the sample size is very large, since the global optimality of local minima depends on the degree of uncertainty. Thus, a homology-embedded sampling-based planner that identifies many (potentially) local-minimum trajectories in different homology classes is developed to aid the sampling process. In combination with a receding-horizon fashion of the optimal control the proposed method produces a dynamically feasible and collision-free motion plans without being trapped in a local minimum. Numerical examples on a synthetic toy problem and on quadrotor control in a complex obstacle field demonstrate the validity of the proposed method.Comment: arXiv admin note: text overlap with arXiv:1510.0534

Planning and estimation algorithms for human-like grasping

Mención Internacional en el título de doctorThe use of robots in human-like environments requires them to be able to sense and model unstructured scenarios. Thus, their success will depend on their versatility for interacting with the surroundings. This interaction often includes manipulation of objects for accomplishing common daily tasks. Therefore, robots need to sense, understand, plan and perform; and this has to be a continuous loop. This thesis presents a framework which covers most of the phases encountered in a common manipulation pipeline. First, it is shown how to use the Fast Marching Squared algorithm and a leader-followers strategy to control a formation of robots, simplifying a high dimensional path-planning problem. This approach is evaluated with simulations in complex environments in which the formation control technique is applied. Results are evaluated in terms of distance to obstacles (safety) and the needed deformation. Then, a framework to perform the grasping action is presented. The necessary techniques for environment modelling and grasp synthesis and path planning and control are presented. For the motion planning part, the formation concept from the previous chapter is recycled. This technique is applied to the planning and control of the movement of a complex hand-arm system. Tests using robot Manfred show the possibilities of the framework when performing in real scenarios. Finally, under the assumption that the grasping actions may not always result as it was previously planned, a Bayesian-based state-estimation process is introduced to estimate the final in-hand object pose after a grasping action is done, based on the measurements of proprioceptive and tactile sensors. This approach is evaluated in real experiments with Reex Takktile hand. Results show good performance in general terms, while suggest the need of a vision system for a more precise outcome.La investigación en robótica avanza con la intención de evolucionar hacia el uso de los robots en entornos humanos. A día de hoy, su uso está prácticamente limitado a las fábricas, donde trabajan en entornos controlados realizando tareas repetitivas. Sin embargo, estos robots son incapaces de reaccionar antes los más mínimos cambios en el entorno o en la tarea a realizar. En el grupo de investigación del Roboticslab se ha construido un manipulador móvil, llamado Manfred, en el transcurso de los últimos 15 años. Su objetivo es conseguir realizar tareas de navegación y manipulación en entornos diseñados para seres humanos. Para las tareas de manipulación y agarre, se ha adquirido recientemente una mano robótica diseñada en la universidad de Gifu, Japón. Sin embargo, al comienzo de esta tesis, no se había realzado ningún trabajo destinado a la manipulación o el agarre de objetos. Por lo tanto, existe una motivación clara para investigar en este campo y ampliar las capacidades del robot, aspectos tratados en esta tesis. La primera parte de la tesis muestra la aplicación de un sistema de control de formaciones de robots en 3 dimensiones. El sistema explicado utiliza un esquema de tipo líder-seguidores, y se basa en la utilización del algoritmo Fast Marching Square para el cálculo de la trayectoria del líder. Después, mientras el líder recorre el camino, la formación se va adaptando al entorno para evitar la colisión de los robots con los obstáculos. El esquema de deformación presentado se basa en la información sobre el entorno previamente calculada con Fast Marching Square. El algoritmo es probado a través de distintas simulaciones en escenarios complejos. Los resultados son analizados estudiando principalmente dos características: cantidad de deformación necesaria y seguridad de los caminos de los robots. Aunque los resultados son satisfactorios en ambos aspectos, es deseable que en un futuro se realicen simulaciones más realistas y, finalmente, se implemente el sistema en robots reales. El siguiente capítulo nace de la misma idea, el control de formaciones de robots. Este concepto es usado para modelar el sistema brazo-mano del robot Manfred. Al igual que en el caso de una formación de robots, el sistema al completo incluye un número muy elevado de grados de libertad que dificulta la planificación de trayectorias. Sin embargo, la adaptación del esquema de control de formaciones para el brazo-mano robótico nos permite reducir la complejidad a la hora de hacer la planificación de trayectorias. Al igual que antes, el sistema se basa en el uso de Fast Marching Square. Además, se ha construido un esquema completo que permite modelar el entorno, calcular posibles posiciones para el agarre, y planificar los movimientos para realizarlo. Todo ello ha sido implementado en el robot Manfred, realizando pruebas de agarre con objetos reales. Los resultados muestran el potencial del uso de este esquema de control, dejando lugar para mejoras, fundamentalmente en el apartado de la modelización de objetos y en el cálculo y elección de los posibles agarres. A continuación, se trata de cerrar el lazo de control en el agarre de objetos. Una vez un sistema robótico ha realizado los movimientos necesarios para obtener un agarre estable, la posición final del objeto dentro de la mano resulta, en la mayoría de las ocasiones, distinta de la que se había planificado. Este hecho es debido a la acumulación de fallos en los sistemas de percepción y modelado del entorno, y los de planificación y ejecución de movimientos. Por ello, se propone un sistema Bayesiano basado en un filtro de partículas que, teniendo en cuenta la posición de la palma y los dedos de la mano, los datos de sensores táctiles y la forma del objeto, estima la posición del objeto dentro de la mano. El sistema parte de una posición inicial conocida, y empieza a ejecutarse después del primer contacto entre los dedos y el objeto, de manera que sea capaz de detectar los movimientos que se producen al realizar la fuerza necesaria para estabilizar el agarre. Los resultados muestran la validez del método. Sin embargo, también queda claro que, usando únicamente la información táctil y de posición, hay grados de libertad que no se pueden determinar, por lo que, para el futuro, resultaría aconsejable la combinación de este sistema con otro basado en visión. Finalmente se incluyen 2 anexos que profundizan en la implementación de la solución del algoritmo de Fast Marching y la presentación de los sistemas robóticos reales que se han usado en las distintas pruebas de la tesis.Programa Oficial de Doctorado en Ingeniería Eléctrica, Electrónica y AutomáticaPresidente: Carlos Balaguer Bernaldo de Quirós.- Secretario: Raúl Suárez Feijoo.- Vocal: Pedro U. Lim

Marine applications of the fast marching method

Path planning is general problem of mobile robots, which has special characteristics when applied to marine applications. In addition to avoid colliding with obstacles, in marine scenarios, environment conditions such as water currents or wind need to be taken into account in the path planning process. In this paper, several solutions based on the Fast Marching Method are proposed. The basic method focus on collision avoidance and optimal planning and, later on, using the same underlying method, the influence of marine currents in the optimal path planning is detailed. Finally, the application of these methods to consider marine robot formations is presented.The research leading to these results has received funding from HEROITEA-Sistema Inteligente Heterogéneo Multirobot para la Asistencia de Personas Mayores-RTI2018-095599-BC21 and from RoboCity2030-DIH-CM, Madrid Robotics Digital Innovation Hub, S2018/NMT-4331), funded by Programas de Actividades I+D en la Comunidad de Madrid and cofunded by Structural Funds of the EU