Configuration Space Decomposition for Scalable Proxy Collision Checking in Robot Planning and Control

Real-time robot motion planning in complex high-dimensional environments remains an open problem. Motion planning algorithms, and their underlying collision checkers, are crucial to any robot control stack. Collision checking takes up a large portion of the computational time in robot motion planning. Existing collision checkers make trade-offs between speed and accuracy and scale poorly to high-dimensional, complex environments. We present a novel space decomposition method using K-Means clustering in the Forward Kinematics space to accelerate proxy collision checking. We train individual configuration space models using Fastron, a kernel perceptron algorithm, on these decomposed subspaces, yielding compact yet highly accurate models that can be queried rapidly and scale better to more complex environments. We demonstrate this new method, called Decomposed Fast Perceptron (D-Fastron), on the 7-DOF Baxter robot producing on average 29x faster collision checks and up to 9.8x faster motion planning compared to state-of-the-art geometric collision checkers.

Optimal Multi-Manipulator Arm Placement for Maximal Dexterity during Robotics Surgery

Robot arm placements are oftentimes a limitation in surgical preoperative procedures, relying on trained staff to evaluate and decide on the optimal positions for the arms. Given new and different patient anatomies, it can be challenging to make an informed choice, leading to more frequently colliding arms or limited manipulator workspaces. In this paper, we develop a method to generate the optimal manipulator base positions for the multi-port da Vinci surgical system that minimizes self-collision and environment-collision, and maximizes the surgeon's reachability inside the patient. Scoring functions are defined for each criterion so that they may be optimized over. Since for multi-manipulator setups, a large number of free parameters are available to adjust the base positioning of each arm, a challenge becomes how one can expediently assess possible setups. We thus also propose methods that perform fast queries of each measure with the use of a proxy collision-checker. We then develop an optimization method to determine the optimal position using the scoring functions. We evaluate the optimality of the base positions for the robot arms on canonical trajectories, and show that the solution yielded by the optimization program can satisfy each criterion. The metrics and optimization strategy are generalizable to other surgical robotic platforms so that patient-side manipulator positioning may be optimized and solved.

DiffCo: Auto-Differentiable Proxy Collision Detection with Multi-class Labels for Safety-Aware Trajectory Optimization

The objective of trajectory optimization algorithms is to achieve an optimal collision-free path between a start and goal state. In real-world scenarios where environments can be complex and non-homogeneous, a robot needs to be able to gauge whether a state will be in collision with various objects in order to meet some safety metrics. The collision detector should be computationally efficient and, ideally, analytically differentiable to facilitate stable and rapid gradient descent during optimization. However, methods today lack an elegant approach to detect collision differentiably, relying rather on numerical gradients that can be unstable. We present DiffCo, the first, fully auto-differentiable, non-parametric model for collision detection. Its non-parametric behavior allows one to compute collision boundaries on-the-fly and update them, requiring no pre-training and allowing it to update continuously in dynamic environments. It provides robust gradients for trajectory optimization via backpropagation and is often 10-100x faster to compute than its geometric counterparts. DiffCo also extends trivially to modeling different object collision classes for semantically informed trajectory optimization.

Stochastic Modeling of Distance to Collision for Robot Manipulators

Evaluating distance to collision for robot manipulators is useful for assessing the feasibility of a robot configuration or for defining safe robot motion in unpredictable environments. However, distance estimation is a timeconsuming operation, and the sensors involved in measuring the distance are always noisy. A challenge thus exists in evaluating the expected distance to collision for safer robot control and planning. In this work, we propose the use of Gaussian process (GP) regression and the forward kinematics (FK) kernel (a similarity function for robot manipulators) to efficiently and accurately estimate distance to collision. We show that the GP model with the FK kernel achieves 70 times faster distance evaluations compared to a standard geometric technique, and up to 13 times more accurate evaluations compared to other regression models, even when the GP is trained on noisy distance measurements. We employ this technique in trajectory optimization tasks and observe 9 times faster optimization than with the noise-free geometric approach yet obtain similar optimized motion plans. We also propose a confidence-based hybrid model that uses model-based predictions in regions of high confidence and switches to a more expensive sensor-based approach in other areas, and we demonstrate the usefulness of this hybrid model in an application involving reaching into a narrow passage.

Autonomous Navigation in Unknown Environments using Sparse Kernel-based Occupancy Mapping

This paper focuses on real-time occupancy mapping and collision checking onboard an autonomous robot navigating in an unknown environment. We propose a new map representation, in which occupied and free space are separated by the decision boundary of a kernel perceptron classifier. We develop an online training algorithm that maintains a very sparse set of support vectors to represent obstacle boundaries in configuration space. We also derive conditions that allow complete (without sampling) collision-checking for piecewise-linear and piecewise-polynomial robot trajectories. We demonstrate the effectiveness of our mapping and collision checking algorithms for autonomous navigation of an Ackermann-drive robot in unknown environments.

Forward Kinematics Kernel for Improved Proxy Collision Checking

Kernel functions may be used in robotics for comparing different poses of a robot, such as in collision checking, inverse kinematics, and motion planning. These comparisons provide distance metrics often based on joint space measurements and are performed hundreds or thousands of times a second, continuously for changing environments. Few examples exist in creating new kernels, despite their significant effect on computational performance and robustness in robot control and planning. We introduce a new kernel function based on forward kinematics (FK) to compare robot manipulator configurations. We integrate our new FK kernel into our proxy collision checker, Fastron, that previously showed significant speed improvements to collision checking and motion planning. With the new FK kernel, we realize a two-fold speedup in proxy collision check speed, 8 times less memory, and a boost in classification accuracy from 75% to over 95% for a 7 degrees-of-freedom robot arm compared to the previously-used radial basis function kernel. Compared to state-of-the-art geometric collision checkers, with the FK kernel, collision checks are now 9 times faster. To show the broadness of the approach, we apply Fastron FK in OMPL across a wide variety of motion planners, showing unanimously faster robot planning.

Learning-Based Proxy Collision Detection for Robot Motion Planning Applications

This paper demonstrates that collision detection-intensive applications such as robotic motion planning may be accelerated by performing collision checks with a machine learning model. We propose Fastron, a learning-based algorithm to model a robot's configuration space to be used as a proxy collision detector in place of standard geometric collision checkers. We demonstrate that leveraging the proxy collision detector results in up to an order of magnitude faster performance in robot simulation and planning than state-of-the-art collision detection libraries. Our results show that Fastron learns a model more than 100 times faster than a competing C-space modeling approach, while also providing theoretical guarantees of learning convergence. Using the OMPL motion planning libraries, we were able to generate initial motion plans across all experiments with varying robot and environment complexities. With Fastron, we can repeatedly perform planning from scratch at a 56 Hz rate, showing its application toward autonomous surgical assistance task in shared environments with human-controlled manipulators. All performance gains were achieved despite using only CPU-based calculations, suggesting further computational gains with a GPU approach that can parallelize tensor algebra. Code is available online.

Fastron: An Online Learning-Based Model and Active Learning Strategy for Proxy Collision Detection

We introduce the Fastron, a configuration space (C-space) model to be used as a proxy to kinematic-based collision detection. The Fastron allows iterative updates to account for a changing environment through a combination of a novel formulation of the kernel perceptron learning algorithm and an active learning strategy. Our simulations on a 7 degree-of-freedom arm indicate that proxy collision checks may be performed at least 2 times faster than an efficient polyhedral collision checker and at least 8 times faster than an efficient high-precision collision checker. The Fastron model provides conservative collision status predictions by padding C-space obstacles, and proxy collision checking time does not scale poorly as the number of workspace obstacles increases. All results were achieved without GPU acceleration or parallel computing.

Robot Autonomy for Surgery

Autonomous surgery involves having surgical tasks performed by a robot operating under its own will, with partial or no human involvement. There are several important advantages of automation in surgery, which include increasing precision of care due to sub-millimeter robot control, real-time utilization of biosignals for interventional care, improvements to surgical efficiency and execution, and computer-aided guidance under various medical imaging and sensing modalities. While these methods may displace some tasks of surgical teams and individual surgeons, they also present new capabilities in interventions that are too difficult or go beyond the skills of a human. In this chapter, we provide an overview of robot autonomy in commercial use and in research, and present some of the challenges faced in developing autonomous surgical robots.