Trajectory planning and control have historically been separated into two modules in automated driving stacks. Trajectory planning focuses on higher-level tasks like avoiding obstacles and staying on the road surface, whereas the controller tries its best to follow an ever changing reference trajectory. We argue that this separation is (1) flawed due to the mismatch between planned trajectories and what the controller can feasibly execute, and (2) unnecessary due to the flexibility of the model predictive control (MPC) paradigm. Instead, in this paper, we present a unified MPC-based trajectory planning and control scheme that guarantees feasibility with respect to road boundaries, the static and dynamic environment, and enforces passenger comfort constraints. The scheme is evaluated rigorously in a variety of scenarios focused on proving the effectiveness of the optimal control problem (OCP) design and real-time solution methods. The prototype code will be released at https://github.com/WATonomous/control.
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…
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（无线通信与导航研究所，科隆-兰道大学）
Enhanced Trust Region Sequential Convex Optimization for Multi-Drone Thermal Screening Trajectory Planning in Urban Environments
Institute of Automation, Chinese Academy of Science, China（中国科学院自动化研究所）；Beijing Institute of Technology, China（北京理工大学）；Institute of Automation, Chinese Academy of Sciences, China（中国科学院自动化研究所）；Tsinghua University, China（清华大学）
This article presents a multi-robot trajectory planning method which not only guarantees optimization feasibility and but also resolves deadlocks in obstacle-dense environments. The method is proposed via formulating a recursive optimization problem, where a novel safe corridor is generated online to ensure obstacle avoidance in trajectory planning. A dynamic-priority mechanism is combined with the right-hand rule to handle potential deadlocks that are much harder to resolve due to static obstacles. Comparisons with other state-of-the-art results are conducted to validate the improved safety a…
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…
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…
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…
Collision-free mobile robot navigation is an important problem for many robotics applications, especially in cluttered environments. In such environments, obstacles can be static or dynamic. Dynamic obstacles can additionally be interactive, i.e. changing their behavior according to the behavior of other entities. The perception and prediction modules of robotic systems create probabilistic representations and predictions of such environments. In this paper, we propose a novel prediction representation for interactive behaviors of dynamic obstacles. Then, we propose a real-time trajectory plan…
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…
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…
Automation and Control Institute (ACIN), TU Wien（维也纳工业大学自动化与控制研究所）；Center for Vision, Automation and Control, AIT Austrian Institute of Technology（奥地利技术研究所视觉、自动化与控制中心）；Department of Computer Science, University of Liverpool（利物浦大学计算机科学系）
This paper presents a two-step algorithm for online trajectory planning in indoor environments with unknown obstacles. In the first step, sampling-based path planning techniques such as the optimal Rapidly exploring Random Tree (RRT*) algorithm and the Line-of-Sight (LOS) algorithm are employed to generate a collision-free path consisting of multiple waypoints. Then, in the second step, constrained quadratic programming is utilized to compute a smooth trajectory that passes through all computed waypoints. The main contribution of this work is the development of a flexible trajectory planning f…
Technion - Israel Institute of Technology（以色列理工学院）
An important capability of autonomous Unmanned Aerial Vehicles (UAVs) is autonomous landing while avoiding collision with obstacles in the process. Such capability requires real-time local trajectory planning. Although trajectory-planning methods have been introduced for cases such as emergency landing, they have not been evaluated in real-life scenarios where only the surface of obstacles can be sensed and detected. We propose a novel optimization framework using a pre-planned global path and a priority map of the landing area. Several trajectory planning algorithms were implemented and evalu…
RoboCup Middle Size League (RoboCup MSL) provides a standardized testbed for research on mobile robot navigation, multi-robot cooperation, communication and integration via robot soccer competition in which the environment is highly dynamic and adversarial. One of important research topic in such area is kinodynamic motion planning that plan the trajectory of the robot while avoiding obstacles and obeying its dynamics. Kinodynamic motion planning for omnidirectional robot based on kinodynamic-RRT* method is presented in this work. Trajectory tracking control to execute the planned trajectory i…
This paper focuses on automatic guided vehicle (AGV) trajectory planning in the presence of moving obstacles with known but complicated trajectories. In order to achieve good solution precision, optimality and unification, the concerned task should be formulated as an optimal control problem, and then discretized into a nonlinear programming (NLP) problem, which is numerically optimized thereafter. Without a near-feasible or near-optimal initial guess, the NLP-solving process is usually slow. With the purpose of accelerating the NLP solution, a search-based rough planning stage is added to gen…
Continuous formulations of trajectory planning problems have two main benefits. First, constraints are guaranteed to be satisfied at all times. Secondly, dynamic obstacles can be naturally considered with time. This paper introduces a novel B-spline based trajectory optimization method for multi-jointed robots that provides a continuous trajectory with guaranteed continuous constraints satisfaction. At the core of this method, B-spline basic operations, like addition, multiplication, and derivative, are rigorously defined and applied for problem formulation. B-spline unique characteristics, su…
Loughborough University（拉夫堡大学）；Defence Science and Technology Laboratory（国防科学技术实验室）
The use of autonomous vehicles for chemical source localisation is a key enabling tool for disaster response teams to safely and efficiently deal with chemical emergencies. Whilst much work has been performed on source localisation using autonomous systems, most previous works have assumed an open environment or employed simplistic obstacle avoidance, separate to the estimation procedure. In this paper, we explore the coupling of the path planning task for both source term estimation and obstacle avoidance in a holistic framework. The proposed system intelligently produces potential gas sampli…
The goal of this paper is to develop a continuous optimization-based refinement of the reference trajectory to 'push it out' of the obstacle-occupied space in the global phase for Multi-rotor Aerial Vehicles in unknown environments. Our proposed approach comprises two planners: a global planner and a local planner. The global planner refines the initial reference trajectory when the trajectory goes either through an obstacle or near an obstacle and lets the local planner calculate a near-optimal control policy. The global planner comprises two convex programming approaches: the first one helps…
Obstacle avoidance between polytopes is a challenging topic for optimal control and optimization-based trajectory planning problems. Existing work either solves this problem through mixed-integer optimization, relying on simplification of system dynamics, or through model predictive control with dual variables using distance constraints, requiring long horizons for obstacle avoidance. In either case, the solution can only be applied as an offline planning algorithm. In this paper, we exploit the property that a smaller horizon is sufficient for obstacle avoidance by using discrete-time control…
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（华中科技大学）
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…
Stanford University（斯坦福大学）；Carnegie Mellon University（卡内基梅隆大学）；University of Washington（华盛顿大学）
We present an algorithm for obtaining an optimal control policy for hybrid dynamical systems in cluttered environments. To the best of our knowledge, this is the first attempt to have a locally optimal solution for this specific problem setting. Our approach extends an optimal control algorithm for hybrid dynamical systems in the obstacle-free case to environments with obstacles. Our method does not require any preset mode sequence or heuristics to prune the exponential search of mode sequences. By first solving the relaxed problem of getting an obstacle-free, dynamically feasible trajectory a…
In unstructured urban canals, regulation-aware interactions with other vessels are essential for collision avoidance and social compliance. In this paper, we propose a regulations aware motion planning framework for Autonomous Surface Vessels (ASVs) that accounts for dynamic and static obstacles. Our method builds upon local model predictive contouring control (LMPCC) to generate motion plans satisfying kino-dynamic and collision constraints in real-time while including regulation awareness. To incorporate regulations in the planning stage, we propose a cost function encouraging compliance wit…
In order to safely and efficiently collaborate with humans, industrial robots need the ability to alter their motions quickly to react to sudden changes in the environment, such as an obstacle appearing across a planned trajectory. In Realtime Motion Planning, obstacles are detected in real time through a vision system, and new trajectories are planned with respect to the current positions of the obstacles, and immediately executed on the robot. Existing realtime motion planners, however, lack the smoothing post-processing step -- which are crucial in sampling-based motion planning -- resultin…
Intelligent Transportation System Research Center, Wuhan University of Technology（武汉理工大学智能交通系统研究中心）；National Engineering Research Center for Water Transport Safety（国家水运安全工程技术研究中心）；School of Navigation, Wuhan University of Technology（武汉理工大学航运学院）
Aiming at an obstacle avoidance problem with dynamic constraints for Unmanned Surface Vehicle (USV), a method based on Circle Grid Trajectory Cell (CGTC) is proposed. Firstly, the ship model and standardization rules are constructed to develop and constrain the trajectory, respectively. Secondly, by analyzing the properties of the circle grid, the circle grid tree is produced to guide the motion of the USV. Then, the kinematics and dynamics of the USV are considered through the on-line trajectory generator by designing a relational function that links the rudder angle, heading angle, and the c…
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（教育部飞行器动力学与控制重点实验室）
Luleå University of Technology（吕勒奥理工大学）；Jet Propulsion Laboratory California Institute of Technology（加州理工学院喷气推进实验室）
This article proposes a Novel Nonlinear Model Predictive Control (NMPC) for navigation and obstacle avoidance of an Unmanned Aerial Vehicle (UAV). The proposed NMPC formulation allows for a fully parametric obstacle trajectory, while in this article we apply a classification scheme to differentiate between different kinds of trajectories to predict future obstacle positions. The trajectory calculation is done from an initial condition, and fed to the NMPC as an additional input. The solver used is the nonlinear, non-convex solver Proximal Averaged Newton for Optimal Control (PANOC) and its ass…
Navigating mobile robots through environments shared with humans is challenging. From the perspective of the robot, humans are dynamic obstacles that must be avoided. These obstacles make the collision-free space nonconvex, which leads to two distinct passing behaviors per obstacle (passing left or right). For local planners, such as receding-horizon trajectory optimization, each behavior presents a local optimum in which the planner can get stuck. This may result in slow or unsafe motion even when a better plan exists. In this work, we identify trajectories for multiple locally optimal drivin…
Obstacle Avoidance using Dynamic Movement Primitives and Reinforcement Learning
Institute of Industrial and Control Engineering, Universitat Politècnica de Catalunya（工业与控制工程研究所，巴塞罗那理工大学）；Department of Computer Science, University of Innsbruck（计算机科学系，因斯布鲁克大学）；Roboception GmbH（Roboception公司）
Robust Dynamic Control Barrier Function Based Trajectory Planning for Mobile Manipulator
School of Mechanical Engineering and Automation, Harbin Institute of Technology Shenzhen（哈尔滨工业大学（深圳）机械工程与自动化学院）；Graduated School of Information Science and Technology, Osaka University（大阪大学信息科学与技术研究生院）
Zhejiang University（浙江大学）；Huzhou Institute of Zhejiang University（浙江大学湖州研究院）；Science and Technology on Complex System Control and Intelligent Agent Cooperation Laboratory（复杂系统控制与智能协同技术重点实验室）
Efficient trajectory generation in complex dynamic environments remains an open problem in the unmanned surface vehicle (USV). The perception of the USV is usually interfered with by the swing of the hull and the ambient weather, making it challenging to plan the optimal USV trajectories. In this paper, a cooperative trajectory planning algorithm for the coupled USV-UAV system is proposed to ensure that USV can execute a safe and smooth path in the process of autonomous advance in multi-obstacle maps. Specifically, the unmanned aerial vehicle (UAV) plays the role of a flight sensor, providing …
Zhejiang University（浙江大学）；Huzhou Institute, Zhejiang University（浙江大学湖州研究院）
For aerial swarms, navigation in a prescribed formation is widely practiced in various scenarios. However, the associated planning strategies typically lack the capability of avoiding obstacles in cluttered environments. To address this deficiency, we present an optimization-based method that ensures collision-free trajectory generation for formation flight. In this paper, a novel differentiable metric is proposed to quantify the overall similarity distance between formations. We then formulate this metric into an optimization framework, which achieves spatial-temporal planning using polynomia…
SRM Institute of Science and Technology（SRM科学技术研究所）
Multi robot systems have the potential to be utilized in a variety of applications. In most of the previous works, the trajectory generation for multi robot systems is implemented in known environments. To overcome that we present an online trajectory optimization algorithm that utilizes communication of robots' current states to account to the other robots while using local object based maps for identifying obstacles. Based upon this data, we predict the trajectory expected to be traversed by the robots and utilize that to avoid collisions by formulating regions of free space that the robot c…
Indian Institute of Technology Madras（印度理工学院马德拉斯分校）；University of Maryland, College Park（马里兰大学帕克分校）
In this paper, we present an online motion planning algorithm (3D-OGSE) for generating smooth, collision-free trajectories over multiple planning iterations for 3-D agents operating in an unknown obstacle-cluttered 3-D environment. Our approach constructs a safe-region, termed 'generalized shape', at each planning iteration, which represents the obstacle-free region based on locally-sensed environment information. A collision-free path is computed by sampling points in the generalized shape and is used to generate a smooth, time-parametrized trajectory by minimizing snap. The generated traject…
College of Control Science and Technology, Zhejiang University（浙江大学控制科学与工程学院）；Institute of Cyber-Systems and Control, Zhejiang University（浙江大学智能系统与控制研究所）
In this paper, we propose an efficient and accurate method for autonomous surface vehicles to generate a smooth and collision-free trajectory considering its dynamics constraints. We decouple the trajectory planning problem as a front-end feasible path searching and a back-end kinodynamic trajectory optimization. Firstly, we model the type of two-thrusts under-actuated surface vessel. Then we adopt a sampling-based path searching to find an asymptotic optimal path through the obstacle-surrounding environment and extract several waypoints from it. We apply a numerical optimization method in the…
Stanford University（斯坦福大学）；University of Illinois at Urbana-Champaign（伊利诺伊大学厄巴纳-香槟分校）
A key challenge for autonomous driving is safe trajectory planning in cluttered, urban environments with dynamic obstacles, such as pedestrians, bicyclists, and other vehicles. A reliable prediction of the future environment, including the behavior of dynamic agents, would allow planning algorithms to proactively generate a trajectory in response to a rapidly changing environment. We present a novel framework that predicts the future occupancy state of the local environment surrounding an autonomous agent by learning a motion model from occupancy grid data using a neural network. We take advan…
