To move through the world, mobile robots typically use a receding-horizon strategy, wherein they execute an old plan while computing a new plan to incorporate new sensor information. A plan should be dynamically feasible, meaning it obeys constraints like the robot's dynamics and obstacle avoidance; it should have liveness, meaning the robot does not stop to plan so frequently that it cannot accomplish tasks; and it should be optimal, meaning that the robot tries to satisfy a user-specified cost function such as reaching a goal location as quickly as possible. Reachability-based Trajectory Design (RTD) is a planning method that can generate provably dynamically-feasible plans. However, RTD solves a nonlinear polynmial optimization program at each planning iteration, preventing optimality guarantees; furthermore, RTD can struggle with liveness because the robot must brake to a stop when the solver finds local minima or cannot find a feasible solution. This paper proposes RTD*, which certifiably finds the globally optimal plan (if such a plan exists) at each planning iteration. This method is enabled by a novel Parallelized Constrained Bernstein Algorithm (PCBA), which is a branch-and-bound method for polynomial optimization. The contributions of this paper are: the implementation of PCBA; proofs of bounds on the time and memory usage of PCBA; a comparison of PCBA to state of the art solvers; and the demonstration of PCBA/RTD* on a mobile robot. RTD* outperforms RTD in terms of optimality and liveness for real-time planning in a variety of environments with randomly-placed obstacles.
This paper presents a method and an open-source implementation, Bernstein/Bézier Optimal Trajectories (BeBOT), for the generation of trajectories for autonomous system operations. The proposed method is based on infinite dimensional optimal control formulations of trajectory generation problems. By approximating the trajectories using Bernstein polynomials, these problems can be transcribed as nonlinear programming problems, which can then be solved using off-the-shelf solvers. Bernstein polynomials possess favorable geometric properties that enable the trajectory planner to efficiently evalua…
This paper presents a new trajectory planning method for multiple quadrotors in obstacle-dense environments. We suggest a relative safe flight corridor (RSFC) to model safe region between a pair of agents, and it is used to generate linear constraints for inter-collision avoidance by utilizing the convex hull property of relative Bernstein polynomial. Our approach employs a graph-based multi-agent pathfinding algorithm to generate an initial trajectory, which is used to construct a safe flight corridor (SFC) and RSFC. We express the trajectory as a piecewise Bernstein polynomial and formulate …
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…
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…
High-speed trajectory planning through unknown environments requires algorithmic techniques that enable fast reaction times while maintaining safety as new information about the operating environment is obtained. The requirement of computational tractability typically leads to optimization problems that do not include the obstacle constraints (collision checks are done on the solutions) or use a convex decomposition of the free space and then impose an ad-hoc time allocation scheme for each interval of the trajectory. Moreover, safety guarantees are usually obtained by having a local planner t…
SafeMove-RL: A Certifiable Reinforcement Learning Framework for Dynamic Motion Constraints in Trajectory Planning
Sino-German College of Intelligent Manufacturing, Shenzhen Technology University（深圳技术大学中德智能制造学院）；Faculty of Science and Technology, University of Macau（澳门大学科技学院）
Language-Conditioned Safe Trajectory Generation for Spacecraft Rendezvous
Department of Aeronautics and Astronautics, 496 Lomita Mall（航空航天系，496号Lomita Mall）；Department of Mechanical Engineering, 440 Escondido Mall Building 530（机械工程系，440号Escondido Mall Building 530）
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（土木与环境工程学院）
FSDP: Fast and Safe Data-Driven Overtaking Trajectory Planning for Head-to-Head Autonomous Racing Competitions
Department of Control Science and Engineering, Zhejiang University（控制科学与工程系，浙江大学）；Center for Project-Based Learning, D-ITET, ETH Zurich（基于项目的学习中心，苏黎世联邦理工学院）
This paper studies the problem of fast and safe aerial payload transport by a single quadcopter in urban areas. The quadcopter payload system (QPS) is considered as a rigid body and modeled with a nonlinear dynamics. The urban area is modeled as an obstacle-laden environment with obstacle geometries obtained by incorporating realistic LIDAR data. Our approach for payload transport is decomposed into high-level motion planning and low-level trajectory control. For the low-level trajectory tracking, a feedback linearization control is applied to stably track the desired trajectory of the quadcop…
This paper presents a tool for addressing a key component in many algorithms for planning robot trajectories under uncertainty: evaluation of the safety of a robot whose actions are governed by a closed-loop feedback policy near a nominal planned trajectory. We describe an adaptive importance sampling Monte Carlo framework that enables the evaluation of a given control policy for satisfaction of a probabilistic collision avoidance constraint which also provides an associated certificate of accuracy (in the form of a confidence interval). In particular this adaptive technique is well-suited to …
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 r…
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…
Rethinking Gaussian Trajectory Predictors: Calibrated Uncertainty for Safe Planning
Department of Electrical and Computer Engineering, University of Illinois at Urbana-Champaign（伊利诺伊大学厄巴纳-香槟分校电子与计算机工程系）；Department of Mathematical and Computational Sciences, University of Toronto（多伦多大学数学与计算科学系）
IIIT-Hyderabad（海得拉巴国际信息技术学院）；TCS Research（塔塔咨询服务研究院）；University of Michigan（密歇根大学）
This paper proposes a novel controller framework that provides trajectory tracking for an Aerial Manipulator (AM) while ensuring the safe operation of the system under unknown bounded disturbances. The AM considered here is a 2-DOF (degrees-of-freedom) manipulator rigidly attached to a UAV. Our proposed controller structure follows the conventional inner loop PID control for attitude dynamics and an outer loop controller for tracking a reference trajectory. The outer loop control is based on the Model Predictive Control (MPC) with constraints derived using the Barrier Lyapunov Function (BLF) f…
Learning-based control algorithms require data collection with abundant supervision for training. Safe exploration algorithms ensure the safety of this data collection process even when only partial knowledge is available. We present a new approach for optimal motion planning with safe exploration that integrates chance-constrained stochastic optimal control with dynamics learning and feedback control. We derive an iterative convex optimization algorithm that solves an \underline{Info}rmation-cost \underline{S}tochastic \underline{N}onlinear \underline{O}ptimal \underline{C}ontrol problem (Inf…
DENSO Automotive Deutschland GmbH（电装德国汽车有限公司）；Carl von Ossietzky University（卡尔·冯·奥西茨基大学）
Ensuring safe behavior for automated vehicles in unregulated traffic areas poses a complex challenge for the industry. It is an open problem to provide scalable and certifiable solutions to this challenge. We derive a trajectory planner based on model predictive control which interoperates with a monitoring system for pedestrian safety based on cellular automata. The combined planner-monitor system is demonstrated on the example of a narrow indoor parking environment. The system features deterministic behavior, mitigating the immanent risk of black boxes and offering full certifiability. By us…
Capsizing-Guided Trajectory Optimization for Autonomous Navigation with Rough Terrain
School of Control Science and Engineering, Shandong University（控制科学与工程学院，山东大学）；State Key Laboratory of Industrial Control Technology and Institute of Cyber-Systems and Control, Zhejiang University（工业控制技术状态重点实验室和控制系统研究所，浙江大学）
Zhejiang University（浙江大学）；Huzhou Institute, Zhejiang University（浙江大学湖州研究院）
This paper proposes Elastic Tracker, a flexible trajectory planning framework that can deal with challenging tracking tasks with guaranteed safety and visibility. Firstly, an object detection and intension-free motion prediction method is designed. Then an occlusion-aware path finding method is proposed to provide a proper topology. A smart safe flight corridor generation strategy is designed with the guiding path. An analytical occlusion cost is evaluated. Finally, an effective trajectory optimization approach enables to generate a spatio-temporal optimal trajectory within the resultant fligh…
Efficient and Safe Trajectory Planning for Autonomous Agricultural Vehicle Headland Turning in Cluttered Orchard Environments
ZJU-Hangzhou Global Scientific and Technological Innovation Center, Zhejiang University（浙江大学ZJU-杭州全球科技创新中心）；University of California, Davis（加州大学戴维斯分校）；College Of Biosystems Engineering And Food Science, Zhejiang University（浙江大学生物系统工程与食品科学学院）
Self-driving vehicles must be able to act intelligently in diverse and difficult environments, marked by high-dimensional state spaces, a myriad of optimization objectives and complex behaviors. Traditionally, classical optimization and search techniques have been applied to the problem of self-driving; but they do not fully address operations in environments with high-dimensional states and complex behaviors. Recently, imitation learning has been proposed for the task of self-driving; but it is labor-intensive to obtain enough training data. Reinforcement learning has been proposed as a way t…
This paper proposes embedded Gaussian Process Barrier States (GP-BaS), a methodology to safely control unmodeled dynamics of nonlinear system using Bayesian learning. Gaussian Processes (GPs) are used to model the dynamics of the safety-critical system, which is subsequently used in the GP-BaS model. We derive the barrier state dynamics utilizing the GP posterior, which is used to construct a safety embedded Gaussian process dynamical model (GPDM). We show that the safety-critical system can be controlled to remain inside the safe region as long as we can design a controller that renders the B…
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…
SANDO: Safe Autonomous Trajectory Planning for Dynamic Unknown Environments
Departments of Aeronautics and Astronautics, and Mechanical Engineering, Massachusetts Institute of Technology（航空工程与机械工程系，麻省理工学院）；Department of Electronics, Automation, and Communications, Comillas ICAI（电子、自动化与通讯系，Comillas ICAI）
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…
IIIT Hyderabad（印度海得拉巴国际信息技术学院）；University of Tartu（塔尔图大学）
We present CCO-VOXEL: the very first chance-constrained optimization (CCO) algorithm that can compute trajectory plans with probabilistic safety guarantees in real-time directly on the voxel-grid representation of the world. CCO-VOXEL maps the distribution over the distance to the closest obstacle to a distribution over collision-constraint violation and computes an optimal trajectory that minimizes the violation probability. Importantly, unlike existing works, we never assume the nature of the sensor uncertainty or the probability distribution of the resulting collision-constraint violations.…
Fraunhofer Institute for Communication, Information Processing and Ergonomics FKIE（弗劳恩霍夫通信、信息处理与工效学研究所FKIE）；University of Bonn（波恩大学）
Safe navigation of Micro Aerial Vehicles (MAVs) requires not only obstacle-free flight paths according to a static environment map, but also the perception of and reaction to previously unknown and dynamic objects. This implies that the onboard sensors cover the current flight direction. Due to the limited payload of MAVs, full sensor coverage of the environment has to be traded off with flight time. Thus, often only a part of the environment is covered. We present a combined allocentric complete planning and trajectory optimization approach taking these sensor visibility constraints into acco…
Seoul National University（首尔大学）；Automation and Systems Research Institute（自动化与系统研究所）
This paper presents a new online multi-agent trajectory planning algorithm that guarantees to generate safe, dynamically feasible trajectories in a cluttered environment. The proposed algorithm utilizes a linear safe corridor (LSC) to formulate the distributed trajectory optimization problem with only feasible constraints, so it does not resort to slack variables or soft constraints to avoid optimization failure. We adopt a priority-based goal planning method to prevent the deadlock without an additional procedure to decide which robot to yield. The proposed algorithm can compute the trajector…
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…
Barrier-Enhanced Parallel Homotopic Trajectory Optimization for Safety-Critical Autonomous Driving
The Hong Kong University of Science and Technology (Guangzhou)（香港科技大学（广州））；School of Engineering（工程学院）；Robotics and Autonomous Systems Thrust（机器人与自主系统方向）；Division of Emerging Interdisciplinary Areas（新兴交叉领域部门）
Fast Safe Rectangular Corridor-based Online AGV Trajectory Optimization with Obstacle Avoidance
State Key Laboratory of Intelligent Manufacturing Equipment and Technology（智能制造装备与技术国家重点实验室）；School of Mechanical Science and Engineering（机械科学与工程学院）；Huazhong University of Science and Technology（华中科技大学）
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 …
University of Texas at Austin（德克萨斯大学奥斯汀分校）；Mitsubishi Electric Research Laboratories（三菱电机研究实验室）
Consider a robot operating in an uncertain environment with stochastic, dynamic obstacles. Despite the clear benefits for trajectory optimization, it is often hard to keep track of each obstacle at every time step due to sensing and hardware limitations. We introduce the Safely motion planner, a receding-horizon control framework, that simultaneously synthesizes both a trajectory for the robot to follow as well as a sensor selection strategy that prescribes trajectory-relevant obstacles to measure at each time step while respecting the sensing constraints of the robot. We perform the motion pl…
Mean Field Game-Based Interactive Trajectory Planning Using Physics-Inspired Unified Potential Fields
School of Computing Science, University of Glasgow（格拉斯哥大学计算机科学学院）；College of Mechanical Engineering, Chongqing University of Technology（重庆理工大学机械工程学院）；Laboratory of Intelligent Home Appliances, College of Science and Technology, Ningbo University（宁波大学智能家电实验室）
