Stochastic Model Predictive Control has proved to be an efficient method to plan trajectories in uncertain environments, e.g., for autonomous vehicles. Chance constraints ensure that the probability of collision is bounded by a predefined risk parameter. However, considering chance constraints in an optimization problem can be challenging and computationally demanding. In this paper, we present a grid-based Stochastic Model Predictive Control approach. This approach allows to determine a simple deterministic reformulation of the chance constraints and reduces the computational effort, while considering the stochastic nature of the environment. Within the proposed method, we first divide the environment into a grid and, for each predicted step, assign each cell a probability value, which represents the probability that this cell will be occupied by surrounding vehicles. Then, the probabilistic grid is transformed into a binary grid of admissible and inadmissible cells by applying a threshold, representing a risk parameter. Only cells with an occupancy probability lower than the threshold are admissible for the controlled vehicle. Given the admissible cells, a convex hull is generated, which can then be used for trajectory planning. Simulations of an autonomous driving highway scenario show the benefits of the proposed grid-based Stochastic Model Predictive Control method.
This paper addresses the trajectory planning for multiple autonomous underwater vehicles (AUVs) in strong waves that can disturb the AUVs' trajectory tracking ability and cause obstacle and inter-vehicle collisions. A novel approach based on Hamilton-Jacobi differential game formulation and model predictive control is proposed to generate safety-guaranteed trajectories for AUV systems considering wave disturbance that is either time-varying state-independent or time-varying state-dependent. This approach plans safety-guaranteed trajectories for multiple AUVs and considers the shapes of AUVs, w…
Trajectory planning for connected and automated vehicles (CAVs) has the potential to improve operational efficiency and vehicle fuel economy in traffic systems. Despite abundant studies in this research area, most of them only consider trajectory planning in the longitudinal dimension or assume the fully CAV environment. This study proposes an approach to the decentralized planning of CAV trajectories at an isolated signalized intersection under the mixed traffic environment, which consists of connected and human-driven vehicles (CHVs) and CAVs. A bi-level optimization model is formulated base…
Motion planning for merging scenarios accounting for measurement and prediction uncertainties is a major challenge on the way to autonomous driving. Classical methods subdivide the motion planning into behavior and trajectory planning, thus narrowing down the solution set. Hence, incomplex merging scenarios, no suitable solution might be found. In this work, we present a planning scheme that solves behavior and trajectory planning together by exploring all possible decision options. A safety strategy is implemented and the risk of violating a safety constraint is minimized as well as the jerk …
Future unmanned aerial vehicles (drones) will be shared by multiple users and will have to operate in conditions where their fully-autonomous function is required. Calculation of a drones trajectory will be important but optimal trajectories cannot be calculated unless mass and flight speed are taken into account. This article presents the case for on-drone trajectory planning in a multi-user dynamic payload mass scenario, allowing a drone to calculate its trajectory with no need for ground control communication. We formulate and investigate on-drone trajectory planning under variable payload …
In this paper we propose a technique that assigns obstacles to clusters used for collision avoidance via Mixed-Integer Programming. This strategy enables a reduction in the number of binary variables used for collision avoidance, thus entailing a decrease in computational cost, which has been a hindrance to the application of Model Predictive Control approaches with Mixed-Integer Programming formulations in real-time. Moreover, the assignment of obstacles to clusters and the sizes of the clusters are decided within the same optimization problem that performs the trajectory planning, thus yield…
Abstract: we present a framework for robust autonomous driving motion planning system in urban environments which includes trajectory refinement, trajectory interpolation, avoidance of static and dynamic obstacles, and trajectory tracking. Given road centerline, our approach smoother the original line via cubic spline. Fifth order Bezier curve is then used to generate more human-like trajectories that guarantee at least second order continuity and curvature continuity. Dynamic trajectory planning task is decoupled into lateral spatial and longitudinal velocity planning problems. A bunch of can…
Shanghai Automotive Industry Corporation, Ltd.（上海汽车工业股份有限公司）
This paper presents an automatic merging algorithm for autonomous driving vehicles, which decouples the specific motion planning problem into a Dual-Layer Automatic Merge Planning (DL_AMP) and a Descent-Based Trajectory Optimization (DBTO). This work leads to great improvements in finding the best merge opportunity, lateral and longitudinal merge planning and control, trajectory postprocessing and driving comfort.
In this paper, we study spline trajectory generation via the solution of two optimisation problems: (i) a quadratic program (QP) with linear equality constraints and (ii) a nonlinear and nonconvex optimisation program. We propose an efficient algorithm to solve (i), which we then leverage to use in an iterative algorithm to solve (ii). Both the first algorithm and each iteration of the second algorithm have linear computational complexity in the number of spline segments. The scaling of each algorithm is such that we are able to solve the two problems faster than state-of-the-art methods and i…
This paper develops a sequential convex programming approach for Mars entry trajectory planning by range discretization. To improve the accuracy of numerical integration, the range of entry trajectory is selected as the independent variable rather than time or energy. A dilation factor is employed to normalize the entry dynamics and integration interval of the performance index so that the difficult free-final-time programming problem can be converted to a fixed-final-range optimization problem. The bank angle rate with respect to the range is introduced as the new control input in order to de…
This paper presents a new efficient algorithm which guarantees a solution for a class of multi-agent trajectory planning problems in obstacle-dense environments. Our algorithm combines the advantages of both grid-based and optimization-based approaches, and generates safe, dynamically feasible trajectories without suffering from an erroneous optimization setup such as imposing infeasible collision constraints. We adopt a sequential optimization method with \textit{dummy agents} to improve the scalability of the algorithm, and utilize the convex hull property of Bernstein and relative Bernstein…
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 …
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…
We tackle the problem of trajectory planning in an environment comprised of a set of obstacles with uncertain time-varying locations. The uncertainties are modeled using widely accepted Gaussian distributions, resulting in a chance-constrained program. Contrary to previous approaches however, we do not assume perfect knowledge of the moments of the distribution, and instead estimate them through finite samples available from either sensors or past data. We derive tight concentration bounds on the error of these estimates to sufficiently tighten the chance-constraint program. As such, we provid…
In this paper, we propose a novel framework of air traffic management (ATM). The framework is in particular characterized by the trajectory planning of weakly supervised aircraft; the air traffic control (ATC) does not completely determine the trajectory of each aircraft unlike conventional planning methods, but determines allowable safe sets of trajectories. ATC requests pilots to select their own trajectories from the sets, and the pilots determine ones by pursuing their own aims. For example, the selection can be based on pilot preferences and airline strategies. This two stage ATM system c…
We develop an optimization-based framework for joint real-time trajectory planning and feedback control of feedback-linearizable systems. To achieve this goal, we define a target trajectory as the optimal solution of a time-varying optimization problem. In general, however, such trajectory may not be feasible due to , e.g., nonholonomic constraints. To solve this problem, we design a control law that generates feasible trajectories that asymptotically converge to the target trajectory. More precisely, for systems that are (dynamic) full-state linearizable, the proposed control law implicitly t…
The main contribution of this paper is a novel method for planning globally optimal trajectories for dynamical systems subject to polygonal constraints. The proposed method is a hybrid trajectory planning approach, which combines graph search, i.e. a discrete roadmap method, with convex optimization, i.e. a complete path method. Contrary to past approaches, which have focused on using simple obstacle approximations, or sub-optimal spatial discretizations, our approach is able to use the exact geometry of polygonal constraints in order to plan optimal trajectories. The performance and flexibili…
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…
Although Trajectory Optimization (TO) is one of the most powerful motion planning tools, it suffers from expensive computational complexity as a time horizon increases in cluttered environments. It can also fail to converge to a globally optimal solution. In this paper, we present Lazy Trajectory Optimization (LTO) that unifies local short-horizon TO and global Graph-Search Planning (GSP) to generate a long-horizon global optimal trajectory. LTO solves TO with the same constraints as the original long-horizon TO with improved time complexity. We also propose a TO-aware cost function that can b…
MC-Swarm: Minimal-Communication Multi-Agent Trajectory Planning and Deadlock Resolution for Quadrotor Swarm
AI Institute of Seoul National University（首尔国立大学人工智能研究所）；Carnegie Mellon University（卡内基梅隆大学）；Department of Mechanical System Design Engineering, Seoul National University of Science and Technology (SEOULTECH)（首尔科学技术大学机械系统设计工程系）
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…
We propose a method for performing automatic docking of a small autonomous surface vehicle (ASV) by interconnecting an optimization-based trajectory planner with a dynamic positioning (DP) controller for trajectory tracking. The trajectory planner provides collision-free trajectories by considering a map with static obstacles, and produces feasible trajectories through inclusion of a mathematical model of the ASV and its actuators. The DP controller tracks the time-parametrized position, velocity and acceleration produced by the trajectory planner using proportional-integral-derivative feedbac…
Trajectory planning at high velocities and at the handling limits is a challenging task. In order to cope with the requirements of a race scenario, we propose a far-sighted two step, multi-layered graph-based trajectory planner, capable to run with speeds up to 212~km/h. The planner is designed to generate an action set of multiple drivable trajectories, allowing an adjacent behavior planner to pick the most appropriate action for the global state in the scene. This method serves objectives such as race line tracking, following, stopping, overtaking and a velocity profile which enables a handl…
Institute for AI Industry Research (AIR), Tsinghua University（清华大学人工智能产业研究院（AIR））；JD iCity & JD Intelligent Cities Research, JD Technology（京东科技京东城市与京东智能城市研究院）
The recent offline reinforcement learning (RL) studies have achieved much progress to make RL usable in real-world systems by learning policies from pre-collected datasets without environment interaction. Unfortunately, existing offline RL methods still face many practical challenges in real-world system control tasks, such as computational restriction during agent training and the requirement of extra control flexibility. The model-based planning framework provides an attractive alternative. However, most model-based planning algorithms are not designed for offline settings. Simply combining …
A novel control & software architecture using ROS C++ is introduced for object interception by a UAV with a mounted depth camera and no external aid. Existing work in trajectory prediction focused on the use of off-board tools like motion capture rooms to intercept thrown objects. The present study designs the UAV architecture to be completely on-board capable of object interception with the use of a depth camera and point cloud processing. The architecture uses an iterative trajectory prediction algorithm for non-propelled objects like a ping-pong ball. A variety of path planning approaches t…
Automation and Control Institute (ACIN), TU Wien（维也纳工业大学自动化与控制研究所（ACIN））；Center for Vision, Automation and Control, AIT Austrian Institute of Technology（奥地利技术研究所（AIT）视觉、自动化与控制中心）
In this paper, a sampling-based trajectory planning algorithm for a laboratory-scale 3D gantry crane in an environment with static obstacles and subject to bounds on the velocity and acceleration of the gantry crane system is presented. The focus is on developing a fast motion planning algorithm for differentially flat systems, where intermediate results can be stored and reused for further tasks, such as replanning. The proposed approach is based on the informed optimal rapidly exploring random tree algorithm (informed RRT*), which is utilized to build trajectory trees that are reused for rep…
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（土木与环境工程学院）
Stanford University（斯坦福大学）；Toyota Research Institute（丰田研究院）
Reasoning about human motion is a core component of modern human-robot interactive systems. In particular, one of the main uses of behavior prediction in autonomous systems is to inform robot motion planning and control. However, a majority of planning and control algorithms reason about system dynamics rather than the predicted agent tracklets (i.e., ordered sets of waypoints) that are commonly output by trajectory forecasting methods, which can hinder their integration. Towards this end, we propose Mixtures of Affine Time-varying Systems (MATS) as an output representation for trajectory fore…
Norwegian University of Science and Technology (NTNU)（挪威科技大学）
We consider warm-started optimized trajectory planning for autonomous surface vehicles (ASVs) by combining the advantages of two types of planners: an A* implementation that quickly finds the shortest piecewise linear path, and an optimal control-based trajectory planner. A nonlinear 3-degree-of-freedom underactuated model of an ASV is considered, along with an objective functional that promotes energy-efficient and readily observable maneuvers. The A* algorithm is guaranteed to find the shortest piecewise linear path to the goal position based on a uniformly decomposed map. Dynamic informatio…
Providing smooth reference trajectories can effectively increase performance and accuracy of tracking control applications while overshoot and unwanted vibrations are reduced. Trajectory planning computations can often be simplified significantly by transforming the system dynamics into decoupled integrator chains using methods such as feedback linearization, differential flatness or the controller canonical form. We present an efficient method to plan time optimal trajectories for integrator chains subject to derivative bound constraints. Therefore, an algebraic precomputation algorithm formu…
University of Rijeka（里耶卡大学）；University of Southern Denmark（南丹麦大学）
The application of autonomous UAVs to infrastructure inspection tasks provides benefits in terms of operation time reduction, safety, and cost-effectiveness. This paper presents trajectory planning for three-dimensional autonomous multi-UAV volume coverage and visual inspection of infrastructure based on the Heat Equation Driven Area Coverage (HEDAC) algorithm. The method generates trajectories using a potential field and implements distance fields to prevent collisions and to determine UAVs' camera orientation. It successfully achieves coverage during the visual inspection of complex structur…
