Search

We introduce an Implicit Game-Theoretic MPC (IGT-MPC), a decentralized algorithm for two-agent motion planning that uses a learned value function that predicts the game-theoretic interaction outcomes as the terminal cost-to-go function in a model predictive control (MPC) framework, guiding agents to implicitly account for interactions with other agents and maximize their reward. This approach applies to competitive and cooperative multi-agent motion planning problems which we formulate as constrained dynamic games. Given a constrained dynamic game, we randomly sample initial conditions and solve for the generalized Nash equilibrium (GNE) to generate a dataset of GNE solutions, computing the reward outcome of each game-theoretic interaction from the GNE. The data is used to train a simple neural network to predict the reward outcome, which we use as the terminal cost-to-go function in an MPC scheme. We showcase emerging competitive and coordinated behaviors using IGT-MPC in scenarios such as two-vehicle head-to-head racing and un-signalized intersection navigation. IGT-MPC offers a novel method integrating machine learning and game-theoretic reasoning into model-based decentralized multi-agent motion planning., Comment: Submitted to 2025 Learning for Dynamics and Control Conference (L4DC)

We propose a novel Stochastic Model Predictive Control (MPC) for uncertain linear systems subject to probabilistic constraints. The proposed approach leverages offline learning to extract key features of affine disturbance feedback policies, significantly reducing the computational burden of online optimization. Specifically, we employ offline data-driven sampling to learn feature components of feedback gains and approximate the chance-constrained feasible set with a specified confidence level. By utilizing this learned information, the online MPC problem is simplified to optimization over nominal inputs and a reduced set of learned feedback gains, ensuring computational efficiency. In a numerical example, the proposed MPC approach achieves comparable control performance in terms of Region of Attraction (ROA) and average closed-loop costs to classical MPC optimizing over disturbance feedback policies, while delivering a 10-fold improvement in computational speed., Comment: Submitted to L4DC 2025

We present a Learning Model Predictive Controller (LMPC) for multi-modal systems performing iterative control tasks. Assuming availability of historical data, our goal is to design a data-driven control policy for the multi-modal system where the current mode is unknown. First, we propose a novel method to select local data for constructing affine time-varying (ATV) models of a multi-modal system in the context of LMPC. Then we present how to build a sampled safe set from multi-modal historical data. We demonstrate the effectiveness of our method through simulation results of automated driving on a friction-varying track., Comment: Submitted to 2024 IEEE Conference on Decision and Control

We present a novel control-oriented motorcycle model and use it for computing racing lines on a nonplanar racetrack. The proposed model combines recent advances in nonplanar road models with the dynamics of motorcycles. Our approach considers the additional camber degree of freedom of the motorcycle body with a simplified model of the rider and front steering fork bodies. We demonstrate the effectiveness of our model by computing minimum-time racing trajectories on a nonplanar racetrack.

We present an approach for predictive braking of a four-wheeled vehicle on a nonplanar road. Our main contribution is a methodology to consider friction and road contact safety on general smooth road geometry. We use this to develop an active safety system to preemptively reduce vehicle speed for upcoming road geometry, such as off-camber turns. Our system may be used for human-driven or autonomous vehicles and we demonstrate it with a simulated ADAS scenario. We show that loss of control due to driver error on nonplanar roads can be mitigated by our approach.

This paper presents a data-driven Model Predictive Control (MPC) for energy-efficient urban road driving for connected, automated vehicles. The proposed MPC aims to minimize total energy consumption by controlling the vehicle's longitudinal motion on roads with traffic lights and front vehicles. Its terminal cost function and terminal constraints are learned from data, which consists of the closed-loop state and input trajectories. The terminal cost function represents the remaining energy-to-spend starting from a given terminal state. The terminal constraints are designed to ensure that the controlled vehicle timely crosses the upcoming traffic light, adheres to traffic laws, and accounts for the front vehicles. We validate the effectiveness of our method through both simulations and vehicle-in-the-loop experiments, demonstrating 19% improvement in average energy efficiency compared to conventional approaches that involve solving a long-horizon optimal control problem for speed planning and employing a separate controller for speed tracking., Comment: Accepted for IEEE Transactions of Intelligent Vehicles. arXiv admin note: text overlap with arXiv:2402.01059

Multi-Objective Learning Model Predictive Control is a novel data-driven control scheme which improves a linear system's closed-loop performance with respect to several convex control objectives over iterations of a repeated task. At each task iteration, collected system data is used to construct terminal components of a Model Predictive Controller. The formulation presented in this paper ensures that closed-loop control performance improves between successive iterations with respect to each objective. We provide proofs of recursive feasibility and performance improvement, and show that the converged policy is Pareto optimal. Simulation results demonstrate the applicability of the proposed approach.

Dynamic games can be an effective approach for modeling interactive behavior between multiple competitive agents in autonomous racing and they provide a theoretical framework for simultaneous prediction and control in such scenarios. In this work, we propose DG-SQP, a numerical method for the solution of local generalized Nash equilibria (GNE) for open-loop general-sum dynamic games for agents with nonlinear dynamics and constraints. In particular, we formulate a sequential quadratic programming (SQP) approach which requires only the solution of a single convex quadratic program at each iteration. The three key elements of the method are a non-monotonic line search for solving the associated KKT equations, a merit function to handle zero sum costs, and a decaying regularization scheme for SQP step selection. We show that our method achieves linear convergence in the neighborhood of local GNE and demonstrate the effectiveness of the approach in the context of head-to-head car racing, where we show significant improvement in solver success rate when comparing against the state-of-the-art PATH solver for dynamic games. An implementation of our solver can be found at https://github.com/zhu-edward/DGSQP., Comment: arXiv admin note: text overlap with arXiv:2203.16478

This paper introduces a novel data-driven hierarchical control scheme for managing a fleet of nonlinear, capacity-constrained autonomous agents in an iterative environment. We propose a control framework consisting of a high-level dynamic task assignment and routing layer and low-level motion planning and tracking layer. Each layer of the control hierarchy uses a data-driven Model Predictive Control (MPC) policy, maintaining bounded computational complexity at each calculation of a new task assignment or actuation input. We utilize collected data to iteratively refine estimates of agent capacity usage, and update MPC policy parameters accordingly. Our approach leverages tools from iterative learning control to integrate learning at both levels of the hierarchy, and coordinates learning between levels in order to maintain closed-loop feasibility and performance improvement of the connected architecture., Comment: 16 pages, 4 figures

This paper focuses on the design of hierarchical control architectures for autonomous systems with energy constraints. We focus on systems where energy storage limitations and slow recharge rates drastically affect the way the autonomous systems are operated. Using examples from space robotics and public transportation, we motivate the need for formally designed learning hierarchical control systems. We propose a learning control architecture which incorporates learning mechanisms at various levels of the control hierarchy to improve performance and resource utilization. The proposed hierarchical control scheme relies on high-level energy-aware task planning and assignment, complemented by a low-level predictive control mechanism responsible for the autonomous execution of tasks, including motion control and energy management. Simulation examples show the benefits and the limitations of the proposed architecture when learning is used to obtain a more energy-efficient task allocation.

The paper presents an approach to solving the Robust Energy Capacitated Vehicle Routing Problem (RECVRP), focusing on electric vehicles and their limited battery capacity. A finite number of customers, each with their own demand, have to be serviced by an electric vehicle fleet while ensuring that none of the vehicles run out of energy. The time and energy it takes to travel between any two points is modeled as a random variable with known distribution. We propose a Mixed Integer Program (MIP) for computing an exact solution and introduce clustering heuristics to enhance the solution speed. This enables efficient re-planning of routes in dynamic scenarios. The methodology transforms the RECVRP into smaller problems, yielding good quality solutions quickly compared to existing methods. We demonstrate the effectiveness of this approach using a well-known benchmark problem set as well as a set of randomly generated problems.

This paper introduces a novel approach to optimize the parking efficiency for fleets of Connected and Automated Vehicles (CAVs). We present a novel multi-vehicle parking simulator, equipped with hierarchical path planning and collision avoidance capabilities for individual CAVs. The simulator is designed to capture the key decision-making processes in parking, from low-level vehicle control to high-level parking assignment, and it enables the effective assessment of parking strategies for large fleets of ground vehicles. We formulate and compare different strategic parking spot assignments to minimize a collective cost. While the proposed framework is designed to optimize various objective functions, we choose the total parking time for the experiment, as it is closely related to the reduction of vehicles' energy consumption and greenhouse gas emissions. We validate the effectiveness of the proposed strategies through empirical evaluation against a dataset of real-world parking lot dynamics, realizing a substantial reduction in parking time by up to 43.8%. This improvement is attributed to the synergistic benefits of driving automation, the utilization of shared infrastructure state data, the exclusion of pedestrian traffic, and the real-time computation of optimal parking spot allocation.

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, Comment: Accepted at IEEE Intelligent Vehicles Symposium 2024

This paper addresses the eco-driving problem for connected vehicles on urban roads, considering localization uncertainty. Eco-driving is defined as longitudinal speed planning and control on roads with the presence of a sequence of traffic lights. We solve the problem by using a data-driven model predictive control (MPC) strategy. This approach involves learning a cost-to-go function and constraints from state-input data. The cost-to-go function represents the remaining energy-to-spend from the given state, and the constraints ensure that the controlled vehicle passes the upcoming traffic light timely while obeying traffic laws. The resulting convex optimization problem has a short horizon and is amenable for real-time implementations. We demonstrate the effectiveness of our approach through real-world vehicle experiments. Our method demonstrates $12\%$ improvement in energy efficiency compared to the traditional approaches, which plan longitudinal speed by solving a long-horizon optimal control problem and track the planned speed using another controller, as evidenced by vehicle experiments., Comment: Accepted for IEEE IV 2024

We propose a Model Predictive Control (MPC) with a single-step prediction horizon to solve infinite horizon optimal control problems with the expected sum of convex stage costs for constrained linear uncertain systems. The proposed method relies on two techniques. First, we estimate the expected values of the convex costs using a computationally tractable approximation, achieved by sampling across the space of disturbances. Second, we implement a data-driven approach to approximate the optimal value function and its corresponding domain, through systematic exploration of the system's state space. These estimates are subsequently used as the terminal cost and terminal set within the proposed MPC. We prove recursive feasibility, robust constraint satisfaction, and convergence in probability to the target set. Furthermore, we prove that the estimated value function converges to the optimal value function in a local region. The effectiveness of the proposed MPC is illustrated with detailed numerical simulations and comparisons with a value iteration method and a Learning MPC that minimizes a certainty equivalent cost., Comment: Submitted to the IEEE Transactions on Automatic Control

We propose a Stochastic MPC (SMPC) formulation for path planning with autonomous vehicles in scenarios involving multiple agents with multi-modal predictions. The multi-modal predictions capture the uncertainty of urban driving in distinct modes/maneuvers (e.g., yield, keep speed) and driving trajectories (e.g., speed, turning radius), which are incorporated for multi-modal collision avoidance chance constraints for path planning. In the presence of multi-modal uncertainties, it is challenging to reliably compute feasible path planning solutions at real-time frequencies ($\geq$ 10 Hz). Our main technological contribution is a convex SMPC formulation that simultaneously (1) optimizes over parameterized feedback policies and (2) allocates risk levels for each mode of the prediction. The use of feedback policies and risk allocation enhances the feasibility and performance of the SMPC formulation against multi-modal predictions with large uncertainty. We evaluate our approach via simulations and road experiments with a full-scale vehicle interacting in closed-loop with virtual vehicles. We consider distinct, multi-modal driving scenarios: 1) Negotiating a traffic light and a fast, tailgating agent, 2) Executing an unprotected left turn at a traffic intersection, and 3) Changing lanes in the presence of multiple agents. For all of these scenarios, our approach reliably computes multi-modal solutions to the path-planning problem at real-time frequencies., Comment: The first three authors contributed equally

This work presents a novel Learning Model Predictive Control (LMPC) strategy for autonomous racing at the handling limit that can iteratively explore and learn unknown dynamics in high-speed operational domains. We start from existing LMPC formulations and modify the system dynamics learning method. In particular, our approach uses a nominal, global, nonlinear, physics-based model with a local, linear, data-driven learning of the error dynamics. We conducted experiments in simulation and on 1/10th scale hardware, and deployed the proposed LMPC on a full-scale autonomous race car used in the Indy Autonomous Challenge (IAC) with closed loop experiments at the Putnam Park Road Course in Indiana, USA. The results show that the proposed control policy exhibits improved robustness to parameter tuning and data scarcity. Incremental and safety-aware exploration toward the limit of handling and iterative learning of the vehicle dynamics in high-speed domains is observed both in simulations and experiments., Comment: Accepted by ICRA 2024

We present two quadrotor raceline optimization approaches which differ in using Euclidean or non-Euclidean geometry to describe vehicle position. Both approaches use high-fidelity quadrotor dynamics and avoid the need to approximate gates using waypoints. We demonstrate both approaches on simulated racetracks with realistic vehicle parameters where we demonstrate 100x faster compute time than comparable published methods and improved solver convergence. We then extend the non-Euclidean approach to compute racelines in the presence of numerous static obstacles.

We present a data-driven optimization approach for robotic controlled deposition with a degradable tool. Existing methods make the assumption that the tool tip is not changing or is replaced frequently. Errors can accumulate over time as the tool wears away and this leads to poor performance in the case where the tool degradation is unaccounted for during deposition. In the proposed approach, we utilize visual and force feedback to update the unknown model parameters of our tool-tip. Subsequently, we solve a constrained finite time optimal control problem for tracking a reference deposition profile, where our robot plans with the learned tool degradation dynamics. We focus on a robotic drawing problem as an illustrative example. Using real-world experiments, we show that the error in target vs actual deposition decreases when learned degradation models are used in the control design., Comment: To be published in the proceedings of IFAC World Congress 2023

This work presents a novel hierarchical approach to increase Battery Electric Buses (BEBs) utilization in transit fleets. The proposed approach relies on three key components. A learning-based BEB digital twin cloud platform is used to accurately predict BEB charge consumption on a per vehicle, per driver, and per route basis, and accurately predict the time-to-charge BEB batteries to any level. These predictions are then used by a Predictive Block Assignment module to maximize the BEB fleet utilization. This module computes the optimal BEB daily assignment and charge management strategy. A Depot Parking and Charging Queue Management module is used to autonomously park and charge the vehicles based on their charging demands. The paper discusses the technical approach and benefits of each level in architecture and concludes with a realistic simulations study. The study shows that if our approach is employed, BEB fleet utilization can increase by 50% compared to state-of-the-art methods., Comment: Accepted at the 35th IEEE Intelligent Vehicles Symposium (IV 2023)

This paper presents a novel energy-efficient motion planning algorithm for Connected Autonomous Vehicles (CAVs) on urban roads. The approach consists of two components: a decision-making algorithm and an optimization-based trajectory planner. The decision-making algorithm leverages Signal Phase and Timing (SPaT) information from connected traffic lights to select a lane with the aim of reducing energy consumption. The algorithm is based on a heuristic rule which is learned from human driving data. The optimization-based trajectory planner generates a safe, smooth, and energy-efficient trajectory toward the selected lane. The proposed strategy is experimentally evaluated in a Vehicle-in-the-Loop (VIL) setting, where a real test vehicle receives SPaT information from both actual and virtual traffic lights and autonomously drives on a testing site, while the surrounding vehicles are simulated. The results demonstrate that the use of SPaT information in autonomous driving leads to improved energy efficiency, with the proposed strategy saving 37.1% energy consumption compared to a lane-keeping algorithm., Comment: IEEE Intelligent Vehicle Symposium, Anchorage, Alaska, June 4-7, 2023

We propose a supervised learning framework for computing solutions of multi-parametric Mixed Integer Linear Programs (MILPs) that arise in Model Predictive Control. Our approach also quantifies sub-optimality for the computed solutions. Inspired by Branch-and-Bound techniques, the key idea is to train a Neural Network/Random Forest, which for a given parameter, predicts a strategy consisting of (1) a set of Linear Programs (LPs) such that their feasible sets form a partition of the feasible set of the MILP and (2) a candidate integer solution. For control computation and sub-optimality quantification, we solve a set of LPs online in parallel. We demonstrate our approach for a motion planning example and compare against various commercial and open-source mixed-integer programming solvers., Comment: First two authors contributed equally

We propose an iterative approach for designing Robust Learning Model Predictive Control (LMPC) policies for a class of nonlinear systems with additive, unmodelled dynamics. The nominal dynamics are assumed to be difference flat, i.e., the state and input can be reconstructed using flat output sequences. For the considered class of systems, we synthesize Robust MPC policies and show how to use historical trajectory data collected during iterative tasks to 1) obtain bounds on the unmodelled dynamics and 2) construct a convex value function approximation along with a convex safe set in the space of output sequences for designing terminal components in the Robust MPC design. We show that the proposed strategy guarantees robust constraint satisfaction, asymptotic convergence to a desired subset of the state space, and non-decreasing closed-loop performance at each policy update. Finally, simulation results demonstrate the effectiveness of the proposed strategy on a minimum time control problem using a constrained nonlinear and uncertain vehicle model.

We present an output feedback stochastic model predictive controller (SMPC) for constrained linear time-invariant systems. The system is perturbed by additive Gaussian disturbances on state and additive Gaussian measurement noise on output. A Kalman filter is used for state estimation and an SMPC is designed to satisfy chance constraints on states and hard constraints on actuator inputs. The proposed SMPC constructs bounded sets for the state evolution and a tube-based constraint tightening strategy where the tightened constraints are time-invariant. We prove that the proposed SMPC can guarantee an infeasibility rate below a user-specified tolerance. We numerically compare our method with a classical output feedback SMPC with simulation results which highlight the efficacy of the proposed algorithm., Comment: IEEE American Control Conference (ACC) 2023, May 31 - June 2, San Diego, CA, USA

This work presents a distributed algorithm for resolving cooperative multi-vehicle conflicts in highly constrained spaces. By formulating the conflict resolution problem as a Multi-Agent Reinforcement Learning (RL) problem, we can train a policy offline to drive the vehicles towards their destinations safely and efficiently in a simplified discrete environment. During the online execution, each vehicle first simulates the interaction among vehicles with the trained policy to obtain its strategy, which is used to guide the computation of a reference trajectory. A distributed Model Predictive Controller (MPC) is then proposed to track the reference while avoiding collisions. The preliminary results show that the combination of RL and distributed MPC has the potential to guide vehicles to resolve conflicts safely and smoothly while being less computationally demanding than the centralized approach.

A distributed coordination method for solving multi-vehicle lane changes for connected autonomous vehicles (CAVs) is presented. Existing approaches to multi-vehicle lane changes are passive and opportunistic as they are implemented only when the environment allows it. The novel approach of this paper relies on the role of a facilitator assigned to a CAV. The facilitator interacts with and modifies the environment to enable lane changes of other CAVs. Distributed MPC path planners and a distributed coordination algorithm are used to control the facilitator and other CAVs in a proactive and cooperative way. We demonstrate the effectiveness of the proposed approach through numerical simulations. In particular, we show enhanced feasibility of a multi-CAV lane change in comparison to the simultaneous multi-CAV lane change approach in various traffic conditions generated by using a data-set from real-traffic scenarios., Comment: 8 pages, 4 figures. Accepted at IFAC World Congress 2023

This paper presents a cloud-based learning model predictive controller that integrates three interacting components: a set of agents, which must learn to perform a finite set of tasks with the minimum possible local cost; a coordinator, which assigns the tasks to the agents; and the cloud, which stores data to facilitate the agents' learning. The tasks consist in traveling repeatedly between a set of target states while satisfying input and state constraints. In turn, the state constraints may change in time for each of the possible tasks. To deal with it, different modes of operation, which establish different restrictions, are defined. The agents' inputs are found by solving local model predictive control (MPC) problems where the terminal set and cost are defined from previous trajectories. The data collected by each agent is uploaded to the cloud and made accessible to all their peers. Likewise, similarity between tasks is exploited to accelerate the learning process. The applicability of the proposed approach is illustrated by simulation results., Comment: Conference on Decision and Control 2022

We present a novel method to address the problem of multi-vehicle conflict resolution in highly constrained spaces. An optimal control problem is formulated to incorporate nonlinear, non-holonomic vehicle dynamics and exact collision avoidance constraints. A solution to the problem can be obtained by first learning configuration strategies with reinforcement learning (RL) in a simplified discrete environment, and then using these strategies to shape the constraint space of the original problem. Simulation results show that our method can explore efficient actions to resolve conflicts in confined space and generate dexterous maneuvers that are both collision-free and kinematically feasible.

This paper presents a stochastic model predictive controller (SMPC) for linear time-invariant systems in the presence of additive disturbances. The distribution of the disturbance is unknown and is assumed to have a bounded support. A sample-based strategy is used to compute sets of disturbance sequences necessary for robustifying the state chance constraints. These sets are constructed offline using samples of the disturbance extracted from its support. For online MPC implementation, we propose a novel reformulation strategy of the chance constraints, where the constraint tightening is computed by adjusting the offline computed sets based on the previously realized disturbances along the trajectory. The proposed MPC is recursive feasible and can lower conservatism over existing SMPC approaches at the cost of higher offline computational time. Numerical simulations demonstrate the effectiveness of the proposed approach., Comment: Submitted to ACC 2023

We propose a Model Predictive Control (MPC) for collision avoidance between an autonomous agent and dynamic obstacles with uncertain predictions. The collision avoidance constraints are imposed by enforcing positive distance between convex sets representing the agent and the obstacles, and tractably reformulating them using Lagrange duality. This approach allows for smooth collision avoidance constraints even for polytopes, which otherwise require mixed-integer or non-smooth constraints. We consider three widely used descriptions of the uncertain obstacle position: 1) Arbitrary distribution with polytopic support, 2) Gaussian distributions and 3) Arbitrary distribution with first two moments known. For each case we obtain deterministic reformulations of the collision avoidance constraints. The proposed MPC formulation optimizes over feedback policies to reduce conservatism in satisfying the collision avoidance constraints. The proposed approach is validated using simulations of traffic intersections in CARLA., Comment: Accepted to CDC'22

We propose a Stochastic MPC (SMPC) approach for autonomous driving which incorporates multi-modal, interaction-aware predictions of surrounding vehicles. For each mode, vehicle motion predictions are obtained by a control model described using a basis of fixed features with unknown weights. The proposed SMPC formulation finds optimal controls which serves two purposes: 1) reducing conservatism of the SMPC by optimizing over parameterized control laws and 2) prediction and estimation of feature weights used in interaction-aware modeling using Kalman filtering. The proposed approach is demonstrated on a longitudinal control example, with uncertainties in predictions of the autonomous and surrounding vehicles., Comment: Accepted to AVEC'22

We study a human-robot collaborative transportation task in presence of obstacles. The task for each agent is to carry a rigid object to a common target position, while safely avoiding obstacles and satisfying the compliance and actuation constraints of the other agent. Human and robot do not share the local view of the environment. The human policy either assists the robot when they deem the robot actions safe based on their perception of the environment, or actively leads the task. Using estimated human inputs, the robot plans a trajectory for the transported object by solving a constrained finite time optimal control problem. Sensors on the robot measure the inputs applied by the human. The robot then appropriately applies a weighted combination of the human's applied and its own planned inputs, where the weights are chosen based on the robot's trust value on its estimates of the human's inputs. This allows for a dynamic leader-follower role adaptation of the robot throughout the task. Furthermore, under a low value of trust, if the robot approaches any obstacle potentially unknown to the human, it triggers a safe stopping policy, maintaining safety of the system and signaling a required change in the human's intent. With experimental results, we demonstrate the efficacy of the proposed approach.

In head-to-head racing, an accurate model of interactive behavior of the opposing target vehicle (TV) is required to perform tightly constrained, but highly rewarding maneuvers such as overtaking. However, such information is not typically made available in competitive scenarios, we therefore propose to construct a prediction and uncertainty model given data of the TV from previous races. In particular, a one-step Gaussian process (GP) model is trained on closed-loop interaction data to learn the behavior of a TV driven by an unknown policy. Predictions of the nominal trajectory and associated uncertainty are rolled out via a sampling-based approach and are used in a model predictive control (MPC) policy for the ego vehicle in order to intelligently trade-off between safety and performance when attempting overtaking maneuvers against a TV. We demonstrate the GP-based predictor in closed loop with the MPC policy in simulation races and compare its performance against several predictors from literature. In a Monte Carlo study, we observe that the GP-based predictor achieves similar win rates while maintaining safety in up to 3x more races. We finally demonstrate the prediction and control framework in real-time in a experimental study on a 1/10th scale racecar platform operating at speeds of around 2.8 m/s, and show a significant level of improvement when using the GP-based predictor over a baseline MPC predictor. Videos of the hardware experiments can be found at https://youtu.be/KMSs4ofDfIs., Comment: Submitted to IROS 2023

We leverage game theory and a new vehicle modeling approach to compute overtaking maneuvers for racecars on a nonplanar surface. We solve for equilibria between noncooperative racing agents and demonstrate that by leveraging the novel nonplanar vehicle dynamics, overtaking can be achieved in situations where simpler models can do not provide a winning strategy.

We present a 10 DoF dynamic vehicle model for model-based control on nonplanar road surfaces. A parametric surface is used to describe the road surface, allowing the surface parameterization to describe the pose of the vehicle. We use the proposed approach to compute minimum-time vehicle trajectories on nonplanar surfaces and compare planar and nonplanar models.

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., Comment: Published at IEEE ITSC 2022

Dynamic games can be an effective approach to modeling interactive behavior between multiple non-cooperative agents and they provide a theoretical framework for simultaneous prediction and control in such scenarios. In this work, we propose a numerical method for the solution of local generalized Nash equilibria (GNE) for the class of open-loop general-sum dynamic games for agents with nonlinear dynamics and constraints. In particular, we formulate a sequential quadratic programming (SQP) approach which requires only the solution of a single convex quadratic program at each iteration. Central to the robustness of our approach is a non-monotonic line search method and a novel merit function for SQP step acceptance. We show that our method achieves linear convergence in the neighborhood of local GNE and we derive an update rule for the merit function which helps to improve convergence from a larger set of initial conditions. We demonstrate the effectiveness of the algorithm in the context of car racing, where we show up to 32\% improvement of success rate when comparing against a state-of-the-art solution approach for dynamic games. \url{https://github.com/zhu-edward/DGSQP}.

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., Comment: Extended version of ITSC 2022 submission

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.

This paper studies the value of communicated motion predictions in the longitudinal control of connected automated vehicles (CAVs). We focus on a safe cooperative adaptive cruise control (CACC) design and analyze the value of vehicle-to-vehicle (V2V) communication in the presence of uncertain front vehicle acceleration. The interest in CACC is motivated by the potential improvement in energy consumption and road throughput. In order to quantify this potential, we characterize experimentally the relationship between inter-vehicular gap, vehicle speed, and (reduction of) energy consumption for a compact plug-in hybrid electric vehicle. The resulting model is leveraged to show efficacy of our control design, which pursues small inter-vehicle gaps between consecutive CAVs and, therefore, improved energy efficiency. Our proposed control design is based on a robust model predictive control framework to systematically account for the system uncertainties. We present a set of thorough simulations aimed at quantifying energy efficiency improvement when vehicle states and predictions exchanged via V2V communication are used in the control law., Comment: Full (extended) version of the accepted paper at IEEE Transactions on Vehicular Technology

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., Comment: 25 pages, 7 figures. Code available at https://github.com/berkeleyautomation/rlqp

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., Comment: arXiv admin note: substantial text overlap with arXiv:2005.05948

We present a simplified model of a vehicle driving on a nonplanar road. A parametric surface is used to describe the nonplanar road which can describe any combination of curvature, bank and slope. We show that the proposed modeling approach generalizes planar vehicle models that reference a centerline, such as the Frenet model. We use the proposed approach for vehicle path planning and following using model predictive control. We also model and control vehicle contact with the road surface. We demonstrate that the proposed controller improves speed and lane following on complex roads compared to planar vehicle controllers, and mitigates loss of control on complex road surfaces including off-camber turns.

We propose a simple and computationally efficient approach for designing a robust Model Predictive Controller (MPC) for constrained uncertain linear systems. The uncertainty is modeled as an additive disturbance and an additive error on the system dynamics matrices. Set based bounds for each component of the model uncertainty are assumed to be known. We separate the constraint tightening strategy into two parts, depending on the length of the MPC horizon. For a horizon length of one, the robust MPC problem is solved exactly, whereas for other horizon lengths, the model uncertainty is over-approximated with a net-additive component. The resulting MPC controller guarantees robust satisfaction of state and input constraints in closed-loop with the uncertain system. With appropriately designed terminal components and an adaptive horizon strategy, we prove the controller's recursive feasibility and stability of the origin. With numerical simulations, we demonstrate that our proposed approach gains up to 15x online computation speedup over a tube MPC strategy, while stabilizing about 98$\%$ of the latter's region of attraction., Comment: Final version for IEEE American Control Conference (ACC), May 2021. arXiv admin note: text overlap with arXiv:2007.00930

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., Comment: Full version of our IROS 2021 paper

"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., Comment: Accepted to L4DC 2021

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., Comment: 7 pages, 7 figures, accepted at ICRA 2021

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., Comment: Final paper for IEEE CASE, Hong Kong, August, 2020. First three authors contributed equally

We propose a novel approach to design a robust Model Predictive Controller (MPC) for constrained uncertain linear systems. The uncertain system is modeled as linear parameter varying with additive disturbance. Set bounds for the system matrices and the additive uncertainty are assumed to be known. We formulate a novel optimization-based constraint tightening strategy around a predicted nominal trajectory which utilizes these bounds. With an appropriately designed terminal cost function and constraint set, we prove robust satisfaction of the imposed constraints by the resulting MPC in closed-loop with the uncertain system, and Input to State Stability of the origin. We highlight the efficacy of our proposed approach via a numerical example., Comment: Dimension of matrices in the MPC problem fixed. Shortened and slightly altered version of this draft published as a full paper in Automatica

This paper presents a distributed learning model predictive control (DLMPC) scheme for distributed linear time invariant systems with coupled dynamics and state constraints. The proposed solution method is based on an online distributed optimization scheme with nearest-neighbor communication. If the control task is iterative and data from previous feasible iterations are available, local data are exploited by the subsystems in order to construct the local terminal set and terminal cost, which guarantee recursive feasibility and asymptotic stability, as well as performance improvement over iterations. In case a first feasible trajectory is difficult to obtain, or the task is non-iterative, we further propose an algorithm that efficiently explores the state-space and generates the data required for the construction of the terminal cost and terminal constraint in the MPC problem in a safe and distributed way. In contrast to other distributed MPC schemes which use structured positive invariant sets, the proposed approach involves a control invariant set as the terminal set, on which we do not impose any distributed structure. The proposed iterative scheme converges to the global optimal solution of the underlying infinite horizon optimal control problem under mild conditions. Numerical experiments demonstrate the effectiveness of the proposed DLMPC scheme.