Autonomous motion planning under unknown nonlinear dynamics presents significant challenges. An agent needs to continuously explore the system dynamics to acquire its properties, such as reachability, in order to guide system navigation adaptively. In this paper, we propose a hybrid planning-control framework designed to compute a feasible trajectory toward a target. Our approach involves partitioning the state space and approximating the system by a piecewise affine (PWA) system with constrained control inputs. By abstracting the PWA system into a directed weighted graph, we incrementally update the existence of its edges via affine system identification and reach control theory, introducing a predictive reachability condition by exploiting prior information of the unknown dynamics. Heuristic weights are assigned to edges based on whether their existence is certain or remains indeterminate. Consequently, we propose a framework that adaptively collects and analyzes data during mission execution, continually updates the predictive graph, and synthesizes a controller online based on the graph search outcomes. We demonstrate the efficacy of our approach through simulation scenarios involving a mobile robot operating in unknown terrains, with its unknown dynamics abstracted as a single integrator model.
University of Illinois Urbana-Champaign（伊利诺伊大学厄巴纳-香槟分校）；Stanford University（斯坦福大学）
Trajectory planning under uncertainty is an active research topic. Previous works predict state and state estimation uncertainties along trajectories to check for collision safety. They assume either stochastic or bounded sensing uncertainties. However, GNSS pseudoranges are typically modeled to contain stochastic uncertainties with additional biases in urban environments. Thus, given bounds for the bias, the planner needs to account for both stochastic and bounded sensing uncertainties. In our prior work we presented a reachability analysis to predict state and state estimation uncertainties …
We present a method for feedback motion planning of systems with unknown dynamics which provides probabilistic guarantees on safety, reachability, and goal stability. To find a domain in which a learned control-affine approximation of the true dynamics can be trusted, we estimate the Lipschitz constant of the difference between the true and learned dynamics, and ensure the estimate is valid with a given probability. Provided the system has at least as many controls as states, we also derive existence conditions for a one-step feedback law which can keep the real system within a small bound of …
Action anticipation, intent prediction, and proactive behavior are all desirable characteristics for autonomous driving policies in interactive scenarios. Paramount, however, is ensuring safety on the road -- a key challenge in doing so is accounting for uncertainty in human driver actions without unduly impacting planner performance. This paper introduces a minimally-interventional safety controller operating within an autonomous vehicle control stack with the role of ensuring collision-free interaction with an externally controlled (e.g., human-driven) counterpart while respecting static obs…
Learning Differentiable Reachability Maps for Optimization-based Humanoid Motion Generation
CNRS-AIST JRL (Joint Robotics Laboratory)（法国国家科学研究中心-日本产业技术综合研究所联合机器人实验室）；IRL（智能机器人实验室）；National Institute of Advanced Industrial Science and Technology (AIST)（国家先进工业科学与技术研究所）
Google AI（谷歌人工智能）；University of New Mexico（新墨西哥大学）
This paper addresses two challenges facing sampling-based kinodynamic motion planning: a way to identify good candidate states for local transitions and the subsequent computationally intractable steering between these candidate states. Through the combination of sampling-based planning, a Rapidly Exploring Randomized Tree (RRT) and an efficient kinodynamic motion planner through machine learning, we propose an efficient solution to long-range planning for kinodynamic motion planning. First, we use deep reinforcement learning to learn an obstacle-avoiding policy that maps a robot's sensor obse…
Preferences play a key role in determining what goals/constraints to satisfy when not all constraints can be satisfied simultaneously. In this paper, we study how to synthesize preference satisfying plans in stochastic systems, modeled as an MDP, given a (possibly incomplete) combinative preference model over temporally extended goals. We start by introducing new semantics to interpret preferences over infinite plays of the stochastic system. Then, we introduce a new notion of improvement to enable comparison between two prefixes of an infinite play. Based on this, we define two solution conce…
RTD-RAX: Fast, Safe Trajectory Planning for Systems under Unknown Disturbances
Georgia Institute of Technology（佐治亚理工学院）；School of Electrical and Computer Engineering（电子与计算机工程学院）；School of Mechanical Engineering（机械工程学院）；School of Civil and Environmental Engineering（土木与环境工程学院）
Reachability analysis is at the core of many applications, from neural network verification, to safe trajectory planning of uncertain systems. However, this problem is notoriously challenging, and current approaches tend to be either too restrictive, too slow, too conservative, or approximate and therefore lack guarantees. In this paper, we propose a simple yet effective sampling-based approach to perform reachability analysis for arbitrary dynamical systems. Our key novel idea consists of using random set theory to give a rigorous interpretation of our method, and prove that it returns sets w…
Game-Theoretic Safe Multi-Agent Motion Planning with Reachability Analysis for Dynamic and Uncertain Environments (Extended Version)
Department of Electrical and Computer Engineering, National University of Singapore（国立新加坡大学电气与计算机工程系）；Department of Control Science and Engineering, Shanghai Institute of Intelligent Science and Technology（上海智能科学与技术研究院控制科学与工程系）
We consider the mobile robot path planning problem for a class of recurrent reachability objectives. These objectives are parameterized by the expected time needed to visit one position from another, the expected square of this time, and also the frequency of moves between two neighboring locations. We design an efficient strategy synthesis algorithm for recurrent reachability objectives and demonstrate its functionality on non-trivial instances.
School of Mechanical Engineering, University of Michigan（密歇根大学机械工程学院）；Robotics Institute, University of Michigan（密歇根大学机器人研究所）；Stanford University（斯坦福大学）
Reinforcement Learning (RL) algorithms have achieved remarkable performance in decision making and control tasks due to their ability to reason about long-term, cumulative reward using trial and error. However, during RL training, applying this trial-and-error approach to real-world robots operating in safety critical environment may lead to collisions. To address this challenge, this paper proposes a Reachability-based Trajectory Safeguard (RTS), which leverages reachability analysis to ensure safety during training and operation. Given a known (but uncertain) model of a robot, RTS precompute…
Carnegie Mellon University（卡内基梅隆大学）；Indian Institute of Technology Kharagpur（印度理工学院克勒格布尔分校）
Constrained Iterative Linear Quadratic Regulator (CILQR), a variant of ILQR, has been recently proposed for motion planning problems of autonomous vehicles to deal with constraints such as obstacle avoidance and reference tracking. However, the previous work considers either deterministic trajectories or persistent prediction for target dynamical obstacles. The other drawback is lack of generality - it requires manual weight tuning for different scenarios. In this paper, two significant improvements are achieved. Firstly, a two-stage uncertainty-aware prediction is proposed. The short-term pre…
Algorithms for motion planning in unknown environments are generally limited in their ability to reason about the structure of the unobserved environment. As such, current methods generally navigate unknown environments by relying on heuristic methods to choose intermediate objectives along frontiers. We present a unified method that combines map prediction and motion planning for safe, time-efficient autonomous navigation of unknown environments by dynamically-constrained robots. We propose a data-driven method for predicting the map of the unobserved environment, using the robot's observatio…
In legged locomotion, the relationship between different gait behaviors and energy consumption must consider the full-body dynamics and the robot control as a whole, which cannot be captured by simple models. This work studies the robot dynamics and whole-body optimal control as a coupled system to investigate energy consumption during balance recovery. We developed a 2-phase nonlinear optimization pipeline for dynamic stepping, which generates reachability maps showing complex energy-stepping relations. We optimize gait parameters to search all reachable locations and quantify the energy cost…
University of Connecticut（康涅狄格大学）；Naval Undersea Warfare Center（海军水下作战中心）
This paper presents a rapid (real time) solution to the minimum-time path planning problem for Dubins vehicles under environmental currents (wind or ocean currents). Real-time solutions are essential in time-critical situations (such as replanning under dynamically changing environments or tracking fast moving targets). Typically, Dubins problem requires to solve for six path types; however, due to the presence of currents, four of these path types require to solve the root-finding problem involving transcendental functions. Thus, the existing methods result in high computation times and their…
Ain Shams University（艾因沙姆斯大学）；Jacobs University（雅各布大学）；Stanford University（斯坦福大学）；KTH Royal Institute of Technology（KTH皇家理工学院）
Reinforcement learning (RL) is capable of sophisticated motion planning and control for robots in uncertain environments. However, state-of-the-art deep RL approaches typically lack safety guarantees, especially when the robot and environment models are unknown. To justify widespread deployment, robots must respect safety constraints without sacrificing performance. Thus, we propose a Black-box Reachability-based Safety Layer (BRSL) with three main components: (1) data-driven reachability analysis for a black-box robot model, (2) a trajectory rollout planner that predicts future actions and ob…
DAG-STL: A Hierarchical Framework for Zero-Shot Trajectory Planning under Signal Temporal Logic Specifications
School of Automation and Intelligent Sensing, Shanghai Jiao Tong University, Shanghai, China（自动化与智能感知学院，上海交通大学，上海，中国）；Institute of Artificial Intelligence, Xiamen University, Xiamen, China（人工智能研究院，厦门大学，厦门，中国）
In this paper, we propose a novel inverse Dynamic Reachability Map (iDRM) that allows a floating base system to find valid end-poses in complex and dynamically changing environments in real-time. End-pose planning for valid stance pose and collision-free configuration is an essential problem for humanoid applications, such as providing goal states for walking and motion planners. However, this is non-trivial in complex environments, where standing locations and reaching postures are restricted by obstacles. Our proposed iDRM customizes the robot-to-workspace occupation list and uses an online …
Rutgers University, The State University of New Jersey（罗格斯大学，新泽西州立大学）
Object rearrangement is important for many applications but remains challenging, especially in confined spaces, such as shelves, where objects cannot be accessed from above and they block reachability to each other. Such constraints require many motion planning and collision checking calls, which are computationally expensive. In addition, the arrangement space grows exponentially with the number of objects. To address these issues, this work introduces a lazy evaluation framework with a local monotone solver and a global planner. Monotone instances are those that can be solved by moving each …
Computer Science and Artificial Intelligence Laboratory (CSAIL), MIT（麻省理工学院计算机科学与人工智能实验室（CSAIL））；Tagup, Inc.（Tagup公司）；GRASP Laboratory, University of Pennsylvania（宾夕法尼亚大学GRASP实验室）
We describe a task and motion planning architecture for highly dynamic systems that combines a domain-independent sampling-based deliberative planning algorithm with a global reactive planner. We leverage the recent development of a reactive, vector field planner that provides guarantees of reachability to large regions of the environment even in the face of unknown or unforeseen obstacles. The reachability guarantees can be formalized using contracts that allow a deliberative planner to reason purely in terms of those contracts and synthesize a plan by choosing a sequence of reactive behavior…
Grasping in dynamic environments presents a unique set of challenges. A stable and reachable grasp can become unreachable and unstable as the target object moves, motion planning needs to be adaptive and in real time, the delay in computation makes prediction necessary. In this paper, we present a dynamic grasping framework that is reachability-aware and motion-aware. Specifically, we model the reachability space of the robot using a signed distance field which enables us to quickly screen unreachable grasps. Also, we train a neural network to predict the grasp quality conditioned on the curre…
Within a robot autonomy stack, the planner and controller are typically designed separately, and serve different purposes. As such, there is often a diffusion of responsibilities when it comes to ensuring safety for the robot. We propose that a planner and controller should share the same interpretation of safety but apply this knowledge in a different yet complementary way. To achieve this, we use Hamilton-Jacobi (HJ) reachability theory at the planning level to provide the robot planner with the foresight to avoid entering regions with possible inevitable collision. However, this alone does …
Robust motion planning entails computing a global motion plan that is safe under all possible uncertainty realizations, be it in the system dynamics, the robot's initial position, or with respect to external disturbances. Current approaches for robust motion planning either lack theoretical guarantees, or make restrictive assumptions on the system dynamics and uncertainty distributions. In this paper, we address these limitations by proposing the robust rapidly-exploring random-tree (Robust-RRT) algorithm, which integrates forward reachability analysis directly into sampling-based control traj…
Planning robust executions under uncertainty is a fundamental challenge for building autonomous robots. Partially Observable Markov Decision Processes (POMDPs) provide a standard framework for modeling uncertainty in many applications. In this work, we study POMDPs with safe-reachability objectives, which require that with a probability above some threshold, a goal state is eventually reached while keeping the probability of visiting unsafe states below some threshold. This POMDP formulation is different from the traditional POMDP models with optimality objectives and we show that in some case…
Institute for Robotics and Intelligent Machines, Georgia Institute of Technology（佐治亚理工学院机器人与智能机器研究所）
Anytime sampling-based methods are an attractive technique for solving kino-dynamic motion planning problems. These algorithms scale well to higher dimensions and can efficiently handle state and control constraints. However, an intelligent exploration strategy is required to accelerate their convergence and avoid redundant computations. Using ideas from reachability analysis, this work defines a "Time-Informed Set", that focuses the search for time-optimal kino-dynamic planning after an initial solution is found. Such a Time-Informed Set (TIS) includes all trajectories that can potentially im…
Interleaved POMDP Planning for Multi-Object Search in Unknown Multi-Room Household Environments
Georgia Institute of Technology（佐治亚理工学院）；The Hong Kong University of Science and Technology（香港科技大学）；Ising AI（伊辛人工智能公司）；MIT（麻省理工学院）；Notre Dame University（圣母大学）
