Learning to Generate Cost-to-Go Functions for Efficient Motion Planning

Haptic devices allow a user to feel either reaction forces from virtual interactions or reaction forces reflected from a remote site during a bilateral teleoperation task. Also, guiding forces can be exerted to train the user in the performance of a virtual task or to assist him/her to safely teleoperate a robot. The generation of guiding forces relies on the existence of a motion plan that provides the direction to be followed to reach the goal from any free configuration of the configuration space ( ${\mathcal C}$ -space). This paper proposes a method to obtain such a plan that interleaves a sampling-based exploration of ${\mathcal C}$ -space with an efficient computation of harmonic functions. A deterministic sampling sequence (with a bias based on harmonic function values) is used to obtain a hierarchical cell decomposition model of ${\mathcal C}$ -space. A harmonic function is iteratively computed over the partially known model using a novel approach. The harmonic function is the navigation function used as motion plan. The approach has been implemented in a planner (called the Kautham planner) that, given an initial and a goal configuration, provides: (a) a channel of cells connecting the cell that contains the initial configuration with the cell that contains the goal configuration; (b) two harmonic functions over the whole ${\mathcal C}$ -space, one that guides motions towards the channel and another that guides motions within the channel towards the goal; and (c) a path computed over a roadmap built with the free samples of the channel. The harmonic functions and the solution path are then used to generate the guiding forces for the haptic device. The planning approach is illustrated with examples on 2D and 3D workspaces.

Sampling-based Motion Planners (SMPs) have become increasingly popular as they provide collision-free path solutions regardless of obstacle geometry in a given environment. However, their computational complexity increases significantly with the dimensionality of the motion planning problem. Adaptive sampling is one of the ways to speed up SMPs by sampling a particular region of a configuration space that is more likely to contain an optimal path solution. Although there are a wide variety of algorithms for adaptive sampling, they rely on hand-crafted heuristics; furthermore, their performance decreases significantly in high-dimensional spaces. In this paper, we present a neural network-based adaptive sampler for motion planning called Deep Sampling-based Motion Planner (DeepSMP). DeepSMP generates samples for SMPs and enhances their overall speed significantly while exhibiting efficient scalability to higher-dimensional problems. DeepSMP's neural architecture comprises of a Contractive AutoEncoder which encodes given workspaces directly from a raw point cloud data, and a Dropout-based stochastic deep feedforward neural network which takes the workspace encoding, start and goal configuration, and iteratively generates feasible samples for SMPs to compute end-to-end collision-free optimal paths. DeepSMP is not only consistently computationally efficient in all tested environments but has also shown remarkable generalization to completely unseen environments. We evaluate DeepSMP on multiple planning problems including planning of a point-mass robot, rigid-body, 6-link robotic manipulator in various 2D and 3D environments. The results show that on average our method is at least 7 times faster in point-mass and rigid-body case and about 28 times faster in 6-link robot case than the existing state-of-the-art.

Steerable needles composed of a highly flexible material and with a bevel tip offer greater mobility compared to rigid needles for minimally invasive medical procedures. In this paper, we apply sampling-based motion planning technique to explore motion planning for the steerable bevel-tip needle in 3D environments with obstacles. Based on the Rapidly-exploring Random Trees (RRTs) method, we develop a motion planner to quickly build a tree to search the configuration space using a new exploring strategy, which generates new states using randomly sampled control space instead of the deterministically sampled one used in classic RRTs. Notice the fact that feasible paths might not be found for any given entry point and target configuration, we also address the feasible entry point planning problem to find feasible entry points in a specified entry zone for any given target configuration. To solve this problem, we developed a motion planning algorithm based on RRTs with backchaining, which grow backward from the target to explore the configuration space. Finally, simulation results with a approximated realistic prostate needle insertion environment demonstrate the performance of the proposed motion planner.

Recent progress in motion planning has made it possible to determine homotopy inequivalent trajectories between an initial and terminal configuration in a robot configuration space. Current approaches have however either assumed the knowledge of differential one-forms related to a skeletonization of the collision space, or have relied on a simplicial representation of the free space. Both of these approaches are currently however not yet practical for higher dimensional configuration spaces. We propose 2D topological task projections (TTPs): mappings from the configuration space to 2-dimensional spaces where simplicial complex filtrations and persistent homology can identify topological properties of the high-dimensional free configuration space. Our approach only requires the availability of collision free samples to identify winding centers that can be used to determine homotopy inequivalent trajectories. We propose the Winding Augmented RRT and RRT* (WA-RRT/RRT*) algorithms using which homotopy inequivalent trajectories can be found. We evaluate our approach in experiments with configuration spaces of planar linkages with 2–10 degrees of freedom. Results indicate that our approach can reliably identify suitable topological task projections and our proposed WA-RRT and WA-RRT* algorithms were able to identify a collection of homotopy inequivalent trajectories in each considered configuration space dimension.

A topological theory initiated recently by the author uses methods of algebraic topology to estimate numerically the character of instabilities arising in motion planning algorithms. The present paper studies random motion planning algorithms and reveals how the topology of the robot's configuration space influences their structure. We prove that the topological complexity of motion planning TC(X) coincides with the minimal n such that there exists an n-valued random motion planning algorithm for the system; here $X$ denotes the configuration space. We study in detail the problem of collision free motion of several objects on a graph G. We describe an explicit motion planning algorithm for this problem. We prove that if G is a tree and if the number of objects is large enough, then the topological complexity of this motion planning problem equals 2m(G)+1 where m(G) is the number of the essential vertices of G. It turns out (in contrast with the results on the collision free control of many objects in space obtained earlier jointly with S. Yuzvinsky) that the topological complexity is independent of the number of particles.

Visual feedback can play a crucial role in a dynamic robotic task such as the interception of a moving target. To utilize the feedback effectively, there is a need to develop robot motion planning techniques that also take into account properties of the sensed data. We propose a motion planning framework that achieves this with the help of a space called the perceptual control manifold (PCM) defined on the product of the robot configuration space and an image-based feature space. We show how the task of intercepting a moving target can be mopped to the PCM. This leads to the generation of motion plans that satisfy various constraints and optimality criteria derived from the robot kinematics, the control system, and the sensing mechanism.

Chapter 18 - Distances in Mathematical Engineering

This work presents an efficient framework to generate a motion plan of a robot with high degrees of freedom (e.g., a humanoid robot). High-dimensionality of the robot configuration space often leads to difficulties in utilizing the widely-used motion planning algorithms, since the volume of the decision space increases exponentially with the number of dimensions. To handle complications arising from the large decision space, and to solve a corresponding motion planning problem efficiently, two key concepts are adopted in this work: First, the Gaussian process latent variable model (GP-LVM) is utilized for low-dimensional representation of the original configuration space. Second, an approximate inference algorithm is used, exploiting through the duality between control and estimation, to explore the decision space and to compute a high-quality motion trajectory of the robot. Utilizing the GP-LVM and the duality between control and estimation, we construct a fully probabilistic generative model with which a high-dimensional motion planning problem is transformed into a tractable inference problem. Finally, we compute the motion trajectory via an approximate inference algorithm based on a variant of the particle filter. The resulting motions can be viewed in the supplemental video. ( https://youtu.be/kngEaOR4Esc )

Presents an approach to sensor-based obstacle modeling in a configuration space for manipulator motion planning in unknown environments. In order to achieve this objective, an efficient algorithm is used to fast map obstacles based on defined fundamental obstacles in the workspace and their images in the configuration space. A robotic manipulator is assumed to be equipped with distance sensors to detect obstacles in the local region. By computation of the critical points of an obstacle based on information acquired by the distance sensors, an obstacle model in the configuration space is constructed. By using this sensor-based configuration space modeling, robot motion planning in unknown environments can be performed in realistic time frames.

Configuration space (C-space) plays an important role not only in motion planning but also in geometric modeling, shape and kinematic reasoning, and is fundamental to several basic geometric operations, such as continuous collision detection and generalized penetration depth estimation, that also find their applications in motion planning, animation and simulation. In this paper, we developed a new method for constructing the boundary of the C-space obstacles (C-obst) of polygons. This method is simpler to implement and is theoretically more efficient than the existing techniques. Our main idea is to devote the computation on the discontinuity in temporal and spatial coherence where the structure of the C-obst changes. We also developed a method for estimating the generalized penetration depth by computing the distance between the query point and the C-obst surface.

This work considers sensor based motion planning for rod-shaped robots in unknown environments. The motion planning scheme is based on the rod hierarchical generalized Voronoi graph (rod-HGVG). The rod-HGVG is a roadmap for rod-like robots, and is an extension of a prior roadmap for point-like robots. We give an incremental method to construct the rod-HGVG thereby enabling exploration of unknown environments. An important practical feature of the algorithm is its sole reliance upon the use of work space distance measurements to objects that are within line of sight. Such measurements can be readily provided by conventional range sensors. Moreover, motion planning in a configuration space is achieved without explicitly constructing each configuration space obstacle. A key result derived in this paper is the distance gradient between two convex sets.

Path planning has been widely studied by computer scientists. However, it is a very simplified version of the motion planning problems occurring in robotics. This paper examines extensions yielding two important issues: controllability and recognizability. The controllability issue arises when the number of controls is smaller than the number of independent parameters defining the robot's configuration: Can the motions span the configuration space? The recognizability issue occurs when there are errors in control and sensing: Can the robot recognize goal achievement? Both issues have interesting impact on the computational complexity of motion planning. This paper will also discuss a new path planning scheme based on random sampling of configuration space, to deal with many-degree-of-freedom robots. The blend of controllability, recognizability, and complexity issues discussed in this paper is unique to robotics and its study is key to the development of autonomous robots.

Omni-directional Wheeled Mobile Robots (Omni-WMR) are holonomic system able to move sideways. They have locomotive advantage and are widely deployed in a larger range of application fields, especially in narrowly constrained service-styled manufacturing industries. To ensure smooth motion and full development of locomotive advantages, motion planning draws great attention. Thanks to the mechanism of powered caster wheel (PCW), the motion of Omni-WMR can be decoupled. The collective motion can be decomposed into dimensions of x, y and θ, then planned independently and superposed. In this way trajectory can be planned in the Cartesian space, and later on transformed to the joint space through inverse instantaneous kinematics model for actuation of the motor at each joint. Quintic-polynomial interpolation is used for the case of start to goal with only initial and final configurations known, and piecewise-cubic-spline interpolation is applied for the case of a sequence of intermediate way-points known in the configuration space as input. Here the geometric points for the robot to go through are assumed to be known as a priori without loss of generality. The approaches are implemented in python on ROS (Robot Operating System), and these modular ROS packages are tested on a physical Omni-WMR developed at our lab. Meanwhile, these ROS packages are also tested equivalently in a virtual simulation environment modelled using ROS stage simulator. The experimental results have proved that the motion planning algorithms are able to generate smooth velocity profile for the robot to achieve the goal, which have validated the effectiveness of these algorithms.

We present a review and reformulation of manifold constrained sampling-based motion planning within a unifying framework, IMACS (implicit manifold configuration space). IMACS enables a broad class of motion planners to plan in the presence of manifold constraints, decoupling the choice of motion planning algorithm and method for constraint adherence into orthogonal choices. We show that implicit configuration spaces defined by constraints can be presented to sampling-based planners by addressing two key fundamental primitives, sampling and local planning, and that IMACS preserves theoretical properties of probabilistic completeness and asymptotic optimality through these primitives. Within IMACS, we implement projection- and continuation-based methods for constraint adherence, and demonstrate the framework on a range of planners with both methods in simulated and realistic scenarios. Our results show that the choice of method for constraint adherence depends on many factors and that novel combinations of planners and methods of constraint adherence can be more effective than previous approaches. Our implementation of IMACS is open source within the Open Motion Planning Library and is easily extended for novel planners and constraint spaces.

One of the ultimate goals of mobile robotics research is to build robots that can safely carry out missions in hazardous and populated environments. Most of today's commercial mobile devices scale poorly along this dimension. Their motion planning relies on accurate, static models of the environments, and therefore they often fail their mission if humans or other unpredictable obstacles block their path. To build autonomous mobile robots one has to build systems that can perceive their environments, react to unforeseen circumstances, and plan dynamically in order to achieve their mission. Thus, the objective of the motion planning and control problem is to find collision-free trajectories, in static or dynamic environments containing some obstacles, between a start and a goal configuration. It has attracted much research in recent years. In this context the term control has a broad meaning that includes many different controls, such as low-level motor control, and behaviour control, where behaviour represents many complicated tasks, like obstacle avoidance and goal seeking. This article describes an intelligent motion planning and navigation system for omnidirectional mobile robots based on fuzzy logic. Owing to its simplicity and hence its short response time, the fuzzy navigator is especially suitable for on-line applications with strong real-time requirements. On-line planning is an on-going activity. The planner receives a continuous flow of information about occurring events and generates new commands in response to the incoming events, while previously planned motions are being executed. The fuzzy-rule-base of the proposed system combines the repelling influence, which is related to the distance and the angle between the robot and nearby obstacles, with the attracting influence produced by the distance and the angular difference between the actual direction and position of the robot and the final configuration, to generate actuating commands for the mobile platform. It can be considered as an on-line local navigation method for omnidirectional mobile robots for the generation of instantaneous collision-free motions. This reactive system is especially suitable for real-time applications. The use of fuzzy logic leads to a transparent system which can be tuned by hand or by a set of learning rules. Furthermore, this approach allows obstacle avoidance and navigation in dynamic environments. The functioning of the fuzzy motion planner with respect to omnidirectional mobile robots and results of simulated experiments are presented.