Safe trajectory planning for high-performance automated vehicles in an environment with both static and moving obstacles is a challenging problem. Part of the challenge is developing a formulation that can be solved in real-time while including the following set of specifications: minimum time-to-goal, a dynamic vehicle model, minimum control effort, both static and moving obstacle avoidance, simultaneous optimization of speed and steering, and a short execution horizon. This paper presents a nonlinear model predictive control-based trajectory planning formulation, tailored for a large, high-speed unmanned ground vehicle, that includes the above set of specifications. This paper also evaluates NLOptControl's ability to solve this formulation in real-time in conjunction with the KNITRO nonlinear programming problem solver; NLOptControl is our open-source, direct-collocation based, optimal control problem solver. This formulation is tested with various sets of the specifications. In particular, a parametric study relating execution horizon and obstacle speed, indicates that the moving obstacle avoidance specification is not needed for safety when the planner has a small execution horizon ($\leq0.375\;s$) and the obstacles are moving slowly ($\leq2.11\frac{m}{s}$). However, a moving obstacle avoidance specification is needed when the obstacles are moving faster, and this specification improves the overall safety by a factor of $6.73$ ($p=2.2\times10^{-16}$) without, in most cases, increasing the solve-times. Overall, the results indicate that (1) safe trajectory planners for high-performance automated vehicles should include the entire set of specifications mentioned above, unless a static or low-speed environment permits a less comprehensive planner; and (2) NLOptControl can solve the formulation in real-time.
Energy efficiency and safety are two critical objectives for marine vehicles operating in environments with obstacles, and they generally conflict with each other. In this paper, we propose a novel online motion planning method of marine vehicles which can make trade-offs between the two design objectives based on the framework of economic model predictive control (EMPC). Firstly, the feasible trajectory with the most safety margin is designed and utilized as tracking reference. Secondly, the EMPC-based receding horizon motion planning algorithm is designed, in which the practical consumed ene…
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…
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…
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…
Within a robot autonomy stack, the planner and controller are typically designed separately, and serve different purposes. As such, there is often a diffusion of responsibilities when it comes to ensuring safety for the robot. We propose that a planner and controller should share the same interpretation of safety but apply this knowledge in a different yet complementary way. To achieve this, we use Hamilton-Jacobi (HJ) reachability theory at the planning level to provide the robot planner with the foresight to avoid entering regions with possible inevitable collision. However, this alone does …
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)（首尔科学技术大学机械系统设计工程系）
Neural network-based driving planners have shown great promises in improving task performance of autonomous driving. However, it is critical and yet very challenging to ensure the safety of systems with neural network based components, especially in dense and highly interactive traffic environments. In this work, we propose a safety-driven interactive planning framework for neural network-based lane changing. To prevent over conservative planning, we identify the driving behavior of surrounding vehicles and assess their aggressiveness, and then adapt the planned trajectory for the ego vehicle …
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…
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…
Motion planning in environments with multiple agents is critical to many important autonomous applications such as autonomous vehicles and assistive robots. This paper considers the problem of motion planning, where the controlled agent shares the environment with multiple uncontrolled agents. First, a predictive model of the uncontrolled agents is trained to predict all possible trajectories within a short horizon based on the scenario. The prediction is then fed to a motion planning module based on model predictive control. We proved generalization bound for the predictive model using three …
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…
