HRA*: hybrid randomized path planning for complex 3D environments

We propose HRA*, a new randomized path planner for complex 3D environments. The method is a modified A* algorithm that uses a hybrid node expansion technique that combines a random exploration of the action space meeting vehicle kinematic constraints with a cost to goal metric that considers only kinematically feasible paths to the goal. The method includes also a series of heuristics to accelerate the search time. These include a cost penalty near obstacles, and a filter to prevent revisiting configurations. The performance of the method is compared against A*, RRT and RRT* in a series of challenging 3D outdoor datasets. HRA* is shown to outperform all of them in computation time, and delivering shorter paths than A* and RRPostprint (author's final draft

HRA*: Hybrid randomized path planning for complex 3D environments

Trabajo presentado al IROS celebrado en Tokyo del 3 al 7 de noviembre de 2013.We propose HRA*, a new randomized path planner for complex 3D environments. The method is a modified A* algorithm that uses a hybrid node expansion technique that combines a random exploration of the action space meeting vehicle kinematic constraints with a cost to goal metric that considers only kinematically feasible paths to the goal. The method includes also a series of heuristics to accelerate the search time. These include a cost penalty near obstacles, and a filter to prevent revisiting configurations. The performance of the method is compared against A*, RRT and RRT* in a series of challenging 3D outdoor datasets. HRA* is shown to outperform all of them in computation time, and delivering shorter paths than A* and RRT.This work has been partially supported by the Mexican Council of Science and Technology with a PhD Scholarship to Ernesto Teniente, by the Spanish Ministry of Science and Innovation under project DPI-2011-27510 and by the EU project ARCAS FP7-287617.Peer Reviewe

Probabilistic motion planning for non-Euclidean and multi-vehicle problems

Trajectory planning tasks for non-holonomic or collaborative systems are naturally modeled by state spaces with non-Euclidean metrics. However, existing proofs of convergence for sample-based motion planners only consider the setting of Euclidean state spaces. We resolve this issue by formulating a flexible framework and set of assumptions for which the widely-used PRM*, RRT, and RRT* algorithms remain asymptotically optimal in the non-Euclidean setting. The framework is compatible with collaborative trajectory planning: given a fleet of robotic systems that individually satisfy our assumptions, we show that the corresponding collaborative system again satisfies the assumptions and therefore has guaranteed convergence for the trajectory-finding methods. Our joint state space construction builds in a coupling parameter $1\leq p\leq \infty$, which interpolates between a preference for minimizing total energy at one extreme and a preference for minimizing the travel time at the opposite extreme. We illustrate our theory with trajectory planning for simple coupled systems, fleets of Reeds-Shepp vehicles, and a highly non-Euclidean fractal space.Comment: 12 pages, 8 figures. Substantial revision

A Hamilton-Jacobi Formulation for Time-Optimal Paths of Rectangular Nonholonomic Vehicles

We address the problem of optimal path planning for a simple nonholonomic vehicle in the presence of obstacles. Most current approaches are either split hierarchically into global path planning and local collision avoidance, or neglect some of the ambient geometry by assuming the car is a point mass. We present a Hamilton-Jacobi formulation of the problem that resolves time-optimal paths and considers the geometry of the vehicle

Motion Planning in Artificial and Natural Vector Fields

This dissertation advances the field of autonomous vehicle motion planning in various challenging environments, ranging from flows and planetary atmospheres to cluttered real-world scenarios. By addressing the challenge of navigating environmental flows, this work introduces the Flow-Aware Fast Marching Tree algorithm (FlowFMT*). This algorithm optimizes motion planning for unmanned vehicles, such as UAVs and AUVs, navigating in tridimensional static flows. By considering reachability constraints caused by vehicle and flow dynamics, flow-aware neighborhood sets are found and used to reduce the number of calls to the cost function. The method computes feasible and optimal trajectories from start to goal in challenging environments that may contain obstacles or prohibited regions (e.g., no-fly zones). The method is extended to generate a vector field-based policy that optimally guides the vehicle to a given goal. Numerical comparisons with state-of-the-art control solvers demonstrate the method\u27s simplicity and accuracy. In this dissertation, the proposed sampling-based approach is used to compute trajectories for an autonomous semi-buoyant solar-powered airship in the challenging Venusian atmosphere, which is characterized by super-rotation winds. A cost function that incorporates the energetic balance of the airship is proposed to find energy-efficient trajectories. This cost function combines the main forces acting on the vehicle: weight, buoyancy, aerodynamic lift and drag, and thrust. The FlowFMT* method is also extended to consider the possibility of battery depletion due to thrust or battery charging due to solar energy and tested in this Venus atmosphere scenario. Simulations showcase how the airship selects high-altitude paths to minimize energy consumption and maximize battery recharge. They also show the airship sinking down and drifting with the wind at the altitudes where it is fully buoyant. For terrestrial applications, this dissertation finally introduces the Sensor-Space Lattice (SSLAT) motion planner, a real-time obstacle avoidance algorithm for autonomous vehicles and mobile robots equipped with planar range finders. This planner uses a lattice to tessellate the area covered by the sensor and to rapidly compute collision-free paths in the robot surroundings by optimizing a cost function. The cost function guides the vehicle to follow an artificial vector field that encodes the desired vehicle path. This planner is evaluated in challenging, cluttered static environments, such as warehouses and forests, and in the presence of moving obstacles, both in simulations and real experiments. Our results show that our algorithm performs collision checking and path planning faster than baseline methods. Since the method can have sequential or parallel implementations, we also compare the two versions of SSLAT and show that the run-time for its parallel implementation, which is independent of the number and shape of the obstacles found in the environment, provides a significant speedup due to the independent collision checks

On Randomized Path Coverage of Configuration Spaces

We present a sampling-based algorithm that generates a set of locally-optimal paths that differ in visibility

Efficient Motion and Inspection Planning for Medical Robots with Theoretical Guarantees

Medical robots enable faster and safer patient care. Continuum medical robots (e.g., steerable needles) have great potential to accomplish procedures with less damage to patients compared to conventional instruments (e.g., reducing puncturing and cutting of tissues). Due to their complexity and degrees of freedom, such robots are often harder and less intuitive for physicians to operate directly. Automating robot-assisted medical procedures can enable physicians and patients to harness the full potential of medical robots in terms of safety, efficiency, accuracy, and precision.Motion planning methods compute motions for a robot that satisfy various constraints and accomplish a specific task, e.g., plan motions for a mobile robot to move to a target spot while avoiding obstacles. Inspection planning is the task of planning motions for a robot to inspect a set of points of interest, and it has applications in domains such as industrial, field, and medical robotics. With motion and inspection planning, medical robots would be able to automatically accomplish tasks like biopsy and endoscopy while minimizing safety risks and damage to the patient. Computing a motion or inspection plan can be computationally hard since we have to consider application-specific constraints, which come from the robotic system due to the mechanical properties of the robot or come from the environment, such as the requirement to avoid critical anatomical structures during the procedure.I develop motion and inspection planning algorithms that focus on efficiency and effectiveness. Given the same computing power, higher efficiency would shorten the procedure time, thus reducing costs and improving patient outcomes. Additionally, for the automation of medical procedures to be clinically accepted, it is critical from a patient care, safety, and regulatory perspective to certify the correctness and effectiveness of the algorithms involved in procedure automation. Therefore, I focus on providing theoretical guarantees to certify the performance of planners. More specifically, it is important to certify if a planner is able to find a plan if one exists (i.e., completeness) and if a planner is able to find a globally optimal plan according to a given metric (i.e., optimality).Doctor of Philosoph

3D mapping and path planning from range data

This thesis reports research on mapping, terrain classification and path planning. These are classical problems in robotics, typically studied independently, and here we link such problems by framing them within a common proprioceptive modality, that of three-dimensional laser range scanning. The ultimate goal is to deliver navigation paths for challenging mobile robotics scenarios. For this reason we also deliver safe traversable regions from a previously computed globally consistent map. We first examine the problem of registering dense point clouds acquired at different instances in time. We contribute with a novel range registration mechanism for pairs of 3D range scans using point-to-point and point-to-line correspondences in a hierarchical correspondence search strategy. For the minimization we adopt a metric that takes into account not only the distance between corresponding points, but also the orientation of their relative reference frames. We also propose FaMSA, a fast technique for multi-scan point cloud alignment that takes advantage of the asserted point correspondences during sequential scan matching, using the point match history to speed up the computation of new scan matches. To properly propagate the model of the sensor noise and the scan matching, we employ first order error propagation, and to correct the error accumulation from local data alignment, we consider the probabilistic alignment of 3D point clouds using a delayed-state Extended Information Filter (EIF). In this thesis we adapt the Pose SLAM algorithm to the case of 3D range mapping, Pose SLAM is the variant of SLAM where only the robot trajectory is estimated and where sensor data is solely used to produce relative constraints between robot poses. These dense mapping techniques are tested in several scenarios acquired with our 3D sensors, producing impressively rich 3D environment models. The computed maps are then processed to identify traversable regions and to plan navigation sequences. In this thesis we present a pair of methods to attain high-level off-line classification of traversable areas, in which training data is acquired automatically from navigation sequences. Traversable features came from the robot footprint samples during manual robot motion, allowing us to capture terrain constrains not easy to model. Using only some of the traversed areas as positive training samples, our algorithms are tested in real scenarios to find the rest of the traversable terrain, and are compared with a naive parametric and some variants of the Support Vector Machine. Later, we contribute with a path planner that guarantees reachability at a desired robot pose with significantly lower computation time than competing alternatives. To search for the best path, our planner incrementally builds a tree using the A* algorithm, it includes a hybrid cost policy to efficiently expand the search tree, combining random sampling from the continuous space of kinematically feasible motion commands with a cost to goal metric that also takes into account the vehicle nonholonomic constraints. The planer also allows for node rewiring, and to speed up node search, our method includes heuristics that penalize node expansion near obstacles, and that limit the number of explored nodes. The method book-keeps visited cells in the configuration space, and disallows node expansion at those configurations in the first full iteration of the algorithm. We validate the proposed methods with experiments in extensive real scenarios from different very complex 3D outdoors environments, and compare it with other techniques such as the A*, RRT and RRT* algorithms.Esta tesis reporta investigación sobre el mapeo, clasificación de terreno y planificación de trayectorias. Estos son problemas clásicos en robótica los cuales generalmente se estudian de forma independiente, aquí se vinculan enmarcandolos con una modalidad propioceptiva común: un láser de rango 3D. El objetivo final es ofrecer trayectorias de navegación para escenarios complejos en el marco de la robótica móvil. Por esta razón también entregamos regiones transitables en un mapa global consistente calculado previamente. Primero examinamos el problema de registro de nubes de puntos adquiridas en diferentes instancias de tiempo. Contribuimos con un novedoso mecanismo de registro de pares de imagenes de rango 3D usando correspondencias punto a punto y punto a línea, en una estrategia de búsqueda de correspondencias jerárquica. Para la minimización optamos por una metrica que considera no sólo la distancia entre puntos, sino también la orientación de los marcos de referencia relativos. También proponemos FAMSA, una técnica para el registro rápido simultaneo de multiples nubes de puntos, la cual aprovecha las correspondencias de puntos obtenidas durante el registro secuencial, usando inicialmente la historia de correspondencias para acelerar el cálculo de las correspondecias en los nuevos registros de imagenes. Para propagar adecuadamente el modelo del ruido del sensor y del registro de imagenes, empleamos la propagación de error de primer orden, y para corregir el error acumulado del registro local, consideramos la alineación probabilística de nubes de puntos 3D utilizando un Filtro Extendido de Información de estados retrasados. En esta tesis adaptamos el algóritmo Pose SLAM para el caso de mapas con imagenes de rango 3D, Pose SLAM es la variante de SLAM donde solamente se estima la trayectoria del robot, usando los datos del sensor como restricciones relativas entre las poses robot. Estas técnicas de mapeo se prueban en varios escenarios adquiridos con nuestros sensores 3D produciendo modelos 3D impresionantes. Los mapas obtenidos se procesan para identificar regiones navegables y para planificar secuencias de navegación. Presentamos un par de métodos para lograr la clasificación de zonas transitables fuera de línea. Los datos de entrenamiento se adquieren de forma automática usando secuencias de navegación obtenidas manualmente. Las características transitables se captan de las huella de la trayectoria del robot, lo cual permite capturar restricciones del terreno difíciles de modelar. Con sólo algunas de las zonas transitables como muestras de entrenamiento positivo, nuestros algoritmos se prueban en escenarios reales para encontrar el resto del terreno transitable. Los algoritmos se comparan con algunas variantes de la máquina de soporte de vectores (SVM) y una parametrizacion ingenua. También, contribuimos con un planificador de trayectorias que garantiza llegar a una posicion deseada del robot en significante menor tiempo de cálculo a otras alternativas. Para buscar el mejor camino, nuestro planificador emplea un arbol de busqueda incremental basado en el algoritmo A*. Incluimos una póliza de coste híbrido para crecer de manera eficiente el árbol, combinando el muestro aleatorio del espacio continuo de comandos cinemáticos del robot con una métrica de coste al objetivo que también concidera las cinemática del robot. El planificador además permite reconectado de nodos, y, para acelerar la búsqueda de nodos, se incluye una heurística que penaliza la expansión de nodos cerca de los obstáculos, que limita el número de nodos explorados. El método conoce las céldas que ha visitado del espacio de configuraciones, evitando la expansión de nodos en configuraciones que han sido vistadas en la primera iteración completa del algoritmo. Los métodos propuestos se validán con amplios experimentos con escenarios reales en diferentes entornos exteriores, asi como su comparación con otras técnicas como los algoritmos A*, RRT y RRT*.Postprint (published version