Francesco Borrelli

Jacopo Guanetti, Yeojun Kim, Francesco Borrelli

Autonomous driving technology pledges safety, convenience, and energy efficiency. Challenges include the unknown intentions of other road users: communication between vehicles and with the road infrastructure is a possible approach to enhance awareness and enable cooperation. Connected and automated vehicles (CAVs) have the potential to disrupt mobility, extending what is possible with driving automation and connectivity alone. Applications include real-time control and planning with increased awareness, routing with micro-scale traffic information, coordinated platooning using traffic signals information, eco-mobility on demand with guaranteed parking. This paper introduces a control and planning architecture for CAVs, and surveys the state of the art on each functional block therein; the main focus is on techniques to improve energy efficiency. We provide an overview of existing algorithms and their mutual interactions, we present promising optimization-based approaches to CAVs control and identify future challenges.

Ugo Rosolia, Francesco Borrelli

A Learning Model Predictive Controller (LMPC) for iterative tasks is presented. The controller is reference-free and is able to improve its performance by learning from previous iterations. A safe set and a terminal cost function are used in order to guarantee recursive feasibility and non-increasing performance at each iteration. The paper presents the control design approach, and shows how to recursively construct terminal set and terminal cost from state and input trajectories of previous iterations. Simulation results show the effectiveness of the proposed control logic.

Sangjae Bae, Yeojun Kim, Jacopo Guanetti et al.

This paper presents the design and implementation results of an ecological adaptive cruise controller (ECO-ACC) which exploits driving automation and connectivity. The controller avoids front collisions and traffic light violations, and is designed to reduce the energy consumption of connected automated vehicles by utilizing historical and real-time signal phase and timing data of traffic lights that adapt to the current traffic conditions. We propose an optimization-based generation of a reference velocity, and a velocity-tracking model predictive controller that avoids front collisions and violations. We present an experimental setup encompassing the real vehicle and controller in the loop, and an environment simulator in which the traffic flow and the traffic light patterns are calibrated on real-world data. We present and analyze simulation and experimental results, finding a significant potential for energy consumption reduction, even in the presence of traffic.

Yi-Wen Liao, Francesco Borrelli

Robust estimation of vehicle sideslip angle is essential for stability control applications. However, the direct measurement of sideslip angle is expensive for production vehicles. This paper presents a novel sideslip estimation algorithm which relies only on sensors available on passenger and commercial vehicles. The proposed method uses both kinematics and dynamics vehicle models to construct extended Kalman filter observers. The estimate relies on the results provided from the dynamics model observer where the tire cornering stiffness parameters are updated using the information provided from the kinematics model observer. The stability property of the proposed algorithm is discussed and proven. Finally, multiple experimental tests are conducted to verify its performance in practice. The results show that the proposed approach provides smooth and accurate sideslip angle estimation. In addition, our novel algorithm provides reliable estimates of bank angles and lateral acceleration sensor bias.

Monimoy Bujarbaruah, Xiaojing Zhang, Ugo Rosolia et al.

This paper proposes an Adaptive Learning Model Predictive Control strategy for uncertain constrained linear systems performing iterative tasks. The additive uncertainty is modeled as the sum of a bounded process noise and an unknown constant offset. As new data becomes available, the proposed algorithm iteratively adapts the believed domain of the unknown offset after each iteration. An MPC strategy robust to all feasible offsets is employed in order to guarantee recursive feasibility. We show that the adaptation of the feasible offset domain reduces conservatism of the proposed strategy, compared to classical robust MPC strategies. As a result, the controller performance improves. Performance is measured in terms of following trajectories with lower associated costs at each iteration. Numerical simulations highlight the main advantages of the proposed approach.

Monimoy Bujarbaruah, Xiaojing Zhang, H. Eric Tseng et al.

This paper proposes an Adaptive Robust Model Predictive Control strategy for lateral control in lane keeping problems, where we continuously learn an unknown, but constant steering angle offset present in the steering system. Longitudinal velocity is assumed constant. The goal is to minimize the outputs, which are distance from lane center line and the steady state heading angle error, while satisfying respective safety constraints. We do not assume perfect knowledge of the vehicle lateral dynamics model and estimate and adapt in real-time the maximum possible bound of the steering angle offset from data using a robust Set Membership Method based approach. Our approach is even well-suited for scenarios with sharp curvatures on high speed, where obtaining a precise model bias for constrained control is difficult, but learning from data can be helpful. We ensure persistent feasibility using a switching strategy during change of lane curvature. The proposed methodology is general and can be applied to more complex vehicle dynamics problems.

Ugo Rosolia, Francesco Borrelli

We present a sample-based Learning Model Predictive Controller (LMPC) for constrained uncertain linear systems subject to bounded additive disturbances. The proposed controller builds on earlier work on LMPC for deterministic systems. First, we introduce the design of the safe set and value function used to guarantee safety and performance improvement. Afterwards, we show how these quantities can be approximated using noisy historical data. The effectiveness of the proposed approach is demonstrated on a numerical example. We show that the proposed LMPC is able to safely explore the state space and to iteratively improve the worst-case closed-loop performance, while robustly satisfying state and input constraints.

Monimoy Bujarbaruah, Xiaojing Zhang, Francesco Borrelli

This paper proposes an adaptive stochastic Model Predictive Control (MPC) strategy for stable linear time invariant systems in the presence of bounded disturbances. We consider multi-input multi-output systems that can be expressed by a finite impulse response model, whose parameters we estimate using a linear Recursive Least Squares algorithm. Building on the work of [1],[2], our approach is able to handle hard input constraints and probabilistic output constraints. By using tools from distributionally robust optimization, we formulate our MPC design task as a convex optimization problem that can be solved using existing tools. Furthermore, we show that our adaptive stochastic MPC algorithm is persistently feasible. The efficacy of the developed algorithm is demonstrated in a numerical example and the results are compared with the adaptive robust MPC algorithm of [2].

Sangjae Bae, Yongkeun Choi, Yeojun Kim et al.

This paper presents the design of an ecological adaptive cruise controller (ECO-ACC) for a plug-in hybrid vehicle (PHEV) which exploits automated driving and connectivity. Most existing papers for ECO-ACC focus on a short-sighted control scheme. A two-level control framework for long-sighted ECO-ACC was only recently introduced. However, that work is based on a deterministic traffic signal phase and timing (SPaT) over the entire route. In practice, connectivity with traffic lights may be limited by communication range, e.g. just one upcoming traffic light. We propose a two-level receding-horizon control framework for long-sighted ECO-ACC that exploits deterministic SPaT for the upcoming traffic light, and utilizes historical SPaT for other traffic lights within a receding control horizon. We also incorporate a powertrain control mechanism to enhance PHEV energy prediction accuracy. Hardware-in-the-loop simulation results validate the energy savings of the receding-horizon control framework in various traffic scenarios.

Xu Shen, Matthew Lacayo, Nidhir Guggilla et al.

The problem of multimodal intent and trajectory prediction for human-driven vehicles in parking lots is addressed in this paper. Using models designed with CNN and Transformer networks, we extract temporal-spatial and contextual information from trajectory history and local bird's eye view (BEV) semantic images, and generate predictions about intent distribution and future trajectory sequences. Our methods outperform existing models in accuracy, while allowing an arbitrary number of modes, encoding complex multi-agent scenarios, and adapting to different parking maps. To train and evaluate our method, we present the first public 4K video dataset of human driving in parking lots with accurate annotation, high frame rate, and rich traffic scenarios.

Ugo Rosolia, Xiaojing Zhang, Francesco Borrelli

A data-based policy for iterative control task is presented. The proposed strategy is model-free and can be applied whenever safe input and state trajectories of a system performing an iterative task are available. These trajectories, together with a user-defined cost function, are exploited to construct a piecewise affine approximation to the value function. Approximated value functions are then used to evaluate the control policy by solving a linear program. We show that for linear system subject to convex cost and constraints, the proposed strategy guarantees closed-loop constraint satisfaction and performance bounds on the closed-loop trajectory. We evaluate the proposed strategy in simulations and experiments, the latter carried out on the Berkeley Autonomous Race Car (BARC) platform. We show that the proposed strategy is able to reduce the computation time by one order of magnitude while achieving the same performance as our model-based control algorithm.

Yi-Wen Liao, Selina Pan, Francesco Borrelli et al.

This paper proposes a new adaptation methodology to find the control inputs for a class of nonlinear systems with time-varying bounded uncertainties. The proposed method does not require any prior knowledge of the uncertainties including their bounds. The main idea is developed under the structure of adaptive sliding mode control; an update law decreases the gain inside and increases the gain outside a vicinity of the sliding surface. The semi-global stability of the closed-loop system and the adaptation error are guaranteed by Lyapunov theory. The simulation results show that the proposed adaptation methodology can reduce the magnitude of the controller gain to the minimum possible value and smooth out the chattering.

Hansung Kim, Siddharth H. Nair, Francesco Borrelli

We propose a hierarchical architecture designed for scalable real-time Model Predictive Control (MPC) in complex, multi-modal traffic scenarios. This architecture comprises two key components: 1) RAID-Net, a novel attention-based Recurrent Neural Network that predicts relevant interactions along the MPC prediction horizon between the autonomous vehicle and the surrounding vehicles using Lagrangian duality, and 2) a reduced Stochastic MPC problem that eliminates irrelevant collision avoidance constraints, enhancing computational efficiency. Our approach is demonstrated in a simulated traffic intersection with interactive surrounding vehicles, showcasing a 12x speed-up in solving the motion planning problem. A video demonstrating the proposed architecture in multiple complex traffic scenarios can be found here: https://youtu.be/-pRiOnPb9_c. GitHub: https://github.com/MPC-Berkeley/hmpc_raidnet

Spencer Schutz, Charlott Vallon, Francesco Borrelli

In adaptive-sampling control, the control frequency can be adjusted during task execution. Ensuring that these on-the-fly changes do not jeopardize the safety of the system being controlled requires careful attention. We introduce robust M-step hold model predictive control (MPC) to address this. This MPC formulation provides robust constraint satisfaction for an uncertain discrete-time system model with a fixed sampling time subject to an adaptable multi-step input hold (referred to as M-step hold). We show how to ensure recursive feasibility of the MPC utilizing M-step hold extensions of robust invariant sets, and demonstrate how to use our framework to enable safe adaptive-sampling control via the online selection of M. We evaluate the utility of the robust M-step hold MPC formulation in a cruise control example.

Jeffrey Ichnowski, Paras Jain, Bartolomeo Stellato et al.

First-order methods for quadratic optimization such as OSQP are widely used for large-scale machine learning and embedded optimal control, where many related problems must be rapidly solved. These methods face two persistent challenges: manual hyperparameter tuning and convergence time to high-accuracy solutions. To address these, we explore how Reinforcement Learning (RL) can learn a policy to tune parameters to accelerate convergence. In experiments with well-known QP benchmarks we find that our RL policy, RLQP, significantly outperforms state-of-the-art QP solvers by up to 3x. RLQP generalizes surprisingly well to previously unseen problems with varying dimension and structure from different applications, including the QPLIB, Netlib LP and Maros-Meszaros problems. Code for RLQP is available at https://github.com/berkeleyautomation/rlqp.

Roya Firoozi, Xiaojing Zhang, Francesco Borrelli

Advances in vehicular communication technologies are expected to facilitate cooperative driving. Connected and Automated Vehicles (CAVs) are able to collaboratively plan and execute driving maneuvers by sharing their perceptual knowledge and future plans. In this paper, an architecture for autonomous navigation of tight multi-lane platoons travelling on public roads is presented. Using the proposed approach, CAVs are able to form single or multi-lane platoons of various geometrical configurations. They are able to reshape and adjust their configurations according to changes in the environment. The proposed architecture consists of two main components: an offline motion planner system and an online hierarchical control system. The motion planner uses an optimization-based approach for cooperative formation and reconfiguration in tight spaces. A constrained optimization scheme is used to plan smooth, dynamically feasible and collision-free trajectories for all the vehicles within the platoon. The paper addresses online computation limitations by employing a family of maneuvers precomputed offline and stored on a look-up table on the vehicles. The online hierarchical control system is composed of three levels: a traffic operation system (TOS), a decision-maker, and a path-follower. The TOS determines the desired platoon reconfiguration. The decision-maker checks the feasibility of the reconfiguration plan. The reconfiguration maneuver is executed by a low-level path-following feedback controller in real-time. The effectiveness of the approach is demonstrated through simulations of three case studies: 1) formation reconfiguration 2) obstacle avoidance, and 3) benchmarking against behavior-based planning in which the desired formation is achieved using a sequence of motion primitives. Videos and software can be found online https://github.com/RoyaFiroozi/Centralized-Planning.

Xiaojing Zhang, Alexander Liniger, Francesco Borrelli

This paper presents a novel method for reformulating non-differentiable collision avoidance constraints into smooth nonlinear constraints using strong duality of convex optimization. We focus on a controlled object whose goal is to avoid obstacles while moving in an n-dimensional space. The proposed reformulation does not introduce approximations, and applies to general obstacles and controlled objects that can be represented in an n-dimensional space as the finite union of convex sets. Furthermore, we connect our results with the notion of signed distance, which is widely used in traditional trajectory generation algorithms. Our method can be used in generic navigation and trajectory planning tasks, and the smoothness property allows the use of general-purpose gradient- and Hessian-based optimization algorithms. Finally, in case a collision cannot be avoided, our framework allows us to find "least-intrusive" trajectories, measured in terms of penetration. We demonstrate the efficacy of our framework on a quadcopter navigation and automated parking problem, and our numerical experiments suggest that the proposed methods enable real-time optimization-based trajectory planning problems in tight environments. Source code of our implementation is provided at https://github.com/XiaojingGeorgeZhang/OBCA.

Hansung Kim, Siddharth H. Nair, Francesco Borrelli

We present SHIELD, a hierarchical algorithm that reduces both the decision-variable dimension and the constraint set in $\ell_1$-regularized convex programs. From strong convexity and Lagrangian duality, we derive certificates that \emph{safely} discard constraints and decision variables while guaranteeing that all removed constraints remain satisfied and all removed variables are null. To further accelerate the proposed algorithm, we propose a transformer-based deep neural network to guide the dual certificate inference. We validate SHIELD on stochastic model predictive control (SMPC) in complex, multi-modal traffic scenarios, comparing against a full-dimensional SMPC policy. Numerical simulations demonstrate order-of-magnitude computational speedups while preserving feasibility and closed-loop safety, highlighting the practicality of certifiably safe, lightweight MPC in complex driving scenes.

Shengfan Cao, Francesco Borrelli

Safety-critical learning requires policies that improve performance without leaving the safe operating regime. We study constrained policy learning where model parameters must satisfy unknown, rollout-based safety constraints. We propose SCPO, a sampling-based weight-space projection method that enforces safety directly in parameter space without requiring gradient access to the constraint functions. Our approach constructs a local safe region by combining trajectory rollouts with smoothness bounds that relate parameter changes to shifts in safety metrics. Each gradient update is then projected via a convex SOCP, producing a safe first-order step. We establish a safe-by-induction guarantee: starting from any safe initialization, all intermediate policies remain safe given feasible projections. In constrained control settings with a stabilizing backup policy, our approach further ensures closed-loop stability and enables safe adaptation beyond the conservative backup. On regression with harmful supervision and a constrained double-integrator task with malicious expert, our approach consistently rejects unsafe updates, maintains feasibility throughout training, and achieves meaningful primal objective improvement.

Shengfan Cao, Eunhyek Joa, Francesco Borrelli

Guaranteeing constraint satisfaction is challenging in imitation learning (IL), particularly in tasks that require operating near a system's handling limits. Traditional IL methods, such as Behavior Cloning (BC), often struggle to enforce constraints, leading to suboptimal performance in high-precision tasks. In this paper, we present a simple approach to incorporating safety into the IL objective. Through simulations, we empirically validate our approach on an autonomous racing task with both full-state and image feedback, demonstrating improved constraint satisfaction and greater consistency in task performance compared to BC.

Siddharth H. Nair, Vijay Govindarajan, Theresa Lin et al.

We propose a Stochastic MPC (SMPC) formulation for autonomous driving at traffic intersections which incorporates multi-modal predictions of surrounding vehicles for collision avoidance constraints. The multi-modal predictions are obtained with Gaussian Mixture Models (GMM) and constraints are formulated as chance-constraints. Our main theoretical contribution is a SMPC formulation that optimizes over a novel feedback policy class designed to exploit additional structure in the GMM predictions, and that is amenable to convex programming. The use of feedback policies for prediction is motivated by the need for reduced conservatism in handling multi-modal predictions of the surrounding vehicles, especially prevalent in traffic intersection scenarios. We evaluate our algorithm along axes of mobility, comfort, conservatism and computational efficiency at a simulated intersection in CARLA. Our simulations use a kinematic bicycle model and multimodal predictions trained on a subset of the Lyft Level 5 prediction dataset. To demonstrate the impact of optimizing over feedback policies, we compare our algorithm with two SMPC baselines that handle multi-modal collision avoidance chance constraints by optimizing over open-loop sequences.

Eunhyek Joa, Yibo Sun, Francesco Borrelli

We address the problem of finding the current position and heading angle of an autonomous vehicle in real-time using a single camera. Compared to methods which require LiDARs and high definition (HD) 3D maps in real-time, the proposed approach is easily scalable and computationally efficient, at the price of lower precision. The new method combines and adapts existing algorithms in three different fields: image retrieval, mapping database, and particle filtering. The result is a simple, real-time localization method using an image retrieval method whose performance is comparable to other monocular camera localization methods which use a map built with LiDARs. We evaluate the proposed method using the KITTI odometry dataset and via closed-loop experiments with an indoor 1:10 autonomous vehicle. The tests demonstrate real-time capability and a 10cm level accuracy. Also, experimental results of the closed-loop indoor tests show the presence of a positive feedback loop between the localization error and the control error. Such phenomena is analysed in details at the end of the article.

Charlott Vallon, Francesco Borrelli

This article proposes a hierarchical learning architecture for safe data-driven control in unknown environments. We consider a constrained nonlinear dynamical system and assume the availability of state-input trajectories solving control tasks in different environments. In addition to task-invariant system state and input constraints, a parameterized environment model generates task-specific state constraints, which are satisfied by the stored trajectories. Our goal is to use these trajectories to find a safe and high-performing policy for a new task in a new, unknown environment. We propose using the stored data to learn generalizable control strategies. At each time step, based on a local forecast of the new task environment, the learned strategy consists of a target region in the state space and input constraints to guide the system evolution to the target region. These target regions are used as terminal sets by a low-level model predictive controller. We show how to i) design the target sets from past data and then ii) incorporate them into a model predictive control scheme with shifting horizon that ensures safety of the closed-loop system when performing the new task. We prove the feasibility of the resulting control policy, and apply the proposed method to robotic path planning, racing, and computer game applications.

Monimoy Bujarbaruah, Yvonne R. Stürz, Conrad Holda et al.

In this paper, we propose a leader-follower hierarchical strategy for two robots collaboratively transporting an object in a partially known environment with obstacles. Both robots sense the local surrounding environment and react to obstacles in their proximity. We consider no explicit communication, so the local environment information and the control actions are not shared between the robots. At any given time step, the leader solves a model predictive control (MPC) problem with its known set of obstacles and plans a feasible trajectory to complete the task. The follower estimates the inputs of the leader and uses a policy to assist the leader while reacting to obstacles in its proximity. The leader infers obstacles in the follower's vicinity by using the difference between the predicted and the real-time estimated follower control action. A method to switch the leader-follower roles is used to improve the control performance in tight environments. The efficacy of our approach is demonstrated with detailed comparisons to two alternative strategies, where it achieves the highest success rate, while completing the task fastest. See the link www.dropbox.com/s/hexadigqkvspaeh/IROS_Video.mp4?dl=0 for a descriptive video of the algorithm.

Hotae Lee, Monimoy Bujarbaruah, Francesco Borrelli

"Bubble Ball" is a game built on a 2D physics engine, where a finite set of objects can modify the motion of a bubble-like ball. The objective is to choose the set and the initial configuration of the objects, in order to get the ball to reach a target flag. The presence of obstacles, friction, contact forces and combinatorial object choices make the game hard to solve. In this paper, we propose a hierarchical predictive framework which solves Bubble Ball. Geometric, kinematic and dynamic models are used at different levels of the hierarchy. At each level of the game, data collected during failed iterations are used to update models at all hierarchical level and converge to a feasible solution to the game. The proposed approach successfully solves a large set of Bubble Ball levels within reasonable number of trials. This proposed framework can also be used to solve other physics-based games, especially with limited training data from human demonstrations.

Xu Shen, Edward L. Zhu, Yvonne R. Stürz et al.

We present a hierarchical control approach for maneuvering an autonomous vehicle (AV) in tightly-constrained environments where other moving AVs and/or human driven vehicles are present. A two-level hierarchy is proposed: a high-level data-driven strategy predictor and a lower-level model-based feedback controller. The strategy predictor maps an encoding of a dynamic environment to a set of high-level strategies via a neural network. Depending on the selected strategy, a set of time-varying hyperplanes in the AV's position space is generated online and the corresponding halfspace constraints are included in a lower-level model-based receding horizon controller. These strategy-dependent constraints drive the vehicle towards areas where it is likely to remain feasible. Moreover, the predicted strategy also informs switching between a discrete set of policies, which allows for more conservative behavior when prediction confidence is low. We demonstrate the effectiveness of the proposed data-driven hierarchical control framework in a two-car collision avoidance scenario through simulations and experiments on a 1/10 scale autonomous car platform where the strategy-guided approach outperforms a model predictive control baseline in both cases.

Monimoy Bujarbaruah, Tony Zheng, Akhil Shetty et al.

Playing the cup-and-ball game is an intriguing task for robotics research since it abstracts important problem characteristics including system nonlinearity, contact forces and precise positioning as terminal goal. In this paper, we present a learning model based control strategy for the cup-and-ball game, where a Universal Robots UR5e manipulator arm learns to catch a ball in one of the cups on a Kendama. Our control problem is divided into two sub-tasks, namely $(i)$ swinging the ball up in a constrained motion, and $(ii)$ catching the free-falling ball. The swing-up trajectory is computed offline, and applied in open-loop to the arm. Subsequently, a convex optimization problem is solved online during the ball's free-fall to control the manipulator and catch the ball. The controller utilizes noisy position feedback of the ball from an Intel RealSense D435 depth camera. We propose a novel iterative framework, where data is used to learn the support of the camera noise distribution iteratively in order to update the control policy. The probability of a catch with a fixed policy is computed empirically with a user specified number of roll-outs. Our design guarantees that probability of the catch increases in the limit, as the learned support nears the true support of the camera noise distribution. High-fidelity Mujoco simulations and preliminary experimental results support our theoretical analysis.

Roya Firoozi, Laura Ferranti, Xiaojing Zhang et al.

This work presents a distributed method for multi-vehicle coordination based on nonlinear model predictive control (NMPC) and dual decomposition. Our approach allows the vehicles to coordinate in tight spaces (e.g., busy highway lanes or parking lots) by using a polytopic description of each vehicle's shape and formulating collision avoidance as a dual optimization problem. Our method accommodates heterogeneous teams of vehicles (i.e., vehicles with different polytopic shapes and dynamic models can be part of the same team). Our method allows the vehicles to share their intentions in a distributed fashion without relying on a central coordinator and efficiently provides collision-free trajectories for the vehicles. In addition, our method decouples the individual-vehicles' trajectory optimization from their collision-avoidance objectives enhancing the scalability of the method and allowing one to exploit parallel hardware architectures. All these features are particularly important for vehicular applications, where the systems operate at high-frequency rates in dynamic environments. To validate our method, we apply it in a vehicular application, that is, the autonomous lane-merging of a team of connected vehicles to form a platoon. We compare our design with the centralized NMPC design to show the computational benefits of the proposed distributed algorithm.

Monimoy Bujarbaruah, Charlott Vallon, Francesco Borrelli

We propose a control design method for linear time-invariant systems that iteratively learns to satisfy unknown polyhedral state constraints. At each iteration of a repetitive task, the method constructs an estimate of the unknown environment constraints using collected closed-loop trajectory data. This estimated constraint set is improved iteratively upon collection of additional data. An MPC controller is then designed to robustly satisfy the estimated constraint set. This paper presents the details of the proposed approach, and provides robust and probabilistic guarantees of constraint satisfaction as a function of the number of executed task iterations. We demonstrate the safety of the proposed framework and explore the safety vs. performance trade-off in a detailed numerical example.

Xu Shen, Ivo Batkovic, Vijay Govindarajan et al.

We investigate the problem of predicting driver behavior in parking lots, an environment which is less structured than typical road networks and features complex, interactive maneuvers in a compact space. Using the CARLA simulator, we develop a parking lot environment and collect a dataset of human parking maneuvers. We then study the impact of model complexity and feature information by comparing a multi-modal Long Short-Term Memory (LSTM) prediction model and a Convolution Neural Network LSTM (CNN-LSTM) to a physics-based Extended Kalman Filter (EKF) baseline. Our results show that 1) intent can be estimated well (roughly 85% top-1 accuracy and nearly 100% top-3 accuracy with the LSTM and CNN-LSTM model); 2) knowledge of the human driver's intended parking spot has a major impact on predicting parking trajectory; and 3) the semantic representation of the environment improves long term predictions.

Dimitris Papadimitriou, Ugo Rosolia, Francesco Borrelli

We present a Model Predictive Control (MPC) strategy for unknown input-affine nonlinear dynamical systems. A non-parametric method is used to estimate the nonlinear dynamics from observed data. The estimated nonlinear dynamics are then linearized over time varying regions of the state space to construct an Affine Time Varying (ATV) model. Error bounds arising from the estimation and linearization procedure are computed by using sampling techniques. The ATV model and the uncertainty sets are used to design a robust Model Predictive Control (MPC) problem which guarantees safety for the unknown system with high probability. A simple nonlinear example demonstrates the effectiveness of the approach where commonly used linearization methods fail.

Edward L. Zhu, Yvonne R. Stürz, Ugo Rosolia et al.

We present a decentralized minimum-time trajectory optimization scheme based on learning model predictive control for multi-agent systems with nonlinear decoupled dynamics and coupled state constraints. By performing the same task iteratively, data from previous task executions is used to construct and improve local time-varying safe sets and an approximate value function. These are used in a decoupled MPC problem as terminal sets and terminal cost functions. Our framework results in a decentralized controller, which requires no communication between agents over each iteration of task execution, and guarantees persistent feasibility, finite-time closed-loop convergence, and non-decreasing performance of the global system over task iterations. Numerical experiments of a multi-vehicle collision avoidance scenario demonstrate the effectiveness of the proposed scheme.

Yeojun Kim, Samuel Tay, Jacopo Guanetti et al.

We present a hardware-in-the-loop (HIL) simulation setup for repeatable testing of Connected Automated Vehicles (CAVs) in dynamic, real-world scenarios. Our goal is to test control and planning algorithms and their distributed implementation on the vehicle hardware and, possibly, in the cloud. The HIL setup combines PreScan for perception sensors, road topography, and signalized intersections; Vissim for traffic micro-simulation; ETAS DESK-LABCAR/a dynamometer for vehicle and powertrain dynamics; and on-board electronic control units for CAV real time control. Models of traffic and signalized intersections are driven by real-world measurements. To demonstrate this HIL simulation setup, we test a Model Predictive Control approach for maximizing energy efficiency of CAVs in urban environments.

Xiaojing Zhang, Monimoy Bujarbaruah, Francesco Borrelli

In this paper, we propose a novel framework for approximating the explicit MPC law for linear parameter-varying systems using supervised learning. In contrast to most existing approaches, we not only learn the control policy, but also a "certificate policy", that allows us to estimate the sub-optimality of the learned control policy online, during execution-time. We learn both these policies from data using supervised learning techniques, and also provide a randomized method that allows us to guarantee the quality of each learned policy, measured in terms of feasibility and optimality. This in turn allows us to bound the probability of the learned control policy of being infeasible or suboptimal, where the check is performed by the certificate policy. Since our algorithm does not require the solution of an optimization problem during run-time, it can be deployed even on resource-constrained systems. We illustrate the efficacy of the proposed framework on a vehicle dynamics control problem where we demonstrate a speedup of up to two orders of magnitude compared to online optimization with minimal performance degradation.

Brijen Thananjeyan, Ashwin Balakrishna, Ugo Rosolia et al.

Reinforcement learning (RL) for robotics is challenging due to the difficulty in hand-engineering a dense cost function, which can lead to unintended behavior, and dynamical uncertainty, which makes exploration and constraint satisfaction challenging. We address these issues with a new model-based reinforcement learning algorithm, Safety Augmented Value Estimation from Demonstrations (SAVED), which uses supervision that only identifies task completion and a modest set of suboptimal demonstrations to constrain exploration and learn efficiently while handling complex constraints. We then compare SAVED with 3 state-of-the-art model-based and model-free RL algorithms on 6 standard simulation benchmarks involving navigation and manipulation and a physical knot-tying task on the da Vinci surgical robot. Results suggest that SAVED outperforms prior methods in terms of success rate, constraint satisfaction, and sample efficiency, making it feasible to safely learn a control policy directly on a real robot in less than an hour. For tasks on the robot, baselines succeed less than 5% of the time while SAVED has a success rate of over 75% in the first 50 training iterations. Code and supplementary material is available at https://tinyurl.com/saved-rl.

Charlott Vallon, Francesco Borrelli

A task decomposition method for iterative learning model predictive control is presented. We consider a constrained nonlinear dynamical system and assume the availability of state-input pair datasets which solve a task T1. Our objective is to find a feasible model predictive control policy for a second task, T2, using stored data from T1. Our approach applies to tasks T2 which are composed of subtasks contained in T1. In this paper we propose a formal definition of subtasks and the task decomposition problem, and provide proofs of feasibility and iteration cost improvement over simple initializations. We demonstrate the effectiveness of the proposed method on autonomous racing and robotic manipulation experiments.

Nitin R. Kapania, Vijay Govindarajan, Francesco Borrelli et al.

As autonomous vehicles (AVs) inch closer to reality, a central requirement for acceptance will be earning the trust of humans in everyday driving situations. In particular, the interaction between AVs and pedestrians is of high importance, as every human is a pedestrian at some point of the day. This paper considers the interaction of a pedestrian and an autonomous vehicle at a mid-block, unsignalized intersection where there is ambiguity over when the pedestrian should cross and when and how the vehicle should yield. By modeling pedestrian behavior through the concept of gap acceptance, the authors show that a hybrid controller with just four distinct modes allows an autonomous vehicle to successfully interact with a pedestrian across a continuous spectrum of possible crosswalk entry behaviors. The controller is validated through extensive simulation and compared to an alternate POMDP solution and experimental results are provided on a research vehicle for a virtual pedestrian.

Roya Firoozi, Shima Nazari, Jacopo Guanetti et al.

We present the design of a safe Adaptive Cruise Control (ACC) which uses road grade and lead vehicle motion preview. The ACC controller is designed by using a Model Predictive Control (MPC) framework to optimize comfort, safety, energy-efficiency and speed tracking accuracy. Safety is achieved by computing a robust invariant terminal set. The paper presents a novel approach to compute such set which is less conservative than existing methods. The proposed controller ensures safe inter-vehicle spacing at all times despite changes in the road grade and uncertainty in the predicted motion of the lead vehicle. Simulation results compare the proposed controller with a controller that does not incorporate prior grade knowledge on two scenarios including car-following and autonomous intersection crossing. The results demonstrate the effectiveness of the proposed control algorithm.

Roya Firoozi, Jacopo Guanetti, Roberto Horowitz et al.

We propose a map-aided vehicle localization method for GPS-denied environments. This approach exploits prior knowledge of the road grade map and vehicle on-board sensor measurements to accurately estimate the longitudinal position of the vehicle. Real-time localization is crucial to systems that utilize position-dependent information for planning and control. We validate the effectiveness of the localization method on a hierarchical control system. The higher level planner optimizes the vehicle velocity to minimize the energy consumption for a given route by employing traffic condition and road grade data. The lower level is a cruise control system that tracks the position-dependent optimal reference velocity. Performance of the proposed localization algorithm is evaluated using both simulations and experiments.

Ugo Rosolia, Francesco Borrelli

A Learning Model Predictive Controller (LMPC) for linear system in presented. The proposed controller is an extension of the LMPC [1] and it aims to decrease the computational burden. The control scheme is reference-free and is able to improve its performance by learning from previous iterations. A convex safe set and a terminal cost function are used in order to guarantee recursive feasibility and non-increasing performance at each iteration. The paper presents the control design approach, and shows how to recursively construct the convex terminal set and the terminal cost from state and input trajectories of previous iterations. Simulation results show the effectiveness of the proposed control logic.

Ugo Rosolia, Ashwin Carvalho, Francesco Borrelli

A novel learning Model Predictive Control technique is applied to the autonomous racing problem. The goal of the controller is to minimize the time to complete a lap. The proposed control strategy uses the data from previous laps to improve its performance while satisfying safety requirements. Moreover, a system identification technique is proposed to estimate the vehicle dynamics. Simulation results with the high fidelity simulator software CarSim show the effectiveness of the proposed control scheme.

Ugo Rosolia, Francesco Borrelli

A Learning Model Predictive Controller (LMPC) for iterative tasks is presented. The controller is reference-free and is able to improve its performance by learning from previous iterations. A safe set and a terminal cost function are used in order to guarantee recursive feasibility and non-increasing performance at each iteration. The paper presents the control design approach, and shows how to recursively construct terminal set and terminal cost from state and input trajectories of previous iterations. Simulation results show the effectiveness of the proposed control logic.