This work addresses the problem of vehicle path planning in the presence of obstacles and uncertainties, which is a fundamental problem in robotics.
While many path planning algorithms have been proposed for decades, many of them have dealt with only deterministic environments or only open-loop uncertainty, i.e., the uncertainty of the system state is not controlled and, typically, increases with time due to exogenous disturbances, which leads to the design of potentially conservative nominal paths.
In order to deal with disturbances and reduce uncertainty, generally, a lower-level feedback controller is used.
We conjecture that, if a path planner can consider the closed-loop evolution of the system uncertainty, it can compute less conservative but still feasible paths.
To this end, in this work we develop a new approach that is based on optimal covariance steering, which explicitly steers the state covariance for stochastic linear systems with additive noise under non-convex state chance constraints.
The proposed framework is verified using simple numerical simulations.
This paper considers the problem of steering the state distribution of a nonlinear stochastic system from an initial Gaussian to a terminal distribution with a specified mean and covariance, subject to probabilistic path constraints. An algorithm is developed to solve this problem by iteratively solving an approximate linearized problem as a convex program. This method, which we call iterative covariance steering (iCS), is numerically demonstrated by controlling a double integrator with quadratic drag force subject to additive Brownian noise while satisfying probabilistic path constraints.
In this paper, we present a new control policy parametrization for the finite-horizon covariance steering problem for discrete-time Gaussian linear systems (DTGLS) which can reduce the latter stochastic optimal control problem to a tractable optimization problem. The covariance steering problem seeks for a feedback control policy that will steer the state covariance of a DTGLS to a desired positive definite matrix in finite time. We consider two different formulations of the covariance steering problem, one with hard terminal LMI constraints (hard-constrained covariance steering) and another o…
This work addresses the optimal covariance control problem for stochastic discrete-time linear time-varying systems subject to chance constraints. Covariance steering is a stochastic control problem to steer the system state Gaussian distribution to another Gaussian distribution while minimizing a cost function. To the best of our knowledge, covariance steering problems have never been discussed with probabilistic chance constraints although it is a natural extension. In this work, first we show that, unlike the case with no chance constraints, the covariance steering with chance constraints p…
This work develops a stochastic model predictive controller~(SMPC) for uncertain linear systems with additive Gaussian noise subject to state and control constraints. The proposed approach is based on the recently developed finite-horizon optimal covariance steering control theory, which steers the mean and the covariance of the system state to prescribed target values at a given terminal time. We call our approach covariance steering-based SMPC, or CS-SMPC. We show that the proposed approach has several advantages over traditional SMPC approaches in the literature. Specifically, it is shown t…
We address the optimal covariance steering (OCS) problem for stochastic discrete linear systems with additive Gaussian noise under state chance constraints and input hard constraints. Because the system state can be unbounded due to the unbounded noise, the state constraints are formulated as probabilistic (chance) constraints, i.e., the maximum probability of constraint violation is constrained. In contrast, because it is hard to interpret the appropriate control action when the control command violates the constraints, probabilistically formulating the control constraints are difficult, and …
This paper extends the optimal covariance steering problem for linear stochastic systems subject to chance constraints to account for optimal risk allocation. Previous works have assumed a uniform risk allocation to cast the optimal control problem as a semi-definite program (SDP), which can be solved efficiently using standard SDP solvers. We adopt an Iterative Risk Allocation (IRA) formalism, which uses a two-stage approach to solve the optimal risk allocation problem for covariance steering. The upper-stage of IRA optimizes the risk, which is proved to be a convex problem, while the lower-s…
This work investigates the finite-horizon optimal covariance steering problem for discrete-time linear systems subject to both additive and multiplicative uncertainties as well as state and input chance constraints. In particular, a tractable convex approximation of the optimal covariance steering problem is developed by tightening the chance constraints and by introducing a suitable change of variables. The solution of the convex approximation is shown to be a valid (albeit potentially suboptimal) solution to the original chance-constrained covariance steering problem.
This work considers the optimal covariance steering problem for systems subject to both additive noise and uncertain parameters which may enter multiplicatively with the state and the control. The unknown parameters are modeled as a constant random variable sampled from a distribution with known moments. The optimal covariance steering problem is formulated using a moment-based representation of the system dynamics, which includes dependence between the unknown parameters and future states, and is solved using sequential convex programming. The proposed approach is demonstrated numerically usi…
We consider the covariance steering problem for nonlinear control-affine systems. Our objective is to find an optimal control strategy to steer the state of a system from an initial distribution to a target one whose mean and covariance are given. Due to the nonlinearity, the existing techniques for linear covariance steering problems are not directly applicable. By leveraging the celebrated Girsanov theorem, we formulate the problem into an optimization over the space path distributions. We then adopt a generalized proximal gradient algorithm to solve this optimization, where each update requ…
This article extends the optimal covariance steering (CS) problem for discrete time linear stochastic systems modeled using moment-based ambiguity sets. To hedge against the uncertainty in the state distributions while performing covariance steering, distributionally robust risk constraints are employed during the optimal allocation of the risk. Specifically, a distributionally robust iterative risk allocation (DR-IRA) formalism is used to solve the optimal risk allocation problem for the CS problem using a two-stage approach. The upper-stage of DR-IRA is a convex problem that optimizes the ri…
In this paper we study the finite-horizon optimal covariance steering problem for a continuous-time linear stochastic system subject to both additive and multiplicative noise. The noise can be continuous or it may contain jumps. Additive noise does not depend on the state or the control, whereas multiplicative noise has a magnitude proportional to the current state. The cost is assumed to be quadratic in both the state and the control. First, the controllability of the state covariance is established under mild assumptions. Then, the optimal control for steering the covariance is provided. Las…
In this work, we consider the problem of steering the first two moments of the uncertain state of an unknown discrete-time stochastic nonlinear system to a given terminal distribution in finite time. Toward that goal, first, a non-parametric predictive model is learned from a set of available training data points using stochastic variational Gaussian process regression: a powerful and scalable machine learning tool for learning distributions over arbitrary nonlinear functions. Second, we formulate a tractable nonlinear covariance steering algorithm that utilizes the Gaussian process predictive…
In this work, we consider the problem of steering the first two moments of the uncertain state of a discrete time nonlinear stochastic system to prescribed goal quantities at a given final time. In principle, the latter problem can be formulated as a density tracking problem, which seeks for a feedback policy that will keep the probability density function of the state of the system close, in terms of an appropriate metric, to the goal density. The solution to the latter infinite-dimensional problem can be, however, a complex and computationally expensive task. Instead, we propose a more tract…
In this paper, we study the covariance steering (CS) problem for discrete-time linear systems subject to multiplicative and additive noise. Specifically, we consider two variants of the so-called CS problem. The goal of the first problem, which is called the exact CS problem, is to steer the mean and the covariance of the state process to their desired values in finite time. In the second one, which is called the ``relaxed'' CS problem, the covariance assignment constraint is relaxed into a positive semi-definite constraint. We show that the relaxed CS problem can be cast as an equivalent conv…
Continuous-Time Covariance Steering with Common Free-Final Time: Finite-Horizon Solutions and Infinite-Horizon Limits
Daniel Guggenheim School of Aerospace, Georgia Institute of Technology（佐治亚理工学院）；Department of Electrical and Computer Engineering, FAMU-FSU College of Engineering（FAMU-FSU工程学院）
The problem of optimizing affine feedback laws that explicitly steer the mean and covariance of an uncertain system state in the presence of a Gaussian random field is considered. Spatially-dependent disturbances are successively approximated with respect to a nominal trajectory by a sequence of jointly Gaussian random vectors. Sequential updates to the nominal control inputs are computed via convex optimization that includes the effect of affine state feedback, the perturbing effects of spatial disturbances, and chance constraints on the closed-loop state and control. The developed method is …
In this paper, we present a new trajectory optimization algorithm for stochastic linear systems which combines Model Predictive Path Integral (MPPI) control with Constrained Covariance Steering (CSS) to achieve high performance with safety guarantees (robustness). Although MPPI can be used to solve complex nonlinear trajectory optimization problems, it may not always handle constraints effectively and its performance may degrade in the presence of unmodeled disturbances. By contrast, CCS can handle probabilistic state and / or input constraints (e.g., chance constraints) and also steer the sta…
The goal of this paper is to address finite-horizon minimum variance and covariance steering problems for discrete-time stochastic (Gaussian) linear systems. On the one hand, the minimum variance problem seeks for a control policy that will steer the state mean of an uncertain system to a prescribed quantity while minimizing the trace of its terminal state covariance (or variance). On the other hand, the covariance steering problem seeks for a control policy that will steer the covariance of the terminal state to a prescribed positive definite matrix. We propose a solution approach that relies…
Operator Splitting Covariance Steering for Safe Stochastic Nonlinear Control
School of Aerospace Engineering, Georgia Institute of Technology（航空航天工程学院，佐治亚理工学院）；School of Electrical and Computer Engineering, Georgia Institute of Technology（电气与计算机工程学院，佐治亚理工学院）
This paper addresses the problem of steering a discrete-time linear dynamical system from an initial Gaussian distribution to a final distribution in a game-theoretic setting. One of the two players strives to minimize a quadratic payoff, while at the same time tries to meet a given mean and covariance constraint at the final time-step. The other player maximizes the same payoff, but it is assumed to be indifferent to the terminal constraint. At first, the unconstrained version of the game is examined, and the necessary conditions for the existence of a saddle point are obtained. We then show …
In this paper, we present new results on the covariance steering problem with Wasserstein distance terminal cost. We show that the state history feedback control policy parametrization, which has been used before to solve this class of problems, requires an unnecessarily large number of variables and can be replaced by a randomized state feedback policy which leads to more tractable problem formulations without any performance loss. In particular, we show that under the latter policy, the problem can be equivalently formulated as a semi-definite program (SDP) which is in sharp contrast with ou…
University of Texas at Austin（德克萨斯大学奥斯汀分校）；University of California, Santa Cruz（加州大学圣克鲁兹分校）
In this work, we analyze the properties of the solution to the covariance steering problem for discrete time Gaussian linear systems with a squared Wasserstein distance terminal cost. In our previous work, we have shown that by utilizing the state feedback control policy parametrization, this stochastic optimal control problem can be associated with a difference of convex functions program. Here, we revisit the same covariance control problem but this time we focus on the analysis of the problem. Specifically, we establish the existence of solutions to the optimization problem and derive the f…
We consider a class of stochastic optimal control problems for discrete-time stochastic linear systems which seek for control policies that will steer the probability distribution of the terminal state of the system close to a desired Gaussian distribution. In our problem formulation, the closeness between the terminal state distribution and the desired (goal) distribution is measured in terms of the squared Wasserstein distance which is associated with a corresponding terminal cost term. We recast the stochastic optimal control problem as a finite-dimensional nonlinear program and we show tha…
In this paper, we present an iterative steering algorithm for nonholonomic systems (also called driftless control-affine systems) and we prove its global convergence under the sole assumption that the Lie Algebraic Rank Condition (LARC) holds true everywhere. That algorithm is an extension of the one introduced in [21] for regular systems. The first novelty here consists in the explicit algebraic construction, starting from the original control system, of a lifted control system which is regular. The second contribution of the paper is an exact motion planning method for nilpotent systems, whi…
We consider the problem of steering, via output feedback, the state distribution of a discrete-time, linear stochastic system from an initial Gaussian distribution to a terminal Gaussian distribution with prescribed mean and maximum covariance, subject to probabilistic path constraints on the state. The filtered state is obtained via a Kalman filter, and the problem is formulated as a deterministic convex program in terms of the distribution of the filtered state. We observe that, in the presence of constraints on the state covariance, and in contrast to classical Linear Quadratic Gaussian (LQ…
We consider the motion planning of an object in a Riemannian manifold where the object is steered from an initial point to a final point utilizing optimal control. Considering Pontryagin Minimization Principle we compute the Optimal Controls needed for steering the object from initial to final point. The Optimal Controls were solved with respect to time t and shown to have norm 1 which should be the case when the extremal trajectories, which are the solutions of Pontryagin Principle, are arc length parametrized. The extremal trajectories are supposed to be the geodesics on the Riemannian manif…
In this paper, a risk map-based path planning algorithm is introduced for autonomous vehicles. Multivariate B-splines are implemented to generate a risk map, which measures the risk of colliding with different objects. In the following step, a two-level optimal control problem is designed. At the first level, an overall lowest risk trajectory is found. Then in the second level, among the paths whose risk value is the lowest, the one with the minimum steering effort is determined. Finally, numerical simulations are carried out with the optimal control software OCPID-DAE1. Results show that this…
Autonomous emergency steering (AES) systems have the promising potential to further reduce traffic fatalities with other (potentially vulnerable) traffic participants by using relatively small lateral deviations to realize collision-free behavior. In this work, a complete and tractable software architecture is presented for such an AES system, comprising of the estimation of the vehicles capabilities, planning a set of paths which exploit these capabilities, checking the feasibility and risk of these paths and eventually triggering the decision to drive along one of these paths, i.e., initiati…
V*: An Efficient Motion Planning Algorithm for Autonomous Vehicles
Institute of Transport and Logistics Studies, Business School, the University of Sydney（悉尼大学交通与物流研究所）；School of Civil Engineering, Faculty of Engineering, the University of Sydney（悉尼大学土木工程学院）
We propose in this paper a constructive procedure that transforms locally, even at singular configurations, the kinematics of a car towing trailers into Kumpera-Ruiz normal form. This construction converts the nonholonomic motion planning problem into an algebraic problem (the resolution of a system of polynomial equations), which we illustrate by steering the two-trailer system in a neighborhood of singular configurations. We show also that the n-trailer system is a universal local model for all Goursat structures and that all Goursat structures are locally nilpotentizable.
Center for Technologies in Robotics and Mechatronics Components, Innopolis University（英诺波利斯大学机器人与机电一体化组件技术中心）
We explore path planning followed by kinodynamic smoothing while ensuring the vehicle dynamics feasibility for MAVs. We have chosen a geometrically based motion planning technique \textquotedblleft RRT*\textquotedblright\; for this purpose. In the proposed technique, we modified original RRT* introducing an adaptive search space and a steering function which help to increase the consistency of the planner. Moreover, we propose multiple RRT* which generates a set of desired paths, provided that the optimal path is selected among them. Then, apply kinodynamic smoothing, which will result in dyna…
The Geometry of Optimal Gait Families for Steering Kinematic Locomoting Systems
Collaborative Robotics and Intelligent Systems (CoRIS) Institute at Oregon State University（俄勒冈州立大学协作机器人与智能系统（CoRIS）研究所）；Johns Hopkins University（约翰斯·霍普金斯大学）
