Asymptotically Optimal Sampling-Based Motion Planning Methods

Motion planning is a fundamental problem in autonomous robotics that requires finding a path to a specified goal that avoids obstacles and takes into account a robot's limitations and constraints. It is often desirable for this path to also optimize a cost function, such as path length. Formal path-quality guarantees for continuously valued search spaces are an active area of research interest. Recent results have proven that some sampling-based planning methods probabilistically converge toward the optimal solution as computational effort approaches infinity. This survey summarizes the assumptions behind these popular asymptotically optimal techniques and provides an introduction to the significant ongoing research on this topic.Comment: Posted with permission from the Annual Review of Control, Robotics, and Autonomous Systems, Volume 4. Copyright 2021 by Annual Reviews, https://www.annualreviews.org/. 25 pages. 2 figure

db-A*: Discontinuity-bounded Search for Kinodynamic Mobile Robot Motion Planning

We consider time-optimal motion planning for dynamical systems that are translation-invariant, a property that holds for many mobile robots, such as differential-drives, cars, airplanes, and multirotors. Our key insight is that we can extend graph-search algorithms to the continuous case when used symbiotically with optimization. For the graph search, we introduce discontinuity-bounded A* (db-A*), a generalization of the A* algorithm that uses concepts and data structures from sampling-based planners. Db-A* reuses short trajectories, so-called motion primitives, as edges and allows a maximum user-specified discontinuity at the vertices. These trajectories are locally repaired with trajectory optimization, which also provides new improved motion primitives. Our novel kinodynamic motion planner, kMP-db-A*, has almost surely asymptotic optimal behavior and computes near-optimal solutions quickly. For our empirical validation, we provide the first benchmark that compares search-, sampling-, and optimization-based time-optimal motion planning on multiple dynamical systems in different settings. Compared to the baselines, kMP-db-A* consistently solves more problem instances, finds lower-cost initial solutions, and converges more quickly.Comment: Accepted at IROS 202

Bidirectional Sampling Based Search Without Two Point Boundary Value Solution

Bidirectional motion planning approaches decrease planning time, on average, compared to their unidirectional counterparts. In single-query feasible motion planning, using bidirectional search to find a continuous motion plan requires an edge connection between the forward and reverse search trees. Such a tree-tree connection requires solving a two-point Boundary Value Problem (BVP). However, a two-point BVP solution can be difficult or impossible to calculate for many systems. We present a novel bidirectional search strategy that does not require solving the two-point BVP. Instead of connecting the forward and reverse trees directly, the reverse tree's cost information is used as a guiding heuristic for the forward search. This enables the forward search to quickly converge to a feasible solution without solving the two-point BVP. We propose two new algorithms (GBRRT and GABRRT) that use this strategy and run multiple software simulations using multiple dynamical systems and real-world hardware experiments to show that our algorithms perform on-par or better than existing state-of-the-art methods in quickly finding an initial feasible solution.Comment: Journal version (Video: https://youtu.be/Rumg66UHfyQ

Toward certifiable optimal motion planning for medical steerable needles

Medical steerable needles can follow 3D curvilinear trajectories to avoid anatomical obstacles and reach clinically significant targets inside the human body. Automating steerable needle procedures can enable physicians and patients to harness the full potential of steerable needles by maximally leveraging their steerability to safely and accurately reach targets for medical procedures such as biopsies. 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 planning algorithms involved in procedure automation. In this paper, we take an important step toward creating a certifiable optimal planner for steerable needles. We present an efficient, resolution-complete motion planner for steerable needles based on a novel adaptation of multi-resolution planning. This is the first motion planner for steerable needles that guarantees to compute in finite time an obstacle-avoiding plan (or notify the user that no such plan exists), under clinically appropriate assumptions. Based on this planner, we then develop the first resolution-optimal motion planner for steerable needles that further provides theoretical guarantees on the quality of the computed motion plan, that is, global optimality, in finite time. Compared to state-of-the-art steerable needle motion planners, we demonstrate with clinically realistic simulations that our planners not only provide theoretical guarantees but also have higher success rates, have lower computation times, and result in higher quality plans

A Framework for Offline Risk-aware Planning of Low-altitude Aerial Flights during Urban Disaster Response

Disaster response missions are dynamic and dangerous events for first responders. Active situational awareness is critical for effective decision-making, and unmanned aerial assets have successfully extended the range and output of sensors. Aerial assets have demonstrated their capability in disaster response missions via decentralized operations. However, literature and industry lack a systematic investigation of the algorithms, datasets, and tools for aerial system trajectory planning in urban disasters that optimizes mission performance and guarantee mission success. This work seeks to develop a framework and software environment to investigate the requirements for offline planning algorithms and flight risk models when applied to aerial assets exploring urban disaster zones. This is addressed through the creation of rapid urban maps, efficient flight planning algorithms, and formal risk metrics that are demonstrated in scenario-driven experiments using Monte Carlo simulation. First, rapid urban mapping strategies are independently compared for efficient processing and storage through obstacle and terrain layers. Open-source data is used when available and is supplemented with an urban feature prediction model trained on satellite imagery using deep learning. Second, sampling-based planners are evaluated for efficient and effective trajectory planning of nonlinear aerial dynamic systems. The algorithm can find collision-free, kinodynamic feasible trajectories using random open-loop control targets. Alternative open-loop control commands are formed to improve the planning algorithm’s speed and convergence. Third, a risk-aware implementation of the planning algorithm is developed that considers the uncertainty of energy, collisions, and onboard viewpoint data and maps them to a single measure of the likelihood of mission failure. The three modules are combined in a framework where the rapid urban maps and risk-aware planner are evaluated against benchmarks for mission success, performance, and speed while creating a unique set of benchmarks from open-source data and software. One, the rapid urban map module generates a 3D structure and terrain map within 20 meters of data and in less than 5 minutes. The Gaussian Process terrain model performs better than B-spline and NURBS models in small-scale, mountainous environments at 10-meter squared resolution. Supplementary data for structures and other urban landcover features is predicted using the Pix2Pix Generative Adversarial Network with a 3-channel encoding for nine labels. Structures, greenspaces, water, and roads are predicted with high accuracy according to the F1, OIU, and pixel accuracy metrics. Two, the sampling-based planning algorithm is selected for forming collision-free, 3D offline flight paths with a black-box dynamics model of a quadcopter. Sampling-based planners prove successful for efficient and optimal flight paths through randomly generated and rapid urban maps, even under wind and noise uncertainty. The Stable-Sparse-RRT, SST, algorithm is shown to improve trajectories for minimum Euclidean distance more consistently and efficiently than the RRT algorithm, with a 50% improvement in finite-time path convergence for large-scale urban maps. The forward propagation dynamics of the black-box model are replaced with 5-15 times more computationally efficient motion primitives that are generated using an inverse lower-order dynamics model and the Differential Dynamic Programming, DDP, algorithm. Third, the risk-aware planning algorithm is developed that generates optimal paths based on three risk metrics of energy, collision, and viewpoint risk and quantifies the likelihood of worst-case events using the Conditional-Value-at-Risk, CVaR, metric. The sampling-based planning algorithm is improved with informative paths, and three versions of the algorithm are compared for the best performance in different scenarios. Energy risk in the planning algorithm results in 5-35% energy reduction and 20-30% more consistency in finite-time convergence for flight paths in large-scale urban maps. All three risk metrics in the planning algorithm generally result in more energy use than the planner with only energy risk, but reduce the mean flight path risk by 10-50% depending on the environment, energy available, and viewpoint landmarks. A final experiment in an Atlanta flooding scenario demonstrates the framework’s full capability with the rapid urban map displaying essential features and the trajectory planner reporting flight time, energy consumption, and total risk. Furthermore, the simulation environment provides insight into offline planning limitations through Monte Carlo simulations with environment wind and system dynamics noise. The framework and software environment are made available to use as benchmarks in the field to serve as a foundation for increasing the effectiveness of first responders’ safety in the challenging task of urban disaster response.Ph.D

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