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 under stochastic and bounded uncertainties. However, we ignored the correlation between these uncertainties, leading to an imperfect approximation of the state uncertainty. In this paper we improve our reachability analysis by predicting state uncertainty as a function of independent quantities. We design a metric for the predicted uncertainty to compare candidate trajectories during planning. Finally, we validate the planner for GNSS-based urban navigation of fixed-wing UAS.
University of Michigan（密歇根大学）；University of Pennsylvania（宾夕法尼亚大学）；Ford Motor Company（福特汽车公司）
Performing real-time receding horizon motion planning for autonomous vehicles while providing safety guarantees remains difficult. This is because existing methods to accurately predict ego vehicle behavior under a chosen controller use online numerical integration that requires a fine time discretization and thereby adversely affects real-time performance. To address this limitation, several recent papers have proposed to apply offline reachability analysis to conservatively predict the behavior of the ego vehicle. This reachable set can be constructed by utilizing a simplified model whose be…
In this work, we propose a trajectory generation method for robotic systems with contact force constraint based on optimal control and reachability analysis. Normally, the dynamics and constraints of the contact-constrained robot are nonlinear and coupled to each other. Instead of linearizing the model and constraints, we directly solve the optimal control problem to obtain the feasible state trajectory and the control input of the system. A tractable optimal control problem is formulated which is addressed by dual approaches, which are sampling-based dynamic programming and rigorous reachabil…
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…
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…
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…
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…
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（土木与环境工程学院）
Max Planck Institute for Intelligent Systems（马克斯·普朗克智能系统研究所）；LAAS-CNRS（LAAS-法国国家科学研究中心）；Université de Toulouse（图卢兹大学）；CNRS（法国国家科学研究中心）；University of Trento（特伦托大学）；Tandon School of Engineering, New York University（纽约大学坦登工程学院）
Generation of robust trajectories for legged robots remains a challenging task due to the underlying nonlinear, hybrid and intrinsically unstable dynamics which needs to be stabilized through limited contact forces. Furthermore, disturbances arising from unmodelled contact interactions with the environment and model mismatches can hinder the quality of the planned trajectories leading to unsafe motions. In this work, we propose to use stochastic trajectory optimization for generating robust centroidal momentum trajectories to account for additive uncertainties on the model dynamics and paramet…
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（上海智能科学与技术研究院控制科学与工程系）
Motion planners for mobile robots in unknown environments face the challenge of simultaneously maintaining both robustness against unmodeled uncertainties and persistent feasibility of the trajectory-finding problem. That is, while dealing with uncertainties, a motion planner must update its trajectory, adapting to the newly revealed environment in real-time; failing to do so may involve unsafe circumstances. Many existing planning algorithms guarantee these by maintaining the clearance needed to perform an emergency brake, which is itself a robust and persistently feasible maneuver. However, …
Naval Air Warfare Center Weapons Division（海军空战中心武器分部）；University of California, Santa Barbara（加州大学圣塔芭芭拉分校）；Naval Air Warfare Center Aircraft Division（海军空战中心飞机分部）
We present a numeric method to compute the safe operating flight conditions for a helicopter such that we can ensure a safe landing in the event of a partial or total engine failure. The unsafe operating region is the complement of the backwards reachable tube, which can be found as the sub-zero level set of the viscosity solution of a Hamilton-Jacobi (HJ) equation. Traditionally, numerical methods used to solve the HJ equation rely on a discrete grid of the solution space and exhibit exponential scaling with dimension, which is problematic for the high-fidelity dynamics models required for ac…
Hong Kong University of Science and Technology（香港科技大学）
It is ubiquitously accepted that during the autonomous navigation of the quadrotors, one of the most widely adopted unmanned aerial vehicles (UAVs), safety always has the highest priority. However, it is observed that the ego airflow disturbance can be a significant adverse factor during flights, causing potential safety issues, especially in narrow and confined indoor environments. Therefore, we propose a novel method to estimate and adapt indoor ego airflow disturbance of quadrotors, meanwhile applying it to trajectory planning. Firstly, the hover experiments for different quadrotors are con…
K. N. Toosi University of Technology（K. N. 图西理工大学）；Max Planck Institute for Intelligent Systems（马克斯·普朗克智能系统研究所）；University of Trento（特伦托大学）；New York University（纽约大学）
Trajectory optimization (TO) is one of the most powerful tools for generating feasible motions for humanoid robots. However, including uncertainties and stochasticity in the TO problem to generate robust motions can easily lead to intractable problems. Furthermore, since the models used in TO have always some level of abstraction, it can be hard to find a realistic set of uncertainties in the model space. In this paper we leverage a sample-efficient learning technique (Bayesian optimization) to robustify TO for humanoid locomotion. The main idea is to use data from full-body simulations to mak…
Mechanical Engineering, University of Michigan（密歇根大学机械工程学院）；Robotics Institute, University of Michigan（密歇根大学机器人研究所）；Mechanical and Industrial Engineering, Louisiana State University（路易斯安那州立大学机械与工业工程学院）；Naval Architecture and Marine Engineering, University of Michigan（密歇根大学船舶与海洋工程学院）；Ford Motor Company（福特汽车公司）；Office of Naval Research（美国海军研究办公室）
For robotic arms to operate in arbitrary environments, especially near people, it is critical to certify the safety of their motion planning algorithms. However, there is often a trade-off between safety and real-time performance; one can either carefully design safe plans, or rapidly generate potentially-unsafe plans. This work presents a receding-horizon, real-time trajectory planner with safety guarantees, called ARMTD (Autonomous Reachability-based Manipulator Trajectory Design). The method first computes (offline) a reachable set of parameterized trajectories for each joint of an arm. Eac…
Department of Computer Engineering（计算机工程系）；German Electron Synchrotron Technical University（德国电子同步加速器技术大学）；Technical University Munich（慕尼黑技术大学）；University of Hamburg（汉堡大学）
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…
Fast and Scalable Game-Theoretic Trajectory Planning with Intentional Uncertainties
The Hong Kong University of Science and Technology (Guangzhou)（香港科学与技术大学（广州））；The Hong Kong University of Science and Technology（香港科学与技术大学）；HKUST Shenzhen-Hong Kong Collaborative Innovation Research Institute（香港科学与技术大学深圳-香港协同创新研究院）
Max Planck Institute for Intelligent Systems（马克斯·普朗克智能系统研究所）；K. N. Toosi University of Technology（K.N.托西理工大学）；Tandon School of Engineering, New York University（纽约大学坦登工程学院）
Trajectory optimization (TO) is one of the most powerful tools for generating feasible motions for humanoid robots. However, including uncertainties and stochasticity in the TO problem to generate robust motions can easily lead to an interactable problem. Furthermore, since the models used in the TO have always some level of abstraction, it is hard to find a realistic set of uncertainty in the space of abstract model. In this paper we aim at leveraging a sample-efficient learning technique (Bayesian optimization) to robustify trajectory optimization for humanoid locomotion. The main idea is to…
Autonomous ground vehicles (AGVs) are receiving increasing attention, and the motion planning and control problem for these vehicles has become a hot research topic. In real applications such as material handling, an AGV is subject to large uncertainties and its motion planning and control become challenging. In this paper, we investigate this problem by proposing a hierarchical control scheme, which is integrated by a model predictive control (MPC) based path planning and trajectory tracking control at the high level, and a reduced-order extended state observer (RESO) based dynamic control at…
Generating safe motion plans in real-time is a key requirement for deploying robot manipulators to assist humans in collaborative settings. In particular, robots must satisfy strict safety requirements to avoid self-damage or harming nearby humans. Satisfying these requirements is particularly challenging if the robot must also operate in real-time to adjust to changes in its environment.This paper addresses these challenges by proposing Reachability-based Signed Distance Functions (RDFs) as a neural implicit representation for robot safety. RDF, which can be constructed using supervised learn…
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 …
PhysV2A: Reachability-Gated and Semantic-Mask-Constrained Feasibility Completion for Video-to-Robot Manipulation
School of Automation, Guangdong University of Technology（广东工业大学自动化学院）；University of Liverpool（利物浦大学）；Department of Computing, The Hong Kong Polytechnic University（香港理工大学计算学系）
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（人工智能研究院，厦门大学，厦门，中国）
Katholieke Universiteit Leuven（鲁汶天主教大学）；University of Melbourne（墨尔本大学）
We propose a safety-guaranteed planning and control framework for unmanned surface vessels (USVs), using Gaussian processes (GPs) to learn uncertainties. The uncertainties encountered by USVs, including external disturbances and model mismatches, are potentially state-dependent, time-varying, and hard to capture with constant models. GP is a powerful learning-based tool that can be integrated with a model-based planning and control framework, which employs a Hamilton-Jacobi differential game formulation. Such a combination yields less conservative trajectories and safety-guaranteeing control s…
Safety guarantees in motion planning for autonomous driving typically involve certifying the trajectory to be collision-free under any motion of the uncontrollable participants in the environment, such as the human-driven vehicles on the road. As a result they usually employ a conservative bound on the behavior of such participants, such as reachability analysis. We point out that planning trajectories to rigorously avoid the entirety of the reachable regions is unnecessary and too restrictive, because observing the environment in the future will allow us to prune away most of them; disregardi…
Air Traffic Management Research Institute (ATMRI)（空中交通管理研究所（ATMRI））；Singapore Centre for 3D Printing (SC3DP)（新加坡3D打印中心（SC3DP））；School of Mechanical and Aerospace Engineering, Nanyang Technological University, Singapore（南洋理工大学机械与航空航天工程学院，新加坡）；Nanyang Technological University（南洋理工大学）
Given a geometric path, the Time-Optimal Path Tracking problem consists in finding the control strategy to traverse the path time-optimally while regulating tracking errors. A simple yet effective approach to this problem is to decompose the controller into two components: (i)~a path controller, which modulates the parameterization of the desired path in an online manner, yielding a reference trajectory; and (ii)~a tracking controller, which takes the reference trajectory and outputs joint torques for tracking. However, there is one major difficulty: the path controller might not find any feas…
Safe UAV navigation is challenging due to the complex environment structures, dynamic obstacles, and uncertainties from measurement noises and unpredictable moving obstacle behaviors. Although plenty of recent works achieve safe navigation in complex static environments with sophisticated mapping algorithms, such as occupancy map and ESDF map, these methods cannot reliably handle dynamic environments due to the mapping limitation from moving obstacles. To address the limitation, this paper proposes a trajectory planning framework to achieve safe navigation considering complex static environmen…
LARC: Lazy Adaptive Reachability Certification of Robot Manipulator Trajectories
National University of Singapore（新加坡国立大学）；Singapore Institute of Manufacturing Technology（新加坡制造技术研究院）；Agency for Science Technology and Research（新加坡科技研究局）
Simon Fraser University（西蒙菲莎大学）；Princeton University（普林斯顿大学）；University of Melbourne（墨尔本大学）；California Institute of Technology（加州理工学院）
Real-time, guaranteed safe trajectory planning is vital for navigation in unknown environments. However, real-time navigation algorithms typically sacrifice robustness for computation speed. Alternatively, provably safe trajectory planning tends to be too computationally intensive for real-time replanning. We propose FaSTrack, Fast and Safe Tracking, a framework that achieves both real-time replanning and guaranteed safety. In this framework, real-time computation is achieved by allowing any trajectory planner to use a simplified \textit{planning model} of the system. The plan is tracked by th…
In this paper, we address the trajectory planning problem in uncertain nonconvex static and dynamic environments that contain obstacles with probabilistic location, size, and geometry. To address this problem, we provide a risk bounded trajectory planning method that looks for continuous-time trajectories with guaranteed bounded risk over the planning time horizon. Risk is defined as the probability of collision with uncertain obstacles. Existing approaches to address risk bounded trajectory planning problems either are limited to Gaussian uncertainties and convex obstacles or rely on sampling…
University of Oxford（牛津大学）；Sabanci University（萨班哲大学）；CNRS（法国国家科学研究中心）
Despite the great progress in quadrupedal robotics during the last decade, selecting good contacts (footholds) in highly uneven and cluttered environments still remains an open challenge. This paper builds upon a state-of-the-art approach, already successfully used for humanoid robots, and applies it to our robotic platform; the quadruped robot ANY-mal. The proposed algorithm decouples the problem into two subprob-lems: first a guide trajectory for the robot is generated, then contacts are created along this trajectory. Both subproblems rely on approximations and heuristics that need to be tun…
Trajectory optimization with contact-rich behaviors has recently gained attention for generating diverse locomotion behaviors without pre-specified ground contact sequences. However, these approaches rely on precise models of robot dynamics and the terrain and are susceptible to uncertainty. Recent works have attempted to handle uncertainties in the system model, but few have investigated uncertainty in contact dynamics. In this study, we model uncertainty stemming from the terrain and design corresponding risk-sensitive objectives under the framework of contact-implicit trajectory optimizatio…
Learning Maximal Safe Sets Using Hypernetworks for MPC-based Local Trajectory Planning in Unknown Environments
Continental Automotive Technologies GmbH, Germany（德国大陆汽车技术有限公司）；Technical University of Berlin, Germany（柏林技术大学）；Free University of Berlin, Germany（柏林自由大学）
In order for automated mobile vehicles to navigate in the real world with minimal collision risks, it is necessary for their planning algorithms to consider uncertainties from measurements and environmental disturbances. In this paper, we consider analytical solutions for a conservative approximation of the mutual probability of collision between two robotic vehicles in the presence of such uncertainties. Therein, we present two methods, which we call unitary scaling and principal axes rotation, for decoupling the bivariate integral required for efficient approximation of the probability of co…
Max Planck Institute for Intelligent Systems（马克斯·普朗克智能系统研究所）；New York University（纽约大学）
We present a general, two-stage reinforcement learning approach to create robust policies that can be deployed on real robots without any additional training using a single demonstration generated by trajectory optimization. The demonstration is used in the first stage as a starting point to facilitate initial exploration. In the second stage, the relevant task reward is optimized directly and a policy robust to environment uncertainties is computed. We demonstrate and examine in detail the performance and robustness of our approach on highly dynamic hopping and bounding tasks on a quadruped r…
