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

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

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

UAVs mission planning with flight level constraint using Fast Marching Square Method

In the last decade, Unmanned Aerial Vehicles (UAVs) have been a research focus for many purposes. Many of these studies require a path planning to perform autonomous flights, as well as the maintenance of a fixed flight level with respect to the ground to capture videos or overlying images. This article presents an approach to plan a mission for UAVs keeping a fixed flight level constraint. The 3D environment where the planning is carried out is an open field with non-uniform terrain. The approach proposed is based on the Fast Marching Square (FM ) method, which generates a path free from obstacles. Our approach includes two adjustment parameters. Depending on the values of these parameters, the restriction of flight level can be modified, as well as the smoothness and safety of the generated paths. Simulated experiments carried out in this work demonstrate that the proposed approach generates trajectories respecting a fixed flight level over the ground with successful results.The research leading to these results has received funding from the RoboCity2030-III-CM project (Robótica aplicada a la mejora de la calidad de vida de los ciudadanos, fase III; S2013/MIT-2748), funded by Programas de Actividades I+D en la Comunidad de Madrid and cofunded by Structural Funds of the EU.Publicad

Unmanned Aerial Vehicle (UAV) mission planning based on Fast Marching Square (FM²) planner and Differential Evolution (DE)

Nowadays, mission planning for Unmanned Aerial Vehicles (UAVs) is a very attractive research field. UAVs have been a research focus for many purposes. In military and civil fields, the UAVs are very used for different missions. Many of these studies require a path planning to perform autonomous flights. Several problems related to the physical limitations of the UAV arise when the planning is carried out, as well as the maintenance of a fixed flight level with respect to the ground to capture videos or overlying images. This work presents an approach to plan missions for UAVs keeping a fixed flight level constraint. An approach is proposed to solve these problems and to generate effective paths in terms of smoothness and safety distance in two different types of environments: 1) 3D urban environments and 2) open field with non-uniform terrain environments. Many proposed activities to be carried out by UAVs in whatever the environment require a control over the altitude for different purposes: energy saving and minimization of costs are some of these objectives. In general terms, the planning is required to avoid all obstacles encountered in the environment and to maintain a fixed flight level during the path execution. For this reason, a mission planning requires robust planning methods. The method used in this work as planner is the Fast Marching Square (FM2) method, which generates a path free of obstacles. As a novelty, the method proposed includes two adjustment parameters. Depending on the values of these parameters, the restriction of flight level can be modified, as well as the smoothness and safety margins from the obstacles of the generated paths. The Dubins airplane model is used to check if the path resulting from the FM2 is feasible according to the constraints of the UAV: its turning rate, climb rate and cruise speed. Besides, this research also presents a novel approach for missions of Coverage Path Planning (CPP) carried out by UAVs in 3D environments. These missions are focused on path planning to cover a certain area in an environment in order to carry out tracking, search or rescue tasks. The methodology followed uses an optimization process based on the Differential Evolution (DE) algorithm in combination with the FM2 planner. Finally, the UAVs formation problem is introduced and addressed in a first stage using the planner proposed in this thesis. A wide variety of simulated experiments have been carried out to illustrate the efficiency and robustness of the approaches presented, obtaining successful results in different urban and open field 3D environments.Hoy en día la planificación de misiones para vehículos aéreos no tripulados (UAV) es un campo de investigación muy atractivo. Los UAV son foco de investigación en numerosas aplicaciones, tanto en el campo civil como militar. Muchas de estas aplicaciones requieren de un sistema de planificación de ruta que permita realizar vuelos autónomos y afrontar problemas relacionados con las limitaciones físicas del UAV y con requerimientos como el nivel de vuelo sobre el suelo para, entre otras funciones, poder capturar videos o imágenes. Este trabajo presenta una propuesta de planificador para vehículos aéreos no tripulados que permite resolver los problemas citados previamente, incluyendo en la planificación las consideraciones cinemáticas del UAV y las restricciones de nivel de vuelo, generando rutas suaves, realizables y suficientemente seguras para dos tipos diferentes de entornos 3D: 1) entornos urbanos y 2) campos abiertos con terrenos no uniformes. El método utilizado en esta tesis como base para la planificación es el método Fast Marching Square (FM2), que genera un camino libre de obstáculos. Como novedad, el método propuesto incluye dos parámetros de ajuste. Dependiendo de los valores de estos parámetros, se puede modificar la restricción de nivel de vuelo, así como la suavidad y los márgenes de seguridad respecto a los obstáculos de las rutas generadas. El modelo cinemático de Dubins se utiliza para verificar si la ruta resultante de nuestro planificador es realizable de acuerdo con las restricciones del UAV: su velocidad de giro, velocidad de ascenso y velocidad de crucero. Además, esta tesis también presenta una propuesta novedosa para la planificación de misiones de Coverage Path Planning (CPP) en entornos 3D. Estas misiones se centran en la planificación de rutas para cubrir un área determinada de un entorno con el fin de llevar a cabo tareas de rastreo, búsqueda o rescate. La metodología seguida utiliza un proceso de optimización basado en el algoritmo Differential Evolution (DE) en combinación con nuestro planificador FM2. Como parte final de la tesis, el problema de formación de UAVs se introduce y aborda en una primera etapa utilizando el planificador FM2 propuesto.Programa Oficial de Doctorado en Ingeniería Eléctrica, Electrónica y AutomáticaPresidente: Antonio Giménez Fernández.- Secretario: Luis Santiago Garrido Bullón.- Vocal: Raúl Suárez Feijó

UAVs mission planning with imposition of flight level through fast marching square

Many proposed activities to be carried out by unmanned aerial vehicles (UAVs) in urban environments require a control over the altitude for different purposes. Energy saving and minimization of costs are some of these objectives. This work presents a method to impose a flight level in a mission planning carried out by a UAV in a 3D urban environment. The planning avoids all obstacles encountered in the environment and maintains a fixed flight level in the majority of the trajectory. The method used as planner is the Fast Marching Square (FM2) method, which includes two adjustment parameters. Depending on the values of these parameters, it is possible to introduce into the planning an altitude constraint, as well as to modify the smoothness of the trajectory and the safety margins from the obstacles. Several simulated experiments have been carried out in different situations obtaining very good results.The research leading to these results has received funding from the RoboCity2030-III-CM project (Robótica aplicada a la mejora de la calidad de vida de los ciudadanos, fase III; S2013/MIT-2748), funded by Programas de Actividades I+D en la Comunidad de Madrid and cofunded by Structural Funds of the EU

The path to efficiency: fast marching method for safer, more efficient mobile robot trajectories

This article provides a comprehensive view of the novel fast marching (FM) methods we developed for robot path planning. We recall some of the methods developed in recent years and present two improvements upon them: the saturated FM square (FM2) and an heuristic optimization called the FM2 star (FM2*) method. The saturated variation of the existing saturated FM2 provides safe paths that avoid unnecessarily long trajectories (like those computed using the Voronoi diagram). FM2* considerably reduces the computation time. As a result, these methods provide not only a trajectory but also an associated control speed for the robot at each point of the trajectory. The proposed methods are complete; if there is a valid trajectory, it will always be found and will always be optimal in estimated completion time.Comunidad de Madrid. S2009/DPI-1559/ROBOCITY2030 IIPublicad

The angle guidance path planning algorithms for unmanned surface vehicle formations by using the fast marching method

By deploying multiple USVs as a formation fleet, benefits such as wide mission area, improved system autonomy and increased fault-tolerant resilience can be achieved. To efficiently and effectively navigate the USV formation, path planning algorithms are required to generate optimal trajectories and provide practical collision avoidance manoeuvres. In particular, as the USV is underactuated and is restricted by various motion constraints, this paper has presented a new algorithm named the ‘angle-guidance fast marching square’ (AFMS), to make the generated path compliant with vehicle's dynamics and orientation restrictions. Based upon the AFMS, a formation path planning algorithm has been proposed to guide the USVs safely navigating in a cluttered environment. In addition, the formation forming problem has been specifically investigated with the algorithm being developed to make the USVs capable of forming the desired shape by following the trajectories from random initial configurations (positions and orientations). In order to eliminate the potential collision risks occurring on the route, a novel priority scheme based upon the distance to the closest point of approaching (DCPA) has also been proposed and developed. Algorithms have been validated on the computer-based simulations and are proven to work effectively in different environments

A Locking Sweeping Method Based Path Planning for Unmanned Surface Vehicles in Dynamic Maritime Environments

Unmanned surface vehicles (USVs) are new marine intelligent platforms that can autonomously operate in various ocean environments with intelligent decision-making capability. As one of key technologies enabling such a capability, path planning algorithms underpin the navigation and motion control of USVs by providing optimized navigational trajectories. To accommodate complex maritime environments that include various static/moving obstacles, it is important to develop a computational efficient path planning algorithm for USVs so that real-time operation can be effectively carried out. This paper therefore proposes a new algorithm based on the fast sweeping method, named the locking sweeping method (LSM). Compared with other conventional path planning algorithms, the proposed LSM has an improved computational efficiency and can be well applied in dynamic environments that have multiple moving obstacles. When generating an optimal collision-free path, moving obstacles are modelled with ship domains that are calculated based upon ships’ velocities. To evaluate the effectiveness of the algorithm, particularly the capacity in dealing with practical environments, three different sets of simulations were undertaken in environments built using electronic nautical charts (ENCs). Results show that the proposed algorithm can effectively cope with complex maritime traffic scenarios by generating smooth and safe trajectories