Real-world environments are inherently uncertain, and to operate safely in these environments robots must be able to plan around this uncertainty. In the context of motion planning, we desire systems that can maintain an acceptable level of safety as the robot moves, even when the exact locations of nearby obstacles are not known. In this paper, we solve this chance-constrained motion planning problem using a sequential convex optimization framework. To constrain the risk of collision incurred by planned movements, we employ geometric objects called $ε$-shadows to compute upper bounds on the risk of collision between the robot and uncertain obstacles. We use these $ε$-shadow-based estimates as constraints in a nonlinear trajectory optimization problem, which we then solve by iteratively linearizing the non-convex risk constraints. This sequential optimization approach quickly finds trajectories that accomplish the desired motion while maintaining a user-specified limit on collision risk. Our method can be applied to robots and environments with arbitrary convex geometry; even in complex environments, it runs in less than a second and provides provable guarantees on the safety of planned trajectories, enabling fast, reactive, and safe robot motion in realistic environments.
We present a novel approach to perform probabilistic collision detection between a high-DOF robot and high-DOF obstacles in dynamic, uncertain environments. In dynamic environments with a high-DOF robot and moving obstacles, our approach efficiently computes accurate collision probability between the robot and obstacles with upper error bounds. Furthermore, we describe a prediction algorithm for future obstacle position and motion that accounts for both spatial and temporal uncertainties. We present a trajectory optimization algorithm for high-DOF robots in dynamic, uncertain environments base…
We present GLAS: Global-to-Local Autonomy Synthesis, a provably-safe, automated distributed policy generation for multi-robot motion planning. Our approach combines the advantage of centralized planning of avoiding local minima with the advantage of decentralized controllers of scalability and distributed computation. In particular, our synthesized policies only require relative state information of nearby neighbors and obstacles, and compute a provably-safe action. Our approach has three major components: i) we generate demonstration trajectories using a global planner and extract local obser…
UC Berkeley（加州大学伯克利分校）；Ford Research and Advanced Engineering（福特研究与高级工程部门）
We propose a Model Predictive Control (MPC) for collision avoidance between an autonomous agent and dynamic obstacles with uncertain predictions. The collision avoidance constraints are imposed by enforcing positive distance between convex sets representing the agent and the obstacles, and tractably reformulating them using Lagrange duality. This approach allows for smooth collision avoidance constraints even for polytopes, which otherwise require mixed-integer or non-smooth constraints. We consider three widely used descriptions of the uncertain obstacle position: 1) Arbitrary distribution wi…
Many path planning algorithms are based on sampling the state space. While this approach is very simple, it can become costly when the obstacles are unknown, since samples hitting these obstacles are wasted. The goal of this paper is to efficiently identify obstacles in a map and remove them from the sampling space. To this end, we propose a pre-processing algorithm for space exploration that enables more efficient sampling. We show that it can boost the performance of other space sampling methods and path planners. Our approach is based on the fact that a convex obstacle can be approximated p…
TRUST-Planner: Topology-guided Robust Trajectory Planner for AAVs with Uncertain Obstacle Spatial-temporal Avoidance
School of Aerospace Engineering, Beijing Institute of Technology（北京理工大学航空航天工程学院）；Key Laboratory of Dynamics and Control of Flight Vehicle, Ministry of Education（教育部飞行器动力学与控制重点实验室）
Manipal Institute of Technology（马尼帕尔理工学院）；Robotics Institute, Carnegie Mellon University（卡内基梅隆大学机器人研究所）
Motion Planning under uncertainty is critical for safe self-driving. In this paper, we propose a unified obstacle avoidance framework that deals with 1) uncertainty in ego-vehicle motion; and 2) prediction uncertainty of dynamic obstacles from environment. A two-stage traffic participant trajectory predictor comprising short-term and long-term prediction is used in the planning layer to generate safe but not over-conservative trajectories for the ego vehicle. The prediction module cooperates well with existing planning approaches. Our work showcases its effectiveness in a Frenet frame planner.…
Many practical applications of robotics require systems that can operate safely despite uncertainty. In the context of motion planning, two types of uncertainty are particularly important when planning safe robot trajectories. The first is environmental uncertainty -- uncertainty in the locations of nearby obstacles, stemming from sensor noise or (in the case of obstacles' future locations) prediction error. The second class of uncertainty is uncertainty in the robots own state, typically caused by tracking or estimation error. To achieve high levels of safety, it is necessary for robots to co…
Simultaneous Localization and Planning (SLAP) under process and measurement uncertainties is a challenge. It involves solving a stochastic control problem modeled as a Partially Observed Markov Decision Process (POMDP) in a general framework. For a convex environment, we propose an optimization-based open-loop optimal control problem coupled with receding horizon control strategy to plan for high quality trajectories along which the uncertainty of the state localization is reduced while the system reaches to a goal state with minimum control effort. In a static environment with non-convex stat…
Cornell University（康奈尔大学）；Massachusetts Institute of Technology（麻省理工学院）
This paper investigates the usefulness of reasoning about the uncertain presence of obstacles during path planning, which typically stems from the usage of probabilistic occupancy grid maps for representing the environment when mapping via a noisy sensor like a stereo camera. The traditional planning paradigm prescribes using a hard threshold on the occupancy probability to declare that a cell is an obstacle, and to plan a single path accordingly while treating unknown space as free. We compare this approach against a new uncertainty-aware planner, which plans two different path hypotheses and…
Trajectory Planning with Model Predictive Control for Obstacle Avoidance Considering Prediction Uncertainty
Institute of Electromobility, RPTU University Kaiserslautern-Landau（电磁移动研究所，科隆-兰道大学）；Institute of Wireless Communications and Navigation, RPTU University Kaiserslautern-Landau（无线通信与导航研究所，科隆-兰道大学）
FZI Research Center for Information Technology（FZI信息技术研究中心）；Karlsruhe Institute of Technology (KIT)（卡尔斯鲁厄理工学院（KIT））
Provable safety is one of the most critical challenges in automated driving. The behavior of numerous traffic participants in a scene cannot be predicted reliably due to complex interdependencies and the indiscriminate behavior of humans. Additionally, we face high uncertainties and only incomplete environment knowledge. Recent approaches minimize risk with probabilistic and machine learning methods - even under occlusions. These generate comfortable behavior with good traffic flow, but cannot guarantee safety of their maneuvers. Therefore, we contribute a safety verification method for trajec…
As drones and autonomous cars become more widespread it is becoming increasingly important that robots can operate safely under realistic conditions. The noisy information fed into real systems means that robots must use estimates of the environment to plan navigation. Efficiently guaranteeing that the resulting motion plans are safe under these circumstances has proved difficult. We examine how to guarantee that a trajectory or policy is safe with only imperfect observations of the environment. We examine the implications of various mathematical formalisms of safety and arrive at a mathematic…
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…
Tsinghua University（清华大学）；ETH Zurich（苏黎世联邦理工学院）；Harbin Institute of Technology（哈尔滨工业大学）
This paper presents an efficient and safe method to avoid static and dynamic obstacles based on LiDAR. First, point cloud is used to generate a real-time local grid map for obstacle detection. Then, obstacles are clustered by DBSCAN algorithm and enclosed with minimum bounding ellipses (MBEs). In addition, data association is conducted to match each MBE with the obstacle in the current frame. Considering MBE as an observation, Kalman filter (KF) is used to estimate and predict the motion state of the obstacle. In this way, the trajectory of each obstacle in the forward time domain can be param…
Provably Safe Motion Planning Under Unknown Disturbances
Department of Aerospace Engineering Sciences at the University of Colorado Boulder（科罗拉多大学博尔德分校航空航天工程科学系）；Department of Aerospace and Ocean Engineering（航空航天与海洋工程系）；Delft University of Technology（代尔夫特理工大学）；Italian Institute of Artificial Intelligence（意大利人工智能研究所）
We present an optimization-based method to plan the motion of an autonomous robot under the uncertainties associated with dynamic obstacles, such as humans. Our method bounds the marginal risk of collisions at each point in time by incorporating chance constraints into the planning problem. This problem is not suitable for online optimization outright for arbitrary probability distributions. Hence, we sample from these chance constraints using an uncertainty model, to generate "scenarios", which translate the probabilistic constraints into deterministic ones. In practice, each scenario represe…
Path planning in an uncertain environment is a key enabler of true vehicle autonomy. Over the past two decades, numerous approaches have been developed to account for errors in the vehicle path while navigating complex and often uncertain environments. An important capability of such planning is the prediction of vehicle dispersion covariances about a candidate path. This work develops a new closed-loop linear covariance (CL-LinCov) framework applicable to a wide range of autonomous system architectures. Important features of the developed framework include the (1) separation of high-level gui…
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…
RRC, IIIT Hyderabad, India（印度海得拉巴国际信息技术学院（IIIT Hyderabad）RRC（可能是某个研究中心或实验室的缩写，此处保留原文））；University of Tartu, Estonia（爱沙尼亚塔尔图大学）；University of Maryland（马里兰大学）
The probabilistic velocity obstacle (PVO) extends the concept of velocity obstacle (VO) to work in uncertain dynamic environments. In this paper, we show how a robust model predictive control (MPC) with PVO constraints under non-parametric uncertainty can be made computationally tractable. At the core of our formulation is a novel yet simple interpretation of our robust MPC as a problem of matching the distribution of PVO with a certain desired distribution. To this end, we propose two methods. Our first baseline method is based on approximating the distribution of PVO with a Gaussian Mixture …
Computing collision-free trajectories is of prime importance for safe navigation. We present an approach for computing the collision probability under Gaussian distributed motion and sensing uncertainty with the robot and static obstacle shapes approximated as ellipsoids. The collision condition is formulated as the distance between ellipsoids and unlike previous approaches we provide a method for computing the exact collision probability. Furthermore, we provide a tight upper bound that can be computed much faster during online planning. Comparison to other state-of-the-art methods is also pr…
NVIDIA（英伟达）；Carnegie Mellon University（卡内基梅隆大学）；University of Washington（华盛顿大学）
In many applications, including logistics and manufacturing, robot manipulators operate in semi-structured environments alongside humans or other robots. These environments are largely static, but they may contain some movable obstacles that the robot must avoid. Manipulation tasks in these applications are often highly repetitive, but require fast and reliable motion planning capabilities, often under strict time constraints. Existing preprocessing-based approaches are beneficial when the environments are highly-structured, but their performance degrades in the presence of movable obstacles, …
Honda Research Institute（本田研究所）；University of California, Berkeley（加州大学伯克利分校）
This paper proposes a discretionary lane selection algorithm. In particular, highway driving is considered as a targeted scenario, where each lane has a different level of traffic flow. When lane-changing is discretionary, it is advised not to change lanes unless highly beneficial, e.g., reducing travel time significantly or securing higher safety. Evaluating such "benefit" is a challenge, along with multiple surrounding vehicles in dynamic speed and heading with uncertainty. We propose a real-time lane-selection algorithm with careful cost considerations and with modularity in design. The alg…
We propose a hybrid feedback control law that guarantees both safety and asymptotic stability for a class of Lagrangian systems in environments with obstacles. Rather than performing trajectory planning and implementing a trajectory-tracking feedback control law, our approach requires a sequence of locations in the environment (a path plan) and an abstraction of the obstacle-free space. The problem of following a path plan is then interpreted as a sequence of reach-avoid problems: the system is required to consecutively reach each location of the path plan while staying within safe regions. Ob…
University of California San Diego（加州大学圣地亚哥分校）；San Diego State University（圣地亚哥州立大学）；University of North Carolina at Chapel Hill（北卡罗来纳大学教堂山分校）
Safety is a critical concern for the success of urban air mobility, especially in dynamic and uncertain environments. This paper proposes a path planning algorithm based on RRT in conjunction with chance constraints in the presence of uncertain obstacles. The chance-constrained formulation for Gaussian distributed obstacles is developed by converting the probabilistic constraints to deterministic constraints in terms of distribution parameters. The probabilistic feasible region at every time step can be established through the simulation of the system state and the evaluation of convex constra…
As robots are being increasingly used in close proximity to humans and objects, it is imperative that robots operate safely and efficiently under real-world conditions. Yet, the environment is seldom known perfectly. Noisy sensors and actuation errors compound to the errors introduced while estimating features of the environment. We present a novel approach (1) to incorporate these uncertainties for robot state estimation and (2) to compute the probability of collision pertaining to the estimated robot configurations. The expression for collision probability is obtained as an infinite series a…
Control Barrier Functions (CBF) are widely used to enforce the safety-critical constraints on nonlinear systems. Recently, these functions are being incorporated into a path planning framework to design safety-critical path planners. However, these methods fall short of providing a realistic path considering both the algorithm's run-time complexity and enforcement of the safety-critical constraints. This paper proposes a novel motion planning approach using the well-known Rapidly Exploring Random Trees (RRT) algorithm that enforces both CBF and the robot Kinodynamic constraints to generate a s…
We address the risk bounded trajectory optimization problem of stochastic nonlinear robotic systems. More precisely, we consider the motion planning problem in which the robot has stochastic nonlinear dynamics and uncertain initial locations, and the environment contains multiple dynamic uncertain obstacles with arbitrary probabilistic distributions. The goal is to plan a sequence of control inputs for the robot to navigate to the target while bounding the probability of colliding with obstacles. Existing approaches to address risk bounded trajectory optimization problems are limited to partic…
University of California, Los Angeles（加利福尼亚大学洛杉矶分校）
We present an end-to-end online motion planning framework that uses a data-driven approach to navigate a heterogeneous robot team towards a global goal while avoiding obstacles in uncertain environments. First, we use stochastic model predictive control (SMPC) to calculate control inputs that satisfy robot dynamics, and consider uncertainty during obstacle avoidance with chance constraints. Second, recurrent neural networks are used to provide a quick estimate of future state uncertainty considered in the SMPC finite-time horizon solution, which are trained on uncertainty outputs of various si…
This paper presents a provably correct method for robot navigation in 2D environments cluttered with familiar but unexpected non-convex, star-shaped obstacles as well as completely unknown, convex obstacles. We presuppose a limited range onboard sensor, capable of recognizing, localizing and (leveraging ideas from constructive solid geometry) generating online from its catalogue of the familiar, non-convex shapes an implicit representation of each one. These representations underlie an online change of coordinates to a completely convex model planning space wherein a previously developed onlin…
Deep Reinforcement Learning is quickly becoming a popular method for training autonomous Unmanned Aerial Vehicles (UAVs). Our work analyzes the effects of measurement uncertainty on the performance of Deep Reinforcement Learning (DRL) based waypoint navigation and obstacle avoidance for UAVs. Measurement uncertainty originates from noise in the sensors used for localization and detecting obstacles. Measurement uncertainty/noise is considered to follow a Gaussian probability distribution with unknown non-zero mean and variance. We evaluate the performance of a DRL agent trained using the Proxim…
To operate reactively in uncertain environments, robots need to be able to quickly estimate the risk that they will collide with their environment. This ability is important for both planning (to ensure that plans maintain acceptable levels of safety) and execution (to provide real-time warnings when risk exceeds some threshold). Existing methods for estimating this risk are often limited to models with simplified geometry (e.g. point robots); others handle complex geometry but are too slow for many applications. In this paper, we present two algorithms for quickly computing upper bounds on th…
