Sampling-Based Motion and Symbolic Action Planning with geometric and differential constraints

Sampling-based motion planning (SBMP) is a major trajectory planning approach in autonomous driving given its high efficiency in practice. As the core of SBMP schemes, sampling strategy holds the key to whether a smooth and collision-free trajectory can be found in real-time. Although some bias sampling strategies have been explored in the literature to accelerate SBMP, the trajectory generated under existing bias sampling strategies may lead to sharp lane changing. To address this issue, we propose a new learning framework for SBMP. Specifically, we develop a novel automatic labeling scheme and a 2-Stage prediction model to improve the accuracy in predicting the intention of surrounding vehicles. We then develop an imitation learning scheme to generate sample points based on the experience of human drivers. Using the prediction results, we design a new bias sampling strategy to accelerate the SBMP algorithm by strategically selecting necessary sample points that can generate a smooth and collision-free trajectory and avoid sharp lane changing. Data-driven experiments show that the proposed sampling strategy outperforms existing sampling strategies, in terms of the computing time, traveling time, and smoothness of the trajectory. The results also show that our scheme is even better than human drivers.

In this paper, we discuss the field of sampling-based motion planning. In contrast to methods that construct boundary representations of configuration space obstacles, samplingbased methods use only information from a collision detector as they search the configuration space. The simplicity of this approach, along with increases in computation power and the development of efficient collision detection algorithms, has resulted in the introduction of a number of powerful motion planning algorithms, capable of solving challenging problems with many degrees of freedom. First, we trace how sampling-based motion planning has developed. We then discuss a variety of important issues for sampling-based motion planning, including uniform and regular sampling, topological issues, and search philosophies. Finally, we address important issues regarding the role of randomization in sampling-based motion planning.

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.

Nearest neighborIchnowski, Jeffrey searchingNearest neighbor searching is aAlterovitz, Ron fundamental building block of most sampling-based motion planners. We present a novel method for fast exact nearest neighbor searching in $$SE(3)$$ —the 6 dimensional space that represents rotations and translations in 3 dimensions. $$SE(3)$$ is commonly used when planning the motions of rigid body robots. Our approach starts by projecting a 4-dimensional cube onto the 3-sphere that is created by the unit quaternion representation of rotations in the rotational group $${ SO}(3)$$ . We then use 4 kd-trees to efficiently partition the projected faces (and their negatives). We propose efficient methods to handle the recursion pruning checks that arise with this kd-tree splitting approach, discuss splitting strategies that support dynamic data sets, and extend this approach to $$SE(3)$$ by incorporating translations. We integrate our approach into RRT and RRT* and demonstrate the fast performance and efficient scaling of our nearest neighbor search as the tree size increases.

This paper develops a tool path optimization method for robotic surface machining by sampling-based motion planning algorithms. In the surface machining process, the tool-tip position needs to strictly follow the tool path curve and the posture of the tool axis should be limited in a certain range. But the industrial robot has at least six degrees-of-freedom (Dof) and has redundant Dofs for surface machining. Therefore, the tool motion of surface machining can be optimized using the redundant Dofs considering the tool path constraints and limits of the tool axis orientation. Due to the complexity of the problem, the sampling-based motion planning method has been chosen to find the solution, which randomly explores the configuration space of the robot and generates a discrete path of valid robot state. During the solving process, the joint space of the robot is chosen as the configuration space of the problem and the constraints for the tool-tip following requirements are in the operation space. Combined with general collision checking, the limited region of the tool axis vector is used to verify the state’s validity of the configuration space. In the optimization process, the sum of the path length of each joint of the robot is set as the optimization objective. The algorithm is developed based on the open motion planning library (OMPL), which contains the state-of-the-art sampling-based motion planners. Finally, two examples are used to demonstrate the effiectiveness and optimality of the method.

We present a novel approach to perform fast probabilistic collision checking in high-dimensional configuration spaces to accelerate the performance of sampling-based motion planning. Our formulation stores the results of prior collision queries, and then uses such information to predict the collision probability for a new configuration sample. In particular, we perform an approximate k-NN (k-nearest neighbor) search to find prior query samples that are closest to the new query configuration. The new query sample’s collision status is then estimated according to the collision checking results of these prior query samples, based on the fact that nearby configurations are likely to have the same collision status. We use locality-sensitive hashing techniques with sub-linear time complexity for approximate k-NN queries. We evaluate the benefit of our probabilistic collision checking approach by integrating it with a wide variety of sampling-based motion planners, including PRM (Probabilistic roadmaps), lazyPRM, RRT Rapidly exploring random trees, and RRT*. Our method can improve these planners in various manners, such as accelerating the local path validation, or computing an efficient order for the graph search on the roadmap. Experiments on a set of benchmarks demonstrate the performance of our method, and we observe up to 2x speedup in the performance of planners on rigid and articulated robots.

We discuss the optimality and computational efficiency of sampling based motion planning (SBMP), which calculates dynamically precise and approximately optimal state transitions using arbitrarily selected nonlinear control system models. To ensure high optimality and computational efficiency, SBMP requires an approximately uniform state sampling function, though the non-linearity of the system models does not allow a perfect function. We propose linear prediction-based uniform state sampling (LPUSS) that samples approximately uniform state points while ensuring a dynamically correct state transition profile with a small calculation cost. LPUSS samples a state by using the given non-linear control system model after determining the input values by using a local linear transition model. We developed a mechanical motion planning system using LPUSS, articulated body algorithm, and parallel computing techniques. To validate LPUSS, we conducted experiments on double, triple, and sixtuple inverted pendulum models. LPUSS showed better optimality and computational efficiency with the double and triple inverted pendulum models, compared with randomized kinodynamic planning (RKP), which is based on rapid random tree (RRT), and our previously proposed rapid semi-optimal motion-planning method in which state sampling is based on uniform inputs. In particular, compared with our previous method, LPUSS was respectively 130 times and 3,000 times faster on double and triple inverted pendulum models under the condition of the same optimality. LPUSS found an approximately optimal swing up motion for the sixtuple inverted pendulum model within 40 minutes. According to our survey, there is no other optimization method that is applicable to higher than quadruple inverted pendulum models with standard constraints.

Sampling-based motion planning techniques have emerged as an efficient algorithmic paradigm for solving complex motion planning problems. These approaches use a set of probing samples to construct an implicit graph representation of the robot's state space, allowing arbitrarily accurate representations as the number of samples increases to infinity. In practice, however, solution trajectories only rely on a few critical states, often defined by structure in the state space (e.g., doorways). In this work we propose a general method to identify these critical states via graph-theoretic techniques (betweenness centrality) and learn to predict criticality from only local environment features. These states are then leveraged more heavily via global connections within a hierarchical graph, termed Critical Probabilistic Roadmaps. Critical PRMs are demonstrated to achieve up to three orders of magnitude improvement over uniform sampling, while preserving the guarantees and complexity of sampling-based motion planning. A video is available at https://youtu.be/AYoD-pGd9ms.

In the Sampling-based motion planning, the collision checking procedure called many times and this operation takes the most planning time. The time spend in the collision checking increases with the number of obstacles. This paper aims to reduce the number of calls of the collision checking procedure in environment containing high number of obstacles. Our approach shows that the complexity of collision checking does not depend on the number of obstacles. The idea is to perform a pre-procedure that localizes the obstacles by segmenting the workspace into a grid and save it in a matrix form. This information is used in the collision checking procedure. We have used the Probabilistic Roadmaps (PRM) motion planner with a punctual robot and polygonal obstacles in 2D environment. Simulation results show the efficiency of our algorithm in reducing the complexity of collision checking and minimizing the planning time.

This paper develops a sensor- and sampling-based motion planner to control a surgical robot in order to explore osteolytic lesions in orthopedic surgery. Because of the difficulty of using conventional surgical tools, such exploration is needed in minimally-invasive treatments of ¿particle diseases,¿ which commonly result from material wear in total hip replacements. Since a geometric model of the osteolytic cavity is not always available, the planner relies only on a robot model that can detect collisions. As such, the planner can work in conjunction with real systems. The planner effectively combines global and local exploration. The global layer determines which regions to explore, while local exploration uses information gain to move the robot tip to positions in the region that increase exploration. Simulation experiments are conducted using a snake-like cannula robot on surgically-relevant osteolytic cavities. As desired in minimally-invasive treatment of osteolysis, performance is measured as the volume explored by the robot tip. The proposed method achieves 83-92% performance rate when compared to methods that require 3D models of osteolytic cavities. Comparisons to sensor-based related work (i.e., no 3D models) show significant improvements in performance.

Fast Trajectory Generation on a Path using Feedback Linearization*

Motion Planning with Cartesian Workspace Information

Proteins are involved in many biochemical processes. The behavior of proteins is influenced by internal void space such as tunnels or cavities. Tunnels are paths leading from an inner protein active site to its surface. The knowledge about tunnels and their evolution over time provides an insight into protein properties (e.g., stability). Sampling-based motion planning techniques like Rapidly Exploring Random Tree (RRT) can be used to find tunnels by sampling the corresponding configuration space. These methods can be easily adapted to operate with protein dynamics. The inner void space of proteins is very limited, which may decrease the ability of RRT to find a solution due to the narrow passage problem. In this paper, we propose to generate random samples using a Voronoi Diagram of the atoms. A subset of Voronoi vertices is dynamically maintained to support the generation of samples in promising regions inside the protein.

Lazy collision checking has been proposed to reduce the computational burden of collision checking which is considered as a major computational bottleneck in sampling-based motion planning. Unfortunately, in complex environments with many obstacles, lazy collision checking can cause an excessive amount of optimistic thrashing problems and significantly degrade the performance of the planner. In this paper, we present an adaptive lazy collision checking method to alleviate the optimistic thrashing, thus broaden the applicability. Our method delays collision checking on the regions predicted to be in the configuration free space, while checking early on the other regions to reduce the optimistic thrashing. To identify such regions, we adopt a configuration free space approximation represented by a set of hyperspheres which can be constructed without significant proximity computation. To demonstrate benefits of our approach we have compared against prior methods including RRT*, PRM* and lazy PRM* across benchmarks with varying dimensions. Overall, our method shows meaningful performance improvement up to three times higher in terms of convergence speed over the tested methods. We also discuss properties and theoretical analysis of the proposed algorithm.

Sampling-based motion planning algorithms have been successfully applied to various types of high-dimensional planning tasks. Recently an extension of PRM algorithm called PRM* planner has been proposed which guarantees asymptotic optimal solutions in terms of path length. However, the high runtime of sampling-based algorithms is still a serious disadvantage. In this paper, a new extension of PRM planner is proposed which incorporates the variable neighborhood radius feature of PRM* and the sampling radius of low-dispersion sampling in order to improve the cost of the generated solutions in terms of path length and runtime. The performance of the proposed algorithm is tested in different planning environments. Furthermore, the proposed planner is compared to the original PRM and the PRM* approaches and shows significant improvement.